面向遙操作機器人的混合層次包圍盒碰撞檢測

劉文聰,李作清,李世其

(華中科技大學(xué)機械科學(xué)與工程學(xué)院,湖北武漢430074)

0 引言

基于虛擬現(xiàn)實的遙操作機器人系統(tǒng)[1],通過建立機器人及其工作環(huán)境的虛擬交互模型,實時地將視覺、力覺、觸覺等信息反饋給本地操作者,使操作者產(chǎn)生身臨其境的感覺,從而有效地控制機器人完成復(fù)雜的作業(yè)任務(wù),不僅提高了遙操作安全性,節(jié)約成本,消除時延對操作者的影響[2],還提高了系統(tǒng)運行的穩(wěn)定性和透明性,極大地改善了遙操作機器人的作業(yè)效率和精細程度,廣泛應(yīng)用于空間服務(wù)、深海探測、核環(huán)境、遠程醫(yī)療等領(lǐng)域。

碰撞檢測是計算機動畫、系統(tǒng)仿真、計算機圖形學(xué)、機器人學(xué)、CAD/CAM等研究領(lǐng)域的經(jīng)典問題,吸引了眾多學(xué)者參與研究,出現(xiàn)了很多成熟的算法。碰撞檢測算法按空間域角度可分為基于實體空間的碰撞檢測方法和基于圖像空間的碰撞檢測方法,其中基于實體空間的算法又包括空間剖分法、層次包圍盒法、基于單純形的GJK及其改進算法、基于距離場的算法和基于智能優(yōu)化技術(shù)算法等[3]。空間劃分法通常適用于環(huán)境稀疏分布均勻的場景,層次包圍體技術(shù)是最為常用的碰撞檢測方法,適合于復(fù)雜物體間的快速碰撞檢測,典型的包圍盒包括包圍球sphere、軸對齊包圍盒AABB、方向包圍盒OBB,每種包圍盒都有自己的優(yōu)缺點,適用于不同幾何特征的物體[4]。

隨著并行機的高速發(fā)展及多核處理器的普及,并行技術(shù)廣泛應(yīng)用于碰撞檢測中。Tang[5]、Thomaszewski[6]、薛廣濤[7]、趙偉[8]、杜鵬[9]等人采用并行處理技術(shù)來加速測試計算,對提高算法效率取得很好的效果。

結(jié)合不同包圍盒的優(yōu)點及并行處理快速計算的特點,本文采用混合層次包圍體技術(shù)和多線程并行技術(shù)來提高檢測質(zhì)量和效率,并實時將干涉的圖元渲染出來,給操作者以直觀的感受。

1 遙操作機器人系統(tǒng)框架

基于虛擬現(xiàn)實的遙操作機器人系統(tǒng)主要包括主端、從端和網(wǎng)絡(luò)通信三個部分:主端為操作端,主要負責(zé)操作者與虛擬場景及外設(shè)的交互操作,及控制指令的發(fā)送和反饋信息的接收;從端是機器人所在的遠端,接收主端的指令完成相關(guān)任務(wù),并把傳感器和視覺等信息發(fā)送給主端,供操作者參考和交互;網(wǎng)絡(luò)通信部分主要負責(zé)主從端之間的信息交互。基于虛擬現(xiàn)實的遙操作機器人系統(tǒng)框架如圖1所示。

圖1 基于虛擬現(xiàn)實的遙操作機器人系統(tǒng)框架

在基于虛擬現(xiàn)實的遙操作機器人系統(tǒng)中,碰撞檢測是機器人路徑規(guī)劃、機器人完成抓取和安裝等任務(wù)計算虛擬力及判斷預(yù)測仿真結(jié)果的可取性等方面的依據(jù)。同時,檢測速度至少要達到300 FPS,以維持觸覺交互的穩(wěn)定性,要達到1 000 FPS才能獲得較好的效果[10]。快速精確的碰撞檢測成為保證遙操作安全性、增強虛擬環(huán)境的真實感和用戶的沉浸感的重要因素。

2 基于混合層次包圍的碰撞檢測

2.1 構(gòu)建混合層次包圍盒

包圍盒的核心思想是優(yōu)先使用代價低廉的測試類型,盡量使測試提早退出。包圍球由于其較少的內(nèi)存占有量、不受旋轉(zhuǎn)變換的影響、快速的測試及相對較差的緊密性而被廣泛應(yīng)用在首次的粗略剔除判斷中,而且在頻繁運動且又具有關(guān)聯(lián)關(guān)系的仿真場景中優(yōu)于空間剖分法。AABB是應(yīng)用最為廣泛的包圍體之一,能夠?qū)崿F(xiàn)快速的相交測試。為了彌補包圍球和AABB緊密性較差的缺點,最后采用適合不斷運動的桿狀結(jié)構(gòu)的OBB包圍盒進行剔除測試。因此,采用包圍球、AABB和OBB樹構(gòu)成物體的混合層次包圍盒。

在預(yù)處理階段,采用自頂向下的方式建立包含包圍球、AABB、OBB樹的混合層次包圍盒,每個物體的混合層次包圍盒結(jié)構(gòu)如圖2所示。

圖2 混合層次包圍盒結(jié)構(gòu)圖

a)構(gòu)建AABB包圍盒

AABB是包含幾何對象且各邊平行于坐標(biāo)軸的最小六面體,可以用各坐標(biāo)軸上的最小值和最大值來表示。首先遍歷模型所有頂點,并向各個坐標(biāo)軸投影獲取沿各個坐標(biāo)軸的最大值和最小值,依此來構(gòu)成包圍盒的最大值點和最小值點。

b)構(gòu)建包圍球

一個物體的包圍球包含球心坐標(biāo)和半徑兩個幾何元素,可以通過物體的AABB來獲取一個逼近的包圍球:AABB的中心作為球心,AABB的最大值點和最小值點的距離的一半作為半徑。

c)構(gòu)建OBB層次包圍盒

一個對象的OBB是包含對象且相對于坐標(biāo)軸方向任意的最小的長方體,關(guān)鍵在找出最佳方向。采用Gottschalk在文獻[11]中提出的計算三角形面片的方向包圍盒的方法,并用自頂向下遞歸的方式構(gòu)建樹型結(jié)構(gòu)。

在包含n個三角形的模型中,第k個三角形的頂點為(pk,qk,rk),面積為 ak,質(zhì)心為 mk,模型三角面片的總面積和加權(quán)質(zhì)心的均值分別為aH和mH,Cij為3×3協(xié)方差矩陣C元素:

協(xié)方差矩陣C是對稱矩陣,其三個特征向量是相互正交的,把三個特征向量單位化后作為OBB局部坐標(biāo)的三個軸向,把所有頂點向這三個軸投影,可以獲得OBB的大小和中心坐標(biāo)。

獲得了模型OBB層次包圍盒最上層的OBB后,用垂直父OBB最長軸且過所有頂點的均值點的平面分割父盒體,根據(jù)圖元在分割面的位置,將圖元劃分到相應(yīng)的子集中,如果圖元跨越分割面,則將其歸于質(zhì)心坐標(biāo)所處的子集中。如果最長軸上無法生成兩個非半空子集,按照長度序列方式嘗試其他軸,若全部軸都失效,那么該盒體是不能分割的。依此遞歸分割,直到不能分割為止。

2.2 運動物體的快速相交測試

在執(zhí)行任務(wù)的過程中,機器人及其他部分物體的位姿和位置不斷發(fā)生變化,因此需要進行混合層次包圍盒的更新。在模型平移運動時,混合層次包圍盒中各包圍盒只需要加上平移向量即可,不需要重新構(gòu)建。由于包圍球和OBB是不受特定方向限制的非對齊包圍體,而AABB是依賴于其既定的方向的對齊包圍體,所以模型在旋轉(zhuǎn)運動時:包圍球沒有方向性不需要重新構(gòu)建;OBB的方向會發(fā)生相應(yīng)的旋轉(zhuǎn)變化,但只需要在基底上乘以旋轉(zhuǎn)矩陣即可,同樣不需要構(gòu)建;至于AABB,鑒于其他測試方法的計算代價較大,這里還是按照上節(jié)方法重新構(gòu)建。

在相交測試過程中按同步規(guī)則進行層次結(jié)構(gòu)的下降,即依次按照 Sphere－Sphere,AABB－ AABB,OBBTree－OBBTree,三角形圖元的順序進行測試,若前者沒有干涉就返回未干涉結(jié)果,提前完成兩個物體之間的測試:球體之間的測試最為簡便,只需比較兩個球心之間的距離與兩球體半徑和的大小,為了避免計算成本較高的平方根操作,用距離的平方值進行相關(guān)運算;通過判斷兩個AABB在三個世界坐標(biāo)軸上是否都存在交集,才能確定兩盒體是否相交;OBBTree之間的測試采取深度優(yōu)先的層次結(jié)構(gòu)遍歷策略[11]用分離軸理論進行OBB間的相交測試;三角形圖元間的相交測試采用Held在文獻[12]提出的穿越算法。

隨著多核處理器的出現(xiàn),計算機的計算性能得到較高提升,現(xiàn)代CPU支持單指令多數(shù)據(jù)流(single instruction multiple data,SIMD)的處理方式。為了加快多物體場景中的模型和混合包圍盒樹型結(jié)構(gòu)遍歷過程,本文將需要處理的不同層次的干涉測試進行任務(wù)劃分:混合包圍盒構(gòu)建、檢測序列遍歷和包圍球測試屬于同一線程;而AABB測試、OBB Tree測試、面片測試與干涉面片的繪制分別用不同的線程來并行處理。如果上層干涉,就將干涉的物體對加入到下層測試線程中。

2.3 算法流程

正如上文所述,包圍球和OBB層次包圍盒只需要構(gòu)建一次,而AABB需要在檢測時進行不斷構(gòu)建,因此在整個場景只在進行首次測試時構(gòu)建混合層次包圍盒,否則只更新AABB。當(dāng)需要檢測的模型對序列遍歷結(jié)束時完成一次整體的檢測流程。如果序列沒有遍歷結(jié)束,首先進行最外層的包圍球檢測,若球檢測未干涉,返回為未干涉結(jié)果,若干涉,就把待檢測的模型對添加到AABB檢測線程處理的檢測序列中。依此類推,若上層未干涉就返回檢測結(jié)果,上層檢測干涉就將待檢測的元素添加到下層檢測線程處理序列中。如果三角形檢測干涉的話,就將干涉的三角形對繪制出來,并返回干涉結(jié)果。整體算法流程如圖3所示。

圖3 碰撞檢測流程圖

3 仿真實驗

利用VC＋＋6.0作為開發(fā)工具,使用面向?qū)ο蠛徒换ナ降娜S圖形開發(fā)軟件包Open Inventor5.0建立仿真界面,計算機系統(tǒng)配置為:操作系統(tǒng)Windows XP 32位;處理器Intel Core i5 CPU 750 2.67 GHz 4核;顯卡NVIDIA Ge-Force GT 220 1GB;內(nèi)存 4 GB。

遙操作機器人虛擬現(xiàn)實仿真平臺如圖4所示,六自由度機器人用末端工具將物體1移動到位置1處,并將物體2移動到物體1原來所在的位置2處。采用CAD軟件Pro/E建立幾何模型,并將弦高和角度值都設(shè)置為0.5,將CAD幾何模型轉(zhuǎn)換成包含幾何信息的IV格式數(shù)據(jù)文件,仿真環(huán)境共包含10個模型對象,22 112個三角面片,模型IV文件大小為6.7 MB。

圖4 實驗仿真平臺(未檢測)

由于機器人自身的裝配特性,在實驗中并不檢測相鄰連桿間的干涉情況,除此之外,要遍歷場景中的所有物體對進行檢測,并將相互干涉的三角面片對實時繪制出來。發(fā)生干涉時的場景圖5所示,存在干涉的三角形用細紅線繪制。檢測時間信息統(tǒng)計如表1所示,其中時間是完成一次完整場景遍歷進行干涉檢測所需要時間。

由表1可知,采用的多線程并行方法比串行計算效率提高13%~20%左右,而且當(dāng)場景中存在的干涉三角面片對數(shù)小于1 000時,幀速率能達到30 FPS以上。

圖5 發(fā)生干涉時的場景

表1 仿真實驗干涉信息統(tǒng)計

4 結(jié)語

為了提高基于虛擬現(xiàn)實的遙操作機器人系統(tǒng)中碰撞檢測的實時性和準(zhǔn)確性,提出基于混合層次包圍盒和多線程的快速測試方法:采用包圍球、軸對齊包圍盒和方向包圍盒樹構(gòu)建模型的混合層次包圍盒,快速逐步排除不相交的物體或物體的一部分,并用三角形間的測試進行精確檢測;同時,在場景的遍歷、層次結(jié)構(gòu)的下降及繪制干涉三角形對中采用多線程技術(shù)加速計算過程。實驗結(jié)果表明該算法在保證檢測實時性方面有較好的表現(xiàn)。

[1]宋愛國.力覺臨場感遙操作機器人技術(shù)研究進展[J].機械制造與自動化,2012(01).

[2]Woo Y.K.,Toshihlko G.,Kawabe H.Model－Based Space Robot Teleoperation of ETS-VII Manipulator[J].IEEE Trans.on Rob.and Auto.,Vol20(3),2004:602-612.

[3]熊玉梅.虛擬環(huán)境中物體碰撞檢測技術(shù)的研究[D].上海:上海大學(xué),2011.

[4]鄒益勝,丁國富,許明恒,等.實時碰撞檢測算法綜述[J].計算機應(yīng)用研究,2008(01).

[5]Min Tang, DineshManochab,Ruofeng Tong.MCCD:Multi-core collision detection between deformable models using front-based decomposition[J].Graphical Models,2010(03).

[6]Bernhard Thomaszewski,WolfgangBlochinger.Physically based simulation of cloth on distributed memory architectures[J].Parallel Computing,2007(06).

[7]薛廣濤,李超,尤晉元.基于凸多面體剖分的并行碰撞檢測算法[J].上海交通大學(xué)學(xué)報,2004(08).

[8]趙偉,何艷爽.一種快速的基于并行的碰撞檢測算法[J].吉林大學(xué)學(xué)報(工學(xué)版),2008(01).

[9]杜鵬,唐敏,童若鋒.多核加速的并行碰撞檢測[J].計算機輔助設(shè)計與圖形學(xué)學(xué)報,2011(05).

[10]金漢均.虛擬環(huán)境中物體碰撞檢測算法研究[D].武漢:華中科技大學(xué),2006.

[11]Stefan Gottschalk.Collision Queries Using Oriented Bounding Boxes[D].Department of Computer Science,University of North Carolina, Chapel Hill, 2000.

[12]Martin Held.ERIT-A Collection of Efficient and Reliable Intersection Tests[J].Journal of Graphics Tools, Volume 2, Issue 4, 1997.