1P5R机械手运动学分析算法研究

倪振松;吴瑞坤;李立耀

【摘 要】为了研究1P5R(1个移动关节5个旋转关节)机械手新的建模情况,应用四元数和对偶四元数的矩阵形式,对空间1P5R机器人的位置正运动学和逆运动学进行分析,然后分两次采用Groebner基进行消元和降低次数,最后采用迪克逊结式进行消元,得到没有增根的一元16次方程,从而得出16组的解.这种与以前的方法有所不同,是算法的有益的补充.

【期刊名称】《福建师大福清分校学报》 【年(卷),期】2015(000)005 【总页数】6页(P29-34)

【关键词】1P5R机械手;正运动学分析;逆运动学分析;四元数;四元数矩阵 【作 者】倪振松;吴瑞坤;李立耀

【作者单位】福建师范大学福清分校电子与信息工程学院,福建福清350300;福建师范大学福清分校电子与信息工程学院,福建福清350300;福建师范大学福清分校电子与信息工程学院,福建福清350300 【正文语种】中 文 【中图分类】TH112

机器人的运动学逆问题是机器人控制的基础。尤其是在机器人的实时控制程序中，机器人的位置反解是基本的问题，许多学者在这方面做了大量的工作。研究逆运动学问题的方法有很多种，较为常用的有解析法和数值法，以数值法居多。Denavit

等提出了一种建立连杆坐标系的方法，并利用D-H 坐标系建立了机器人的运动学方程［1］。其后国内外很多机器人研究领域的学者都是利用D-H 参数建立了不同机器人的运动学和动力学方程［2］。由于5R串联机器人能完成焊接、装配等许多任务且结构比6R 串联机器人简单而引起广泛的兴趣［3］。文献［4］首先用解析法对5R 单开链的逆运动学进行研究，他们引入一个虚拟R副，使其转化为6R 封闭回路，并利用球面三角法给出了求解过程的轮廓。文献［5，6］ 利用D-H 矩阵给出了一般4R 单开链的解析解，同时给出具有2 解的4R 单开链的十种类型，又利用数值迭代方法给出了5R 单开链的数值解。文献［7］ 用矢量代数法对5R 单开链的解析解进行了研究，得到了8 解，又提出以唯一解为追求目标。在文献［6］ 的基础上，文献［7］ 增加了一个运动学方程，组成由4个原始运动学方程的方程组，并进行求解，得出5R 单开链为唯一解的结论。此种求解方法对某些5R 单开链有多解时可能失效，一解和多解难于统一于该方法。

本文提出了一套以四元数变换矩阵和对偶四元数矩阵运动学方程为核心的可以求解空间1P5R机器人位置反解的数学模型。通过对以D-H变换方法和用对偶四元数矩阵表示刚体转动与移动相结合的方法建立的数学模型，并用前人求解空间1P5R机器人位置反解［8］的结论论证了用四元数矩阵方法可以实现实时地获得精确数据的可行性。

1.1 D-H参数矩阵表示

为了便于进行机器人的位姿分析，Denauit和Hartenbery于1956年提出了一种机器人坐标系建立方法，该方法严格定义了每个坐标系的坐标轴，并对连杆和关节定义了4个参数：ai为连杆长度，αi为连杆扭角，di为两连杆距离，θi为两连杆夹角。其中ai和αi描述连杆i的结构信息，di和θi描述相邻两连杆的相对位置信息。

采用该方法建立的关节i的坐标系｛i｝相对于关节i-1的坐标系｛i-1｝的坐标变

换矩阵，即该运动过程可用矩阵形式表示为 其中前一个是关于轴的变换： 其中：

其中后一个是关于轴的变换： 其中：

1.2 逆运动学分析

对于一个1P5R机器人来说，要求出其末端位置，首先要知道其运动形式，1P5R机器S人总共 -1/有26个关节，其中有5个是旋转关节，一个是平移关节，1P5R医用机器人已经用于病人手术［9］。

正运动学：对于一个1P5R机器人，该种结构的关节式机器人本身有5个自由度，加上最后穿刺针的转动共6个自由度。其中第一个关节是滑动副，后5个关节都是旋转副。其结构如图2所示。

其中，d1，H2，H3，H4，H5，H6为该1P5R机器人的6个关节变量. 其运动学方程可以表示为

机器人末端执行器坐标系（即连杆坐标系｛6｝）的坐标相对于机身坐标系｛0｝的可以用上面推导出的四元数矩阵公式转化为：

式中：θi、si、αi、ai为6R机器人操作臂的结构参数；为分别绕Z轴X轴旋转且平移的四元数矩阵形式的变换。M是机器人末端位姿的四元数矩阵形式的表达式。 对一般1P5R机器人来说，未知量为iθ（i=1，2，…，6），其他量为由机器人结构所决定的已知参数。把分别写成

乔曙光［8］在他的硕士论文中，把（2）式左边的6个变量取出两个移到等式右边，然后用Dixon结式求解。本文把左边的三个变量移到右边，即把式（2）变成： 令式（3）左右两边分别为T7和T8，这里T7和T8也是一个矩阵形式的四元数，并通过Mathematic6.0软件计算可得到，可写成如下形式：

由u1i=v1i及u2i=v2i（i=1，2，3，4）（根据矩阵相等原则），并且令，j=1，2…6得到：

其中：ai和bi为机械手结构所决定的已知参数。 1.3 消去x4，x5和x6

令（5）式中方程的右边8项分别为： 或

代入（4）中求解yi。得到：

式中Ci，为机械手结构所决定的已知参数。

对式（7）组成的方程组，按照单项式的分次字典序x4＞x5＞x6排列，计算方程组Groebner基。采用计算机代数系统进行符号运算。得到21个基，其中有11个基中含有各个x4，x5，x6，故舍去，选另外10个关于yi次数较低的基，即二次的基，如下：

把式（7）中y1～y8分别代入式（8）中，两边都乘上得到关于x1,x2,x3均不超过4次的10个方程。其形式为：

对于（9）式，令再次应用计算机代数系统计算分次字典序Groebner基，可以把该式化简，具体步骤如下：

采用计算机代数系统Mathematica在Intel Pentium Ⅳ 2.93 GHz RAM 1 G 的PC机上，按照X1＞X2＞X3排列计算（9）式Groebner基，得到11个基，其中7个基形式如下：

另外还有4个基中含有X1，X2的次数较高，把它们舍弃。（10）式中各个系数eij分别为X3的4，3，1，3，2，2，1代数式。 对于上面导出的7个式子，可以写成矩阵形式 其中，

由代数学知，上面式子有解的条件是其系数行列式等于零，即

按照前面对X3次数的分析，矩阵N7×7关于X3的其总和等于16。可知，展开式（12）后得到关于X3的单变量多项式最高次数不会超过16。

根据式（12），不需要提取任何公因式，可以直接得到的关于变量X3的一元16次输入输出方程

其中，si是由输入参数确定的实系数。求解（12）式，将得到16个解。并利用公式：

可得到3θ的值。

将所求得的x3代入（9）式，取其中8个组成线性方程组，把其中作为假想变元，解此方程组，可求得对应于X1的X2的值。按式（14）形式即可解得对应的θ1和θ2。将其代入式（7）即可线性解得yi，再把yi代入到（6）式中，就可以容易求得x4,x5,x6的值。在通过式（14）形式 即可解得对应的θ4、θ5和θ6。 1P5 R串联机器人的结构参数如表1所示。通过以上参数计算出机器人的末端位姿四元数矩阵，最终得到16组关节角。反解结果见表2，其中4组实数解，第8组解与给定的初值是一样的，并且表2中的4组实数解通过式（3）进行正解验证与末端位姿四元数矩阵一致，为了验证这个算法的正确性，本文把这16组解重新代入到方程（1）中求出它的末端对偶四元数，求出结果如表3所示，从表3可以看出其末端四元数的实部都是一致的，虽然虚部有的会有点小误差，但是误差非常小，这样就可以忽略不计，从而证明了该算法的正确性。

在对矩阵形式的四元数的理论进行研究的基础上，将其应用在1P5R机器人的建模中，通过两次求解分次字典序Groebner基，第一次消去3个变量得到10个基，对这10个基的解再次求取Groebner基得到11个基，使两个变量的次数降低，取其中7个基，通过结式消元获得了一元16次方程的解析解，数值算例获得了与以前的文献相同的结果，程序算法简单，可应用于空间1P5R机器人的逆解求解，

为实际的应用奠定了新的基础。

【相关文献】

［1］ Denavit J，Hartenberg R S.A kinematic notation for lower pair mechanisms based on matrices［J］.Journal of Applied Mechanics，1995，21（ 5） ：215-221.

［2］ Santolaria J，Juan-José Aguilar，José-Antonio Yague，etal.Kinematic parameter estimation technique for calibration and repeatability improvement of articulated arm coordinate measuring machines［J］.Precision Engineering，2008，32（4） ：251-268. ［3］ Duffy J .Analysis of mechanisms and robot manipulators［M］ .Edword Arnold，London，1980.

［4］ Sugminto，Duffy.Analysis of five-degree-offreedomrobotarms［J］ .ASME，Mechanical design，1983，105（1）：23-27.

［5］ Manseur R，Doty K L .A complete kinematic analysis off our-revolute-axis robot manipulators［J］ .Mech.Mach.Th，1992，27（ 5） ：575-586.

［6］Zhou Y B，Buchal R O，Fenton R G.Analysis of the general 4R and 5R robots using a vectoral gebraic approach［J］ .Mech.Mach.Th，1995，30（ 3） ：421～432.

［7］ 杨廷力.机械系统基本理论—结构学· 运动学·动力学［M］ .北京：机械工业出版社，1996. ［8］ Qiao S，Liao Q，Wei S，et al.Inverse Kinematicanalysis of the general 6R serial robot mechanism based on double quaternions［J］.Mechanism and Machine Theory，2010，45（2）：193-199.

［9］ 于艳秋，廖启征.1P5R医用机器人的反解算法研究［J］.北京邮电大学学报，2004，27（ 5） ：31-35.