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

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

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

1 引 言

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

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

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

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

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

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

2.1 碰撞檢測算法

2.1.1 碰撞檢測基本原理

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

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

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

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

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

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

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

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

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

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

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

2.1.2 碰撞檢測算法

本文碰撞檢測算法分為預(yù)碰撞篩選檢測和精細(xì)碰撞檢測,詳細(xì)流程如下:

1)預(yù)碰撞篩選檢測

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

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

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

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

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

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

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

2)精細(xì)碰撞檢測

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

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

(1)

(2)

(3)

(4)

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

(5)

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

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

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

Vi=VC-dN

(6)

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

(7)

(8)

(9)

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

2.2.1 A*算法

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

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

(10)

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

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

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

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

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

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

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

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

2.2.2 本文算法

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

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

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

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

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

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

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

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

while(true){

在已選列表L2中添加當(dāng)前結(jié)點(diǎn)c;

對(duì)當(dāng)前結(jié)點(diǎn)c進(jìn)行粗試探;

if(粗試探成功){

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

}

else{

if(當(dāng)前結(jié)點(diǎn)c的鄰域?yàn)榭? break;

else{

對(duì)當(dāng)前結(jié)點(diǎn)c進(jìn)行精搜索;

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

}

}

}

}

3 實(shí)驗(yàn)結(jié)果與分析

3.1 碰撞檢測算法

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

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

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

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

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

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

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

表2 兩種算法的處理時(shí)間對(duì)比
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)化改進(jìn)的算法(s)0.04018740.08683640.1126950.07564820.125513

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

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

4 結(jié) 論

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