文章目录
一、scara机器人运动学正解
二、scara机器人运动学逆解
1、正装scara机器人运动学逆解
2、吊装scara机器人运动学逆解
3、几个值得思考的问题
(1)、手系handcoor的确定
(2)、标志位flagJ1与flagJ2的确定
(3)、选取最短关节路径逆解
三、正逆解正确性验证
1、单点验证
2、直线验证
四、MATLAB代码
一、scara机器人运动学正解
scara机器人运动学正解

末端 B 的 x 坐标为向量 O A 与向量 A B 在 x 轴上投影之和,末端 B 的 y 坐标亦然:

第三轴为丝杆上下平移运动,设丝杆螺距 s ,末端 B的 z 坐标:

末端 B 的姿态角 c :

综上,scara机器人的运动学正解为:

二、scara机器人运动学逆解

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

scara机器人运动学逆解

 连接 O B,过 B 作 B C 垂直于 O A 于 C,在 Δ O A B 中,由余弦定理:

  记 c 2 = c o s θ 2 ,式(5)可写成:

  记 s 2 = s i n θ 2  ,则 s 2 有两个解:

取正数解,机器人处于右手系(right handcoor);取负数解,机器人处于左手系(left handcoor);特殊地,若 s 2 = 0 ,机器人处于奇异位置(singular position),此时 θ 2 = k π ( k ∈ Z ) ,一般 θ 2 ∈ { − 2 π , − π , 0 , π , 2 π } 至此,可求得 θ 2  :

如上图,易得:

在 Δ O B C 中,记 r = ∥ O B ∥ :

由于 r > 0 ,由式 ( 10 ) 和式 ( 11 ) 得:

至此,可求得 θ 1

 由式 ( 4 ) 容易求得 θ 3  和 θ 4 。
  综上,正装scara机器人的运动学逆解为:

在求解 θ 2 时,完全可以根据式 ( 6 ) 用反余弦函数  acos求得,但这里采用了双变量反正切函数 atan2,至于原因,可以参见这篇博文:为什么机器人运动学逆解最好采用双变量反正切函数atan2而不用反正/余弦函数?
  由于 atan2的值域为 [−π,π],可得关节角度 θ 1 在 −2π和 2π之间(区间左端点 −2π和区间右端点 2π不一定能达到), θ 2​ ∈[−π,π], θ 3  范围与丝杆螺距 s 和 z 轴的行程有关, θ 4  一般范围在 [−2π,2π]。对于正装scara机器人,为避免机构干涉,一般 θ 1 ∈ [ − 3 π 4 , 3 π 4 ] , θ 2 ∈ [ − 2 π 3 , 2 π 3 ] 。  (14)的运动学逆解无法做到使机器人在工作空间360°无死角运动,需要将其推广,使得θ 1∈[−2π,2π]且 θ 2 ∈ [ − 2 π , 2 π ],即吊装scara机器人关节1和关节2的角度范围。
2、吊装scara机器人运动学逆解

3、几个值得思考的问题

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

2、直线验证
  (1)在 [−2π,2π]中随机生成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(marc_pony@163.com)
%}
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(marc_pony@163.com)
%}
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(marc_pony@163.com)
%}
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(marc_pony@163.com)
%}
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(marc_pony@163.com)
%}
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(marc_pony@163.com)
%}
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

1

在这里插入图片描述

在这里插入图片描述

在这里插入图片描述