SCARA机器人运动学模型建立

1 DH模型

DH模型是目前机器人建模过程中使用最多的方法。此方法不仅简单好用，且适用范围广。如图表达了通用关节-连杆之间相对位置关系。

  关节-连杆组合之间位置关系

    D-H 建模第一步便是为关节定义坐标系。坐标系建立方法如下：

(1)确定连杆坐标系原点。首先找到轴i-1和轴i的公垂线，坐标原点即位于公垂线与轴i-1的交点上。

(2)确定z轴。轴i-1的关节轴线即是该连杆坐标系的z轴。正方向可以任意指定，但为了统一，所有z轴方向应尽量保持一致。

(3)确定x轴。轴i-1和轴i的公垂线即x轴，其方向沿着轴i-1指向i。

(4)确定y轴。y轴确定方式简单，已知xz轴，依据右手法则就可以确定y轴。

通过以上介绍的方式为各个关节建立坐标系后，就可以根据两两坐标系之间的变换 关系来获得 D-H 参数，具体规则如下：

为 和 轴之间沿着xi 方向的长度，及连杆长度； 为 和 轴沿着 轴之间的角度，即扭角； 为 轴和 轴之间沿着 方向的距离，即关节偏移； 为 轴和 轴沿着 轴之间的角度，即关节旋转角。

两个坐标系之间，通过四个参数的确定，即将一个坐标系变换到了另一坐标系，重复上述过程，并将获得的每个变换矩阵右乘，即可实现 SCARA 一系列连杆之间的转换， 其转换通式如下：

(1)

2.1：机器人运动学正解

SCARA 运动学正解，根据给定的机器人各杆件的几何参数和关节变量来求解末端作业工具相对于基础坐标系的位姿和姿态，且具有唯一解。即从基座开始，在已知四个关节的增量值的情况下求解出 SCARA 末端位置和姿态的过程。通过 D-H 参数表，我们可以获得两两关节之间的坐标 变换矩阵，然后将获得的矩阵依次右乘，最终，就可以得到 SCARA 的位姿矩阵方程： ,其中：

(2)

(3)

(4)

(5)

以上分别求出四个关节的连杆变换矩阵，然后通过数学知识将四个变换矩阵相乘。可以得到末端工具的正运动学方程，也就是求出了在给定关节参数的情况下，机器人末端作业工具在空间坐标中的位姿。

于是总的齐次变换矩阵为：

( ,  (6)

通过齐次变换矩阵式(6)可得,运动学正解求解末端位置公式如下：

(7)

(8)

(9)

末端姿态仅与 有关。

2.2：机器人运动学逆解

与正运动学相反，机器人逆运动学是：给定了机器人末端作业工具在空间坐标系的位姿，推导末端在这个位置时其余的关节D-H坐标系中相应的位姿。机器人逆运动学是运动学中最为重要的部分。由于逆运动学解的不唯一性，其也是机器人轨迹规划的基础。实际中可以根据逆运动学的求解来调节驱动电机，满足关节所需的条件，是机器人末端作业工具到达指定的位姿。对逆运动学的求解可以通过解析法、代数法、几何法等方法求解。基本求解过程为：

联立运动学正解公式(7)和(8)可以消去 和 得到如下公式：

(10)

令 ， ， ，可解得：

(11)

由式(7)(8)得：

(12)

解得：

(13)

3  代码与结果分析

我们进行了scare机器人正逆运动学构建，以型号为KR 6 R500 Z200的KUKA SCARA机器人为例，并查询KUKA官网网站获取了详细参数，如下图所示：

  KR 6 R500 Z200的KUKA SCARA机器人参数

我们首先用matlab rvctools进行机器人运动学仿真，并打开示教界面，如图8。在示教界面中，可以用关节变量控制示教点，由此进行运动学正逆解的仿真验证。

%% scara机器人工具箱建模
clc
clear ;
L1=Link([0 0 0 0 0],'modified');
L2=Link([0 0 225 0 0],'modified');
L3=Link([0 0 275 0 1],'modified');%移动关节最后一个参数为1
L4=Link([0 0 0 0 0],'modified');
Robot=SerialLink([L1 L2 L3 L4],'name','Scara');
Robot.qlim=[-132*pi/180 132*pi/180;-145*pi/180 145*pi/180;-200 0;-355*pi/180 355*pi/180];%KR 6 R500 Z200关节限制
q1=[0 0 0 0];
%Robot.plot([0 0 0 0], 'workspace',[-400 400 -400 400 -300 300]);%空间范围定义
T=Robot.fkine(q1);
Robot.plotopt = {'workspace',[-500 500 -500 500 -500 500],'tilesize',300};
Robot.teach();
disp(T)

我们将    代入(6)式求正解，得到齐次变换矩阵为：

            

即：

%% 正运动学
clc;
clear;
L1 = 225.0; %mm
L2 = 275.0; %mm
theta1 = input('输入theta1(rad)');
theta2 = input('输入theta2(rad)');
d3 = input('输入d3(mm)');
theta4 = input('输入theta4(rad)');
x = L1 * cos(theta1) + L2 * cos(theta1 + theta2);
y = L1 * sin(theta1) + L2 * sin(theta1 + theta2);
z = d3;
fi = theta1+theta2+theta4;
T=[cos(fi) -sin(fi) 0 x;
   sin(fi) cos(fi) 0 y;
   0 0 1 z;
   0 0 0 1];
disp(T);

该结果在matlab示教仿真验证通过，验证结果如图9所示。

  正解仿真验证结果

我们将位姿矩阵 代入式(6)(9)(11)(13)进行求逆解，得到结果为：

   或   。

%% 逆运动学
clc;
clear;
L1 = 225.0; %mm
L2 = 275.0; %mm
x = input('输入x');
y = input('输入y');
z = input('输入z');
gama = input('输入位姿，即末端相对于基座的z轴旋转角(rad)');
d3 = z;
k1 = 2.0 * y * L1;
k2 = 2.0 * x * L1;
k3 = x^2 + y^2 + L1^2 - L2^2;
temp = k1^2 + k2^2 - k3^2;
if temp < -eps
    disp('区域不可到达');
    return;
elseif temp < eps
    disp('奇异点');
else
    %do nothing
end
theta1 = zeros(2, 1);
theta1(1) = atan2(k1 * k3 - k2 * sqrt(temp), k2 * k3 + k1 * sqrt(temp));
theta1(2) = atan2(k1 * k3 + k2 * sqrt(temp), k2 * k3 - k1 * sqrt(temp));
s1 = sin(theta1);
c1 = cos(theta1);
theta2 = atan2(y - L1 * s1, x - L1 * c1) - theta1;
theta4 = gama-theta1-theta2;
for i = 1 : 2
    if theta2(i) > pi
        theta2(i) = theta2(i) - 2.0 * pi;
    end
    if theta2(i) < -pi
        theta2(i) = theta2(i) + 2.0 * pi;
    end
end
xx = L1 * cos(theta1) + L2 * cos(theta1 + theta2);
yy = L1 * sin(theta1) + L2 * sin(theta1 + theta2);
disp(['输出(°):',num2str(theta1(1)*180/pi), ',', num2str(theta2(1)*180/pi), ',' ,num2str(d3), ',', num2str(theta4(1)*180/pi), '; ',newline, ...
num2str(theta1(2)*180/pi), ',', num2str(theta2(2)*180/pi), ',' ,num2str(d3), ',', num2str(theta4(2)*180/pi)]);

该结果已在matlab示教仿真验证通过，验证结果如图10，图11所示。

 反解仿真验证结果1

  反解仿真验证结果2

SCARA 机器人的工作空间是指其末端所能达到的所有点的集合。工作空间的求解可以清楚的知道 SCARA 所不能到达的地方，可以避免在做规划或者进行任务作业时，进入到机器人死区。

SCARA 是平面型机器人，所以其在一个平面中所有点的集合加上第三个关节的竖直位移就是它的整个工作空间，因为每个工作点的竖直位移是一样的，故只需对 XY 平面上求解其运动范围即可。

我们编程用逆解的方法求解了SCARA机器人的工作空间，关节限制参考了图7中KR 6 R500 Z200的轴数据。我们通过枚举x和y的取值，利用工作空间判断变量 来解算工作空间，工作空间判断变量即 。当 时，区域不可到达；当 时，给定位置是奇异点。

%% 工作空间求解
% 设置步长和范围
step = 5;
x_range = [-500 500];
y_range = [-500 500];
L1=225.0;
L2=275.0;
Robot.qlim=[-132*pi/180 132*pi/180;-145*pi/180 145*pi/180;0 200;-355*pi/180 355*pi/180];%KR 6 R500 Z200关节限制
% 初始化可到达点列表
reachable_points = [];
% 枚举x和y的取值
for x = x_range(1):step:x_range(2)
    for y = y_range(1):step:y_range(2)
        % 计算temp值
        k1 = 2.0 * y * L1;
        k2 = 2.0 * x * L1;
        k3 = x^2 + y^2 + L1^2 - L2^2;
        temp = k1^2 + k2^2 - k3^2;
        
        % 如果temp大于0，说明有解
        if temp > 0
            % 计算解集
            theta1 = zeros(2, 1);
            theta1(1) = atan2(k1 * k3 - k2 * sqrt(temp), k2 * k3 + k1 * sqrt(temp));
            theta1(2) = atan2(k1 * k3 + k2 * sqrt(temp), k2 * k3 - k1 * sqrt(temp));
 
            s1 = sin(theta1);
            c1 = cos(theta1);
            theta2 = atan2(y - L1 * s1, x - L1 * c1) - theta1;
            for i = 1 : 2
             if theta2(i) > pi
            theta2(i) = theta2(i) - 2.0 * pi;
             end
            if theta2(i) < -pi
            theta2(i) = theta2(i) + 2.0 * pi;
            end
            end
 
            % 判断四个关节角度是否在范围内
            if ((theta1(1) <= Robot.qlim(1,2) && theta1(1) >= Robot.qlim(1,1)) || (theta1(2) <= Robot.qlim(1,2) && theta1(2) >= Robot.qlim(1,1))) && ...
            ((theta2(1) <= Robot.qlim(2,2) && theta2(1) >= Robot.qlim(2,1)) || (theta2(2) <= Robot.qlim(2,2) && theta2(2) >= Robot.qlim(2,1)))
             
            reachable_points = [reachable_points; x y];
            end
        end
    end
end
% 绘制工作空间
scatter(reachable_points(:,1), reachable_points(:,2), '.');
axis equal;

SCARA机器人工作空间平面投影效果图如图所示：

  SCARA机器人工作空间平面投影效果图