工業(yè)機(jī)器人運(yùn)動碰撞的仿真實現(xiàn)

陳勝奮，謝明紅

（華僑大學(xué) 機(jī)電及自動化學(xué)院，福建 廈門361021）

工業(yè)機(jī)器人工作的范圍廣、靈活性強(qiáng)等特點使其在自動化領(lǐng)域得到了廣泛的應(yīng)用［1］.但工業(yè)機(jī)器人在運(yùn)動過程中容易發(fā)生碰撞，碰撞的后果十分嚴(yán)重［2］.因此，工業(yè)機(jī)器人的碰撞檢測是十分必要的.目前，對碰撞檢測普遍的做法是以包圍盒碰撞檢測進(jìn)行快速粗檢測，然后以三角形面片求交進(jìn)行精檢測.包圍盒法的思想是用略大且規(guī)則的簡單包圍盒代替復(fù)雜的模型.包圍盒（長方體）模型主要有AABB，OBB等，每一種包圍盒都具有各自的優(yōu)勢和不足［3］.對包圍盒進(jìn)行粗略的碰撞檢測可以節(jié)省時間［4－5］.當(dāng)前，對包圍盒碰撞的研究有很多.Gottschalk等［6］采用OBB 樹進(jìn)行碰撞檢測研究；Wang等［7］采用AABB包圍盒近似表述模型并粗略判斷碰撞的位置；Chang等［8］結(jié)合包圍盒和包圍球進(jìn)行快速碰撞檢測.但是，很少有對包圍盒本身進(jìn)行研究.在綜合考慮層次包圍盒檢測效率下，本文提出一種改進(jìn)的層次包圍盒法的碰撞檢測方法，以提高檢測速度.

圖1 工業(yè)機(jī)器人兩種不同的建模方式Fig.1 Two different modeling methods of building industrial robot

1 工業(yè)機(jī)器人模型的建立

工業(yè)機(jī)器人的碰撞檢測是判斷各個桿件之間是否存在碰撞或干涉，其中首要任務(wù)是建立工業(yè)機(jī)器人模型.建立模型的方法一般有：1）使用glut庫函數(shù)代碼編程，由于glut庫函數(shù)只有一些簡單的三維模型，如圓柱，長方體等，這樣生成的模型也就相對簡單；2）使用三維建模軟件，如UG，PRO／E，這些軟件可以建立各種各樣的模型且操作簡便.相比采用第二種方法生成的模型可以達(dá)到更好的仿真效果.

由三維建模軟件建立各個桿件模型并將模型保存為3ds格式文件，再由VC＋＋代碼編程，讀取桿件模型的文件，通過VC＋＋代碼將各個桿件進(jìn)行一系列的平移和旋轉(zhuǎn)，變換到空間相應(yīng)位置并用OpenGL顯示［9－10］，以實現(xiàn)工業(yè)機(jī)器人的模型建立.工業(yè)機(jī)器人兩種不同的建模方式，如圖1 所示.比較圖1（a），1（b）可知：三維建模軟件建立的工業(yè)機(jī)器人模型可以相對復(fù)雜且更有效率.

2 改進(jìn)層次包圍盒的建立

2.1 改進(jìn)包圍盒的建立

構(gòu)造AABB包圍盒計算量較少，只需比較模型對象各點元素的坐標(biāo)值，就可以得到坐標(biāo)軸上的最大值與最小值.這些值構(gòu)成了AABB 包圍盒的頂點（圖2（a））.OBB包圍盒計算則相對復(fù)雜，首先得通過這些點元素的坐標(biāo)值計算出協(xié)方差矩陣；然后計算協(xié)方差矩陣的特征值和特征向量；最后以特征向量為新的坐標(biāo)軸，把所有點元素的坐標(biāo)投影到新的坐標(biāo)軸上，得到在新坐標(biāo)軸上坐標(biāo)最大值和最小值，這些值構(gòu)成了OBB包圍盒的頂點（圖2（b））.

綜上可知，AABB包圍盒計算簡單，但結(jié)構(gòu)不緊湊；OBB包圍盒結(jié)構(gòu)緊湊，但計算復(fù)雜.因此結(jié)合兩者的優(yōu)點，提出了一種基于AABB－OBB結(jié)合改進(jìn)的包圍盒（圖2（c））.程序仿真的工業(yè)機(jī)器人模型是由代碼讀取每個獨立的桿件模型文件并顯示，以實現(xiàn)快速建模.每個桿件模型都有各自的坐標(biāo)系，每個包圍盒都是基于對應(yīng)桿件模型的坐標(biāo)系建立的.選擇合適的桿件坐標(biāo)系是十分必要的，它可以減少包圍盒建立的計算量，一般選擇桿件旋轉(zhuǎn)中心為坐標(biāo)系原點.各個桿件模型的坐標(biāo)系是獨立的，不相互影響，所以構(gòu)造的包圍盒只需在對應(yīng)桿件模型的坐標(biāo)系下，通過比較點元素坐標(biāo)最值，由最值構(gòu)造出改進(jìn)的包圍盒頂點.改進(jìn)的包圍盒除了坐標(biāo)系與原來包圍盒坐標(biāo)系不同外，其他的步驟與構(gòu)造傳統(tǒng)AABB包圍盒相同.由圖2可知：改進(jìn)的包圍盒既有OBB包圍盒的緊湊，又有AABB包圍盒構(gòu)造的簡便.

圖2 包圍盒的二維示意圖Fig.2 2Dschematic diagram of bounding box

工業(yè)機(jī)器人作業(yè)時，桿件模型會發(fā)生旋轉(zhuǎn)，各模型點元素坐標(biāo)就會變化，構(gòu)造的包圍盒也會隨著變化.AABB或OBB采用重構(gòu)的方法是先重新計算新的桿件模型點元素坐標(biāo)，再由原先構(gòu)造包圍盒的步驟進(jìn)行重構(gòu)；而改進(jìn)的包圍盒針對這種情況采用的方法比較靈活，只需計算桿件模型坐標(biāo)系間的變換矩陣，無需重新計算點元素坐標(biāo).較傳統(tǒng)包圍盒重構(gòu)，改進(jìn)的包圍盒重構(gòu)減少了大量運(yùn)算，提高了效率.

2.2 八叉樹結(jié)構(gòu)的建立

八叉樹是層次包圍盒進(jìn)行有效快速分割的一種方法［11］.從根節(jié)點出發(fā)將根節(jié)點分割成8個節(jié)點，再將節(jié)點分割成新的8個節(jié)點.重復(fù)此步驟直至滿足分割深度條件時，把包圍盒內(nèi)的三角形序號存儲到對應(yīng)的葉子節(jié)點數(shù)組內(nèi).八叉樹結(jié)構(gòu)的示意圖和建立流程圖，分別如圖3，4所示.

圖3 八叉樹結(jié)構(gòu)示意圖Fig.3 Octree structure diagram

圖4 八叉樹建立的流程圖Fig.4 Flow chart of establishing octree

構(gòu)建層次包圍盒模塊需要構(gòu)造一個合適的存儲數(shù)據(jù)結(jié)構(gòu)，以便代碼程序調(diào)用.一般構(gòu)造層次包圍盒采用的數(shù)據(jù)結(jié)構(gòu)有鏈表和數(shù)組.定義數(shù)組時，數(shù)組長度大小必須是已知的，但包圍盒的層數(shù)是變化的，很容易造成數(shù)組的越界或者內(nèi)存空間的浪費(fèi).鏈表采用指針的方式，可以動態(tài)申請內(nèi)存空間，需要時就向系統(tǒng)申請內(nèi)存，不需要時也可以動態(tài)刪除.這樣既避免了越界的風(fēng)險，又提高了對內(nèi)存空間的利用.

定義存儲數(shù)據(jù)結(jié)構(gòu)如下

3 碰撞檢測

3.1 粗略檢測

依據(jù)分離軸理論［12］，不同桿件模型的包圍盒碰撞檢測需在同一個坐標(biāo)系下進(jìn)行.由于改進(jìn)的包圍盒都是基于對應(yīng)桿件模型的坐標(biāo)系，所以不同桿件模型坐標(biāo)系之間的相對位姿就顯得格外重要.工業(yè)機(jī)器人模型初始的位置和姿態(tài)信息保存在裝配文件里，通過VC＋＋編程讀取裝配文件獲得所需的位置和姿態(tài)，通常位姿信息是用4維方陣表示.由桿件模型的初始位姿信息和工業(yè)機(jī)器人運(yùn)動學(xué)性質(zhì)［13］，可以得出2個不同桿件模型a，b之間的相對位置和姿態(tài)關(guān)系，即

式（1）中：Mba為桿件模型a和b之間的相對位置和姿態(tài)關(guān)系；Tba為3×3姿態(tài)矩陣；lba為位置向量.

在進(jìn)行檢測前，需將不同包圍盒的中心位置轉(zhuǎn)換到同一個坐標(biāo)系下.包圍盒間的中心位置隨著桿件模型運(yùn)動變化，而包圍盒中心坐標(biāo)相對于對應(yīng)的模型坐標(biāo)系是不變的.因此，只需知道模型坐標(biāo)系間的相對位姿關(guān)系，就可以快速將改進(jìn)包圍盒中心位置轉(zhuǎn)換到同一坐標(biāo)系下.式（1）的相對位姿是兩桿件模型的旋轉(zhuǎn)點之間的位姿.桿件模型旋轉(zhuǎn)中心為桿件坐標(biāo)系原點，在桿件模型坐標(biāo)系下，包圍盒的相對姿態(tài)Tba不隨點移動而變化.可以得到不同包圍盒的中心相對姿態(tài)，即

式（2）中：Mbcac包圍盒中心點相對位姿；i為桿件模型a的中心在自身坐標(biāo)系下的位置矢量；j為桿件模型b的中心在a坐標(biāo)系下的位置矢量.

把不同包圍盒坐標(biāo)轉(zhuǎn)換到同一坐標(biāo)系后，依據(jù)分離軸理論，經(jīng)過15次投影判斷，就可以快速判斷包圍盒是否碰撞.

3.2 精確檢測

八叉樹層數(shù)越多，包圍盒就越能準(zhǔn)確地包圍模型，碰撞檢測結(jié)果也就越近似模型碰撞結(jié)果.但如果僅依靠增加八叉樹層數(shù)來提高判斷精度，模型碰撞會產(chǎn)生誤判，使正常加工難以進(jìn)行.這是因為采用層次包圍盒并不能精確表示模型，包圍盒碰撞檢測只能確定可能發(fā)生碰撞的區(qū)域，并不能確定工業(yè)機(jī)器人模型在該區(qū)域發(fā)生碰撞.當(dāng)八叉樹層數(shù)超過一定數(shù)目時，系統(tǒng)會占用很大的內(nèi)存，造成程序不能順暢運(yùn)行.所以，判斷模型是否產(chǎn)生碰撞，不能僅通過增加八叉樹層數(shù)，而需要新的方法來進(jìn)行精確碰撞檢測.

雖然機(jī)器人造型的軟件各不相同，但一般通過軟件建立的模型都是由三角網(wǎng)格面構(gòu)成的.模型的三角網(wǎng)格就可以精確表示模型，所以對模型進(jìn)行精確的碰撞檢測就可以變成對不同桿件模型之間的三角網(wǎng)格求交的問題.只要三角面片相交，對應(yīng)的兩個模型就會發(fā)生碰撞.

粗略碰撞檢測確定了碰撞區(qū)域后，可以通過索引直接獲得區(qū)域的三角形序號i及三角形的三個頂點坐標(biāo)p0，p1，p2；然后采用Devillers算法，即通過兩個三角形的頂點構(gòu)成的行列式的正負(fù)來判斷點與面之間的相對位置；接著計算與平面的交點的位置是否在三角形內(nèi)，判斷兩個三角形是否產(chǎn)生干涉，流程如圖5所示.

求線段兩端點相對三角形的位置函數(shù)為bool vector4＿det（float3p0，float3p1，float3p2，float3p3，float3p4）；求線段與三角形交點的位置函數(shù)為bool ValidPoint（float3p0，float3p1，float3p2，float3p3，float3p4）.其中，p0，p1，p2 是△p0p1p2的三個頂點坐標(biāo)；p3，p4是△p3p4p5兩頂點p3和p4坐標(biāo).通過函數(shù)vector4＿det的返回值來判斷線段p3p4的兩端點相對△p0p1p2的位置，如果是true就表示p3和p4在△p0p1p2異側(cè)；否則，就是同側(cè).通過函數(shù)ValidPoint的返回值判斷線段與三角形交點的位置，如果是false，則線段p3p4與△p0p1p2的交點不在△p0p1p2內(nèi)，△p3p4p5中的線段p4p5和p3p5重復(fù)判斷線段p3p4的過程；如果是true，則△p0p1p2和△p3p4p5相交，可判定桿件模型產(chǎn)生碰撞.重復(fù)區(qū)域內(nèi)所有的三角形之間的干涉檢測，只要有一次判斷干涉，工業(yè)機(jī)器人就產(chǎn)生碰撞.

圖5 碰撞檢測的流程圖Fig.5 Flow charts of collision detection

4 軟件仿真實現(xiàn)實例

定義ModelContext類存儲讀入的桿件的基本屬性；定義GSMatrix4X4＿f四維矩陣存儲桿件的位置和姿態(tài)；定義COctree類存儲八叉樹的子節(jié)點、深度、索引號等信息；定義CVector類存儲點坐標(biāo)和矢量坐標(biāo)等.

應(yīng)用VC＋＋和OpenGL進(jìn)行軟件編程.讀入桿件模型文件和裝配信息文件，把基本信息屬性存儲在ModelContext類，把桿件模型的位置和姿態(tài)存儲在GSMatrix4X4＿f類.根據(jù)ModelContext類和GSMatrix4X4＿f類的數(shù)據(jù)信息建立改進(jìn)的層次包圍盒（圖6（a）），并把包圍盒的數(shù)據(jù)存儲在COctree類里.依據(jù)COctree類和GSMatrix4X4＿f類的數(shù)據(jù)信息可以進(jìn)行粗略的碰撞檢測（圖6（b））.精確碰撞檢測（圖6（c））則需要ModelContext類、COctree類和GSMatrix4X4＿f類的數(shù)據(jù)，得出碰撞的精確檢測.通過時間測量，構(gòu)造AABB包圍盒并完成精確檢測約需要5.6ms，構(gòu)造OBB包圍盒并完成精確檢測約需要5.5ms，而構(gòu)造改進(jìn)的包圍盒并完成精確檢測約需要5ms，減少了檢測時間.

圖6 軟件實現(xiàn)的過程示意圖Fig.6 Schematic diagram of the software implement

5 結(jié)束語

采用三維建模軟件建立工業(yè)機(jī)器人模型，用VC＋＋編程讀取模型文件并顯示，這種方法可以快速建立比較復(fù)雜的模型.首先，在各自模型的坐標(biāo)系下建立對應(yīng)的包圍盒；其次，通過坐標(biāo)系間的變換關(guān)系，把包圍盒坐標(biāo)轉(zhuǎn)換到同一個坐標(biāo)系下，再應(yīng)用分離軸理論進(jìn)行包圍盒碰撞檢測；然后，利用三角形求交的方法進(jìn)行精確檢測；最后，生成一個實例程序完成碰撞檢測的整個過程.通過時間測量，改進(jìn)的算法提高了檢測速度.

［1］孫志杰，王善軍，張雪鑫.工業(yè)機(jī)器人發(fā)展現(xiàn)狀與趨勢［J］.吉林工程技術(shù)師范學(xué)院學(xué)報，2011，27（7）：61－62.

［2］劉雄偉.基于三角面片檢測的五軸數(shù)控加工碰撞干涉檢測智能算法研究［D］.廣州：廣東工業(yè)大學(xué)，2011：1－4.

［3］鄒益勝，丁國富，許明恒，等.實時碰撞檢測算法綜述［J］.計算機(jī)應(yīng)用研究，2008，25（1）：8－10.

［4］GARCIA－ALONSO A，SERRANO N，F(xiàn)LAUER J.Solving the collision detection problem ［J］.IEEE Computer Graphics and Applications，1994，14（3）：36－43.

［5］GOTTSCHALK S.Collision queries using oriented bounding boxes［D］.North Carolina：University of North Carolina at Chapel Hill，2000：64－86.

［6］GOTTSCHALK S，LIN M，MANOCHA D.OBBtree：A hierarchical structure for rapid interference detection［C］∥Proceedings of the 23rd Annual Conference on Computer Graphics and Interactive Techniques.New Orlean：ACM，1996：171－180.

［7］WANG Yao，HU Yan－juan，F(xiàn)AN Jiu－chen.Collision detection based on bounding box for NC machining simulation［J］.Physics Procedia，2012，24（1）：247－252.

［8］CHANG J W，KIM M S.Efficient triangle－triangle intersection test for OBB－based collision detection［J］.Computer and Graphics，2009，33（3）：235－240.

［9］侯俊杰.深入淺出MFC［M］.2版.武漢：華中科技大學(xué)出版社，2001：309－334.

［10］牛書濤，庫祥臣.基于Visual C＋＋與OpenGL的數(shù)控車削加工仿真系統(tǒng)研究［J］.機(jī)電一體化，2012，8（3）：35－39.

［11］吳明華，余勇翔，周濟(jì).采用空間分割技術(shù)的八叉樹干涉檢驗算法［J］.計算機(jī)學(xué)報，1997，20（9）：849－854.

［12］吳峰，于東，張曉輝，等.數(shù)控機(jī)床實時碰撞檢測算法的研究與實現(xiàn)［J］.組合機(jī)床與自動化加工技術(shù)，2011（12）：49－51.

［13］于靖軍，劉辛軍，丁希侖，等.機(jī)器人機(jī)構(gòu)學(xué)的數(shù)學(xué)基礎(chǔ)［M］.北京：機(jī)械工業(yè)出版社，2008：278－281.