基于網格包絡的工業機器人仿真碰撞檢測算法

趙 亮 張義德 胡旭曉

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年生。浙江理工大學機械與自動控制學院教授。