改進A*算法的移動機器人最短路徑規劃

王 維，裴 東，2*，馮 璋

(1．西北師范大學物理與電子工程學院，蘭州730030; 2．甘肅省智能信息技術與應用工程研究中心，蘭州730030)

(*通信作者電子郵箱peidong@nwnu．edu．cn)

0 引言

自主移動機器人能夠通過一些傳感器實現自主移動和完成某種任務，能夠替代人類進行一些作業［1－3］。路徑規劃是機器人完成導航以及其他任務的前提。機器人路徑規劃是指按照一定的指標為機器人搜索出一條從起點到目標點的無碰撞路徑，是自主移動機器人研究的基礎［4］。移動機器人路徑規劃研究非常廣泛，有許多方法來實現，如柵格法［5］、人工勢場法、蟻群算法、模糊邏輯和神經網絡算法［6－7］等。柵格法是對機器人所處環境的一種表示方法，以柵格為單位記錄環境信息，“0”表示無障礙物(地圖上表現為白色)，“1”表示有障礙物(地圖上表現為黑色);柵格劃分越小精確度越高，但占用存儲空間越大，搜索時間指數級增長。人工勢場法是機器人路徑規劃算法中一種簡單有效的方法，但存在諸多問題，當機器人遠離目標點時引力過大，存在碰撞障礙物的危險，在障礙物之間行走時存在振蕩問題，易陷入局部最優值從而使目標不可達。蟻群算法收斂速度慢，容易陷入局部最優值。神經網絡算法的網絡模型參數對規劃器要求高［8］，而且其中的參數只能進行反復的試驗才能得到最優路徑［9］。模糊邏輯方法規劃的機器人運動路徑存在著“對稱無法確定”的現象［10］，盡管文獻［10］和文獻［11］提出了改進的方法，但最終得到的路徑都不是最優路徑。

Dijkstra算法采用遍歷搜索方式，缺點是當節點較多時，節點網絡變得非常龐大，算法效率非常低下［12］。在Dijkstra算法的基礎上，Nilsson在1980年提出了 A*算法，它是一種基于啟發式搜索的全局路徑規劃［13］，缺點是計算效率低、路徑非最優［14］。文獻［15］通過引入父節點的影響提高了A*算法的實時性，但是在對估價函數進行加權時未考慮權重的變化。

對此，本文提出對估價函數進行指數衰減的方式加權，進一步改進A*算法，并對路徑軌跡進行5次多項式平滑處理，提高了算法的實時性，處理后路徑變短變光滑。

1 算法描述

Dijkstra算法和A*算法是運用最廣泛的兩種路徑規劃算法，A*算法是在Dijkstra算法的基礎上提出的，先介紹一下Dijkstra算法和A*算法的原理。

1．1 Dijkstra算法

Dijkstra算法采用貪心算法來解決最短路徑問題，是一種局部最優選擇，在每一次尋找下一個節點時，總是選取到源節點最近的節點作為子節點，直到遍歷整個網絡。因此，其準確性是以計算速度為代價的，對于較大網絡，該算法遍歷整個網絡將耗時非常長。

Dijkstra算法只考慮了起始點到某節點的實際路徑代價，沒有考慮到目標點的影響，屬于盲目的遍歷式搜索。

1．2 A*算法

A*算法是在Dijkstra算法基礎上加入了目標點到當前節點的估計代價，根據地圖上從起始點經過當前節點到達目標點的代價決定搜索的方向，已知條件是全局地圖信息已知，效率較Dijkstra算法大大提高。

A*算法將當前節點的啟發函數定義為:

其中:f(n)為當前節點的估價函數;g(n)是起始點到當前節點的實際路徑代價;h(n)是當前節點到目標點的最小估計代價;h(n)應當滿足不大于當前節點到目標點的實際路徑代價的條件。當估價函數中h(n)=0時，即為Dijkstra算法。h(n)有曼哈頓距離(式(2))、切比雪夫距離(式(3))及歐幾里德距離(式(4))等表示形式:

歐氏距離應用較為廣泛，因此，本文采用該距離。A*算法廣泛運用于解決機器人路徑規劃問題，適用于環境信息已知的全局規劃。

全局路徑規劃可分為3步:地圖構建;根據算法規則選取地圖點;連接這些點形成路徑。A*算法在搜索下一個節點時，有相鄰的4個或8個節點兩種選擇。不同方法所規劃的路徑不同，耗時、路徑長度也不同。

1．3 算法流程

圖1給出A*算法的算法流程，得到初始規劃路徑。傳統A*算法存在搜索節點多、耗費時間長、路徑節點多及轉折角度大的缺點，機器人追蹤此路徑軌跡耗時長且不便于控制。

2 算法改進

2．1 指數加權

A*算法按照傳統的估價函數進行路徑搜索時，會不斷往返搜索，搜索節點過多。實際上，當前節點到目標點的估計路徑代價h(n)的值不大于實際路徑代價值，因此，應該加大啟發函數中h(n)的權重。對此文獻［16］進行了改進，如式(5)，改進后實時性較傳統A*算法有所提高。文獻［15］又加入了父節點對當前節點擴展的影響，如式(6)，大幅減少了往返搜索次數，改進后實時性得到進一步提高。

式中:h(p)是當前節點的父節點到目標點的距離，a為權重。

圖1 A*算法流程Fig．1 Flow chart of A*algorithm

本文考慮到當前節點到目標點的估計代價占實際代價的比重與當前節點的位置有關，當前節點到目標點的估計路徑代價為最小路徑代價，小于實際路徑代價。當節點離目標點較遠時，估計值遠小于實際值，估計值權重應該大一些;當節點逐漸靠近目標點時，估計值逐漸逼近實際值，估計值權重隨之降低;當節點到達目標點時，估計值等于實際值。對此，本文在文獻［15］的基礎上提出指數衰減的方式進行加權，如式(7):當h(n)較大時，權重大，這樣使節點迅速向目標點行進;當h(n)較小時，權重變小;目標點附近時，權重接近1，可保證目標點可達。

仿真實驗如圖2所示，圖(a)為Dijkstra算法搜索結果，有3110個節點;圖(b)為傳統A*算法搜索結果，有2 362個節點;圖(c)為文獻［15］改進算法搜索結果，有1026個節點;圖(d)為本文改進算法搜索結果，只有158個節點，路徑搜索節點數明顯降低，實時性提高效果明顯。

2．2 五次多項式平滑處理

Dijkstra算法和A*算法搜索的初始路徑存在許多冗余點，文獻［17］通過消除兩連通節點之間的節點對路徑進行了平滑處理，存在轉折點。文獻［18］提出了三次Ferguson樣條法，優點是易于實現，缺點是加速度不平滑。文獻［19］提出了“圓弧－直線－圓弧”的方法，缺點是圓弧段加速度不平滑，并且在一些拐點處需要轉一圈，增加了路徑長度。

圖2 幾種算法節點搜索結果對比Fig．2 Comparison of node searching results of several algorithms

本文對路徑中的冗余點進行剔除，偽代碼如下:

BEGIN

path1(end+1，:)← path(1，:);

FOR t←1 to size(path，1)

IF直線lt穿過障礙物 ＆＆直線lt－1不穿過障礙物

(lt表示連接 path1(end，:)、path1(t，:)兩點的直線)

THEN

path1(end+1， :) ← path(t－1，:);

END IF

t←t+1;

END FOR

END

經過對路徑冗余點處理，只保存起點、拐點以及終點，再采用五次多項式進行平滑處理［20］。在滿足初始時刻和T時刻加速度為零的前提下，其加速度變化率也是連續平滑的，即二次函數型，如式(8):

則速度v(t)=s'(t)=5At4+4Bt3+3Ct2+2Dt+E，加速度a(t)=s″(t)=20At3+12Bt2+6Ct+2D，加速度變化率為a'(t)=s(t)=60At2+24Bt+6C，寫成矩陣形式如式(9):

滿足約束條件:s(T)=AT5+BT4+CT3+DT2+ET+F;s(0)=常數;v(0)=v(T)=常數;a(0)=a(T)=0時，解出系數向量(A，B，C，D，E，F)。在給定T時即可畫出s(t)的軌跡。

各算法規劃的最終路徑如圖3所示，通過對比可以看出本文算法轉折角明顯減少，路徑更短、更光滑。

圖3 算法規劃路徑結果對比Fig．3 Comparison of path results of algorithm

表1為Dijkstra算法、傳統A*算法、文獻［15］算法和本文算法實驗數據比較。

表1 幾種算法實驗結果比較Tab．1 Experimental results comparison of several algorithms

可以看出，本文改進算法在實時性和路徑長度方面都有很大改善。本文算法較傳統Dijkstra算法運行時間降低95%、路徑長度降低19．5%;較傳統A*算法運行時間降低94%、路徑長度降低19．5%;較文獻［15］算法運行時間降低85%、路徑長度降低23．9%。

為了驗證本文算法的可靠性，在簡單室內環境一和隨機地圖環境二下對算法進行驗證，如圖4所示。

圖4 本文算法驗證結果Fig．4 Verification results of the proposed algorithm

2．3 實驗結果

本仿真實驗采用微軟 Core i5 6500處理器，主頻3．2 GHz，內存4 GB。在Matlab2014b環境下，為了驗證算法的可行性，除文獻［15］采用的環境外，再通過環境一、環境二對改進算法進行驗證，得出本文改進A*算法的訪問節點數、運行時間以及路徑長度數據，如表2所示。

表2 本文算法測試實驗結果Tab．2 Test results of the proposed algorithm

3 結語

在環境信息已知的柵格環境中，本文在已有的改進基礎上，對A*算法提出了進一步改進，在不同環境下，本文算法在耗費時間上大大減少，路徑更短且得到了平滑處理，便于機器人不間斷地跟蹤路徑軌跡到達目標點。對本文改進算法的仿真結果與Dijkstra算法、傳統A*算法、文獻［15］改進的A*算法進行了比較，結果證明了本文算法的正確性、合理性、很好的實時性以及復雜環境下的可靠性。