王 維，裴 東，2*，馮 璋

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

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

0 引言

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

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

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

1 算法描述

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

1．1 Dijkstra算法

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

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

1．2 A*算法

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

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

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

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

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

1．3 算法流程

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

2 算法改進(jìn)

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

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

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

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

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

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

2．2 五次多項(xiàng)式平滑處理

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

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

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

BEGIN

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

FOR t←1 to size(path，1)

IF直線lt穿過(guò)障礙物 ＆＆直線lt－1不穿過(guò)障礙物

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

THEN

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

END IF

t←t+1;

END FOR

END

經(jīng)過(guò)對(duì)路徑冗余點(diǎn)處理，只保存起點(diǎn)、拐點(diǎn)以及終點(diǎn)，再采用五次多項(xiàng)式進(jìn)行平滑處理［20］。在滿足初始時(shí)刻和T時(shí)刻加速度為零的前提下，其加速度變化率也是連續(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，寫(xiě)成矩陣形式如式(9):

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

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

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

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

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

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

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

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

2．3 實(shí)驗(yàn)結(jié)果

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

表2 本文算法測(cè)試實(shí)驗(yàn)結(jié)果Tab．2 Test results of the proposed algorithm

3 結(jié)語(yǔ)

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