一种求取运动学逆解的新算法

第２卷第１期２００４年１月 中国工程机械学报Ｑ于ＩＮＥＳＥ　ＪＯＵＲＮＡＬ　ＯＦ　ＣＯＮＳＴＲＵＣＴＩＯＮ　ＭＡＣＨＩＮＥＲＹ、场」．２　Ｎｏ．　ｌ

Ｊａｎ．　２００４

一种求取运动学逆解的新算法黄志雄，何清华，邹湘伏，柳 （中南大学机电工程学院，湖南长沙波，张新海

４１００８３）

摘要：从计算机原理中位置数制的描述获得启发，将其中的某些规律进行合理的转换，形成搜索算法的步长表达式，在吸收传统登山法的优点的基础上，提出了求取多关节机械臂运动学逆解的新算法—智能登山法，并对算法进行了描述，写出了完整的通用流程图．运用该方法求取机器人运动学逆解，无需矩阵运算、三角变换；收敛速度快，适于实时控制．仿真试验验证了该方法的可行性、实用性．

关键词：多关节机械臂；运动学逆解；步长；智能登山法中图分类号：ＴＨ　１１３．２

Ｎｅｗ　Ａｌｇｏｒｉｔｈｍ　ｆｏｒ　Ｓｅａｒｃｈｉｎｇ　Ｋｉｎｅｍａｔｉｃ　Ｉｎｖｅｒｓｅ　ＳｏｌｕｔｉｏｎＨＵＡＮＧ　Ｚｈｉ－，ｘｉｏｎｇ，ＨＥ　Ｑｉｎｇ－ｈｕａ，ＺＯＵ　Ｘｉａｎｇ－ｆｕ，ＬＩＵ　Ｂｏ，ＺＨＡＮＧ　Ｘｉｎ－ｈａｉ（Ｃｏｌｌｅｇｅ　ｏｆ　Ｍｅｃｈａｎｉｃａｌ　ａｎｄ　Ｅｌｅｃｔｒｉｃａｌ　Ｅｎｇｉｎｅｅｒｉｎｇ，　Ｃｅｎｔｒａｌ　Ｓｏｕｔｈ　Ｕｎｉｖｅｒｓｉｔｙ，　Ｃｈａｎｇｓｈａ　４１００８３，　Ｃｈｉｎａ）

Ａｂｓｔｒａｃｔ：　Ｉｎ　ｔｈｉｓ　ｐａｐｅｒ，　ｉｎｓｐｉｒｅｄ　ｆｒｏｍ　ｔｈｅ　ｐｏｓｉｔｉｏｎ－ｎｕｍｅｒａｌ　ｄｅｓｃｒｉｐｔｉｏｎ　ｉｎ　ｃｏｍｐｕｔｅｒ，　ｔｈｅ　ｓｔｅｐ＇ｓ　ｅｘｐｒｅｓｓｉｏｎ　ｏｆｔｈｅ　ｓｅａｒｃｈ　ａｌｇｏｒｉｔｈｍ　ｗａｓ　ｇａｉｎｅｄ　ｔｈｒｏｕｇｈ　ｒｅａｓｏｎａｂｌｅ　ｃｏｎｖｅｒｓｉｏｎ　ｏｆ　ｓｏｍｅ　ｉｎｔｅｒｒｅｌａｔｅｄ　ｒｕｌｅｓ．　Ｂａｓｅｄ　ｏｎ　ｉｎｈｅｒｉｔｉｎｇｔｈｅ　ａｄｖａｎｔａｇｅｓ　ｏｆ　ｔｒａｄｉｔｉｏｎａｌ　ｈｉｌｌ　ｃｌｉｍｂｉｎｇ，　ａ　ｎｅｗ　ｗａｙ　ｔｏ　ｓｅａｒｃｈ　ｔｈｅ　ｋｉｎｅｍａｔｉｃ　ｉｎｖｅｒｓｅ　ｓｏｌｕｔｉｏｎ　ｏｆ　ｍｕｌｔｉ一ｏｉｎｔｒｏｂｏｔ－ｉｎｔｅｌｌｉｇｅｎｔ　ｈｉｌｌ　ｃｌｉｍｂｉｎｇ，　ｗａｓ　ｐｕｔ　ｆｏｒｗａｒｄ．　Ｍｏｒｅｏｖｅｒ，　ｔｈｅ　ａｌｇｏｒｉｔｈｍ　ｗａｓ　ｄｅｓｃｒｉｂｅｄ，　ａｎｄ　ｔｈｅ　ｉｎｔｅｇｒａｔｅｄｆｌｏｗ　ｃｈａｒｔ　ｗａｓ　ｇｉｖｅｎ．　Ｍａｋｉｎｇ　ｕｓｅ　ｏｆ　ｔｈｉｓ　ｎｅｗ　ｗａｙ，　ｋｉｎｅｍａｔｉｃ　ｉｎｖｅｒｓｅ　ｓｏｌｕｔｉｏｎ　ｃａｎ　ｂｅ　ｇｏｔｔｅｎ　ｃｏｎｖｅｎｉｅｎｔｌｙ　ａｎｄｆａｓｔｌｙ，　ｆｕｒｔｈｅｒｍｏｒｅ，　ｉｔ＇ｓ　ｎｏｔ　ｎｅｃｅｓｓａｒｙ　ｆｏｒ　ｍａｔｒｉｘ　ｏｐｅｒａｔｉｏｎ　ａｎｄ　ｔｒｉａｎｇｕｌａｒ　ｔｒａｎｓｆｏｒｍａｔｉｏｎ．　Ａｎｄ　ｉｔ＇ｓ　ｓｕｉｔａｂｌｅ　ｆｏｒｒｅａｌ　ｔｉｍｅ　ｃｏｎｔｒｏｌ　ｆｏｒ　ｉｔｓ　ｆａｓｔ　ｃｏｎｖｅｒｇｅｎｃｅ　ｖｅｌｏｃｉｔｙ．　Ｔｈｅ　ｆｅａｓｉｂｉｌｉｔｙ　ａｎｄ　ｐｒａｃｔｉｃａｂｉｌｉｔｙ　ｗｅｒｅ　ｖｅｒｉｆｉｅｄ　ｂｙ　ｓｉｍｕｌａｔｉｏｎ

Ｋｅｙ　ｗｏｒｄｓ：　ｍｕｌｔｉ一ｏｉｎｔ　ｒｏｂｏｔ；　ｋｉｎｅｍａｔｉｃ　ｉｎｖｅｒｓｅ　ｓｏｌｕｔｉｏｎ；　ｓｔｅｐ；　ｉｎｔｅｌｌｉｇｅｎｔ　ｈｉｌｌ　ｃｌｉｍｂｉｎｇ　ａｌｇｏｒｉｔｈｍ（ＩＨＣＡ） 机器人运动学逆解在机器人学中占有重要地位，它直接关系到运动分析、离线编程、轨迹规划等，是将工作空间内机器人末端件的位置、姿势转化成关节量的方法． 国内外学者对机器人（包括冗余度机器人）运动学逆解的求取进行了广泛而深人的研究．有关机器人逆解的求解方法很多，其中主要有下面一些：反变换（又称解析法和代数法）、旋量代数、对偶矩阵、对偶四元素、迭代法和几何法，以及以这些方法为基础演变而成的一些方法等〔‘〕．这些方法中有些计算量比较小，适于实时计算，但只能得到局部最优解，关节运动不具有可重复性，如Ａ．　Ｌｉｅｇｅｏｉｓ提出的梯度投影法Ｉ２１，　Ｉ．　Ｄ．　Ｗａｌｋｅ：提出的约束变量法等（３１；有些可以得到全局最优解，关节运动具有可重复性，但计算量

非常大，不适于实时控制，如Ｋ．　Ｗａｚｅｒｏｕｎｉａｎ利用最优控制理论建立边值问题的微分方程组，从而确定关节转角［’］等． 在此，笔者试图寻求一种简捷、优化的算法来求取多关节工业机器人运动学逆解，这种方法无需矩阵运算、三角变换等繁琐的计算，只要给出控制目标与约束条件，通过该算法便能搜索到各关节变量的值；换

作者简介：黄志雄（１９７６一），男，湖南长沙人，博士生．中国工程机械学报第ｚ卷

言之，就是在已知执行器结构参数、位姿的情况下，设计一套快速求取各关节变量的算法．１算法的推导１．１登山搜索算法原理 人工智能领域研究的几个方面都是十分激动人心的．但多数人工智能应用的基础却是问题求解．运动学求逆实际上就是一个问题求解的过程，搜索可能解的技术有多种，登山搜索（ｈｉｌｌ－ｃｌｉｍｂｉｎｇ　ｓｅａｒｃｈ）　Ｅ５，６】就是用得非常普遍的一种． 登山搜索源于深度优先搜索．登山算法选择下一步节点时，考虑是否最接近目标（即离现在位置最远的位置）・这个名字来源于登山者在半山腰处迷路的类似情形，假如营地在山顶，那么尽覃在夜里，他也知道所走的每一步方向都是正确的．由于有启发信息的引导，登山法使问题可尽快得到解．登山算法在许多情况下能提供一个较好的解，因为它试图减少到达解之前应访问的节点数一般说来，登山算法比其他非启发式算法都能更快地找到一个比较理想的解． 对人而言，寻求搜索算法来求解多关节机械臂运动学逆解，目标是“尽快找到一个比较理想的解”，并且，在搜索各关节变量的解时，要求的是每一步搜索都更接近目标点（或者说不远离目标点），而不在乎它是否最接近目标．这同“登山法”的思想很接近．１．２搜索表达式的导出 计算机最基本的功能就是对数进行计算和处理，数在机器中是用器件的物理状态表示的，而具有两种物理状态的器件容易制造，稳定可靠，所以计算机中的数是用二进制表示的． 最常用的数制是位置数制，它是按位定值的数制，即是按各个数码的位置规定了该数码所具有的数值，在位置数制中，数Ｎ可写成：

Ｎ二（ｄｎ＿ｌｒｎ　－ｉ　＋　ｄ，－２ｒ　ｎ－２＋二十ｄｌｒｉ　＋　ｄｏｒ０　＋　ｄ－ｉ二一‘＋．二＋ｄ＿ｍｒ－ｍ卜Ｅｄ＝ｒ２式中：基值．ｍ，　ｎ是正整数，ｎ为整数的位数，ｍ为小数的位数；ｄ＊是０，１，２－二，（ｒ一１）中的任意一个数

当ｒ取不同值时，就形成不同的进位制．

（１）ｒ表示

例如：当ｒ＝２时，就是二进制，Ｎ＝兄　ｄ１　（２）可以看出，这种计数制有如下规律：①ｒ进制中ｄ‘可取Ｏ－　ｒ一１个不同的数码，这些数码为非负整数．②每个数位有一定的位值，它是基值ｒ的某次幂，所以，相邻高位的位值是其低位的ｒ倍；在加减法运算中，采用“逢ｒ进一”和“借一当ｒ”的规则．③对于正数，表达式（１）中Ｎ的值随ｉ增大而递增；对于负数，表达式（１）中Ｎ的值随ｉ增大而递减．④对于某一进制，基值ｒ是定值，且为正整数．⑤ｉ为整数，可取负值． 尝试把以上这些规律作如下修改：①针对某一个数，ｄ的取值是固定的，而且不明确确定ｄ一定为整数，在此基础上再赋予ｄ一个符号值ｋ，将其称作“权”，ｋ可取（一１，０，１）三个数值．②每个数位有一定的位值，它是基值ｒ的某次幂，但是，相邻高位的位值并不一定是其低位的ｒ倍，因为它还要受ｋ的影响．④表达式Ｎ的值随ｉ的变化是非单调的，但通过运用新算法进行搜索，能够很快逼近Ｎ．⑤基值ｒ理论上可取（０，１）范围内的数值，但具体取某个值，应根据实际情况而定．⑥ｉ为正整数．⑦一般情况下，表达式的值只有无限地逼近Ｎ，而得不到绝对精确的Ｎ． 显然，新旧规则的差异很明显．这样，在上述七条新规则的制约下，十进制数值Ｎ可描述成

Ｎ、又ｋｄｒｚ在充分逼近的条件下可以认为Ｎ＝又ｋｄｒ｀（２）第１期黄志雄，等：一种求取运动学逆解的新算法

式中：ｋ为符号常量，将其称作“权”，取（１，０，一１）中的一个值，ｒＧ（０，１），称作基值，ｄ称为初值．把式（３）称为带权的特殊进制方法．在误差一定，充分逼近十进制数值Ｎ时，ｎ，ｋ，ｄ和ｒ的组合可有无穷多个．将此方法应用于人工智能中的搜索技术（登山法）来描述每次搜索的步长，就形成一种新的启发式搜索技术—带权变步长搜索方法，称之为“智能登山法”．

开始初始化

。＞。。？＼、一Ｎ输出

２算法描述 十Ｙ

ｎ＝ｎ＋１

由智能登山算法的表达式：Ｎ＝来描述该算法：搜索一个值Ｎ时． ｆ｝）初始化：选定基值ｒ、初值ｄ．

，可以这样搜索失败！

（２）　ｎ＝ｉ时，以ｄｒ－｀（或一ｄｒ｀）作步长搜索，若接近目标，Ｎ＝Ｎ＋ｄｒ｀（或Ｎ＝Ｎ－　ｄｒ＇），ｎ＝ｎ＋１． （３）若远离目标，则以一２ｄｒ｀（或２ｄｒ＇）作步长搜索，若接近目标，Ｎ＝Ｎ－２ｄｒ＇（Ｎ＝Ｎ　＋２ｄｒ＇），ｎ＝ｎ

＋１． （４）若远离目标，ｎ＝ｎ＋１． （５）重复（２）．（３），（４），直到达到目标或ｎ超过设

定值． 将以上步骤写成程序流程框图，如图１．

Ｂ＝Ｂ；＋ｋ；　ｄ；　ｒ＂伍ｎ，　ａ，　ｏ）＝Ｆ（　Ｂ；　）Ｅ，　ｆ　（ｐ，　ｎ，　ａ，　ｏ）

。】＞。？＼卜Ｎ爪：＝　Ｅ一

图１智能登山搜索算法流程图 Ｆｉｇ．　ｌ Ｆｌｏｗ　ｃｈａｒｔ　ｏｆ　ＩＮＣＡ

３仿真验证 对于多关节机械手，对应某一位姿，至少存在一组解０１，．．．，马７．．．　９０Ｍ能实现该位姿，这组解可以用新算法描述为

０，一Ｅ　ｋｉｄｌ：、，…，ｅ；＝＝Ｚ，　ｋ｝ｄ｝ｒｊ＇，…，０．一另ｋｍｄｍｒｍ（３）

式中，ｅｌ｝．．．｝ｅｊ，．．．，０在初始值开始最多经过ｎ次搜索后，必定充分逼近目标点（０１０９＊＊．９ｅｉ０ｑ．．．，ｇｍｏ）－ 下面，就以水轮机叶形现场检测机械臂川为例，用智能登山法来求解其运动学逆解．确定目标函数为 ｆ（ｐ）＝（ｐ一ＰＯ），＝（ｐ二一ｐｘｏ）２　＋　（ｐ，，一ｐ，ｏ　）２　＋　（ｐ二一ｐｚ０）２ （４） ｆ（ｎ，ａ，ｏ）＝ｆ　（ｎ）＋ｆ　（ａ）＋ｆ　（ｏ）＝（ｎ一ｎｏ　）２＋（。一ａｏ）２＋（。一。０）２＝ （ｎ二一ｎ．０）２＋（ｎ，一ｎ刃 ）２．ｖ０＋（ｎ二一ｎｚ０　）２＋（ａ二一ａｘｏ　）２＋（ａ，一ａ刃　）２＋（ａ二一ａｚｏ）２＋ （０ｘ一０．ｘ０）２　＋　（０，．一０，０）２　＋　（０二一０２０）２ （５）其中，（ｐｏｔ　ｎｏ｝ａｏｙｏｏ）是目标值，（ｐ，ｎ，ａ，ｏ）是实际搜索值 优化指标为 ｆ（ｐ）＜Ｅｌ，ｆ（ｎ，ａ，０）＜ｅ２

为了计算的简化，将姿态项ｆ（ｎ，ａ，ｏ）省略，只保留位置项ｆ　（ｐ）．并令ｆ　（ｐ）的综合误差Ｅ＝０．１．各关节变量采用智能登山法的描述形式为