云模型遗传蚁群算法的机器人逆运动学求解

顾嘉伟，吴功平，彭向阳，王 锐

(1.武汉大学动力与机械学院，湖北 武汉 430072；2.广东电网有限责任公司电力科学研究院，广东 广州 510080)

1 引言

运动学逆解[1]即已知机器人末端在坐标空间中的位姿，计算各关节的角度值。机器人求逆影响到后序的运动规划、运动控制等工作。

常见的求逆方法有封闭解法，迭代法和智能算法。又因为机器人的机构设计问题或者装配时存在误差，很多时候机器人的三个相邻关节轴未交于一点或三轴线平行，即不满足Pieper准则[2]，此时不具有封闭解。而常用的迭代算法因计算量大，且在奇异位置雅可比矩阵行列式为0，无法进行求逆运算。故智能算法具有明显的优势。常见的智能算法有遗传算法，粒子群算法和神经网络等。其中遗传算法具有搜索能力强的特点，被广泛的应用于优化问题当中。文献[3]采用二次编码的方法对PUMA560机器人进行求逆，并实现了在冗余机器人上的求逆运算。同时也有改进的遗传算法，进行机器人求逆。文献[5]采用多种群遗传算法求逆，利用遗传算法中潜在的并行性，来提升求解的精度和收敛速度，并在Comau NJ-220型机器人上进行了验证。文献[6]将遗传算法与粒子群算法相结合，进行求逆优化。但是以上算法，对于非线性、高维问题的解决效果仍然不是十分理想。

针对高压输电线跨越越障式巡检机器人，采用了一种基于云模型的遗传网格蚁群算法。利用遗传算法进行全局寻优，利用蚁群算法[7]进行局部搜索。与传统遗传算法相比，并采用了适应度值函数尺度变换方法[8]增加种群的多样性以，使用云模型[9]实现算法中交叉和变异参数的自适应；同时，用基于网格划分策略的连续域蚁群算法[10]进行局部寻优。从而使得算法局部寻优能力得到提升，从而提高了算法的精度及收敛速度。

2 逆运动学问题模型的建立

以高压输电线跨越越障式巡检机器人为对象开展研究。该机器人实物图及机构简图，如图1所示。该机器人采用双臂反对称设计，行走轮在架空地线上运动，不同于穿越越障式巡检机器人[11]。其对杆塔的改造很小，遇到悬垂线夹，辅助导轨[12]等障碍物时，需要单臂夹紧地线，另一机械臂离线完成跨越障碍物的动作。

图1 机器人实物图及机构简图Fig.1 Robot And Its Schematic Diagram of Mechanism

因为压紧关节、俯仰关节与回转关节三个关节轴未交于一点或者平行，如图1(b)所示。因此不满足Pieper准则[2]。机器人越障时，各关节坐标系按以下方式建立。

固定臂设置基坐标系｛1｝，在俯仰关节处设置坐标系｛2｝，旋转关节处为坐标系｛3｝，如图2所示。因为两臂采用反对称结构，离线臂的坐标系设置同固定臂。控制箱、行走轮机构和夹紧机构等对机器人的位姿无影响因此不予考虑。

并补充该机器人各关节的上下边界：

可见机器人各个关节的边界差距很大，因此在优化过程中各关节值采取归一化的措施。

利用坐标变换法对机器人进行运动学建模。对变换矩阵进行连乘所得到的矩阵即可表达机器人与机器人基坐标系间的变换关系。

式中：a—末端进入物体的方向；o—从一个位置指向下一个位置的方向，法向量n与之前两向量满足右手定则。以上三向量均为单位向量；Px、Py、Pz—末端位置矢量的三个方向上的分量；R3×3—机器人末端坐标系相对于基坐标系的旋转矩阵(称姿态矩阵)；P—机器人末端坐标系相对于基坐标系的位置矢量。

图2 机器人坐标系分布Fig.2 Distribution of Robotic Coordinate System

可将位姿矩阵分为位置函数与姿态函数，因此求逆问题转化为在变量取值范围内求解函数最值的问题。如下所示在变量域中找到一组θ使得与目标的位姿矩阵之间的误差最小。

式中：Pcur—当前关节角下得到的机器人末端的位姿矩阵，而θi—机器人对应的7个关节变量；Pdes—末端理想位姿矩阵，利用二者之差来表示当前个体所对应的机器人末端位姿与理想位姿之间的误差。式中ΔR3×3—姿态误差，取矩阵的谱范数；ΔP—位置误差，取向量的模。可按以下方式构造寻优函数：

在研究过程中发现，姿态误差与位置误差之间存在数量级上的差距，因此引入权值系数α=1500和β=1，使姿态误差与位置误差对整体的影响相当，X表示关节变量。

3 云模型遗传蚁群算法的实现

将式(3)作为遗传算法的适应度值函数。针对遗传算法存在的“早熟”以及局部搜索能力差的现象。从以下4点进行改善。

3.1 适应度值尺度变换

通常地，遗传算法采用的是固定的适应度函数，但该策略在算法运行的后期，随着，种群中个体趋于接近，易造成种群多样性的下降，进而带来“早熟”问题。为了避免上述问题，采用了适应度值尺度变换的方法。变换的方法为：指数尺度变换[8]。

式中：F′—新的适应度值，式中系数γ—参数，γ越小产生的新的适应度值就会有更大的差别，从而增加了种群的多样性。又因为采用了指数变化的方式，因此适当调节参数γ便可以取得较好的效果。

3.2 浮点数编码及算数交叉

传统的遗传算法，采用的是二进制编码方案[3]。采用浮点数编码及算数交叉的方法以提升进化效果。

因待优化问题属于连续域函数优化问题，使用二进制编码方案时存在着连续变量离散化映射误差，另外额外的编码解码也增加了程序的时间复杂度及空间复杂度。因此采用浮点数编码方案。即针对解向量的每一维，根据机器人关节的范围，在该范围内取随机数来生成染色体。算数交叉是将两个体通过线性组合产生出新个体的方法。

式中：t—第t代个体，ε—一参数。这里采用随机数的方法来生成该参数。即：每一次交叉运算，ε采用随机生成的方法。

3.3 基于云模型的参数自适应

遗传算法中交叉和变异概率通常取固定值，随着迭代的进行易出现“早熟”现象。

云模型[9]基于模糊集合理论和概率论实现了概念和数据之间转化，由3个数字特征描述：

期望Ex：区域空间上云滴的均值，表示定性概念的值。

熵En：量化了定性概念的不确定性，受自身模糊性和随机性的影响。同时也是定性概念随机性度量，反映了概念的云滴的离散程度。

超熵He：量化了熵的不确定性，受熵的随机性和模糊性的影响。

En和He的改变会影响云滴的形状。同时云模型满足正态分布，云滴的中心为Ex，呈对称分布，具有中间多，两边少的特点。引入正向正态云发生器，即：将定性的知识转化为定量的数据表示，通过数字特征值生成云滴的过程。将个体的平均适应度作为期望，通过对En和He的控制调节交叉概率和变异概率，实现了基于云模型的参数自适应策略。

交叉概率Pc的生成算法：

变异概率Pm的生成算法：

式中：E′n～N(En，He)。fita、fitb—待操作的两个体的适应度值；fitmax与fitmin—种群中适应度值的最大值和最小值。f′与—当前待操作个体的适应度值和种群中适应度值的平均值。m1-2，n1-2和t1-4—调节系数。通常t1和t2取概率的下限值，t3和t4取概率的上限值。

其中6≤m≤3×Population(Population为种群中个体总数)，5≤n≤15。

3.4 网格划分策略的连续域蚁群算法

为了改善遗传算法局部搜索能力不足的缺点，采用局部搜索性能强的蚁群算法进行弥补。通常地，蚁群算法的信息量操作及最优解的选择都是通过离散变量来求解的；而对于连续变量，其变量分布在一定的区域内，而不是点集。因此采用了网格划分策略[10]，将连续问题离散化，实现对解的进一步优化。

该算法的思路为：首先估计出所求变量的取值范围xjlower≤xj≤xjupper(j=1，2，3，…，n)。在变量区域内画网格，蚂蚁只能从空间网格点移动至空间内的另一网格点，并根据各网格点的对应函数值留下不同大小的信息量，以影响之后蚂蚁的移动，不断循环至满足截至条件。

将遗传算法迭代中得到的最优个体作为蚁群算法的初始解，以它为中心在变量空间进行网格划分。每一维变量，按照其取值范围进行N等分。某只蚂蚁在空间中的位置可以表示为(n1，n2，…，nn)ni∈［0，N］，且ni为自然数。蚂蚁空间坐标(n1，n2，…，nn)与变量(x1，x2，…，xn)转换公式为：

将(n1，n2，…，nn)称为状态，蚂蚁从第1级到第n级之间进行状态转移时，其概率，如式(17)所示。

第j级第i个节点的信息量取为τij按式(18)计算：

式中：ρ—信息素挥发系数；Q—信息素强度，它影响了算法的收敛速度；f—目标函数值。

这里动态调整的方法，对信息素挥发系数进行动态变化，进而提高蚁群算法的求解效率。

改进后蚁群算法具体实现步骤如下：

(1)估计出解中各维变量的取值范围，即：xjlower≤xj≤xjupper(j=1，2，…，n)，并将各变量N等分。

(2)循环次数itc=0，给τij矩阵赋相同的数值，设置ρ，Q以及itmax的初始值。

(3)设置蚂蚁的数量为num_ant，每只蚂蚁选取节点的依据为式(17)。

(4)按式(18)修改信息量，并使循环次数加1。

(5)若未达到最大迭代次数，则跳转至(3)；否则找出τij每列最大的元素所对应的行(m1，m2，…，mn)，将其作为优良解输出。

4 算法描述

该算法将遗传算法、蚁群算法与云模型相结合，将遗传算法中每一次进化得到的最优个体送入到蚁群算法中进行进一步寻优。将算法主要分为7个步骤，具体流程及描述如下。

(1)初始化种群。设置初始种群中的个体数M，交叉概率Pc和变异概率Pm。

(2)确定选择策略。选择算子参考了“自然选择”过程，适应度值越高的个体存活下来的概率就越高。

具体而言采用轮盘赌法。其基本思想是：个体被选中的概率与其适应度值大小线性相关，对环境适应度更好的个体，其优良性状保留到下一代的概率就会更大。设种群的大小为M，个体i的适应度Fitnessi，则个体i被选中的概率Pis为：

式中：Fitnessi—个体i对应的适应度值。

(3)确定交叉算子。由于采用了浮点数编码方案，因此采用算数交叉的方法。具体方法如上所述，不再赘述，交叉概率满足式(10)。

(4)确定变异算子。在每次迭代过程中，每个个体染色体上的基因均有一定的概率发生变异，概率满足式(15)。若发生，则以该基因取值范围内的随机数进行替换。

(5)网格划分策略的连续域蚁群算法局部寻优。找出每次迭代适应度值最高的个体，该个体的染色体作为蚁群算法的初始解Xbest。以Xbest为搜索中心，按照每一维的取值范围依次进行N等分，向周围空间进行网格划分。设置初始蚁群总数为num_ant，最大循环次数itmax，信息素挥发系数ρ，信息素强度Q，每只蚂蚁按照随机数方法生成网格空间坐标。如上所述以最有个体为中心进行寻优。

(6)更新最优个体。当前文所述的最优个体Xbest经过网格蚁群算法后产生最优蚂蚁BestAnt，BestAnt经过式(16)转化后称为个体X′best，计算两个个体适应度值函。若Fitness(Xbest)≤Fitness(X′best)则表明经过网格蚁群算法后最优个体发生了改变，表明该算法在Xbest附近找到了比自身性状更加优良的个体。并用X′best当作当前种群中的最优个体去替换当前种群中的最差个体。反之，若Fitness(Xbest)＞Fitness(X′best)，则最优个体未发生改变，将Xbest替换当前种群中的最差个体。

(7)进化结束输出最优解。若遗传算法不满足截止条件时，继续进行迭代，直至达到收敛条件，输出最优解，即运动学方程的逆解。

图3 基于云模型的遗传蚁群算法流程图Fig.3 Flow Chart of The Algorithm Based on Cloud Model Genetic and Ant Colony Algorithm

5 实验验证

验证针对跨越越障式巡检机器人开展，在MATLAB中进行比较(实验设备为一般PC机，英特尔core i5处理器1.7 GHz，4 GB内存，Windows 10操作系统)。第一组，已知某一位姿分别采用传统的遗传算法和所用算法进行计算，验证这里算法的收敛速度上的优势；第二组实验，利用跨越机器人在跨越辅助导轨时记录下的各个关节角参数，利用正运动学公式，得到一系列目标位姿。利用基于云模型的遗传蚁群算法及遗传算法进行逆运动学求解，说明算法在精度上的提升。

5.1 算法收敛速度分析

本组实验具体参数设置如下。

(1)遗传算法：二进制编码方案，交叉概率Pc=0.6，变异概率Pm=0.2，选择操作采用轮盘赌法，单点交叉，进化代数为200代。

(2)云模型遗传蚁群算法：浮点数编码，算数交叉，参与计算的个体总数M=200，进化代数取200代，在进化代数达到80%(160次)时，采用适应度值函数尺度，变换取参数γ=0.5。云模型的参数取值为：

网格蚁群参数设置为：

图4 算法收敛速度比较Fig.4 Comparison of Convergence Speed of Two Algorithms

5.2 算法收敛精度分析

图5 算法的姿态误差对比Fig.5 Comparison of Postural Error between Two Algorithms

本实验数据来自机器人跨越辅助导轨时获取的关节参数，经正运动学公式得到机器人的位姿矩阵。通常地，辅助导轨上的落点距离机器人坐标原点(500～600)mm。采用遗传算法和基于云模型的遗传蚁群算法进行求逆运算，比较其姿态精度和位置精度。进化代数取100代，两算法的收敛条件同取个体适应度值小于等于0.95，其余参数同上一组实验。

经过求运算，两算法的姿态误差及位置误差对比如下所示。

图6 算法的位置误差对比Fig.6 Comparison of Position Error between Two Algorithms

采用基于云模型的遗传蚁群算法进行跨越越障型机器人求逆时，位置精度和姿态精度均远高于遗传算法，如图(5)、图(6)所示。将上述50个实验点的平均姿态误差和位置误差统计与表1中。这里的算法要比标准遗传算法精度上显著提升，如表1所示。

表1 对比实验数据分析Tab.1 Data Analysis of Compared Experiments

6 结论

(1)提出了用于跨越越障型巡检机器人逆运动学求解的基于云模型的遗传蚁群算法。

(2)针对传统的遗传算法的编码方式，适应度值函数计算方式，交叉变异操作存在的不足，分别采用了浮点数编码，适应度值尺度变化，以及云模型参数自适应的方法实现优化。

(3)在跨越越障型巡检机器人上开展对比实验。结果表明：相比遗传算法，基于云模型的遗传蚁群算法改善了原有的局部收敛现象，且运算的精度有了较大的提升。