融合A*算法與人工勢(shì)場(chǎng)法的動(dòng)態(tài)路徑規(guī)劃*

胡 錚,徐 斌

(上海工程技術(shù)大學(xué)機(jī)械與汽車(chē)工程學(xué)院,上海 201620)

0 引言

動(dòng)態(tài)路徑規(guī)劃是移動(dòng)機(jī)器人研究領(lǐng)域的重要內(nèi)容之一,是指機(jī)器人在有動(dòng)態(tài)障礙物的環(huán)境中,如何找到一條從起點(diǎn)到終點(diǎn)的路徑,使機(jī)器人在運(yùn)動(dòng)過(guò)程中能安全、無(wú)碰撞地繞過(guò)障礙物[1]。根據(jù)路徑規(guī)劃中環(huán)境信息是否已知可將路徑規(guī)劃算法分為全局路徑規(guī)劃和局部路徑規(guī)劃[2]。目前常用全局路徑規(guī)劃算法有:A*算法[3]、粒子群算法[4]以及蟻群算法[5]等。局部路徑規(guī)劃常用的算法有:模糊邏輯算法[6]、人工勢(shì)場(chǎng)法[7]等。A*算法是一種啟發(fā)式搜索算法,其實(shí)現(xiàn)方法較為簡(jiǎn)潔,相較于粒子群、蟻群等算法運(yùn)行效率較高。但是傳統(tǒng)A*算法存在拐點(diǎn)多,易斜穿障礙物頂點(diǎn),路徑不平滑等缺點(diǎn)。王玉中等[8]通過(guò)合理地設(shè)置估價(jià)函數(shù)的權(quán)重比例,縮短算法的運(yùn)行時(shí)間并優(yōu)化了生成的路徑?？谆鄯嫉萚9]通過(guò)將轉(zhuǎn)彎修正代價(jià)參數(shù)加入到啟發(fā)函數(shù)中,從而減少了路徑的轉(zhuǎn)彎次數(shù)。

當(dāng)工作環(huán)境存在未知障礙物時(shí),一般采用局部路徑規(guī)劃的算法。人工勢(shì)場(chǎng)法只需要根據(jù)周?chē)沫h(huán)境和當(dāng)前狀態(tài)來(lái)確定接下來(lái)的運(yùn)動(dòng)方向,具有計(jì)算速度快、路徑平滑等優(yōu)點(diǎn)[10]。但是該方法易在局部極小點(diǎn)陷入停滯,且面對(duì)動(dòng)態(tài)障礙物時(shí),傳統(tǒng)人工勢(shì)場(chǎng)法難以達(dá)到最優(yōu)路徑指標(biāo)[11]。為此,徐小強(qiáng)等[12]設(shè)置障礙物的預(yù)測(cè)距離,當(dāng)機(jī)器人在預(yù)測(cè)距離內(nèi)將要陷入局部最小時(shí),預(yù)先設(shè)置虛擬目標(biāo)點(diǎn)幫助機(jī)器人避開(kāi)陷阱區(qū)域。張旭等[13]提出了扇形區(qū)域探測(cè)法,能夠有效地規(guī)避動(dòng)態(tài)障礙物。但常規(guī)改進(jìn)的人工勢(shì)場(chǎng)法很難達(dá)到既能在動(dòng)態(tài)環(huán)境下高效避障的同時(shí)兼具一定的全局最優(yōu)性。

針對(duì)上述的難點(diǎn),本文提出一種融合A*算法與人工勢(shì)場(chǎng)法的算法(IA*-APF)。利用改進(jìn)A*算法規(guī)劃出的全局路徑中的拐點(diǎn)作為改進(jìn)人工勢(shì)場(chǎng)法避障的子目標(biāo)點(diǎn)。實(shí)驗(yàn)仿真結(jié)果表明,IA*-APF不僅能夠有效解決傳統(tǒng)人工勢(shì)場(chǎng)法易陷入局部最小的問(wèn)題,而且在提高動(dòng)態(tài)環(huán)境下的避障效率的同時(shí)保證一定的全局最優(yōu)性。

1 A*算法

1.1 環(huán)境模型

機(jī)器人的工作環(huán)境對(duì)其路徑規(guī)劃具有重要的影響,構(gòu)造環(huán)境地圖的常用方法主要有柵格法、矢量法和自由空間法等。柵格法是將機(jī)器人所處的區(qū)域均分為多個(gè)方格的一種表示方法。本文采用柵格法創(chuàng)建30×30的柵格模型地圖進(jìn)行實(shí)驗(yàn)。在柵格地圖中,黑色柵格為障礙物區(qū)域,白色柵格為可行區(qū)域。

1.2 傳統(tǒng)A*算法

A*算法[14]是一種經(jīng)典的啟發(fā)式搜索算法。其基本理論是,以機(jī)器人的起始節(jié)點(diǎn)為父節(jié)點(diǎn),搜索出其周?chē)?個(gè)方向的節(jié)點(diǎn),根據(jù)估價(jià)函數(shù)計(jì)算它們的f(n)值,選擇f(n)值最小的節(jié)點(diǎn)為父節(jié)點(diǎn),繼續(xù)搜索計(jì)算,當(dāng)搜索到目標(biāo)點(diǎn)時(shí)算法結(jié)束。最后從目標(biāo)點(diǎn)沿著父節(jié)點(diǎn)反向搜索回到起始節(jié)點(diǎn),獲得一條成本最小的路徑即為最終路徑。A*算法的估價(jià)函數(shù)f(n)如式(1)所示:

f(n)=g(n)+h(n)

(1)

式中:g(n)為從起始節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)n的實(shí)際代價(jià)值,h(n)為啟發(fā)函數(shù),表示從當(dāng)前節(jié)點(diǎn)到n到目標(biāo)點(diǎn)的估計(jì)代價(jià)值。通常h(n)用當(dāng)前節(jié)點(diǎn)n與目標(biāo)點(diǎn)的歐氏距離來(lái)表示:

(2)

式中:xg為目標(biāo)點(diǎn)的橫坐標(biāo),yg為目標(biāo)點(diǎn)的縱坐標(biāo),xn為當(dāng)前節(jié)點(diǎn)n的橫坐標(biāo),yn為當(dāng)前節(jié)點(diǎn)n的縱坐標(biāo)。

1.3 改進(jìn)的A*算法

傳統(tǒng)A*算法雖能夠有效地進(jìn)行全局路徑規(guī)劃,但其存在著搜索速度慢、易斜穿障礙物頂點(diǎn)且路徑冗余節(jié)點(diǎn)、拐點(diǎn)較多等缺點(diǎn),導(dǎo)致機(jī)器人路徑規(guī)劃效率低、安全性差。本文旨在從以下3個(gè)方面對(duì)A*算法進(jìn)行改進(jìn)。

(1)改進(jìn)估價(jià)函數(shù)。A*算法的搜索效率由啟發(fā)函數(shù)值決定。由于當(dāng)前節(jié)點(diǎn)n到目標(biāo)點(diǎn)所消耗的h(n)總是小于當(dāng)前點(diǎn)n到目標(biāo)點(diǎn)的實(shí)際消耗,所以在搜索過(guò)程中會(huì)搜索過(guò)多的節(jié)點(diǎn)。如果能夠提升估計(jì)消耗h(n)的權(quán)重,算法的搜索效率將會(huì)提高。鮑久圣等[15]提出使用指數(shù)函數(shù)作為啟發(fā)函數(shù)的加權(quán)系數(shù),優(yōu)化后的估價(jià)函數(shù)為:

f(n)=g(n)+eh(n)×h(n)

(3)

經(jīng)過(guò)加權(quán)改進(jìn)后,算法的搜索效率有一定的提升,但其搜索范圍是父節(jié)點(diǎn)周?chē)?個(gè)方向的節(jié)點(diǎn),這樣會(huì)導(dǎo)致一定的無(wú)效搜索,算法花費(fèi)的時(shí)間增加。本文在式(3)的基礎(chǔ)上,提出基于角度定義的估價(jià)函數(shù):

(4)

式中:θi(i=1,2,…,8)為各子節(jié)點(diǎn)與父節(jié)點(diǎn)及目標(biāo)點(diǎn)的夾角。若θi不滿足式(4)的判斷條件,則消除該子節(jié)點(diǎn)。改進(jìn)后的算法與傳統(tǒng)A*算法及指數(shù)加權(quán)A*算法進(jìn)行對(duì)比,結(jié)果如圖1所示。

(a) 傳統(tǒng)A*算法路徑 (b) 指數(shù)加權(quán)A*算法路徑

(c) 角度定義指數(shù)加權(quán)A*算法路徑圖1 改進(jìn)A*算法路徑對(duì)比圖

圖1中各種算法的數(shù)據(jù)如表1所示,由此可得,角度定義指數(shù)加權(quán)A*算法搜索的節(jié)點(diǎn)數(shù)最少,搜索時(shí)間最短,可以有效改善傳統(tǒng)A*算法搜索效率低的問(wèn)題。

表1 改進(jìn)A*算法路徑對(duì)比數(shù)據(jù)

(2)優(yōu)化子節(jié)點(diǎn)的選擇。由傳統(tǒng)A*算法可知,搜索時(shí)f(n)值最小的子節(jié)點(diǎn)會(huì)成為下一次搜索的父節(jié)點(diǎn),這樣的搜索方法忽略了算法的安全性。最終規(guī)劃的路徑有穿過(guò)障礙物頂點(diǎn)的現(xiàn)象,機(jī)器人與障礙物會(huì)有互相碰撞的可能。因此設(shè)置障礙物的安全距離對(duì)子節(jié)點(diǎn)的選擇進(jìn)行優(yōu)化。判斷各障礙物到父節(jié)點(diǎn)和各子節(jié)點(diǎn)連成的直線的距離是否大于安全距離。若大于,保留該子節(jié)點(diǎn),否則,消除該子節(jié)點(diǎn)。對(duì)于保留下來(lái)的子節(jié)點(diǎn),根據(jù)式(4)計(jì)算出它們的f(n)值。選擇f(n)值最小的子節(jié)點(diǎn)為下一次搜索的父節(jié)點(diǎn)。通過(guò)上述方法對(duì)算法進(jìn)行優(yōu)化,優(yōu)化后的路徑如圖2所示。

由圖2可以看出,優(yōu)化子節(jié)點(diǎn)的選擇后的算法路徑與障礙物之間保持了一定距離,提升了算法的安全性。

圖2 優(yōu)化子節(jié)點(diǎn)路徑圖 圖3 二次路徑優(yōu)化路徑圖

(3)二次路徑優(yōu)化。在圖2中,優(yōu)化子節(jié)點(diǎn)后的路徑仍存在拐點(diǎn)過(guò)多,路徑長(zhǎng)度較大的問(wèn)題,因此需要對(duì)路徑進(jìn)行二次優(yōu)化。首先,按順序提取出路徑中所有拐點(diǎn),起始節(jié)點(diǎn)和目標(biāo)點(diǎn)也看作拐點(diǎn);接著,連接拐點(diǎn)與其不相鄰的后續(xù)拐點(diǎn)得到一條新的路徑,以該新路徑的f(n)值是否小于原路徑的f(n)值以及其是否在障礙物的安全距離外作為該新路徑是否成立的判斷標(biāo)準(zhǔn)。如果新路徑成立,則剔除中間拐點(diǎn),否則,保留原路徑;最后,生成的路徑為最短路徑。二次路徑優(yōu)化后的結(jié)果如圖3所示。

表2為路徑二次優(yōu)化前后的數(shù)據(jù)對(duì)比,可以看出,二次路徑優(yōu)化減少了路徑中的拐點(diǎn)和路徑長(zhǎng)度,有效地簡(jiǎn)化了路徑。

表2 路徑數(shù)據(jù)對(duì)比

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

2.1 傳統(tǒng)人工勢(shì)場(chǎng)法

傳統(tǒng)人工勢(shì)場(chǎng)法[16]的主要原理是梯度勢(shì)場(chǎng)法。在算法中,機(jī)器人受到的引力勢(shì)場(chǎng)為:

(5)

式中:m為引力增益常數(shù),ρ為目標(biāo)點(diǎn)與機(jī)器人的距離。機(jī)器人受到的引力為引力勢(shì)場(chǎng)函數(shù)的負(fù)梯度:

Fatt=-▽Uatt=mρ

(6)

機(jī)器人受到的斥力勢(shì)場(chǎng)為:

(7)

式中:k為斥力場(chǎng)常量,q為機(jī)器人與障礙物的距離,λ為障礙物對(duì)機(jī)器人的影響距離。機(jī)器人受到的斥力為斥力勢(shì)場(chǎng)函數(shù)的負(fù)梯度:

(8)

傳統(tǒng)人工勢(shì)場(chǎng)法通過(guò)引力與斥力的合力大小與方向控制機(jī)器人運(yùn)動(dòng),對(duì)于只存在靜態(tài)障礙物的環(huán)境能夠快速規(guī)劃出一條無(wú)碰撞路徑,但是在復(fù)雜動(dòng)態(tài)的障礙物環(huán)境中,其往往無(wú)法滿足規(guī)劃要求。本文通過(guò)改進(jìn)人工勢(shì)場(chǎng)法的斥力函數(shù)來(lái)解決這一問(wèn)題。

2.2 改進(jìn)人工勢(shì)場(chǎng)法

動(dòng)態(tài)路徑規(guī)劃中視機(jī)器人為質(zhì)點(diǎn),其運(yùn)動(dòng)方向?yàn)槠浜狭Φ姆较?速度的大小為步長(zhǎng)。當(dāng)傳統(tǒng)人工勢(shì)場(chǎng)法在動(dòng)態(tài)環(huán)境中進(jìn)行路徑規(guī)劃時(shí),并沒(méi)有考慮機(jī)器人與障礙物的位置和相對(duì)運(yùn)動(dòng)速度的關(guān)系,導(dǎo)致規(guī)劃的路徑變長(zhǎng),導(dǎo)航效率降低。為了在動(dòng)態(tài)環(huán)境下提高機(jī)器人的導(dǎo)航效率,在斥力函數(shù)中加入相對(duì)速度斥力。改進(jìn)后的斥力勢(shì)場(chǎng)為:

(9)

圖4 機(jī)器人與動(dòng)態(tài)障礙物有碰撞風(fēng)險(xiǎn)

改進(jìn)斥力勢(shì)場(chǎng)后相應(yīng)的斥力Frep1為:

(10)

相對(duì)速度斥力的方向與動(dòng)態(tài)障礙物對(duì)機(jī)器人的斥力方向一致。引進(jìn)相對(duì)速度斥力可以讓機(jī)器人在與動(dòng)態(tài)障礙物有碰撞風(fēng)險(xiǎn)時(shí)獲得更大的斥力,在相對(duì)安全的狀態(tài)下可以避免盲目的避障。

2.3 改進(jìn)A*算法結(jié)合人工勢(shì)場(chǎng)法

A*算法通過(guò)上述的改進(jìn)之后可以有效解決搜索效率低和避障安全性等問(wèn)題,能在已知環(huán)境下規(guī)劃出一條全局最優(yōu)路徑。人工勢(shì)場(chǎng)法在上述改動(dòng)中可以有效解決動(dòng)態(tài)環(huán)境中的避障問(wèn)題。由于A*算法具有全局最優(yōu)性,人工勢(shì)場(chǎng)法具有避障的實(shí)時(shí)性。本文將兩種改進(jìn)算法相結(jié)合,提出的IA*-APF算法的流程圖如圖5所示。

圖5 IA*-APF算法的流程圖

在IA*-APF算法中,改進(jìn)A*算法在已知的障礙物環(huán)境中進(jìn)行全局路徑規(guī)劃,全局路徑的拐點(diǎn)依次作為人工勢(shì)場(chǎng)法的路徑規(guī)劃的虛擬目標(biāo)點(diǎn),直至到達(dá)終點(diǎn)算法結(jié)束。

3 算法仿真與結(jié)果分析

為了驗(yàn)證IA*-APF的可行性,本文構(gòu)造陷阱區(qū)域和動(dòng)態(tài)障礙物環(huán)境進(jìn)行仿真研究,仿真基本參數(shù)如表3所示。

表3 仿真參數(shù)設(shè)定

3.1 陷阱區(qū)域環(huán)境下的實(shí)驗(yàn)結(jié)果

在30×30的柵格地圖中隨機(jī)創(chuàng)建障礙物,在障礙物中隨機(jī)設(shè)置如U型、L型等局部陷阱區(qū)域。機(jī)器人的起始點(diǎn)為(3,2),目標(biāo)點(diǎn)為(17,20)。圖6為傳統(tǒng)人工勢(shì)場(chǎng)法與IA*-APF在陷阱區(qū)域環(huán)境下的避障結(jié)果。

(a) 傳統(tǒng)人工勢(shì)場(chǎng)法避障路徑圖 (b) IA*-APF算法避障路徑圖圖6 存在陷阱區(qū)域的環(huán)境路徑對(duì)比

如圖6a所示,傳統(tǒng)人工勢(shì)場(chǎng)法在避障過(guò)程中遇到L型障礙物陷阱,機(jī)器人在陷阱障礙物內(nèi)部陷入局部最小,無(wú)法避開(kāi)障礙物。在圖6b中,IA*-APF中全局路徑的拐點(diǎn)作為局部路徑的虛擬目標(biāo)點(diǎn),幫助機(jī)器人逃離陷阱區(qū)域。

3.2 動(dòng)態(tài)環(huán)境下的實(shí)驗(yàn)結(jié)果

為了進(jìn)一步驗(yàn)證IA*-APF算法求解動(dòng)態(tài)路徑規(guī)劃問(wèn)題的有效性,在陷阱區(qū)域環(huán)境的基礎(chǔ)上,增加動(dòng)態(tài)障礙物進(jìn)行實(shí)驗(yàn)仿真。環(huán)境1為單個(gè)動(dòng)態(tài)障礙物混合靜態(tài)障礙物,動(dòng)態(tài)障礙物的起始點(diǎn)為 (25,13),其運(yùn)動(dòng)速度分別為vobs1=[-0.35,0.35]。環(huán)境2為多個(gè)動(dòng)態(tài)障礙物混合靜態(tài)障礙物,在環(huán)境中隨機(jī)設(shè)置3個(gè)動(dòng)態(tài)障礙物,它們的起始點(diǎn)分別為(9,13),(15,21),(19,9),運(yùn)動(dòng)速度分別為vobs2=[0.5,0],vobs3=[0,-0.5],vobs4=[0.6,0]。將IA*-APF算法與文獻(xiàn)[17]算法、傳統(tǒng)人工勢(shì)場(chǎng)法進(jìn)行比較,結(jié)果如圖7和圖8所示。圖中的箭頭表示障礙物的運(yùn)動(dòng)方向,深灰色×代表動(dòng)態(tài)障礙物的起始點(diǎn),以×為頂點(diǎn)的深灰色線條表示障礙物的運(yùn)動(dòng)軌跡。

(a) 傳統(tǒng)人工勢(shì)場(chǎng)法的路徑結(jié)果 (b) 文獻(xiàn)[17]算法的路徑結(jié)果

(c) IA*-APF的路徑結(jié)果圖7 環(huán)境1的算法路徑對(duì)比

(a) 傳統(tǒng)人工勢(shì)場(chǎng)法的路徑結(jié)果 (b) 文獻(xiàn)[17]算法的路徑結(jié)果

(c) IA*-APF的路徑結(jié)果圖8 環(huán)境2的算法路徑對(duì)比

由圖7的實(shí)驗(yàn)結(jié)果可以看出,傳統(tǒng)人工勢(shì)場(chǎng)法在動(dòng)態(tài)環(huán)境一中因避障幅度過(guò)大以至于陷入陷阱區(qū)域,無(wú)法完成避障任務(wù)。文獻(xiàn)[17]算法雖能成功到達(dá)目標(biāo)點(diǎn)但路徑轉(zhuǎn)折較多。在圖8的實(shí)驗(yàn)中,傳統(tǒng)人工勢(shì)場(chǎng)法和文獻(xiàn)[17]算法在面對(duì)動(dòng)態(tài)障礙物時(shí)陷入徘徊狀態(tài)。而IA*-APF在兩種環(huán)境中既能較好地避開(kāi)動(dòng)態(tài)障礙物又能保持一定的全局最優(yōu)性。

表4得出了3種算法在兩個(gè)不同環(huán)境下的實(shí)驗(yàn)仿真結(jié)果,在實(shí)驗(yàn)環(huán)境1中,IA*-APF的步數(shù)、路徑長(zhǎng)度以及運(yùn)行時(shí)間均略?xún)?yōu)于文獻(xiàn)[17]算法。在實(shí)驗(yàn)環(huán)境2中,IA*-APF相較于傳統(tǒng)人工勢(shì)場(chǎng)法,步數(shù)減少25%,路徑縮短25.6%,運(yùn)行時(shí)間縮短14.8%,相較于文獻(xiàn)[17]算法,步數(shù)減少12.4%,路徑縮短12.6%,運(yùn)行時(shí)間縮短7.4%。

表4 動(dòng)態(tài)環(huán)境下的算法數(shù)據(jù)

4 結(jié)束語(yǔ)

針對(duì)移動(dòng)機(jī)器人動(dòng)態(tài)路徑規(guī)劃問(wèn)題,提出了一種改進(jìn)A*算法與人工勢(shì)場(chǎng)法的混合算法。首先在傳統(tǒng)A*算法的基礎(chǔ)上,提出基于角度的加權(quán)估價(jià)函數(shù)和利用障礙物的安全閾值優(yōu)化子節(jié)點(diǎn)的選擇策略,然后對(duì)路徑進(jìn)行二次優(yōu)化,提高了算法的路徑規(guī)劃效率和安全性。為了能讓機(jī)器人的動(dòng)態(tài)路徑規(guī)劃更加高效,在人工勢(shì)場(chǎng)法的斥力函數(shù)中引入相對(duì)速度斥力函數(shù)。利用改進(jìn)A*算法得到的全局路徑的拐點(diǎn)作為人工勢(shì)場(chǎng)法避障的子目標(biāo)點(diǎn)。陷阱區(qū)域和動(dòng)態(tài)環(huán)境下實(shí)驗(yàn)結(jié)果表明, IA*-APF算法可以有效地避開(kāi)局部極小陷阱區(qū)域,且在動(dòng)態(tài)未知的環(huán)境中能規(guī)劃出一條安全的、高效的路徑的同時(shí)保持一定的全局最優(yōu)性,為移動(dòng)機(jī)器人在動(dòng)態(tài)環(huán)境下的避障提供了理論基礎(chǔ)。