绘制scara机器人工作空间

一、绘制scara机器人工作空间

  如上图，scara机器人大臂长$L_1$L1​，小臂长$L_2$L2​，$\theta_1$θ1​为关节1角度，$\theta_2$θ2​为关节2角度，且$\theta_1\in[\theta_{L1},\theta_{U1}],\theta_2\in[\theta_{L2},\theta_{U2}]$θ1​∈[θL1​,θU1​],θ2​∈[θL2​,θU2​]。

  scara机器人工作空间由四段圆弧组成(如上图)，圆弧方程如下：
$\begin{cases} x^2+y^2=(L_1+L_2)^2 \\ x^2+y^2=L_1^2+L_2^2+2L_1L_2cos\theta \\ (x-L_1cos\theta_{L1})^2+(y-L_1sin\theta_{L1})^2=L_2^2 \\ (x-L_1cos\theta_{U1})^2+(y-L_1sin\theta_{U1})^2=L_2^2 \tag 1 \end{cases}$⎩⎪⎪⎪⎨⎪⎪⎪⎧​x2+y2=(L1​+L2​)2x2+y2=L12​+L22​+2L1​L2​cosθ(x−L1​cosθL1​)2+(y−L1​sinθL1​)2=L22​(x−L1​cosθU1​)2+(y−L1​sinθU1​)2=L22​​(1)
  其中，左手系时，$\theta=\theta_{L2}$θ=θL2​；右手系时，$\theta=\theta_{U2}$θ=θU2​。

二、MATLAB代码

  绘制2D圆弧：

%{
Function: draw_2d_arc
Description: 绘制平面圆弧
Input: 圆弧圆心(x0, y0),半径r，起始角度theta1(rad)，结束角度theta2(rad), 曲线样式选择options
Output: 无
Author: Marc Pony([e-mail])
%}
function draw_2d_arc(x0, y0, r, theta1, theta2, options)
deltaTheta = 0.1 * pi / 180;
theta = theta1 : deltaTheta : theta2;
x = x0 + r * cos(theta);
y = y0 + r * sin(theta);
plot(x, y, 'LineStyle', options.LineStyle, 'Color', options.Color, 'LineWidth', options.LineWidth);
axis equal;
end

  绘制scara机器人工作空间：

%{
Function: draw_scara_workspace
Description: 绘制scara机器人工作空间
Input: 大臂L1，小臂L2，关节1限位角度thetaLimit1(rad)，关节2限位角度thetaLimit2(rad)，手系handcoor
Output: 无
Author: Marc Pony([e-mail])
%}
function draw_scara_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor)
thetaL1 = thetaLimit1(1);
thetaU1 = thetaLimit1(2);
thetaL2 = thetaLimit2(1);
thetaU2 = thetaLimit2(2);

hold on;
if(handcoor == 1) %right handcoor
    options.LineStyle = '-';
    options.Color='g';
    options.LineWidth = 3;
    
    x0 = 0;
    y0 = 0;
    r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2));
    alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
    thetaStart = thetaL1 + alpha;
    thetaEnd = thetaU1 + alpha;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = 0;
    y0 = 0;
    r = L1 + L2;
    thetaStart = thetaL1;
    thetaEnd = thetaU1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = L1 * cos(thetaU1);
    y0 = L1 * sin(thetaU1);
    r = L2;
    thetaStart = thetaU1;
    thetaEnd = thetaU1 + thetaU2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    x0 = L1 * cos(thetaL1);
    y0 = L1 * sin(thetaL1);
    r = L2;
    thetaStart = thetaL1;
    thetaEnd = thetaL1 + thetaU2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    title('Workspace in right handcoor', 'fontsize', 16);
else  %left handcoor
    options.LineStyle = '-';
    options.Color='b';
    options.LineWidth = 3;
    
    x0 = 0;
    y0 = 0;
    r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2));
    alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
    thetaStart = thetaL1 - alpha;
    thetaEnd = thetaU1 - alpha;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = 0;
    y0 = 0;
    r = L1 + L2;
    thetaStart = thetaL1;
    thetaEnd = thetaU1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = L1 * cos(thetaU1);
    y0 = L1 * sin(thetaU1);
    r = L2;
    thetaStart = thetaU1 + thetaL2;
    thetaEnd = thetaU1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    x0 = L1 * cos(thetaL1);
    y0 = L1 * sin(thetaL1);
    r = L2;
    thetaStart = thetaL1 + thetaL2;
    thetaEnd = thetaL1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    title('Workspace in left handcoor', 'fontsize', 16);
end
set(gcf, 'color', 'w');
axis off;
end

  绘制scara机器人工作空间草图：

%{
Function: draw_scara_workspace_sketch
Description: 绘制scara机器人工作空间草图
Input: 大臂L1，小臂L2，关节1限位角度thetaLimit1(rad)，关节2限位角度thetaLimit2(rad)，手系handcoor
Output: 无
Author: Marc Pony([e-mail])
%}
function draw_scara_workspace_sketch(L1, L2, thetaLimit1, thetaLimit2, handcoor)

thetaL1 = thetaLimit1(1);
thetaU1 = thetaLimit1(2);
thetaL2 = thetaLimit2(1);
thetaU2 = thetaLimit2(2);

hold on;
if(handcoor == 1) %right handcoor
    options.LineStyle = '-';
    options.Color='g';
    options.LineWidth = 3;
    
    x0 = 0;
    y0 = 0;
    r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2));
    alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
    thetaStart = thetaL1 + alpha;
    thetaEnd = thetaU1 + alpha;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = 0;
    y0 = 0;
    r = L1 + L2;
    thetaStart = thetaL1;
    thetaEnd = thetaU1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = L1 * cos(thetaU1);
    y0 = L1 * sin(thetaU1);
    r = L2;
    thetaStart = thetaU1;
    thetaEnd = thetaU1 + thetaU2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    x0 = L1 * cos(thetaL1);
    y0 = L1 * sin(thetaL1);
    r = L2;
    thetaStart = thetaL1;
    thetaEnd = thetaL1 + thetaU2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    %-------------
    options.LineStyle = '--';
    options.Color='r';
    options.LineWidth = 0.5;
    
    x0 = 0;
    y0 = 0;
    r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2));
    thetaStart = 0;
    thetaEnd = 2 * pi;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    r = L1 + L2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = L1 * cos(thetaU1);
    y0 = L1 * sin(thetaU1);
    r = L2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    x0 = L1 * cos(thetaL1);
    y0 = L1 * sin(thetaL1);
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    xA1 = L1 * cos(thetaL1);
    yA1 = L1 * sin(thetaL1);
    xB1 = xA1 + L2 * cos(thetaL1 + thetaU2);
    yB1 = yA1 + L2 * sin(thetaL1 + thetaU2);
    xA2 = L1 * cos(thetaU1);
    yA2 = L1 * sin(thetaU1);
    xB2 = xA2 + L2 * cos(thetaU1 + thetaU2);
    yB2 = yA2 + L2 * sin(thetaU1 + thetaU2);
    xC1 = (L1 + L2) * cos(thetaL1);
    yC1 = (L1 + L2) * sin(thetaL1);
    xC2 = (L1 + L2) * cos(thetaU1);
    yC2 = (L1 + L2) * sin(thetaU1);
    
    plot([0, xA1, xB1], [0, yA1, yB1], 'lineStyle', '-', 'color', 'k', 'lineWidth', 3);
    plot([0, xA2, xB2], [0, yA2, yB2], 'lineStyle', ':', 'color', 'k', 'lineWidth', 3);
    
    fontsize = 15;
    delta = 25;
    text(0, 0, 'O', 'Fontsize', fontsize);
    text(xA1, yA1 - delta, 'A_1', 'fontsize', fontsize);
    text(xB1, yB1 - delta, 'B_1', 'fontsize', fontsize);
    text(xA2, yA2 + delta, 'A_2', 'fontsize', fontsize);
    text(xB2, yB2 - delta, 'B_2', 'fontsize', fontsize);
    text(xC1, yC1, 'C_1', 'fontsize', fontsize);
    text(xC2, yC2, 'C_2', 'fontsize', fontsize);
    title('Workspace sketch in right handcoor', 'fontsize', 16);
    
else  %left handcoor
    options.LineStyle = '-';
    options.Color='b';
    options.LineWidth = 3;
    
    x0 = 0;
    y0 = 0;
    r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2));
    alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
    thetaStart = thetaL1 - alpha;
    thetaEnd = thetaU1 - alpha;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = 0;
    y0 = 0;
    r = L1 + L2;
    thetaStart = thetaL1;
    thetaEnd = thetaU1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = L1 * cos(thetaU1);
    y0 = L1 * sin(thetaU1);
    r = L2;
    thetaStart = thetaU1 + thetaL2;
    thetaEnd = thetaU1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    x0 = L1 * cos(thetaL1);
    y0 = L1 * sin(thetaL1);
    r = L2;
    thetaStart = thetaL1 + thetaL2;
    thetaEnd = thetaL1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    %-------------
    options.LineStyle = '--';
    options.Color='r';
    options.LineWidth = 0.5;
    
    x0 = 0;
    y0 = 0;
    r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2));
    thetaStart = 0;
    thetaEnd = 2 * pi;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    r = L1 + L2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = L1 * cos(thetaU1);
    y0 = L1 * sin(thetaU1);
    r = L2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    x0 = L1 * cos(thetaL1);
    y0 = L1 * sin(thetaL1);
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    xA1 = L1 * cos(thetaL1);
    yA1 = L1 * sin(thetaL1);
    xB1 = xA1 + L2 * cos(thetaL1 + thetaL2);
    yB1 = yA1 + L2 * sin(thetaL1 + thetaL2);
    xA2 = L1 * cos(thetaU1);
    yA2 = L1 * sin(thetaU1);
    xB2 = xA2 + L2 * cos(thetaU1 + thetaL2);
    yB2 = yA2 + L2 * sin(thetaU1 + thetaL2);
    xC1 = (L1 + L2) * cos(thetaL1);
    yC1 = (L1 + L2) * sin(thetaL1);
    xC2 = (L1 + L2) * cos(thetaU1);
    yC2 = (L1 + L2) * sin(thetaU1);
    
    plot([0, xA1, xB1], [0, yA1, yB1], 'lineStyle', '-', 'color', 'k', 'lineWidth', 3);
    plot([0, xA2, xB2], [0, yA2, yB2], 'lineStyle', ':', 'color', 'k', 'lineWidth', 3);
    
    fontsize = 15;
    delta = 25;
    text(0, 0, 'O', 'fontsize', fontsize);
    text(xA1, yA1 - delta, 'A_1', 'fontsize', fontsize);
    text(xB1, yB1 + delta, 'B_1', 'fontsize', fontsize);
    text(xA2, yA2 + delta, 'A_2', 'fontsize', fontsize);
    text(xB2, yB2 - delta, 'B_2', 'fontsize', fontsize);
    text(xC1, yC1, 'C_1', 'fontsize', fontsize);
    text(xC2, yC2, 'C_2', 'fontsize', fontsize);
    title('Workspace sketch in left handcoor', 'fontsize', 16);
end
set(gcf, 'color', 'w');
axis off;
end

clc;
clear;
close all;
L1 = 200;
L2 = 200;
thetaLimit1 = [-135, 135] * pi / 180;
thetaLimit2 = [-145, 145] * pi / 180;

%% 画工作空间
figure(1);
handcoor = 0;
draw_scara_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor)
figure(2);
handcoor = 1;
draw_scara_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor)

%% 画工作空间草图
figure(3);
handcoor = 0;
draw_scara_workspace_sketch(L1, L2, thetaLimit1, thetaLimit2, handcoor)
figure(4);
handcoor = 1;
draw_scara_workspace_sketch(L1, L2, thetaLimit1, thetaLimit2, handcoor)