改進(jìn)JPS的無人機(jī)路徑規(guī)劃研究

唐嘉寧,閆搏遠(yuǎn),陳云浩,顏 衡,程俊濤

(1.云南民族大學(xué) 電氣信息工程學(xué)院, 昆明 650000;2.云南民族大學(xué) 無人自主系統(tǒng)研究院, 昆明 650000)

0 引言

路徑規(guī)劃是無人機(jī)在執(zhí)行飛行任務(wù)時(shí)的關(guān)鍵問題。隨著無人機(jī)領(lǐng)域的發(fā)展,在災(zāi)后探測、環(huán)境檢測、電力巡檢等復(fù)雜的環(huán)境飛行時(shí),快速準(zhǔn)確地規(guī)劃出一條飛行軌跡就尤為重要。全局規(guī)劃是無人機(jī)完成導(dǎo)航等任務(wù)的前提。全局規(guī)劃是指在設(shè)定好起點(diǎn)和終點(diǎn)后,根據(jù)規(guī)劃算法規(guī)劃出一條從起點(diǎn)到終點(diǎn)的與障礙物碰撞的最佳路徑[1]。常用的全局規(guī)劃算法有Dijkstra算法[2]、A*算法[3]、概率路線圖PRM方法、快速擴(kuò)展隨機(jī)樹RRT算法[4]。

由于A*算法代價(jià)函數(shù)的優(yōu)越性和對搜索路徑的直接性,因此在全局規(guī)劃中得到廣泛應(yīng)用。但使用A*算法進(jìn)行全局路徑規(guī)劃時(shí),實(shí)時(shí)性較差,節(jié)點(diǎn)的計(jì)算量大,內(nèi)存占用大,計(jì)算時(shí)間長。隨著節(jié)點(diǎn)數(shù)的增加,A*算法的規(guī)劃時(shí)間也相應(yīng)的變長。因此,對于A*算法的這些問題,Harabor等[5]提出了跳點(diǎn)搜索算法(jump point search,JPS)。在路徑規(guī)劃時(shí),JPS算法不僅刪除大量的對稱性中間節(jié)點(diǎn)和無用節(jié)點(diǎn),還保留了關(guān)鍵的跳點(diǎn),這樣就可以減少搜索路徑,有效提高了路徑規(guī)劃效率[6]。宋曉茹等[7]為了提高移動(dòng)機(jī)器人在復(fù)雜環(huán)境下的可靠性和智能性,用一種“塊”操作方法加快搜索跳點(diǎn)的速度,同時(shí)刪除了代表路徑方向發(fā)生改變的中間轉(zhuǎn)折點(diǎn)。JPS算法定義了“強(qiáng)制鄰居”和“跳躍點(diǎn)”,去除規(guī)劃路徑時(shí)的冗余點(diǎn),保留下關(guān)鍵點(diǎn)(即跳躍點(diǎn))。將這些跳躍點(diǎn)連接起來,就可以形成一個(gè)完整的全局路徑。Zhao等[8]通過將JPS算法應(yīng)用到移動(dòng)機(jī)器人的路徑規(guī)劃上,證明了JPS算法要優(yōu)于A*算法。對于全局規(guī)劃路徑的速度問題,Iakushkin等[9]提出了一種雙向JPS算法,這種方法從起點(diǎn)和終點(diǎn)同時(shí)搜索路徑,提高了路徑的查找速度,減少了規(guī)劃時(shí)間,提高了全局規(guī)劃的效率。Traish等[10]為了獲得更快的路徑規(guī)劃,提出通過記錄障礙物邊界來減少搜索過程中的迭代計(jì)算。黃智榜等[11]為了解決規(guī)劃出的路徑可能存在的碰撞問題,將跳點(diǎn)進(jìn)行了一種危險(xiǎn)化評(píng)估,改進(jìn)了跳點(diǎn)的篩選方式,并同時(shí)對障礙物進(jìn)行膨脹化處理,從而降低了危險(xiǎn)碰撞度的平均值。黃健萌等[12]為了得到更多有價(jià)值的路徑,使用多段高階多項(xiàng)式對所得路徑進(jìn)行軌跡優(yōu)化,對JPS的搜索規(guī)則做了改進(jìn)。邱磊[13]提出了一種雙向JPS算法,從正、反2個(gè)方向采用改進(jìn)的JPS算法交替進(jìn)行路徑搜索。蔡佳成等[14]為了解決路徑規(guī)劃時(shí)在大尺度復(fù)雜的場景下存在內(nèi)存資源消耗過大、規(guī)劃出的路徑不夠平滑的問題,提出了一種通過融合安全勢場等級(jí)函數(shù)和優(yōu)化的Floyd算法的改進(jìn)JPS算法。黃智榜等[15]為了解決JPS算法規(guī)劃出的路徑距離障礙物過近的問題,提出了一種基于規(guī)劃路徑碰撞危險(xiǎn)度的改進(jìn)JPS算法,該算法通過對傳統(tǒng)跳點(diǎn)規(guī)則進(jìn)行危險(xiǎn)評(píng)估來使跳點(diǎn)與障礙物保持距離。周熙棟等[16]通過提出分層?xùn)鸥竦貓D路徑規(guī)劃算法,來解決移動(dòng)機(jī)器人在大范圍非結(jié)構(gòu)化場景下的路徑規(guī)劃問題。張慶等[17]通過利用切比雪夫距離代替歐式距離來改進(jìn)啟發(fā)函數(shù),用來解決在場景較大的柵格地圖中冗余節(jié)點(diǎn)過多的情況。黃健萌等[18]為了解決在路徑規(guī)劃時(shí)存在的軌跡不夠平滑和規(guī)劃效率不高等問題,提出了一種對目標(biāo)路徑序列進(jìn)行優(yōu)化處理并且同時(shí)對JPS搜索規(guī)則進(jìn)行改進(jìn)的方法。

現(xiàn)在也有很多無人機(jī)路徑規(guī)劃算法,比如RRT算法、DWA算法、PRM算法等。但這些算法由于自身的特性都有一些不足之處。RRT算法是一種是樹型算法,由根節(jié)點(diǎn)作為起點(diǎn)進(jìn)行隨機(jī)生長,它的優(yōu)點(diǎn)是可以快速高效地規(guī)劃出一條路徑,但由于其隨機(jī)性采樣的原因,生成的路徑并不一定是最優(yōu)路徑。由于JPS算法是通過起點(diǎn)與目標(biāo)點(diǎn)之間的方向進(jìn)行跳點(diǎn)搜索,不存在采樣的隨機(jī)性,故JPS算法可以很好地彌補(bǔ)RRT算法的不足。DWA算法主要通過對速度空間中的速度進(jìn)行多組采樣,并通過這些采集來的速度對接下來一段時(shí)間內(nèi)的軌跡進(jìn)行預(yù)測,通過評(píng)價(jià)函數(shù)對得到的多組預(yù)測軌跡進(jìn)行評(píng)價(jià)來選擇一條最優(yōu)軌跡。DWA算法由于只考慮速度和加速度的限制,所以它的計(jì)算復(fù)雜度低。但由于DWA算法的前瞻性不足的原因,在無人機(jī)遇到“C”字形狀的障礙物時(shí),不能很好地避障,而JPS算法在這種情況時(shí),可以根據(jù)跳點(diǎn)搜索規(guī)則下的強(qiáng)制鄰居搜索規(guī)則來解決這一情況。PRM算法也是通過采樣進(jìn)行路徑規(guī)劃的算法,與RRT算法不同的是,PRM算法在進(jìn)行路徑規(guī)劃時(shí),首先要將空間中連續(xù)的點(diǎn)轉(zhuǎn)換為離散的點(diǎn),然后在采樣區(qū)域隨機(jī)撒點(diǎn),將落在障礙物上的點(diǎn)剔除出去,將保留的點(diǎn)作為中心,在中心的一定范圍內(nèi)進(jìn)行鄰域搜索,再將它們連接成無數(shù)條路徑,對連接的路徑進(jìn)行碰撞檢測,如果沒有碰撞那么就將該條路徑保存。但在遇到狹窄通道時(shí),無障礙物的區(qū)域采樣點(diǎn)過于密集,障礙物多的地方采樣點(diǎn)又過少,可能會(huì)存在無法找到最優(yōu)路徑的情況。而JPS算法由于自身的跳點(diǎn)搜索規(guī)則,在無障礙物時(shí)可以直接跳過中間的搜索,在障礙物密集區(qū)域根據(jù)自然鄰居搜索和強(qiáng)制鄰居搜索規(guī)則可以有效地解決PRM算法遇到的問題。由于相對于其他路徑規(guī)劃算法,JPS算法具有以上優(yōu)勢,故選擇改進(jìn)JPS算法作為無人機(jī)的路徑規(guī)劃算法。

在上述分析和討論的基礎(chǔ)上,提出了一種在三維空間進(jìn)行路徑的S-JPS算法,并將此算法在三維空間下與JPS算法進(jìn)行對比實(shí)驗(yàn)。通過比較,證明了S-JPS算法具有較高的綜合性能。仿真實(shí)驗(yàn)驗(yàn)證了S-JPS具有JPS算法的快速規(guī)劃性能和域擴(kuò)展性能,在特殊地圖上仍然具有靈活性。

1 JPS算法

對于A*算法運(yùn)行過程中openlist列表的計(jì)算量大、運(yùn)行時(shí)間長等問題,提出了JPS算法。從圖1可以看出,為了使節(jié)點(diǎn)的展開方向保持不變,就需要在水平方向上連續(xù)移動(dòng),但A*算法仍需要計(jì)算水平方向上的每個(gè)節(jié)點(diǎn)的代價(jià)值,因此JPS算法基于這種情況,省略了中間的迭代計(jì)算過程,提出了跳躍搜索方法。在JPS算法中,起始的節(jié)點(diǎn)考慮了各個(gè)方向上的鄰居節(jié)點(diǎn)。當(dāng)由目標(biāo)函數(shù)確定了路徑規(guī)劃的方向時(shí),JPS算法就會(huì)繼續(xù)向這個(gè)方向擴(kuò)展,但是在遇到障礙物或者由算法定義的跳躍點(diǎn)之前,不需要計(jì)算這個(gè)方向節(jié)點(diǎn)的代價(jià)值。

圖1 A*算法路徑規(guī)劃代價(jià)計(jì)算過程示意圖

1.1 跳點(diǎn)搜索規(guī)則

JPS算法的跳點(diǎn)搜索規(guī)則分為沒有障礙物時(shí)和有障礙物時(shí)2種情況。當(dāng)沒有障礙物時(shí),使用自然鄰居搜索規(guī)則,當(dāng)存在障礙物時(shí),使用強(qiáng)制鄰居搜索規(guī)則。

1.1.1 自然鄰居搜索規(guī)則

當(dāng)沒有障礙物時(shí),使用自然鄰居搜索規(guī)則。在圖2所示的柵格地圖中,灰色的區(qū)域表示可刪除的冗余點(diǎn)。當(dāng)前節(jié)點(diǎn)a的父節(jié)點(diǎn)為p(a),其中擴(kuò)展節(jié)點(diǎn)的當(dāng)前方向?yàn)閜(a)到a的方向。自然鄰居的搜索規(guī)則如圖2所示,主要有2種情況:直線方向和對角線方向的搜索規(guī)則。

圖2 自然鄰居搜索規(guī)則示意圖

在圖2(a)中,父節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)的方向?yàn)樗椒较?所以此時(shí)當(dāng)前節(jié)點(diǎn)a的擴(kuò)展方向?yàn)橹本€方向。如果父節(jié)點(diǎn)p(a)要到達(dá)節(jié)點(diǎn)2,那么在直線方向搜索時(shí),虛線和直線的路徑代價(jià)相同,也就是說父節(jié)點(diǎn)p(a)如果要到達(dá)節(jié)點(diǎn)2可以不用經(jīng)過節(jié)點(diǎn)a并且路徑的代價(jià)不會(huì)增加。那么就稱節(jié)點(diǎn)2為可刪除的冗余節(jié)點(diǎn)。根據(jù)上面提到的規(guī)則,除了節(jié)點(diǎn)1以外的節(jié)點(diǎn)都是可以被去除的冗余點(diǎn),那么就可以將白色節(jié)點(diǎn)1看作是節(jié)點(diǎn)a的自然鄰居。

在圖2(b)中,節(jié)點(diǎn)a為對角線方向搜索,父節(jié)點(diǎn)p(a)不經(jīng)過節(jié)點(diǎn)a到達(dá)節(jié)點(diǎn)2路徑最短,在圖中虛線所表示的路徑為最短路徑。和直線方向搜索的情況一樣,白色的節(jié)點(diǎn)看作是自然鄰居,其他節(jié)點(diǎn)作為冗余節(jié)點(diǎn)刪除。

1.1.2 強(qiáng)制鄰居搜索規(guī)則

當(dāng)有障礙物時(shí),使用強(qiáng)制鄰居搜索規(guī)則。在圖3所示的柵格地圖中,障礙物用黑色柵格表示,其他區(qū)域?yàn)榭赏ㄟ^的路徑。當(dāng)前節(jié)點(diǎn)a的父節(jié)點(diǎn)為p(a),當(dāng)有障礙物存在時(shí),無論是按照直線方向搜索還是按照對角線方向搜索,都無法避免節(jié)點(diǎn)a直接到達(dá)節(jié)點(diǎn)1。強(qiáng)制鄰居的搜索規(guī)則如圖3所示,主要有2種情況:直線方向和對角線方向的搜索規(guī)則。

圖3 強(qiáng)制鄰居搜索規(guī)則示意圖

1.2 代價(jià)函數(shù)

假設(shè)集合A表示展開點(diǎn)的集合,集合A={a1,…,ai,…,an},其中ai為展開的第i個(gè)節(jié)點(diǎn),n是展開的節(jié)點(diǎn)數(shù),astart是起點(diǎn),agoal是終點(diǎn)。D表示路徑的距離,d(astart,a1)表示從起點(diǎn)到第一個(gè)節(jié)點(diǎn)的距離,d(ai,ai+1)表示在尋路過程擴(kuò)展的任意2個(gè)節(jié)點(diǎn)之間的距離,d(an,agoal)表示從最后一個(gè)擴(kuò)展點(diǎn)到目標(biāo)點(diǎn)的距離。路徑長度的目標(biāo)函數(shù)為:

(1)

代價(jià)函數(shù)為:

f(n)=g(n)+h(n)

(2)

在式(2)中,f(n)表示起點(diǎn)到目標(biāo)點(diǎn)的代價(jià)函數(shù),g(n)表示當(dāng)前節(jié)點(diǎn)到起點(diǎn)的代價(jià)函數(shù),h(n)表示當(dāng)前節(jié)點(diǎn)到目標(biāo)點(diǎn)的預(yù)計(jì)代價(jià)函數(shù),其中:

(3)

常用的預(yù)計(jì)代價(jià)函數(shù)公式有:曼哈頓距離(式(4))、歐幾里得距離(式(5))、切比雪夫距離(式(6))、八位數(shù)距離(式(7))。

h(n)=|x2-x1|+|y2-y1|

(4)

(5)

h(n)=max(|x2-x1|,|y2-y1|)

(6)

(7)

式中:(x1,y1)為當(dāng)前節(jié)點(diǎn)的坐標(biāo),(x2,y2)為目標(biāo)點(diǎn)的坐標(biāo)。

采用歐式距離作為預(yù)計(jì)代價(jià)函數(shù)。

JPS算法尋路過程如圖4所示。

圖4 JPS算法尋路過程示意圖

在圖4中,綠色柵格是起點(diǎn),紅色柵格是終點(diǎn),黃色柵格是搜索的跳躍點(diǎn)。從圖4可知,JPS算法丟棄了大量不需要擴(kuò)展的節(jié)點(diǎn),相對于A*算法,大大提高了搜索效率。

1.3 算法流程

JPS算法流程如圖5所示。JPS算法的流程與傳統(tǒng)A*算法的流程相似。都需要對openlist和closelist進(jìn)行操作,當(dāng)前節(jié)點(diǎn)的下一節(jié)點(diǎn)由代價(jià)函數(shù)f=g+h更新。JPS算法在節(jié)點(diǎn)運(yùn)動(dòng)的方向?yàn)?直線和對角線之間尋找跳點(diǎn)。

圖5 JPS算法流程

2 改進(jìn)JPS算法

在傳統(tǒng)的JPS算法里,前端的路徑規(guī)劃里規(guī)劃出的節(jié)點(diǎn)較多,效率低。在后端的軌跡優(yōu)化里只考慮了距離信息,沒有考慮方向信息,軌跡優(yōu)化比較困難。針對這2個(gè)問題,提出基于JPS算法改進(jìn)的S-JPS算法。

2.1 引入新的啟發(fā)函數(shù)

在S-JPS算法中,提出一個(gè)新的啟發(fā)函數(shù)f(n)來解決前面提到的2個(gè)問題,如下:

f(n)=aL-bcosα

(8)

式中:a、b表示距離和方向的權(quán)重,L表示距離信息,如式(9)所示;cosα是方向信息,表示父節(jié)點(diǎn)和擴(kuò)展節(jié)點(diǎn)之間的角度以及擴(kuò)展節(jié)點(diǎn)和目標(biāo)點(diǎn)之間的角度,它表示根據(jù)這個(gè)節(jié)點(diǎn)擴(kuò)展所需要的方向成本。

當(dāng)2個(gè)方向相同時(shí),方向代價(jià)最小,cosα的值為+1;當(dāng)2個(gè)方向相反時(shí),方向代價(jià)最大,cosα的值為-1。

(9)

式中:dx1和dx2分別表示當(dāng)前節(jié)點(diǎn)S和目標(biāo)節(jié)點(diǎn)G的橫坐標(biāo);dy1和dy2分別表示當(dāng)前節(jié)點(diǎn)S和目標(biāo)節(jié)點(diǎn)G的縱坐標(biāo)。

2.2 節(jié)點(diǎn)更新規(guī)則

S-JSP算法可以更高效、準(zhǔn)確地規(guī)劃出路徑。雖然JPS算法可以顯著地減少擴(kuò)展節(jié)點(diǎn)的數(shù)量,但它會(huì)使障礙物區(qū)域的擴(kuò)展節(jié)點(diǎn)數(shù)目增加,這樣將會(huì)增加后端軌跡優(yōu)化的難度。在這個(gè)基礎(chǔ)上,對拐點(diǎn)進(jìn)行了修整,使路徑的平滑度得到了進(jìn)一步提高。對于所有的擴(kuò)展節(jié)點(diǎn)(a0,a1,…,aN),如果相鄰節(jié)點(diǎn)形成的直線an-1an和anan+1的斜率不同,則連接節(jié)點(diǎn)an-1和an+1。如果直線an-1an+1沒有穿過障礙物,則舍棄原來的an-1an和anan+1,保留線段an-1an+1。然后比較an-1an+1和an+1an+2,以此類推。

如圖6所示,藍(lán)色線段為JPS算法規(guī)劃出的路徑,一共得到5個(gè)規(guī)劃點(diǎn)。在JPS算法的基礎(chǔ)上改進(jìn)得到S-JPS算法,使用S-JPS算法進(jìn)行規(guī)劃,得到的新路徑,用紅色線段表示。S-JPS算法將點(diǎn)a0和a2連接起來,因?yàn)槁窂絘0a2沒有通過障礙物,所以舍棄了原來的路徑a0a1和a1a2,保留了路徑a0a2。繼續(xù)連接點(diǎn)a0和a3,由于路徑a0a3穿越障礙物,所以舍棄a0a3,保留路徑a0a2。再次從點(diǎn)a2開始,連接a2a3和a3a4,依次類推,最后將路徑a2a3、a3a4、a4a5、a5aN舍棄,保留路徑a2aN。從圖6中可以看出,擴(kuò)容點(diǎn)a1、a3、a4、a5被舍棄,只保留節(jié)點(diǎn)a2。雖然算法的擴(kuò)展節(jié)點(diǎn)保持不變,但改進(jìn)后的規(guī)劃路徑更高效。

圖6 基于JPS改進(jìn)算法示意圖

S-JPS算法的偽代碼如下:

Algorithm1S-JPS

Input:initialastart,targetagoal,priority queue

Output:optimal path

1g(astral)=0,g(n)=infinite

2 while

3 if the queue is empty,return FALSE break

4 calculate theh(n)=aL-bcosα

5 remove the nodenwith the lowestf(n)=g(n)+h(n) from the priority queue

6 mark nodenas expanded

7 if the nodenis the goal stateagoal,return TRUE; break

8 for all unexpanded jumping neighborsmof the noden

9 ifg(m)=infinite

10g(m)=g(n)+Cnm

11 push nodeminto the queue

12 ifg(m)>g(n)+Cnm

13g(m)=g(n)+Cnm

14 end

15 fori=1 ton-2(number of points,up date every cycle)

16 whilen-i>0

17 ifanan+2does not pass through the obstacles

18 discardan+1

19 else

20 break

21 end

22 end

3 基于Bezier曲線和直線混合的軌跡優(yōu)化

基于JPS算法提出的S-JPS算法雖然能夠生成最優(yōu)路徑,但是沒有考慮無人機(jī)的運(yùn)動(dòng)學(xué)模型,得到的路徑不是光滑的,路徑的曲率也不是連續(xù)的。針對這個(gè)問題,提出一種基于Bezier曲線和直線混合的軌跡優(yōu)化算法對軌跡進(jìn)行平滑處理,提高全局路徑的平滑度。其中Bezier曲線的一般形式如下:

(10)

式中

(11)

B5(t)=(1-t)5a0+5(1-t)4ta1+
10(1-t)3t2a2+10(1-t)2t3a3+
5(1-t)t4a4+t5a5

(12)

利用5次Bezier將規(guī)劃出的路徑優(yōu)化成平滑、曲率連續(xù)的路徑,實(shí)現(xiàn)無人機(jī)導(dǎo)航的軌跡生成。5次Bezier曲線的表達(dá)式如式(12)所示,曲線本身需要6個(gè)控制點(diǎn),除起點(diǎn)和終點(diǎn)以外還有4個(gè)中間控制點(diǎn)。對于本文算法,如果路徑有N個(gè)拐點(diǎn),則整個(gè)路徑有N+2個(gè)路徑點(diǎn):a0,a1,a2,…,aN,aN+1,N+1條折線。因此,軌跡優(yōu)化后的N條Bezier曲線和N+1條直線段采用5次Bezier曲線直線段,它們的路徑和曲率都是連續(xù)的。

由于該算法需要Bezier曲線的曲率與直線段連續(xù),因此Bezier曲線與直線段連接處的曲率為0。如圖7所示,由于Bezier曲線的凹凸性,優(yōu)化出的軌跡在線段內(nèi)[19-20]。其中紅色軌跡為Bezier曲線處理后的軌跡。用Bezier曲線代替原直線段的最重要的部分為控制點(diǎn)的選取。

圖7 Bezier曲線的凹凸性

如圖8所示,圖中ai-1、ai、ai+1為相鄰的3個(gè)關(guān)鍵拐點(diǎn),這3個(gè)點(diǎn)的坐標(biāo)分別為(xai-1,yai-1)、(xai,yai)、(xai+1,yai+1),其中拐點(diǎn)的平滑突變區(qū)又有4個(gè)關(guān)鍵控制點(diǎn)Pi、Pi+1、Pi+2、Pi+3。式(13)為控制點(diǎn)Pi的坐標(biāo):

圖8 Bezier曲線平滑策略

將上述求解過程中控制點(diǎn)Pi的xai-1和yai-1用xai+1和yai+1替換,就是控制點(diǎn)Pi+3的坐標(biāo)。Pi+1和Pi+2為中間的2個(gè)控制點(diǎn),坐標(biāo)為:

(15)

4 仿真分析與實(shí)驗(yàn)

4.1 仿真分析

為了對改進(jìn)的算法進(jìn)行驗(yàn)證,對傳統(tǒng)JPS算法和S-JPS算法進(jìn)行仿真分析。在同一個(gè)地圖環(huán)境下,起點(diǎn)相同,分別選取6個(gè)不同的目標(biāo)點(diǎn),進(jìn)行三維路徑規(guī)劃。實(shí)驗(yàn)所用的硬件平臺(tái)為Intel(R) Core(TM) i7-8750H CPU@ 2.20 GHz,運(yùn)行內(nèi)存為16 GB,操作系統(tǒng)為Ubuntu18.04。在ROS中隨機(jī)生成一個(gè)三維地圖,三維地圖的3個(gè)不同視角的視圖如圖9所示。

圖9 隨機(jī)生成三維環(huán)境示意圖

將本文算法與JPS算法同時(shí)進(jìn)行仿真測試。白色柵格為JPS算法規(guī)劃出的節(jié)點(diǎn),黑色柵格本文算法規(guī)劃出的節(jié)點(diǎn)。在生成的三維環(huán)境中依次進(jìn)行選取目標(biāo)點(diǎn)進(jìn)行規(guī)劃,仿真結(jié)果如圖10所示。

圖10 JPS算法與S-JPS算法仿真結(jié)果

為便于觀察,在圖10中規(guī)劃出節(jié)點(diǎn)的地方已用黑色圓圈圈出,并用紅色箭頭指出。通過仿真結(jié)果可以看出,在目標(biāo)點(diǎn)和起點(diǎn)相同時(shí),本文算法規(guī)劃出的節(jié)點(diǎn)要比JPS算法規(guī)劃出的節(jié)點(diǎn)少。表1為JPS算法與S-JPS算法在同一地圖中的實(shí)驗(yàn)數(shù)據(jù)對比情況。S-JPS算法相比于JPS算法,時(shí)間代價(jià)減少98.6%,路徑代價(jià)減少81.1%,拜訪節(jié)點(diǎn)個(gè)數(shù)減少99.7%。

表1 JPS算法與S-JPS算法實(shí)驗(yàn)數(shù)據(jù)對比

4.2 實(shí)驗(yàn)

為了驗(yàn)證S-JPS算法在無人機(jī)中實(shí)際運(yùn)行的有效性,將JPS算法與S-JPS算法同時(shí)在同一地圖進(jìn)行規(guī)劃,將算法移植到自主搭建的無人機(jī)上,如圖11(a)所示。無人機(jī)采用D435I雙目相機(jī)獲取二維點(diǎn)云數(shù)據(jù),結(jié)合陀螺儀和里程計(jì)數(shù)據(jù)進(jìn)行三維地圖的構(gòu)建,最后通過S-JPS算法對無人機(jī)進(jìn)行路徑規(guī)劃。

圖11 自主搭建的無人機(jī)與實(shí)驗(yàn)環(huán)境

如圖12所示,在無人機(jī)實(shí)際起飛后,可以在Rviz工具中顯示出越過障礙物時(shí)經(jīng)過Bezier曲線和直線混合的優(yōu)化軌跡。其中藍(lán)色軌跡為無Bezier曲線和直線混合的軌跡優(yōu)化,紅色軌跡為基于Bezier曲線和直線混合的軌跡優(yōu)化。從實(shí)驗(yàn)結(jié)果可以看出,藍(lán)色軌跡會(huì)出現(xiàn)穿越障礙物或者距離障礙較近的問題,而紅色軌跡則不會(huì)穿越障礙物并且距離障礙物會(huì)保持一定的安全距離。

圖12 無人機(jī)起飛過程中的Rivz界面

5 結(jié)論

路徑規(guī)劃作為無人機(jī)完成給定任務(wù)的最重要的環(huán)節(jié),在路徑規(guī)劃當(dāng)中引入了改進(jìn)的JPS和Bezier曲線算法。首先,利用基于距離和方向的啟發(fā)式函數(shù),更準(zhǔn)確地描述了從當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的估計(jì)代價(jià),以降低時(shí)間代價(jià)、路徑代價(jià)和拜訪的節(jié)點(diǎn)數(shù)。最后通過Bezier曲線和直線混合的軌跡優(yōu)化算法將規(guī)劃出的路徑進(jìn)行平滑處理,使它更滿足無人機(jī)的動(dòng)力學(xué)約束。實(shí)驗(yàn)結(jié)果表明,相比于傳統(tǒng)JPS算法,S-JPS算法在時(shí)間代價(jià)、路徑代價(jià)和拜訪節(jié)點(diǎn)數(shù)目上都遠(yuǎn)小于JPS算法。

對于提出的算法,還存在以下改進(jìn)空間:由于JPS是在A*算法的基礎(chǔ)上提出的,在較為空曠的區(qū)域時(shí)A*算法的效率要高于JPS,在障礙物密集區(qū)域時(shí)JPS算法效率又遠(yuǎn)高于A*算法。所以在S-JPS算法中,在啟發(fā)函數(shù)階段設(shè)置一個(gè)閾值k,當(dāng)障礙物密度小于閾值k時(shí)使用A*算法的啟發(fā)函數(shù);當(dāng)障礙物密度大于閾值k時(shí),使用S-JPS算法的啟發(fā)函數(shù),動(dòng)態(tài)啟發(fā)函數(shù)公式如式(16)所示。通過定性的分析可以得出改進(jìn)后算法的規(guī)劃時(shí)間要小于S-JPS算法,規(guī)劃效率要高于S-JPS算法。

(16)

式中:H為當(dāng)前障礙物的密度,k為設(shè)置的障礙物密度閾值。