一種適用于多機器人的動態(tài)包圍體層次樹碰撞檢測算法*

陳 琳，付 兵，潘海鴻，戴 駿，董海濤

(1. 廣西大學(xué) 機械工程學(xué)院，南寧 530004；2. 廣西制造系統(tǒng)與先進制造技術(shù)重點實驗室，南寧 530004；3.湖北汽車工業(yè)學(xué)院 機械工程學(xué)院，湖北 十堰 442002)

一種適用于多機器人的動態(tài)包圍體層次樹碰撞檢測算法*

陳 琳1,2，付 兵1,3，潘海鴻1,2，戴 駿1,2，董海濤1,2

(1. 廣西大學(xué) 機械工程學(xué)院，南寧 530004；2. 廣西制造系統(tǒng)與先進制造技術(shù)重點實驗室，南寧 530004；3.湖北汽車工業(yè)學(xué)院 機械工程學(xué)院，湖北 十堰 442002)

多機器人協(xié)同工作在噴涂、焊接、裝配等工業(yè)現(xiàn)場應(yīng)用廣泛。多機器人碰撞檢測是多機器人系統(tǒng)安全、可靠工作的內(nèi)在保證。針對現(xiàn)有多機器人間碰撞檢測算法耗時過多問題，提出動態(tài)包圍體層次樹碰撞檢測算法，該算法利用不同包圍體的長處構(gòu)建三層結(jié)構(gòu)的動態(tài)包圍體層次樹，采用層次展開的遍歷方式提高多機器人間碰撞檢測效率。在多機器人工作站中進行仿真實驗，得出對2、4、6、8個機器人采用動態(tài)包圍體層次樹碰撞檢測算法的碰撞檢測效率分別是RAPID算法的1.657，1.738，2.088，2.149倍。這驗證所提出的動態(tài)包圍體層次樹碰撞檢測算法能有效提高多機器人間碰撞檢測效率。

動態(tài)包圍體層次樹;碰撞檢測;多機器人

0 引言

碰撞檢測的目的是用來探測空間中移動的幾何物體之間是否相交，相交則找出相交的特征(overlap features)。碰撞檢測廣泛應(yīng)用于工程實踐，機器人，計算圖形處理，動畫，CAD/CAM，游戲中。因其重要性，許多學(xué)者在過去的二十年里對碰撞檢測算法進行了廣泛研究[1]。他們分別提出各具特色的AABB[2]，OBB[3]，bounding sphere[4]，K-dops[5]，SSV[6]等包圍體(bounding volume)算法，并用于碰撞檢測中。目前，比較成熟的碰撞檢測算法有RAPID[6]，Opcode[7]，適合解決兩個單獨的剛性物體之間的碰撞檢測問題。為加快剛體碰撞檢測的速度，包圍體層次樹[1,3,6]被廣泛應(yīng)用于碰撞檢測中。如果包圍體層次樹中父節(jié)點不存在碰撞，則無須對子節(jié)點進行檢測，從而提高碰撞檢測的速度。

隨著碰撞檢測算法的研究深入，碰撞檢測的對象從剛體擴展到多剛體、變形體和鉸接模型。其中以鉸接模型為研究對象的碰撞檢測是現(xiàn)在研究的一個熱點和難點[7-8]。典型的鉸接模型如多自由度工業(yè)機器人、虛擬環(huán)境中的手臂等。鉸接模型中關(guān)節(jié)是剛性,關(guān)節(jié)之間串并聯(lián)，相鄰關(guān)節(jié)不分離，空間運動時存在一定的相對位置關(guān)系。其不同于多剛體[9]間松散的位置關(guān)系，也不同于由大量稠密的基元組成的變形體[10]，變形體的基元位置和外形在模型坐標(biāo)系中是變化的[11]。機器人中關(guān)節(jié)的實際運動是其前端關(guān)節(jié)運動的合成，關(guān)節(jié)數(shù)增多造成關(guān)節(jié)實際運動軌跡復(fù)雜，檢測環(huán)境中物體發(fā)生碰撞的情況變得復(fù)雜，計算量劇增。

已有的包圍體層次樹的應(yīng)用局限于幾個剛體間的碰撞檢測[1-7]，且使用時包圍體層次樹是提前計算出來，因而其不適合直接應(yīng)用于由多個運動的單元組成的鉸接模型[7]。Redon[7]研究單個鉸接模型與工作環(huán)境的碰撞檢測時，在關(guān)節(jié)模型的掃略體包圍體基礎(chǔ)上計算AABB包圍體構(gòu)建鉸接模型的包圍體層次樹，剔除與環(huán)境距離遠(yuǎn)的單元模型； Zhang[12]研究多個鉸接模型間的碰撞檢測，他采用單元模型的泰勒模型來計算單元模型的AABB包圍體，構(gòu)建鉸接模型的AABB包圍體層次樹，剔除不碰撞的單元模型對。對于多個由單元模型串并聯(lián)而成的鉸接模型，如何提高其碰撞檢測效率這是一個值得研究的問題。這涉及到選擇哪種類型包圍體構(gòu)建鉸接模型的包圍體層次樹,如何構(gòu)建適合鉸接模型的包圍體層次等方面的研究。

以六自由度機器人為研究對象，該研究對象是由7個剛體(一個底座與六個連桿)串聯(lián)組成。為提高鉸接模型的碰撞檢測的效率和速度，提出一種動態(tài)包圍體層次樹結(jié)構(gòu)。鉸接模型中單元模型之間的位置是變化的，以LSS為基元構(gòu)建AABB包圍體，其構(gòu)建、更新速度快，能夠快速剔除不碰撞的單元模型，進而加快鉸接模型間的碰撞檢測速度。

1 動態(tài)包圍體層次樹及其構(gòu)建

動態(tài)包圍體層次樹包含三層：頂層為由單元模型的AABB包圍體構(gòu)建；中間層為OBB包圍體層次樹；底層為三角形。所提出的動態(tài)包圍體層次樹中包圍體大小、層次樹層次結(jié)構(gòu)、層次樹中相交節(jié)點個數(shù)以及層次樹遍歷深度是隨著機器人相對位置變化而變化的。

對于多個剛體串并聯(lián)的鉸接模型特點，提出動態(tài)包圍體層次樹，其包含三層結(jié)構(gòu)：

頂層：在關(guān)節(jié)碰撞檢測對的基礎(chǔ)上，構(gòu)建頂層，用于剔除不相交的包圍體。頂層選用LSS[6]包圍機器人關(guān)節(jié)模型，并作為剔除層中AABB包圍體的構(gòu)建基元(LSS包圍體緊湊性好，更新代價低；AABB測試代價小，基于LSS計算AABB簡單)。

根據(jù)圖1所示，以LSS為基元計算AABB包圍體：

圖1 以LSS為基元計算AABB包圍體

(a) 計算AABB包圍體中心坐標(biāo)

(1)

(b) 計算AABB包圍體半徑

(2)

注：O(X,Y,Z)是AABB包圍體的中心坐標(biāo)；RX,RY,RZ是AABB包圍體三個半徑。A和B是LSS的兩個端點；R是LSS半徑。

中間層：中間層檢測采用OBB包圍體(OBB包圍體緊湊易于更新)構(gòu)建包圍體層次樹[3]。鉸接模型中關(guān)節(jié)是剛性的，采用OBB包圍體來建立動態(tài)包圍體層次樹的中間層。每一個關(guān)節(jié)中的三角形采用自頂而下的方法來構(gòu)建一個OBB包圍體層次樹, OBB包圍體層次樹的葉子節(jié)點OBB包圍體中有一個三角形。這里采用二叉樹的數(shù)據(jù)結(jié)構(gòu)來構(gòu)建包圍體層次樹。有多少個關(guān)節(jié)就有多少個OBB包圍體層次樹。其中，每一個關(guān)節(jié)的OBB包圍體層次樹根節(jié)點都對應(yīng)頂層層次樹的葉子節(jié)點中包圍這個關(guān)節(jié)的包圍體。當(dāng)關(guān)節(jié)位置和姿態(tài)發(fā)生變化后，只需要更新動態(tài)包圍體層次樹的中間層層次樹中OBB包圍體的位置和姿態(tài)，無須重新計算其大小，因而其可實現(xiàn)快速更新中間層層次樹。

底層：動態(tài)包圍體層次樹的底層構(gòu)建過程比較特殊，它是在完成中間關(guān)節(jié)層包圍體碰撞檢測后，且當(dāng)中間關(guān)節(jié)層OBB包圍體層次樹葉子節(jié)點OBB發(fā)生碰撞后才需要建立。建立時將葉子節(jié)點OBB包圍體中三角形頂點坐標(biāo)分別轉(zhuǎn)換到世界坐標(biāo)系下，這些轉(zhuǎn)換后得到的三角形對構(gòu)成動態(tài)包圍體層次樹的底層。底層中三角形對的相交測試方法為moller算法。

2 動態(tài)包圍體層次樹遍歷與執(zhí)行過程

2.1 動態(tài)包圍體層次樹的遍歷

鉸接模型運動使得機器人彼此關(guān)節(jié)之間位置時刻發(fā)生變化，因而多機器人碰撞檢測過程中包圍體相交測試的數(shù)量，層次樹遍歷深度也是時刻變化的。為提高碰撞檢測效率，有必要知道包圍體層次樹的實際遍歷過程。

圖2 動態(tài)包圍體遍歷示意圖

建立包圍體遍歷層次樹[14]，二叉樹數(shù)據(jù)結(jié)構(gòu)類型的層次樹中節(jié)點遍歷順序可以抽象成節(jié)點包圍體遍歷順序圖。如圖2中OBB遍歷樹所示，包括層次樹相交測試節(jié)點和非相交測試節(jié)點。可知每個層次樹遍歷后節(jié)點個數(shù)不同，層次樹的遍歷深度也不一樣。另外，在遍歷順序圖中上層節(jié)點相交測試中如果不相交或者不用進行相交測試時，該上層節(jié)點的子節(jié)點則不需要進行相交測試。

動態(tài)包圍體層次樹實際遍歷過程如圖2所示，頂層中Ji(i= 1，……n)表示2個機器人關(guān)節(jié)組成的AABB檢測對；中間關(guān)節(jié)層表示兩個關(guān)節(jié)的兩個OBB包圍體層次樹之間的遍歷；底層表示需要進行相交測試的三角形檢測對。由于機器人間相對運動，動態(tài)包圍體層次樹的執(zhí)行過程中，頂層中AABB包圍體檢測中相交AABB對數(shù)是變化的；關(guān)節(jié)中間層中每個OBB包圍體層次樹遍歷深度不同(如第二個OBB遍歷樹沒有遍歷到底端就已經(jīng)結(jié)束碰撞檢測)，進行包圍體相交測試的數(shù)量也會不同；底層中進行相交測試的三角對個數(shù)是變化的或者不用進行底層相交測試。因此，動態(tài)包圍體層次樹相交測試的計算量在每個檢查點時刻是不同的。

2.2 動態(tài)包圍體層次樹的執(zhí)行過程

上面分別介紹了構(gòu)建鉸接模型的動態(tài)包圍體層次樹中各個部分的建立過程以及動態(tài)包圍體層次樹的遍歷過程。動態(tài)包圍體層次樹在每一個離散點的碰撞檢測中依次按照頂層，中間層，底層進行，如圖3所示。其應(yīng)用在鉸接模型間碰撞檢測中的執(zhí)行過程需按照以下3步：

(1)頂層層次樹中包圍體相交測試，得到頂層中相交的葉子節(jié)點包圍體，進一步獲得需要執(zhí)行相交測試的中間層OBB包圍體層次樹的根節(jié)點OBB包圍體；

(2)在中間層進行OBB包圍體相交測試，其目的在于獲取OBB包圍體層次樹中相交的葉子節(jié)點OBB包圍體，再利用他們?nèi)カ@取對應(yīng)的需要執(zhí)行相交測試的三角形檢測對。中間層中OBB包圍體層次樹按照層次展開進行遍歷，計算時先對檢測對中尺寸大的OBB包圍體子節(jié)點優(yōu)先遍歷；

(3)進行動態(tài)包圍體層次樹底層中三角形的相交測試，得到相交三角形對。

如果頂層中沒有發(fā)生葉子節(jié)點包圍體相交，則不進行中間層和底層的碰撞檢測；如果中間層中沒有葉子節(jié)點OBB包圍體相交，則不用進行底層碰撞檢測。

圖3 動態(tài)包圍體層次樹在一次 碰撞檢測中的執(zhí)行過程

對于一條確定軌跡(包含很多離散點)，整個碰撞檢測過程中按照以下5個步驟進行：

①構(gòu)建包圍關(guān)節(jié)的LSS包圍體或OBB包圍體基元(用于動態(tài)包圍體層次樹的構(gòu)建)；

②構(gòu)建動態(tài)包圍體層次樹；

③在一個離散點處執(zhí)行動態(tài)包圍體層次樹碰撞檢測；

④重新計算動態(tài)包圍體層次樹頂層；

⑤返回執(zhí)行③，④，直到完成所有離散點碰撞檢測。

3 實驗與結(jié)果分析

3.1 實驗仿真平臺及仿真實驗設(shè)計

基于OpenGL自主開發(fā)軟件仿真平臺，在VS2008中采用C++進行程序編寫。采用處理器Inter(R) Core(TM) i5-2300(主頻：2.8GHz)，4GB內(nèi)存，計算機操作系統(tǒng)為Windows XP。

以6自由度工業(yè)機器人TA1400作為實驗對象，仿真驗證多機器人動態(tài)包圍體層次樹碰撞檢測算法有效性。為獲得一般性結(jié)論，分別以2、4、6、8個機器人組成的多機器人工作站進行碰撞檢測仿真實驗。為進行動態(tài)包圍體層次樹碰撞檢測算法效率對比分析，這4組實驗中每組設(shè)計兩個實驗：①動態(tài)包圍體層次樹；②常規(guī)RAPID碰撞檢測算法(RAPID算法是公認(rèn)的用于碰撞檢測算法效率對比的標(biāo)準(zhǔn))。圖4是設(shè)計的8個機器人工作站，實驗中相同數(shù)量機器人工作站中對應(yīng)機器人運動的軌跡相同。

圖4 8個機器人工作站

3.2 實驗仿真結(jié)果與分析

根據(jù)設(shè)計實驗分別統(tǒng)計2種碰撞檢測算法的耗時，結(jié)果如表1所示：

表1 多機器人工作站碰撞檢測耗時

從表1中可以看出在2，4，6，8個機器人組成的工作站，采用動態(tài)包圍體層次樹的碰撞檢測效率明顯高于使用RAPID碰撞檢測效率：所提出的動態(tài)包圍體層次樹檢測算法的效率分別是RAPID算法的1.657，1.738，2.088，2.149倍。表明隨著工作站中機器人數(shù)目的增加，所提出的動態(tài)包圍體層次樹碰撞檢測效率將比RAPID效率明顯提高。

下面結(jié)合算法原理對碰撞檢測效率提高的主要原因進行分析：

①由于在碰撞檢測中采用動態(tài)包圍體層次樹，即在頂層剔除不碰撞關(guān)節(jié)對(關(guān)節(jié)模型上的OBB包圍體)，使得進行相交測試OBB包圍體對數(shù)量減少，從而縮短碰撞檢測時間。

②因機器人關(guān)節(jié)為細(xì)長桿狀模型，選用LSS包圍體包圍機器人關(guān)節(jié)會得到緊湊包圍體，并以此作為頂層AABB包圍體構(gòu)建基元，一方面使得AABB包圍體的計算簡化，層次樹節(jié)點更新耗時也相對少；另一方面較緊湊的包圍體，使得頂層和中間關(guān)節(jié)層包圍體相交測試對數(shù)相對小。雖然，動態(tài)包圍體層次樹中基元相交測試耗時與RAPID中相同。但是，在相同實驗條件下，RAPID中根節(jié)點OBB檢測對的數(shù)量比動態(tài)包圍體層次樹中的多得多。如在所設(shè)計試驗中測得的RAPID中根節(jié)點OBB檢測對的數(shù)量均是動態(tài)包圍體層次樹的50倍以上。

③根據(jù)已有文獻統(tǒng)計AABB相交測試耗時小于OBB對檢測耗時[15]。因而所提出的動態(tài)包圍體層次樹算法的碰撞檢測耗時在理論上小于RAPID。

4 結(jié)論

為提高多機器人間的碰撞檢測效率，借助多個不同包圍體的長處，提出具有三層結(jié)構(gòu)的動態(tài)包圍體層次樹碰撞檢測算法。結(jié)合實驗結(jié)果可知：①隨著機器人數(shù)量的增加，動態(tài)包圍體層次樹碰撞檢測效率相比RAPID會有明顯提高。②三層結(jié)構(gòu)的動態(tài)包圍體層次樹碰撞檢測算法是有效的。以LSS為基元構(gòu)建AABB包圍體作為頂層，使得動態(tài)包圍體層次樹構(gòu)建、更新速度加快，并能夠快速剔除不碰撞的關(guān)節(jié)，能夠加速碰撞檢測。③由于所提出的動態(tài)包圍體層次樹采用層次展開，計算時其先對中間層檢測對中尺寸大的包圍體的子節(jié)點優(yōu)先遍歷，這樣減少相交測試的包圍體對數(shù)，因此該方式可以進一步減少多機器人間碰撞檢測時間。

總之，該動態(tài)包圍體層次樹碰撞檢測算法可以為后續(xù)采用并行結(jié)構(gòu)的多機器人碰撞檢測研究奠定基礎(chǔ)，為多機器人碰撞檢測算法研究提供一定的借鑒作用。

[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

國家自然科學(xué)基金項目(51065005)；廣西自然科學(xué)基金項目(2012GXNSFAA053200)資助;廣西制造系統(tǒng)與先進制造技術(shù)重點實驗室項目(13-051-09S13)

陳琳(1973—)，女，山東青島人，廣西大學(xué)教授，博士，研究方向為機器人控制技術(shù)、數(shù)字信號檢測與處理、伺服電機控制，(E-mail)gxdxcl@163.com；通訊作者：潘海鴻(1966— )，男，廣西武鳴人，廣西大學(xué)教授，博士生導(dǎo)師,研究方向為多機器人協(xié)調(diào)控制技術(shù)，(E-mail)hustphh@163.com.cn。

TH166;TG659

A