基于Dubins路徑的A*算法的多無(wú)人機(jī)路徑規(guī)劃

宋雪倩， 胡士強(qiáng)

(上海交通大學(xué)，上海 200240)

0 引言

無(wú)人機(jī)(Unmanned Aerial Vehicle,UAV)已經(jīng)被廣泛運(yùn)用于民用和軍用領(lǐng)域。為了達(dá)到真正的“無(wú)人”和“智能”，在環(huán)境已知并給定起始位置和航向角之后，無(wú)人機(jī)應(yīng)能夠自主規(guī)劃一條滿足任務(wù)需求的避障路徑，通常要求這條路徑最短；而當(dāng)空中有兩架無(wú)人機(jī)在同一高度執(zhí)行任務(wù)時(shí)，必須考慮防撞，同時(shí)還要滿足飛行性能約束。對(duì)于固定翼無(wú)人機(jī)，它們不能像旋翼機(jī)一樣懸停，必須保持最小速度，且轉(zhuǎn)彎受到最小轉(zhuǎn)彎半徑的限制。

A*算法[1]是路徑規(guī)劃中最著名的離散搜索算法之一。通常先將二維平面進(jìn)行柵格化，再使用A*算法或改進(jìn)A*算法進(jìn)行最短路徑搜索[2-3]。這在規(guī)劃時(shí)沒(méi)有考慮飛行器的始末航向要求，得到的路徑不能滿足飛行性能約束且會(huì)對(duì)一些不必要節(jié)點(diǎn)進(jìn)行搜索。文獻(xiàn)[4-5]都使用了A*算法進(jìn)行移動(dòng)機(jī)器人的路徑規(guī)劃，都是在規(guī)劃完成之后，對(duì)路徑進(jìn)行光滑處理，其中涉及較大計(jì)算量且無(wú)法保證光滑之后的路徑依舊安全；文獻(xiàn)[6]考慮了飛行性能限制，利用可視圖法與 Dijkstra算法，但仍需要較長(zhǎng)計(jì)算時(shí)間；而Dubins路徑[7]是給定始末方向以及在轉(zhuǎn)彎半徑有約束的情況下、兩點(diǎn)之間的最短曲線，被廣泛用于自主機(jī)器人的路徑規(guī)劃當(dāng)中[8-10]。本文提出的基于Dubins路徑的A*算法的路徑規(guī)劃方法，考慮了固定翼無(wú)人機(jī)的最小轉(zhuǎn)彎半徑，利用Dubins路徑進(jìn)行可飛路徑建模，以Dubins曲線與障礙圓切入點(diǎn)，即無(wú)人機(jī)的位姿為節(jié)點(diǎn)，邊搜索下一步的有效節(jié)點(diǎn)，邊利用A*算法進(jìn)行尋優(yōu)，由于剔除了不必要節(jié)點(diǎn)，提升了搜索效率。在兩架固定翼無(wú)人機(jī)處于同一高度飛行的情況下，利用兩機(jī)之間脫靶距離大小判斷沖突，結(jié)合向量共享法(Vector Sharing Resolution)[11]，在線調(diào)整航向并進(jìn)行路徑重規(guī)劃，可以得到兩者的安全航跡。

1 Dubins路徑

1.1 Dubins路徑

Dubins路徑定義為：在最大曲率限制下，平面內(nèi)兩個(gè)有方向的點(diǎn)間的最短可行路徑是CLC路徑或CCC路徑，或是其子集，其中，C表示圓弧段，L表示與C相切的直線段[9]。這里只考慮CLC型路徑，如圖1所示。

圖1 一種CLC型Dubins路徑Fig.1 Dubins path of the CLC type

1.2 CLC型Dubins路徑存在的充分條件

如果兩個(gè)位姿分別對(duì)應(yīng)的兩個(gè)Dubins圓之間存在內(nèi)外切線，即

|rs+rf|>|c|

(1)

則CLC型Dubins路徑一定存在。式中:rs為起始圓弧半徑;rf為終止圓弧半徑。當(dāng)滿足這個(gè)條件時(shí)，則稱兩個(gè)位姿之間在給定起始和終止圓弧半徑情況下，最短CLC型Dubins路徑為Dubins路徑。

1.3 Dubins路徑的長(zhǎng)度

L(qIqF)=Rα+S+Rγ

>。

(2)

1.4 基于Dubins路徑的避障

當(dāng)路徑不安全時(shí)，無(wú)人機(jī)有2種避障方法：順時(shí)針(CW)繞過(guò)障礙圓(如圖2綠線所示)或逆時(shí)針(CCW)繞過(guò)障礙圓(如圖2藍(lán)線所示)。

圖2 基于Dubins路徑的兩種避障方法Fig.2 Two possible Dubins paths to avoid an obstacle

2 基于Dubins路徑的A*算法的路徑規(guī)劃

具體規(guī)劃流程如圖3所示。

圖3 基于Dubins路徑的A*算法的路徑規(guī)劃流程圖Fig.3 The flow chart of planning a path by Dubins-path based A* algorithm

假設(shè)環(huán)境已知，所有障礙物分散且可以用圓來(lái)建模。當(dāng)所有障礙圓之間均滿足CLC型Dubins路徑存在的充分條件情況下，即可用本文方法進(jìn)行路徑規(guī)劃。路徑規(guī)劃中加入了A*算法的啟發(fā)式特點(diǎn)。

(3)

3 多無(wú)人機(jī)防撞方法

假設(shè)兩架固定翼無(wú)人機(jī)在同一高度飛行，且實(shí)時(shí)位姿、速度等狀態(tài)都相互可知。首先可對(duì)兩者分別用基于Dubins的A*算法進(jìn)行路徑規(guī)劃，在按照此路徑飛行過(guò)程中進(jìn)行相撞判斷并采取改變航向角的措施從而防止相撞，確認(rèn)不會(huì)相撞之后，再利用第2章所述算法分別對(duì)兩者進(jìn)行航跡重規(guī)劃。

3.1 相撞檢測(cè)

最接近時(shí)間為

(4)

如圖4可得，兩架無(wú)人機(jī)之間脫靶距離為

(5)

圖4 兩架無(wú)人機(jī)的相對(duì)運(yùn)動(dòng)狀態(tài)Fig.4 The relative motion between 2 UAVs

3.2 防撞措施

(6)

對(duì)無(wú)人機(jī)A有

(7)

對(duì)無(wú)人機(jī)B有

(8)

設(shè)無(wú)人機(jī)將以最小轉(zhuǎn)彎半徑進(jìn)行速度方向改變，為了保證兩架無(wú)人機(jī)在改變方向時(shí)不發(fā)生碰撞，在速度改變時(shí)，要滿足

(9)

圖5 向量共享法防撞方法Fig.5 Collision avoidance by Vector Sharing Resolution

4 仿真驗(yàn)證

4.1 單無(wú)人機(jī)避障路徑規(guī)劃

區(qū)域中共有4個(gè)障礙物O1～O4，其位置和半徑分別表示為：Oobs1=(25>10>3),Oobs2=(25 23 4),Oobs3=(39>31>0.8),Oobs4=(41>40>3)。A的起始位姿qA,s=(20 m>0 m>60°)，目標(biāo)位姿qA, f=(45 m>0 m>60°)。

圖6中紅線為所得路徑，由4段Dubins路徑組成，長(zhǎng)度為67.9 m，且路徑光滑。在相同條件下，將地圖進(jìn)行離散，如圖7所示，藍(lán)線為通過(guò)A*算法規(guī)劃所得結(jié)果，路徑長(zhǎng)度為71.1 m，其長(zhǎng)度大于圖6中結(jié)果，且路徑不夠光滑，不能直接投入實(shí)際應(yīng)用。

圖6 基于Dubins的A* 算法的路徑規(guī)劃仿真結(jié)果Fig.6 The path planning simulation result for single UAV by Dubins-path based A* algorithm

圖7 基于網(wǎng)格地圖的A*算法路徑規(guī)劃Fig.7 Path planning by A* algorithm applied on a grid map

4.2 雙無(wú)人機(jī)避障防撞路徑規(guī)劃

區(qū)域中共有4個(gè)障礙物O1～O4，其位置和半徑分別表示為：Oobs1=(35>23>4.2),Oobs2=(35>10>3),Oobs3=(49>31>0.8),Oobs4=(35>45>3.2)。A的起始位姿qA,s=(30 m>0 m>60°)，目標(biāo)位姿qA,f=(55 m>60 m>60°);B的起始位姿qB,s=(0 m>0 m>30°) ,目標(biāo)位姿qB, f=(80 m>65 m>30°)，狀態(tài)采樣頻率為20 Hz。

圖8 基于Dubins的A* 算法的雙無(wú)人機(jī)路徑規(guī)劃仿真結(jié)果Fig.8 The simulation result of path planning for2 UAVs by Dubins-path based A* algorithm

圖9為兩架無(wú)人機(jī)之間距離的示意圖。其中，綠線為兩者最小安全距離,藍(lán)線為兩者按照各自所規(guī)劃的路徑飛行時(shí)的距離。如果按照原始路徑飛行，在5.5 s左右，兩者會(huì)碰撞。而如果采取了防撞規(guī)避，則兩者會(huì)在2.85 s采取規(guī)避措施，改變速度。兩架無(wú)人機(jī)之間最小距離為13.15 m，不會(huì)發(fā)生相撞。

圖9 兩架無(wú)人機(jī)之間距離Fig.9 The distance between 2 UAVs

5 結(jié)束語(yǔ)

本文提出了一種基于Dubins路徑的A*算法的多無(wú)人機(jī)航跡規(guī)劃方法。利用Dubins路徑、環(huán)境中的有效節(jié)點(diǎn)，再利用A*啟發(fā)式算法的思想進(jìn)行路點(diǎn)確定，有效提高了航跡規(guī)劃的效率，且考慮了固定翼無(wú)人機(jī)的飛行性能限制以及保證得到的路徑光滑；在兩架無(wú)人機(jī)同一高度飛行的情況下，結(jié)合向量共享方法，解得防撞避障所需偏航角度，調(diào)整航線，確保沒(méi)有相撞危險(xiǎn)后，再進(jìn)行重規(guī)劃。仿真表明，此方法所得路徑優(yōu)于傳統(tǒng)A*算法，且在雙無(wú)人機(jī)同一高度飛行時(shí)可以得到安全可飛行路徑。

本文的多無(wú)人機(jī)情況，僅考慮了兩架無(wú)人機(jī)的情況以及航跡安全和避障等基本問(wèn)題，在下一步研究中將研究如何優(yōu)化每條無(wú)人機(jī)航跡,并且考慮如何拓展至更多無(wú)人機(jī)執(zhí)行復(fù)雜任務(wù)的情況。