基于精准碰撞检测算法的机械臂避障轨迹规划

摘要: 为完成6自由度机械臂在多种障碍物环境下的关节轨迹规划，提出一种精确的碰撞检测算法，结合6次多项式关节空间轨迹规划算法完成机械臂的轨迹规划避障算法。通过对障碍物与机械臂进行几何模型简化将6自由度机械臂的碰撞检测问题转化为模型之间最小距离计算问题；结合机械臂正逆运动学与碰撞检测结果，创建遗传优化算法的适应度函数，使机械臂在完成避障的同时优化各关节轨迹长度和转动角度，将机械臂避障轨迹规划问题转化为约束条件下的多目标优化问题。最后通过MATLAB对算法进行仿真实验，结果表明，该算法在多种障碍物的环境下能够规划出满足要求的运动轨迹。

1. 引言

随着工业自动化的不断发展，工业生产制造领域的很多人工作业岗位被工业机器人取代，工业机器人作为形成工业自动化生产线的关键设备，大力发展工业机器人是我国实现自动化生产线和智能车间的重要措施。工业机器人应用在各种不同的作业环境中，完成简单机器人轨迹规划外，需考虑不同作业环境中存在的障碍物。不同工作环境中，障碍物的位置、形状、大小等各不相同，对工业机器人进行轨迹规划的避障研究是实现自动化生产线的关键。根据实际安全生产与稳定的要求，首先，工业机器人避障轨迹规划需满足全局避障，不仅仅是满足末端执行器的避障，而是做到各个连杆的避障 [1]；其次机械臂关节空间轨迹规划的各个关节的角速度与角加速度必须连续，以提高机械臂的运动效率和降低关节冲击 [2]；最后，为得到更加安全节能的轨迹，在进行避障轨迹规划的同时需要优化各关节运动角度以及各连杆运动轨迹长度。

目前应用在机械臂碰撞检测的算法主要有传感器、图像、空间几何模型等。传感器的检测精度受环境影响较大，不适用于复杂的作业环境中 [3]；基于图像的检测算法计算量过大，对硬件要求高，不适合实际应用 [4] [5]；基于空间几何模型的方法相对前两种算法，计算量小、结果稳定以及检测效率高，更适合实际应用 [6]。包卫卫等通过对6自由的检修机械臂进行研究，采用AABB包围盒和圆柱体简化机械臂模型 [7]。通过对空间线段间的几种情况进行分析，对机械臂可能发生碰撞的部位进行计算，但该算法耗时巨大。吴长征等采用胶囊体为机器人连杆几何模型，通过计算两胶囊体之间的距离进行碰撞检测 [8]，但是此方法将两线段最小距离直接简化为线段端点及两线段延长为直线后共垂线交点之间的距离，这样的处理方式忽略了一些可能发生碰撞的问题；王新达等也采用了上述的简化方法，提出一种针对多自由度机械臂在运动过程中自身发生碰撞的问题 [9]，上述论文均只进行了机械臂碰撞检测的自检，未对机械臂在工作环境中的障碍物进行碰撞检测。

现有的机械臂轨迹规划主要分为笛卡尔空间轨迹规划和关节空间轨迹规划 [10]，两者各有优缺点，其中笛卡尔空间规划可以很好的确定机械臂末端执行器的位置，但是笛卡尔轨迹规划存在计算量大，对芯片的性能要求高；关节空间轨迹规划在操作时非常简便，而且可以有效避免机械臂在运动空间中的奇异点，实时性高，计算量小 [11]。针对机械臂避障轨迹规划需求，本文采用关节空间轨迹规划。祁若龙、谭燕等均采用遗传算法搜索中间点的方式进行空间机械臂避障轨迹规划 [12] [13]，采用中间点的方式进行避障轨迹规划，使得机械臂在中间点时角速度、角加速度为零，运动不连续。马宇豪等提出了一种基于六次多项式轨迹规划的避障算法 [14]，但该算法进行避障类型单一且精度不高，无法完成对多种障碍物类型的避障。江鸿怀提出基于粒子群优化算法的五自由度机械臂轨迹规划 [15]，该算法对机械臂运动轨迹的长度及角度进行优化，但是粒子群算法容易陷入局部最优，所求解并非最优解。综上所述，上述均为对存在多种形状的障碍物的环境进行有效的避障轨迹规划。本论文提出一种基于精确碰撞检测的机械臂避障轨迹规划。该方法能够完成在不同形状障碍物下的避障要求，并且可以在避开障碍物的同时对机械臂运动轨迹进行最大限度的优化。

结合遗传优化算法，可以将避障轨迹规划问题转化为多目标优化问题，根基机械臂避障轨迹要求，约束条件为：机械臂不能与障碍物发生碰撞，角速度和角加速度连续；优化目标为：机械臂末端执行器移动位移长度和关节转动角度。算法设计主要分为以下几步：1、机械臂与障碍物几何模型简化2、碰撞检测算法设计3、轨迹规划方案4、遗传优化算法设计，根据以上几点通过matlab进行仿真验证。

2. 机械臂与障碍物模型简化

首先，对机器人进行建模，为保证研究具有一般性，本文采用被广泛应用的两连杆六自由度的串联机械臂为研究对象，由1个腰关节、1个肩关节、3个腕关节组成。其中四、五、六关节垂线相互垂直且交与同一点 [16]，共同决定了末端执行器的姿态，其中图1为机械臂腕部结构运动简图；图2为机械臂连杆坐标简图。机械臂具体的D-H参数变量如表1。

机械臂的连杆多为类圆柱体或类六面体，故机械臂各个连杆简化为圆柱体。再对障碍物进行几何模型简化，由于障碍物的不确定性，比较难建立准确的模型，充分结合考虑机械臂实际避障轨迹规划中的安全性和高效性。本文考虑两种障碍物简化模型：1、采用球体几何模型对障碍物进行简化2、采用圆柱体几何模型对障碍物进行简化。如图3、图4所示，其中灰色部分为障碍物。其中球形包络面是以O1为圆心，R1为半径的包络面，圆柱体包络面是以O2为底面圆心，R2为底面半径，L为圆柱体高的包络面。

Figure 1. 3R wrist joint structure diagram

图1. 3R腕部关节结构简图

Figure 2. Schematic diagram of linkage of manipulator arm

图2. 机械臂连杆坐标简图

连杆i	θ i	α i − 1	d i	a i − 1
1	θ 1	0	0	0
2	θ 2	-pi/2	d 2	0
3	θ 3	0	0	a 2
4	θ 4	-pi/2	d 4	a 3
5	θ 5	Pi/2	0	0
6	θ 6	-pi/2	0	0

Table 1. D-H parameter table of space manipulator

表1. 空间机械臂D-H参数表

Figure 3. Spherical envelope surface of obstacles

图3. 障碍物球形包络面

Figure 4. Envelope surface of obstacle cylinder

图4. 障碍物圆柱体包络面

3. 精准碰撞检测算法

根据机械臂和障碍物的几何简化模型，设机械臂连杆的最大径向半径为RL，球形包络面障碍物的半径为R1，圆柱体包络面障碍物的径向半径为R2。故机械臂与障碍物之间的碰撞检测可以转化为两者之间最小距离的求解，求解过程可以分为圆柱体与圆柱体、圆柱体与球体之间的距离求解，设两种情况下的最小距离分别为 dccdcc 、 dcsdcs。若满足 dccdcc 大于 RL+R2RL+R2 、 dcsdcs 大于 RL+R1RL+R1，则不会发生碰撞，下面分别给出求解过程。

3.1. 圆柱体与圆柱体间最小距离算法

设两圆柱体的中轴线分别为L1、L2，半径分别为R1、R2，两端端点坐标分别为 E(x1,y1,z1)E(x1,y1,z1) 、 F(x2,y2,z2)F(x2,y2,z2) ； T(x3,y3,z3)T(x3,y3,z3) 、 H(x4,y4,z4)H(x4,y4,z4)。如图5所示。

Figure 5. Two arbitrary cylinders in space

图5. 空间任意两圆柱体

A、B分别是L1、L2上的任意一点，用圆柱体两端坐标点可以表示为：

根据公式(5)可以解出参数 s,ts,t。由参数 s,ts,t 的取值范围，可以将空间中两条线段的位置情况投影分为9种，详情见图6 (若两线段共面，图6(a)~(i)视为本身，若异面，则视为两线段向各自所在的线段的公垂线的法面的投影图)。其中 t<0t<0 表示两线段公垂线交点在TH的反向延长线上， 0≤t≤10≤t≤1 表示公垂线交点在TH上， t>1t>1 表示公垂线交点在TH的延长线上，s表示的含义与t相同。

Figure 6. Position projection relationship of two line segments in space

图6. 空间两线段位置投影关系

空间中两线段的投影都存在几种位置情况，以图6(a)为例 t<0,s<0t<0,s<0，以TH为参考线段，线段EF延向量TH方向依次右移，可以得出两线段最小距离的五种情况，如图7，其中 dccdcc 表示两空间线段间的最小距离。

Figure 7. t<0,s<0t<0,s<0 The minimum distance between two line segments

图7. t<0,s<0t<0,s<0 时两线段的最小距离

对以上五种情况进行总结，根据图7(a)、图7(b)、图7(c)，可以很容易的得出两线段间的最小距离是点T到线段EF间的距离，根据图7(d)、图7(e)，得出两线段间的最小距离是点E到线段TH间的距离，故对其他8种空间投影关系进行分析，可以得出两线段间最小距离 dccdcc。

其中公式(5)中的 dE,dF,dT,dHdE,dF,dT,dH 分别表示点E到线段HT上的最小距离，点F到线段HT上的最小距离，点T到线段EF上的最小距离，点H到线段EF上的最小距离。 dCOEFdCOEF 表示线段EF和线段TH间公垂线的距离。

综上所述，空间两线段间最小距离的计算转化为点到线段的距离，下面具体计算点到线段的距离，线段与点的位置分布关系可以有三种情况，如图8，显然当点E与线段TH的垂足M在向量的反向延长线上时，最小距离为点E到点T的距离，当点E与线段TH的垂足M位于线段TH上时，最小距离为点E到垂足M的距离，当点E与线段TH的垂足M位于向量TH的延长线上时，最小距离为点E到点H的距离。

对于求解点到直线的距离，采用矢量法，定义参数 v=TH⋅TE|TH|2v=TH⋅TE|TH|2 ；根据参数v的取值不同有以下分段函数：

Figure 8. Relationship between points and line segments

所以由公式(6)~(8)可以求解出两圆柱体间的最小距离 dccdcc。故通过判断 dccdcc 是否大于两圆柱体半径之和，即 dcc−R1−R2>0dcc−R1−R2>0，则不会碰撞；反之，将发生碰撞。

3.2. 球体与圆柱体间最小距离计算

设圆柱体半径为 RLRL，球体半径为 R1R1，根据上述原理，球体与圆柱体之间的碰撞检测，等同于计算球心到圆柱体中心轴上的距离，由于上述圆柱体与圆柱体之间最小距离计算时，已经包含了点到直接距离的计算，故在此不在重复。既 dcs−RL−R1>0dcs−RL−R1>0，不会发生碰撞；反之，将发生碰撞。在机械臂轨迹规划后，要满足机械臂在整个运动轨迹中，两个连杆均不会与障碍物发生碰撞，才实现了机械臂的避障策略。

4. 关节空间轨迹规划与遗传优化算法

4.1. 机械臂关节空间轨迹规划

本文采用机械臂关节空间轨迹规划方法，常用的关节空间轨迹方法有多项式插值、梯形速度插补、样条曲线插补等。为保证机械臂运动加速度平滑、避免产生冲击和震动，现在应用最多的五多次项式插值法对机械臂进行轨迹规划，但是五次多项式插值规划出来的角度、角速度、角加速度是唯一的，当规划出来的路径上存在障碍物，由于唯一性，不能做到避障的要求。故本文采用六次多项式插值法进行轨迹规划，增加一个六次项系数C，通过改变系数C来调整轨迹，从而达到避障的目的。六次多项式轨迹函数如下：

其中 θθ 、 ωω 、 αα 分别表示关节的角度、角速度、角加速度，根据6次多项式各阶的系数，可以规划出机械臂各关节角度、角速度、角加速度都连续的轨迹。

4.2. 遗传优化算法

遗传算法简称GA (genetic algorithms)，是1962年由美国Michigan大学的Holland教授提出的模拟自然遗传机制和生物进化论而成的一种并行随机搜索算法 [17]。该算法已经被各界学者证实了在求解非线性、不可微、多极值和高维的复杂函数具有很好的鲁棒性。因为本文研究的机械臂就是高维非线性的问题，所以选择遗传算法进行优化。

遗传算法基本步骤为：1、选择在优化对象的取值范围内选择一部分个体进行编码，组成初始种群；2、按照选择的适应度函数对每个个体进行适应度计算并通过遗传算法中的轮赌盘等算法每个个体进行筛选，选择适应度高的个体组成的新一代种群；3、再通过遗传算法中的复制、变异以及交叉产生新的子代，用子代中适应度更高的个体取代父代中的个体，又产生新的种群。这样周而复始，种群中的适应度不断提高，直达满足终止条件停止迭代。

4.2.1. 适应度函数

机械臂在实际应用中，在满足避障的基本要求下，应尽最大可以的做到高效和节能。故为减少不必要的运动，适应度函数在满足避障的同时考虑机械臂各个连杆的移动距离最短以及各个关节转动角度最小。设机械臂第一连杆运动轨迹的长度为 fL1fL1，第二连杆即末端执行器的运动轨迹长度为 fL2fL2，各连杆运动轨迹的总长度为 fL1+fL2fL1+fL2，各个关节角变化总量为 fQfQ，碰撞检测结果为 fcsfcs。采用六次多项式插值法规划出的轨迹分为n小段，各参数计算公式如下：

其中，q、p、 θθ 分别表示中间连杆坐标、末端执行器坐标、关节角。由于遗传算法求的是适应度函数的最小值，故设计适应度函数如下：

其中 fcsfcs 为碰撞检测的结果，当机械臂与障碍物发生碰撞时， fcs=0fcs=0，从而 fC=0fC=0，当机械臂与障碍物不发生碰撞时， fcs=1fcs=1，即 fC<0fC<0。故在进行遗传优化时，一定会将发生碰撞的参数C值滤除。

4.2.2. 遗传优化算法流程

通过优化参数C达到避障效果的算法流程图，如图9所示，首先，对机械臂、障碍物进行建模和表示，同时，由遗传算法生成函数产生初始种群C，然后通过计算碰撞检测结果 fcsfcs 、各关节角的变化总量 fQfQ 和各连杆运动轨迹的总长度为 fL1+fL2fL1+fL2，进而求出适应度 fCfC，通过遗传算法选择、交叉、变异、替换得到子代，多次迭代进化后达到终止条件，找出使适应度最小的最优参数C，最后，带入六次多项式中得到最优的运动轨迹。

Figure 9. Flow chart of obstacle avoidance trajectory planning algorithm

图9. 避障轨迹规划算法流程图

5. 仿真实验

本文采用MATLAB 进行实验仿真，主要使用MATLAB Robotics Toolbox工具箱和优化工具箱进行机械臂避障仿真实验。首先通过MATLAB Robotics Toolbox工具箱建立两杆六自由度机械臂模型，设置两连杆的长度分别为 L1=L2=5 cmL1=L2=5 cm。连杆的最大径向半径为 RL=0.5 cmRL=0.5 cm，设置机械臂初始位置坐标是 α0=[0,0,5]α0=[0,0,5]，终止位置坐标为 α1=[0,5,0]α1=[0,5,0]，通过机械臂逆运动学可将笛卡尔坐标转化为关节角度表示的形式，此外，设step = 50，即设置50个插值点对机械臂进行轨迹规划。

对MATLAB优化工具箱进行参数配置，求解程序选择ga-Geneice Algoriathm；适应度函数(Fitness function)根据式(14)编写；研究对象机械臂是六自由度，优化对象 C=[C1,C2,C3,C4,C5,C6]C=[C1,C2,C3,C4,C5,C6]，故设置变量个数(number of variable)为6；种群大小(population size)设置为200；选择函数(selection function)设置为Roulette(轮盘赌算法)；杂交、突变、迁移设置采用默认值；终止条件(stopping criteria)设置迟滞代数为20代，精度为10−6。

5.1. 球形障碍物避障实验

5.1.1. 单个球形障碍物

设置障碍物球心坐标为 O=[1,3,2.5]O=[1,3,2.5]，障碍物半径为 R1=2 cmR1=2 cm，即机械臂各连杆中轴线到球形障碍物中心点之间的最小距离 dcsdcs 大于安全距离 S=RL+R1=2.5 cmS=RL+R1=2.5 cm 时，机械臂不会发生碰撞。遗传算法迭代过程中最优适应值和平均适应值的迭代过程及最优参数C值如图10所示，经过遗传迭代42次，得出最优参数 C=[19.1339,−43.6603,32.5593,30.3290,−2.0237,−15.4773]C=[19.1339,−43.6603,32.5593,30.3290,−2.0237,−15.4773]。最优适应值 fC=−0.0383fC=−0.0383，连杆L1运动轨迹的长度 fL1=7.1709 cmfL1=7.1709 cm，末端执行器运动轨迹的长度 fL2=9.3944 cmfL2=9.3944 cm，各个关节转动总量 fQ=9.5296 radfQ=9.5296 rad。机械臂运动轨迹如下图11所示，其中曲线s1、s2分别表示连杆L2和连杆L1运动轨迹。

机械臂两连杆运动过程中距障碍物球心之间的距离变化过程如图12所示，其中曲线d1、d2分别表示连杆L1、L2中心线距球形障碍物中心点的距离变化过程。连杆1距球形障碍物中心点的最小距离 dcs1=3.431 cm>2.5 cmdcs1=3.431 cm>2.5 cm，连杆2距球形障碍物中心点的最小距离 dcs2=2.504 cm>2.5 cmdcs2=2.504 cm>2.5 cm，故机械臂运动过程中与障碍物之间的距离均大于安全距离，机械臂不会发生碰撞。

机械臂轨迹规划过程中角度、角速度、角加速度变化过程分别如图13(a)~(c)所示，由图可知，机械臂各个关节的角速度、角加速度均连续、平滑；避免了机械臂自身的冲击和震动。因此，规划出一条无碰撞运动轨迹的同时对连杆运动轨迹长度以及关节转动角进行协同优化的理性运动轨迹。

Figure 10. Iterative process of genetic algorithm

图10. 遗传算法迭代过程

Figure 11. Motion trajectory diagram of manipulator arm

图11. 机械臂运动轨迹图

Figure 12. Distance variation process between two connecting rods and the center point of a spherical obstacle

图12. 两连杆距球形障碍物中心点距离变化过程

(a) 关节角度变化图 (b) 关节角速度变化图 (c) 关节角加速度变化图

Figure 13. Trajectory planning diagram of manipulator joint space

图13. 机械臂关节空间轨迹规划图

5.1.2. 多个球形障碍物

改变机械臂始末位置，其他条件不变，设机械臂起始位置 α0=[0,5,0]α0=[0,5,0]，终止位置 α1=[−3,4,6]α1=[−3,4,6]，两球形障碍物的球心坐标为 O1=[0.5,3,3];O2=[2,−3,2]O1=[0.5,3,3];O2=[2,−3,2] ；两障碍物半径大小均为 RO1=RO2=2 cmRO1=RO2=2 cm，故机械臂各连杆中轴线到障碍物中心点的最小距离 dcsdcs 大于安全距离 S=RL+RO1=2.5 cmS=RL+RO1=2.5 cm 时，机械臂不会发生碰撞。遗传算法迭代过程中最优适应值和平均适应值的迭代过程以及最优参数C值如图14所示，经过遗传迭代21次，得出最优参数 C=[7.400,24.5883,−14.8077,31.1375,7.4239,−5.9574]C=[7.400,24.5883,−14.8077,31.1375,7.4239,−5.9574]。最优适应值 fC=−0.0203fC=−0.0203，连杆L1运动轨迹的长度 fL1=14.3200 cmfL1=14.3200 cm，末端执行器运动轨迹的长度 fL2=18.3640 cmfL2=18.3640 cm，各个关节转动总量 fQ=16.5967 radfQ=16.5967 rad。机械臂运动轨迹如下图15所示，其中曲线s1、s2分别表示连杆L2和连杆L1运动轨迹。

Figure 14. Iterative process of genetic algorithm

图14. 遗传算法迭代过程

Figure 15. Motion trajectory of the manipulator arm

图15. 机械臂运动轨迹

机械臂两连杆运动过程中距两障碍物球心之间的距离变化过程如图16所示，其中曲线d11、d12分别表示障碍物1球心距连杆L1、L2中轴线的距离变化过程，曲线d21、d22分别表示障碍物2球心距连杆L1、L2中轴线的距离变化过程。可知，连杆L1、L2距离球心障碍物1中心点的最小距离分别为 dcs11=2.999 cmdcs11=2.999 cm 、 dcs12=2.514 cmdcs12=2.514 cm ；连杆L1、L2距离球形障碍物2中心点的最小距离分别为 dcs21=2.834 cmdcs21=2.834 cm 、 dcs22=3.472 cmdcs22=3.472 cm，故机械臂在运动过程中，连杆到障碍物的距离均大于安全距离S，机械臂不会与障碍物发生碰撞。

Figure 16. Distance variation process between two connecting rods and a spherical obstacle

图16. 两连杆与球形障碍物之间距离变化过程

机械臂轨迹规划过程中角度、角速度、角加速度变化过程分别如图17(a)~(c)所示，同样机械臂各个关节的角速度、角加速度均连续、平滑。

(a) 关节角度变化图 (b) 关节角速度变化图 (c) 关节角加速度变化图

Figure 17. Trajectory planning diagram of manipulator joint space

图17. 机械臂关节空间轨迹规划图

5.2. 圆柱形障碍物避障实验

保持其他条件不变，设障碍物两端点坐标分别为 T=[2.5,2.5,2.5]T=[2.5,2.5,2.5] ； H=[−2.5,2.5,2.5]H=[−2.5,2.5,2.5]。圆柱体障碍物半径 RC=0.7RC=0.7。故机械臂连杆中轴线到障碍物圆柱体中轴线之间的安全距离为 S=RL+RC=1.2 cmS=RL+RC=1.2 cm，即连杆与障碍物圆柱体间最小距离 dcc>Sdcc>S 时，机械臂不会发生碰撞。遗传算法迭代过程中最优适应值和平均适应值的迭代过程以及最优参数C值如图18所示，经过遗传迭代107次，得出最优参数 C=[19.1895,−27.2539,49.5119,−11.76086.6817,15.3590]C=[19.1895,−27.2539,49.5119,−11.76086.6817,15.3590]。最优适应值 fC=−0.03979fC=−0.03979，连杆L1运动轨迹的长度 fL1=7.1935 cmfL1=7.1935 cm，末端执行器运动轨迹的长度 fL2=8.0961 cmfL2=8.0961 cm，各关节转动总量 fQ=9.8402 radfQ=9.8402 rad。机械臂动轨迹如图19所示，其中曲线s1、s2分别表示连杆L2和连杆L1运动轨迹。

Figure 18. Iterative process of genetic algorithm

图18. 遗传算法迭代过程

Figure 19. Motion trajectory of the manipulator arm

图19. 机械臂运动轨迹

机械臂两连杆中轴线距圆柱体障碍物中轴线间的距离变化如图20所示，其中曲线d1、d2分别表示连杆L1、L2中心线距圆柱体障碍物中轴线的距离变化过程，连杆L1距障碍物中轴线距离的最小值 dcc1=2.662 cm>1.2 cmdcc1=2.662 cm>1.2 cm，连杆L2距障碍物中轴线距离的最小值 dcc2=1.806 cm>1.2 cmdcc2=1.806 cm>1.2 cm，故机械臂运动过程中与障碍物间距离均大于安全距离，机械臂不会发生碰撞，验证得到算法在存在圆柱体障碍物时，进行避障轨迹规划同样有效。

Figure 20. Distance variation process of two connecting rods from the central axis of the cylinder obstacle

图20. 两连杆距圆柱体障碍物中轴线的距离变化过程

机械臂轨迹规划过程中角度、角速度、角加速度变化过程分别如图21(a)~(c)所示，同样机械臂各个关节的角速度、角加速度均连续、平滑。

(a) 关节角度图 (b) 关节角速度图 (c) 关节角加速度图

Figure 21. Trajectory planning of manipulator joint space

图21. 机械臂关节空间轨迹规划

5.3. 不同障碍物同时存在的避障实验

保持机械臂条件不变，验证球形障碍物和圆柱体障碍物同时存在时算法的有效性，设球形障碍物中心点 O0=[2.5,3,−3]O0=[2.5,3,−3]，半径 R0=1 cmR0=1 cm ；圆柱体障碍物两端坐标分别为 T=[2.5,2.5,2.5]T=[2.5,2.5,2.5] 、 H=[−2.5,2.5,−2.5]H=[−2.5,2.5,−2.5]，半径 R1=0.7 cmR1=0.7 cm。故机械臂与球形障碍物的安全距离 S1=1.5 cmS1=1.5 cm ；与圆柱体障碍物的安全距离 S2=1.2 cmS2=1.2 cm。遗传算法迭代过程中最优适应值和平均适应值的迭代过程以及最优参数C值如图22所示，经过遗传迭代33次,得出最优参数 C=[17.8894,−26.8963,49.7783,−10.1727,3.3489,−14.1221]C=[17.8894,−26.8963,49.7783,−10.1727,3.3489,−14.1221]。最优适应值 fC=−0.0315fC=−0.0315，连杆L1运动轨迹的长度 fL1=10.3164 cmfL1=10.3164 cm，末端执行器运动轨迹的长度 fL2=12.1008 cmfL2=12.1008 cm，各个关节转动总量 fQ=9.3382 radfQ=9.3382 rad。机械臂运动轨迹如下图23所示，其中曲线s1、s2分别表示连杆L2和连杆L1运动轨迹。

机械臂两连杆距障碍物中心的距离变化过程如图24所示，其中曲线dc1、dc2分别表示连杆L1、L2距圆柱体障碍物中轴线的距离变化过程；曲线ds1、ds2分别表示连杆L1、L2距球形障碍物中心线的距离变化图。可知，连杆L1、L2距圆柱体障碍物的最小距离分别为 dcc1=1.205 cmdcc1=1.205 cm 、 dcc2=1.354 cmdcc2=1.354 cm，两者均大于安全距离S2；连杆L1、L2距球形障碍物的最小距离分别为 dcs1=1.545 cmdcs1=1.545 cm 、 dcs2=1.579 cmdcs2=1.579 cm，两者均大于安全距离S1。故机械臂与障碍物之间不会发生碰撞。

Figure 22. Iterative process of genetic algorithm

图22. 遗传算法迭代过程

Figure 23. Motion trajectory of the manipulator arm

图23. 机械臂运动轨迹

机械臂轨迹规划过程中角度、角速度、角加速度变化过程分别如图25(a)~(c)所示，同样机械臂各个关节的角速度、角加速度均连续、平滑。

综上，经过MATLAB中的多组仿真实验，验证了该算法在多个障碍物和多种障碍物的环境中进行避障轨迹规划的有效性，并且在完成避障的同时，对机械臂各个关节角度及连杆轨迹长度进行优化，规划出机械臂各个关节角速度、角加速度均连续的轨迹，故概算法满足了实际作业的中的高效、稳定、节能的要求。

Figure 24. Distance variation process between two connecting rods and obstacles

图24. 两连杆距障碍物距离变化过程

(a) 关节角度变化图 (b) 关节角速度变化图 (c) 关节角加速度变化图

Figure 25. Trajectory planning of manipulator joint space

图25. 机械臂关节空间轨迹规划

6. 结论

本文提出的基于精准碰撞检测的机械臂轨迹规划避障算法，通过MATLAB的仿真实验，验证了该算法能够完成环境中存在不同类型、不同大小、不同位置的单个障碍物或多个障碍物的避障轨迹规划，两种障碍物的包络面相对于单一的球形包络面碰撞检测极大的提高了碰撞检测的精度；扩大的机械臂避障范围。并且该算法在机械臂完成避障的同时优化了机械臂连杆运动轨迹长度和关节转动角度，使得机械臂更加安全、节能、高效的工作。不过本文采用MATLAB进行程序编写，在工程应用中适用性较小，并且规划六次多项式参数C的时间相对较长，后续可以考虑利用C语言和其他优化算法来提高适用性与运行速度；此外本文碰撞检测针对类似圆柱体与球体形状的障碍物进行检测，后续可以不断扩展不同障碍物类型间的碰撞检测。