方向性JPS的移動機(jī)器人全局路徑規(guī)劃方法

馬少博，王立勇，丁炳超，王 超，蘇清華

(北京信息科技大學(xué) 現(xiàn)代測控技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室， 北京 100192)

0 引言

隨著人工智能技術(shù)的快速發(fā)展，移動機(jī)器人技術(shù)越來越受到研究人員的關(guān)注。移動機(jī)器人技術(shù)主要分為感知、規(guī)劃與控制三部分，其中，路徑規(guī)劃是移動機(jī)器人感知和控制的紐帶，是實(shí)現(xiàn)自主導(dǎo)航的核心基礎(chǔ)[1]。路徑規(guī)劃是移動機(jī)器人在已知或未知狀態(tài)空間中，規(guī)劃出一條從起點(diǎn)到目標(biāo)點(diǎn)的路徑長度最短或規(guī)劃時(shí)間最少的無碰撞最優(yōu)路徑。目前，經(jīng)典的路徑規(guī)劃算法主要有人工勢場法[2]、Dijkstra算法[3-4]、RRT算法[5-6]、蟻群算法、粒子群算法和神經(jīng)網(wǎng)絡(luò)算法等一些智能算法[7-9]。

A*算法[10-13]是一種基于圖搜索的算法，由于在搜索路徑時(shí)增加了啟發(fā)式函數(shù)，因此具有較好的實(shí)時(shí)性，被廣泛應(yīng)用于移動機(jī)器人自主導(dǎo)航當(dāng)中。但是A*算法需要頻繁訪問Openlist和Closelist來存儲節(jié)點(diǎn)，這會導(dǎo)致算法保留較多的無意義節(jié)點(diǎn)，增加了算法的復(fù)雜度。為了解決上述問題，Harabor等[14]提出了跳點(diǎn)搜索算法(jump point search，JPS)，通過特殊的節(jié)點(diǎn)選取規(guī)則,在搜索路徑時(shí)只保留滿足節(jié)點(diǎn)選取規(guī)則的節(jié)點(diǎn)，丟棄A*算法產(chǎn)生的無意義節(jié)點(diǎn)，提高了尋優(yōu)速度。但是該算法沒有利用目標(biāo)點(diǎn)的方向信息，導(dǎo)致會在大型地圖中搜索較多不必要節(jié)點(diǎn)。馬小陸等[15]提出一種雙向JPS搜索算法，通過起點(diǎn)和目標(biāo)點(diǎn)交替進(jìn)行搜索路徑減少一部分無關(guān)節(jié)點(diǎn)，加快算法的收斂速度，但是在單向搜索路徑時(shí)，和傳統(tǒng)JPS算法一致，缺少目標(biāo)導(dǎo)向，在大型復(fù)雜地圖環(huán)境中，搜索計(jì)算中遠(yuǎn)離目標(biāo)點(diǎn)的無效路徑節(jié)點(diǎn)依然大量存在。

針對以上問題，提出一種具有目標(biāo)導(dǎo)向的改進(jìn)JPS算法，通過計(jì)算每個跳點(diǎn)與目標(biāo)點(diǎn)之間的方向向量，引導(dǎo)搜索方向的優(yōu)先級排序和方向選取，提高機(jī)器人規(guī)劃路徑過程中的方向性，最終實(shí)現(xiàn)全局路徑規(guī)劃。

1 JPS算法及存在的問題

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

(1)

len(〈Nodep,Nodec,Nodeg〉)<

len(〈Nodep,…,Nodeg〉Nodec)

(2)

圖1 跳點(diǎn)搜索規(guī)則示意圖

len(〈Nodep,Nodec,Nodef〉) <

len(〈Nodep,…,Nodef〉Nodec)

(3)

(4)

1.2 路徑規(guī)劃及存在的問題

在傳統(tǒng)JPS算法規(guī)劃時(shí)，起點(diǎn)缺乏父節(jié)點(diǎn)和強(qiáng)迫鄰居，部分中間跳點(diǎn)的強(qiáng)迫鄰居遠(yuǎn)離目標(biāo)點(diǎn)，造成了算法在搜索路徑時(shí)存在盲目性，會出現(xiàn)搜索時(shí)間長、節(jié)點(diǎn)數(shù)量多等問題。

傳統(tǒng)JPS搜索算法路徑規(guī)劃是從起點(diǎn)開始，通過跳點(diǎn)搜索規(guī)則選取跳點(diǎn)并將其放入Openlist中。在每次迭代時(shí)，將Openlist中代價(jià)值最小的跳點(diǎn)作為下一個待擴(kuò)展節(jié)點(diǎn)，重復(fù)此過程，直到搜索到目標(biāo)點(diǎn)或Openlist為空。跳點(diǎn)的代價(jià)值是由評價(jià)函數(shù)確定的，常見的評價(jià)函數(shù)表示為：

f(Noden)=g(Noden)+h(Noden)

(5)

式中:f(Noden)由2部分組成，g(Noden)是指從起點(diǎn)經(jīng)過一系列中間跳點(diǎn)到達(dá)跳點(diǎn)Noden的實(shí)際距離，如式(6)所示;h(Noden)是指從跳點(diǎn)Noden到達(dá)目標(biāo)點(diǎn)Nodet的估計(jì)距離，通常由歐氏距離計(jì)算，如式(7)所示。

g(Noden)=

(6)

(7)

圖2 傳統(tǒng)JPS算法路徑規(guī)劃示意圖

2 改進(jìn)JPS算法

2.1 跳點(diǎn)搜索方向優(yōu)化

為解決上述問題，對跳點(diǎn)搜索方向進(jìn)行優(yōu)化，提出一種具有目標(biāo)導(dǎo)向的改進(jìn)JPS算法。首先，該算法在搜索跳點(diǎn)過程中，計(jì)算跳點(diǎn)與目標(biāo)點(diǎn)構(gòu)成的方向向量：

(8)

計(jì)算該跳點(diǎn)的方向向量后，將其往x軸和y軸做正交分解，具體計(jì)算過程如下：

(9)

(10)

(11)

圖3 方向向量及正交分解結(jié)果示意圖

(12)

圖4 搜索方向優(yōu)先級搜索示意圖

最后，按照搜索方向的優(yōu)先級依次擴(kuò)展，當(dāng)搜索到最優(yōu)跳點(diǎn)時(shí)，跳出當(dāng)前節(jié)點(diǎn)的擴(kuò)展，并從Openlist中挑選評價(jià)值最低的跳點(diǎn)擴(kuò)展；重復(fù)此過程，直到搜索到目標(biāo)點(diǎn)。通過上述方法可以降低在路徑規(guī)劃過程中搜索非必要跳點(diǎn)和反向搜索的概率，提高搜索效率。

2.2 改進(jìn)JPS算法路徑規(guī)劃

改進(jìn)JPS算法對搜索到每個跳點(diǎn)都進(jìn)行搜索方向優(yōu)先級判定，在起點(diǎn)沒有父節(jié)點(diǎn)和強(qiáng)迫鄰居方向遠(yuǎn)離目標(biāo)點(diǎn)的情況下，可以加強(qiáng)跳點(diǎn)的目標(biāo)導(dǎo)向性。由于后續(xù)的每個跳點(diǎn)都是在最優(yōu)方向下搜索到的，因此可以通過數(shù)量更少的跳點(diǎn)得到最優(yōu)路徑。

圖5 改進(jìn)JPS算法路徑規(guī)劃過程示意圖

改進(jìn)JPS算法的路徑規(guī)劃流程如圖6所示。改進(jìn)JPS算法步驟如下：

圖6 改進(jìn)JPS算法路徑規(guī)劃流程框圖

步驟1 初始化參數(shù)；

步驟2計(jì)算起點(diǎn)方向向量并進(jìn)行正交分解，確定搜索方向優(yōu)先級，將起點(diǎn)放入Openlist中；

步驟3選擇評價(jià)值最小的跳點(diǎn)作為當(dāng)前跳點(diǎn)，并將其從Openlist中移除；當(dāng)前跳點(diǎn)按搜索方向優(yōu)先級依次擴(kuò)展，當(dāng)搜索到最優(yōu)跳點(diǎn)時(shí)，更新最優(yōu)跳點(diǎn)評價(jià)值，計(jì)算其方向向量進(jìn)行正交分解，構(gòu)建搜索方向優(yōu)先級，并將最優(yōu)跳點(diǎn)放入Openlist中；當(dāng)前跳點(diǎn)放入Closelist中，進(jìn)行下一跳點(diǎn)的擴(kuò)展；

步驟4如果最優(yōu)跳點(diǎn)是目標(biāo)點(diǎn)，則規(guī)劃結(jié)束；如果最優(yōu)跳點(diǎn)不是目標(biāo)點(diǎn)，則重復(fù)步驟3，直到搜索到目標(biāo)點(diǎn)或Openlist為空。

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

3.1 仿真及結(jié)果分析

使用Matlab 2020a軟件，在不同尺寸和障礙物復(fù)雜度的柵格地圖上分別進(jìn)行傳統(tǒng)JPS算法和改進(jìn)JPS算法路徑規(guī)劃仿真實(shí)驗(yàn)。實(shí)驗(yàn)柵格地圖分為20×20，50×50，100×100三種，每種地圖分別測試50次。通過比較2種算法得到的有效路徑長度、平均搜索時(shí)間、節(jié)點(diǎn)總數(shù)(路徑點(diǎn)個數(shù)、跳點(diǎn)個數(shù)與擴(kuò)展節(jié)點(diǎn)數(shù)的總和)和跳點(diǎn)利用率(路徑點(diǎn)個數(shù)與跳點(diǎn)個數(shù)的比值)等，分析2種算法的性能及路徑規(guī)劃效率。使用Windows 10操作系統(tǒng)，配有主頻2.6 Hz的Intel I7-6700HQ處理器和8G內(nèi)存。

圖7所示為20×20柵格地圖下的仿真實(shí)驗(yàn)，其中障礙物形狀簡單、規(guī)則，起點(diǎn)坐標(biāo)為(3,3)，目標(biāo)點(diǎn)坐標(biāo)為(20,20)。從圖7可以看出，在相同的環(huán)境當(dāng)中，2種算法生成的路徑相同；在圖7(a)中，起點(diǎn)在擴(kuò)展時(shí)無方向性地朝著周邊8個方向搜索跳點(diǎn)，共搜索到3個跳點(diǎn)，14個擴(kuò)展點(diǎn)，而對于最優(yōu)路徑來說，只有1個跳點(diǎn)、3個擴(kuò)展點(diǎn)是相關(guān)跳點(diǎn)和擴(kuò)展點(diǎn)，其余2個跳點(diǎn)和11個擴(kuò)展點(diǎn)不是最優(yōu)的，并且這些跳點(diǎn)在后續(xù)擴(kuò)展中有可能從Openlist中被選出來當(dāng)作當(dāng)前最優(yōu)跳點(diǎn)繼續(xù)擴(kuò)展，導(dǎo)致搜索更多的不必要跳點(diǎn)；而在圖7(b)中，改進(jìn)JPS算法的起點(diǎn)有了目標(biāo)導(dǎo)向，直接向右、下、右下3個方向擴(kuò)展，減少了跳點(diǎn)數(shù)量，降低了由非最優(yōu)跳點(diǎn)擴(kuò)展的可能性，并且中間跳點(diǎn)添加了目標(biāo)導(dǎo)向，避免向遠(yuǎn)離目標(biāo)點(diǎn)方向擴(kuò)展。

圖7 20×20簡單柵格地圖

隨著地圖尺寸增大及障礙物形狀變得復(fù)雜，改進(jìn)JPS算法的路徑規(guī)劃效率更明顯。如圖8所示，在50×50柵格地圖中，障礙物增多，存在局部封閉空間，在這種復(fù)雜的環(huán)境中，改進(jìn)JPS算法依然能夠找到最優(yōu)路徑，且擴(kuò)展節(jié)點(diǎn)數(shù)少于JPS算法。如圖8(a)所示，JPS算法在起點(diǎn)處往無關(guān)方向擴(kuò)展搜索到的不必要跳點(diǎn)和擴(kuò)展點(diǎn)隨著地圖尺寸的增大而增多，并且由于部分跳點(diǎn)的強(qiáng)迫鄰居方向遠(yuǎn)離目標(biāo)點(diǎn)，導(dǎo)致JPS算法往局部封閉位置擴(kuò)展，降低規(guī)劃效率，而改進(jìn)JPS算法優(yōu)先搜索目標(biāo)點(diǎn)方向，如圖8(b)紅色圓圈標(biāo)注所示，藍(lán)色跳點(diǎn)的強(qiáng)迫鄰居方向?yàn)橛蚁?，而目?biāo)點(diǎn)在左下，改進(jìn)JPS算法優(yōu)先往目標(biāo)點(diǎn)方向擴(kuò)展，并搜索到最優(yōu)跳點(diǎn)后放棄往強(qiáng)迫鄰居方向擴(kuò)展，極大程度上避免了上述情況。

圖8 50×50復(fù)雜柵格地圖

圖9是2種算法在100×100柵格地圖中的效果。起點(diǎn)處于局部封閉環(huán)境，只有1個方向能通行。在圖9(a)中，由于JPS算法的盲目性，會將8個方向都擴(kuò)展之后才進(jìn)行后繼跳點(diǎn)的擴(kuò)展，而改進(jìn)JPS算法是有方向性的擴(kuò)展，如圖9(b)綠色圓圈所示，首先朝著右、下、右下3個方向擴(kuò)展，在沒有搜索到跳點(diǎn)時(shí)，會朝著其他方向繼續(xù)擴(kuò)展，當(dāng)左下方向搜索到跳點(diǎn)時(shí)，起點(diǎn)立馬結(jié)束擴(kuò)展，進(jìn)行后繼跳點(diǎn)的擴(kuò)展，能有效降低計(jì)算復(fù)雜度，減少節(jié)點(diǎn)的搜索數(shù)量。

圖9 100×100復(fù)雜柵格地圖

2種算法在不同尺寸地圖下的搜索結(jié)果如表1所示。由表1可得，在不同環(huán)境中，雖然2種算法規(guī)劃路徑長度相同，但在搜索時(shí)間、節(jié)點(diǎn)總數(shù)和跳點(diǎn)利用率方面差異明顯。在20×20柵格地圖中，改進(jìn)JPS算法相比JPS算法搜索時(shí)間減少43%，跳點(diǎn)利用率提高47%，節(jié)點(diǎn)總數(shù)減少38%，說明改進(jìn)JPS算法的部分跳點(diǎn)能避免向遠(yuǎn)離目標(biāo)點(diǎn)方向擴(kuò)展；隨著地圖尺寸的增大，搜索方向優(yōu)先級的效果更加明顯，在50×50柵格地圖中，改進(jìn)JPS算法的搜索時(shí)間比JPS算法節(jié)約64%，節(jié)點(diǎn)總數(shù)減少64%，節(jié)點(diǎn)利用率提高137%，是JPS算法的2.4倍；在100×100柵格地圖中，改進(jìn)JPS算法花費(fèi)0.30 s完成規(guī)劃，比JPS算法少50%，節(jié)點(diǎn)總數(shù)比JPS算法少1 923個，跳點(diǎn)利用率高34%。上述結(jié)果表明，改進(jìn)JPS算法比JPS算法搜索時(shí)間更短，搜索節(jié)點(diǎn)數(shù)量更少，并且跳點(diǎn)利用率更高。

表1 2種算法不同地圖下仿真搜索數(shù)據(jù)

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

為了驗(yàn)證改進(jìn)JPS算法的有效性和可行性，將JPS算法和改進(jìn)JPS算法分別應(yīng)用于基于ROS的移動機(jī)器人，實(shí)驗(yàn)設(shè)備如圖10所示，搭載速騰32線激光雷達(dá)和JETSON AGX XAVIER算法盒子，在Ubuntu 20.04 的ROS noetic系統(tǒng)中通過相應(yīng)傳感器獲取周圍環(huán)境信息。利用amcl和gmapping模塊完成環(huán)境的柵格地圖構(gòu)建，并在“move_base”中編寫全局路徑規(guī)劃算法插件代替原有的A*算法進(jìn)行全局規(guī)劃。算法盒子通過can通訊將速度信息發(fā)送給SCOUT底盤，控制機(jī)器人實(shí)現(xiàn)自主導(dǎo)航。

圖10 實(shí)驗(yàn)設(shè)備實(shí)景圖

選取實(shí)驗(yàn)室作為環(huán)境場景，2種算法在相同的起點(diǎn)和目標(biāo)點(diǎn)下規(guī)劃路徑，結(jié)果如圖11所示，起點(diǎn)在地圖右下角，目標(biāo)點(diǎn)在地圖左上角，藍(lán)色柵格為跳點(diǎn)，紅色為最優(yōu)路徑。由圖11(a)可以看出，JPS算法在規(guī)劃過程中存在跳點(diǎn)數(shù)量多、向遠(yuǎn)離目標(biāo)點(diǎn)方向擴(kuò)展等問題。而在圖11(b)中，改進(jìn)JPS算法在規(guī)劃最優(yōu)路徑過程中，由于增加了機(jī)器人當(dāng)前位置和目標(biāo)點(diǎn)之間的導(dǎo)向，并對機(jī)器人擴(kuò)展方向進(jìn)行優(yōu)先級排序，根據(jù)搜索方向優(yōu)先級擴(kuò)展跳點(diǎn)，使得機(jī)器人能更有方向性地規(guī)劃，有效解決了上述問題。

圖11 2種算法的路徑規(guī)劃結(jié)果示意圖

圖12是圖11(a)中黑色圓圈標(biāo)注的局部放大圖，在圖12(a)中，JPS算法向下、左下、右下、右、右上方向擴(kuò)展，由于缺少目標(biāo)方向和搜索方向優(yōu)先級，導(dǎo)致搜索到無關(guān)跳點(diǎn)，增加了規(guī)劃復(fù)雜度。在圖12(b)中，改進(jìn)JPS算法向著目標(biāo)點(diǎn)方向擴(kuò)展，減少了向遠(yuǎn)離目標(biāo)點(diǎn)方向擴(kuò)展的跳點(diǎn)，有效避免算法陷入局部最優(yōu)的問題，從而提高了機(jī)器人的工作效率。

圖12 2種算法的局部效果示意圖

根據(jù)表2可知，在保證最優(yōu)路徑相同的基礎(chǔ)上，改進(jìn)JPS算法的搜索時(shí)間比JPS算法減少57%，同時(shí)規(guī)劃過程中搜索到的跳點(diǎn)數(shù)量比JPS算法少118個，減少了機(jī)器人計(jì)算負(fù)擔(dān)，提高了規(guī)劃效率。

表2 2種算法在實(shí)際地圖下的數(shù)據(jù)

上述實(shí)驗(yàn)結(jié)果表明，融合了目標(biāo)導(dǎo)向和具有搜索方向優(yōu)先級的改進(jìn)JPS算法能夠高效、快速地完成機(jī)器人路徑規(guī)劃任務(wù)，且相比于JPS算法，改進(jìn)JPS算法的搜索方向性更強(qiáng)，跳點(diǎn)數(shù)量更少，尋路效率更高。

4 結(jié)論

傳統(tǒng)JPS算法在路徑規(guī)劃過程中存在搜索方向不明確、內(nèi)存消耗大、搜索節(jié)點(diǎn)數(shù)量多、搜索時(shí)間長等缺點(diǎn)，無法滿足移動機(jī)器人路徑規(guī)劃的實(shí)時(shí)性要求。為此，利用目標(biāo)點(diǎn)和當(dāng)前跳點(diǎn)之間的方向向量，結(jié)合傳統(tǒng)JPS算法，對跳點(diǎn)的搜索方向進(jìn)行優(yōu)化，分為2種情況：在起點(diǎn)缺少父節(jié)點(diǎn)方向的情況下，由起點(diǎn)的方向向量及其正交分解向量對應(yīng)的搜索方向?yàn)榈谝粌?yōu)先級進(jìn)行擴(kuò)展；在中間跳點(diǎn)的強(qiáng)迫鄰居方向遠(yuǎn)離目標(biāo)點(diǎn)的情況下，由中間跳點(diǎn)的方向向量對應(yīng)的搜索方向?yàn)榈谝粌?yōu)先級，強(qiáng)迫鄰居方向?yàn)樽畹蛢?yōu)先級進(jìn)行擴(kuò)展，有效減少擴(kuò)展節(jié)點(diǎn)數(shù)量和搜索時(shí)間，提高跳點(diǎn)利用率和尋路效率。在不同尺寸柵格環(huán)境下的仿真實(shí)驗(yàn)證明，改進(jìn)JPS算法在最優(yōu)路徑長度相等的情況下，平均搜索時(shí)間比JPS算法減少43%～64%，節(jié)點(diǎn)總數(shù)比JPS算法減少38%～64%，跳點(diǎn)利用率提高47%～137%，有效提高了移動機(jī)器人規(guī)劃效率，實(shí)時(shí)性更好。將改進(jìn)JPS算法應(yīng)用于實(shí)際機(jī)器人，結(jié)果表明，改進(jìn)JPS算法是一種高效、可行的全局路徑規(guī)劃算法。

本文針對搜索時(shí)間、搜索節(jié)點(diǎn)個數(shù)和跳點(diǎn)利用率進(jìn)行了優(yōu)化，但忽略了路徑的曲折性，導(dǎo)致移動機(jī)器人在追蹤過程中能量損耗增加，下一步研究將著重考慮路徑不平滑和狹窄空間的路徑安全性。