宋曉茹， 任怡悅

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

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

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

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

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

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

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

1 跳點搜索算法(JPS)

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

1.1 修剪規(guī)則

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

圖1 修剪規(guī)則示意圖Fig.1 Diagram of pruning rule

1.2 跳點識別規(guī)則

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

(1)節(jié)點y目標(biāo)點。

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

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

跳點識別規(guī)則示意圖如圖2所示。

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

2 改進的跳點搜索算法

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

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

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

2.1 基于“塊”操作

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

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

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

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

2.2 剔除中間轉(zhuǎn)折點

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

圖4 最優(yōu)路徑的3種轉(zhuǎn)折點Fig.4 Three turning points of the optimal path

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

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

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

3 仿真實驗驗證及結(jié)果分析

為了驗證本文算法的可行性和有效性，將本文算法與傳統(tǒng)A*算法、JPS算法和JPS+算法進行對比，分析定性和定量結(jié)果。實驗環(huán)境為規(guī)則的網(wǎng)格地圖、測試庫基準(zhǔn)地圖，計算機配置為Windows7，處理器為AMD A8-4500M，運行內(nèi)存為4 GB。

3.1 網(wǎng)格地圖仿真實驗

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

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

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

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

表2 30×60柵格環(huán)境下數(shù)據(jù)對比Table 2 Data comparison in 30×60 grid environment

3.2 基準(zhǔn)庫地圖仿真實驗

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

表3 基準(zhǔn)庫地圖Table 3 Benchmark sets

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

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

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

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

4 實驗驗證及結(jié)果分析

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

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

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

圖9 實驗場景與SLAM構(gòu)建的環(huán)境地圖Fig.9 Experimental scene and environment map constructed by SLAM

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

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

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

5 結(jié)論

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