同伦算法在6R机器人运动学逆解上的应用

? 同伦算法在6R机器人运动学逆解上的应用 同伦算法在6R机器人运动学逆解上的应用

党磊，熊瑞平，唐静莹，孙飞

（四川大学制造科学与工程学院，四川成都610065）

摘要：针对一般6R机器人运动学的逆解问题，提出了一种自适应同伦算法的求解方法。利用SolidWorks对机器人进行结构建模，根据D-H坐标法建立了相应的坐标系，建立机器人运动学模型后得到含六个未知变量的六个非线性运动学方程。在定点同伦和牛顿同伦的基础上，引入自适应参数构造自适应的同伦算法，避免了运动学逆解的雅可比矩阵逆阵奇异。再结合求解常微分方程初值问题的的四阶Runge-Kutta法和Matlab编程，最终得到机器人运动学逆解的全部解。该算法简单易行，计算效率高，为一般型机器人运动学逆解的研究提供了一种有效的方法。

关键词：6R机器人；同伦算法；D-H坐标法；四阶Runge-Kutta法；Matlab

机器人运动学逆解是机器人机构学中最基础也是最重要的研究问题之一，它直接关系到机器人运动分析、离线编程、轨迹规划和实时控制等工作[1]。一般来说，机器人运动学逆解问题经过处理后可归结于非线性方程组的求解，但由于该方程组为超越方程组且具有高维性，因此求解过程相对复杂。国内外关于运动学逆解的求解方法主要有代数法和数值解法。

对于代数法，Raghavan和Roth[2]采用变量消元法对关节变量半角正切函数的一元16次方程进行求解，得到6R机器人的全部组解。倪振松等[3]等将对偶四元数的复指数形式引入到串联机械手位置逆解分析中，通过求解一元16次方程得到机器人的逆运动学的16组解。Y.Gao[4]基于指数积公式，将机器人运动学逆解问题分解成若干个解已知的Paden-Kahan子问题，从而得到运动学逆解的解析解。代数法虽然能求出逆解的精确解，但计算复杂且实时性差。对于数值解法，韩兴国等[5]针对如何提高6R机器人逆运动学求解精度和效率问题，提出了一种基于动态模糊神经网络进行求解的方法。刘永超[6]等采用了遗传算法的二次编码技术求解运动学逆解，但该算法对初始值要求较高，不当的初始值将导致迭代发散。

同伦算法作为数值解法的一种，是一种大范围收敛的搜索算法，对任意的初始值都能保证迭代收敛得到最终的数值解，本文采用一种自适应的同伦算法对一般6R机器人的运动学反解进行研究，以期得到运动学逆解的全部解。该算法对迭代初值要求不高且算法高效可行，对于研究一般6R机器人运动学的逆解具有一定的理论价值。

1 同伦算法的基本原理

对于如下的非线性方程组：

　　（1）

若

　　是光滑映射，那么，对于任意的

　　，向量映射：

式中：

　　。

则原非线性方程组可写为：

　　（2）

同伦算法的思想是对所考虑的问题引入任意的参数

　　和

　　，构造一簇同伦映像

　　，使H满足

　　，有：

　　（3）

则式（2）非线性方程组的求解变成求同伦方程

　　的解

　　，其中：

　　（4）

G(x)为已知映像。对式（4）中的参数t进行求导，可得到：

　　（5）

由此求解非线性方程组的解转化为求解常微分方程组初值问题的解。

2 机器人运动学建模

本文采用的一般型6R机器人如图1所示，该机器人主要用于复合钢管的喷砂作业，其结构特点是后三个关节不满足Pieper准则[7]，即后三个关节轴线不交于一点。根据D-H坐标法建立坐标系的规则，将机器人模型进行简化，建立如图2的各连杆的附体坐标系。则两相邻连杆i－1和i之间的齐次变换矩阵可以表示为：

　　（6）

式中：s表示sin，c表示cos。

略去上标i－1，机器人运动学方程可以表示为：

　　（7）

机器人逆运动学问题即是在已知连杆几何参数ai、di、αi和机器人末端执行器位姿

　　的前提下，求解关节变量角θi（其中i＝1,2,…,）。

图1 机器人模型图

图2 机器人连杆附体坐标系图

根据式（7）中的矩阵等式，为降低待解方程组的复杂度，将式（7）改写为：

　　（8）

将式（8）的左右矩阵分别定义为矩阵U和V，则：

　　（9）

矩阵相等表明其对应元素分别相等，可得到12个方程式，任取其中六组，本文选取U和V中第二列和第四列对应元素分别相等，得到一个含有六个未知参数的非线性方程组为：

　　（10）

式中：

　　；未知参数向量

　　。

3 自适应同伦算法建模及解法分析

3.1自适应同伦算法建模

根据式（10），构建同伦算法模型，即构造同伦方程组。同伦方程组的构造形式多种多样，不同的构造形式对于方程组的求解方法也有所差别。最常用的同伦主要有凸组合同伦和牛顿同伦，当选取

　　时，构成凸组合同伦：

　　（11）

当选取

　　（

　　为自变量x选定初值为x0时对应的函数值），则构成牛顿同伦方程：

　　（12）

牛顿同伦具有不破坏原有非线性方程组Jacobi矩阵结构的优点，给求解大规模的非线性多项式方程组带来极大方便，但是当jacobi矩阵奇异时，牛顿同伦方法将失去意义，为此引入自适应控制参数λ，构造一种自适应同伦算法，将定点同伦和牛顿同伦结合起来，通过自适应变化的控制参数λ避免Jacobi矩阵奇异，具体形式为：

　　（13）

式中：

　　。

结合式（4），对同伦方程两边的t求导得到一阶微分方程组的初值问题，即：

　　（14）

式中：

　　为f在x处的Jacobi矩阵。

　　（15）

3.2同伦算法的解法分析

根据前述的方法，求解同伦方程组可转化为微分方程初值问题的求解。要得到式（14）中的全部解，可采用曲线路径跟踪的方法对同伦方程曲线的路径进行跟踪搜寻。常用的方法为经典的四阶Runge-Kutta法，其表达式为：

　　（16）

值得注意的是，对于式（16）中的经典四阶Runge-Kutta法，其hk为定值。从单步上来看，hk值越小，一定范围内的迭代步数将增加而导致计算量明显增大；hk值越大，求得的近似点越有可能偏离原曲线而产生较大误差，因此合理的选择步长将有助于减小误差和提高计算效率。为此采用变步长的方式，使步长值根据给定的控制精度进行自适应选取，选取择的原则为：从节点tk出发，先以hk为步长，根据式（16）求出下一点的近似函数值

　　，然后将步长折半，以hk/2代入式中求出下一点的另一近似函数值

　　，则两近似值间的偏差为：

　　（17）

当给定的数值精度为ε时：

（1）如果Δ＞ε，则将步长折半后进行计算，直至Δ＜ε为止，取最后一次步长的计算结果作为xk＋1；

（2）如果Δ＜ε，将步长加倍后进行计算，直至Δ＞ε为止，并取最后一次步长的计算结果作为xk+1，如此反复计算。

在路径跟踪的过程中，需要对迭代的终点进行判定，本文采用的判定方法为：假定已求得曲线上的若干点

　　，

　　，

　　以yk为起始点，根据求出同伦曲线上下一点的近似点为

　　，当迭代过程满足式（18）时，迭代终止：

　　（18）

式中：ε为给定的数值精度，并令H(xn+1)＝

　　。

至此完成一个周期计算过程，得到的最终解即为待解方程组所求的解。由于机器人运动学的逆解存在多解情况，因此微分方程的解也将存在多解，为此更改不同的初始值，采取上述的计算方法进行反复计算，直到找出方程组的全部解为止。

4 计算实例

对于图1中的机器人，各连杆的结构参数如表1所示。

给定末端执行器的期望位姿矩阵：

根据上述的方法，利用Matlab编制相应的M文件。为防止当程序计算结果始终无法满足式（18）的条件而导致程序进入死循环，将迭代的最大次数设置为1000次，当中间迭代次数超过设定阈值时程序终止，并将作为最终结果显示在命令窗口上。运行程序并打印结果，得到待解方程组的全部解如表2所示。

表1 连杆结构参数表

连杆iαi-1/°ai-1/mmdi/mmθi变化范围 1000-360360 290080-8015 3014521.5-7020 4-90159.31115-360360 5-900110-120120 6900190-360360

表2 机器人运动学逆解的全部解

个数θ1/radθ2/radθ3/radθ4/radθ5/radθ6/rad 12.428589－0.016984i0.299325－0.037903i3.841064＋0.027789i2.569636－0.051603i－1.843805－0.012707i－2.661729＋0.041206i 22.428589＋0.016984i0.299325＋0.037903i3.841064－0.027789i2.569636＋0.051603i－1.843805＋0.012707i－2.661729－0.041206i 32.030264－0.3808227－1.4304241.705983－0.1693128－1.43013 4－0.781744－0.029876i－2.112826－0.090001i－0.7972304＋0.131796i0.6416046＋0.084865i2.386158－0.132655i－0.566564－0.046974i 5－0.781744＋0.029876i－2.112826＋0.090001i－0.7972304－0.131796i0.6416046－0.0848645i2.386158＋0.132655i－0.566564＋0.0469739i 61.570796－0.785398－0.2617990.7853981.0471980.523599 72.4560610.9499449－ 2.629532－ 2.6608881.840286－2.68748 82.436674－0.009312i－1.408425＋0.019954i1.0－0.030883i2.595921－0.029387i－1.939161＋0.174686i－2.83139＋0.079172i 92.436674＋0.009312i－1.408425－0.019954i1.0＋0.030882i2.595921＋0.029387i－1.939161－0.174686i－.83139－0.079172i 102.46595＋0.004720i－2.022237＋0.007409i1.724049－0.007306i－2.69644－0.01766i1.74771－0.094292i－2.737604＋0.044284i 112.46595－0.004720i－2.022237－0.007409i1.724049＋0.007306i－2.69644＋0.01766i1.74771＋0.094292i－2.737604－0.044284i 121.397928－0.2011292－1.3666470.091584132.0639820.994628 13－1.635613－0.005644i2.962883－0.010564i0.7897375＋0.016565i2.406163＋0.014260i0.8018634－0.23533i0.7194309＋0.112073i 14－1.635613＋0.005644i2.962883＋0.010564i0.7897375－0.016565i2.406163－0.014260i0.8018634＋0.23533i0.7194309－0.112073i 152.200499－0.021604i－0.1025811－0.033989i－1.986647＋0.054624i2.033883－0.043496i1.391279－0.099202i－1.315414＋0.036929i 162.200499＋0.021604i－0.1025811＋0.033989i－1.986647－0.054624i2.033883＋0.043496i1.391279＋0.099202i－1.315414－0.036929i

5 结论

针对一般6R机器人运动学的逆解问题，建立了机器人的运动学模型，得到求解运动学逆解的非线性方程组。根据求解方程组的要求以及求解过程中可能遇到的Jacobi矩阵逆阵奇异的情况，提出了一种自适应的同伦算法，并且对自适应的同伦算法的求解过程进行了分析和叙述，最终得到机器人运动学逆解的全部解。该计算过程较为简洁，计算效率高，为求解一般型机器人的运动学逆解提供了一种很有效的方法。

参考文献：

[1]Liang C G，Lee H Y，Liao Q Z.Analysisof spatial linkages and robot mechanisms [J].Beijing University of Posts & Telecommunica-tion Publishing House，Beijing，China，1988.

[2]Raghavan M，Roth B.Displacement Analysis of the 6R Manipulators of General Geometry [C].International Symposium on Robotics Research，1989：314-320.

[3]倪振松，廖启征，魏世民，等. 空间6R机器人位置反解的对偶四元数法[J]. 机械工程学报，2009，45（11）：25-29.

[4]YGao.Deconposable Closed-Form Inverse Kinematics for Reconfigurable Robots Usi-ng Product-of-Exponentials.Master Thesis. School of Mechanical&Production Engineering，NanyangTechnological University，Singapore，2000.

[5]韩兴国，殷鸣，刘晓刚，等. 6R机器人逆运动学求解与运动轨迹仿真[J]. 四川大学学报，2015，47（6）：185-190.

[6]刘永超，黄玉美，王效岳，张学良，高峰. 基于遗传算法的机器人运动学逆解[J].机器人，1998（6）：421-426.

[7]Craig J J.机器人学导论[M]. China Mac-hine Press，2005.

Application of Homotopy Algorithm to Inverse Kinematics of 6R Manipulators

DANG Lei，XIONG Ruiping，TANG Jingying，SUN Fei

(School of Manufacturing Science and Engineering, Sichuan University, Chengdu 610065, China )

Abstract：Aiming at the inverse kinematics problem of general 6R manipulators, an adaptive homotopy algorithm is proposed. The structure of the manipulators is modeled with SolidWorks, and the corresponding coordinate system is established according to the D-H coordinate method. Six nonlinear kinematics equations with six unknown variables are obtained after the manipulators kinematics model is established. On the basis of fixed - point homotopy and Newton homotopy, an adaptive homotopy algorithm is constructed by introducing adaptive parameters, which avoids the singularity of inverse Jacobian matrix in inverse kinematics. Combined with the fourth-order Runge-Kutta method to solve the initial value problem of ordinary differential equation and using Matlab programming, the final solution of the inverse kinematics of the manipulators is obtained. The algorithm is simple and efficient, and it provides an effective method for the inverse kinematics of the general manipulators.

Key words：6R manipulators；homotopy algorithm；D-H coordinate method；the fourth order Runge-Kutta method；Matlab

中图分类号：O311

文献标志码：A

doi：10.3969/j.issn.1006-0316.2017.04.002

文章编号：1006－0316 (2017) 04－0005－05

收稿日期：2016－11－17

作者简介：党磊（1991－），男，湖北荆州人，硕士研究生，主要研究方向为工业机器人误差分析。