scara机器人运动学正逆解

一、scara机器人运动学正解

  末端$B$的$x$坐标为向量$\mathbf{O}\mathbf{A}$与向量$\mathbf{A}\mathbf{B}$在$x$轴上投影之和，末端$B$的$y$坐标亦然：
$$\{\begin{array}{c}x=L_{1}cos{\theta }_1+L_{2}cos({\theta }_1+{\theta }_2)\\ y=L_{1}sin{\theta }_1+L_{2}sin({\theta }_1+{\theta }_2)\end{array}\text{\hspace{1em}(1)}$$
  第三轴为丝杆上下平移运动，设丝杆螺距$s$，末端$B$的$z$坐标：
$$z=\frac{{\theta }_{3}s}{2\pi }\text{\hspace{1em}(2)}$$
  末端$B$的姿态角$c$：
$$c={\theta }_1+{\theta }_2+{\theta }_4\text{\hspace{1em}(3)}$$
  综上，scara机器人的运动学正解为：
$$\{\begin{array}{l}x=L_{1}cos{\theta }_1+L_{2}cos({\theta }_1+{\theta }_2)\\ y=L_{1}sin{\theta }_1+L_{2}sin({\theta }_1+{\theta }_2)\\ z=\frac{{\theta }_{3}s}{2\pi }\\ c={\theta }_1+{\theta }_2+{\theta }_4\end{array}\text{\hspace{1em}(4)}$$

二、scara机器人运动学逆解

1、正装scara机器人运动学逆解

  连接$OB$，过$B$作$BC$垂直于$OA$于$C$，在$\Delta OAB$中，由余弦定理：
$$cos(\pi -{\theta }_2)=\frac{L_1^2+L_2^2-(x^2+y^2)}{2L_1{L}_2}\text{\hspace{1em}(5)}$$
  记$c_2=cos{\theta }_2$，式(5)可写成：
$$c_2=\frac{x^2+y^2-L_1^2-L_2^2}{2L_1{L}_2}\text{\hspace{1em}(6)}$$
  记$s_2=sin{\theta }_2$，则$s_2$有两个解：
$$s_2=\pm \sqrt{1-c_2^2}\text{\hspace{1em}(7)}$$
  取正数解，机器人处于右手系(right handcoor)；取负数解，机器人处于左手系(left handcoor)；特殊地，若$s_2=0$，机器人处于奇异位置(singular position)，此时${\theta }_2=k\pi (k\in Z)$，一般 θ 2 ∈ { − 2 π , − π , 0 , π , 2 π } \theta_2\in{\{-2\pi,-\pi,0,\pi,2\pi}\} θ2​∈{ −2π,−π,0,π,2π}。
  至此，可求得${\theta }_2$：
$${\theta }_2=atan2(s_2,c_2)\text{\hspace{1em}(8)}$$
  如上图，易得：
$$\alpha =atan2(y,x)\text{\hspace{1em}(9)}$$
  在$\Delta OBC$中，记$r=\Vert OB\Vert$：
$$sin\beta =\frac{\Vert BC\Vert }{\Vert OB\Vert }=\frac{L_2{s}_2}{r}\text{\hspace{1em}(10)}$$
$$cos\beta =\frac{\Vert OC\Vert }{\Vert OB\Vert }=\frac{\Vert OA\Vert +\Vert AC\Vert }{\Vert OB\Vert }=\frac{L_1+L_2{c}_2}{r}\text{\hspace{1em}(11)}$$
  由于$r>0$，由式$(10)$和式$(11)$得：
$$\beta =atan2(L_2{s}_2,L_1+L_2{c}_2)\text{\hspace{1em}(12)}$$
  至此，可求得${\theta }_1$：
$${\theta }_1=\alpha -\beta =atan2(y,x)-atan2(L_2{s}_2,L_1+L_2{c}_2)\text{\hspace{1em}(13)}$$
  由式$(4)$容易求得${\theta }_3$和${\theta }_4$。
  综上，正装scara机器人的运动学逆解为：
$$\{\begin{array}{l}{\theta }_1=atan2(y,x)-atan2(L_2{s}_2,L_1+L_2{c}_2)\\ {\theta }_2=atan2(s_2,c_2)\\ {\theta }_3=\frac{2\pi z}{s}\\ {\theta }_4=c-{\theta }_1-{\theta }_2\end{array}\text{\hspace{1em}(14)}$$
  在求解${\theta }_2$时，完全可以根据式$(6)$用反余弦函数$acos$求得，但这里采用了双变量反正切函数$atan2$，至于原因，可以参见这篇博文：为什么机器人运动学逆解最好采用双变量反正切函数atan2而不用反正/余弦函数？
  由于$atan2$的值域为$[-\pi ,\pi ]$，可得关节角度${\theta }_1$在$-2\pi$和$2\pi$之间(区间左端点$-2\pi$和区间右端点$2\pi$不一定能达到)，${\theta }_2\in [-\pi ,\pi ]$，${\theta }_3$范围与丝杆螺距$s$和$z$轴的行程有关，${\theta }_4$一般范围在$[-2\pi ,2\pi ]$。对于正装scara机器人，为避免机构干涉，一般${\theta }_1\in [-\frac{3\pi }{4},\frac{3\pi }{4}]$，${\theta }_2\in [-\frac{2\pi }{3},\frac{2\pi }{3}]$。 式$(14)$的运动学逆解无法做到使机器人在工作空间360°无死角运动，需要将其推广，使得${\theta }_1\in [-2\pi ,2\pi ]$且${\theta }_2\in [-2\pi ,2\pi ]$，即吊装scara机器人关节1和关节2的角度范围。

2、吊装scara机器人运动学逆解

  通过式$(14)$计算得到${\theta }_1$与${\theta }_2$后，增加关节1与关节2角度标志位flagJ1与flagJ2，利用以下的规则，可以将${\theta }_1$与${\theta }_2$范围限定在$[-2\pi ,2\pi ]$，实现吊装scara机器人360°无死角运动。
  将${\theta }_1$范围变换到$[-\pi ,\pi ]$:
  若${\theta }_1<-\pi$，则${\theta }_1={\theta }_1+2\pi$；
  若${\theta }_1>\pi$，则${\theta }_1={\theta }_1-2\pi$

  对于${\theta }_1$，当$flagJ1=1$时：
  若${\theta }_1\ge 0$，则${\theta }_1={\theta }_1-2\pi$；
  若${\theta }_1<0$，则${\theta }_1={\theta }_1+2\pi$

  对于${\theta }_2$，当$flag2=1$时：
  若${\theta }_2\ge 0$，则${\theta }_2={\theta }_2-2\pi$；
  若${\theta }_2<0$，${\theta }_2={\theta }_2+2\pi$

  ${\theta }_4=c-{\theta }_1-{\theta }_2$

3、几个值得思考的问题

(1)、手系handcoor的确定

  当${\theta }_2\in (0,\pi )\bigcup (-2\pi ,-\pi )$时，机器人处于右手系，此时$handcoor=1$
  当${\theta }_2\in (-\pi ,0)\bigcup (\pi ,2\pi )$时，机器人处于左手系，此时$handcoor=0$
  特别的，当 θ 2 ∈ { − 2 π , − π , 0 , π , 2 π } \theta_2\in{\{-2\pi,-\pi,0,\pi,2\pi}\} θ2​∈{ −2π,−π,0,π,2π}，机器人处于奇异位置。
  当机器人示教了一个点位，${\theta }_2$便确定了，因此机器人当前示教点的手系便确定。

(2)、标志位flagJ1与flagJ2的确定

  当${\theta }_1\in [-\pi ,\pi ]$时，$flagJ1=0$；否则，$flagJ1=1$。
  当${\theta }_2\in [-\pi ,\pi ]$时，$flagJ2=0$；否则，$flagJ2=1$。
  当手系handcoor确定后，便可以根据手系选逆解。当标志位flagJ1与flagJ2确定后，便可以确定关节角度值是否需要$\pm 2\pi$。

(3)、选取最短关节路径逆解

  在做笛卡尔空间插补(如直线、圆弧、样条插补等)时，如果${\theta }_1\in [-2\pi ,2\pi ]$且${\theta }_2\in [-2\pi ,2\pi ]$，求逆解过程可能会有$\pm 2\pi$的跳变。这时需要通过对比上一次关节位置，选取最短关节路径的解，以达到插补过程关节位置平滑变化，此时不再需要标志位flagJ1与flagJ2。

三、正逆解正确性验证

1、单点验证

  (1)在$[-2\pi ,2\pi ]$中随机生成4个关节角度（模拟示教过程）
  (2)计算标志位flagJ1和flagJ2，手系handcoor
  (3)计算正运动学
  (4)计算逆运动学
  (5)逆运动学结果与随机生成的关节角度作差，验证运动学正逆解的正确性

2、直线验证

  (1)在$[-2\pi ,2\pi ]$中随机生成4个关节角度，计算手系handcoor（模拟示教过程）
  (2)运动学正解计算直线起点坐标(x,y,z,c)
  (3)在工作空间里随机获取直线的终点x，y坐标，z在[-50mm,50mm]里随机生成,
   c在[-30°, 30°]里随机生成，手系与起点的相同（模拟示教过程）
  (4)速度规划(采用归一化五次多项式规划，详见博文：归一化5次多项式速度规划)
  (5)直线插补
  (6)计算每个插补点的逆运动学
  (7)计算每个插补点的正运动学
  (8)画出直线插补结果，合成位置、速度、加速度，关节位置曲线，验证正逆解的正确性

四、MATLAB代码

%{
Function: find_handcoor
Description: 计算scara机器人当前手系
Input: 关节2角度theta2(rad)
Output: 机器人手系
Author: Marc Pony([email protected])
%}
function handcoor = find_handcoor( theta2 )
if (theta2 > 0.0 && theta2 < pi) || (theta2 > -2.0 * pi && theta2 < -pi)
    handcoor = 1;
else
    handcoor = 0;
end
end

%{
Function: find_flagJ1
Description: 计算scara机器人当前J1关节标志位
Input: 关节1角度theta1(rad)
Output: 机器人J1关节标志位flagJ1
Author: Marc Pony([email protected])
%}
function flagJ1 = find_flagJ1( theta1 )
if (theta1 >= -pi && theta1 <= pi)
    flagJ1 = 0;
else
    flagJ1 = 1;
end
end

%{
Function: find_flagJ2
Description: 计算scara机器人当前J2关节标志位
Input: 关节1角度theta2(rad)
Output: 机器人J2关节标志位flagJ2
Author: Marc Pony([email protected])
%}
function flagJ2 = find_flagJ2( theta2 )
if (theta2 >= -pi && theta2 <= pi)
    flagJ2 = 0;
else
    flagJ2 = 1;
end
end

%{
Function: scara_forward_kinematics
Description: scara机器人运动学正解
Input: 大臂长L1(mm)，小臂长L2(mm)，丝杆螺距screw(mm)，机器人关节位置jointPos(rad)
Output: 机器人末端位置cartesianPos(mm或rad)
Author: Marc Pony([email protected])
%}
function cartesianPos = scara_forward_kinematics(L1, L2, screw, jointPos)
theta1 = jointPos(1);
theta2 = jointPos(2);
theta3 = jointPos(3);
theta4 = jointPos(4);
x = L1 * cos(theta1) + L2 * cos(theta1 + theta2);
y = L1 * sin(theta1) + L2 * sin(theta1 + theta2);
z = theta3 * screw / (2 * pi);
c = theta1 + theta2 + theta4;
cartesianPos = [x; y; z; c];
end

%{
Function: scara_inverse_kinematics
Description: scara机器人运动学逆解
Input: 大臂长L1(mm)，小臂长L2(mm)，丝杆螺距screw(mm)，机器人末端坐标cartesianPos(mm或rad)
       手系handcoor，标志位flagJ1与flagJ2
Output: 机器人关节位置jointPos(rad)
Author: Marc Pony([email protected])
%}
function jointPos = scara_inverse_kinematics(L1, L2, screw, cartesianPos, handcoor, flagJ1, flagJ2)
x = cartesianPos(1);
y = cartesianPos(2);
z = cartesianPos(3);
c = cartesianPos(4);
jointPos = zeros(4, 1);

calculateError = 1.0e-8;
c2 = (x^2 + y^2 - L1^2 -L2^2) / (2.0 * L1 * L2);%若(x,y)在工作空间里，则c2必在[-1,1]里，但由于计算精度，c2的绝对值可能稍微大于1
temp = 1.0 - c2^2;

if temp < 0.0
    if temp > -calculateError
        temp = 0.0;
    else
        error('区域不可到达');
    end
end
if handcoor == 0    %left handcoor
    jointPos(2) = atan2(-sqrt(temp), c2);
else                %right handcoor
    jointPos(2) = atan2(sqrt(temp), c2);
end
s2 = sin(jointPos(2));
jointPos(1) = atan2(y, x) - atan2(L2 * s2, L1 + L2 * c2);
jointPos(3) = 2.0 * pi * z / screw;

if jointPos(1) <= -pi
    jointPos(1) = jointPos(1) + 2.0*pi;
end
if jointPos(1) >= pi
    jointPos(1) = jointPos(1) - 2.0*pi;
end

if flagJ1 == 1
    if jointPos(1) >= 0.0
        jointPos(1) = jointPos(1) - 2.0*pi;
    else
        jointPos(1) = jointPos(1) + 2.0*pi;
    end
end

if flagJ2 == 1
    if jointPos(2) >= 0.0
        jointPos(2) = jointPos(2) - 2.0*pi;
    else
        jointPos(2) = jointPos(2) + 2.0*pi;
    end
end
jointPos(4) = c - jointPos(1) - jointPos(2);
end

%{
Function: scara_shortest_path_inverse_kinematics
Description: scara机器人运动学逆解
Input: 大臂长L1(mm)，小臂长L2(mm)，丝杆螺距screw(mm)，机器人末端坐标cartesianPos(mm或rad)
       手系handcoor，上一次的关节位置lastJointPos(rad)
Output: 机器人关节位置jointPos(rad)
Author: Marc Pony([email protected])
%}
function jointPos = scara_shortest_path_inverse_kinematics(len1, len2, screw, cartesianPos, handcoor, lastJointPos)
x = cartesianPos(1);
y = cartesianPos(2);
z = cartesianPos(3);
c = cartesianPos(4);
jointPos = zeros(4, 1);

calculateError = 1.0e-8;
c2 = (x^2 + y^2 - len1^2 -len2^2) / (2.0 * len1 * len2);%若(x,y)在工作空间里，则c2必在[-1,1]里，但由于计算精度，c2的绝对值可能稍微大于1
temp = 1.0 - c2^2;

if temp < 0.0
    if temp > -calculateError
        temp = 0.0;
    else
        error('区域不可到达');
    end
end
if handcoor == 0    %left handcoor
    jointPos(2) = atan2(-sqrt(temp), c2);
else                %right handcoor
    jointPos(2) = atan2(sqrt(temp), c2);
end
s2 = sin(jointPos(2));
jointPos(1) = atan2(y, x) - atan2(len2 * s2, len1 + len2 * c2);
jointPos(3) = 2.0 * pi * z / screw;

temp = zeros(3, 1);
for i = 1 : 2
    temp(1) = abs((jointPos(i) + 2.0*pi) - lastJointPos(i));
    temp(2) = abs((jointPos(i) - 2.0*pi) - lastJointPos(i));
    temp(3) = abs(jointPos(i) - lastJointPos(i));
    if temp(1) < temp(2) && temp(1) < temp(3)
        jointPos(i) = jointPos(i) + 2.0*pi;
    elseif temp(2) < temp(1) && temp(2) < temp(3)
        jointPos(i) = jointPos(i) - 2.0*pi;
    else
        %do nothing
    end
end
jointPos(4) = c - jointPos(1) - jointPos(2);
end

clc;
clear;
close all;

%% 输入参数
len1 = 200.0; %mm
len2 = 200.0; %mm
screw = 20.0; %mm
linearSpeed = 100; %mm/s
linearAcc = 800;   %mm/s^2
orientationSpeed = 200; %°/s
orientationAcc = 600;   %°/s^2
dt = 0.05; %s

%% 单点验证：
%(1)在[-2*pi, 2*pi]中随机生成4个关节角度（模拟示教过程）
%(2)计算标志位flagJ1和flagJ2，手系handcoor
%(3)计算正运动学
%(4)计算逆运动学
%(5)逆运动学结果与随机生成的关节角度作差，验证运动学正逆解的正确性
testCount = 1000000;
res = zeros(testCount, 4);
k = 1;
while k < testCount
    theta1 = -2.0*pi + 4.0*pi*rand;
    theta2 = -2.0*pi + 4.0*pi*rand;
    theta3 = -2.0*pi + 4.0*pi*rand;
    theta4 = -2.0*pi + 4.0*pi*rand;
    flagJ1 = find_flagJ1( theta1 );
    flagJ2 = find_flagJ2( theta2 );
    handcoor = find_handcoor( theta2 );
    jointPos = [theta1, theta2, theta3, theta4];
    cartesianPos = scara_forward_kinematics(len1, len2, screw, jointPos);
    
    jointPos2 = scara_inverse_kinematics(len1, len2, screw, cartesianPos, handcoor, flagJ1, flagJ2);
    res(k, :) = [theta1, theta2, theta3, theta4] - jointPos2';
    k = k + 1;
end
flag = all(abs(res - 0.0) < 1.0e-8) % flag = [1 1]则表示正逆解正确

while 1
    %% 直线验证：
    %(1)在[-2*pi, 2*pi]中随机生成4个关节角度，计算手系handcoor（模拟示教过程）
    %(2)运动学正解计算直线起点坐标(x,y,z,c)
    %(3)在工作空间里随机获取直线的终点x，y坐标，z在[-50mm,50mm]里随机生成,
    %   c在[-30°, 30°]里随机生成，手系与起点的相同（模拟示教过程）
    %(4)速度规划
    %(5)直线插补
    %(6)计算每个插补点的逆运动学
    %(7)计算每个插补点的正运动学
    %(8)画出直线插补结果，合成位置、速度、加速度，关节位置曲线，验证正逆解的正确性
    theta1 = -2.0*pi + 4.0*pi*rand;
    theta2 = -2.0*pi + 4.0*pi*rand;
    theta3 = -2.0*pi + 4.0*pi*rand;
    theta4 = -2.0*pi + 4.0*pi*rand;
    handcoor = find_handcoor( theta2 );
    jointPos = [theta1, theta2, theta3, theta4];
    cartesianPos = scara_forward_kinematics(len1, len2, screw, jointPos);
    lastJointPos = jointPos;
    x1 = cartesianPos(1);
    y1 = cartesianPos(2);
    z1 = cartesianPos(3);
    c1 = cartesianPos(4)*180.0/pi;
    
    figure(1)
    set(gcf, 'color','w');
    a = 0 : 0.01 : 2*pi;
    plot((len1 + len2)*cos(a), (len1 + len2)*sin(a), 'r--')
    hold on
    plot(x1, y1, 'ko')
    xlabel('x/mm');
    ylabel('y/mm');
    axis equal tight
    [x2, y2] = ginput(1);
    z2 = -50 + 50*rand;
    c2 = -30 + 60*rand;
    
    L = sqrt((x2 - x1)^2 + (y2 - y1)^2 + (z2 - z1)^2);
    C = abs(c2 - c1);
    T = max([1.875 * L / linearSpeed, 1.875 * C / orientationSpeed, ...
        sqrt(5.7735 * L / linearAcc), sqrt(5.7735 * C / orientationAcc)]);
    
    t = (0 : dt : T)';
    u = t / T;
    if abs(t(end) - T) > 1.0e-8
        t = [t; T];
        u = [u; 1];
    end
    u = t / T;
    s = 10*u.^3 - 15*u.^4 + 6*u.^5;
    ds = 30*u.^2 -60*u.^3 + 30*u.^4;
    dds = 60*u - 180*u.^2 + 120*u.^3;
    
    len = L * s;
    vel = (L / T) * ds;
    acc = (L / T^2) * dds;
    pos = zeros(length(t), 4);
    jointPos = zeros(length(t), 4);
    cartesianPos = zeros(length(t), 4);
    rate = [x2 - x1, y2 - y1, z2 - z1, c2 - c1] / L;
    for i = 1 : length(t)
        pos(i, :) = [x1, y1, z1, c1] + len(i) * rate;
        pos(i, 4) = pos(i, 4)*pi/180.0;
        jointPos(i, :) = scara_shortest_path_inverse_kinematics(len1, len2, screw, pos(i, :), handcoor, lastJointPos);
        cartesianPos(i, :) = scara_forward_kinematics(len1, len2, screw, jointPos(i, :));
        lastJointPos = jointPos(i, :);
    end
    plot3(cartesianPos(:, 1), cartesianPos(:, 2), cartesianPos(:, 3), 'ro');
    plot3([x1 x2], [y1 y2], [z1 z2], '+','markerfacecolor','k','markersize', 14)
    
    figure(2)
    set(gcf, 'color','w');
    subplot(3,1,1)
    plot(t, len)
    hold on
    plot([0 t(end)], [L L], 'r--')
    xlabel('t/s');
    ylabel('len/mm');
    subplot(3,1,2)
    plot(t, vel)
    hold on
    plot([0 t(end)], [linearSpeed linearSpeed], 'r--')
    xlabel('t/s');
    ylabel('vel/ mm/s');
    subplot(3,1,3)
    plot(t, acc)
    hold on
    plot([0 t(end)], [linearAcc linearAcc], 'r--')
    plot([0 t(end)], [-linearAcc -linearAcc], 'r--')
    xlabel('t/s');
    ylabel('acc/ mm/s^2');
    
    figure(3)
    set(gcf, 'color','w');
    subplot(3,1,1)
    plot(t, len*rate(4))
    hold on
    plot([0 t(end)], [c2-c1 c2-c1], 'r--')
    xlabel('t/s');
    ylabel('c/°');
    subplot(3,1,2)
    plot(t, vel*rate(4))
    hold on
    plot([0 t(end)], [orientationSpeed orientationSpeed], 'r--')
    plot([0 t(end)], [-orientationSpeed -orientationSpeed], 'r--')
    xlabel('t/s');
    ylabel('dc/ °/s');
    subplot(3,1,3)
    plot(t, acc*rate(4))
    hold on
    plot([0 t(end)], [orientationAcc orientationAcc], 'r--')
    plot([0 t(end)], [-orientationAcc -orientationAcc], 'r--')
    xlabel('t/s');
    ylabel('ddc/ °/s^2');
    
    figure(4)
    set(gcf, 'color','w');
    subplot(2,2,1)
    plot(t, jointPos(:,1)*180.0/pi)
    xlabel('t/s');
    ylabel('J1/°');
    subplot(2,2,2)
    plot(t, jointPos(:,2)*180.0/pi)
    xlabel('t/s');
    ylabel('J2/°');
    subplot(2,2,3)
    plot(t, jointPos(:,3)*180.0/pi)
    xlabel('t/s');
    ylabel('J3/°');
    subplot(2,2,4)
    plot(t, jointPos(:,4)*180.0/pi)
    xlabel('t/s');
    ylabel('J4/°');
    
    pause()
    close all
end