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

陳勝奮，謝明紅

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

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

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

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

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

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

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

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

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

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

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

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

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

八叉樹是層次包圍盒進(jìn)行有效快速分割的一種方法［11］.從根節(jié)點(diǎn)出發(fā)將根節(jié)點(diǎn)分割成8個(gè)節(jié)點(diǎn)，再將節(jié)點(diǎn)分割成新的8個(gè)節(jié)點(diǎn).重復(fù)此步驟直至滿足分割深度條件時(shí)，把包圍盒內(nèi)的三角形序號(hào)存儲(chǔ)到對(duì)應(yīng)的葉子節(jié)點(diǎn)數(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)造一個(gè)合適的存儲(chǔ)數(shù)據(jù)結(jié)構(gòu)，以便代碼程序調(diào)用.一般構(gòu)造層次包圍盒采用的數(shù)據(jù)結(jié)構(gòu)有鏈表和數(shù)組.定義數(shù)組時(shí)，數(shù)組長度大小必須是已知的，但包圍盒的層數(shù)是變化的，很容易造成數(shù)組的越界或者內(nèi)存空間的浪費(fèi).鏈表采用指針的方式，可以動(dòng)態(tài)申請(qǐng)內(nèi)存空間，需要時(shí)就向系統(tǒng)申請(qǐng)內(nèi)存，不需要時(shí)也可以動(dòng)態(tài)刪除.這樣既避免了越界的風(fēng)險(xiǎn)，又提高了對(duì)內(nèi)存空間的利用.

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

3 碰撞檢測(cè)

3.1 粗略檢測(cè)

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

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

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

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

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

3.2 精確檢測(cè)

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

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

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

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

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

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

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

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

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

5 結(jié)束語

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

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

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

［3］鄒益勝，丁國富，許明恒，等.實(shí)時(shí)碰撞檢測(cè)算法綜述［J］.計(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ù)的八叉樹干涉檢驗(yàn)算法［J］.計(jì)算機(jī)學(xué)報(bào)，1997，20（9）：849－854.

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

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