基于一次逆乘法的机器人运动学分析

闵柏成，杜康健，张世超

(中国船舶集团有限公司第八研究院，江苏 扬州 225101)

0 引 言

机器人是机电一体化最高成就的体现，处于科技发展的前沿[1-2]。机器人技术包括了多种学科和领域的最新研究成果，汇集了机械工程、计算机技术、电子技术、人工智能及自动控制理论等[3]。机器人运动学是分析机器人控制的基础。本文以FANUC-M10i六自由度机器人为研究对象，利用D-H法建立机器人的坐标系并以一次逆乘法进行正逆运动学算法的推导。推导出的运动学算法与MATLAB机器人工具箱进行对比以验证其正确性[4]。

1 机器人D-H坐标系的建立

FANUC-M10i型机器人具有6个自由度，如图1所示。根据其结构建立各连杆坐标系，其中4、5、6系均在腕部，如图2所示[5]。机器人的6个关节都是旋转关节，由D-H法建立连杆坐标系，建立连杆参数表如表1所示，其中连杆参数可定义如下：

(1)ai-1表示从zi-1到zi沿xi-1测量的距离；

(2)αi-1表示从zi-1到zi绕xi-1旋转的角度；

(3)di表示从xi-1到xi沿zi测量的距离；

(4)θi表示从xi-1到xi沿zi旋转的角度。

坐标系{i-1}与{i}系之间的位姿关系代表连杆之间的变换，可以用连杆参数来表示。一个参数可以用一种简单的变换来表示，4种简单的变换就可以组成连杆坐标系之间的变换。得到的连杆变换公式如下：

图1 M10I型机器人结构简图

图2 连杆坐标系图

表1 M-10i型机器人关节坐标及参数

(1)

2 正运动学分析

根据上述FANUC-M10i型机器人的连杆坐标系图与参数表，由连杆变换公式可以进行连杆坐标系{i}相对于坐标系{i-1}之间关系的推导，推导结果可以表示如下：

(2)

其相邻两连杆的变换结果可以表示为：

(9)

式中：si=sinθi;ci=cosθi。

根据上述结果可得：

(11)

求得表示姿态的公式：

nx=-s1(c5c6s4+c4s6)+c1(c6c23s5+

c23(c4c5c6-s4s6))

(12)

ny=c6s1s23s5+c1(c5c6s4+c4s6)+

c23s1(c4c5c6-s4s6)

(13)

nz=-c4c5c6s23+c23c6s5+s23s4s6

(14)

ox=c6(c4s1+c1c23s4)+s6(-c5s1s4+

c1(c23c4c5+s23s5)),ox=-ox6

(15)

oy=c1(-c4c6+c5s4s6)+s1(s23s5s6+

c23(c6s4+c4c5s6)),oy=-oy6

(16)

oz=-c6s23s4+s6(-c4c5s23+c23s5),oz=-oz6

(17)

ax=-s1s4s5+c1(-c5s23+c23c4s5),ax=-ax6

(18)

ay=-c5s1s23+(c23c4s1s5+c1s4s5),ay=-ay6

(19)

az=-c23c5-c4s23s5,az=-az6

(20)

求得表示位置的公式：

px=-L7s1s4s5+c1(a1+a2c2-(d4+L7c5)s23+

c23(a3+L7c4s5))，px6=px-L7ax

(21)

py=L7c1s4s5+s1(a1+a2c2-(d4+L7c5)s23+

c23(a3+L7c4s5))，py6=py-L7ay

(22)

pz=-d4c23-s2(a2+L7c5s3+c3(a3+L7c4s5))+

c2(-L7c3c5+s3(a3+L7c4s5))，pz6=pz-L7az

(23)

式中：s23=sin(θ2-θ3)；c23=cos(θ2-θ3)。

3 逆运动学分析

逆运动学分析是在知道机器人末端位置和姿态的情况下，通过一系列的算法推导得出对应的机器人各个关节角的过程。机器人逆运动学求解是一大难题，通常在机器人末端运动空间内给定机器人末端位置和姿态的情况下，机器人各个关节转角的求解结果会有多种[6]。多解的选择和优化直接关系到最终机器人运动轨迹的可行性。本文以FANUC-M10i机器人为例，采取逆运动学求解方法，这样避开了繁琐的多次矩阵逆运算，在明确正运动学方程的基础上加以变换，只需要1次求逆就可以得到所对应转角的值。

具体计算过程：第1步将式(10)两边左乘一个逆矩阵进行变换，然后利用变换后等式两边对应元素一致建立方程，最后用建立的方程求解，把关节变量分离出来。将式(10)两侧左乘逆矩阵得：

(24)

由式(24)中两侧第3行第4列对应一致求解得：

θ1=atan2(-ayd7+py，±(axd7-px))

(25)

由式(24)两侧第1行第4列和第2行第4列元素对应一致求解得：

(26)

u=a1+(axd7-px)c1+ayd7s1-pys1

(27)

(28)

v=azd7-pz

(29)

由等式(24)两侧第1行第4列和第2行第4列元素对应一致求解得：

θ3=atan2(m1，n1)

(30)

m1=[(-a2-uc2+vs2)d6-

(-vc2-us2)a3]/(a32+d62)

(31)

n1=(-a2-uc2+vs2)a3+

(-vc2-us2)d6/(a32+d62)

(32)

由式(24)两侧第1行第2列和第2行第2列元素对应一致求解得：

(33)

将式(33)两侧先同时乘以c3和s3，其次作差，最后化简得：

(34)

式中：h=-s23(axc1+ays1)-azc23，且θ5≠0。

将式(33)两侧先同时乘以c3和s3，然后相加求和，最后化简得：

θ4=atan(m2,n2)

(35)

m2=(ayc1-axs1)/s5

(36)

n2=[(s2s3+c2c3)(axc1+ays1)+

az(c2s3-s2c3)]/s5

(37)

由式(24)两侧第3行第1列和第3行第3列元素对应一致求解得：

θ6=atan(m3,n3)

(38)

m3=c5s4(oyc1-oxs1)-c4(nxs1-nyc1)

(39)

n3=-c5s4(nys1-nyc1)+c4(oxs1-oyc1)

(40)

上述解的结果有多个，最终实际上只能选用1组解。首先得排除掉在机器人各个关节行程范围外的解，其次需要排除在奇异点位置的解，最后剩余的解需要根据实际情况来确定选取唯一解。

4 MATLAB进行运动学方程验证

MATLAB中Robotics工具包是由Peter Croke团队开发的基于MATLAB平台的机器人运算、仿真的强大工具，可以作为验证相关机器人算法的工具[7]。为了验证推导出的正逆运动学算法的正确性，可以先用MATLAB进行编程计算；其次在MATLAB中应用Robotics工具包建立机器人模型，工具包可以自动进行相关正逆运动学解算并得出正确结果；最后将推导出来的算法对比MATLAB中的Robotics工具包，若结果一致，即可证明推导出来的算法是正确无误的。

(1) 正运动学方程验证

不妨取(θ1,θ2,θ3,θ4,θ5,θ6)=(10,20,30,40,50,60)，单位°。在MATLAB中输入算法进行运算得出末端位姿矩阵：

T=

在MATLAB机器人工具箱中建模后，输入各个关节转角，得出结果如图3所示。与T相对应，即证明了正运动学方程的正确性。

图3 机器人工具箱仿真

(2) 逆运动学方程验证

在MATLAB中写入逆运动学算法，输入矩阵T，得出表2所示结果。

表2 MATLAB逆解8组解

逆解有8组，由表2可知逆解3为对应的(10,20,30,40,50,60)逆解。细微的误差是因为T取有效数字时会省略一些位数造成的，对应同一个机器人末端位姿。

5 结束语

本文提出了用一次逆乘法推导出机器人的逆运动学算法的方法。选择了日本FANUC-M10I机器人为研究对象，用D-H法建立了机器人的连杆坐标系，计算得出各个相关参数，并用一次逆乘法推出正逆运动学算法；将推导出的算法在MATLAB中进行编程求解，并与机器人工具箱结果进行对比。结果表明1次逆乘法只需要1次矩阵求逆就可以正确推导出逆运动学方程，可以免除以往传统逆运动学推导所需的复杂多次的逆运算。