改进遗传算法搜索中间点的机械臂轨迹规划∗

谭 燕

(重庆三峡职业学院机械与电子工程系重庆404155)

机械臂正朝着灵活、控制精确、高效的方向发展，可以越来越多地完成人类难以完成或具有危险的工作。机械臂工作轨迹直接决定了机械臂工作效率[1-2］，因此研究机械臂轨迹规划问题对于提高经济效益意义明显。

机械臂轨迹规划的常用方法是以边界约束为限定条件，使用多项式函数[3］、抛物线过渡的线性函数、B样条曲线[4］等进行拟合，使用待定系数法确定轨迹函数。文献[5］以6自由度机械臂为研究对象，使用多项式插值法进行轨迹规划，使用杂交算法寻优，得到了速度约束下的时间最优轨迹；文献[6］使用牛顿下山法求出了轨迹关键点，使用三次样条插值拟合了轨迹，实现了能量消耗最优；文献[7］以距离最优为目标选择了空间梯形轨迹关键点，使用5次多项式插值法进行拟合，实现了预期目标；文献[8］在速度约束下，提出了粒子群优化的多项式插值轨迹规划方法，得到了时间最优轨迹。以上研究成果都达到了各自的优化目标，但是目前没有成熟统一的轨迹规划方法。本文研究了避障同时的多目标优化轨迹规划方法。

本文研究机械臂轨迹规划问题，设置了3个目标:避障、轨迹长度最短、关节角变化量最小。给出了简单高效的碰撞检测策略，当发生碰撞时，使用改进遗传算法寻得最优中间点，而后使用轨迹基元规划起点至中间点、中间点至终点的轨迹，最终达到避障同时保证轨迹最短和关节角变化量最小的目的。

1 机械臂运动学建模

1.1 仿人手臂机械臂

本文研究对象为如图1所示的仿人手臂机械臂，图中L1＝120 mm为大臂长度，L2＝100 mm为小臂长度。肩关节具有两个旋转自由度。肘关节具有1个旋转自由度，腕关节具有3个旋转自由度，图中为每个旋转自由度建立了坐标系，z轴为其旋转轴，按照图中各坐标系的脚标，将坐标系记为坐标系1～6。

1.2 运动学建模

机械臂运动学模型是机械臂各关节角与末端执行器位姿互相求解问题。当各关节角已知，求解末端执行器位姿时称为运动学正解；当末端执行器位姿已知，求解各机械臂关节角时称为运动学逆解。

本文使用D-H法建立运动学正解模型，记坐标系n到坐标系n＋1的齐次变换矩阵为，经过两次旋转和两次平移可由坐标系n转换到坐标系n＋1，得矩阵形式为:

那么从基座坐标系(记为0坐标系)到末端执行器的齐次变换矩阵为:

式(2)即为机械臂运动学正解模型，当机械臂各关节角确定时，可以通过式(2)确定末端执行器的位姿。

机械臂运动学逆解与正解过程恰好相反，在式(2)两端分别左乘()-1、()-1、…、()-1，再使用反正切函数可以求得θi，i＝ 1，2，…，6。 运动学逆解存在多解问题，一般需根据实际情况进一步确定。

2 插入最优中间点的轨迹规划

2.1 插入最优中间点轨迹规划原理

本文对机械臂轨迹规划的目标包括3个:避障、轨迹最短和关节角变化量最小。轨迹规划的整体思路是:根据机械臂任务确定轨迹起点和终点，使用轨迹基元进行轨迹规划，然后对规划出的轨迹进行碰撞检测，若发生碰撞则插入最优中间点，再使用轨迹基元规划从起点至中间点、中间点至终点的轨迹，在保证轨迹不发生碰撞的同时，保证轨迹最短且关节角变化量最小。机械臂避障轨迹规划原理如图2所示。

由图2可知，还有3个问题需要明确:一是确定轨迹基元，二是碰撞检测如何实现，三是最优中间点如何选取。轨迹基元为点与点之间拟合使用的曲线函数。

常用函数包括多项式函数、B样条曲线等。本文选择使用5次多项式作为轨迹基元。

2.2 碰撞检测方法

机械臂轨迹规划的首要要求是安全，即避开障碍物，因此必须提出机械臂与障碍物的碰撞检测策略。为了提出简单高效、利于推广的碰撞检测方法，本文将障碍物模型简化为球形，球心为障碍物几何中心，球半径为障碍物最大半径。将机械臂模型简化为线段，线段长度与机械臂长度一致，为了实现避障，将机械臂包络半径叠加到障碍物半径中，最终得到障碍物球形模型的膨胀半径为R＝Rob＋Rlink，式中Rob为障碍物半径，Rlink为连杆包络半径。

通过障碍物与机械臂简化模型可以看出，机械臂与障碍物之间碰撞检测问题可以简化为球模型与线段的位置关系判断问题。确定连杆线段的位姿，只需用到关节2的位姿、关节3的位姿、关节4的位姿，则两连杆在三维空间的矢量表达式为:

式中:为大臂的矢量表达式；为小臂的矢量表达式；(1，4)表示矩阵第1行、第4列的元素。

记障碍物圆球模型的球心坐标为P＝[px，py，pz］，选取连杆线段上任意一点到球心的矢量表达为，则连杆与球心距离为:

式中:d1为大臂到球心的距离；d2为小臂到球心的距离。若di＞R，i＝1，2则机械臂与障碍物不会发生碰撞，否则会发生碰撞。

本文提出的碰撞检测策略在一定程度上牺牲了部分机械臂工作空间，但是其简单高效的特点使这种方法易于推广使用。

3 最优中间点选取方法

3.1 建立适应度函数

本文规划机械臂轨迹时，在避障的同时保证轨迹最短且各关节角变化量最小，因此需要从轨迹长度和关节角变化量两个方面建立适应度函数。

记fQ为关节角变化量，fL为机械臂运动轨迹长度，使用插值法对这两个量进行计算。在运动轨迹中插入n-1个点，从而将运动轨迹划分为n段，通过求得相邻插入点的关节角变化量和轨迹长度，再通过累加得到整个轨迹的关节角变化量和轨迹长度，为:

式中:fOB为表征碰撞检测结果的二值变量，若机械臂与障碍物不发生碰撞则fOB＝1，若机械臂与障碍物发生碰撞则fOB＝0；η1、η2为权重因子，角度变化量与轨迹长度数量级相差较大，约为1 000倍，为保证两个因素对适应度函数具有相同的影响程度，取η1＝1、η2＝0.001。

对于多障碍物环境时，需要设置与障碍物相同数量的中间点，此时适应度函数为各中间点适应度函数的累加和。

3.2 改进遗传算法的寻优方法

遗传算法模拟自然界遗传和优胜劣汰的自然选择过程，使种群适应度不断提高，寻得最优解。遗传算法包括染色体编码、初始种群生成、遗传操作(选择、交叉、变异)、解码等过程[9］。其中初始种群生成方法、交叉概率、变异概率对算法寻优效果影响极大[10］。本文对此3个方面进行改进。

(1)染色体编码。使用实数编码方式，由于1个中间点对应6个关节角，那么对于需要设置n个中间点的规划问题，其需要6n个关节角。即编码方式为(θ1，θ2，…，θ6n)。

(2)初始种群生成。在传统遗传算法中使用随机方法产生初始种群，这种方法产生的初始种群适应度不高，没有优势。本文提出以均匀方式产生初始种群，将机械臂工作空间分成z个区域，在每个区域中使用均匀数值产生p个染色体，这样就得到了z×p个染色体，依据适应度函数选择最大的前N个个体作为初始种群。这种初始种群产生方式保证了初始种群在解空间的分散性，有利于全局搜索。

(3)选择操作。以个体适应度为选择依据，每个个体被选概率为f(i)/∑f(i)，式中f(i)为个体i的适应度值，∑f(i)为所有个体适应度之和。这种选择方

式中:qij为第i段轨迹第j个关节角；fQ为整个运动轨迹的 6 个关节角变化量之和；(pi，x，pi，y，pi，z)为第i个插入点的三维坐标；fL为整个运动轨迹长度。

以轨迹长度和各关节角变化量为基础，建立适应度函数fG为:式既保证了优秀个体被选可能性较大，也保证了任意个体被选可能性，保持了种群多样性。

(4)交叉算子。交叉算子可以提高物种多样性，提高物种进化能力。那么，对于物种多样性较差的种群使用较大的交叉概率，提高物种多样性；对于物种多样性较好的种群使用较小的交叉概率，保持物种多样性。本文使用适应度值方差和分布熵衡量物种多样性，并提出相应的交叉概率。记种群规模为N，第i个个体适应度为fi，种群平均适应度为，则第j代种群适应度方差Gj为:

种群分布熵计算方法为:将可行解空间划分为z个子空间，种群个体落在第i个子空间数量记为Ai，那么第j代种群的分布熵Hj为:

式中:pi＝Ai/N。

适应度值方差、种群分布熵越大，表示种群空间分布和适应度值分布越均匀，进化能力越强，此时使用较小交叉概率。由此提出交叉概率为:

(5)变异算子。变异算子可以破坏优良基因，也可以使劣质基因变异为优良个体，因此对于优良基因应使用较小的变异概率，对于劣质基因应使用较大的变异概率。记第i个个体适应度值与平均值之差为θ，则第i个个体的变异概率为:

式中:pi＝1/(1＋e-θ)。 分析式(8)可知，当个体适应度值较大时，变异概率较小，保护了优良基因；当个体适应度值较小时，变异概率较大，劣质基因具有更大机会变异为优良基因。

(6)解码。解码方法与式(2)一致，即通过6个关节角确定中间点的位姿。

4 仿真验证

本文设置了两组仿真，一是确定中间点数量的仿真实验，二是为了验证本文提出方法的路径规划效果，同时比较改进遗传算法与传统遗传算法搜索最优中间点的性能。

4.1 中间点数量的选取

本文中提到，当机械臂轨迹与障碍物发生碰撞时，插入最优中间点，而后使用轨迹基元分别规划起点至中间点、中间点至终点的轨迹，但是中间点数量问题一直未得到明确或验证。

首先设置单障碍物情况，障碍物中心为P＝(50，100，50)，膨胀半径为R＝30 mm，使用轨迹基元直接规划机械臂轨迹会发生碰撞，因此需要插入中间点。当插入一个中间点时，插入中间点为θ＝[0.229，0.748，-0.841，0，0.407，0.383］，轨迹如图 3a 所示。 当插入两个中间点时，寻优得到的两个中间点分别为:

插入两个中间点时的轨迹如图3b所示。

为了对比两条机械臂轨迹的性能，计算两条轨迹的适应度函数值如表1所示。

表1 单障碍物时不同数量中间点对比

表1中迭代次数是指最早出现最优解时的迭代次数。由表1可以看出，当插入两个中间点时，适应度值相差-0.000 9，下降百分比为1.29%，说明增加一个中间点的优化效果不明显，但是增加一个中间点时计算量和迭代次数明显增加，因此存在一个障碍物时应当插入一个中间点。

接下来研究两个障碍物的情况，障碍物中心分别为P1＝(150，50，30)、P2＝(50，100，50)，膨胀半径为R＝30 mm。分别插入一个中间点和两个中间点进行轨迹规划，插入一个中间点时中间点为θ＝[0.493，0.996，-0.438，0，0.1472，0.716］， 插 入 两 个中间点时，两个中间点分别为:

这里不再给出轨迹规划结果，计算两条轨迹的适应度值，结果如表2所示。

表2 两个障碍物时不同数量中间点对比

由表2可知，当工作环境中存在两个障碍物需要躲避时，与插入一个中间点相比，插入两个中间点时适应度值增加了0.006 4，增加百分比为10.4%，优化效果非常明显，因此当存在两个障碍物时应当插入两个中间点。

基于对单个障碍物和两个障碍物的情况分析，本文粗略地规定:当工作空间中存在N≥0个障碍物时，考虑到轨迹优劣和计算量大小，最佳的避障策略是插入相同数量的中间点。

4.2 改进遗传算法与传统算法对比

本节设置了包含两个障碍物的工作环境，用于对比改进遗传算法与传统算法在中间点选取上的性能优劣。 两个障碍物位置分别为P1＝(150，50，30)、P2＝(50，100，50)，膨胀半径为R＝30 mm。 由于环境中包含2个障碍物，因此需要优化出2个中间点，遗传算法编码时需设置12个角度量。

使用改进遗传算法寻优得到的中间点见式(9)，使用传统遗传算法寻优得到的两个中间点为:

而后使用5次多项式分别规划起点至中间点、中间点至中间点、中间点至终点的轨迹，计算两条轨迹各自的适应度值，结果如表3所示。

表3 两种遗传算法的轨迹规划结果

由表2可知，改进遗传算法在迭代131次时出现最优个体，而传统算法在196次时才出现；与传统遗传算法规划的轨迹相比，改进遗传算法寻得的中间点用于轨迹规划时，轨迹长度减少了12.1%，关节角变化量减少了24.4%，适应度值增加了23.6%，轨迹优势非常明显。这是因为改进遗传算法使用均匀方式产生初始种群，非常有利于全局搜索，而且交叉概率和变异概率的设置方法非常有利于种群进化，使得改进遗传算法出现最优个体的迭代次数少，且轨迹适应度值更大。

对于本节设置的两个障碍物工作环境，规划出的最优轨迹如图4所示，图中实线表示实际轨迹，虚线表示点与点之间的连线。以上分析及仿真结果充分证明了插入最优点的机械臂轨迹规划方法，在实现避障的同时，能够规划出长度最短、关节角变化量最小的工作轨迹。

5 结语

本文研究了仿人手臂机械臂避障轨迹规划问题，在避障的同时，保证了关节角变化量最小和轨迹最短。得出了以下结论:(1)插入最优中间点避障算法能够准确规划出一条避障轨迹，且关节角变化量最小和轨迹最短；(2)通过改进遗传算法的初始种群生成方法、交叉变异概率，有利于算法全局寻优能力，且加强了种群进化能力，与传统遗传算法相比，不仅寻优速度快，而且结果更优。