6R关节式机器人运动学仿真分析

周群海，王兆辉

(山东大学 机械工程学院 机械工程国家级实验教学示范中心，济南 250061)

0 引言

机器人的运动学是机器人运行控制和轨迹规划中的重要基础，其实时性能对于机器人的控制系统具有极为重要的意义。运动学的研究内容主要为运动学正、逆解问题[1]。运动学正解问题是已知机器人的各个关节变量和连杆参数，求解机器人末端执行器的位置和姿态。而运动学逆解问题则正好相反，是给定机器人末端执行器的位姿，计算相对应位姿的各个关节变量。

当前，机器人运动学算法多种多样，但主要可分为解析法和数值法两类。解析法有几何法[2]、矩阵法[3]和矢量代数法[4]等，其优点是可以得到所有的解析解，但是计算较为困难，方法通用性不强。而数值法[5]则是通过对约束方程组进行迭代运算，从而求解得出机器人的数值解，其优点是算法更具有通用性，适用于求解冗余自由度机器人逆运动学。

本文是运用D-H建模方法，依据机器人的结构特征建立相对应的约束方程组，而后从方程组中消去中间参数，得到单参数方程后最终求解，并精确推算运动学正逆解算法以及分析机器人的奇异状态位置，与常规的逆解算法相比，该算法只需进行一次矩阵逆乘，从而使计算过程大为简化。在Matlab仿真软件中的Robotics Toolbox工具箱环境下[6]，对PR05型号机器人建立虚拟三维模型，并在关节空间中采用多项式插值法对空间任意两点进行轨迹规划[7]，从而对机器人运行过程中的性能参数和运动轨迹进行仿真分析，同时证明在Matlab中所建立的机器人运动学模型的正确性和正逆解算法的有效性。

1 机器人D-H方法建模

目前运用较为广泛的机器人建模方法是由Denavit和Hartenberg于1955年提出的D-H建模方法[8]，主要是用来描述机器人相邻两个连杆之间相对平移和转动的位置关系。D-H建模方法是在机器人的每个连杆上都各自设立一个坐标系，并通过相对应的齐次变换矩阵来表示各个坐标系的相对位置和姿态，而后使用递归相乘的方法得到机器人的末端执行器相对于基坐标系的齐次变换矩阵。

本研究所使用的机器人为国内某公司研发的PR05型号机器人，该型号机器人是一种6自由度关节式串联机器人，主体的关节机构由基座、腰部、大臂、小臂和腕部等部分组成，相邻两个关节之间都通过转动关节连接。根据PR05型号机器人的结构构型如图1所示，遵从D-H建模方法，以如下原则建立坐标系：①确定Zi轴，Zi轴沿关节i+1的轴向；②确定点Oi，Oi在过Zi-1和Zi轴的公法线上；③确定Xi轴，Xi轴通过Zi-1和Zi轴的公法线方向，从Zi-1指向Zi；④确定Yi轴，Yi=Zi×Xi，使坐标系为右手坐标系。

图1 PR05型号机器人结构构型

分析PR05型号机器人的关节机构特性，按照D-H建模方法设立机器人运动学坐标系模型，如图2所示[9]。而相对应的各连杆参数如表1所示。其中ai-1表示相邻两个关节之间的连杆长度，αi-1表示相邻两个坐标系之间的扭转角度，di表示相邻两个关节之间的连杆偏移量，θi表示第i个关节的角度值，在各参数之中只有θ为变量，其余都为常量[10]。

图2 PR05型号机器人运动学坐标系

iai-1/mmαi-1/(°)di/mmθi/(°)100213.5020-90182.5-903360180176.5043150125-9050-9012518060-9000

2 机器人运动学算法研究

2.1 运动学正解

根据所设立的各个连杆坐标系和相对应连杆的D-H参数，可建立PR05型号机器人运动学方程为：

(1)

(2)

将表1中的D-H参数代入式(2)，可得到各个连杆之间的齐次变换矩阵为：

由此，可以求出：

(3)

为简化公式，使用si代表sinθi，ci代表cosθi，s4+3-2代表sin(θ4+θ3-θ2)，c4+3-2代表cos(θ4+θ3-θ2)，故式(3)中：

nx=s6c1s4+3-2-c6(s1s5-c5c1c4+3-2)

ny=s6s1s4+3-2+c6(c5s1c4+3-2+c1s5)

nz=c5c6s4+3-2-s6c4+3-2

ox=s6(s1s5-c5c1c4+3-2)+c6c1s4+3-2

oy=c6s1s4+3-2-s6(c5s1c4+3-2+c1s5)

oz=-c6c4+3-2-c5s6s4+3-2

ax=-c5s1-s5c1c4+3-2

ay=c1c5-s5s1c4+3-2

az=-s5s4+3-2

px=s1(d4+d3-d2)+a3c1c3-2-d5c1s4+3-2+a2c1c2

py=c1(d2-d3-d4)-d5s1s4+3-2+a3s1c3-2+a2c2s1

pz=d1-a2s2+d5c4+3-2+a3s3-2

2.2 运动学逆解

当机器人的连杆结构符合Pieper准则，即该机器人的三个相邻关节轴交于一点或者三轴线平行时，就可使用封闭解法来求取一定数量的运动学逆解，对于6自由度机器人来说，最多可有8组封闭解[11]。本研究中的PR05型号机器人的第2、3、4轴线相互平行且第4、5、6轴线相交于一点，故可求封闭解。

(4)

式中，

(5)

d2-d3-d4=-s1px+c1py

(6)

通过三角函数公式化简，可得θ1：

θ1=atan2(py,px)-atan2(M,±N)

(7)

得到θ1后，继续求解θ5和θ6，考虑式(5)左右两边矩阵元素(2,1)，(2,2)和(2,3)相等，可获得关于θ5和θ6的方程：

c6s5=-s1nx+c1ny

(8)

-s5s6=-s1ox+c1oy

(9)

c5=-s1ax+c1ay

(10)

联立式(8)和式(9)，求其平方和可消去变量θ6，从而得到：

(11)

再联立式(10)和式(11)，由反正切函数atan2可得到θ5：

θ5=atan2(s5,-s1ax+c1ay)

(12)

由式(8)和式(9)分析可得：当s5=0，即θ5=0或θ5=π时，机器人处于奇异状态，无法求得θ6；而当θ5≠0或θ5=π时，可以求得θ6：

(13)

然后考虑式(5)左右两边矩阵中元素(1,3)和(3,3)相等，可获得如下方程：

-s5c4+3-2=c1ax+s1ay

(14)

-s5s4+3-2=az

(15)

此时，由式(14)和式(15)分析可得：当s5=0，即θ5=0或θ5=π时，机器人处于奇异状态，无法求得θ4+θ3-θ2；而当θ5≠0或θ5≠π时，可以求得θ4+θ3-θ2。从而得到:

(16)

考虑式(5)左右两边矩阵中元素(1,4)和(3,4)相等，可获得如下方程：

a2c2-d5s4+3-2+a3c3-2=c1px+s1py

(17)

d5c4+3-2-a2s2+a3s3-2=pz-d1

(18)

再联立式(17)和式(18)，消去变量θ3-θ2，可得到：

(19)

式中，令c1px+s1py+d5s4+3-2=E，

pz-d1-d5c4+3-2=F，A=2a2E,B=2a2F，

C=Ac2-Bs2

(20)

从而可以求出θ2：

(21)

在求得θ2后，再次联立式(17)和式(18)，从而可求出θ3-θ2：

(22)

故由式(22)可得到θ3：

(23)

最后由式(16)和式(22)可得到θ4：

(24)

至此，关节变量θ1～θ6全部求出，从而完成机器人运动学逆解求解过程。

2.3 机器人奇异状态分析

当机器人处于奇异状态时[12]，无法求取运动学逆解，即不能使用逆解算法将末端坐标转化为关节变量，并且此时末端坐标的微小波动将会导致关节变量的剧烈变化，故在机器人控制时需要明确了解其奇异状态的具体位置，从而避免机器人运动控制失效，无法达成预期目的。

PR05型号机器人奇异状态位置分析：

(2)关节2处奇异状态位置。当式(21)中的A2+B2-C2=0时，不能求取θ2，此时z2与z4z5平面共面，机构奇异。

(3)关节6处奇异状态位置。当式(13)中的s5=0时，即θ5=0或θ5π，不能求取θ6，此时z4与z6平行，机构奇异。

3 机器人Matlab建模仿真分析

Matlab仿真软件中的Robotics Toolbox工具箱含有机器人学研究所需要的运动学、动力学以及轨迹规划等方面的功能函数[13]，运用该工具箱不仅可进行机器人运动学的正逆解运算，还能够对机器人进行图形仿真和轨迹规划，以便观察研究其整体运动过程。本次仿真分析使用的软件配置为：Matlab(R2014a)和Robotics Toolbox for Matlab(release 8)。

3.1 机器人运动学建模

在Robotics Toolbox工具箱中使用link和robot函数建立PR05型号机器人仿真模型，并用plot函数显示出该机器人的三维空间模型，编制相关程序可得到PR05型号机器人三维空间模型如图3所示。

图3 PR05型号机器人三维空间模型图

同时可以调用drivebot函数打开PR05型号机器人的关节驱动器，关节驱动器如图4所示，可通过手动调节滑块来改变机器人的各个关节变量，从而得到机器人的不同位姿。

图4 PR05型号机器人关节驱动器图

3.2 机器人运动学仿真

在Robotics Toolbox工具箱中使用fkine正解函数和ikine逆解函数对PR05型号机器人的运动学正逆解进行仿真计算，并验证前文所提出的正逆解算法。

3.2.1 运动学正解仿真

假设PR05型号机器人的6个关节变量分别为pi/6、pi/5、pi/4、pi/3、pi/2、pi，即设向量q=[pi/6 pi/5 pi/4 pi/3 pi/2 pi]，使用fkine正解函数编制相关程序如下：

q=[pi/6 pi/5 pi/4 pi/3 pi/2 pi]；

q=0.5236 0.6283 0.7854 1.0472 1.5708 3.1416

Z=fkine(PR05,q)；

Z= 0.5000 -0.8085 -0.3104 0.4801

-0.8660 -0.4668 -0.1792 0.1398

-0.0000 0.3584 -0.9336 0.0960

0 0 0 1.0000

Z值即为PR05型号机器人在关节变量为q时的末端执行器位姿。且经过验证，所得末端位姿与式(3)计算得到的位姿相同，从而验证了Matlab中所建立机器人运动学模型的正确性以及正解算法的有效性。

3.2.2 运动学逆解仿真

假设PR05型号机器人末端执行器的位姿为Z,反求机器人各个关节变量的角度。

使用ikine逆解函数编制相关程序如下:

N=ikine(PR05,Z);

N=0.5236 0.6283 0.7854 1.0472 1.5708 3.1416

N值即为PR05型号机器人在末端位姿为Z时的各个关节变量的角度。且经过比较N=q，从而再次证明了在Matlab中建立的机器人运动学模型正确以及逆解算法的有效性。

3.3 机器人轨迹规划及仿真

机器人的轨迹规划问题是指在工作任务需求下，对机器人运行过程中的位移、速度和加速度等性能参数进行仿真分析，从而得出符合预期目标的运动轨迹。机器人轨迹规划中常用的规划方式主要有关节空间轨迹规划和笛卡尔空间轨迹规划两类[14]。这两类轨迹规划方式各有其优缺点，其中关节空间方式相对容易，运算量较少且通常不会进入奇异状态位置，但其末端执行器的运动轨迹不易观察，而笛卡尔空间方式相对而言较直观，但缺点是在运动过程中机器人容易出现奇异状态位置，并且其运动学反解相对复杂。本研究是为验证在工作任务需求下PR05型号机器人运行过程中的性能表现，只研究机器人末任意端两点间运动的动力学性能，与具体的运动轨迹无关，故采用关节空间轨迹规划方式。

关节空间轨迹规划[15]是将机器人各个关节转动角度作为变量，再使用合理的多项式进行插值处理，从而生成运动轨迹。为使生成的机器人运动轨迹光滑平稳，采用5次多项式对空间中任意两点间进行插补处理，并使用Robotics Toolbox工具箱中的jtraj和ctraj函数进行关节空间轨迹规划仿真。

设空间中任意两点的关节值分别为q0=[0 0 0 0 0 0]和q1=[pi/6 pi/5 pi/4 pi/3 pi/2 pi]，机器人仿真时间为5s，采样间距为0.025s，编制轨迹规划仿真程序使得机器人末端执行器位置从q0点移动到q1点，从而得到PR05型号机器人末端执行器位置以及各个关节的角位移仿真曲线，如图5和图6所示。

图5 PR05型号机器人末端执行器位置仿真曲线

图6 PR05型号机器人各个关节角位移仿真曲线

将相关程序中的运动学参数修改后再次运行，就可得到机器人各个关节的角速度和角加速度的仿真曲线，如图7和图8所示。

图7 PR05型号机器人各个关节角速度仿真曲线

图8 PR05型号机器人各个关节角加速度仿真曲线

通过Robotics Toolbox工具箱中的函数也可得到机器人末端运动路径和运动过程，如图9和图10所示。

图9 PR05型号机器 人末端运动路径图

图10 PR05型号机器人运动仿真过程

3.4 仿真结果分析

通过仿真结果可知，在关节空间中采用5次多项式插补对空间任意两点之间进行轨迹规划仿真，PR05型号机器人的各个关节角位移、角速度以及角加速度等动力学性能参数仿真曲线光滑平稳，无间断、跳跃和拐点等情况。由图10可观察到机器人的各个关节在仿真过程中运行正常，各个连杆之间没有发生错位碰撞的现象，从而验证了本文中D-H连杆参数设置的合理性以及运动学算法的正确性，并且说明该机器人能够完成预定的工作任务需求。

4 结论

本文根据D-H参数法对PR05型号6自由度关节型机器人进行了参数设置，分析了该机器人的结构类型满足其存在运动学封闭解的要求，并基于机器人的结构特点构建了约束方程组，得到了一种6自由度机器人运动学逆解算法，与常规的运动学算法相比，该算法只需进行一次矩阵逆乘，从而使计算过程和运算量简化，而后分析得到了该机器人的奇异状态位置，可作为以后机器人控制和奇异状态处理的基础。运用了Matlab仿真软件中的Robotics Toolbox工具箱模块建立了该机器人的仿真模型，调用了相关功能函数对机器人的关节空间轨迹规划进行仿真分析，得到了机器人各个关节的仿真曲线以及末端执行器的运动轨迹数据，并验证了各个关节角位移、角速度以及角加速度等动力学参数仿真曲线平稳无振动，符合正常运动的状况，为今后的机器人实时控制和研究规划打下基础。