黃智榜，胡立坤，張 宇，黃 彬

廣西大學(xué) 電氣工程學(xué)院，南寧530004

機器人路徑規(guī)劃是指機器人在有障礙物的工作環(huán)境中，按照一定的性能指標(biāo)，尋找出一條從起始位置到目標(biāo)位置的無碰撞路徑。路徑規(guī)劃是移動機器人自主導(dǎo)航最關(guān)鍵的一個環(huán)節(jié)，也是機器人領(lǐng)域的研究熱點[1]。路徑規(guī)劃常用的規(guī)劃算法有A*算法[2]、蟻群算法[3]、人工勢場法[4]、D*算法[5]、跳點搜索算法[6]、快速擴展隨機樹算法[7]和泡沫法[8]等。在靜態(tài)的柵格環(huán)境中，A*算法規(guī)劃效率最高。然而，由于A*算法在大型地圖中需要對柵格地圖的每個柵格點進行檢測，造成需要檢測的冗余節(jié)點過多。針對這個問題，Harabor等人[9]提出一種基于網(wǎng)格尋路的跳點搜索算法（JPS），該算法提出不必對每個柵格點進行檢測，只需根據(jù)鄰居裁剪規(guī)則篩選一些關(guān)鍵節(jié)點作為跳點進行評估，將評估后的最優(yōu)跳點組合連接起來就是一條完整的最優(yōu)路徑。JPS算法盡管解決了A*算法計算冗余等問題，但是經(jīng)研究發(fā)現(xiàn)該算法存在跳點數(shù)量多，規(guī)劃的路徑存在安全隱患等問題。

針對這些問題，文獻[10]通過引入?yún)^(qū)域矩陣，將障礙物之間的中點作為跳點，使跳點與障礙物保持一定的距離，規(guī)劃出安全的路徑。但是該方法完全舍棄算法的路徑尋優(yōu)性能只考慮路徑的安全性能。文獻[11-12]提出雙向跳點搜索算法，進一步減少了跳點的數(shù)量，加快了跳點搜索算法的搜索速度。然而，該方法沒有綜合考慮路徑的安全性能。本文提出一種改進JPS算法，該算法通過改進跳點的篩選規(guī)則，將障礙物膨化處理，更改算法的行進模式，得到一條綜合性能最優(yōu)的路徑。最后，通過仿真和實驗驗證了算法的可行性。

1 傳統(tǒng)的JPS算法

1.1 鄰居裁剪規(guī)則

JPS算法是通過搜索跳點來代替向四周擴展節(jié)點的一種改進A*算法[13]，JPS算法的本質(zhì)是通過避開一些冗余節(jié)點，來減少所需要計算的節(jié)點數(shù)量，提高算法的運行速度。如圖1 所示，假設(shè)在空白的柵格地圖中存在s到g的三條路徑，每條路徑都存在一個中間節(jié)點分別為a2、b2、c2，其中以經(jīng)過節(jié)點b2的路徑最短，所以只需檢測b2節(jié)點而避開不必要的a2、c2節(jié)點，就可以搜索出最優(yōu)路徑。這就是JPS算法的原理。

圖1 跳點原理圖示

利用跳點加速算法的搜索速度[14-15]，就是將A*算法中向鄰節(jié)點擴展改為向四周搜索跳點擴展，就類似于現(xiàn)實中的城市十字路口一樣，對每條路路口進行評估，選出評估值最低的路口，將它們連接起來就是一條到達目標(biāo)點的最優(yōu)路徑。傳統(tǒng)JPS 算法中采用鄰居裁剪規(guī)則來對跳點進行篩選，其具體規(guī)則如圖2 所示，其中灰色不需要查詢的節(jié)點，白色為需要查詢的節(jié)點。

圖2 傳統(tǒng)鄰居裁剪規(guī)則

對比以上的圖2（a）與圖2（b）可以知道，在有障礙物的柵格環(huán)境中比無障礙物柵格環(huán)境需要多查詢一個節(jié)點，稱為被迫節(jié)點n，而無障礙物環(huán)境中查詢的節(jié)點稱為自然節(jié)點。從有障礙物裁剪可以看出，傳統(tǒng)JPS算法尋找出的被迫節(jié)點比較靠近障礙物，且它們之間的路徑經(jīng)過障礙物的拐角，這影響了規(guī)劃路徑的質(zhì)量。在文獻[16]中跳點具有以下定義：

定義1 存在n ∈neighbours(x)，n不是x的自然節(jié)點，如果有l(wèi)en( p,x,n )＜len( p,n x )關(guān)系成立，則點n為被迫節(jié)點。

定義2 如果n以最小的值k，使得n=x+，并且下列條件之一成立，則節(jié)點n為來自節(jié)點x的跳點：（1）n為目標(biāo)點。（2）節(jié)點n至少有一個鄰居節(jié)點是被迫節(jié)點。（3）若為對角查詢狀態(tài)，且存在符合條件（1）和（2）的跳點時，則n為x的跳點。

根據(jù)以上的定義，傳統(tǒng)的跳點分三種：分布在障礙物周圍的被迫節(jié)點、存在被迫節(jié)點的自然節(jié)點、目標(biāo)點。

1.2 跳點的評估函數(shù)

通過鄰居裁剪規(guī)則可以從柵格地圖中挑選出一些關(guān)鍵節(jié)點作為跳點，但是還需對跳點進行評估和計算，增強算法的啟發(fā)性。假設(shè)已知起點s、目標(biāo)點g、當(dāng)前節(jié)點n以及父節(jié)點p，則算法搜索當(dāng)前節(jié)點的實際代價為：

由以上幾個公式就可得出跳點的評價函數(shù)為：

通過評價函數(shù)挑選出最優(yōu)的跳點組合，連接起來就是一條起點到目標(biāo)點的最優(yōu)路徑，其具體的算法模型如圖3所示。模型中紅色柵格為跳點，利用跳點能夠在地圖上構(gòu)建出一條起始點到目標(biāo)點的無碰撞最優(yōu)路徑。

圖3 傳統(tǒng)JPS算法

1.3 碰撞危險度評估模型

為了評估機器人路徑的碰撞風(fēng)險程度，采用二維高斯模型來建立危險度函數(shù)[17]，其模型如下：

其中，M為影響范圍內(nèi)障礙物的個數(shù)，ρ(xi,xobs)是路徑當(dāng)前點到障礙物的最近距離，d0是障礙物的影響范圍，與機器人的機身大小相關(guān)，α、β決定著碰撞對機器人造成危險程度，由機器人的速度矢量以及障礙物的質(zhì)材決定。本文取α=3，β=1，N=100。

2 改進的JPS算法

2.1 改進路徑的行進方式

在傳統(tǒng)的JPS算法中，路徑的行進模式是固定的八鄰域行進模式，這種模式好處在于能夠簡潔形象的顯示算法的優(yōu)劣性，但是卻增加了規(guī)劃路徑的長度。故在本文中選擇與實際較相符合的無限鄰域行進模式[18]，即只要兩個跳點之間沒有障礙物，就能夠直接將跳點相連形成路徑。這種行進模式的優(yōu)點在于能夠縮短算法的路徑長度和減少路徑的轉(zhuǎn)彎次數(shù)。

2.2 改進的鄰居裁剪規(guī)則

在傳統(tǒng)JPS算法中，機器人按照一個質(zhì)點來處理，只能沿著8個方向移動。但是在本文中，機器人并不是一個質(zhì)點，而是具有一定形狀的物體。所以在使用傳統(tǒng)的鄰居裁剪規(guī)則時，機器人會與障礙物碰撞。本文基于對路徑安全性能的考慮，將位于障礙物拐角處的兩個傳統(tǒng)跳點結(jié)合形成新類型的跳點。具體如圖4所示。

圖4 改進的鄰居裁剪規(guī)則

如圖4（b）所示，在有障礙物的柵格環(huán)境中，將傳統(tǒng)跳點結(jié)合形成新類型的跳點，新選取的跳點具有以下的幾個優(yōu)點：

（1）跳點之間的路徑與障礙物保持一定的距離，使得規(guī)劃路徑的安全性能得到很大的提高。

（2）跳點位于障礙物尖角處，使其容易被檢測出來，提高了算法的穩(wěn)定性。

（3）跳點的輻射范圍大，使算法搜索目標(biāo)的能力增強，迭代次數(shù)變少，搜索速度增加。

2.3 改進算法流程

本文使用無限鄰域行進模式構(gòu)成路徑，其改進的算法流程如下所示。

（1）初始化地圖信息map，根據(jù)地圖規(guī)格對障礙物進行膨化處理，設(shè)定起點s以及目標(biāo)點g，并將起點s加入Open列表中。

（2）搜索Open 中實際代價與估計代價和最小的節(jié)點n，將n加入Closed列表并刪除Open列表中的n。

（3）基于父節(jié)點n向四周擴展，具體為向四個傾斜方向擴展，即地圖的右上、右下、左上、左下四個方向，并且每擴展一步的同時向相應(yīng)的方向搜索，將搜索到的跳點加入到跳點集合中。

（4）刪除跳點集合中與父節(jié)點n的直接路徑穿過障礙物的節(jié)點，確保可以用無限鄰域行進模式構(gòu)成路徑。

（5）將跳點集合中的點添加到Open列表中。

（6）如果跳點集合中存在目標(biāo)點g，則終止主程序并輸出路徑；否則回到步驟（2），直到找到目標(biāo)點。

其效果如圖5所示。

圖5 改進算法模型

3 算法的仿真與實驗

3.1 改進JPS算法的仿真與對比

3.1.1 復(fù)雜地圖仿真

為了說明本文提出的改進JPS算法的效果，在同一臺計算機（Windows10，Intel Core i5，內(nèi)存4 GB）上進行仿真。分別在U 型地圖（200×200）和大型樓層地圖（530×530）上進行仿真。

如圖6 所示，傳統(tǒng)的JPS 算法規(guī)劃的路徑中存在一部分路徑貼著障礙物，這部分路徑會增加機器人行走時與障礙物碰撞的幾率。而改進JPS 算法規(guī)劃出來的路徑則不存在這種問題，并且由于改進JPS算法減少了構(gòu)成路徑的跳點的數(shù)量，使得規(guī)劃路徑的轉(zhuǎn)彎次數(shù)大大減少。為了排除實驗的偶然性，分別在兩個地圖中各取50組不同的起點和目標(biāo)點來進行仿真，并將一百組數(shù)據(jù)按路徑長度排序，可以看到其數(shù)據(jù)根據(jù)地形以及路徑長短的不同而變化，具體如圖7、8所示。

圖6 兩種算法仿真

圖7 算法搜索時間比較

在圖7中可以看到在路徑長度較短的實驗組中，改進的JPS算法與其傳統(tǒng)的JPS算法相比所用的時間基本上相差不大，并存在改進JPS 算法比JPS 算法搜索時間多的現(xiàn)象，但隨著路徑長度的增加，地圖復(fù)雜程度的加大，改進JPS算法明顯比傳統(tǒng)JPS算法要快，最快可以達到一個數(shù)量級以上。同時，查看平均時間可以看到，在這一百組數(shù)據(jù)中，改進JPS算法的所用時間明顯比傳統(tǒng)JPS算法所用時間要少。將100組數(shù)據(jù)中的兩種算法規(guī)劃的路徑長度進行對比，結(jié)果如圖8所示。

可以看出，改進JPS算法搜索出來的路徑平均長度都比傳統(tǒng)JPS算法的路徑平均長度要短，但是在路徑長度短實驗組中，有時會出現(xiàn)傳統(tǒng)算法求出的路徑比改進后的算法路徑短的情況，這是由于本文算法對障礙物進行膨化處理導(dǎo)致的，屬于正常的現(xiàn)象。隨著路徑長度的增加，地圖復(fù)雜度的加大，這種現(xiàn)象會慢慢消失。為了對比兩種算法的路徑危險度，將由100組起訖點得出的路徑進行等距離采樣100 個點，設(shè)定影響范圍d0=3，利用方程（5）和方程（6）來計算路徑的危險程度，在路徑危險度評估中，對每個點計算危險度，并將其加起來，就是整條路徑的危險度評分。其具體數(shù)據(jù)如圖9所示。

圖8 算法搜索的路徑長度比較

圖9 路徑危險度比較

如圖9 所示，改進JPS 算法的危險度遠遠低于傳統(tǒng)JPS算法的危險度，從上面的數(shù)據(jù)可以看到，改進JPS算法的平均路徑危險度為45.69，傳統(tǒng)的JPS算法的平均路徑危險度為77.85，改進JPS算法比傳統(tǒng)JPS算法危險度降低41.3%。其具體數(shù)據(jù)如表1所示。

表1 實驗組平均數(shù)據(jù)

3.1.2 不同規(guī)格地圖仿真

為了探索改進JPS算法在不同規(guī)格地圖中的性能，用MATLAB 在不同規(guī)格的簡單地圖中進行仿真，仿真數(shù)據(jù)如表2所示。

表2 不同規(guī)格地圖仿真數(shù)據(jù)

從表2 看到隨著地圖規(guī)格的增加，改進JPS 算法搜索效率的比值減小，綜合表1 的數(shù)據(jù)可以得出結(jié)論：改進JPS算法搜索速度的提升幅度與地圖規(guī)格大小無關(guān)，與障礙物復(fù)雜程度有關(guān)，障礙物復(fù)雜程度度越高，搜索速度提升幅度越大。

3.2 實驗測試

為了驗證算法的有效性，在現(xiàn)實中搭建一個模擬的柵格地圖，并將兩種算法規(guī)劃出來的路徑導(dǎo)入兩輪自平衡小車中進行實驗。實驗設(shè)備如圖10 所示，其實驗場景路線如圖11所示。

圖10 兩輪平衡小車

可以看到，在圖11（a）中，傳統(tǒng)JPS算法的規(guī)劃路徑在拐彎處距離障礙物過近，導(dǎo)致兩輪平衡小車的左輪與障礙物相撞，不能到達目標(biāo)位置，故實際路徑只有一小段。在圖11（b）中，改進JPS 算法的規(guī)劃路徑與實際路徑并不重合，在每個拐彎處，由于小車具有速度不能原地轉(zhuǎn)彎，所以實際路徑超出規(guī)劃路徑的范圍，但是最后卻能夠成功地到達目標(biāo)位置。

圖11 規(guī)劃出來的路徑與實際路徑比較

4 結(jié)論

傳統(tǒng)的JPS 算法規(guī)劃的路徑存在安全隱患。本文在柵格地圖中對移動機器人的工作環(huán)境進行建模，為了提高JPS算法的性能提出改進JPS算法。改進JPS算法能快速地生成一條起點到終點的無碰撞路徑，這不僅解決了路徑安全性的問題，還保留了傳統(tǒng)JPS算法高效尋優(yōu)的性能。通過在MATLAB 上仿真，驗證了本文提出的改進JPS 算法無論是路徑尋優(yōu)還是安全性能都優(yōu)于傳統(tǒng)算法。