利用虛擬傳感器的巡視器機械臂碰撞檢測算法

居鶴華，冷　舒

(北京工業(yè)大學(xué)深空機器人研究中心，100022北京)

?

利用虛擬傳感器的巡視器機械臂碰撞檢測算法

居鶴華，冷舒

(北京工業(yè)大學(xué)深空機器人研究中心，100022北京)

摘要:為提高傳統(tǒng)AABB樹碰撞檢測的精度和效率，提出一種基于虛擬傳感器的月面巡視器機械臂碰撞檢測算法．建立月面巡視器機械臂的逆運動學(xué)解算模型;對地面環(huán)境點云數(shù)據(jù)進行Delaunay三角化，采用多叉樹代替二叉樹作為AABB樹儲存環(huán)境點云三角面集;利用虛擬傳感器簡化巡視器機械臂結(jié)構(gòu)模型，通過虛擬傳感器遍歷AABB樹中的環(huán)境點云三角面集進行碰撞檢測，避免機械臂與環(huán)境發(fā)生干涉．月面巡視器就位探測任務(wù)內(nèi)場實驗表明:基于虛擬傳感器的月面巡視器碰撞檢測算法使碰撞檢測精度在1 mm內(nèi)，碰撞檢測時間降低至10 s內(nèi)．基于虛擬傳感器的碰撞檢測算法具有高效性和可行性．

關(guān)鍵詞:月面巡視器;機械臂; Delaunay三角化;虛擬傳感器; AABB樹;碰撞檢測

嫦娥三號月面巡視器是一個六輪搖臂式月面巡視器［1］，其前端攜帶機械臂的末端裝載X射線譜儀接近月面探測目標．月面巡視器就位探測任務(wù)書中要求執(zhí)行探測任務(wù)時，X射線譜儀與探測面的距離＜30 mm，且就位探測規(guī)劃任務(wù)在1 min之內(nèi)完成．在該任務(wù)中滿足巡視器與環(huán)境不存在干涉是一個重要的約束條件，準確、高效地進行碰撞檢測是其中的難點．

目前，國內(nèi)外對碰撞檢測都進行了大量研究．Wong等［2］提出通過連續(xù)自我碰撞檢測的方法進行避障，但這種方法適用于形狀易于發(fā)生改變的物體與非變形體進行碰撞檢測，與月面巡視器機械臂碰撞檢測情況不同．賈慶軒等［3］提出使用幾何方法進行機械臂與環(huán)境的碰撞檢測，PAUL［4］等提出通過幾何方法解決火星車機械臂碰撞檢測問題．但上述方法都并非以月面巡視器機械臂作為碰撞檢測的研究對象．王偉等［5］提出基于OBB包圍盒［6］的碰撞檢測算法．OBB包圍盒算法可以精確進行碰撞檢測計算，但在物體姿態(tài)變換時，需重新計算包圍盒，導(dǎo)致效率不高，難以滿足高效性需要．Gino［7］提出基于GJK算法［8］的碰撞檢測，Cameron［9］提出增強GJK的碰撞檢測算法，Mirtich［10］提出基于V－Clip的碰撞檢測算法．上述3種算法對于外形復(fù)雜機構(gòu)的碰撞檢測十分有效，但月面巡視器機械臂外形簡單，用上述方法會降低碰撞檢測效率．Gino［11］還提出一種基于AABB包圍盒的碰撞檢測算法，該算法的效率優(yōu)于OBB包圍盒算法，但仍需要進行大量的包圍盒計算，難以滿足任務(wù)需求．

綜上所述，本文提出一種基于虛擬傳感器的月面巡視器機械臂碰撞檢測算法:

1)通過建立機械臂運動學(xué)模型，精確計算機械臂上的任意一點的位置，保證機械臂碰撞檢測順利進行; 2)簡化機械臂模型，將機械臂桿用有限根虛擬傳感器包圍; 3)將月面環(huán)境點云圖進行Delaunay三角化，創(chuàng)建點云三角面集，并通過多叉樹構(gòu)造的AABB樹儲存該面集; 4)通過虛擬傳感器與AABB樹內(nèi)的面集進行代數(shù)分析計算．確定機械臂與環(huán)境是否發(fā)生碰撞; 5)以嫦娥三號月面巡視器進行機械臂內(nèi)場探測任務(wù)為例，驗證算法的正確性．

1　月面巡視器機械臂逆運動學(xué)

1．1機械臂建模

巡視器機械臂采用三自由度關(guān)節(jié)型串聯(lián)構(gòu)型，安裝在巡視器前端，末端安裝有效載荷——粒子激發(fā)X射線譜儀探頭．機械臂結(jié)構(gòu)如圖1所示．

圖2　機械臂D－H坐標系

機械臂對應(yīng)的D－H坐標系如圖2所示．

為確定X射線譜儀中心C，建立虛運動副R3/E，其對應(yīng)原點是o4，并建立相應(yīng)坐標系4．

D－H參數(shù)表如表1所示．

表1　月面巡視器機械臂D－H參數(shù)

符號物理意義如下:jri為體系j原點至體系i原點的位置矢量在體系j下的投影，jrS為體系j原點至空間內(nèi)一點S的位置矢量在體系j下的投影，jrS［i］)為體系j原點至空間內(nèi)一點S的位置矢量在體系j下的投影的第i個分量，jpS為體系j原點至空間內(nèi)一點S的齊次位置矢量在體系j下的投影，jQi為體系j至體系i的旋轉(zhuǎn)變換陣在體系j下的表示，為體系j至體系i的繞體系j的x軸進行定軸轉(zhuǎn)動的旋轉(zhuǎn)變換陣在體系j下的表示，jTi為體系j至體系i的齊次變換陣在體系j下的表示．

記λi=cos(αi)，μi=sin(αi)，cos(θi)=C(θi)，sin(θi)=S(θi)，其中θi為軸轉(zhuǎn)角，則相鄰兩個DH系的旋轉(zhuǎn)變換陣iQi+1為

鏈節(jié)位矢在根向體系表示為

1．2機械臂逆運動學(xué)

1．2．1機械臂逆運動學(xué)

本文采用解析方法對機械臂逆運動學(xué)進行求解［12～13］．

三自由度串聯(lián)機械臂系統(tǒng)定位方程:

將式(1)、式(2)帶入式(3)中，展開得

由式(4)得三軸角度

1．2．2機械臂正逆運動學(xué)計算結(jié)果校驗

機械臂正運動學(xué)與逆運動學(xué)是互逆的行為．可用表2所示實驗數(shù)據(jù)驗證其計算的正確性．由表1可知，軸轉(zhuǎn)角有角度范圍約束，故逆運動學(xué)解數(shù)目不固定．

機械臂正逆運動學(xué)相互校驗仿真結(jié)果如組圖3所示．

表2　巡視器機械臂正逆運動計算驗證

圖3　機械臂正逆運動學(xué)相互校驗仿真

2　Delaunay三角化地面點云數(shù)據(jù)

Delaunay三角化是構(gòu)造三角網(wǎng)格的經(jīng)典方法．通過該方法得到的三角網(wǎng)格不存在奇異性，并具有最優(yōu)性和唯一性［14］．Delaunay三角化通過一個平衡二叉樹結(jié)構(gòu)實現(xiàn)．其三角面集需滿足條件:在平面內(nèi)，通過三角型的3個頂點的圓中不會包含平面內(nèi)第4個頂點．

本文使用節(jié)點增量法構(gòu)造Delaunay三角網(wǎng)格［15］．在增加節(jié)點前，首先通過節(jié)點數(shù)量確定網(wǎng)格密度，若增加的節(jié)點與網(wǎng)格中已有的節(jié)點在XY剖分面內(nèi)投影的距離小于網(wǎng)格密度，則過濾掉該點．否則，增加該點．Delaunay三角化流程如圖4所示．

圖4　Delaunay三角化流程

3　月面巡視器機械臂碰撞檢測

3．1碰撞檢測步驟

月面巡視器機械臂就位探測任務(wù)提出:要準確檢測機械臂與地面環(huán)境是否發(fā)生碰撞．若不碰撞，需求得機械臂與地面間的最小距離，否則，在任務(wù)中提示碰撞．根據(jù)任務(wù)要求，碰撞檢測執(zhí)行步驟如下:

1)簡化機械臂模型:將機械臂劃分為5部分，分別記為A1，B2，C3，D4，E5．對每部分，環(huán)繞該部分桿件最大外徑并平行于桿件軸線分別固接n根如同激光測距儀的虛擬傳感器線段將機械臂包起來，記為dSi，i∈1，2，…，n，如圖5所示．

2)將地面點云數(shù)據(jù)Delaunay三角化后形成的三角網(wǎng)格面集以AABB包圍盒的形式存儲于AABB樹中．

圖5　虛擬傳感器建模

3)使用虛擬傳感器射線與地形三角面集的包圍盒進行粗碰撞檢測．若非碰撞，則該虛擬傳感器與地面不碰撞．反之，進行射線與相應(yīng)三角面的精碰撞檢測．若仍碰撞，可得到一條碰撞線段記為dRi，i∈1，2，…，36．再判斷該虛擬傳感器線段是否與三角面集碰撞．若碰撞，則機械臂與地面碰撞．反之，機械臂與地面距離為

選取dRSi中最短的線段為最短距離．

碰撞檢測流程圖如圖6所示．

圖6　碰撞檢測流程圖

3．2碰撞檢測實現(xiàn)

3．2．1碰撞檢測數(shù)學(xué)模型

機械臂第i部分上虛擬傳感器檢測線段起點A及終點B位置分別為irA、irB，由式得1rA、1rB間的線段方程為

故虛擬傳感器檢測點在機械臂底座系下的位置已完全可知．通過虛擬傳感器線段方程與障礙三角面方程聯(lián)列求解，可判斷虛擬傳感器與三角面是否碰撞．

若障礙三角面Δ［s1，s2，s3］在機械臂底座坐標系下的坐標分別為1rs1，1rs2，1rs3，則三角面平面方程為

若t?［0，1］，線段與三角面不相交，否則將式(7)帶入式(5)中，求得交點1rt:

通過交點是否在三角面內(nèi)，判定線段與三角面是否相交．

3．2．2虛擬傳感器的碰撞檢測

傳統(tǒng)基于AABB樹的碰撞檢測主要為通過構(gòu)造二叉樹存儲機構(gòu)的AABB包圍盒，再查找AABB樹結(jié)構(gòu)中節(jié)點間的包圍盒交疊情況．若查找到一對節(jié)點出現(xiàn)交疊，則根據(jù)節(jié)點所處位置，繼續(xù)進行如下判斷，得到最終結(jié)果，原理如圖7所示．

圖7　傳統(tǒng)AABB樹的碰撞檢測原理

1)一對節(jié)點都是葉節(jié)點:判定碰撞．

2)一對節(jié)點中，一個節(jié)點為葉節(jié)點，另一個節(jié)點是內(nèi)部節(jié)點:葉節(jié)點對內(nèi)部節(jié)點的所有子節(jié)點進行檢測，判斷是否存在碰撞，直到該內(nèi)部節(jié)點對應(yīng)的所有葉節(jié)點檢測完畢．

3)一對節(jié)點都是內(nèi)部節(jié)點:選擇子節(jié)點少的節(jié)點記為A，另一節(jié)點記為B．A與B的每一個子節(jié)點都進行檢測，判斷是否碰撞．

在嫦娥三號內(nèi)場任務(wù)中，環(huán)境點云數(shù)據(jù)量超過40萬個點，Delaunay三角化產(chǎn)生的三角面集數(shù)量龐大，巡視器機械臂中也有1 000個以上的三角面．若采用傳統(tǒng)AABB樹方法檢測碰撞，需大量遍歷AABB樹中的節(jié)點，導(dǎo)致效率降低．

本文采用多叉樹將所有三角面集分別存儲至AABB樹葉節(jié)點，則不存在傳統(tǒng)AABB樹中的第2)、3)步，如圖8所示．通過機械臂簡化模型后的虛擬傳感器遍歷AABB樹的葉節(jié)點進行碰撞檢測．只要存在一次碰撞，檢測結(jié)束．否則，可檢測機械臂與地面環(huán)境的最小距離，原理見圖9．

圖8　多叉樹構(gòu)成的AABB樹碰撞檢測原理

圖9　虛擬傳感器碰撞檢測原理

4　月面巡視器碰撞檢測仿真實驗

月面巡視器碰撞檢測通過機械臂就位探測仿真軟件輔助任務(wù)進行．軟件使用VC++搭建了月面巡視器機械臂就位探測仿真環(huán)境．環(huán)境點云數(shù)據(jù)由月面巡視器攜帶的相機獲得．軟件導(dǎo)入該數(shù)據(jù)，并通過3D仿真環(huán)境點云數(shù)據(jù)(見圖10)．

由點云數(shù)據(jù)生成的Delaunay三角網(wǎng)格面和仿真圖見圖11．通過月面巡視器內(nèi)場實驗——機械臂探測平地任務(wù)和探測石塊任務(wù)的碰撞檢測驗證碰撞檢測算法的可靠性．

圖10　地面環(huán)境點云圖

圖11　地面環(huán)境Delaunay三角面和仿真

4．1巡視器機械臂探測平地的碰撞檢測

平地探測是機械臂就位探測任務(wù)的基礎(chǔ)．通過機械臂結(jié)構(gòu)數(shù)據(jù)可知，機械臂只有E5部分可能與地面環(huán)境發(fā)生碰撞．故主要研究E5部分的虛擬傳感器與地面環(huán)境三角面的碰撞檢測問題．

機械臂探測平地時，初始狀態(tài)的3個關(guān)節(jié)角為0°，0°，0°，如圖12所示．

圖12　探測平地初始狀態(tài)

機械臂3個關(guān)節(jié)角為－27°，－30°，－210°時，機械臂狀態(tài)如圖13所示．由圖13可知，機械臂未與地面碰撞．碰撞檢測結(jié)果如表3所示．機械臂在該位形狀態(tài)時，其E5部分的n條虛擬傳感器與地面環(huán)境三角面均未發(fā)生碰撞．機械臂距地面最小距離約為0．045 m．

圖13　關(guān)節(jié)角為(－27°，－30°，－210°)時機械臂狀態(tài)

表3　機械臂碰撞檢測結(jié)果

機械臂3個關(guān)節(jié)角為(－27°，－40°，－210°)時，機械臂狀態(tài)如圖14所示．

圖14　關(guān)節(jié)角為(－27°，－40°，－210°)時機械臂狀態(tài)

由圖14可知，機械臂E 5部分與地面進行碰撞．碰撞檢測結(jié)果如表4所示．機械臂在該位形狀態(tài)時，其n條虛擬傳感器中部分與地面環(huán)境三角面發(fā)生碰撞．

表4　機械臂地面碰撞檢測結(jié)果

4．2巡視器機械臂探測石塊的碰撞檢測

機械臂探測石塊是探測任務(wù)中的難點．石塊的形狀大小不同，導(dǎo)致機械臂任何一桿，在執(zhí)行任務(wù)時會與石塊發(fā)生碰撞．為簡化問題，在進行探測任務(wù)時，先計算好石塊距車體的距離，保證只有E 5部分可能與石塊進行碰撞，將車體移至指定位置后，再進行探測任務(wù)．

機械臂探測石塊時，初始狀態(tài)3個關(guān)節(jié)角為(0°，0°，0°)，如圖15所示．

圖15　探測石塊初始狀態(tài)圖

機械臂3個關(guān)節(jié)角為(－27°，－40°，－210°)時，機械臂狀態(tài)如圖16所示．

由圖16可知，E5的36條線段中，部分線段與石塊三角面發(fā)生碰撞，碰撞檢測結(jié)果如表5所示．

圖16　關(guān)節(jié)角為(－27°，－40°，－210°)，機械臂狀態(tài)圖

表5　機械臂與石塊三角面碰撞檢測結(jié)果

4．3兩種方法碰撞檢測對比

基于虛擬傳感器算法的碰撞檢測速度和基于傳統(tǒng)AABB樹的碰撞檢測速度結(jié)果見圖17．碰撞點數(shù)相同時，基于虛擬傳感器法進行碰撞檢測的時間優(yōu)于傳統(tǒng)AABB樹碰撞檢測的時間．

圖17　兩種方法碰撞檢測時間對比

5　結(jié)　論

1)建立了機械臂逆運動學(xué)模型，通過解析方法精確計算機械臂上各點在任意姿態(tài)下的位置．該方法利用機械臂串聯(lián)構(gòu)型的特點，只用小規(guī)模的計算就可完成機械臂位置姿態(tài)的確定．

2)通過Delaunay三角化地面環(huán)境點云文件，保證點云數(shù)據(jù)的魯棒性．

3)用多叉樹代替二叉樹作為AABB樹存儲環(huán)境點云三角面，減少AABB樹節(jié)點的存儲量．

4)簡化機械臂模型，利用虛擬傳感器與AABB樹存儲的三角面進行碰撞檢測，減少了AABB樹對節(jié)點的遍歷時間，提高了效率，保證了精確性．

5)內(nèi)場試驗驗證了算法的高效性和可行性．

參考文獻

［1］孫澤洲，張廷新，張熇，等．嫦娥三號探測器的技術(shù)設(shè)計與成就［J］．中國科學(xué)(技術(shù)科學(xué))，2014，44(4) : 331－343．

［2］WONG S K，LIN W C，HUNG C H，et al．Radial view based culling for continuous self-collision detection of skeletal models［J］．ACM Transaction on Graphics，2013，32(4) : 1－10．

［3］賈慶軒，陳鋼，孫漢旭，等．基于A*算法的空間機械臂避障路徑規(guī)劃［J］．機械工程學(xué)報，2010，46(13) : 109－115．

［4］PAUL B，ANTONIO D C，MATTHEW R，et al．Automated rover position and instrument placement［C］/ / Aerospace Conference．Piscataway: IEEE，2005: 1－12．

［5］王偉，馬峻，劉偉．基于OBB包圍盒的碰撞檢測研究與應(yīng)用［J］．計算機仿真，2009，26(9) : 180－183．

［6］GOTTSCHALK S，LIN M C，MANOCHA D．OBBTree: a hierarchical structure for rapid Interference detection［C］/ / 23rd annual conference on Computer graphics and interactive techniques．New York: ACM Publications，1996: 171－180．

［7］GINO V D B．A fast and robust GJK implementation for collision detection of convex objects［J］．Journal of Graphics Tools，1999，4(2) : 7－25．

［8］GILBERT E G，JOHNSON D W，KEERTHI S S．A fast procedure for computing the distance between complex objects in three-dimensional space［J］．IEEE Journal of Robotics and Automation，1988，4(2) : 193－203．

［9］CAMERON S．Enhancing GJK: Computing minimum and penetration distances between convex polyhedra［C］/ / International Conference on Robotics and Automation．Albuquerque: IEEE Press，1997: 3112－3117．

［10］MIRTICH B．V-Clip: fast and robust polyhedral collision detection［J］．ACM Transaction on Graphics，1998，17 (3) : 177－208．

［11］GINO V D B．Collision detection in interactive 3D environments［M］．Eindhoven: University Press Facilities，1999: 98－109．

［12］付榮，居鶴華．高精度解耦六自由度機械臂逆運動學(xué)解法［J］．計算機測量與控制，2010，18(7) : 1637－1640．

［13］JORGE Angeles．機器人機械系統(tǒng)原理－理論、方法和算法［M］．宋偉剛，譯．北京:機械工業(yè)出版社，2004: 83－90．

［14］RAJAN V T．Optimality of the delaunay triangulation in Rd［J］．Discrete＆Computational Geometry，1994，12(1) : 189－202．

［15］LEE D T，SCHACHTER B J．Two algorithms for constructing a Delaunay triangulation［J］．International Journal of Computer＆Information Sciences，1980，9(3) : 219－242．

(編輯楊波)

A collide detection algorithm based on virtual sensors of lunar rover manipulator

JüHehua，LENG Shu
(Research Center of Deepspace Robot，Beijing University of Technology，100022 Beijing，China)

Abstract:To improve the accuracy and efficiency of classic AABB collide detection，An algorithm of the virtual sensors which is applied to the domain of Rover manipulator collide detection is proposes．At first，the inverse kinematics model of rover manipulator is established．Secondly，lunar terrain point cloud data is triangulated by Delaunay triangulation method，the triangulation face set is then stored in the leaf nodes of a multiple tree (AABB tree)．In the end，the rover manipulator model is simplified by virtual sensors．By utilizing the virtual sensors to traverse the face set stored in the multiple tree，manipulator/lunar terrain collision are detected and avoided．The inner-yard experiment of lunar rover in-situ exploration task show that the accuracy of the collide detection increased to within 1mm and the time of collide detection decreased to 10 s by the algorithm proposed．The feasibility and efficiency of the collide detection algorithm is justified．

Keywords:lunar rover; manipulator; delaunay triangulation; virtual sensors; AABB tree; collide detection

通信作者:冷舒，ptwaixingren@ 126．com．

作者簡介:居鶴華(1969—)，男，教授，碩士生導(dǎo)師．

基金項目:國家自然科學(xué)基金(10002011200902)．

收稿日期:2014－11－03．

doi:10．11918/j．issn．0367-6234．2016．01．009

中圖分類號:TP241

文獻標志碼:A

文章編號:0367－6234(2016) 01－0058－08