面向遙操作機(jī)器人的混合層次包圍盒碰撞檢測(cè)

劉文聰,李作清,李世其

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

0 引言

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

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

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

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

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

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

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

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

2 基于混合層次包圍的碰撞檢測(cè)

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

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

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

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

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

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

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

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

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

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

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

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

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

2.2 運(yùn)動(dòng)物體的快速相交測(cè)試

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

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

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

2.3 算法流程

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

圖3 碰撞檢測(cè)流程圖

3 仿真實(shí)驗(yàn)

利用VC＋＋6.0作為開(kāi)發(fā)工具,使用面向?qū)ο蠛徒换ナ降娜S圖形開(kāi)發(fā)軟件包Open Inventor5.0建立仿真界面,計(jì)算機(jī)系統(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。

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

圖4 實(shí)驗(yàn)仿真平臺(tái)(未檢測(cè))

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

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

圖5 發(fā)生干涉時(shí)的場(chǎng)景

表1 仿真實(shí)驗(yàn)干涉信息統(tǒng)計(jì)

4 結(jié)語(yǔ)

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

[1]宋愛(ài)國(guó).力覺(jué)臨場(chǎng)感遙操作機(jī)器人技術(shù)研究進(jìn)展[J].機(jī)械制造與自動(dòng)化,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)境中物體碰撞檢測(cè)技術(shù)的研究[D].上海:上海大學(xué),2011.

[4]鄒益勝,丁國(guó)富,許明恒,等.實(shí)時(shí)碰撞檢測(cè)算法綜述[J].計(jì)算機(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]薛廣濤,李超,尤晉元.基于凸多面體剖分的并行碰撞檢測(cè)算法[J].上海交通大學(xué)學(xué)報(bào),2004(08).

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

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

[10]金漢均.虛擬環(huán)境中物體碰撞檢測(cè)算法研究[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.