六自由度机械臂运动学旋量逆解及简化算法*

刘国平，杨先永，钟飞飞，王大海

(南昌大学机电工程学院，南昌 330031)

0 引言

机器人的运动学是机器人控制的基础，其包括正运动学和逆运动学两个部分；当前国内外运动学研究主要采用D-H参数法和旋量法[1-2]。相比于传统的D-H参数法，旋量法具有不需要建立局部坐标系因此避开了局部坐标系带来的奇异性问题，并且能够很好的区分多解和具有明显的几何意义等优点[3-5]，所以越来越多的学者将旋量引用到机器人的研究中。

文献[6]将paden-kahan子问题和消元法相结合解决了后3关节交于一点的六自由度逆解问题。文献[7]将吴方法(吴方法是我国数学家吴文俊于20世纪70年代提出的处理多项式代数问题的一种数学方法)引入6R机器人的逆运动学问题中来，解决旋量算法子问题的限制。文献[8]将希尔维斯特(sylvester)结式法与旋量理论相结合来求取6自由度机器人的逆解，摆脱了对paden-kahan子问题的依赖性。文献[9]针对前3关节轴线均不相交，后3关节相交于一点的六自由度机器人，提出了几何法和旋量法相结合的运动学逆解求解算法。文献[10]针对前3关节不相交，后3关节相交构型的“钱江一号”机器人提出了一种新的运动学逆解子问题算法。文献[11]针对工业机器人提出了一类子问题，并以IRB1400弧焊机器人为例验证算法的正确性。文献[12]提出了新的旋量子问题改进一类前3关节均不相交的六自由度运动学逆解求解方法。

综合上述文献，当前旋量理论在逆运动学求解上的研究还存在以下问题：①多数研究主要针对后3关节交于一点构型的六自由度机械臂，缺少旋量理论解决前3关节交于一点的六自由度机械臂逆解研究。②仅仅依靠传统的旋量理论和panden-kahan子问题来求取逆解，其过程和结果还不够简洁。

本文针对前3关节交于一点的六自由度机械臂提出一种旋量逆解算法，并采用几何法及欧拉角对算法进行简化改进。

1 基于旋量理论的运动学模型

1.1 旋量描述刚体运动

欧拉定理说明，任何一个刚体的旋转运动都可以用一个旋转矩阵R来描述，该旋转矩阵R可以表示成转角θ和转轴的单位矢量ω的函数：

(1)

(2)

根据Chasles定理，任何刚体运动都可以拆分为沿着某直线的移动和旋转来表示，可以用如下的指数积形式来表示。

(3)

当工具坐标系{T}相对于参考坐标系{S}的初始位形为gST(0)时，绕某轴螺旋运动的刚体，{T}相对于{S}的位姿可以表示为gST(θ)：

(4)

1.2 机械臂正运动学建模

图1和图2分别为六自由度机械臂的三维模型图和结构示意图，其6个关节均为旋转关节，前3关节和5、6关节轴线分别交于一点，第1和第2关节、第2和第3关节、第5和第6关节始终垂直，第4和第5关节异面垂直。

图1 三维模型图

图2 机械臂结构示意图

选取初始位形如图2所示，此时第1与第3关节垂直。在此初始位形下末端工具坐标系{T}在惯性坐标系{S}中的变换为：

(5)

根据POE公式，可以得到六自由度机械臂的正运动方程：

(6)

2 机械臂逆运动学建模及简化算法

2.1 基于panden-kahan子问题的逆解

机械臂的运动学逆解是已知末端的期望位置gST(θ)，求出该位置下对应的各关节角度。旋量逆解的基本思想就是将复杂的机器人运动学逆解，拆分成多个具有几何意义明确的逆解子问题，当前公认的3个最基本子问题为paden-kahan的3个子问题[13]。

该六自由度机械臂是前3关节和5、6轴线分别交于一点，第4关节偏移中心轴线一定距离并与第5关节异面垂直；这样的结构不能做简单的拆分，所以不能直接利用paden-kahan的子问题求得逆解；这里我们需要利用前3关节和5、6关节分别相交一点的条件来消去其中的部分变量，才能将其转化成相应的子问题，该结构机械臂的旋量运动学逆解具体求解如下。

(1)求解θ4

因为1、2、3关节和5、6关节轴线分别相交于一点，利用这一条件我们可以将除θ4以外的变量消去，这样可以优先求得θ4。

记机械臂的前3关节交于点p，p=q2=q3，第5和第6关节交于点q5。

(7)

第5、6关节轴线交于q5，转动后位置不变，

(8)

式(7)两端同时右乘q5有：

(9)

前3关节交于点p，有：

(10)

上两式相减有：

(11)

在刚体旋转的过程中，其上两点之间的距离始终保持不变，并对上式两端取范数：

(12)

那么θ4求解变成旋转到指定距离的子问题3。

(13)

(2)求解θ5、θ6

由式(7)可得：

(14)

(15)

所以有：

(16)

以上问题转变成绕两相交轴旋转的子问题2。

(17)

可以得到：

(18)

以上结果可以应用子问题1求得θ5、θ6：

(19)

(3)求解θ1、θ2，

(20)

以上结果可以应用子问题2进行求得θ1、θ2，

(21)

可以得到：

(22)

以上结果可以应用子问题1求得θ1、θ2，

(23)

(4)求解θ3，

由式(7)可得下式，并记已知量为g3：

(24)

(25)

以上结果可以应用子问题1求解得θ3：

(26)

至此，已经求解得到基于旋量六自由度机械臂的所有运动学封闭逆解。其中关节4有2组解，关节5与关节6组合有2组解，关节1与关节2组合也有2组解，所以该构型的机械臂共有8组解。

2.2 结合几何法及欧拉角简化前4关节角求解

2.2.1 几何法简化θ4的求解

图3 第4关节几何关系图

LBD2=LBC2+LCD2-2LBCLCDcos(π-β)

(27)

(28)

2.2.2 欧拉角简化θ1,θ2,θ3的求解

在求得θ4,θ5,θ6的基础上，可以通过以下关系求解θ1,θ2,θ3。

(29)

由于前3关节交于一点，第1关节与第2关节、第2关节与第3关节始终垂直，在初始位形下3个关节两两垂直，且3个轴都是纯旋转关节，在工具坐标系{S}下，θ1、θ2、θ3分别依次绕工具坐标系{S}的Z轴、X轴、Y轴转动后的结果，可以等价为绕Z-X-Y顺序的欧拉角变换结果：

(30)

其中，c=cos,s=sin,1=θ1,2=θ2,3=θ3。

由式(30)、式(31)，通过以下关系可以求得θ1,θ2,θ3。

(31)

如果利用旋量理论和几何法及欧拉角相结合来求解机械臂逆解，从上面可以看到，运动学逆解的求解过程和求解结果将得到很大简化。

3 逆运动学实例验证

六自由度机械臂的各个关节杆件参数如下：

d1=129.5 mm，l1=260.5 mm，a1=44 mm，l2=256.12 mm，l3=0 mm。

(1)任取一组不在奇异位形上的一组关节角，[θ1θ2…θ6]=[-66°55°-13°25°4°-10°]，将其带入机械臂的正解公式中去，得到该组关节角对应末端姿态矩阵如下：

(32)

(2)利用前面推导的旋量逆解算法进行求解，得到机械臂各组关节角如表1所示，各组逆解位形如图4所示。

表1 8组运动学逆解

续表

(a) 第1、2组解位形 (b) 第3、4组解位形

(c) 第5、6组解位形 (d) 第7、8组解位形图4 8组逆解位形图

表1的结果与前面分析一致，一共8组解，从上表可以看出，第8组解与我们取定的关节角一模一样，1和2、3和4、5和6、7和8分别对应同一位形，8组解总共对应4种位形。

(3)再将以上8组解的结果再次带入机械臂的正运动学公式中去，得到各组解对应的末端姿态矩阵，可以看到以上8组解对应着几乎相同的末端姿态矩阵，这里再一次证明该算法的正确性；将得到的8组解对应的末端矩阵与给定角度对应的末端矩阵做差，结果看作各组解的误差，其中第4组解的误差最大，为：

(33)

最大误差在10-13数量级，该算法具有非常高的精度。实际应用中，需要对求得的多组解进行筛选，要求各个关节要在机械臂的运动范围内、避开机械臂的各个奇异位形和工作范围内的障碍物等，再根据运动行程最短等寻优条件去选择一组最优解。

(4)同样的，取式θ5=4°,θ6=-10°和式(32)的末端姿态，带入简化算法中可以得到：[θ1θ2θ3θ4]=[-66°55°-13°24.992 2°]，该结果与给定关节角几乎一样，验证了简化算法的正确性。

4 结束语

(1)结合旋量理论和已知的paden-kahan子问题，推导出前3关节、第5、6关节分别相交于一点，第4关节偏移的六自由度机械臂运动学逆解。该方法相对于传统的D-H法，其几何意义明显，过程和结果都更加简洁，并且算法精度能够达到10-13数量级，有非常高的精度，能够很好的满足各种应用的控制需求。

(2)采用欧拉角及几何法对前3关节和第4关节运动学逆解算法进行简化改进，应用旋量理论、欧拉角和几何法相结合求取运动学逆解，能够极大的简化过程和结果。

(3)本文提出的方法适用于其他类似构型，特别是前3关节交于一点构型机械臂的运动学逆解，具有一定通用性。