融合JPS 和改進A*算法的移動機器人路徑規劃

張 慶，劉 旭，彭 力，朱鳳增

物聯網技術應用教育部工程研究中心（江南大學 物聯網工程學院），江蘇 無錫214122

路徑規劃是移動機器人完成復雜任務的前提和保證，也是移動機器人的關鍵技術之一[1-2]。目的是根據有障礙物的環境中的某些優化條件，找到從起點到目標點的最優或次優路徑?？紤]移動機器人在室內環境中的應用，如AGV（automated guided vehicle）小車[3]、智能家居[4]、無人機[5]等，要求機器人避開墻壁等障礙物，選擇最優路徑移動。在這個過程中，小車不僅需要具備簡單的避障功能，更重要的是能夠快速實現路徑規劃，提高效率。為此，本文研究了具有墻等障礙物的復雜室內環境下移動機器人的快速路徑規劃問題。

針對移動機器人的路徑規劃問題，國內外許多學者進行了大量的研究。根據對環境信息掌握程度的不同，可分為全局路徑規劃[6-9]和局部路徑規劃[10-13]兩類。本文主要研究了全局路徑規劃問題。目前，用于全局路徑規劃的方法有蟻群算法[14]、A*算法等。蟻群算法是一種模擬自然界中螞蟻覓食行為的仿生進化算法。然而，該算法計算量大，收斂速度慢，求解時間長，且解的質量取決于參數設置，容易陷入局部最優解。隨著遺傳算法種群數量的增加，也存在著計算量大、效率低的問題，容易陷入“早熟”，使目標點無法到達。A*算法相比于蟻群算法往往能更快地求出最優路徑。但A*算法仍存在一些缺點。由于A*算法在尋路的過程中，要不斷將每個節點及周圍八個鄰點添加到OpenList 和ClosedList 兩個列表中進行評估，尋找代價值最低的節點，A*算法主要的計算量在于對節點的評估和代價最小節點的選擇。當應用的場景較大時，由于計算量巨大導致計算速度慢，內存占用大，尋路效率低下。針對此問題，文獻[15]提出了將A*算法與迭代加深算法融合，優化了狀態判重和估價排序，提高了運算效率；文獻[16]提出了提高評估函數的啟發強度對算法進行加速，相較于傳統A*算法速度大幅度提高，但存在較高的空間復雜度，使得算法對內存占用嚴重；文獻[17]利用無限領域的思想解決了A*算法局限八運動方向的問題，但降低了運算速度；Harabor等人提出跳點搜索算法，對于A*算法路徑上的節點進行修剪，然后實現較長距離的跳躍，大幅度提高了運算速度[18]。

本文基于A*算法，但傳統的A*算法難以處理復雜的工業大場景，且搜索過程中存在大量冗余節點。

因此，本文從兩方面對傳統A*算法進行了設計和改進：

（1）改進啟發函數的具體計算方式，使啟發式函數精確地等于實際最佳路徑，減少A*拓展的節點。

（2）使用JPS 搜索策略篩選出跳點添加到OpenList 和ClosedList 中代替A*算法中大量不必要的鄰節點，通過篩選跳點不斷進行拓展，減少了尋路過程拓展的節點，提高了搜索效率。

1 問題的描述

1.1 環境的表示方法

在移動機器人路徑規劃之前，首先建立一個機器人坐標系系統，把機器人各傳感器信息傳遞到統一坐標系，基于不同的傳感器分別建立里程計運動模型、激光雷達觀測模型和IMU 運動模型，最后建立了創建地圖所需的柵格地圖模型。

本文將移動機器人視為二維柵格地圖上的質點，移動機器人的位置以坐標表示，如圖1 所示。柵格地圖模型將機器人工作環境用若干連續同一尺度的柵格表示，用兩種狀態表示環境中的可通行區域和障礙區域。圖中空白柵格表示移動機器人可以通過，即為空閑狀態；黑色柵格表示不可通行的障礙物，為占據狀態。

機器人實際運行中，會有多個方向的移動和控制精度導致的偏差。但考慮到模型的復雜性，A*算法中規定，移動機器人在不處于邊界和四周有障礙物的情況下，可以向周圍8 個方向的柵格移動。由于是二維柵格，不考慮機器人及環境高度，如圖2 所示。

1.2 傳統A*尋路算法引入

Fig.1 Grid map圖1 柵格地圖

Fig.2 Motion direction of mobile robot圖2 機器人運動方向

A*算法是通過使用啟發式函數來引導尋路，從而確保有效地找到最優路徑。其基本思想是，通過一個估價函數來確定搜索的方向，朝著目標點開始拓展。再利用評價函數計算得到每個節點的代價值添加到OpenList 中并選擇代價最小的鄰節點作為下一個拓展節點，重復這一過程直到將目標點添加進OpenList中，最后通過節點的父輩關系反向生成最終路徑。A*算法的評價函數f(n)表示為：

式中，n是當前正在拓展的節點；f(n)表示從起始點經過當前節點n到達目標點的評價函數；g(n)表示起點到節點n的實際代價；h(n)表示節點n到目標點的估計代價，h(n)的正確選取能夠提升A*算法的效率和準確性。傳統A*算法通常采用歐氏距離來計算g(n)和h(n)。

式中，(xs,ys)和(xt,yt)分別是起始點Ps坐標和目標點Pt坐標?？梢詮脑u價函數清楚看出，A*算法是將Dijkstra 算法和最佳優先搜索（best-first-search，BFS）算法相結合。當g(n)=0 的時候，A*算法就等價于使用貪心策略的BFS 算法，算法啟發性更強即偏向于接近目標點的節點，雖然計算速度最快，但不一定能得到最優解；當h(n)=0 時，A*算法就等價于廣度優先搜索的Dijkstra算法，更偏向于接近起點的節點，雖然一定能得到最優解，但計算量大，效率低下。A*算法結合了二者的優點，評價函數綜合了g(n)和h(n)，在保證找到最優路徑的前提下，提高了搜索效率。因此，合理的計算方式會調整A*算法評價函數的權重比例，能有效提高算法的效果。

1.3 A*算法的實施

A*算法在進行路徑規劃時會定義兩個鏈表：一個是ClosedList，用來存放已經遍歷過的節點；另一個是OpenList，用來存放未遍歷并將要被遍歷的節點。算法執行過程中，從OpenList中選擇代價最低節點加入到ClosedList 同時把該節點周圍的鄰節點添加到OpenList，并且更新起點到各個節點的g(n)和各個節點到目標點的h(n)，重復以上步驟直到目標點也被添加到OpenList中，算法結束。A*算法的尋路過程如圖3 所示。

Fig.3 Path-finding process of A*algorithm圖3 A*算法尋路過程

2 改進A*算法

為了解決A*算法路徑規劃過程中存在的計算量大，內存耗費嚴重等問題，本文改進評價函數計算方式并結合JPS 跳點算法對傳統A*算法進行改進。

2.1 改進評價函數計算方式

啟發式函數作為A*搜索算法的核心，對算法的行為有重大的影響。當啟發式函數h(n)始終為0 時，則將由從起點到節點n的距離g(n)決定，此時A*算法將等效于Dijkstra 算法；當h(n)始終小于等于節點n到目標點的實際代價，則A*算法一定可以求出最優解，但是h(n)的值越小，算法需要拓展的節點越多，計算量越大；當h(n)大于節點n到目標點的實際代價，則A*不能保證找到一條最短路徑，但計算速度更快。

傳統A*算法都是采用歐氏距離計算h(n)，但是算法定義了機器人移動是8 方向的，導致規劃出的路徑會大于歐氏距離計算值，如圖4 所示，由于歐氏距離計算出的h(n)小于節點n到目標點的實際代價，導致拓展的節點數量增加。

Fig.4 Euclidean distance and actual path圖4 歐氏距離與實際路徑

本文采用切比雪夫距離代替歐氏距離，切比雪夫距離是指兩個點之間的距離定義為其各坐標數值差的最大值。在向量空間中又稱為L∞范數。在直角坐標系下，兩點之間的切比雪夫距離為：

因此切比雪夫的啟發式函數為：

式中，D是直線移動1 格的代價，(xn,yn)和(xt,yt)分別是當前節點Pn坐標和目標點Pt坐標。

圖4 中，使用歐氏距離計算出的代價值低于實際路徑成本，相當于在啟發函數前加了一個縮放的系數，這樣就導致啟發性降低，算法更偏向于Dijkstra算法。使用切比雪夫距離計算的代價值等價于實際路徑成本，算法的啟發性增強，需要評估的節點減少，這樣就能使得算法在保證最優路徑的前提下也能提高速度。

如圖5所示，藍色為起始點，紫色為目標點，黃色是添加到OpenList 中的節點，紅色是添加到ClosedList中參與評估的節點，綠色是最終路徑。在同一規劃任務下，切比雪夫距離改進的算法參與評估的節點大大減少，規劃效果明顯優于傳統A*算法。

Fig.5 Simulation results of two kinds of heuristic functions圖5 兩種啟發函數的仿真結果

2.2 JPS 策略下改進A*

A*算法結合JPS 策略是在算法執行前增加一個預處理的過程，所謂預處理就是通過JPS 迭代跳點搜索函數，尋找當前節點的繼承跳點，修剪掉中間無用的節點，連接跳點實現兩點間的長距離跳躍。JPS 算法將會利用兩個規則：修剪規則和跳躍規則。

2.2.1 修剪規則

修剪的基本思想就是拓展到節點x的時候，在鄰居集合（即neighbours(x)）中篩選出不需要添加到OpenList 進行評估的任何節點n，以便最快地到達目標點。通過比較兩條路徑的代價值來進行篩選：路徑L1從父節點p(x)出發經過節點x到達節點n；另一條路徑L2從父節點p(x)出發不經過節點x到達節點n。此外，兩條路徑上的每個節點都屬于neighbours(x)。

根據當前節點的鄰節點是否存在障礙物，對修剪規則分兩種情況進行討論。

情況1節點周圍不存在障礙物

如圖6（a）所示，當前節點x是從父節點p(x)直線向右拓展而至，此時評估灰色節點是毫無意義的，因為從p(x)經過x到達這些節點的代價一定不會比從p(x)直接到這些節點的代價更低；圖6（b）是當前節點x從父節點p(x)對角拓展而至，同樣的，此時評估灰色節點會導致計算量大且毫無意義，為了篩選出跳點，需要修剪掉這些灰色節點。

Fig.6 Pruning rules without an adjacent obstacle圖6 節點周圍無障礙物時修剪規則

因此，在節點周圍不存在障礙物時，對于直線移動的拓展，有以下修剪條件：

（1）直線移動時，修剪任何滿足下式的節點：

（2）對角移動時，修剪任何滿足下式的節點：

當A*算法拓展到當前節點x時，根據上述修剪條件對不必要鄰節點進行刪除操作，這時定義x的剩余鄰節點為自然鄰節點。

情況2節點周圍存在障礙物

如圖7 所示，當拓展到節點x時，若neighbours(x)存在障礙物，則無法修剪所有非自然鄰節點。對于圖7 中綠色的節點，不存在一條不經過節點x的代價最小路徑可以從p(x)出發而達到。若要達到它們，則必須經過節點x，否則就不是代價最低路徑。對于每一個這樣的被迫評估的鄰節點，給出篩選條件：

（1）n∈neighbours(x)且不是自然鄰節點；

滿足篩選條件的neighbours(x)定義為強制鄰節點，在拓展過程中這些強制鄰節點將作為跳點進行操作。

Fig.7 Pruning rules with adjacent obstacle圖7 節點周圍有障礙物時修剪規則

2.2.2 跳躍規則

根據修剪規則，篩選出了所有的自然鄰節點和強制鄰節點，這些就是跳點。將所有的跳點添加到OpenList中進行代價評估，這樣減少了A*算法尋路過程中對大量中間節點的計算。

相比于傳統A*算法每次只能拓展一格的策略，改進后的A*算法可以通過連接跳點，實現較長距離的“跳躍”。在尋路的過程中只需要花費很短的時間進行預處理，就可以減少大量不必要的節點，降低了計算過程中的內存開銷，極大地提高了尋路的效率。

如圖8 所示，先從起點S開始向水平和豎直方向遞歸拓展（從父節點到當前節點的方向是拓展的方向，若無父節點，則先向水平和豎直方向拓展），如果遞歸過程中遇到了障礙物，那么遞歸停止，且失敗路徑上的所有節點都被忽略，JPS 不會產生任何東西。否則，遞歸將繼續并導向后繼節點xn，xn是強制鄰節點或目標點，此時搜索從當前節點x直接跳到xn，并不會將沿途的任何一個節點添加到OpenList。當水平和豎直方向都遞歸失敗時，才會在對角線上進行遞歸，這樣可以確保不會錯過任何一個轉折點。當沿著對角線遞歸到新的節點時，重新沿著水平和豎直方向遞歸拓展。

Fig.8 Path-finding process of improved A*algorithm圖8 改進A*算法尋路過程

圖8 中，從起始點S直接跳轉到節點X，繼續向水平方向遞歸到節點Y，由于是一個非失敗的直線式跳躍，遞歸停止；由于節點Z的原因，若遞歸繼續，則會失去一個轉向的機會。當發現目標節點G時，停止遞歸，根據節點的父輩關系，反推出最終路徑。

3 仿真及實驗驗證

3.1 仿真分析

為了驗證改進算法的效果，本文對傳統A*算法和JPS 策略下改進的A*算法進行仿真分析。選取了5個不同尺寸、障礙物密度相同的柵格地圖，在CPU 為i7-9750，操作系統為Windows 10，主頻2.6 GHz，運行內存為16 GB 的計算機上進行仿真。

如圖9 所示，兩種算法在40×40 柵格地圖上進行仿真實驗，左上角的藍色格點為起始節點，右下角的紫色格點為目標節點，黑色為障礙物節點，綠色節點為最終生成的路徑。在圖9（a）中，黃色節點為A*算法添加到ClosedList 評估的節點，灰色節點為添加到OpenList 的節點；圖9（b）中，黃色節點為JPS 算法篩選出的跳點，灰色節點為修剪掉的節點。可以看出，相同環境下兩種算法生成的路徑區別不大，且JPS 策略下改進的A*算法在尋路過程中參與評估的節點數量遠少于A*算法。

Fig.9 Simulation results of A*algorithm and improved A*algorithm圖9 A*算法和改進A*算法的仿真結果

表1 給出了兩種算法在不同尺寸柵格地圖中尋路的搜索時間和評估節點數量的對比?？梢郧宄乜闯?，改進后的A*算法生成的路徑長度和A*算法生成的路徑長度基本相等，且尋路過程中評估的節點數量大大減少，計算時間也明顯降低，減少了尋路過程中對內存的耗費，隨著地圖尺寸的擴大，這種效果更加明顯。

仿真結果表明，JPS 策略下改進的A*算法不僅尋路速度遠超傳統A*算法，對內存的消耗也遠低于傳統A*算法。

3.2 實驗驗證

為了驗證改進后的A*算法在移動機器人實際運行中的有效性，將改進后的算法應用到本人研發的麥克納姆移動機器人上，如圖10（b）所示。移動機器人采用激光測距雷達RPLIDAR A1 獲取二維點云數據，融合陀螺儀姿態和里程計數據后利用Gmaping 功能包完成定位和二維地圖的構建，最后利用Move_base 功能包中的global_planner 完成機器人的全局路徑規劃。本文的實驗場景為實驗室內一段過道，如圖10（b）所示。

如圖11 所示，麥克納姆機器人在實驗室場地進行兩次路徑規劃，規劃結果利用可視化工具Rviz 顯示出來，其中藍色方塊為小車模型，紅色點為激光雷達數據，綠色線條為規劃出的路徑。

表2 給出了4 種算法在實驗室環境下兩次路徑規劃的尋路時間對比。可以清楚地看出，本文改進后的算法相較于傳統A*算法，同樣能規劃出近似的路徑，但是速度卻遠超傳統A*算法；ACO 和GA 算法規劃的路徑雖然更優，但規劃的時間太長，不能滿足移動機器人的實時路徑規劃，因此本文算法更適合移動機器人路徑規劃。

實驗結果表明，在JPS 策略下改進的A*算法能夠有效提高移動機器人路徑規劃的速度，減少了內存的消耗，提高了尋路效率。

Table 1 Comparison between traditional A*algorithm and improved A*algorithm in this paper表1 傳統A*算法和本文改進A*算法的對比

Fig.10 Mobile robots and experimental environments圖10 移動機器人與實驗環境

Fig.11 Path planning result of improved A*algorithm圖11 改進A*算法的路徑規劃結果

Table 2 Comparison of pathfinding time of 4 algorithms under 2 paths表2 2 種路徑下4 種算法尋路時間對比

4 結束語

本文針對傳統A*算法在工廠場景較大的柵格地圖路徑規劃時，遍歷很多冗余的節點導致尋路算法內存消耗大、計算速度慢等問題，提出了一種融合JPS 跳點算法與A*算法的改進策略。通過對不同尺寸柵格地圖的路徑規劃分析與對比，證明了改進算法的有效性。仿真結果表明，本文算法不僅能很大程度地加速A*算法，而且使系統運行時消耗的內存也明顯地減少。

本文也尚存在不足之處，例如本文改進的A*算法雖然在路徑規劃的速度上有了巨大的提升，但最終生成的路徑不夠平滑。在實際機器人工作時，平滑的路徑可以減少電機的加減速，進而提高機器人的能耗。針對這個問題將會引入不同的平滑函數對算法進行改進，進一步提高該算法的實際應用價值。