王 維，裴 東，2*，馮 璋

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

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

0 引言

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

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

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

1 算法描述

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

1．1 Dijkstra算法

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

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

1．2 A*算法

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

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

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

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

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

1．3 算法流程

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

2 算法改進

2．1 指數(shù)加權

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

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

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

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

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

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

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

圖2 幾種算法節(jié)點搜索結果對比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

經(jīng)過對路徑冗余點處理，只保存起點、拐點以及終點，再采用五次多項式進行平滑處理［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所示，通過對比可以看出本文算法轉折角明顯減少，路徑更短、更光滑。

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

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

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

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

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

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

2．3 實驗結果

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

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

3 結語

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