改進(jìn)A*算法的移動機(jī)器人最短路徑規(guī)劃

王 維，裴 東，2*，馮 璋

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

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

0 引言

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

Dijkstra算法采用遍歷搜索方式，缺點是當(dāng)節(jié)點較多時，節(jié)點網(wǎng)絡(luò)變得非常龐大，算法效率非常低下［12］。在Dijkstra算法的基礎(chǔ)上，Nilsson在1980年提出了 A*算法，它是一種基于啟發(fā)式搜索的全局路徑規(guī)劃［13］，缺點是計算效率低、路徑非最優(yōu)［14］。文獻(xiàn)［15］通過引入父節(jié)點的影響提高了A*算法的實時性，但是在對估價函數(shù)進(jìn)行加權(quán)時未考慮權(quán)重的變化。

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

1 算法描述

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

1．1 Dijkstra算法

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

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

1．2 A*算法

A*算法是在Dijkstra算法基礎(chǔ)上加入了目標(biāo)點到當(dāng)前節(jié)點的估計代價，根據(jù)地圖上從起始點經(jīng)過當(dāng)前節(jié)點到達(dá)目標(biāo)點的代價決定搜索的方向，已知條件是全局地圖信息已知，效率較Dijkstra算法大大提高。

A*算法將當(dāng)前節(jié)點的啟發(fā)函數(shù)定義為:

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

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

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

1．3 算法流程

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

2 算法改進(jìn)

2．1 指數(shù)加權(quán)

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

式中:h(p)是當(dāng)前節(jié)點的父節(jié)點到目標(biāo)點的距離，a為權(quán)重。

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

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

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

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

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

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

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

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

經(jīng)過對路徑冗余點處理，只保存起點、拐點以及終點，再采用五次多項式進(jìn)行平滑處理［20］。在滿足初始時刻和T時刻加速度為零的前提下，其加速度變化率也是連續(xù)平滑的，即二次函數(shù)型，如式(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)=常數(shù);v(0)=v(T)=常數(shù);a(0)=a(T)=0時，解出系數(shù)向量(A，B，C，D，E，F(xiàn))。在給定T時即可畫出s(t)的軌跡。

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

圖3 算法規(guī)劃路徑結(jié)果對比Fig．3 Comparison of path results of algorithm

表1為Dijkstra算法、傳統(tǒng)A*算法、文獻(xiàn)［15］算法和本文算法實驗數(shù)據(jù)比較。

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

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

為了驗證本文算法的可靠性，在簡單室內(nèi)環(huán)境一和隨機(jī)地圖環(huán)境二下對算法進(jìn)行驗證，如圖4所示。

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

2．3 實驗結(jié)果

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

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

3 結(jié)語

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