机械臂与障碍物的双层次碰撞检测算法仿真

高嘉材，李丽宏，原 钢

(1. 太原理工大学电气与动力工程学院，山西 太原 030000； 2. 中国煤炭科工集团太原研究院，山西 太原 030000)

1 引言

当今工业社会中出现了许多具有复杂功能的机械臂，如装配机械臂、焊接机械臂、打磨机械臂等。这些机械臂的工作环境复杂，任务量大，对运动的稳定性、安全性提出了较高要求。高效的碰撞检测方法能够提前检测出危险情况，避免对机械臂本体以及环境设施的损坏，是机械臂能够应用于各工作场景的必要条件。

目前常用的检测方法有传感器法和几何体模型法。传感器法的检测效果易受环境影响，成本高，效果不稳定。Lumelsky.V.J等提出在机械臂上布置感知皮肤来检测碰撞，这种方式虽然能够检测出所有碰撞，但检测过程复杂，需要处理的数据很多，检测速度依赖于处理器的性能[1]。相较而言，几何体模型法更为可靠，其主要思想是计算机械臂运动轨迹插补点位置与障碍物几何体模型位置的距离来判断是否碰撞。陈友东[2]等提出将机械臂的关节和连杆等效成球体及胶囊体，将碰撞问题转换为距离计算问题，一定程度上简化了检测过程。这种方法的弊端在于若选择检测的插补点间隔数目过大会损失检测精度，遗漏碰撞情况；反之，会增加碰撞检测的时间，特别是当机械臂与障碍物距离较远时，这种方法的效率很低。因此，连续碰撞检测(continuous collision detection)的概念受到广泛关注。F.Schwarzer[3]等提出了一种自适应动态碰撞检测方法，通过估算运动轨迹的长度判断机械臂与障碍物的碰撞情况；金明杰[4]等将其加入到运动规划过程，提高了路径规划过程的完备性。这种方法能够判断出连续运动轨迹是否与障碍物发生碰撞，虽然速度更快，但该方法仅与障碍物的特定模型进行判断，检测精度有待提高。

本文结合几何体模型法与自适应动态碰撞检测法的思想，提出了一种更为有效的双层次碰撞检测算法。初次判断过程利用自适应动态碰撞检测法快速筛选出运动轨迹中可能与障碍物球体模型发生碰撞的轨迹范围，二次判断过程采用分离轴法准确判断该范围内的轨迹点与障碍物OBB模型的碰撞情况。研究对象选用以UR5为原型的六轴协作机械臂，通过仿真分别与自适应动态碰撞检测法及几何体模型法进行对比，验证了本文算法在确保实时性的同时，检测结果更为准确。

2 机械臂及障碍物模型

本文的研究对象为连杆关节式机械臂，具有六个自由度，由两个连杆依次连接关节模组组成，其结构如图1(a)所示，机械臂自身的碰撞检测算法可以参考文献[5]中的圆柱体等效模型法。使用D-H法[6]建立的机械臂连杆坐标系如图1(b)所示，对应的参数见表1，其中：ai-1表示沿Xi-1方向从Zi-1到Zi移动的距离；αi-1表示绕Xi-1轴Zi-1转到Zi的角度；di表示沿Zi方向从Xi-1到Xi移动的距离；θi表示绕Zi轴Xi-1转到Xi的角度。

图1 六自由度机械臂示意图

表1 六自由度机械臂D-H参数

由正运动学得到的末端连杆坐标系相对世界坐标系的位置方程为

式中s1=sinθ1，c1=cosθ1，c23=cos(θ2+θ3)，s23=sin(θ2+θ3)，其余类推。从上式可看出，六轴机械臂的位置仅与前三个关节的角度有关，末端腕部位置可等效为第三轴末端的位置。因此，可将机械臂等效为三自由度机械臂[2]。

对于障碍物来说，球体模型是完全包围障碍物的最小球体，通过选取物体上最远两点的距离作为直径来获得[7]。虽然球体模型的包围面积过大，包含多余空间，但其生成方法简单，计算速度快，适用于以快速筛选可疑碰撞轨迹为目的的初次判断过程。OBB(Oriented Bounding Box)，即有向包围盒，是根据障碍物自身的几何条件生成的紧凑长方体包围盒子[8]，具有方向性，紧密性好，不会损失避障空间。相较于球体模型，OBB更加逼近不规则障碍物，适用于以准确判断碰撞情况为目的的二次判断过程。机械臂等效模型由qa状态运动到qb状态与障碍物球体模型的平面位置关系如图2，此时的运动轨迹可能与障碍物碰撞。

图2 机械臂等效模型与障碍物位置关系

3 自适应动态碰撞检测

3.1 算法原理

机械臂在关节空间中运行时，碰撞检测需要在轨迹插补运动中判断自身与障碍物之间是否出现重合或越界的情况。以二维平面为例分析动态碰撞检测算法的理论基础。

假设物体1由起始位置a运动到终点位置c，物体2由起始位置b运动到终点位置d，其一般的运动轨迹为曲线ac、bd；当物体1与物体2在运动中发生碰撞，曲线ac与曲线bd交于o点，两种运动状态如图3所示。

图3 平面运动示意图

在发生碰撞的情况中可推出

根据三角形两边之和大于第三边定理，得到

由上述两个式子可推出：

因此得到结论：若平面中两条曲线ac、bd相交，则曲线ac、bd的长度之和大于等于直线段ab、cd的长度之和。进而推出逆否命题，若曲线ac、bd的长度之和小于直线段ab、cd的长度之和，即满足(1)式时，两条曲线ac、bd不相交。

(1)

将上述结论扩展到三维空间，得到结论1：

若空间中两物体Ai、Aj分别沿曲面从qa状态到qb状态的最大移动距离λi(qa，qb)、λj(qa，qb)之和小于Ai、Aj分别在qa、qb状态时的直线距离ηij(qa)、ηij(qb)之和，即(2)式成立，则Ai、Aj在从qa状态到qb状态的运动过程中不会发生碰撞。

λi(qa，qb)+λj(qa，qb)<ηij(qa)+ηij(qb)

(2)

采用反证法证明此结论：

假设两物体Ai、Aj的运动过程在满足(2)式的前提下发生碰撞，设发生碰撞时的状态为qc，则有

进而推出

又由已知条件得

进而推出：λi(qa，qb)+λj(qa，qb)≥ηij(qa)+ηij(qb)。这与假设条件(2)式成立相矛盾，从而结论1得证。

将结论1应用于机械臂Ai与固定障碍物Aj的碰撞检测情况中，此时λj(qa，qb)=0，则(2)式变为

λi(qa，qb)<ηij(qa)+ηij(qb)

(3)

即在已知机械臂移动的最大距离λi(qa，qb)及qa、qb状态时机械臂与障碍物的直线距离ηij(qa)、ηij(qb)的条件下，若(3)式成立，则机械臂从qa状态到qb状态的运动轨迹不会与障碍物发生碰撞。

3.2 估计运动轨迹的最大长度

文献[3]中的引理2给出了估算最大运动轨迹长度的公式，内容如下：

当机械臂从qa状态运动到qb状态时，第i轴运动轨迹的最大长度为

4 碰撞检测过程

4.1 初次判断

初次判断过程将自适应动态碰撞检测法与二分法融合，对运动轨迹与障碍物球体模型进行判断，筛选出可疑碰撞轨迹范围，过程如下：

已知轨迹从qa状态运动到qb状态的最大移动距离λi(qa，qb)，并计算出两种状态下机械臂与球体模型的直线距离ηij(qa)、ηij(qb)，接着判断：

若λi(qa，qb)≥ηij(qa)+ηij(qb)

若qa到qb中的轨迹插补点数目大于2

获取qa与qb之间的中间点状态qmid，对(qa，qmid)、(qmid，qb)按照上述步骤计算；

否则：

(qa，qb)之间可能与障碍物发生碰撞，进行二次判断；

否则：

(qa，qb)轨迹之间不发生碰撞。

相较于文献[4]中使用的自适应动态检测算法需要不停计算中间状态轨迹点与特定障碍物模型的距离判断是否碰撞，本文算法去除了该计算部分，初次判断过程仅将可能碰撞的轨迹段筛选出来，由二次判断来进行更准确的检测。

4.2 二次判断

对于可能发生碰撞的轨迹段需要进行更严格的检测，使用分离轴法判断该范围内的轨迹点与障碍物OBB模型的碰撞关系，具体过程如下：

设障碍物OBB模型的长度宽度为2d，高度为h，上下端面的中心点AB的坐标分别为(xA，yA，zA)，(xB，yB，zB)，轨迹点M坐标为(x1，y1，z1)，如图4所示。以世界坐标系中的X轴、Y轴、Z轴作为分离轴，将OBB模型与轨迹点分别向X轴、Y轴、Z轴投影，(4)式成立则判断轨迹点M与障碍物碰撞，否则该点不会与障碍物碰撞。

(4)

图4 轨迹点与OBB投影示意图

若待检轨迹段内任一轨迹点满足(4)式则终止检测并得出机械臂会与障碍物碰撞的结论；否则该轨迹段不会与障碍物发生碰撞。当所有可疑轨迹段都判断为不碰撞情况时，得出最终结论：机械臂的规划轨迹不会与障碍物发生碰撞。

5 实验分析

本节将通过两次实验从运行时间方面对比双层次碰撞检测算法、自适应动态碰撞检测法及几何体模型法，其中后两种算法用到的障碍物模型皆为球体模型。

实验平台为个人笔记本，参数配置为：Intel(R)core(TM)i5-8250U CPU @1.6GHz，8GB内存，使用matlab软件编写三种算法的脚本程序。测试数据选用关节空间下以[-30；-120；120；-130；120；0]为各轴起始角度，[-90；-90；180；-90；90；50]为各轴终止角度，插补周期为100ms的S型曲线速度控制算法[10]规划得到的95组末端轨迹点，各轴位置、速度变化曲线及世界坐标系中轨迹点的位置变化如图5所示。

图5 机械臂各部变化图

1)运动轨迹不与障碍物球体模型发生碰撞的情况。设障碍物的球体模型半径为20，中心点坐标为(250，-200，50)，在xoy平面上的投影如图6所示。此时，λ3(qa，qb)=504.64，ηij(qa)+ηij(qb)=577.34，(3)式成立，本文算法及自适应动态碰撞检测算法得出不发生碰撞的结论；几何体模型法经过95次计算轨迹点与障碍物的距离后得出不碰撞的结论。分别记录三种算法运行100次的平均时间，结果见表2。

图6 未碰撞情况运动轨迹与障碍模型投影图

表2 未碰撞情况下各算法运行时间记录表

由实验结果看出，对于运动轨迹不会与障碍物球体模型发生碰撞的情况，三种算法都能得到正确结论。双层次碰撞检测算法仅需运行初次判断过程即可得到正确结果，因此与自适应动态碰撞检测法的运行时间大体一致；相较与几何体模型法，双层次碰撞检测算法仅需一次判断即可得到正确结果，运行时间缩短了22.96%。

上述结果说明在运动轨迹不与障碍物球体模型发生碰撞的情况中，双层次碰撞检测算法的检测结果准确，计算量小，速度快。

图7 碰撞情况运动轨迹与障碍模型投影图

2)运动轨迹与障碍物球体模型碰撞但不与OBB模型碰撞的情况。设障碍物中心点坐标为(250，-140，108)，球体模型半径为20，OBB模型的三条棱长为37、14、6，在xoy平面上的投影如图7所示。本文算法的初次判断过程经过6次迭代筛选后得到第45、46、47三组可疑轨迹点，坐标分别为(254.56，-130.82，117.75)、(250.32，-131.64，108.83)、(245.98，-132.12，100.08)，二次判断过程的(4)式判别情况见表3，得出不与OBB模型发生碰撞的结论；自适应动态碰撞检测算法在仅与球体模型判断的条件下得出发生碰撞的结论；几何体模型法在经过95次距离计算后同样得出了发生碰撞的结论。再次记录三种算法运行100次的平均时间，结果见表4。

表3 二次判断可疑点碰撞情况分析表

表4 碰撞情况下各算法运行时间记录表

由实验结果看出，相较与自适应动态碰撞检测法，双层次碰撞检测算法多了二次判断步骤，结果更准确，但检测速度稍有减慢；与几何体模型法相比，双层次碰撞检测算法在得到准确结果的同时，运行时间缩短了18.68%。

上述结果表明，在运动轨迹与障碍物球体模型发生碰撞但不与OBB模型碰撞的情况中，双层次碰撞检测算法在不失速度性的前提下，能够得到更为准确的检测结果。

6 结论

本文提出了一种机械臂与障碍物之间的双层次碰撞检测算法，初次判断过程通过自适应动态碰撞检测算法快速检测轨迹是否与障碍物球体模型碰撞并筛选出可能碰撞的轨迹点范围；二次判断过程采用分离轴法判断可疑轨迹范围是否与障碍物OBB模型碰撞，得到最终的检测结果。实验结果表明，本文算法能提升自适应动态碰撞检测法的准确性，同时能够克服几何体模型法计算量大、实时性较差的问题，更适用于对实时性、准确性有较高要求的机械臂控制系统。