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

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

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

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

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

1.1 鄰居裁剪規(guī)則

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

圖1 跳點(diǎn)原理圖示

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

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

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

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

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

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

1.2 跳點(diǎn)的評(píng)估函數(shù)

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

由以上幾個(gè)公式就可得出跳點(diǎn)的評(píng)價(jià)函數(shù)為：

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

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

1.3 碰撞危險(xiǎn)度評(píng)估模型

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

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

2 改進(jìn)的JPS算法

2.1 改進(jìn)路徑的行進(jìn)方式

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

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

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

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

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

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

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

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

2.3 改進(jìn)算法流程

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

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

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

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

（4）刪除跳點(diǎn)集合中與父節(jié)點(diǎn)n的直接路徑穿過(guò)障礙物的節(jié)點(diǎn)，確?？梢杂脽o(wú)限鄰域行進(jìn)模式構(gòu)成路徑。

（5）將跳點(diǎn)集合中的點(diǎn)添加到Open列表中。

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

其效果如圖5所示。

圖5 改進(jìn)算法模型

3 算法的仿真與實(shí)驗(yàn)

3.1 改進(jìn)JPS算法的仿真與對(duì)比

3.1.1 復(fù)雜地圖仿真

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

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

圖6 兩種算法仿真

圖7 算法搜索時(shí)間比較

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

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

圖8 算法搜索的路徑長(zhǎng)度比較

圖9 路徑危險(xiǎn)度比較

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

表1 實(shí)驗(yàn)組平均數(shù)據(jù)

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

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

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

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

3.2 實(shí)驗(yàn)測(cè)試

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

圖10 兩輪平衡小車

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

圖11 規(guī)劃出來(lái)的路徑與實(shí)際路徑比較

4 結(jié)論

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