一种适用于多机器人的动态包围体层次树碰撞检测算法*

陈 琳，付 兵，潘海鸿，戴 骏，董海涛

(1. 广西大学 机械工程学院，南宁 530004；2. 广西制造系统与先进制造技术重点实验室，南宁 530004；3.湖北汽车工业学院 机械工程学院，湖北 十堰 442002)

一种适用于多机器人的动态包围体层次树碰撞检测算法*

陈 琳1,2，付 兵1,3，潘海鸿1,2，戴 骏1,2，董海涛1,2

(1. 广西大学 机械工程学院，南宁 530004；2. 广西制造系统与先进制造技术重点实验室，南宁 530004；3.湖北汽车工业学院 机械工程学院，湖北 十堰 442002)

多机器人协同工作在喷涂、焊接、装配等工业现场应用广泛。多机器人碰撞检测是多机器人系统安全、可靠工作的内在保证。针对现有多机器人间碰撞检测算法耗时过多问题，提出动态包围体层次树碰撞检测算法，该算法利用不同包围体的长处构建三层结构的动态包围体层次树，采用层次展开的遍历方式提高多机器人间碰撞检测效率。在多机器人工作站中进行仿真实验，得出对2、4、6、8个机器人采用动态包围体层次树碰撞检测算法的碰撞检测效率分别是RAPID算法的1.657，1.738，2.088，2.149倍。这验证所提出的动态包围体层次树碰撞检测算法能有效提高多机器人间碰撞检测效率。

动态包围体层次树;碰撞检测;多机器人

0 引言

碰撞检测的目的是用来探测空间中移动的几何物体之间是否相交，相交则找出相交的特征(overlap features)。碰撞检测广泛应用于工程实践，机器人，计算图形处理，动画，CAD/CAM，游戏中。因其重要性，许多学者在过去的二十年里对碰撞检测算法进行了广泛研究[1]。他们分别提出各具特色的AABB[2]，OBB[3]，bounding sphere[4]，K-dops[5]，SSV[6]等包围体(bounding volume)算法，并用于碰撞检测中。目前，比较成熟的碰撞检测算法有RAPID[6]，Opcode[7]，适合解决两个单独的刚性物体之间的碰撞检测问题。为加快刚体碰撞检测的速度，包围体层次树[1,3,6]被广泛应用于碰撞检测中。如果包围体层次树中父节点不存在碰撞，则无须对子节点进行检测，从而提高碰撞检测的速度。

随着碰撞检测算法的研究深入，碰撞检测的对象从刚体扩展到多刚体、变形体和铰接模型。其中以铰接模型为研究对象的碰撞检测是现在研究的一个热点和难点[7-8]。典型的铰接模型如多自由度工业机器人、虚拟环境中的手臂等。铰接模型中关节是刚性,关节之间串并联，相邻关节不分离，空间运动时存在一定的相对位置关系。其不同于多刚体[9]间松散的位置关系，也不同于由大量稠密的基元组成的变形体[10]，变形体的基元位置和外形在模型坐标系中是变化的[11]。机器人中关节的实际运动是其前端关节运动的合成，关节数增多造成关节实际运动轨迹复杂，检测环境中物体发生碰撞的情况变得复杂，计算量剧增。

已有的包围体层次树的应用局限于几个刚体间的碰撞检测[1-7]，且使用时包围体层次树是提前计算出来，因而其不适合直接应用于由多个运动的单元组成的铰接模型[7]。Redon[7]研究单个铰接模型与工作环境的碰撞检测时，在关节模型的扫略体包围体基础上计算AABB包围体构建铰接模型的包围体层次树，剔除与环境距离远的单元模型； Zhang[12]研究多个铰接模型间的碰撞检测，他采用单元模型的泰勒模型来计算单元模型的AABB包围体，构建铰接模型的AABB包围体层次树，剔除不碰撞的单元模型对。对于多个由单元模型串并联而成的铰接模型，如何提高其碰撞检测效率这是一个值得研究的问题。这涉及到选择哪种类型包围体构建铰接模型的包围体层次树,如何构建适合铰接模型的包围体层次等方面的研究。

以六自由度机器人为研究对象，该研究对象是由7个刚体(一个底座与六个连杆)串联组成。为提高铰接模型的碰撞检测的效率和速度，提出一种动态包围体层次树结构。铰接模型中单元模型之间的位置是变化的，以LSS为基元构建AABB包围体，其构建、更新速度快，能够快速剔除不碰撞的单元模型，进而加快铰接模型间的碰撞检测速度。

1 动态包围体层次树及其构建

动态包围体层次树包含三层：顶层为由单元模型的AABB包围体构建；中间层为OBB包围体层次树；底层为三角形。所提出的动态包围体层次树中包围体大小、层次树层次结构、层次树中相交节点个数以及层次树遍历深度是随着机器人相对位置变化而变化的。

对于多个刚体串并联的铰接模型特点，提出动态包围体层次树，其包含三层结构：

顶层：在关节碰撞检测对的基础上，构建顶层，用于剔除不相交的包围体。顶层选用LSS[6]包围机器人关节模型，并作为剔除层中AABB包围体的构建基元(LSS包围体紧凑性好，更新代价低；AABB测试代价小，基于LSS计算AABB简单)。

根据图1所示，以LSS为基元计算AABB包围体：

图1 以LSS为基元计算AABB包围体

(a) 计算AABB包围体中心坐标

(1)

(b) 计算AABB包围体半径

(2)

注：O(X,Y,Z)是AABB包围体的中心坐标；RX,RY,RZ是AABB包围体三个半径。A和B是LSS的两个端点；R是LSS半径。

中间层：中间层检测采用OBB包围体(OBB包围体紧凑易于更新)构建包围体层次树[3]。铰接模型中关节是刚性的，采用OBB包围体来建立动态包围体层次树的中间层。每一个关节中的三角形采用自顶而下的方法来构建一个OBB包围体层次树, OBB包围体层次树的叶子节点OBB包围体中有一个三角形。这里采用二叉树的数据结构来构建包围体层次树。有多少个关节就有多少个OBB包围体层次树。其中，每一个关节的OBB包围体层次树根节点都对应顶层层次树的叶子节点中包围这个关节的包围体。当关节位置和姿态发生变化后，只需要更新动态包围体层次树的中间层层次树中OBB包围体的位置和姿态，无须重新计算其大小，因而其可实现快速更新中间层层次树。

底层：动态包围体层次树的底层构建过程比较特殊，它是在完成中间关节层包围体碰撞检测后，且当中间关节层OBB包围体层次树叶子节点OBB发生碰撞后才需要建立。建立时将叶子节点OBB包围体中三角形顶点坐标分别转换到世界坐标系下，这些转换后得到的三角形对构成动态包围体层次树的底层。底层中三角形对的相交测试方法为moller算法。

2 动态包围体层次树遍历与执行过程

2.1 动态包围体层次树的遍历

铰接模型运动使得机器人彼此关节之间位置时刻发生变化，因而多机器人碰撞检测过程中包围体相交测试的数量，层次树遍历深度也是时刻变化的。为提高碰撞检测效率，有必要知道包围体层次树的实际遍历过程。

图2 动态包围体遍历示意图

建立包围体遍历层次树[14]，二叉树数据结构类型的层次树中节点遍历顺序可以抽象成节点包围体遍历顺序图。如图2中OBB遍历树所示，包括层次树相交测试节点和非相交测试节点。可知每个层次树遍历后节点个数不同，层次树的遍历深度也不一样。另外，在遍历顺序图中上层节点相交测试中如果不相交或者不用进行相交测试时，该上层节点的子节点则不需要进行相交测试。

动态包围体层次树实际遍历过程如图2所示，顶层中Ji(i= 1，……n)表示2个机器人关节组成的AABB检测对；中间关节层表示两个关节的两个OBB包围体层次树之间的遍历；底层表示需要进行相交测试的三角形检测对。由于机器人间相对运动，动态包围体层次树的执行过程中，顶层中AABB包围体检测中相交AABB对数是变化的；关节中间层中每个OBB包围体层次树遍历深度不同(如第二个OBB遍历树没有遍历到底端就已经结束碰撞检测)，进行包围体相交测试的数量也会不同；底层中进行相交测试的三角对个数是变化的或者不用进行底层相交测试。因此，动态包围体层次树相交测试的计算量在每个检查点时刻是不同的。

2.2 动态包围体层次树的执行过程

上面分别介绍了构建铰接模型的动态包围体层次树中各个部分的建立过程以及动态包围体层次树的遍历过程。动态包围体层次树在每一个离散点的碰撞检测中依次按照顶层，中间层，底层进行，如图3所示。其应用在铰接模型间碰撞检测中的执行过程需按照以下3步：

(1)顶层层次树中包围体相交测试，得到顶层中相交的叶子节点包围体，进一步获得需要执行相交测试的中间层OBB包围体层次树的根节点OBB包围体；

(2)在中间层进行OBB包围体相交测试，其目的在于获取OBB包围体层次树中相交的叶子节点OBB包围体，再利用他们去获取对应的需要执行相交测试的三角形检测对。中间层中OBB包围体层次树按照层次展开进行遍历，计算时先对检测对中尺寸大的OBB包围体子节点优先遍历；

(3)进行动态包围体层次树底层中三角形的相交测试，得到相交三角形对。

如果顶层中没有发生叶子节点包围体相交，则不进行中间层和底层的碰撞检测；如果中间层中没有叶子节点OBB包围体相交，则不用进行底层碰撞检测。

图3 动态包围体层次树在一次 碰撞检测中的执行过程

对于一条确定轨迹(包含很多离散点)，整个碰撞检测过程中按照以下5个步骤进行：

①构建包围关节的LSS包围体或OBB包围体基元(用于动态包围体层次树的构建)；

②构建动态包围体层次树；

③在一个离散点处执行动态包围体层次树碰撞检测；

④重新计算动态包围体层次树顶层；

⑤返回执行③，④，直到完成所有离散点碰撞检测。

3 实验与结果分析

3.1 实验仿真平台及仿真实验设计

基于OpenGL自主开发软件仿真平台，在VS2008中采用C++进行程序编写。采用处理器Inter(R) Core(TM) i5-2300(主频：2.8GHz)，4GB内存，计算机操作系统为Windows XP。

以6自由度工业机器人TA1400作为实验对象，仿真验证多机器人动态包围体层次树碰撞检测算法有效性。为获得一般性结论，分别以2、4、6、8个机器人组成的多机器人工作站进行碰撞检测仿真实验。为进行动态包围体层次树碰撞检测算法效率对比分析，这4组实验中每组设计两个实验：①动态包围体层次树；②常规RAPID碰撞检测算法(RAPID算法是公认的用于碰撞检测算法效率对比的标准)。图4是设计的8个机器人工作站，实验中相同数量机器人工作站中对应机器人运动的轨迹相同。

图4 8个机器人工作站

3.2 实验仿真结果与分析

根据设计实验分别统计2种碰撞检测算法的耗时，结果如表1所示：

表1 多机器人工作站碰撞检测耗时

从表1中可以看出在2，4，6，8个机器人组成的工作站，采用动态包围体层次树的碰撞检测效率明显高于使用RAPID碰撞检测效率：所提出的动态包围体层次树检测算法的效率分别是RAPID算法的1.657，1.738，2.088，2.149倍。表明随着工作站中机器人数目的增加，所提出的动态包围体层次树碰撞检测效率将比RAPID效率明显提高。

下面结合算法原理对碰撞检测效率提高的主要原因进行分析：

①由于在碰撞检测中采用动态包围体层次树，即在顶层剔除不碰撞关节对(关节模型上的OBB包围体)，使得进行相交测试OBB包围体对数量减少，从而缩短碰撞检测时间。

②因机器人关节为细长杆状模型，选用LSS包围体包围机器人关节会得到紧凑包围体，并以此作为顶层AABB包围体构建基元，一方面使得AABB包围体的计算简化，层次树节点更新耗时也相对少；另一方面较紧凑的包围体，使得顶层和中间关节层包围体相交测试对数相对小。虽然，动态包围体层次树中基元相交测试耗时与RAPID中相同。但是，在相同实验条件下，RAPID中根节点OBB检测对的数量比动态包围体层次树中的多得多。如在所设计试验中测得的RAPID中根节点OBB检测对的数量均是动态包围体层次树的50倍以上。

③根据已有文献统计AABB相交测试耗时小于OBB对检测耗时[15]。因而所提出的动态包围体层次树算法的碰撞检测耗时在理论上小于RAPID。

4 结论

为提高多机器人间的碰撞检测效率，借助多个不同包围体的长处，提出具有三层结构的动态包围体层次树碰撞检测算法。结合实验结果可知：①随着机器人数量的增加，动态包围体层次树碰撞检测效率相比RAPID会有明显提高。②三层结构的动态包围体层次树碰撞检测算法是有效的。以LSS为基元构建AABB包围体作为顶层，使得动态包围体层次树构建、更新速度加快，并能够快速剔除不碰撞的关节，能够加速碰撞检测。③由于所提出的动态包围体层次树采用层次展开，计算时其先对中间层检测对中尺寸大的包围体的子节点优先遍历，这样减少相交测试的包围体对数，因此该方式可以进一步减少多机器人间碰撞检测时间。

总之，该动态包围体层次树碰撞检测算法可以为后续采用并行结构的多机器人碰撞检测研究奠定基础，为多机器人碰撞检测算法研究提供一定的借鉴作用。

[1] Zhang X Y, Kim Y J. Interactive collision detection for deformable models using streaming AABBs [J]. IEEE Transactions on Visualization and Computer Graphics. 2007,13(2): 61-81.

[2] Terdiman P. Memory-optimized bounding-volume hierarchies [J/OL]. http://www.codecorner.com/ Opcode. 2001.

[3] Gottschalk S, Lin M C, Manocha D. OBBTree: A hierarchical structure for rapid interference detection [C]. //Proceedings of the 23rd annual conference on Computer graphics and interactive techniques. ACM 1996:171-180.

[4] Bradshaw G,Sullivan C. Sphere-tree construction using dynamic medial axis approximation [C]. //Proceedings Of the 2002 ACM SIGGRAPH/Eurographics Symposium on Computer animation. ACM, 2002, 33-40.

[5] Klosowski J T, Held M, Mitchell J S B, et al. Efficient collision detection using bounding volume hierarchies of K-Dops [J]. IEEE Transactions on Visualization and Computer Graphics, 1998, 4(1): 21-36.

[6] Larsen E, Gottschalk S, Lin M C, et al. Fast proximity queries with swept sphere volumes [R]. Technical Report TR99-018. Department. of Computer Science, University of North Carolina. 1999.

[7] Redon S, Lin M C, Manocha D, et al. Fast continuous collision detection for articulated models [J]. Journal of Computing and Information Science in Engineering, 2005, 5: 126-137.

[8] Yoon S, Kim Y J, Harada T,et al. Recent advances in real-time collision and proximity computations for games and simulations [C].//ACM SIGGRAPH ASIA 2010 Courses. ACM,2010.

[9] Avril Q, Gouranton V, Arnaldi B. Fast collision culling in large-scale environments using GPU mapping function [J]. ACM Eurographics Parallel Graphics and Visualization. 2012, 71-80.

[10] Mendoza C, Sullivan C O. Interruptible collision detection for deformable objects [J]. Computers & Graphics, 2006, 30(3): 432-438.

[11] Bergen G V. Efficient collision detection of complex deformable models using aabb trees [J]. Journal of Graphics Tools, 1997, 2(4):1-13.

[12] Zhang X, Redon S, Lee M, et al. Continuous collision detection for articulated models using taylor models and temporal culling [C]. //ACM Transactions on Graphics(TOG).ACM,2007, 26(3)15.

[13] Keiser B H M T R,Gross M M M.Consistent penetration depth estimation for deformable collision response [C].//Vision, Modeling,and Visualization 2004:Proceedings,November 16-18,2004,Standford,USA.IOS Press,2004:339-346.

[14] Lee Y G, Kim Y J. Simple and parallel proximity algorithms for general polygonal models [J]. Computer Animation and Virtual worlds, 2010, 21: 365-374.

[15] Ericson C. Real-time collision detection [M].USA,CRC Press,2004.

(编辑 李秀敏)

A Dynamic Bounding Volume Hierarchy Tree Collision Detection Algorithm for Multi-robot

CHEN Lin1,2, FU Bing1,3, PAN Hai-hong1,2, DAI Jun1,2, DONG Hai-tao1,2

(1. College of Mechanical Engineering, Guangxi University, Nanning 530004, China; 2. Guangxi Key Laboratory of Manufacturing System & Advanced Manufacturing Technology，Nanning 530004, China)

Cooperated multi-robots are used widely in painting, welding, assembly and other fields. The collision detection among multi-robots is a guarantee for the safety and reliable operations for multi-robots systems. For the existing collision detection algorithms for multi-robots are excessively time-consuming, a dynamic bounding volume hierarchy tree collision detection algorithm was proposed for collision detection among multi-robots. The algorithm took advantages of different bounding volumes to build a dynamic bounding volume hierarchy tree with three layers structure, and it used a level of traverse way to achieve detection so that it could improve the efficiency of the collision detection among multi-robots. The simulation experiments were performed in a multi-robot workstation. The simulation results display that the efficiencies of this dynamic bounding volume hierarchy tree collision detection algorithm are 1.657, 1.738, 2.088, 2.149 times as that of RAPID when the multi-robot workstation is 2, 4, 6,or 8 robots, respectively. It indicates that the proposed algorithm can effectively improve the collision detection efficiency for multi-robots.

dynamic bounding volume hierarchy tree; collision detection; multi-robots

1001-2265(2014)07-0073-04

10.13462/j.cnki.mmtamt.2014.07.020

2013-09-30；

2013-10-28

国家自然科学基金项目(51065005)；广西自然科学基金项目(2012GXNSFAA053200)资助;广西制造系统与先进制造技术重点实验室项目(13-051-09S13)

陈琳(1973—)，女，山东青岛人，广西大学教授，博士，研究方向为机器人控制技术、数字信号检测与处理、伺服电机控制，(E-mail)gxdxcl@163.com；通讯作者：潘海鸿(1966— )，男，广西武鸣人，广西大学教授，博士生导师,研究方向为多机器人协调控制技术，(E-mail)hustphh@163.com.cn。

TH166;TG659

A