面向移動機器人快速全局路徑規劃的改進跳點搜索算法

宋曉茹， 任怡悅

(1.西安工業大學電子信息工程學院， 西安 710021； 2.西安工業大學自主智能創新團隊， 西安 710021)

路徑規劃是移動機器人實現自主化和智能化的關鍵技術，路徑規劃算法的性能直接影響移動機器人的工作效率，成為一個重要的研究領域[1-2]。柵格法因其便于機器人控制器存儲、處理、更新和使用[3]，常用于完成環境的構建和表示，基于此衍生出的全局路徑規劃算法有Dijkstra算法、A*算法、Swamps算法、SUB算法、跳點搜索算法等。

Dijkstra算法[4]是典型的最短路徑算法，Chen等[5]基于Dijkstra算法建立了用于車輛疏散的動態道路網絡模型，為公共場所的最優緊急疏散路徑選擇和應急救援決策提供了良好的方案；然而Dijkstra算法需要遍歷大量節點，導致效率低下。針對此問題，Ren等[6]采用鄰接列表和循環鏈表，進行權重排序得到改進的Dijkstra算法，用于解決最短路徑的交通網絡問題。A*算法[7]是一種求解最短路徑最有效的直接搜索算法，針對其存在內存開銷大，計算時間長等缺點，Korf[8]提出IDA*，融合了迭代加深算法于A*算法中，不需要進行狀態判重和估價排序，減少空間需求；Botea等[9]提出HPA*，通過提高啟發函數的準確度，減小搜索空間，提高效率，但空間復雜度高。Pochter等[10]提出Swamps算法，采用離線預計算的方式將網格地圖分解為一系列相鄰的區域，識別并忽略與最優解無關的無效區域，減少搜索時間與搜素節點。Uras等[11]提出SUB算法，通過預先將網格地圖轉換為可視化圖(稱為子目標圖)來工作，然后算法存儲和搜索的是子目標圖，而不是原始的網格來尋找最優路徑。

在柵格環境中，影響算法效率的最重要因素是存在大量對稱性路徑，將路徑視為無序的向量而不是有序的節點序列時，可以看到地圖中有許多路徑共享相同的起點和終點，且能通過交換其中一條路徑組成向量之間的順序，得到另一條路徑。這些大量對稱性路徑的存在導致算法需要去評估許多等效狀態，阻止了向目標點的真正進展。然而上述研究均不是通過識別并消除對稱性而得到最優路徑。

Harabor[12]提出跳點搜索算法(jump point search，JPS)，通過圖裁剪來減少搜索過程的對稱性，并在擴展節點的過程中篩選特定的節點——“跳點”，不僅提升了性能而且降低了內存成本。Jia等[13]在迷宮搜索方面對比了各算法的路徑規劃能力，比較了跳點搜索算法、A*算法與HPA*算法的搜索時間和效率，實驗結果證明跳點搜索算法明顯優于其他算法；趙曉等[14]結合跳點搜索算法改進A*算法，篩選出關鍵點進行擴展，加速全局路徑規劃的效率。但以上都只是簡單的應用跳點搜索算法，并未對其進行改進。

跳點搜索法中識別關鍵跳點涉及了大量的迭代過程，成為了算法一個新的瓶頸，因此Harabor等[15]提出了JPS+算法，將柵格地圖信息預處理為查詢表，通過查找表格信息直接獲得路徑中的下一個跳轉點，消除了跳點搜索算法引起的最大的處理開銷。Traish等[16]提出了BL-JPS算法，通過預處理網格內障礙物邊界和地圖邊緣位置來加速跳躍點的識別。但以上對跳點搜索算法的兩種改進方式都是離線操作，通過預處理地圖信息，大幅度提高搜索速度，當地圖發生連續改變時，重新評估最佳路徑過程中的任何開銷都會成為實時路徑規劃的性能問題。

針對以上問題，采用“塊”操作方法，在一次搜索中快速掃描底層網格中的一個區域，將跳點搜索算法中的修剪規則一次應用于多個節點，以達到快速識別跳點的目的，并對僅僅只具有改變方向性質的跳點進行剔除。此策略完全為在線方式，不需要任何特殊的數據結構，也不存儲或計算任何其他信息，并同時保留了與原始算法相同的固有優勢:完整性與最優性。為了驗證算法的有效性與可行性，分別在規則的網格地圖、測試庫基準地圖及移動機器人Turtlebot2進行對比實驗。

1 跳點搜索算法(JPS)

跳點搜索算法的主要思想是對稱修簡規則和跳點識別規則，搜索過程會遞歸的調用這兩種規則。優勢在于考慮了與擴展節點相關的父節點位置，即每條規則都會根據上一步的方向(直行或沿對角線)來決定這一步應該朝什么方向前進，并為正在評估的節點標識一組“自然”鄰居和“強制性”鄰居。“自然”鄰居由擴張方向定義：基本方向上(水平方向、豎直方向)的自然鄰居定義為同一方向的下一個節點；對角線方向的自然鄰居集包括3個節點：沿著對角線的下一個節點，以及下一個垂直和水平節點。規則的例外是擴展與障礙物相鄰的節點，在這種情況下必須考慮無法直接從父節點訪問的路徑，識別強制性鄰居。

1.1 修剪規則

識別出不需要被評估的節點，以便快速到達目標，具體通過比較兩條路徑的長度來完成。一條路徑起始于p(x)，經過節點x進行直線或對角線運動；另一條同樣起始于p(x)進行直線或對角線運動，但是不經過節點x，如圖1所示。其中經過節點x的路徑明顯更短且減少了對周圍鄰節點的重復訪問，因此為了篩選跳點，需要刪掉這些不必要的節點，即圖中的灰色柵格。不論兩條路徑中任意一條所涉及的節點必須屬于x的鄰居集內。若鄰居集內不包含障礙物，應用直線或對角線修剪之后的節點稱為x的自然鄰居，如圖1中的白色柵格；當鄰居集內包含障礙物時，這時評估的節點稱為強制性節點，如圖1中的斜杠柵格。

圖1 修剪規則示意圖Fig.1 Diagram of pruning rule

1.2 跳點識別規則

識別并選擇性擴展某些特定的點，這些被選中的節點稱之為跳點，用于加速尋找最優路徑。兩個跳點所連接路徑上的中間節點不被擴展，直接從一個跳點移動到下一個跳點。跳點識別規則可歸納為y=x+kd，從x點出發，通過在d方向移動k步到達y，其中擁有最小k的節點y稱為x的跳點。

(1)節點y目標點。

(2)節點y含有至少一個強制性節點。

(3)若d為對角線移動，存在z=y+kidi，其中kiN，z是y的跳點，則y也是x的跳點。

跳點識別規則示意圖如圖2所示。

圖2 跳點識別規則示意圖Fig.2 Diagram of jump point identification

2 改進的跳點搜索算法

如何快速有效地發現跳點已成為跳點搜索算法的瓶頸問題。表1給出了在3種模擬真實環境的測試庫基準地圖上運行大量示例所獲得的數據，可觀察到跳點搜索算法需要花費約90%的時間用于生成后繼者，A*算法花費約40%的時間，而在對Openlist和Closedlist列表中節點的操作約占10%，因此跳點搜索算法的效率取決于能否快速生成后繼節點。

表1 3種模擬真實環境的基準地圖上所占搜索時間比例的對比Table 1 Comparison of the proportion of search time on three benchmark maps that simulate real environments %

提出在一次搜索中快速掃描一個區域而不是單獨的節點，將修剪規則一次應用于多個節點，節省大量且毫無意義的節點操作，以達到快速識別跳點的目的，并在采取對角優先的方式的前提下，剔除僅具有改變方向的中間轉折點。當遞歸的應用這些規則時，可達到快速識別跳轉點的目的，有效提升最優路徑搜尋的效率，顯著提高尋路搜索的整體性能。

2.1 基于“塊”操作

跳點搜索法產生跳點的原因有3個：在當前行檢測到死胡同、在相鄰行找到強制鄰居和檢測到目標節點。首先針對死胡同的檢測，將網格編碼為位矩陣，其中一個位表示一個位置，記錄障礙物信息，指示關聯節點是否可遍歷。當沿著固定的行或列遞歸搜索時，一次性讀取固定設置好的32位輸入，這32位的節點信息賦予算法“遠眺”功能，快速檢測出當前行是否為死胡同，并立即給出是否應當放棄對當前行的進一步操作的指令。

在算法第一次遇到死胡同之前，可能存在“直線-對角線”的轉折點，即在鄰行中出現強制性鄰節點，這時需要綜合當前行、當前行的上一行及下一行3行信息。若在上一行或下一行檢測出前一位置存在障礙物而在當前位置沒有障礙物，則在當前位置上存在潛在的強制鄰居，如圖3(a)所示。但算法也可能出現一直前跳的情況，以設置好的固定長度丈量地圖，可實現對地圖的快速遍歷。

為了避免算法跳過目標節點，將目標位置、當前跳點與下一個跳點的連接，若此路徑與目標位置所在的行或者列存在交集，則在交集位置添加一個中間節點。如圖3(b)所示，當從N跳到S時，可看出路徑穿過目標點所在的列，為避免跳過目標點T，在點T的列上插入一個中間后繼點J。

圖3 基于“塊”操作示例Fig.3 Example of “block” operation based

2.2 剔除中間轉折點

路徑中的轉折點代表著路徑方向發生了改變，即當nk-1到nk的行進方向與nk到nk+1的行進方向不同時，節點nk為轉折點。最優路徑π中轉折點會有以下3種情況：對角線-對角線、直線-對角線、對角線-直線，如圖4所示。

圖4 最優路徑的3種轉折點Fig.4 Three turning points of the optimal path

對于這3種情況需要進一步區分至少有一個強制性鄰居的轉折點和沒有強制性鄰居的轉折點，第1種類型至少緊鄰一個障礙物，如果將其修剪就可能無法返回最優路徑；第2種類型的跳點不緊鄰障礙物，只是一個用來改變方向的中間節點。

對以上3種情況進行分析，首先“對角線-對角線”轉折點：因為π是最優的，所以在緊鄰nk和nk-1的附近必然存在一個障礙物，強制路徑繞行。若不存在障礙物，必然存在dist(nk-1,nk+1)

提出刪除第2種類型的跳點，將其后繼節點存儲在列表中，并且每個新孤立的后繼節點的父節點將成為開始跳轉的起始位置，后繼節點的g并沒有因此而發生改變，在提取具體的路徑時，以“對角優先”的方式從最后路徑中的一個跳點移動到下一個跳點。此策略完全為在線方式，不需要任何特殊的數據結構，也不存儲或計算任何其他信息。

3 仿真實驗驗證及結果分析

為了驗證本文算法的可行性和有效性，將本文算法與傳統A*算法、JPS算法和JPS+算法進行對比，分析定性和定量結果。實驗環境為規則的網格地圖、測試庫基準地圖，計算機配置為Windows7，處理器為AMD A8-4500M，運行內存為4 GB。

3.1 網格地圖仿真實驗

在兩種規格的網格地圖中進行仿真，分別為13×19和30×60。圖5所示為13×19網格地圖下的仿真實驗，障礙物隨機生成，障礙物的平均密度設定約為20%。

綠色帶圓圈柵格表示起始節點；紅色帶星星柵格為目標節點；灰色柵格表示尋路算法在搜索過程中訪問過的節點；藍色折線表示生成的最終路徑。圖5 A*、JPS及改進后的JPS網格地圖仿真實驗結果Fig.5 Grid map simulation experiment results of A*、JPS and improved JPS algorithm

從圖5中可直觀看出，A*搜索過的節點幾乎覆蓋所有網格，搜索量巨大，導致耗時長，實時性差；JPS減少了搜索的節點數量，在搜索過程中識別出跳點，然后直接從一個跳點移動到下一個跳點，并在對稱性路徑中進行對角優先選擇；改進后的JPS進一步減少搜索的節點數量，并一次性讀取固定長度的節點信息，快速識別出當前行是否存在死胡同，若為死胡同則快速舍棄對當前行的操作，并剔除了僅具有改變方向的中間轉折點，加快關鍵跳點的搜尋。

表2所示為30×60網格環境下仿真實驗的數據對比。分析表2數據得知，與傳統A*算法相比，改進后的JPS擴展節點數目縮減了68.9%，搜索耗費時間降低了71.9%，與JPS相比，擴展節點數目縮減了41.3%，搜索耗費時間降低了33.4%。主要在于改進后的JPS通過“塊”操作提高了節點數目查詢的效率，剔除中間轉折點縮減了擴展節點數目，使得最終在返回同等長度最優路徑的前提下，搜索耗費時間下降。

表2 30×60柵格環境下數據對比Table 2 Data comparison in 30×60 grid environment

3.2 基準庫地圖仿真實驗

實驗地圖采用基于網格路徑規劃競賽(grid-based path planning competition，GPPC)[17]中的基準庫地圖，該比賽旨在提供一套標準的地圖，對算法性能進行有意義的比較，已得到IBM Research，University of New South Wales等研究機構的廣泛認可，地圖集可從比賽官網上直接獲得。實驗選用Rooms、Dragon Age Origins及Adaptive depth三類地圖，如圖6、表3所示。

表3 基準庫地圖Table 3 Benchmark sets

為了進行有說服力的算法驗證，將改進后的JPS算法與JPS、JPS+算法進行性能比較，評估了搜索時間和路徑長度。在搜索的過程中重復生成起點到終點之間的最優路徑，最優路徑上每相鄰的兩點搜尋時間至少計算100次，直到這兩點間最優路徑運行時間累加到至少需要5 ms，然后最優路徑平均搜索時間為總時間除以總迭代次數。

JPS+將柵格地圖信息預處理為查詢表，通過查找表格直接獲得路徑中的下一個跳轉點，提出的改進算法是通過“塊”操作方法獲得路徑中的跳點，相較于JPS，以上兩者一個是通過預處理來獲得更快的搜索速度，一個是一次性將修剪規則用于獲得更多的節點信息來加快運行速度。JPS、JPS+與本文算法三者所使用的修剪規則與跳點識別規則是一致的，雖然本文算法剔除了僅具有改變方向的中間轉折點，但保留了后繼點在列表中，這步操作只是減少了擴展節點數目，是加快搜索速度的一部分，所以在每幅測試地圖中3種算法返回的最優路徑的長度是一致的。

抽取實驗中部分數據如圖7所示。圖7中橫坐標表示測試的每幅地圖中最優路徑長度，縱坐標表示所耗費的搜索時間，可看出在每幅地圖中，JPS+都快于在線搜索的JPS和改進的JPS，但本文算法也大幅度提高了搜索速度，且是完全在線的，不需要任何特殊的數據結構，也不存儲或計算任何其他信息，充分表明了本文算法的優越性。

圖7 基準庫地圖下3種算法時間對比Fig.7 Time comparison of three algorithms under the benchmark sets

4 實驗驗證及結果分析

為了驗證改進算法在實際應用中的可行性，在基于機器人操作系統(robot operating system, ROS)的移動機器人Turtlebot2進行真實場景下的實驗，計算機為華碩筆記本(i5-5200),系統為Ubuntu14.04+ROS Indigo版本。該機器人基于差速兩輪驅動，配備了由微軟開發的Kinect深度傳感器作為視覺傳感器，韓國的Yujin Kobuki作為移動基座，如圖8所示。

圖8 TurtleBot 2移動機器人Fig.8 TurtleBot 2 mobile robot

實驗場景為5 m×3 m，障礙物隨機放置，占有率為20%。首先，3D體感相機Kinect獲取外界環境信息，然后調用Gmapping模塊的數據創建地圖，初始掃描設置為20 cm/s的前進速度和20 cm/s的旋轉速度，確保機器人可充分分析環境數據并建立環境地圖，如圖9所示，其中黑色部分被認為是檢測和識別后的障礙物。

圖9 實驗場景與SLAM構建的環境地圖Fig.9 Experimental scene and environment map constructed by SLAM

在上述SLAM構建的環境地圖中進行改進JPS算法的實驗驗證，起點選擇為SLAM地圖的原點，即建圖的起始位置，目標點選為起始點的對角位置，amcl模塊完成機器人自定位，move_base模塊調用改進后的JPS算法，驅動和控制機器人移動到選定的目標，在rviz可視化界面中，點擊2Dpose Esitimate選取地圖中機器人初始位姿，2DNavGoal給定小車在地圖中的目標位置，綠線為機器人規劃出的路徑，實驗過程如圖10所示，實驗結果如圖11所示。

圖10 改進的JPS Turtlebot2路徑規劃過程Fig.10 Path planning process of improved JPS on Turtlebot2

圖11 改進的JPS Turtlebot2路徑規劃結果圖及示意圖Fig.11 Path planning real result of improved JPS on Turtlebot2 and diagrammatic drawing

5 結論

針對JPS搜尋跳點時所涉及大量迭代產生的過大計算量，提出通過“塊”操作方法，在一次搜索中快速掃描底層網格中的一個區域，將JPS中的修剪規則一次應用于多個節點，并在采取對角優先方式的前提下，剔除僅具有改變方向的中間轉折點，提高了單個節點的平均處理時間，達到了快速識別跳轉點的目的，同時保留了與原始算法相同的固有優勢:完整性、最優性。最終實驗結果表明了本文方法的優越性。下一步計劃利用歧路檢測和狀態空間的剪枝算法，如Dead-end heuristic，Swamps或者Portal heuristic算法，在搜索過程中通過識別并忽略無需探測的區域來最優化地到達目標點。