基于网格包络的工业机器人仿真碰撞检测算法

赵 亮 张义德 胡旭晓

1.浙江大学机械工程学院，杭州，3100272．浙江理工大学机械与自动控制学院，杭州，310018

基于网格包络的工业机器人仿真碰撞检测算法

赵 亮1张义德1胡旭晓2

1.浙江大学机械工程学院，杭州，3100272．浙江理工大学机械与自动控制学院，杭州，310018

为提高工业机器人在复杂作业环境下的碰撞检测效率，提出了一种网格包络的碰撞检测算法，以大量等尺寸的立方体网格来包络模型本身，并在网格内部建立网格子模型的AABB树结构。该算法在建模过程中将网格的空间坐标进行有序存储，在遍历阶段可快速搜索到相交的网格，之后遍历网格内部的树结构来进一步判断模型是否碰撞。该算法网格内部的子模型几何数据量远小于整体模型几何数据量，其网格内的检测速度远快于以整体模型建模的传统层次包围盒方法的检测速度。实验结果表明，在大型复杂模型碰撞检测仿真中，该算法在不同网格数量下的检测效率比传统的Solid算法的检测效率快数倍到数十倍。

碰撞检测；网格包络；轴对齐包围盒；工业机器人

0 引言

碰撞检测技术应用在机器人路径规划[1]、医学仿真[2]、3D游戏等虚拟现实领域，在工业机器人仿真中，碰撞检测技术提供对工业机器人路径规划的技术支持，如车架的点焊、零件的装配、复杂环境下物料的搬运等。潘海鸥等[3]研究了多机器人系统下的碰撞检测算法；金明杰等[4]研究了工业机器人运动规划中的自适应碰撞检测算法。

碰撞检测技术主要有层次包围盒技术、空间划分技术和基于GPU[5-6]的碰撞检测技术等。层次包围盒技术主要有球包围盒树[7]、AABB层次树[8]、OBB层次树[9]、K-DOPs树[10]等。空间划分技术包括均匀网格、八叉树[11]和BSP树[12]等。其中，层次包围盒技术因建模方便、检测效率高而得到广泛的应用和研究[13]。层次包围盒检测技术中较为成熟的有基于AABB包围盒层次树结构的Solid[8]算法和基于OBB包围盒层次树结构的RAPID[9]算法，由于其高效的检测性能，被广泛用于对比和作为改进对象。

机器人仿真中的碰撞检测技术研究大多对机器人模型进行了简化，模型的几何数据量较小。工业机器人的机械手和作业环境的三维模型结构复杂，包含几何数据量大，作业精度要求高，使用传统的包围盒碰撞检测技术检测效率不高。本文提出了一种网格包络算法，将三维模型离散化成大量整齐排列的均匀网格，在建模阶段对网格内的子模型建立AABB层次树，在碰撞检测阶段快速定位发生相交的网格区域。该网格以模型本身为建模基准而不以整个空间为建模基准，通过遍历相交网格内的AABB层次树来进一步准确判断碰撞情况。

1 网格包络模型的建立

1.1 仿真系统的搭建

图1为6轴工业机器人点焊作业的三维显示图，焊接夹具模型含有约694×103个三角形面片，汽车侧门模型含有约286×103个三角形面片，点焊机含有约12.5×103个三角形面片。在仿真环境中实现工业机器人的运动控制，通过求解轨迹运动中的碰撞检测情况，可以在仿真中实现运动轨迹曲线的调整。

图1 工业机器人点焊汽车侧门仿真Fig.1 Simulation of a industrial robot spot-welding door frame

1.2 网格包络层的建立

以模型的最外层的轴向包围盒为边界，采用等大小的立方体网格层覆盖模型，网格的建立以Z、Y、X坐标轴为优先级顺序并以最外层包围盒为边界依次建立。模型由大量三角形面片构成，检测网格内是否包含属于模型的三角形面片来判断网格是否包络模型。当网格内含有三角形面片时，则存储该网格的中心点坐标和三角形数据索引；当网格内不包含模型的三角形面片时，则舍弃该网格节点，直至建完所有网格。网格包络法的建模伪代码如下：

for(Xi=Xmin；Xi

for(Yi=Ymin；Yi

for(Zi=Zmin；Zi

if (有三角形在网格i内或与网格i相交) {创建该网格节点i并记录中心点坐标(Xi，Yi，Zi)；对网格节点i创建AABBi树结构；}

else {舍弃该网格节点；}

其中，(Xmin，Ymin，Zmin)为最外层长方体包围盒顶点的最小点坐标；(Xmax，Ymax，Zmax)为最大点坐标，i为网格节点序号，(Xi，Yi，Zi)为网格i的中心点坐标，r为网格半边长。AABBi为网格i的内部AABB树，即对网格内所有三角形进行AABB建模。以最外层轴向包围盒为边界，以网格边长为移动单位，以Z、Y、X坐标轴为优先级顺序依次建立网格，若有三角形在网格内或与网格相交，则存储该网格的中心点坐标；否则删除该网格。

在二维平面中，网格包络算法的建模方法与AABB层次树的建模方法的对比如图2所示。以模型的最外层轴向包围长方形为边界建立正方形网格层，遍历所有网格并检测网格是否和模型相交，若网格与模型相交则标记该网格并存储相交的模型数据，图2中阴影网格为所有被标记的网格。网格的建立顺序如下：以第一列网格最底端为起点(即左下角网格)，以网格边长为单位向上移动，当网格超出长方形顶端界限，则从第二列网格底端继续向上移动，如此往复直至长方形内所有网格遍历完毕。

图2 AABB树和网格包络的建模方法对比Fig.2 Comparison of modeling methods between AABB and grids covering algorithm

图3所示为网格包络下的焊接夹具模型，网格数量为2256，网格尺寸越小，网格数越多，网格内部包含的模型特征量越少。

图3 网格包络下的焊接夹具模型Fig.3 Model of grids covering welding fixture

1.3 网格与三角形位置关系的快速判断

如图4所示，点P为网格中心点，点Q为三角形ABC上与点P相距最近的点，三角形ABC上距点P的最近点Q采用Voronoi特征域法可快速求解得到。若线段PQ在X、Y、Z轴上的投影值都小于或等于网格半边长r，则表明三角形ABC在网格内或与网格相交；若PQ在X、Y、Z轴上的任一投影值大于网格半边长r，则说明三角形在网格外，快速求解三角形与包围盒的位置关系可加速建模过程。网格不仅存储网格内部的三角形索引数据，而且需存储与网格边界相交的三角形索引数据，与网格相交的三角形会被多个相邻网格存储。

图4 三角形与网格位置关系判断Fig.4 Judging of the position relation between triangle and grid

1.4 分层加速建模

当模型含有大量三角形面片时，为每个网格循环检索模型的所有三角形索引数据耗时情况严重。为加速检索网格内是否含有模型三角形面片，采用建立多层不同尺寸网格的预处理方法。每层网格存储属于该网格的所有三角形索引，第N层网格从第N-1层网格获取三角形索引而无需检索根模型的所有三角形索引。

如图5所示，先建立较大尺寸的网格，网格内存储属于该网格的模型数据，然后以较大网格尺寸的一半作为下一层网格尺寸。在三维空间中，多层网格以八叉树结构形式存在，在二维空间中以四叉树结构来表示。

图5 分层加速建立网格层Fig.5 Layered acceleration to build grid layer

如图6所示，第N层网格尺寸为第N-1层网格的一半，当网格尺寸降到目标尺寸r时，建立最后一层网格。为节省存储消耗，建完第N层网格后立即删除N-1层网格存储的三角形索引数据，只在最后一层存储模型的三角形索引。

图6 分层建模流程图Fig.6 Flow chart of layered modeling

为节省内存消耗，网格按Z、Y、X坐标轴为优先级先后顺序，采用一维线性存储方式依次进行存储来记录三维空间位置信息。

1.5 网格内部建立AABB树结构

每个网格不仅存储该网格的坐标位置，同时存储了在网格内部或与网格相交的三角形数据，在网格建立的同时对网格内部的网格子模型建立AABB层次树结构，在后续的碰撞检测中可以实时调用网格内部的树结构检测三角形碰撞情况。如图7所示，在建立网格的同时在网格内部建立网格子模型的AABB层次树结构，将所有网格内的树结构存入堆栈，在检测阶段可实时读取，网格内部AABB树用Solid算法进行建模。

图7 网格内部建立AABB树结构Fig.7 AABB trees structure established in the grid

网格尺寸大小对算法的影响主要在整体网格建模和碰撞检测两个阶段。在建模阶段：网格越小，网格数越多，计算所有网格的位置和存储网格内部的三角形数据越耗时。在碰撞检测阶段：网格越小，网格对模型的覆盖越紧密，同时网格平均包含的模型数据量越小，网格内树结构的遍历速度越快。

2 碰撞检测

2.1 网格遍历检测

设模型A、B分别含有网格数n、m，遍历模型A的所有网格，判断模型B中是否有网格与模型A 中的网格相交来初步判断两模型是否碰撞。网格的相交判断采用 “分离轴法”以适应旋转更新变换。

本文算法采用一维线性存储，使用二分法进行快速查找来定位可能相交区域。对于模型A中的网格i，在模型B中以X、Y、Z坐标轴为先后顺序采用二分法进行搜索，检索模型B中是否有网格j与网格i相交，时间复杂度为Ο(nlog2m)。根据时间复杂度公式，应以网格数少的模型作为遍历对象(即模型A)，网格数多的模型作为二分法检索对象(即模型B)。

当检测到A中的网格i与B中的网格j相交时，遍历网格i、j内存储的子模型的AABB层次包围盒树结构，检测网格i、j内的三角形面片是否发生碰撞。图8为本文算法碰撞检测判断流程图。

图8 碰撞检测流程图Fig.8 Flow chart of collision detection

如图9所示，对于图中左边的每一个网格，循环检测右边所有网格是否与之相交。图9中黑色部分网格为相交网格，之后对黑色网格内部进行精确检测。

图9 网格相交遍历检索Fig.9 Grid intersection traversal search

2.2 分层碰撞检测

为加速网格相交的检测判断，采用网格最小外接球代替网格进行初步判断。如图10所示，模型采用三层结构进行建模。图10中由上到下，最外层为轴向包围盒，中间层为网格外接球层，内层为网格层。当两个模型相距较远时，用最外层判断可快速检测而省去大量遍历过程；当两个模型相距较近时，以中间层遍历判断为主。

图10 点焊夹具模型三层包络结构Fig.10 Three layer structure of spot-welding fixture modle

设模型A中网格i的坐标为(Xi，Yi，Zi)，模型B中的网格j的坐标为(Xj，Yj，Zj)，网格外接球的半径为R。当两坐标之间的距离D>2R时，则表示两网格最小外接球不相交，即网格i与网格j不相交。当两坐标之间的距离D≤2R时，采用OBB包围盒检测方法中的“分离轴法”继续判断两网格是否相交。

当模型旋转时，网格需要进行坐标更新和方向更新，而网格外接球只需更新坐标。由于采用网格外接球代替网格进行初步判断，故无需对所有网格进行旋转更新而只需更新外接球相交的网格。

2.3 网格内部相交检测

遍历两模型网格，检测是否有相交网格，当检测到分别属于不同模型的网格相交时，对相交的网格内部的子模型AABB层次树深度遍历，进行网格碰撞区域的AABB层次树的碰撞判断。网格内部的子模型AABB层次树的检测速度主要与网格内部的三角形数量有关，三角形数量越多，则检测时间越长。

网格尺寸越小，则网格内包含的三角形数量越少，网格内的相交检测越快。但网格数量越多，网格层建立越慢，因此，网格尺寸不宜过小。如图11所示，遍历两模型的网格外接球，计算网格外接球是否相交，若相交则继续检测网格是否相交，若网格也相交则检测网格内部的三角形面片是否相交，图11中两模型的非碰撞状态将在网格内部的AABB层次树遍历阶段被检测出。

图11 网格包络模型碰撞情况Fig.11 Collision situation of the grids covering models

3 仿真实验

3.1 仿真系统的搭建

本文对算法的性能进行了实验验证，实验的硬件环境如下：CPU为Corei5-4210H(2.9G)、内存8GB，显卡为NVIDIAGT960；软件环境为VisualStudio2010和DXSDK_Jun10。在三维建模软件中将模型导出为STL格式，并使用3DSMAX将模型导出为“.X”类型文件。在3DSMAX中将模型的位置和姿态调整正确，在仿真环境中建立6轴工业机器人运动控制模块，实现末端位置的准确定位。图12为点焊汽车侧门仿真作业的三维显示图。

图12 点焊汽车侧门仿真图Fig.12 Welding Simulation of a car door

3.2 两种方法检测速度的对比

在两模型最外层包围盒相交的情况下，对点焊机分别与焊接夹具模型、汽车侧门模型10×103个不同相对姿态、位置，用不同算法计算碰撞检测时间。焊接夹具模型含有约694×103个三角形面片，汽车侧门模型含有约286×103个三角形面片，点焊机含有约12.5×103个三角形面片。

如表1所示，在Solid算法下，点焊机与焊接夹具碰撞状态平均每个点的检测时间约323.5ms，最慢位置为8.22s。网格包络算法在不同网格尺寸下，检测速度和建模速度有所差异。网格尺寸越小，建模时间越长，但平均检测时间和最慢位置检测时间越短。网格包络算法在焊接夹具网格数为2256的情况下，碰撞的平均检测时间约11ms，最慢位置约492ms。对比传统基于AABB层次树结构的Solid算法，网格包络算法在检测速度上提高了数十倍。

表1 点焊机与焊接夹具模型碰撞检测时间

如表2所示，由于汽车侧门的结构比焊接夹具简单，所以点焊机与汽车侧门的碰撞检测速度较快。在Solid算法下，点焊机与汽车侧门的碰撞状态的平均检测时间约为10.5 ms，网格包络算法在网格数为1616时的平均检测时间约为2.6 ms，网格包络算法比Solid算法快数倍。

表2 点焊机与汽车侧门模型碰撞检测时间

网格包络算法在建模时间上比Solid算法慢，网格数越多，建模时间越长，但检测效率越高。由于工业机器人仿真中模型较为固定，模型更新少，故可将碰撞检测的建模数据输出为文本文件。因此，每次运行仿真软件读取建模数据的文本文件即可，无需重复建模过程。

4 结论

(1)网格包络算法以大量等尺寸的立方体网格包络模型，并在网格内部建立子模型AABB树结构，网格与模型同步运动。在检测阶段，遍历两模型的网格可以快速定位碰撞区域，在网格碰撞区域对该网格内部子模型的AABB树结构进行遍历检测，从三角形层面判断是否发生碰撞。

(2)将大模型拆解为大量网格子模型，由于子模型含有的三角形数据量小，故其检测速度远快于以整体模型为检测对象的传统方法的检测速度。通过细化网格可以减少网格内的数据量，但网格数量过大会导致初次建模时间的延长，可以根据模型的复杂程度以及对检测速度的要求来确定网格尺寸。

(3)相比传统包围盒算法，网格包络算法针对大型模型的碰撞检测速度快数倍至数十倍。在工业机器人的作业和路径规划仿真中，三维模型的结构复杂且包含的三角形数量多，网格包络算法在工业机器人作业和路径规划仿真中能够提供稳定快速的碰撞检测。

[1] 刘忠，曹其新，朱笑笑，等．基于VRML节点树的双臂移动机器人碰撞检测及优化[J]．上海交通大学学报，2011, 45(7): 985-989. LIU Zhong, CAO Qixin, ZHU Xiaoxiao, et al. Collision Detection and Optimization of Dual-arm Mobile Robot Based on Node Tree of VRML[J]. Journal of Shanghai Jiaotong University, 2011, 45(7): 985-989.

[2] GAO Baofeng, HU Kangqi, GUO Shuxiang. Collision Detection Algorithm Based on AABB for Minimally Invasive Surgery[C]//Proceedings of 2014 IEEE International Conference on Mechatronics and Automation. Tianjin,2014:315-320.

[3] 潘海鸿，戴骏，陈琳，等．多机器人并行动态包围盒层次树碰撞检测算法[J]．计算机辅助设计与图形学学报，2014，26(11)：1948-1956. PAN Haihong, DAI Jun, CHEN Lin, et al. Multi-robot Parallel Dynamic Bounding Volume Hierarchy Tree Collision Detection Algorithm[J]. Journal of Computers Aided Design & Computer Graphic, 2014, 26(11):1948-1956.

[4] 金明杰，楼云江，刘冠峰，等．基于自适应动态碰撞检测的工业机器人运动规划算法研究[J]．中国科技大学学报，2012，42(6)：448-455. JIN Mingjie, LOU Yunjiang, LIU Guanfeng, et al. A Novel Motion Planning Algorithm with Adaptive Dynamic Collision Detection for Industrial Robot[J]. Journal of University of Science and Technology of China,2012, 42(6): 448-455.

[5] 唐敏，林江，童若锋．图形硬件加速的柔性物体连续碰撞检测[J]．计算机学报，2010，33(10)：2022-2030 TANG Min, LIN Jiang, TONG Ruofeng. Graphics Hardware Accelerated Continuous Collision Detection Between Deformable Objects[J]. Chinese Journal of Computers, 2010, 33(10):2022-2030.

[6] 印桂生，王海玲, 张菁，等. 快速高效的碰撞检测算法[J]．上海交通大学学报，2012, 46(6)：962-971 YIN Guisheng, WANG Hailing, ZHANG Jing, et al. Fast Efficient Collision Detection[J]. Journal of Shanghai Jiaotong University,2012,42(6):962-971.

[7] PALMERI J, GRIMSDALE R L. Collision Detection for Animation Using Sphere-trees[J]. Computer Graphics Forum, 1995, 14(2):105-116.

[8] van den BERGEN G. Efficient Collision Detection of Complex Deformable Models Using AABB Trees[J]. Journal of Graphics Tools, 1997, 2(4):1-14.

[9] GOTTSCHALK S, LIN M, MANOCHA D. OBB Tree: A Hierarchical Structure for Rapid Interference Detection[C]// Proceedings of the SIGGRAPH. New York, 1996 :171-180.

[10] KLOSOWSKI J, HELD M, MITCHELL J, et al. Efficient Collision Detection Using Bounding Volume Hierarchies of K-DOPs[J]. IEEE Transactions on Visualization and Computer Graphics,1998, 4(1):21-37.

[11] NAYLORB F. Interactive Solid Geometry via Partitioning Trees[C]//Conference on Graphics Interface. Toronto, 1992:11-18.

[12] 鲍义东，吴冬梅．自适应细分及优化编码八叉树碰撞检测算法[J]．上海交通大学学报，2015，49(8)：1114-1122. BAO Yidong, WU Dongmei. A Novel Algorithm for Collision Detection Based on Octree of Adaptive Subdivision and Encoding[J]. Journal of Shanghai Jiaotong University, 2015, 49(8):1114-1122.

[13] 唐勇，杨偲偲，吕梦雅，等. 自适应椭球包围盒改进织物碰撞检测方法[J]．计算机辅助设计与图形学学报，2013， 25(10)：1589-1596. TANG Yong, YANG Sisi, LYU Mengya, et al. Collision Detection for Cloth Based on Adaptive Enclosing Ellipsoids[J]. Journal of Computer-Aided Design & Computer Graphic, 2013, 25(10)：1589-1596.

(编辑 陈 勇)

A Novel Collision Detection Algorithm Based on Grids Enveloping for Industrial Robot Simulations

ZHAO Liang1ZHANG Yide1HU Xuxiao2

1.College of Mechanical Engineering, Zhejiang University, Hangzhou, 310027 2.College of Mechanical and Automatic Control, Zhejiang Sci-Tech University, Hangzhou, 310018

To speed up the collision detection efficiency of industrial robots in the complex working environments, a novel collision detection algorithm was proposed using equal-sized cubic grids to cover the model and building tree structure of AABB in the grid. The space coordinates of these grids were stored orderly in the modeling progresses so as to determine whether there were grids intersecting in the traversal periods. Then traverse the hierarchical structure in the intersecting grids to detect collision precisely. Due to the grids had far less model data than the whole model, the detection speeds in the grids were far more fast than the traditional hierarchical bounding volume method where the building model was based on the whole model. The experimental results show that the detection efficiency in the novel algorithm is several times to dozes times (which depends on the size of grids) more than that in the traditional SOLID method for the large complex model environments.

collision detection; grids enveloping; axis aligned bounding box (AABB); industrial robot

2016-03-28

浙江省自然科学基金资助项目(LZ14E050003)

TP391.9DOI：10.3969/j.issn.1004-132X.2017.03.011

赵 亮，男，1976年生。浙江大学机械工程学院副教授。主要研究方向为工业机器人和智能检测设备。E-mail:mezl@zju.edu.cn。张义德，男，1988年生。浙江大学机械工程学院硕士研究生。胡旭晓，男，1965年生。浙江理工大学机械与自动控制学院教授。