王曉燕，呂金豆

（西安建筑科技大學(xué)，西安 710000）

0 引言

路徑規(guī)劃問(wèn)題一直是機(jī)器人研究領(lǐng)域的一個(gè)重點(diǎn)問(wèn)題，其目的是在含有障礙物的環(huán)境中，尋找一條從起始位置到達(dá)目標(biāo)位置的無(wú)碰撞路徑[1]。在一個(gè)含有動(dòng)態(tài)障礙物的環(huán)境中對(duì)機(jī)器人進(jìn)行路徑規(guī)劃是當(dāng)下的一類研究熱點(diǎn)[2]。

而障礙已知的環(huán)境中，障礙又分為靜態(tài)障礙和動(dòng)態(tài)障礙，動(dòng)態(tài)障礙一般使用的算法如：滾動(dòng)路徑規(guī)劃法[3]、人工勢(shì)場(chǎng)法、蟻群算法[4]、遺傳算法[5]、D*算法[6]等。人工勢(shì)場(chǎng)法（Artificial potential field method，APF）由Khatib于 1986 年提出的[7]，它的基本原理是：將移動(dòng)機(jī)器人在一定環(huán)境中的運(yùn)動(dòng)虛擬為一種在抽象的人造勢(shì)力場(chǎng)中的運(yùn)動(dòng)，目標(biāo)點(diǎn)和機(jī)器人間產(chǎn)生引力，力的大小與兩者距離成正比；障礙物和機(jī)器人間產(chǎn)生斥力，力的大小與兩者距離成反比。通過(guò)求合力來(lái)對(duì)移動(dòng)機(jī)器人的運(yùn)動(dòng)進(jìn)行控制。

在尋路問(wèn)題上，作為一種較為完善的方法，人工勢(shì)場(chǎng)法以其局部實(shí)時(shí)性、平滑安全性等特點(diǎn)，常被用于含動(dòng)態(tài)障礙物的避障算法中，但其明顯缺少全局搜索能力，較易出現(xiàn)局部最優(yōu)解、目標(biāo)點(diǎn)不可達(dá)等停滯問(wèn)題[8]。而A*算法[9]以其全局最優(yōu)性、完備性和高效性時(shí)常用于全局最優(yōu)路徑規(guī)劃中，是一種典型的靜態(tài)路徑全局規(guī)劃算法，但其無(wú)法做到實(shí)時(shí)避障，不適于含有動(dòng)態(tài)障礙物的環(huán)境。

本文的主要思路是在含動(dòng)態(tài)障礙物的模擬環(huán)境中，對(duì)傳統(tǒng)人工勢(shì)場(chǎng)法改進(jìn)并與A*算法相結(jié)合，規(guī)劃出一條移動(dòng)機(jī)器人由起始點(diǎn)到目標(biāo)點(diǎn)的無(wú)碰撞路徑。當(dāng)未進(jìn)入動(dòng)態(tài)障礙物影響范圍ρ0時(shí)，移動(dòng)機(jī)器人按照A*算法所規(guī)劃的路徑行使；進(jìn)入影響范圍ρ0時(shí)，采用改進(jìn)人工勢(shì)場(chǎng)法進(jìn)行實(shí)時(shí)動(dòng)態(tài)避障。以此來(lái)彌補(bǔ)傳統(tǒng)人工勢(shì)場(chǎng)法的靜態(tài)陷阱和震蕩問(wèn)題，并有效減小路徑長(zhǎng)度。

1 基本算法原理描述

1.1 人工勢(shì)場(chǎng)法

傳統(tǒng)人工勢(shì)場(chǎng)法是在建模后的環(huán)境中假設(shè)有虛擬的引力場(chǎng)和斥力場(chǎng)存在，二者的力的作用平衡使得移動(dòng)機(jī)器人能夠躲避障礙。對(duì)人工勢(shì)場(chǎng)法進(jìn)行如下定義：

機(jī)器人R的當(dāng)前方位為X=（x，y），目標(biāo)位置G為Xg=（xg，yg），式(1)為機(jī)器人與目標(biāo)點(diǎn)之間的引力場(chǎng)：

由該引力場(chǎng)所生成的引力：

其中Katt是引力增益系數(shù)，|X-Xg|是機(jī)器人與目標(biāo)點(diǎn)的直線距離。機(jī)器人與障礙物之間的斥力場(chǎng)如下式：

由斥力場(chǎng)所生成的斥力：

其中Krep是斥力增益系數(shù)，Xobs是障礙物的位置，|X-Xobs|是機(jī)器人與障礙物的直線距離，ρ0是障礙物的影響距離。

綜合上式，分別可得移動(dòng)機(jī)器人在運(yùn)動(dòng)空間中的合勢(shì)場(chǎng)及合力：

1.2 A＊算法

A*算法是一種經(jīng)典的啟發(fā)式算法，能夠在靜態(tài)環(huán)境中高效地求出從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑[10]。將其應(yīng)用于移動(dòng)機(jī)器人全局路徑規(guī)劃，其原理可簡(jiǎn)述為：將移動(dòng)機(jī)器人的運(yùn)動(dòng)環(huán)境分割為若干相同的節(jié)點(diǎn)，創(chuàng)建兩張表OPEN LIST與CLOSED LIST，CLOSED表用來(lái)記錄已訪問(wèn)過(guò)的節(jié)點(diǎn)，OPEN表用來(lái)保存所有已生成而未檢查的節(jié)點(diǎn)。

首先進(jìn)行初始化，將起始點(diǎn)START放入OPEN表中，CLOSE表清空；

算法開(kāi)始，從OPEN表內(nèi)取第一個(gè)節(jié)點(diǎn)n，若n是目標(biāo)解則繼續(xù)尋找或終止算法，若不是，則按一定規(guī)則展開(kāi)與n點(diǎn)相關(guān)的子節(jié)點(diǎn)，并將非障礙物、地圖邊緣的子節(jié)點(diǎn)以及n放入CLOSE表，其他放入OPEN表；

計(jì)算每一個(gè)子節(jié)點(diǎn)的估計(jì)值f（n），將OPEN表按照f(shuō)（n）由小到大排序；

重復(fù)上述過(guò)程直至目標(biāo)點(diǎn)。保存這些f(n)最小的節(jié)點(diǎn)，將它們沿目標(biāo)點(diǎn)到起始點(diǎn)連接，便得到A*算法計(jì)算出的全局最優(yōu)路徑。

估價(jià)函數(shù)為：

式中，f（n）表示起始節(jié)點(diǎn)經(jīng)由節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的估計(jì)代價(jià)值；g（n）表示在狀態(tài)空間中從起始節(jié)點(diǎn)到節(jié)點(diǎn)n的實(shí)際代價(jià)值；h（n）表示節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的最小路徑的估計(jì)代價(jià)值。

2 改進(jìn)A＊?jiǎng)輬?chǎng)算法

2.1 改進(jìn)依據(jù)

本文采用A*算法進(jìn)行移動(dòng)機(jī)器人的全局路徑規(guī)劃，改進(jìn)勢(shì)場(chǎng)法實(shí)現(xiàn)局部在線規(guī)劃。利用MATLAB R2014a軟件對(duì)兩種基本算法進(jìn)行仿真，通過(guò)大量的仿真與分析，驗(yàn)證了在靜態(tài)環(huán)境中，A*算法較人工勢(shì)場(chǎng)法全局路徑規(guī)劃在軌跡震蕩和路徑長(zhǎng)度問(wèn)題上的表現(xiàn)更加優(yōu)越。

選取如圖1所示位置隨機(jī)的10個(gè)障礙物，用以兩種算法仿真普遍結(jié)果的比對(duì)。從圖中可以看出，當(dāng)運(yùn)動(dòng)環(huán)境相同時(shí)，采用A*算法進(jìn)行全局路徑規(guī)劃能夠高效減少軌跡轉(zhuǎn)折次數(shù)，消除震蕩，并解決了人工勢(shì)場(chǎng)法目標(biāo)點(diǎn)不可達(dá)的問(wèn)題。

圖1 A*算法與人工勢(shì)場(chǎng)法全局靜態(tài)路徑

表1 A＊算法與人工勢(shì)場(chǎng)法仿真結(jié)果對(duì)比

由表1可知在相同環(huán)境進(jìn)行全局路徑規(guī)劃時(shí)，人工勢(shì)場(chǎng)法路線較長(zhǎng)，累計(jì)轉(zhuǎn)折角遠(yuǎn)大于A*算法，并且出現(xiàn)了震蕩、轉(zhuǎn)折次數(shù)多、規(guī)劃路徑不平滑等現(xiàn)象。A*算法路線轉(zhuǎn)折次數(shù)少，累計(jì)轉(zhuǎn)折角度遠(yuǎn)小于人工勢(shì)場(chǎng)法且路徑較平滑。因此本文選用A*算法對(duì)已知環(huán)境進(jìn)行全局路徑規(guī)劃，人工勢(shì)場(chǎng)法作為局部規(guī)劃算法。

2.2 引入速度勢(shì)場(chǎng)模型

2.2.1 速度斥力場(chǎng)

針對(duì)動(dòng)態(tài)環(huán)境，本文對(duì)傳統(tǒng)人工勢(shì)場(chǎng)法的斥力勢(shì)場(chǎng)進(jìn)行了改進(jìn)，即引入相對(duì)速度斥力勢(shì)場(chǎng)。對(duì)局部動(dòng)態(tài)避碰問(wèn)題做了如下簡(jiǎn)化：已知機(jī)器人和障礙物的運(yùn)動(dòng)狀態(tài)、兩者的相對(duì)位置矢量以及相對(duì)速度矢量；視機(jī)器人與障礙物為質(zhì)點(diǎn)；移動(dòng)機(jī)器人與障礙物都具有全方位的運(yùn)動(dòng)能力。

記機(jī)器人剛好進(jìn)入障礙物影響范圍的時(shí)刻為t，圖2為t時(shí)刻移動(dòng)機(jī)器人與障礙物的運(yùn)動(dòng)狀態(tài)，此時(shí)生成虛擬目標(biāo)點(diǎn)G，以速度沿A*算法路徑移動(dòng)，并與移動(dòng)機(jī)器人速度重合，障礙物以速度作直線運(yùn)動(dòng)駛向機(jī)器人。

圖2 t時(shí)刻移動(dòng)機(jī)器人與障礙物運(yùn)動(dòng)狀態(tài)

設(shè)定速度勢(shì)場(chǎng)為：

2.2.2 速度引力場(chǎng)

機(jī)器人駛離動(dòng)態(tài)障礙物后需回到原A*路徑上，因此位于A*路徑上的虛擬目標(biāo)點(diǎn)G是動(dòng)態(tài)的，由t時(shí)刻產(chǎn)生，直到移動(dòng)機(jī)器人速度與其再次重合時(shí)消失。圖3為期間任意時(shí)刻t'移動(dòng)機(jī)器人與障礙物的運(yùn)動(dòng)圖示。建立虛擬目標(biāo)點(diǎn)G的引力勢(shì)場(chǎng)模型如式(9)所示，該模型由相對(duì)位置和相對(duì)速度兩部分勢(shì)場(chǎng)函數(shù)構(gòu)成：

圖3 t'時(shí)刻移動(dòng)機(jī)器人與障礙物運(yùn)動(dòng)狀態(tài)

結(jié)合上述公式，可求得引入速度勢(shì)場(chǎng)后機(jī)器人所受的合力：

當(dāng)移動(dòng)機(jī)器人進(jìn)入障礙物影響范圍ρ0，且兩者運(yùn)動(dòng)方向相靠近時(shí)，機(jī)器人所受總合力由綜合引力Fatt、斥力Frep及速度斥力Frv構(gòu)成；當(dāng)移動(dòng)機(jī)器人進(jìn)入障礙物影響范圍，但兩者的運(yùn)動(dòng)方向相背離時(shí)，機(jī)器人所受總合力由綜合引力Fatt及斥力Frep構(gòu)成；當(dāng)移動(dòng)機(jī)器人在影響范圍之外，總合力為0。

2.3 改進(jìn)算法具體步驟

圖4所示為算法流程圖，改進(jìn)算法具體步驟為：

步驟1：初始化參數(shù)。選擇起始位置xStart、yStart，目標(biāo)位置xTarget、yTarget，創(chuàng)建列表OPEN、CLOSED，將起始節(jié)點(diǎn)設(shè)為第一個(gè)節(jié)點(diǎn)。

步驟2：初始化參數(shù)。設(shè)置人工勢(shì)場(chǎng)法步長(zhǎng)l，循環(huán)迭代次數(shù)J，增益系數(shù)Katt、Krep，速度斥力常量λrv，速度勢(shì)場(chǎng)增益系數(shù)Kattp和Kattv等其他參數(shù)的初始化。

步驟3：A*算法路徑規(guī)劃。將起始節(jié)點(diǎn)放入關(guān)閉列表CLOSED，更新后續(xù)節(jié)點(diǎn)放入打開(kāi)列表OPEN，對(duì)后續(xù)節(jié)點(diǎn)做最小f（n）檢查并選取。遍歷列表，通過(guò)最后一個(gè)節(jié)點(diǎn)（如果它是目標(biāo)節(jié)點(diǎn)）開(kāi)始，然后識(shí)別它的交節(jié)點(diǎn)，直到它到達(dá)起始節(jié)點(diǎn)，生成靜態(tài)最優(yōu)路徑。

步驟4：改進(jìn)勢(shì)場(chǎng)法動(dòng)態(tài)避障。進(jìn)入ρ0后，勢(shì)場(chǎng)法開(kāi)始運(yùn)行，虛擬目標(biāo)點(diǎn)沿步驟3）生成路徑以運(yùn)動(dòng)。移動(dòng)機(jī)器人依據(jù)式(8)進(jìn)行避碰，依據(jù)式(9)回到步驟3）生成路徑，使得保存機(jī)器人走過(guò)的每個(gè)坐標(biāo)，生成路徑。

步驟5：輸出最優(yōu)路徑。將步驟4中求出的動(dòng)態(tài)路徑與步驟3）求出的靜態(tài)路徑相結(jié)合，可得出本文全局動(dòng)態(tài)路徑規(guī)劃最優(yōu)路徑。

圖4 算法流程

3 實(shí)驗(yàn)研究與仿真分析

將本文的改進(jìn)A*及勢(shì)場(chǎng)算法應(yīng)用于機(jī)器人動(dòng)態(tài)路徑規(guī)劃問(wèn)題求解，為驗(yàn)證本文算法的可行性和有效性，進(jìn)行大量仿真實(shí)驗(yàn)，并通過(guò)與傳統(tǒng)人工勢(shì)場(chǎng)法進(jìn)行比對(duì)，驗(yàn)證本文算法的優(yōu)越性。算法運(yùn)行環(huán)為：Windows10 64bit，仿真軟件MATLAB R2014a。

為了驗(yàn)證改進(jìn)算法動(dòng)態(tài)避障的可行性，選取靜態(tài)障礙物位置同圖1。設(shè)改進(jìn)勢(shì)場(chǎng)法初始參數(shù)引力場(chǎng)常量Katt=2，速度引力常量Kattv=2，斥力場(chǎng)常量Krep=1，速度斥力常量λrv=1，斥力影響范圍ρ0=1.5，步長(zhǎng)l=0.1，步長(zhǎng)設(shè)置不同，每一次循環(huán)迭代機(jī)器人移動(dòng)的距離不同。選取四個(gè)不同循環(huán)迭代次數(shù)下，移動(dòng)機(jī)器人的實(shí)時(shí)路徑，如圖5所示。其中實(shí)心方塊為動(dòng)態(tài)障礙物，沿y軸負(fù)方向作速度為0.1/s的勻速直線運(yùn)動(dòng)，空心方框?yàn)殪o態(tài)障礙物，粗實(shí)線為A*算法路徑，細(xì)實(shí)線為改進(jìn)勢(shì)場(chǎng)法路徑。

圖5(a)表示改進(jìn)算法開(kāi)始，機(jī)器人剛好進(jìn)入移動(dòng)障礙物影響范圍，移動(dòng)機(jī)器人開(kāi)始局部路徑規(guī)劃；圖5(b)、圖5(c)分別表示第10步與第18步，移動(dòng)機(jī)器人基于改進(jìn)勢(shì)場(chǎng)法進(jìn)行動(dòng)態(tài)避障；圖5(d)表示機(jī)器人在第25步完成避碰要求，開(kāi)始追蹤原路徑。

圖5 改進(jìn)算法動(dòng)態(tài)避障實(shí)時(shí)路徑

當(dāng)機(jī)器人行駛至第36步，即循環(huán)迭代次數(shù)J=36時(shí)，移動(dòng)機(jī)器人追蹤到虛擬目標(biāo)點(diǎn)，并繼續(xù)沿A*算法規(guī)劃的原路徑繼續(xù)行使至目標(biāo)點(diǎn)，路徑規(guī)劃結(jié)束。圖5驗(yàn)證了基于本文改進(jìn)算法的移動(dòng)機(jī)器人對(duì)動(dòng)態(tài)路徑規(guī)劃的可行性。

將移動(dòng)機(jī)器人走過(guò)的位置相連，得到最終全局規(guī)劃路徑，如圖6(a)所示，與人工勢(shì)場(chǎng)法路徑圖6(b)相比，可以看出本文算法改善了動(dòng)態(tài)避障中人工勢(shì)場(chǎng)法的震蕩問(wèn)題，使得路徑更加平滑，轉(zhuǎn)折次數(shù)更少。對(duì)比改進(jìn)算法與人工勢(shì)場(chǎng)法仿真數(shù)據(jù)，如表2所示。

圖6 改進(jìn)算法與人工勢(shì)場(chǎng)法全局動(dòng)態(tài)路徑規(guī)劃最優(yōu)路徑

表2 兩種算法仿真結(jié)果對(duì)比

通過(guò)表2可得出，本文算法較人工勢(shì)場(chǎng)法規(guī)劃路徑長(zhǎng)度減少3.1712，累計(jì)轉(zhuǎn)折角度減少12.5574rad，運(yùn)行時(shí)間相當(dāng)。

4 結(jié)語(yǔ)

本文將A*算法與改進(jìn)的人工勢(shì)場(chǎng)法相結(jié)合，提出了一種改進(jìn)的動(dòng)態(tài)避碰策略。利用MATLAB仿真軟件，模擬在含有動(dòng)態(tài)障礙物的環(huán)境中，對(duì)移動(dòng)機(jī)器人從起始點(diǎn)至目標(biāo)點(diǎn)進(jìn)行了全局的動(dòng)態(tài)路徑規(guī)劃，同時(shí)達(dá)到避碰要求。

1）該算法利用A*算法求得的初始路徑作為全局最優(yōu)路徑，改進(jìn)勢(shì)場(chǎng)法執(zhí)行動(dòng)態(tài)避障，兩者結(jié)合轉(zhuǎn)換，使得傳統(tǒng)人工勢(shì)場(chǎng)法動(dòng)態(tài)路徑規(guī)劃中目標(biāo)點(diǎn)不可達(dá)的問(wèn)題得到了解決，并消除震蕩現(xiàn)象。

2）針對(duì)局部動(dòng)態(tài)避障問(wèn)題，提出了引入速度斥力場(chǎng)對(duì)障礙物的運(yùn)動(dòng)進(jìn)行判斷；當(dāng)移動(dòng)機(jī)器人駛離障礙物影響范圍時(shí)，提出并解決了“動(dòng)態(tài)追蹤”問(wèn)題，引入速度引力場(chǎng)，使機(jī)器人駛回原A*規(guī)劃的路線，簡(jiǎn)化了結(jié)合算法的規(guī)劃策略。

3）改進(jìn)算法對(duì)動(dòng)態(tài)障礙物進(jìn)行規(guī)劃，與全局勢(shì)場(chǎng)法規(guī)劃相比，最優(yōu)路徑長(zhǎng)度降低約20.4%，累計(jì)轉(zhuǎn)折角度減少約86.6%。