三維場景中實時路徑規(guī)劃優(yōu)化算法

鄭紅波,左少華,程燕飛,秦緒佳,張美玉,徐曉剛

1(浙江工業(yè)大學 計算機科學與技術(shù)學院,杭州 310023) 2(海軍大連艦艇學院 航海系,遼寧 大連 116018)

1 引 言

在三維空間場景中,運動物體需要隨時了解自己所處的位置,掌握快速到達目的地的路徑.由于運動物體都具有一定的體積和寬度,因此,保證所選擇道路足夠?qū)掗?能夠順利通過是一個重要的約束條件.在運動過程中,諸如房屋、樹木等景物是無法穿透的,這是選擇運動路線中的約束條件之一,需要進行碰撞檢測.從上可以看出,在三維復雜場景中,往往因為復雜的環(huán)境和視點范圍有限,無法識別出運動物體所在的位置,目的地位置也同樣很難被快速辨認出,因此需要預先進行路徑規(guī)劃[1],而在此過程中,需要通過碰撞檢測來獲得可行的運動路徑.

在碰撞檢測方面已經(jīng)有一些不錯的工作.Jiménez、Segura提出了基于四面體的復雜多邊形的碰撞檢測算法[2],使得兩個待檢測的物體特征之間的相交測試數(shù)被大幅度地降低了,但與此同時,四面體的構(gòu)建、包圍體的分層和區(qū)域的劃分過程較為復雜,并且多個物體間的重復區(qū)域未被排除導致重復多次檢測;Chang、Kim 提出的面向包圍盒(OBB)的檢測算法[3]雖然能夠一定程度上減少檢測處理時間,但不易于碰撞點的坐標采集和碰撞的響應,是因為采用本地對象坐標而不是全局對象坐標來進行檢測;Schauer、Nüchter提出了基于高效k-d樹的碰撞檢測算法[4],主要通過kd-CD和簡單kd-CD檢測算法、kd-PD和快速kd-PD碰撞程度計算方法進行碰撞檢測,但該算法僅在數(shù)量點不多的情況下有較好的適應性.

在路徑規(guī)劃方面,前人也提出了多種不同的算法.Khalil和 Kazem 提出的機器人移動路徑規(guī)劃方法[5]通過求解多方向路徑可行性概率,并利用概率遞歸函數(shù)來獲得最佳路徑,消除了Bezier曲線的缺點;Bircher、Kamel和Alexis等人提出了一種基于自主測試檢查與覆蓋的機器人路徑規(guī)劃算法[6],由于該算法在迭代計算視點路徑開銷過程中,早已走過的路徑開銷會被重復計算,從而導致算法的計算量和路徑的規(guī)劃時間有所增長;王殿君提出了一種改進的針對室內(nèi)機器人移動路徑規(guī)劃的A*算法[7],該算法主要通過計算拐點、旋轉(zhuǎn)方向和旋轉(zhuǎn)最小角度來優(yōu)化算法,減少了冗余點的計算和提高了機器人拐點處的調(diào)整速度,但由于計算點的增加,側(cè)重于提高部分運作效率但影響了整體路徑規(guī)劃的效率和質(zhì)量.

在上述路徑規(guī)劃工作中,基本上著力于算法的優(yōu)化工作,沒有考慮運動目標本身物理特征和道路特征對規(guī)劃工作的影響,屬于比較理想的狀態(tài),而實際目標運動中體積、大小、道路寬闊程度等都會影響到路徑的選擇.根據(jù)上述需求,本文提出了一種路徑規(guī)劃優(yōu)化算法.算法在傳統(tǒng) A*算法的基礎(chǔ)上,采用粗試探和精搜索的策略進行路徑規(guī)劃,用預碰撞篩選檢測和精細碰撞檢測的策略進行層次包圍盒碰撞檢測,規(guī)避視點小半徑球根本不會與之碰撞的景物,使算法更接近于工程應用.

2 路徑規(guī)劃優(yōu)化算法

本文提出的路徑規(guī)劃優(yōu)化算法,主要從碰撞檢測算法和A*算法兩部分進行分析與研究.

2.1 碰撞檢測算法

2.1.1 碰撞檢測基本原理

碰撞檢測[8-10]是指檢測兩個移動物體或者移動物體與三角平面之間有沒有產(chǎn)生碰撞,即檢測移動物體視點軌跡是否相交于三角平面.具體檢測步驟如下:

1)計算檢測平面S和平面S法線方向的最近距離.

2)判斷前一幀視點位置OldPosition以及后一幀視點位置NewPosition和檢測平面S之間的關(guān)系:若OldPosition和NewPosition同時分布在檢測平面S的同一邊,則跳轉(zhuǎn)到步驟5);若如圖1所示,則跳轉(zhuǎn)到步驟3).

3)判斷視點是否與平面相交.過平面S的三條邊分別作平面S的垂面PS1、PS2和PS3,如圖2所示.

圖1 點與面的關(guān)系圖2 碰撞檢測處理圖1Fig.1 Relationship Fig.2 Collision detectionbetween points and planeprocessing diagram 1

將平面PS1、PS2和PS3分別沿著各自法線方向向外移動距離L,以此來擴大防碰撞范圍,提高碰撞檢測的敏感度,如圖3所示.

將平面PS1、PS2和PS3分別沿著各自法線方向向?qū)斀瞧揭?得到平面PS4、PS5和PS6,來避免由于三角形向外擴張而導致檢測區(qū)域過大,如圖4所示.

此時若視點處在由平面PS1、PS2、PS3、PS4、PS5和PS6所圍成的區(qū)域內(nèi),轉(zhuǎn)至步驟4),不在則轉(zhuǎn)至步驟5).

4)視點與三角面片發(fā)生碰撞,改變視點沿著平行于三角平面S的方向運動并將NewPosition更正為平行方向的位置.

5)視點沒有與三角平面S發(fā)生碰撞,可沿當前方向繼續(xù)運動.

平面PS6平面PS5平面PS4區(qū)域過大圖3 碰撞檢測處理圖2圖4 碰撞檢測處理圖3Fig.3 Collision detection Fig.4 Collision detection processing diagram 2processing diagram 3

2.1.2 碰撞檢測算法

本文碰撞檢測算法分為預碰撞篩選檢測和精細碰撞檢測,詳細流程如下:

1)預碰撞篩選檢測

預碰撞篩選檢測是指篩選碰撞檢查單元.先將三維場景進行八等分,劃分為八個相同的單元,劃分的結(jié)果就是場景中的物體可能處在一個或幾個單元中,如圖5所示;然后通過結(jié)點的方式,將單元組成八叉樹模型;再篩選出與視點小半徑球在同一最小子單元中的物體,如圖6所示,物體B與視點小半徑球A處在同一單元中,而物體C則不是.

ABC圖5 八叉樹模型示意圖圖6 預碰撞篩選檢測Fig.5 Octree model Fig.6 Pre-collision screeningdiagramdetection

制定一定的控制規(guī)則并且選取適當?shù)膭澐殖叽鐏肀苊饪臻g占用浪費和計算效率的減低:

a.規(guī)定結(jié)點數(shù)量最大值Nmax和三角面片數(shù)量最小值Fmin.

圖7 球心到平面的距離Fig.7 Distance form the center of the sphere to the plane

b.進一步細分單元結(jié)點直到該單元結(jié)點內(nèi)三角面片數(shù)小于Fmin.

c.每次劃分后統(tǒng)計單元結(jié)點數(shù),若大于Nmax,則停止劃分.

2)精細碰撞檢測

通過預篩選去除不在同一單元的三角面片,然后判斷處在同一單元中的視點小半徑球與所有三角面片所在平面是否相交:若二者相交且存在處在三角面片上的交點,則發(fā)生碰撞.具體算法如下:

1)由公式求解出視點小半徑球球心到三角面片所在平面的距離.如圖7所示,其中VC(XC,YC,ZC)為球心坐標,R為半徑,平面L0L1L2為三角面片所在平面,L0(x0.y0,z0)、L1(x1.y1,z1)和L2(x2.y2,z2)是三角面片三點的空間坐標,N為L0L1L2的法向量.

(1)

(2)

(3)

(4)

由公式(1)、(2)、(3)和(4),球心到三角面片所在平面的距離d可以被表示為:

(5)

2)判斷距離d與球體半徑R的大小關(guān)系:若d 小于R則代表視點小半徑球相交于三角平面,計算出交點坐標;若d大于等于R,則代表視點小半徑球和三角面片相離,碰撞不會發(fā)生.

yxVdNViO圖8 球體與平面相交圖9 交點與三角面片的關(guān)系Fig.8 Intersect of the sphereFig.9 Relationship between and the planeintersection and triangle

3)通過公式(6)求出交點坐標向量.如圖8所示,球與平面相交于點Vi:

Vi=VC-dN

(6)

4)如果Vi位于三角面片內(nèi),則代表視點小半徑球與三角面片發(fā)生碰幢,否則沒有發(fā)生碰撞.具體判斷方法如圖9所示,連接Vi和三角面片的三個頂點ABC,再利用公式(7)、(8)和(9)計算連線夾角的度數(shù),若夾角度數(shù)之和等于360度,則表示Vi位于三角面片內(nèi),否則Vi位于三角面片外部或者三邊上.

(7)

(8)

(9)

2.2 路徑規(guī)劃優(yōu)化算法

2.2.1 A*算法

路徑規(guī)劃算法中常用的是啟發(fā)式搜索[11-13],是指利用求解問題的啟發(fā)信息,指導搜索向最接近最優(yōu)結(jié)果的方向進行.A*算法是一種使用評價函數(shù)計算拓展點代價估值并進行排序的有序搜索算法,公式(10)為評價函數(shù).

f(c)=g(c)+h(c)

(10)

其中c為當前結(jié)點,函數(shù)f(c)為從初始點Str開始經(jīng)由結(jié)點c而到達目的點Fin的代價估計值,函數(shù)g(c)為從初始點Str開始到結(jié)點c的實際代價值,而函數(shù)h(c)則為從當前結(jié)點c到達目的點Fin的最佳路徑的代價估計值.

A*算法[14,15]具體步驟如下:

1)定義待選列表L1和已選列表L2.表L1保存代價評估值已知但領(lǐng)域拓展點的代價估計值未知的結(jié)點;表L2保存表L1中且對領(lǐng)域拓展點進行過代價評估的結(jié)點.

2)將c從表L1中移除并放入表L2中,然后通過碰撞檢測找出結(jié)點c的所有無碰撞領(lǐng)域拓展點N,再將結(jié)點c記為N的父結(jié)點.

3)由公式(10)計算出N的代價估值,并將N放入表L1中.

4)若當前結(jié)點c為目的點Fin或者表L1為空,則算法結(jié)束.否則重新選取L1中最小代價估值的結(jié)點為結(jié)點c,重復步驟2)、3)和4).

實驗結(jié)果表明,A*算法可以搜索出從Str到Fin的最優(yōu)路徑,并且函數(shù)g(c)、h(c)和f(c)遵從一致性原則,基于以上優(yōu)點,A*算法可以被用來進行路徑規(guī)劃.

但是A*算法本身也具有一些明顯的不足,A*算法的關(guān)鍵是設(shè)計一個合適的估價函數(shù)h(c),最好的情況當然是估價函數(shù)h(c)等于結(jié)點c到目的點Fin的實際代價值,在這種情況下保證可以得到最優(yōu)解的同時搜索效率較高,而如果h(c)過度小于實際代價值會導致搜索結(jié)點過多,搜索效率低下.A*算法在路徑規(guī)劃過程中,一些潛在最優(yōu)結(jié)點在較早搜索階段被刪除了,導致傳統(tǒng)A*算法解決路徑規(guī)劃問題時,有時候規(guī)劃出來的路徑是一個較優(yōu)的可行解但不是全局最優(yōu)路徑.并且傳統(tǒng)A*算法的擴展鄰域結(jié)點的優(yōu)先級是一樣的,導致在路徑規(guī)劃過程中碰撞檢測的處理時間較多,時間效率隨著碰撞檢測處理的三角面片數(shù)量增加而明顯下降.

2.2.2 本文算法

本文算法在原始A*算法的基礎(chǔ)上,采用粗試探和精搜索的策略進行路徑規(guī)劃,其中粗試探和精搜索采用本文優(yōu)化的碰撞檢測算法進行碰撞檢測.具體步驟如下:

1)粗試探.當前結(jié)點c作為中心,除去結(jié)點c的父結(jié)點方向和父結(jié)點反方向,在四周三維度上的六個方向中剩下的四個方向上以包圍盒的形式進行碰撞檢測,試探與場景中景物有沒有產(chǎn)生碰撞.例如對于方向X,構(gòu)建一個緊靠當前結(jié)點c所在包圍盒的可無限擴展的包圍盒T并進行碰撞檢測,如果包圍盒T沒有與場景中物體對象發(fā)生碰撞,則說明當前結(jié)點c所在包圍盒可以在包圍盒T內(nèi)自由移動且不會發(fā)生碰撞.隨著c所在包圍盒的移動,包圍盒T也會隨之移動,并一直進行碰撞檢測.

2)精搜索.如果粗試探失敗,先除去c的父結(jié)點方向,在c的四周三維度上剩下的五個方向依次進行碰撞檢測,記未碰撞鄰域為l.之后計算出未碰撞領(lǐng)域l的范圍最大距離d,并比較d與視點的范圍最大距離的大小關(guān)系,若d小于視點最大距離則計算l的代價估值并按降序的方式進行排序,然后添加到表L1中(代價估值越小在表L1中的位置越靠近前,代價估值越大在表L1中的位置越靠近后);否則將該碰撞領(lǐng)域l去除,重新進行精搜索.

3)從表L1中循環(huán)選取結(jié)點,就可以找到代價評估值越小的點.

本文優(yōu)化改進的算法的偽代碼如下:

while(待選列表L1不為空){

從待選列表L1前端中選取結(jié)點c;

if(c不在包圍盒內(nèi))break;

while(true){

在已選列表L2中添加當前結(jié)點c;

對當前結(jié)點c進行粗試探;

if(粗試探成功){

經(jīng)過當前結(jié)點c的路徑已經(jīng)遍歷結(jié)束,跳轉(zhuǎn)到下一結(jié)點循環(huán);

}

else{

if(當前結(jié)點c的鄰域為空) break;

else{

對當前結(jié)點c進行精搜索;

從待選列表L1前端中選取結(jié)點c;

}

}

}

}

3 實驗結(jié)果與分析

3.1 碰撞檢測算法

分別使用文獻[16]的AABB樹方法、文獻[17]的OBB樹方法以及本文碰撞檢測方法,在同一種三維空間場景和同一種運動路徑中分別進行多次視點與三維碰撞物體的碰撞試驗,三種方法的三角形測試數(shù)、面測試數(shù)、真正涉及的三角形數(shù)以及總時間的均值如表1所示.圖10為碰撞實驗示意圖,圖10(a)為三維球體碰撞渲染圖,圖10(b)為三維球體碰撞網(wǎng)格圖.

圖10 碰撞示意圖Fig.10 Collision diagram

由表1可知,與文獻[16]的AABB樹碰撞檢測算法相比,本文優(yōu)化的碰撞檢測算法的三角形測試數(shù)和真正涉及的三角形數(shù)有所減少;與文獻[17]的OBB樹碰撞檢測算法相比,三角形測試數(shù)和面測試數(shù)也有所減少,因而本文優(yōu)化的碰撞檢測算法的總時間小于其他兩種算法,能夠提高一定的碰撞檢測效率.

3.2 本文路徑規(guī)劃優(yōu)化算法

運用傳統(tǒng)A*算法和本文優(yōu)化改進的A*算法對同一模型的五條路徑進行實驗仿真,表2為兩種算法的處理時間對比結(jié)果.如圖11(a)所示,分別用數(shù)字1到5表示五條路徑,其中有數(shù)字一端的為終點，沒有數(shù)字的一端為起點.

表1 三種算法的測試數(shù)和總時間對比
Table 1 Comparison of the testing number and the total time between three algorithms

AABB樹算法OBB樹算法本文優(yōu)化的碰撞檢測算法三角形測試數(shù)168512141102面測試數(shù)590680590真正涉及的三角形數(shù)936584584總時間(s)0.0245080.0235930.020421

表2 兩種算法的處理時間對比
Table 2 Comparison of processing time between two algorithms

名稱路徑1路徑2路徑3路徑4路徑5三角面片數(shù)57529890106381323411354物體數(shù)25678傳統(tǒng)A?算法(s)0.04647410.1079230.1223560.09288930.167038本文優(yōu)化改進的算法(s)0.04018740.08683640.1126950.07564820.125513

(a)五種路徑規(guī)劃圖(b)本文路徑規(guī)劃效果對比圖12.011.511.010.510.09.5路徑6路徑7路徑8代價評估值圖11 路徑規(guī)劃圖圖12 路徑代價評估值柱形圖Fig.11 Path planning Fig.12 Column chart of pathdiagram cost evaluation

在圖11(b)中,起點為汽車所在位置,終點為球體所在位置,汽車從起點開始前往終點運送貨物,從圖11中可以看出有三條運動路徑,分別為路徑6、路徑7和路徑8,圖12為它們的代價評估值,路徑6的代價評估值最小,路徑8的代價評估值最大.其中采用傳統(tǒng)A*算法規(guī)劃結(jié)果為路徑6,而本文算法規(guī)劃結(jié)果為路徑7.相對于路徑8而言,傳統(tǒng)A*算法和本文算法都能規(guī)劃出代價評估值較小的路徑.在不考慮實際應用情況下,最優(yōu)路徑選擇確實是路徑6,但因為卡車的體積無法通過兩個柱狀物之間的路徑6,因此,排除掉不可能的路徑6后,本文算法相對于路徑8選擇了代價評估值較小的路徑7,更加符合實際應用情況.由實驗結(jié)果可知,在路徑規(guī)劃的處理時間上,本文優(yōu)化的路徑規(guī)劃算法比傳統(tǒng)A*有所減少,路徑的搜索效率得到提高,且對于無法通過的未碰撞區(qū)域進行了規(guī)避,擇優(yōu)選擇其他代價評估低的路徑,符合實際應用的要求,可以更好地應用于各種實際工程應用的路徑規(guī)劃問題.

4 結(jié) 論

本文在傳統(tǒng)A*算法的基礎(chǔ)上,針對三維場景的路徑規(guī)劃問題,提出了一種基于層次包圍盒碰撞檢測的路徑規(guī)劃優(yōu)化算法.優(yōu)化算法在原始A*算法的基礎(chǔ)上,采用粗試探和精搜索的策略進行路徑規(guī)劃,并通過對層次包圍盒碰撞檢測算法的基本原理的研究,采用預碰撞篩選檢測和精細碰撞檢測的策略進行碰撞檢測,實驗驗證表明,算法保持了很好的實時性,同時更接近工程應用的要求,可以規(guī)避無法通過的道路,選擇出具有參考價值的優(yōu)化路徑,在達到效果的前提下,提高了時間效率.進一步的工作包括:根據(jù)運動目標的形狀,建立更加合理包圍盒;改進碰撞檢測算法,使得檢測效率更加高效,進而獲得更加合理的優(yōu)化路徑.