基于JPS改進的移動機器人路徑規劃算法

摘 要：針對跳點搜索（jump point search，JPS）路徑規劃算法在大尺度復雜場景下存在內存資源消耗較大、路徑結果平滑度較低且路徑過于靠近障礙物等問題，提出融合安全勢場等級函數與優化Floyd算法的改進JPS算法。首先建立了安全等級函數對柵格地圖中的柵格狀態進行重新賦值構建安全等級地圖；然后改進了啟發式函數，引入目標與主方向兩項偏置函數項結合安全等級函數項，進一步減少對稱性搜索帶來的時間消耗，改善了所規劃路徑的安全程度;其次通過添加二次平滑算法流程優化了Floyd算法；最后結合B-spline樣條插值法，進一步提高了改進算法所規劃路徑的平滑程度。仿真實驗驗證了改進優化算法在內存資源消耗、路徑長度、路徑平滑程度以及路徑安全程度都有顯著提升。

關鍵詞：移動機器人； 路徑規劃； JPS算法； Floyd算法； B-spline

中圖分類號：TP242 文獻標志碼：A

文章編號：1001-3695（2022）07-010-1985-07

doi：10.19734/j.issn.1001-3695.2021.12.0671

基金項目：裝備預研教育部聯合基金資助項目;四川省科技計劃資助項目（2021YJ0334）

作者簡介：蔡佳成（1996-），男，四川達州人，碩士研究生，主要研究方向為移動機器人路徑規劃、移動機器人智能感知；白克強（1979-），男，甘肅慶陽人，副教授，碩導，主要研究方向為移動機器人、人工智能；李旭春（1998-），女，四川巴中人，碩士研究生，主要研究方向為自動控制、智能移動機器人；黃正良（1962-），男，湖南益陽人，教授，博導，主要研究方向為控制理論和計算機應用技術研究；劉知貴（1966-），男（通信作者），四川射洪人，教授，博導，主要研究方向為計算機技術及應用研究、控制理論應用及自動化裝置研究（liuzhigui@swust.edu.cn.）.

Improved path planning algorithm for mobile robot based on JPS

Cai Jiacheng， Bai Keqiang， Li Xuchun， Huang Zhengliang， Liu Zhigui?

（School of Information Engineering， Southwest University of Science amp; Technology， Mianyang Sichuan 621010， China）

Abstract：Under a large-scale and complex environment， for the path planning algorithm of JPS， the memory consumption is large， the smoothness of path result is low， and the path result is excessively close to obstacles. Aiming at these problems， this paper proposed an improved JPS algorithm that integrated a level function of safety potential field and an optimized Floyd algorithm. It firstly built the safety level function and reassigned the grid status in the grid map to build a safety level map. Then by introducing two bias functions of goal and cardinal direction， and by combining the safety level function， this paper further reduced the time consumption brought by symmetric search， which improved the safety level of the path planned. Besides， it also optimized Floyd algorithm by adding a flow of second smoothing algorithm. At last， the paper integrated B-spline interpolation， improving the smoothness of path planned by the improved algorithm. The simulation tests verify that there are significant improvements in memory consumption， path length， path smoothness and path safety level for the improved and optimized algorithm.

Key words：mobile robot; path planning; JPS algorithm; Floyd algorithm; B-spline

0 引言

如今自主移動機器人因其高度智能、高效、可靠等特點，已被廣泛應用在各類工業、農業、醫療居家服務以及軍事科考等危險環境下的勘探巡檢等行業之中，其場景環境的復雜程度隨著機器人應用場景的多樣化以及更多的人機互動場景而增加^［1］。為保證機器人在此類環境中運行的可靠性、完備性、能耗最優以及速度最優等性質，需通過環境感知獲得自身的位姿與環境地圖的一致性估計之后，依據估計所得的先驗環境信息運用有效的路徑規劃算法可保證機器人在復雜大尺度環境下能快速有效地規劃出一條最優的可行路徑^［2］。

根據先驗環境信息的已知程度以及時效性可將路徑規劃分為全局路徑規劃與局部路徑規劃^［3］。全局路徑規劃為局部路徑規劃提供導引，避免局部路徑規劃陷入局部死鎖或局部最優。局部路徑規劃為全局路徑規劃提供應對動態環境實時快速規劃的能力^［4］。全局路徑規劃的速度與所得路徑的質量直接影響整個路徑規劃的效果。現有的全局路徑規劃算法可分為三大類：a）基于柵格圖的啟發式搜索路徑規劃算法，如Dijkstra算法^［5］、A*算法^［6］、D*算法^［7］等；b）基于采樣的路徑規劃算法，如PRM^［8］、RRT^［9］以及RRT*算法^［10］等；c）各類智能仿生算法^［11］，如粒子群算法、人工魚群算法等。

基于柵格圖的啟發式搜索路徑規劃算法相較于基于采樣的算法和各類智能優化算法存在路徑短、計算量小、資源消耗小、不會陷入局部最優導致目標點不可達等優點。其中A*算法相較于其他基于柵格圖的啟發式搜索算法依據較為合理的啟發式設計被廣泛應用在各類移動機器人路徑規劃中，但A*算法仍存在搜索內存消耗較大、路徑平滑性較差以及無法在動態環境下運行等問題。文獻［12］改進雙向時效的方法，采用多鄰柵格距離計算方式加快路徑規劃效率的同時提升了路徑的平滑程度，但存在內存占用資源較大的問題。文獻［13］運用K-means聚類算法對柵格地圖進行分區建模進一步提高A*算法的搜索速率。文獻［14］為保障自主移動消防機器人快速滅火的目的，添加Floyd算法平滑路徑，消除A*算法鄰域拓展時僅沿八個方向進行拓展而產生的冗余節點問題。

以上方法皆是以A*算法的鄰域拓展方式進行搜索，這將導致open list與closed list存在大量由相鄰柵格節點搜索產生的冗余判斷路徑節點，在大尺度環境下將大幅度降低路徑規劃的速度。Harabor等人^［15］提出了跳點搜索算法，通過對A*算法在拓展的路徑節點進行修剪，大幅度提高了路徑規劃的速率。文獻［16］構建正六邊形柵格，修改鄰居剪枝和強制鄰域判斷規則，提高了路徑規劃的效率以及規劃路徑的質量，但由于鄰域拓展方向僅包含六邊形的六個方向，仍存在大量冗余的中繼節點。文獻［17］修改JPS算法，原有啟發式函數運用切比雪夫距離代替歐氏距離能更精確地估計出實際響應距離，進一步加快了JPS算法路徑規劃的速度，但該方法未考慮所規劃路徑的平滑性。文獻［18］通過融合JPS算法與動態窗口法加快獲得局部路徑規劃所需的局部目標點，但該類目標點不是最優的局部目標節點，導致最終動態窗口法所規劃的路徑長度增加。文獻［19］通過刪除中間的冗余節點進而提高所規劃路徑的平滑程度，該方法未考慮JPS算法所生成路徑節點存在不連續的問題，在大尺度環境中的平滑性有所欠缺。文獻［20］定義一種新的跳點以及跳點域矩陣，通過選擇不同大小的跳點域矩陣獲取與障礙物間的有效距離，進而提高所規劃路徑的安全性，但該方法并未考慮所規劃路徑的平滑程度。

綜上所述，現有JPS算法可以有效解決大尺度環境地圖下的路徑規劃算法效率問題，但并未兼顧規劃所得的路徑質量，無法同時滿足規劃路徑的安全性以及平滑性。本文在此基礎上提出一種平滑安全跳點搜索（smooth safe jump point search，SSJPS）算法，該算法在保證高效性的同時進一步提高了規劃路徑的平滑程度，降低了資源占用，提高了路徑的安全程度，同時解決了路徑姿態不連續問題。

1 算法基礎

1.1 環境地圖模型構建

環境地圖模型構建是整個路徑規劃最初的一個步驟，建立一個能準確描述障礙物信息的環境地圖模型決定了一個路徑規劃方法的有效性。運用在經過激光雷達與視覺相機的信息建立機器人觀測模型，以及IMU 與編碼器信息建立機器人運動學模型，通過后端優化的方法以及回環檢測建立全局一致性良好的全局地圖，同時將全局地圖柵格化建立全局的柵格地圖，如圖1所示。圖中黑色柵格為障礙物區域，記為不可通行節點，白色柵格為無障礙物區域，記為可通行區域節點。

1.2 JPS算法

JPS算法是由Harabor在A*算法的基礎上提出的一種減少鄰域拓展過程產生的大量冗余中繼節點而帶來的資源消耗的優化策略^［15］。該策略包含跳點篩選規則、強制鄰域節點規則與跳躍規則。跳點篩選規則是對自然鄰域節點以及強制鄰域節點兩類節點進行篩選。自然鄰域節點篩選規則是在鄰域拓展過程中所拓展節點x相較于其他節點而言，自父節點p（x）出發經過該拓展節點x到目標節點n的距離代價len小于其余中間節點，其規則定義為

強制鄰域節點篩選規則是指當鄰域搜索過程中相鄰八向鄰域存在障礙物時，所拓展節點x的父節點p（x）經過x到達目標節點n的距離代價相較于其他不經過x到達目標節點n的距離代價len小，則稱n是x的強制鄰域節點，其規則定義為

跳躍規則是將篩選出的自然鄰域節點xnature中由前一節點經對角線跳躍得到當前節點x，以及具有強制鄰域節點xforced的當前節點x作為鄰域拓展節點加入open list，并對相應節點的啟發式代價函數值進行比較，輸出最優代價節點加入close list中。最終反向遞歸得到規劃路徑。

2 改進JPS算法

2.1 構建融入安全勢場函數的全局柵格地圖

針對JPS算法所規劃的路徑結果與障礙物距離較近的問題，本文提出一種融入安全勢場等級的全局柵格地圖構建方法。通過改進人工勢場函數作為安全等級函數，將與障礙物間的距離關系進行更細化的表示，設立一定的安全閾值，解決路徑結果中的節點靠近障礙物這一問題。

為使機器人在不同尺度地圖中均具有較好的安全等級表示，添加自適應調整因子α和β改進斥力場函數F（n），求解障礙物相鄰區域每個柵格n的斥力函數值。其中自適應調整因子可由全局柵格地圖大小msize與機器人本體柵格大小nsize通過不同的縮放倍數ζ與η求得。

則改進斥力函數可被定義為

其中：r（n，nobs）為障礙物柵格節點與相鄰柵格節點的歐氏距離，即

同時為對安全等級進行明確的層級劃分，進一步對斥力函數作歸一化取整處理，獲得安全等級函數：

其中：argmaxx F（n）為全局柵格地圖所有柵格中的最大斥力函數值;argminx F（n）為最小斥力函數值，同時可通過N指定安全等級劃分層級。圖2為N=3的安全全局柵格地圖，S（n）函數值越大表示危險程度越高，越小則安全程度越高。

2.2 基于目標偏置函數與主方向偏置函數的改進啟發式函數

JPS算法由于保留了A*算法的啟發式函數，導致其在鄰域拓展過程中依然存在對稱性搜索現象，本文SSJPS算法通過添加目標偏置函數gbias（n）以及主方向偏置函數mbias（n），用于改善對稱性搜索帶來的冗余跳點、減少時間消耗以及內存資源消耗，同時將安全等級函數加入啟發式函數進一步提高所規劃路徑的安全性。其中目標偏置函數gbias（n）為

其中：dmxi-goal與dmyi-goal分別為待估鄰域節點與目標節點間的x與y方向上的曼哈頓距離;nsize與msize分別為機器人本體所占柵格大小和全局柵格地圖大小。

主方向偏置函數mbias（n）為

其中：dmxstart-goal與dmystart-goal分別為起始節點與目標節點間的x與y方向上的曼哈頓距離。

由兩項偏置函數gbias（n）與mbias（n）、安全等級函數S（n）、當前節點與起始節點間的距離代價G（n），可得改進后的啟發式函數：

2.3 融合改進Floyd與B-spline平滑路徑

JPS路徑規劃算法在鄰域拓展過程中僅沿著上、左、下、右、左上、左下、右上、右下八個方向行進，這一過程產生多個冗余節點，增加路徑中總轉折角度，降低路徑的平滑程度。本文SSJPS算法引入了改進Floyd算法，運用其動態規劃的思想，將規劃所得的多個節點進行平滑處理去除冗余節點，通過優化流程改進Floyd算法優化平滑處理的速度以及優化結果，同時添加二次平滑優化算法改善由跳點路徑坐標不連續帶來的路徑平滑效果不佳問題，最后運用B-spline保證所規劃路徑姿態的連續性。

2.3.1 改進Floyd初步平滑路徑獲得關鍵節點

通過引入安全等級函數S（n）的改進JPS算法規劃得到最終路徑及路徑中的多個跳點n1，n2，…，ni，…，nN，將其坐標（xni，yni）放入節點矩陣Gn中，即

1）構建距離判斷函數 為運用Floyd算法求解余下跳點間的最近路徑節點配置Pathoptimal，構造對應的初始鄰接矩陣Dneb，需求出節點矩陣中任意兩點ni，nj∈Gn間的距離，同時判讀節點間的線路是否被障礙物阻擋，構建如下函數：

其中：r（ni，nj）為節點ni、nj間的歐氏距離，可由式（5）求取;ε（ni，nj，nobs.k）為單位階躍函數;nobs.k∈Aobs表示在碰撞判定區域Aobs中的障礙物集合。該集合滿足以下條件：

2）建立初始鄰接矩陣與頂點矩陣 建立鄰接矩陣與頂點矩陣是Floyd算法的核心。初始鄰接矩陣中每個矩陣元素的數值dij等于相應角標兩點間的距離d（ni，nj），該距離可由距離判斷函數式（11）求得，即初始鄰接矩陣為

同時可建立相應的初始頂點矩陣，初始頂點矩陣中每個元素為-1，頂點矩陣為

初始鄰接矩陣和頂點矩陣的求解算法偽代碼如下。

算法1 初始鄰接矩陣Dneb與初始頂點T求解

輸入：路徑節點Gn=［n1，n2，…，nk，…，nN］=［（xn1，yn1），（xn2，yn2），…，（xnk，ynk），…，（xnN，ynN）］;障礙物節點Dobs=［dobs.1，dobs.2，…，dobs.N］。

輸出：初始鄰接矩陣Dneb與初始頂點矩陣T。

a）if N≤2 do

b） 不存在冗余節點

c）else

d） for i=1 to N

e） for j=1 to N

f） DCurrent=［ni，nj］=［（xni，yni），（xnj，ynj）］

g） 由式（11）求得dij

h） Dneb［i］［j］=dij

i） T［i］［j］=－1

j） end for

k） end for

l）end if

3）改進Floyd算法迭代更新鄰接矩陣與頂點矩陣 在初始鄰接矩陣和頂點矩陣后，用Floyd動態規劃的思想，迭代獲得最優的鄰接矩陣與頂點矩陣。初始鄰接矩陣Dneb與T頂點矩陣已由2）所求得，對于每個節點nk，其中k∈［1，N］，以及任意一對節點ni、nj，其中i，j∈［1，N］，若存在Dneb［i］［j］gt;Dneb［i］［k］+Dneb［k］［j］，則更新鄰接矩陣Dneb［i］［j］的值為Dneb［i］［k］+Dneb［k］［j］，同時將頂點矩陣T［i］［j］改為k，由此迭代遍歷可獲得更新后的鄰接矩陣NewDneb與頂點矩陣NewT。

算法2 改進Floyd算法更新鄰接矩陣Dneb與頂點矩陣T

輸入：初始鄰接矩陣Dneb;頂點矩陣T;節點個數N。

輸出：更新后的NewDneb;更新后的頂點矩陣NewT。

a）NewDneb=Dneb

b）NewT=T

c）for i=1 to N

d） for j=1 to N

e） for k=1 to N

f） if NewDneb［i］［j］gt;NewDneb［i］［k］+NewDneb［k］［j］

g） NewDneb［i］［j］=NewDneb［i］［k］+NewDneb［k］［j］

h） NewT［i］［j］=k

i） end if

j） end for

k） end for

l）end for

4）求解平滑路徑節點配置 確定起始節點nu和終末節點nv，最優的鄰接矩陣NewDneb與頂點矩陣NewT已由3）求得，初始化兩個中繼節點nmid、nvar，令兩者分別為起始節點和終末節點，即nmid=nu，nvar=nv。然后判斷NewT［mid］［var］是否為-1，當為-1時則判斷nvar為最優節點，將其放入最優節點配置Pathoptimal中，同時更新nmid節點為nvar，以及再次將節點nvar變為終末節點nv，重復這一過程直到中繼節點nmid與終末節點nv重合，輸出改進Floyd算法的平滑節點配置Pathfloyd。

算法3 改進Floyd算法回溯平滑節點配置

輸入：更新后的鄰接矩陣NewDneb與頂點矩陣NewT;起點nu與終點nv（u，v∈［1，N］）。

輸出：平滑節點配置Pathfloyd。

a）mid=u

b）var=v

c）將節點nu添加入Pathfloyd

d）while mid≠v do

e） if NewT［mid］［var］=－1

f） 將節點nvar加入平滑節點配置Pathfloyd中

g） mid=var

h） var=v

i） else

j） var=NewT［mid］［var］

k） end if

l）end while

5）二次平滑Floyd后的路徑節點 由于JPS算法輸出路徑節點僅為open list中的跳點，這將導致Floyd算法中無法獲取路徑中的每一個相鄰柵格的路徑節點，使得平滑路徑效果較差。本文設計二次平滑處理方法。依據4）可獲得改進Floyd算法的最優節點配置Pathfloyd，以相鄰三個節點作為一組，以步長為a提取前兩個節點間的路徑節點。然后依次判斷提取的路徑節點與第三個節點間是否可通行，將第一個可通行節點彈出替代第二個節點放入最優節點配置中。相鄰三個最優節點配置Pathfloyd=［ni，ni+1，ni+2］，三點坐標為（xni，yni）、（xni+1，yni+1）、（xni+2，yni+2），以步長為a提取節ni、ni+1兩點間的j個路徑節點［n¹i_i+1，n²i_i+1，…，nji_i+1］，當判斷從第一個節點ni以步長d路徑節點過程中第一次出現可通行情況的路徑節點nki_i+1，則用該路徑節點替代節點ni，具體算法流程如算法4所示。

算法4 二次平滑最優節點配置

輸入：最優節點配置Pathfloyd=［n1，n2，…，nk，…，nN－M］=［（xn1，yn1），（xn2，yn2），…，（xnk，ynk），…，（xnN－M，ynN－M）］;障礙物矩陣Dobs=［dobs.1，dobs.2，…，dobs.k］;步長為a。

輸出：二次平滑后的最終路徑節點配置finalPath。

a）for i=1 to M－N－2

b） if xnigt;xni+1 or ynigt;yni+1

c） d=a

d） else if xni≤xni+1 or yni≤yni+1

e） d=－a

f） end if

g） if xni≠xni+1 or yni=yni+1

h） while nki_i+1≠ni+1 do

i） k++

j） nki_i+1=（xni+kd，yni）

k） 由式（11）求得d（nki_i+1，ni+2）

l） if d（nki_i+1，ni+2）≠∞

m） 更新最優節點配置Pathfloyd，令ni+1=nki_i+1

n） end if

o） end while

p） else if xni=xni+1 and yni≠yni+1

q） 過程同步驟h）～o），修改步驟j）為nki_i+1=（xni，yni+kd）

r） else if xni≠xni+1 and yni≠yni+1

s） if |xni－xni+1|gt;|yni－yni+1|

t） 過程同步驟h）～o），修改步驟j）為nki_i+1=（xni+kd，yni+kd（yni+1－yni）（xni+1－xni））

u） else if |xni－xni+1|≤|yni－yni+1|

v） 過程同步驟h）～o），修改步驟j）為nki_i+1=（xni+kd（xni+1－xni）（yni+1－yni），yni+kd）

w） end if //對應步驟s）

x） end if //對應步驟g）

y）end for //對應步驟a）

z）finalPath=Pathfloyd

2.3.2 B-spline平滑關鍵節點

針對Floyd算法平滑后仍存在少量關鍵節點，即拐點存在角速度和線速度突變的問題，引入B-spline方法設定控制點與對應的樣條基函數，通過線性組合獲得最終的平滑曲線。本文運用三次B-spline平滑關鍵拐點獲得二階可導的平滑曲線，保證規劃路徑速度可導、加速度連線。如圖3所示，圖中節點ni即為上述提及的關鍵節點，運用B-spline平滑此類節點主要是求解控制點坐標以及對應的樣條基函數，通過線性組合的方式以獲得最終的B-spline曲線F（u），即

其中：n為控制點數量;Ci為控制點坐標;Bi，d（u）為控制點Ci的樣條基函數。

求解平滑關鍵拐點ni的B-spline步驟如下：

a）確定控制點數量n。本文在關鍵拐點處設定如圖3所示的四個控制點Ci、Ci+1、Ci+2、Ci+3，由此平滑關鍵拐點ni。

b）求解控制點坐標。如圖3所示，圖中Pni－1、Pni、Pni+1三點為相鄰三個關鍵拐點的中心點，其坐標分別為（xPn_i－1，yPn_i－1）、（xPn_i，yPn_i）、（xPn_i+1，yPn_i+1），求解四個關鍵控制點Ci、Ci+1、Ci+2、Ci+3的位置坐標。首尾兩控制點為Ci和Ci+3，其中控制點Ci的坐標為

控制點Ci+3則是將控制點Ci求解過程中的xPn_i－1與yPn_i－1變換為xPn_i+1與yPn_i+1，即可求得其對應坐標。中間兩控制點Ci+1、Ci+2坐標分別為

c）求解樣條基函數Bi，d（u）主要運用Cox-deBoor遞推公式與基樣條初值迭代求解。樣條基函數為

Bi，d（u）=u－uiui+k－1－ui×Bi，d－1（u）+ui+k－uui+k－ui+1×Bi+1，d－1（u）（19）

基樣條初值為

其中：u為knots節點，其個數為N，由樣條函數的階數d=3以及控制點的個數n確定，即

本文為保證平滑曲線能更好地符合移動機器人運動學與動力學約束，取樣條函數階數d=3，控制點個數n=4，即可獲得knots節點序列U為

整體遞推迭代流程如圖4所示。

d）獲得B-spline。通過b）獲得的控制點Ci以及c）中求得控制點Ci對應樣條基函數Bi，d（u），可獲得B-spline為

F（u）=［Ci Ci+1 Ci+2 Ci+3 ］Bi，3（u）Bi+1，3（u）Bi+2，3（u）Bi+3，3（u）（23）

本文SSJPS算法整體流程如圖5所示。

3 算法仿真與分析

為驗證本文SSJPS算法在大尺度復雜環境下的有效性，在物流倉庫地圖、室內場景地圖以及復雜迷宮地圖三個地圖中與傳統A*算法^［6］、運用Floyd算法改進A*算法^［14］、JPS算法^［15］、IJPS算法^［17］、SD-JPS算法^［20］進行對比實驗。算法實驗運行硬件環境：CPU為AMD R7-4800H主頻2.9 GHz，內存大小16 GB。仿真軟件為MATLAB 2021a。整個實驗首先驗證引入的目標偏置函數以及主方向偏置函數對JPS路徑規劃效率的提升效果；然后針對不同步長a所對應的二次平滑優化結果進行實驗；之后對比A*、Floyd改進A*、JPS、IJPS、SD-JPS以及SSJPS在三個地圖場景中的時間消耗與內存消耗。最后以路徑結果質量，即路徑長度、轉動角度以及碰撞危險程度三項指標對六種算法進行對比分析。

3.1 兩項偏置函數算法效率優化效果

目標及主方向兩項偏置函數主要針對大尺度復雜環境的路徑規劃效率進行改善，消除對稱性搜索導致的冗余時間，具體實驗設置如下：設定大小分別為50×50、100×100、300×300、500×500以及1000×1000的隨機地圖，起點分別為（3，3）、（3，3）、（5，5）、（5，5）與（15，15），終點分別為（47，47）、（98，98）、（295，295）、（495，495）與（985，985），對比本文所提兩項偏置函數下的JPS算法相較于常用以歐氏距離為啟發式函數的JPS算法的路徑規劃時間，取10次路徑規劃所耗的平均時間作為主要評價指標，兩類算法規劃路徑結果如圖6所示，所用時間如圖7所示。路徑規劃所耗時間的具體數值結果如表1所示，本文算法所提兩項偏置函數有效解決了隨機地圖中大量的對稱性搜索帶來的時間消耗。

3.2 二次平滑優化結果

二次平滑主要解決JPS算法所得路徑節點不連續，導致改進Floyd算法平滑效果不佳，而不同步長a對應不同的二次平滑效果。該部分實驗主要研究不同步長a平衡路徑長度、角度縮減量與時間耗散程度三者之間的關系，以最大線速度和角速度作為中間變量，求解平衡效果最佳的步長參數a。設置如下實驗：設定200×200大小的倉庫地圖;起點坐標為（6，6） ，終點坐標為（194，194）;以改進Floyd算法平滑結果（無二次平滑）、步長分別為a=10、a=5、a=1、a=0.5以及a=0.25六個對象進行實驗。

實驗中評價指標主要包含路徑長度縮減率Ratel、轉角度數縮減率Rateθ、時間耗散率RateT以及綜合所有的三者的優化率Rateoptimal四項指標作為最優參數a的評價指標。其中前三項指標皆為六個對象相應指標xjps_a與平滑前路徑對應指標xjps_init的變化率，x分別為路徑長度l、轉角度數θ以及耗散時間T，則有三項評價指標為

路徑長度縮減率：

轉角度數縮減率：

時間耗散率：

優化率則是假設機器人在最高線速度vmax=1 m/s，最高角速度wmax=3 rad/s的條件下。假設在相應時間消耗情況下保持最高線速度與角速度對應的位姿變化量ΔPmax，平滑后的位姿變化量為ΔPa，則優化率為

其中：ΔPmax=（ljps_init－ljps_a）+（θjps_init－θjps_a），ΔPa=（vmax+wmax）（Tjps_a－Tjps_init）。

實驗結果如圖8所示，其中圖8（a）為不同步長a下的路徑規劃結果，步長a值越小，則對應路徑長度與轉角度數越小。同時如圖8（b）所示，當步長alt;1時，長度與角度縮短率變緩，但時間耗散率急劇增加，導致最終優化率開始下降，遂選擇步長a=1平衡時間消耗與路徑質量間的關系。

3.3 規劃算法效率與資源占用對比分析

本文以平局路徑規劃時間Tavg、占用內存大小SizeRAM作為評價指標衡量路徑規劃算法效率以及資源占用的問題。其中平局路徑規劃時間Tavg是六種路徑規劃方法取10次規劃所用時間的平均值，占用內存大小則是存儲相應路徑規劃結果的路徑節點所占用的內存大小。

實驗設置A*、Floyd A^*、JPS、IJPS、SD-JPS以及SSJPS六種算法分別在三個地圖場景中進行對比實驗，為保證算法的實用性以及在應對大尺度復雜環境的有效性，設置三個地圖場景分別為200×200大小的物流倉庫地圖、300×300大小的室內場景地圖以及500×500大小的復雜迷宮地圖。物流倉庫地圖設定起點坐標為（6，6） ，終點坐標為（194，194）；室內場景地圖設定起點坐標為（6，6），終點坐標為（294，294）；復雜迷宮地圖設定起點坐標為（6，6），終點坐標為（494，494）。路徑規劃結果如圖9所示。

表2中SSJPS算法相較于傳統A*與Floyd A*算法而言，平均路徑規劃時間均有較大提升，在物流倉庫中相較于傳統A*算法時間縮短了93.77%，相較于Floyd A*算法縮短了95.60%。地圖越復雜尺度越大，SSJPS算法提升效果越明顯。在500×500迷宮地圖中，傳統A*以及Floyd A*算法規劃所耗時間分別約為SSJPS算法的66倍以及91倍，SSJPS算法效率相對于A*類算法有顯著提升。在物流倉庫地圖中，SSJPS算法引入兩項偏置函數，相較于JPS類算法雖減少了平滑前的路徑規劃時間，但地圖中并未出現太多對稱性搜索情況，并且兩階段的平滑過程還需消耗少量時間，導致最終規劃時間相較于JPS算法增加了28.71%。不過在資源消耗方面，SSJPS算法相較于JPS、IJPS以及SD-JPS算法的占用內存分別減少了42.85%、50%以及42.85%，在室內地圖中相對于JPS、IJPS以及SD-JPS算法分別以30.91%、33.05%與23.50%的時間代價減少了42.85%、50%以及42.85%的占用內存大小。且隨著地圖的尺度與復雜程度增加，SSJPS相較于JPS算法以更少的時間作為代價進一步減少占用內存大小。在500×500的復雜迷宮地圖中，相較JPS、IJPS算法的時間代價分別為12.44%、18.40%，相較于SD-JPS則速度提升了6.39%，同時占用內存大小相較于JPS與SD-JPS算法分別減少了53.33%以及70.83%，而IJPS則是由于啟發式函數的影響導致其并未進入復雜環境區域而進行了繞行，導致其路徑節點減少。SSJPS算法在算法效率上，相較傳統A*和Floyd A*算法有顯著提升，在資源占用方面相較A*與JPS算法均有明顯改善。

3.4 規劃算法路徑質量對比分析

本文為對比規劃算法所規劃路徑的平滑程度，以及安全程度以路徑長度lsum、轉動角度θsum、速度突變次數Nmutation以及危險評價函數值Frisk作為路徑質量的評價指標。路徑長度以一個柵格為1 m作為標度；轉動角度θsum是指本體位姿發生的角度變化；速度突變次數Nmutation是指機器人按照規劃路徑跟蹤控制速度突變導致路徑姿態不連續的次數；危險評價函數值Frisk由路徑長度lsum與大于設定安全等級函數閾值的危險路徑長度lrisk的比值確定，即

該部分實驗以融入N=3的安全勢場等級函數后的三種不同地圖作為背景，得到如圖10所示的路徑規劃結果顯示圖。

表3中SSJPS與Floyd A*算法在路徑長度lsum以及轉角度數θsum上要明顯優于傳統A*、JPS、LJPS以及SD-JPS，但SSJPS考慮到路徑的安全程度，導致路徑長度lsum與轉角度數θsum略大于Floyd A*算法。同時SSJPS算法通過引入安全等級函數以及B-spline提高規劃路徑安全性的同時，保證了路徑姿態的連續性。

4 結束語

本文針對JPS路徑規劃算法在大尺度復雜環境中存在內存資源占用較大、安全性較低、規劃路徑不夠平滑、姿態變化不連續等問題，提出一種改進優化算法SSJPS，構建安全等級函數，將其加入啟發式函數，提高路徑規劃的安全性；運用主方向偏置函數與目標偏置函數減少JPS算法在大尺度復雜環境中的對稱性搜索，加快路徑規劃的速率，減少不必要的中間路徑節點；結合改進優化Floyd算法，減少由八向鄰域拓展產生的冗余路徑節點，提高了路徑平滑程度，同時優化其平滑處理算法加快平滑處理運算；設計二次平滑方法，避免改進優化Floyd算法在處理JPS路徑規劃算法所得路徑節點，即跳點間的路徑節點不連續問題，影響路徑平滑質量；設定控制節點運用B-spline對平滑后的拐點進行處理，保證整體路徑節點間姿態的連續性。通過實驗對比分析，證明了SSJPS算法在大尺度復雜環境中不耗散過多時間的同時，降低了內存資源占用、提高了路徑平滑程度、保證了路徑的安全性，為之后的路徑跟蹤提供了連續的姿態節點。在未來研究中，可將SSJPS算法與局部路徑規劃方法進行更好的結合，解決當局部路徑規劃方法在依據全局路徑規劃作為導向時出現二次死鎖并進行重規劃的這類問題，同時考慮在接下來的工作中將本文算法應用于實際的機器人系統中。

參考文獻：

[1]Garaffa L C，Basso M，Konzen A A，et al.Reinforcement learning for mobile robotics exploration：a survey［J/OL］.IEEE Trans on Neural Networks and Learning Systems.（2021）.https：//doi.org/10.1109/TNNLS.2021.3124466.

［2]Rubio F，Valero F，Llopis-Albert C.A review of mobile robots：concepts，methods，theoretical framework，and applications［J］.International Journal of Advanced Robotic Systems，2019，16（2）：1736996972.

［3]Low E S，Ong P，Cheah K C.Solving the optimal path planning of a mobile robot using improved Q-learning［J］.Robotics and Autonomous Systems，2019，115：143-161.

［4]王洪斌，尹鵬衡，鄭維，等.基于改進的A*算法與動態窗口法的移動機器人路徑規劃［J］.機器人，2020，42（3）：346-353.（Wang Hongbin，Yin Pengheng，Zheng Wei，et al.Mobile robot path planning based on improved A* algorithm and dynamic window method［J］.Robots，2020，42（3）：346-353.）

［5]Wang Zhiyong，Zlatanova S.Safe route determination for first respon-ders in the presence of moving obstacles［J］.IEEE Trans on Intelligent Transportation Systems，2020，21（3）：1044-1053.

［6]Everson L R，Sapatnekar S S，Kim C H.A time-based intra-memory computing graph processor featuring A* wavefront expansion and 2-D gradient control［J］.IEEE Journal of Solid-State Circuits，2021，56（7）：2281-2290.

［7]Maurovic＇ I，Seder M，Lenac K，et al.Path planning for active SLAM based on the D* algorithm with negative edge weights［J］.IEEE Trans on Systems，Man，and Cybernetics：Systems，2018，48（8）：1321-1331.

［8]Kai Cao，Qian Cheng，Song Gao，et al.Improved PRM for path planning in narrow passages［C］//Proc of the 16th IEEE International Conference on Mechatronics and Automation .Piscataway，NJ：IEEE Press，2019：45-50.

［9]Da Silva Costa L，Tonidandel F.DVG+A* and RRT path-planners：a comparison in a highly dynamic environment［J］.Journal of Intelligent amp; Robotic Systems，2021，101（3）：article No.58.

［10]Karaman S，Frazzoli E.Sampling-based algorithms for optimal motion planning［J］.The International Journal of Robotics Research，2011，30（7）：846-894.

［11]于振中，李強，樊啟高.智能仿生算法在移動機器人路徑規劃優化中的應用綜述［J］.計算機應用研究，2019，36（11）：3210-3219.（Yu Zhenzhong，Li Qiang，Fan Qigao.Survey on application of bioinspired intelligent algorithms in path planning optimization of mobile robots［J］.Application Research of Computers，2019，36（11）：3210-3219.）

［12]高民東，張雅妮，朱凌云.應用于機器人路徑規劃的雙向時效A*算法［J］.計算機應用研究，2019，6（3）：792-795，800.（Gao Min-dong，Zhang Yani，Zhu Lingyun.Bidirectional time-efficient A* algorithm for robot path planning［J］.Application Research of Compu-ters，2019，36（3）：792-795，800.）

［13]余文凱，章政，付雪畫，等.基于地圖預處理及改進A*算法的路徑規劃［J］.高技術通信，2020，30（4）：383-390.（Yu Wenkai，Zhang Zheng，Fu Xuehua，et al.Path planning based on map partition preprocessing and improved A* algorithm［J］.Chinese High Technology Letters，2020，30（4）：383-390.）

［14]Liu Yongtao，Sun Ruizhi，Zhang Tianyi，et al.Warehouse-oriented optimal path planning for autonomous mobile fire-fighting robots［J］.Security and Communication Networks，2020，2020：article ID 6371814 .

［15]Harabor D，Grastien A.Online graph pruning for pathfinding on grid maps［C］//Proc of the 25th AAAI Conference on Artificial Intelligence.Palo Alto，CA：AAAI Press，2011：1114-1119.

［16]王文明，杜佳璐.基于正六邊形柵格JPS算法的智能體路徑規劃［J］.系統工程與電子技術，2021，43（12）：3635-3642.（Wang Wenming，Du Jialu.Agent path planning based on regular hexagon grid JPS algorithm［J］.Systems Engineering and Electronics，2021，43（12）：3635-3642.）

［17]張慶，劉旭，彭力，等.融合JPS和改進A*算法的移動機器人路徑規劃 ［J］.計算機科學與探索，2021，15（11）：2233-2240.（Zhang Qing，Liu Xu ，Peng Li，et al.Path planning for mobile robots based on JPS and improved A* algorithm［J］.Journal of Frontiers of Computer Science and Technology，2021，15（11）：2233-2240.）

［18]Liu Lisang，Yao Jinxin，He Dongwei，et al.Global dynamic path planning fusion algorithm combining jump-A* algorithm and dynamic window approach［J］.IEEE Access，2021，9：19632-19638.

［19]Zafar M A，Zhang Zheng，Yu Wenkai.Mobile robots path planning based on A* algorithm improved with jump point search［C］//Proc of the 18th International Bhurban Conference on Applied Sciences and Technologies .Piscataway，NJ：IEEE Press，2021：536-544.

［20]Zheng Xue ，Tu Xiaowei，Yang Qinghua.Improved JPS algorithm using new jump point for path planning of mobile robot［C］//Proc of the 16th IEEE International Conference on Mechatronics and Automation.Piscataway，NJ：IEEE Press，2019：2463-2468.