基于三维物体特征图的动态碰撞检测方法

马 礼1，王 强1，杨银刚2，马东超1，张永梅1

（1.北方工业大学计算机学院，北京 100144；2.北京信息高技术研究所，北京 100094）

基于三维物体特征图的动态碰撞检测方法

马 礼1，王 强1，杨银刚2，马东超1，张永梅1

（1.北方工业大学计算机学院，北京 100144；2.北京信息高技术研究所，北京 100094）

碰撞是多机器人系统作业过程中一定要避免的关键问题，碰撞检测对系统安全、避障策略至关重要；针对现有碰撞检测方法在结构复杂的多机器人系统，特别是高精度作业中检测精度不能满足实际的问题，提出了一种基于物体特征图的碰撞检测方法；该方法根据不同物体的结构特征，构造出能够紧密包裹该物体的特征图，以遍历特征图的方式检测物体间的碰撞；同时，结合使用碰撞检测掩码方法实现动态碰撞检测，以提高碰撞检测效率；通过实验分析表明，该方法比传统的碰撞检测方法拥有更高的碰撞检测精度以及检测效率。

多机器人；碰撞检测；特征图检测；精度动态检测

0 引言

碰撞检测是检测空间中的运动物体是否发生接触以及干涉的重要技术，所以也是多机器人系统开发过程中最为重要的研究内容之一。随着多机器人仿真场景中物体数目的增多以及三维模型复杂度的不断增加，对场景中碰撞检测功能的实时性、精确性以及真实性等提出了越来越大的挑战。当今主要的虚拟环境碰撞检测方法分为基于图形的碰撞检测方法和基于图像的碰撞检测方法，而基于图形的碰撞检测方法由于其不需要过多依赖机器硬件的性能而得到了越来越广泛的重视和研究。到目前为止，已经有许多学者包括商业公司对虚拟场景中的碰撞检测做了大量的研究，也得出了一些非常实用的碰撞检测方法，如空间分割法和包围盒法。

针对传统的碰撞检测方法存在的检测精度低以及检测效率低等问题并结合模型构造复杂的多机器人仿真研究的实际，提出一种新型的基于模型特征图的动态碰撞检测方法。通过实验表明该方法相比传统的碰撞检测方法具有更高的检测效率以及更高的检测精度，能够极大增强多机器人仿真系统的真实感。

1 新型碰撞检测方法的提出

1.1 空间分割方法

空间分割方法是将整个仿真场景分割成为一个个更小的区域，而每一次运动物体只检测所在运动区域内的对象物体。该方法可以减少每一次碰撞检测的检测对象，从而获得较高的检测效率，因为每次都只需要检测运动物体所在的网格范围内的物体。张振华等［1］对基于空间域的碰撞检测算法作了详细的介绍和分析；张国飚等［2］利用物体空间分布特性以及实时碰撞的局部性，使用空间剖分的方法并结合包围盒检测方法，完成了场景布局相对简单的运动物体模型的碰撞检测，得到了较好的运行效率；Johannes Schauer［3］等使用空间分割的K－D树方法实现了点云模型的碰撞检测，获得了很高的检测效率和准确度。

1.2 包围盒方法

包围盒方法是通过使用简单的几何模型来虚拟外形复杂的物体，然后使用简单的几何模型来进行碰撞检测，如果包围盒之间发生干涉，则说明发生碰撞，如果包围盒没有发生干涉，则说明没有发生碰撞。包围盒方法可以简化原有模型的结构复杂度，从而极大减少碰撞检测计算量，加快系统的执行效率。Yao Wang等［4］使用包围盒方法完成了数控机床仿真中的碰撞检测，因为数控机床中的物体模型相对简单，而且流程也相对固定；陈琳［5］等使用层次包围盒方法实现了多机器人系统的碰撞检测，该方法能够获得较高的碰撞检测效率，但是只是适用于物体外形相对规则的连杆机器人，针对结构造型复杂的物体却难以实现高精度的碰撞检测；姜光焱［6］使用OBB和AABB层次包围盒，实现的碰撞检测方法相比RAPID方法有了更高的检测效率。

空间分割方法对于处于两个或者多个网格之间的物体的不会被纳入检测目标，所以该方法比较适合场景中物体分部比较均匀的场合，这样可以使得划分的网格尽量包含所有的物体。但是对于场景复杂的多机器人系统，可能由于各个网格不能完全包括所有的物体而出现漏检情况。对于复杂构型的机器人结构，简单的立方体或者圆柱体等集合模型往往难以近似的表示其外形，而且会过多扩大原有物体的轮廓，特别是针对凹多面体而言，包围盒方法更加会放大原有物体的外表轮廓，从而会出现过检的情况。

同时，传统的碰撞检测方法往往需要多种检测方法的配合使用才能完成较好的碰撞检测效果。比如包围盒和空间分割的混合碰撞检测方法［7］，然而这样的碰撞检测方法需要经过至少一次初步检以及至少一次再检测，使得算法复杂度极高，检测效率和精度都难以达到要求。

基于以上分析，提出一种基于物体模型特征图的碰撞检测方法。该方法通过提取物体表面的特征点，然后依据特征点构造一个能够紧密包围物体模型的特征图，最后使用特征图的边与碰撞检测目标的表面进行干涉检测以完成碰撞检测，这样可以提高碰撞检测的精确度。同时，依据运动物体的运动路径，以及物体运动的时间及空间局部性，使用掩码方法，动态的设置碰撞检测的目标，使得运动物体只需要检测处于运动路径周围的物体，从而减少了每一次碰撞检测的检测对象数量，可以使碰撞检测的效率得到极大提高。

2 基于特征图的碰撞检测方法

基于物体模型特征图的碰撞检测方法能够极大限度的接近物体外形，从而可以实现高精度的碰撞检测。依据物体表面特征点构造特征图，再用特征图的边来检测碰撞情况，将以往的面面求交算法简化为了线面求交。此外，根据物体的运动路径，使用掩码方法实现了碰撞检测目标的动态设置，也极大提高了系统中碰撞检测算法的执行效率。该方法需要完成的工作有：

1）提取物体模型特征点；

2）根据体征点生成物体特征图；

3）根据路径获取物体的运动范围；

4）设置碰撞掩码，以实现动态设置检测目标。

2.1 提取物体模型特征点

为了实现基于物体特征图的碰撞检测功能，需要先提取运动物体的表面特征点。根据仿真系统中模型的构建过程可以知道，场景中的物体模型都是由多个面片构成，所以可以依据面片之间的邻接线端点来选取特征点。依据邻接面所构成的夹角，邻接线存在谷线和脊线两种情况，以下对两种情况的特征点提取做进一步分析。

1）谷线特征点：如图1所示，物体模型的面片α与β的法线分别是，l是α与β的邻接线。由于α与β构成了山谷形状，所以l就是谷线。设定θ为二面角α－l－β的大小，则θ＝π，若θ＜ε（ε为设定的角度阈值），则邻接线l的端点A、B为运动物体的两个特征点。

2）脊线特征点：如图2所示，物体模型的面片α与β的法线分别是n■，l是α与β的邻接线。由于α与β构成了山脊形状，所以l就是脊线。设定θ为二面角α－l－β的大小，则，若θ＞ε（ε为设定的角度阈值），则邻接线l的端点A、B为运动物体的两个特征点。

根据以上的基础算法分析，通过依次遍历运动物体模型的各个面片，分别对相邻面片的夹角进行计算，就可以提取得到运动物体模型表面的所有特征点。针对不同复杂度的运动物体模型，可以通过设置ε的值来限制提取到的特征点数目。在提取特征点的同时，将特征点存入物体的特征点集合V。成功提取出特征点的物体模型如图3所示。

图1 谷线特征点

图2 脊线特征点

2.2 根据物体特征点生成特征图

由于碰撞检测精度的要求，最理想的情况就是使用运动物体的表面轮廓线进行检测，如果运动物体的表面轮廓线与被检测对象物体的表面发生相交，就说明发生了碰撞。在3.1节中，已经成功提取出了物体表面的特征点，此处就以这些特征点作为特征图的顶点构造一个运动物体特征图的简单回路，该图就是最接近物体轮廓的一个网状结构。

此处假设提取出的特征点集合V＝｛v1，v2，v3...vn｝（n为特征点的个数）。为了表示生成的图，需要用到式（1）所示的邻接矩阵。

式中，n表示图中节点的个数，也就是提取出的运动物体特征点的数目。若aij＝0（i，j∈［1，n］），表示节点i与节点j之间没有连线；反之，若aij＝1，则表示节点i与节点j之间有连线。

初始化的邻接矩阵G中每个元素的值都为0，够造简单回路的同时需要修改G中元素的值，由于连接顶点的线段不需要有方向及代价信息，所以构造的是一个无向图。若提取4个特征点，则构造的特征图的邻接矩阵就该如式（2）所示。由于无向图的邻接矩阵是对称矩阵，所以可以采取压缩存储来存储邻接矩阵。

由于特征图的节点信息已经存放在特征点集合V中，所以只需要根据V中的节点构造简单回路。通过对运动物体模型特征点集合中的特征点进行首尾相连，构造无向图的简单回路，该无向图的简单回路则构成了最接近物体外形的轮廓线。特征图构造完成之后，将其存储在运动物体结构体的特征图指针中。特征图构造完成的效果如图4所示。

2.3 根据路径获取物体的运动范围

传统的碰撞检测方法在运行时的检测对象都是整个场景中的所有物体，然而这些检测目标中有三分之二以上的检测对象都是不需要在当前的运行步骤中检测的，因为有很大一部分的物体并不在机器人的运动路径上。为了减少运动物体单步运动时的碰撞检测运算量，提高碰撞检测效率，需要根据机器人的运动路径来动态设置碰撞检测的目标。

运动物体的每一次运动，必然有一个运动起点和终点，通过对该运动物体运动路径的起点和终点进行坐标投影，就可以获得物体的运动区间。再将处于运动物体运动区间中的可能产生碰撞的其他障碍性物体构造为可能碰撞的集合vi＝｛（i，j）| j∈Cn，j≠i｝，其中Cn＝｛1，2，3...n｝是场景中的所有物体，在运动物体的单步运动过程中，只需要检测是否和可能碰撞集合中的物体是否碰撞，而不需要检测场景中的其他物体。如图5为坐标投影法的效果图。

图3 特征点提取效果

图4 生成图所构成的物体外表轮廓

图5 坐标投影法的效果图

在图5中，运动物体R从原点O出发到达目标点T，折线为所规划的运动物体R的运动路径，所得虚线立方体为R的运动路径区间。在场景中有机器人R以及A、B、C三个物体，其中物体A和B在R的路径坐标投影区间中，而物体C在之外。

2.4 设置碰撞掩码

为了实现根据路径进行动态的碰撞检测，引入碰撞检测掩码方法。该方法为每一个检测对象分配一个掩码属性。初始掩码都配置为0，若运动物体开始运动，则运动物体的碰撞检测掩码置1，同时根据运动物体的运动路径，通过3.3节的方法会获得一个可能碰撞的集合v，然后再将集合v中的物体的碰撞掩码置为1。在每一次碰撞检测之前，先将物体的掩码相与，若结果不为0，则该物体是机器人的碰撞检测目标，需要进行碰撞检测，否则不需要进一步检测。在运动完成之后，需要将设置的掩码重新清0。如果在当前步骤中没有发生碰撞事件，运动物体又进行了一次新的路径规划，那么在下一次的运动之前，需要再一次投影物体的运动区间，再一次设置碰撞掩码。

在图5中，物体A、B在运动物体R的运动路径投影区间之中，则A、B及R的掩码应该都设置为1，即mask＝1，而物体C不在R的运动路径投影区间中，则R在当前步骤中不会与物体C发生碰撞，所以物体C的mask＝0。在进行碰撞检测之前，先将物体的mask相与，则R只需检测A、B是否碰撞即可。

2.5 碰撞检测方法实现

对模型结构相对复杂的多机器人仿真系统，通过提取出模型的表面特征点，进而构成一个可以紧密包围模型的特征图，使用特征图的各个边来检测模型之间是否碰撞，可以简化以往的面面检测方法；此外再使用碰撞掩码方法动态添加碰撞检测对象，使得碰撞检测效率得到较大提高。实现本文所提出的碰撞检测方法需要完成以下工作。

1）将建立完成的模型按初始状态需求布局到仿真场景中，布局物体的同时，记录下场景中的物体以及每个物体所在位置等信息，所以需要建立模型链表modle_List，modle_List中的每个节点Node是物体模型信息的数据结构。模型链表节点Node的结构设计如下：

其中ID、Name、Position和Collion_mask需要在布局物体时进行初始化，Collion_mask初始化都为0，默认都不进行碰撞检测。特征点集合指针以及特征图指针需要待特征点提取完成和特征图生成之后才能赋值。

2）针对已经初始化完成的场景，遍历模型链表并依次对每一个物体提取特征点。为了存储提取到的特征点，需要为每一个物体建立特征点集合V，然后使用3.2节所提方法提取出物体模型的特征点，最后生成特征点集合V＝｛v1，v2，v3... vn｝（n为特征点的个数）并将集合地址赋值给物体链表节点的Sp_point指针。

3）根据生成的物体模型特征点集合构造特征图。首先根据特征点集合中元素的个数n定义图的邻接矩阵G并初始化G中元素aij＝0｛i∈（0，n），j∈（0，n）｝；然后对特征点集合中的特征点使用首尾相连的方法构造特征图，同时修改特征图的邻接矩阵，最后得到包围物体模型的特征图G。

4）根据物体运动的路径修改物体的碰撞检测掩码。首先根据机器人的当前位置O的坐标和目标位置T的坐标，使用3.3节所提的坐标投影法获得机器人的运动包围盒；然后遍历场景物体模型链表modle_List，如果物体的position∈（O，T）则修改物体的Collion_mask为1，最后再修改运动机器人的Collion_mask为1。

5）运动物体开始运动之后，首先对运动物体和场景中其他物体的Collion_mask相与，若结果不为0，则需要对二者进行碰撞检测。

当物体开始运动时，就需要进行碰撞检测，先对运动物体模型的特征图进行遍历，由于特征图是一个无向循环图，所以进行深度遍历，若邻接矩阵G中的元素值为1，则说明对应的两个节点之间存在边，所以需要获取两个顶点的坐标并依据其坐标构成一条线段，设遍历所得的两个点为P1（x1，y1，z1）和P2（x2，y2，z2），则线段公式如式（3）所示：

其中：（x ∈［x1，x2］，y∈［y1，y2］，z∈［z1，z2］）。

使用该线段与目标物体的各个面片进行干涉检测。首先求解线段与目标物体的其中一个面片所在平面是否存在交点，若不存在，则与该面片不干涉，检测目标物体的下一个面片；若存在，判断该交点是否位于该面片中，若不在该目标面片中，则不干涉，检测下一个面片；否则线段与该面片相交，即发生碰撞，进入碰撞处理程序；如果特征图的每一条边都不与目标物体的任何面相交，则说明没有碰撞，继续运动。

程序的运行流程如图6所示。

图6 碰撞检测方法流程图

3 仿真验证

3.1 仿真验证平台

1）在进行多机器人仿真之前，需要对多机器人场景中的物体建模。当今世面上存在许多优秀的计算机建模软件，如AutoCAD、Inventor、Solid Works以及Creator等。由于 Multigen－Paradigm公司的Creator软件具有针对实时应用优化的OpenFlight数据格式，强大的多边形建模、矢量建模等功能，能高效、最优化地生成实时三维（RT3D）数据库，所以本实验采用Creator作为多机器人系统的模型建立软件。

2）在完成建模工作之后，还需要模型驱动引擎以呈现仿真效果。如同建模软件，当今同样存在多款模型驱动引擎软件，比如Direct X和OpenGL等。本试验为了方便展开试验，减少由于平台不同而带来的影响，所以同样采用Multigen－Paradigm公司的图形引擎软件Vega Prime。通过Vega Prime可以进行场景初始化以及设置模型位置和状态信息。

3）使用MFC框架，以及Vega Prime的API驱动多机器人场景中的模型实现实时三维运动仿真。在运动仿真的过程中使用上文提出的碰撞检测方法，可以观察到仿真过程流畅，模型之间发生碰撞事件时可以得到实时提醒。

3.2 仿真实验结果

为了使用本文所提碰撞检测方法来验证仿真环境中物体间的碰撞检测情况，设计了如图7所示的仿真运行环境。该场景中包含了一个运动物体以及3个罐体障碍物，实验希望运动物体可以从图（a）的位置以最短路径运动到图（b）中的位置。

图7 仿真实验场景

在物体运动之前，已经使用以上方法完成了特征图的构建，并且使用坐标投影方法计算出2号和3号罐体在运动物体的路径包围盒之中，所以2号和3号罐体都是运动物体的碰撞检测目标，而1号罐体并不需要检测。当物体运动时与2号罐体发生碰撞并且成功检测到碰撞发生的情况，如图8所示。

图8 碰撞检测效果

4 结论

针对多机器人仿真系统中传统碰撞检测方法存在的漏检、过检，以及在大规模场景中检测效率低下等问题，本文提出一种基于物体模型特征图的碰撞检测方法。通过提取物体表面特征点，并进一步生成紧密包裹模型表面的特征图，使用该特征图的边检测碰撞可以完成高精度的碰撞检测。又使用掩码方法依据物体运动路径完成碰撞检测目标的动态添加，使得在大规模场景中的碰撞检测效率得到极大提高。最后，通过实验验证了该方法的可行性。

［1］张振华，周文理，等.虚拟场景中基于空间域的碰撞检测算法研究［J］.计算机应用，2012，32（S2）：151-153.

［2］张国飚，张 华，刘满禄，等.基于空间剖分的碰撞检测算法研究［J］.计算机工程与应用，2014，50（7）：46-55.

［3］Schauer J，Nuchter A.Collision detection between point clouds using efficient k－d tree implementation［J］.Advantage Engineering Informatics，2015，29（3）：440-458.

［4］Wang Y，Hu Y Y，Fan J C.Collision detection based on bounding box for NC machining simulation［A］.2012 International Conference on Applied Physics and Industrial Engineering［C］.2012，24：247-252.

［5］陈 琳，付 兵，潘海鸿，等.一种适用于多机器人的动态包围体层次树碰撞检测方法［J］.组合机床与自动加工技术，2014 （7）：73-76.

［6］姜光焱.基于包围盒的碰撞检测算法及应用［D］.成都：电子科技大学，2012.

［7］沈学利，吴 琼.基于包围盒和空间分割的混合碰撞检测算法［J］.计算机工程，2012，38（6）：256-258.

［8］潘 翔，章国栋，陈启华.三维可变形物体的特征点层次提取［J］.计算机科学，2014，41（4）：292-296.

［9］郭凌云，郑延斌，刘晶晶.基于时空相关性的快速碰撞检测算法［J］.计算机应用与软件，2013，30（5）：174-176.

［10］邵晓东，高 巍，刘焕玲.基于自适应检测线的碰撞检测算法［J］.计算机集成制造系统，2013，19（12）：3148-3154.

［11］Yang J D，Shang S Y.An Improved collision detection algorithm based on K－DOPS［A］.International Conference on Computer Science andNetworkTechnology，2012［C］.2012（2）：842 -846.

［12］Yang S M，Guo W，Tang W.Simulation of collision resolution algorithm based on self－similar traffic model［J］.IEEE，2009，182-186.

Dynamic Collision Detection Method Based on Figure Graph of 3D Models

Ma Li1，Wang Qiang1，Yang Yingang2，Ma Dongchao1，Zhang Yongmei2

（1.North China University of Technology，Beijing 100144，China；2.Institute of Beijing High Information Technology，Beijing 100094，China）

Collision problem is a key issue in multi－robot system which must be solved，and the collision detection method is important for system security and avoidance strategy.In the multi－robot systems which have a complex structure，existing collision detection methods cannot meet the actual accuracy needs，especially in high－precision operation system，raise a new collision detection way based on the feature graph of objects.According to the structure characteristics of different objects，construct the feature graph which tightly wrapped the object，and the collision can be detected by traversing the feature graph.At the same time，in order to improve the efficiency of the detection method，in combination with the detection masks method.Through experimental analysis shows that this method has a higher collision detection accuracy and detection efficiency than the traditional method of collision detection.

multi－robot；collision detection；feature graph detection；accuracy dynamic detection

1671-4598（2016）08-0059-04

10.16526／j.cnki.11－4762／tp.2016.08.016

：TP391.9

：A

2016-01-30；

：2016-02-29。

部委预研项目（513060803）；国家自然科学基金项目（61300171，61371143）。

马 礼（1968-），男，博士，教授，硕士生导师，主要从事分布式系统，多agent机器人系统方向的研究。