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

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

0 引言

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

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

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

1 JPS算法及存在的問題

1.1 跳點搜索規(guī)則

(1)

len(〈Nodep,Nodec,Nodeg〉)<

len(〈Nodep,…,Nodeg〉Nodec)

(2)

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

len(〈Nodep,Nodec,Nodef〉) <

len(〈Nodep,…,Nodef〉Nodec)

(3)

(4)

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

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

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

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

(5)

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

g(Noden)=

(6)

(7)

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

2 改進JPS算法

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

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

(8)

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

(9)

(10)

(11)

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

(12)

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

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

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

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

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

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

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

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

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

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

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

3 仿真與實驗分析

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

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

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

圖7 20×20簡單柵格地圖

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

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

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

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

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

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

3.2 實驗

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

圖10 實驗設(shè)備實景圖

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

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

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

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

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

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

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

4 結(jié)論

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

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