王曉燕,呂金豆
(西安建筑科技大學(xué),西安 710000)
路徑規(guī)劃問題一直是機(jī)器人研究領(lǐng)域的一個(gè)重點(diǎn)問題,其目的是在含有障礙物的環(huán)境中,尋找一條從起始位置到達(dá)目標(biāo)位置的無碰撞路徑[1]。在一個(gè)含有動(dòng)態(tài)障礙物的環(huán)境中對機(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]、人工勢場法、蟻群算法[4]、遺傳算法[5]、D*算法[6]等。人工勢場法(Artificial potential field method,APF)由Khatib于 1986 年提出的[7],它的基本原理是:將移動(dòng)機(jī)器人在一定環(huán)境中的運(yùn)動(dòng)虛擬為一種在抽象的人造勢力場中的運(yùn)動(dòng),目標(biāo)點(diǎn)和機(jī)器人間產(chǎn)生引力,力的大小與兩者距離成正比;障礙物和機(jī)器人間產(chǎn)生斥力,力的大小與兩者距離成反比。通過求合力來對移動(dòng)機(jī)器人的運(yùn)動(dòng)進(jìn)行控制。
在尋路問題上,作為一種較為完善的方法,人工勢場法以其局部實(shí)時(shí)性、平滑安全性等特點(diǎn),常被用于含動(dòng)態(tài)障礙物的避障算法中,但其明顯缺少全局搜索能力,較易出現(xiàn)局部最優(yōu)解、目標(biāo)點(diǎn)不可達(dá)等停滯問題[8]。而A*算法[9]以其全局最優(yōu)性、完備性和高效性時(shí)常用于全局最優(yōu)路徑規(guī)劃中,是一種典型的靜態(tài)路徑全局規(guī)劃算法,但其無法做到實(shí)時(shí)避障,不適于含有動(dòng)態(tài)障礙物的環(huán)境。
本文的主要思路是在含動(dòng)態(tài)障礙物的模擬環(huán)境中,對傳統(tǒng)人工勢場法改進(jìn)并與A*算法相結(jié)合,規(guī)劃出一條移動(dòng)機(jī)器人由起始點(diǎn)到目標(biāo)點(diǎn)的無碰撞路徑。當(dāng)未進(jìn)入動(dòng)態(tài)障礙物影響范圍ρ0時(shí),移動(dòng)機(jī)器人按照A*算法所規(guī)劃的路徑行使;進(jìn)入影響范圍ρ0時(shí),采用改進(jìn)人工勢場法進(jìn)行實(shí)時(shí)動(dòng)態(tài)避障。以此來彌補(bǔ)傳統(tǒng)人工勢場法的靜態(tài)陷阱和震蕩問題,并有效減小路徑長度。
傳統(tǒng)人工勢場法是在建模后的環(huán)境中假設(shè)有虛擬的引力場和斥力場存在,二者的力的作用平衡使得移動(dòng)機(jī)器人能夠躲避障礙。對人工勢場法進(jìn)行如下定義:
機(jī)器人R的當(dāng)前方位為X=(x,y),目標(biāo)位置G為Xg=(xg,yg),式(1)為機(jī)器人與目標(biāo)點(diǎn)之間的引力場:
由該引力場所生成的引力:
其中Katt是引力增益系數(shù),|X-Xg|是機(jī)器人與目標(biāo)點(diǎn)的直線距離。機(jī)器人與障礙物之間的斥力場如下式:
由斥力場所生成的斥力:
其中Krep是斥力增益系數(shù),Xobs是障礙物的位置,|X-Xobs|是機(jī)器人與障礙物的直線距離,ρ0是障礙物的影響距離。
綜合上式,分別可得移動(dòng)機(jī)器人在運(yùn)動(dòng)空間中的合勢場及合力:
A*算法是一種經(jīng)典的啟發(fā)式算法,能夠在靜態(tài)環(huán)境中高效地求出從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑[10]。將其應(yīng)用于移動(dòng)機(jī)器人全局路徑規(guī)劃,其原理可簡述為:將移動(dòng)機(jī)器人的運(yùn)動(dòng)環(huán)境分割為若干相同的節(jié)點(diǎn),創(chuàng)建兩張表OPEN LIST與CLOSED LIST,CLOSED表用來記錄已訪問過的節(jié)點(diǎn),OPEN表用來保存所有已生成而未檢查的節(jié)點(diǎn)。
首先進(jìn)行初始化,將起始點(diǎn)START放入OPEN表中,CLOSE表清空;
算法開始,從OPEN表內(nèi)取第一個(gè)節(jié)點(diǎn)n,若n是目標(biāo)解則繼續(xù)尋找或終止算法,若不是,則按一定規(guī)則展開與n點(diǎn)相關(guān)的子節(jié)點(diǎn),并將非障礙物、地圖邊緣的子節(jié)點(diǎn)以及n放入CLOSE表,其他放入OPEN表;
計(jì)算每一個(gè)子節(jié)點(diǎn)的估計(jì)值f(n),將OPEN表按照f(n)由小到大排序;
重復(fù)上述過程直至目標(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à)值。
本文采用A*算法進(jìn)行移動(dòng)機(jī)器人的全局路徑規(guī)劃,改進(jìn)勢場法實(shí)現(xiàn)局部在線規(guī)劃。利用MATLAB R2014a軟件對兩種基本算法進(jìn)行仿真,通過大量的仿真與分析,驗(yàn)證了在靜態(tài)環(huán)境中,A*算法較人工勢場法全局路徑規(guī)劃在軌跡震蕩和路徑長度問題上的表現(xiàn)更加優(yōu)越。
選取如圖1所示位置隨機(jī)的10個(gè)障礙物,用以兩種算法仿真普遍結(jié)果的比對。從圖中可以看出,當(dāng)運(yùn)動(dòng)環(huán)境相同時(shí),采用A*算法進(jìn)行全局路徑規(guī)劃能夠高效減少軌跡轉(zhuǎn)折次數(shù),消除震蕩,并解決了人工勢場法目標(biāo)點(diǎn)不可達(dá)的問題。
圖1 A*算法與人工勢場法全局靜態(tài)路徑
表1 A*算法與人工勢場法仿真結(jié)果對比
由表1可知在相同環(huán)境進(jìn)行全局路徑規(guī)劃時(shí),人工勢場法路線較長,累計(jì)轉(zhuǎn)折角遠(yuǎn)大于A*算法,并且出現(xiàn)了震蕩、轉(zhuǎn)折次數(shù)多、規(guī)劃路徑不平滑等現(xiàn)象。A*算法路線轉(zhuǎn)折次數(shù)少,累計(jì)轉(zhuǎn)折角度遠(yuǎn)小于人工勢場法且路徑較平滑。因此本文選用A*算法對已知環(huán)境進(jìn)行全局路徑規(guī)劃,人工勢場法作為局部規(guī)劃算法。
2.2.1 速度斥力場
針對動(dòng)態(tài)環(huán)境,本文對傳統(tǒng)人工勢場法的斥力勢場進(jìn)行了改進(jìn),即引入相對速度斥力勢場。對局部動(dòng)態(tài)避碰問題做了如下簡化:已知機(jī)器人和障礙物的運(yùn)動(dòng)狀態(tài)、兩者的相對位置矢量以及相對速度矢量;視機(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è)定速度勢場為:
2.2.2 速度引力場
機(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的引力勢場模型如式(9)所示,該模型由相對位置和相對速度兩部分勢場函數(shù)構(gòu)成:
圖3 t'時(shí)刻移動(dòng)機(jī)器人與障礙物運(yùn)動(dòng)狀態(tài)
結(jié)合上述公式,可求得引入速度勢場后機(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。
圖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è)置人工勢場法步長l,循環(huán)迭代次數(shù)J,增益系數(shù)Katt、Krep,速度斥力常量λrv,速度勢場增益系數(shù)Kattp和Kattv等其他參數(shù)的初始化。
步驟3:A*算法路徑規(guī)劃。將起始節(jié)點(diǎn)放入關(guān)閉列表CLOSED,更新后續(xù)節(jié)點(diǎn)放入打開列表OPEN,對后續(xù)節(jié)點(diǎn)做最小f(n)檢查并選取。遍歷列表,通過最后一個(gè)節(jié)點(diǎn)(如果它是目標(biāo)節(jié)點(diǎn))開始,然后識別它的交節(jié)點(diǎn),直到它到達(dá)起始節(jié)點(diǎn),生成靜態(tài)最優(yōu)路徑。
步驟4:改進(jìn)勢場法動(dòng)態(tài)避障。進(jìn)入ρ0后,勢場法開始運(yùn)行,虛擬目標(biāo)點(diǎn)沿步驟3)生成路徑以運(yùn)動(dòng)。移動(dòng)機(jī)器人依據(jù)式(8)進(jìn)行避碰,依據(jù)式(9)回到步驟3)生成路徑,使得保存機(jī)器人走過的每個(gè)坐標(biāo),生成路徑。
步驟5:輸出最優(yōu)路徑。將步驟4中求出的動(dòng)態(tài)路徑與步驟3)求出的靜態(tài)路徑相結(jié)合,可得出本文全局動(dòng)態(tài)路徑規(guī)劃最優(yōu)路徑。
圖4 算法流程
將本文的改進(jìn)A*及勢場算法應(yīng)用于機(jī)器人動(dòng)態(tài)路徑規(guī)劃問題求解,為驗(yàn)證本文算法的可行性和有效性,進(jìn)行大量仿真實(shí)驗(yàn),并通過與傳統(tǒng)人工勢場法進(jìn)行比對,驗(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ù)引力場常量Katt=2,速度引力常量Kattv=2,斥力場常量Krep=1,速度斥力常量λrv=1,斥力影響范圍ρ0=1.5,步長l=0.1,步長設(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)勢場法路徑。
圖5(a)表示改進(jìn)算法開始,機(jī)器人剛好進(jìn)入移動(dòng)障礙物影響范圍,移動(dòng)機(jī)器人開始局部路徑規(guī)劃;圖5(b)、圖5(c)分別表示第10步與第18步,移動(dòng)機(jī)器人基于改進(jìn)勢場法進(jìn)行動(dòng)態(tài)避障;圖5(d)表示機(jī)器人在第25步完成避碰要求,開始追蹤原路徑。
圖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ī)器人對動(dòng)態(tài)路徑規(guī)劃的可行性。
將移動(dòng)機(jī)器人走過的位置相連,得到最終全局規(guī)劃路徑,如圖6(a)所示,與人工勢場法路徑圖6(b)相比,可以看出本文算法改善了動(dòng)態(tài)避障中人工勢場法的震蕩問題,使得路徑更加平滑,轉(zhuǎn)折次數(shù)更少。對比改進(jìn)算法與人工勢場法仿真數(shù)據(jù),如表2所示。
圖6 改進(jìn)算法與人工勢場法全局動(dòng)態(tài)路徑規(guī)劃最優(yōu)路徑
表2 兩種算法仿真結(jié)果對比
通過表2可得出,本文算法較人工勢場法規(guī)劃路徑長度減少3.1712,累計(jì)轉(zhuǎn)折角度減少12.5574rad,運(yùn)行時(shí)間相當(dāng)。
本文將A*算法與改進(jìn)的人工勢場法相結(jié)合,提出了一種改進(jìn)的動(dòng)態(tài)避碰策略。利用MATLAB仿真軟件,模擬在含有動(dòng)態(tài)障礙物的環(huán)境中,對移動(dòng)機(jī)器人從起始點(diǎn)至目標(biāo)點(diǎn)進(jìn)行了全局的動(dòng)態(tài)路徑規(guī)劃,同時(shí)達(dá)到避碰要求。
1)該算法利用A*算法求得的初始路徑作為全局最優(yōu)路徑,改進(jìn)勢場法執(zhí)行動(dòng)態(tài)避障,兩者結(jié)合轉(zhuǎn)換,使得傳統(tǒng)人工勢場法動(dòng)態(tài)路徑規(guī)劃中目標(biāo)點(diǎn)不可達(dá)的問題得到了解決,并消除震蕩現(xiàn)象。
2)針對局部動(dòng)態(tài)避障問題,提出了引入速度斥力場對障礙物的運(yùn)動(dòng)進(jìn)行判斷;當(dāng)移動(dòng)機(jī)器人駛離障礙物影響范圍時(shí),提出并解決了“動(dòng)態(tài)追蹤”問題,引入速度引力場,使機(jī)器人駛回原A*規(guī)劃的路線,簡化了結(jié)合算法的規(guī)劃策略。
3)改進(jìn)算法對動(dòng)態(tài)障礙物進(jìn)行規(guī)劃,與全局勢場法規(guī)劃相比,最優(yōu)路徑長度降低約20.4%,累計(jì)轉(zhuǎn)折角度減少約86.6%。