融合改進(jìn)A*算法與動(dòng)態(tài)窗口法的機(jī)器人避障研究

張 濤,陳 璋,李玉梅,房 萍,魯 娜,鞏紅雨

(1.北京信息科技大學(xué),高動(dòng)態(tài)導(dǎo)航技術(shù)北京市重點(diǎn)實(shí)驗(yàn)室,現(xiàn)代測控技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室,北京 100192; 2.中國石油天然氣股份有限公司華北油田分公司第三采油廠,河北滄州 062450)

0 引言

在井場滅火救援作業(yè)中,機(jī)器人的參與起到了至關(guān)重要的作用[1]。井場的工作環(huán)境復(fù)雜多變,機(jī)器人在行走的路徑中可能因?yàn)橐苿?dòng)的障礙物而產(chǎn)生相互碰撞[2],所以機(jī)器人在路徑規(guī)劃的過程中,不但要規(guī)劃出一條全局最優(yōu)的預(yù)測路線,同時(shí)還要具備局部隨機(jī)防撞的能力。

A*算法是進(jìn)行全局路徑規(guī)劃的常用方法,擁有快速的搜索效率和全局路線最優(yōu)求解的優(yōu)勢[3],但要求消耗很大的內(nèi)存,而且計(jì)算的路徑有較多的冗余轉(zhuǎn)折點(diǎn),與機(jī)器人的實(shí)際運(yùn)動(dòng)軌跡有偏差[4]。王洪斌等[5]提出一種平滑A*算法,可以對全局路徑進(jìn)行平滑處理,但不適用于局部路徑計(jì)算。龍建全等[6]利用bi-RRT算法解決了機(jī)器人在窄路口重復(fù)搜索的問題,并提高了全局搜索效率。Wang Huanwei等[7]提出EBS-A*算法,提高了算法的高效性和路徑的魯棒性。張敬寒等[8]利用最小二叉堆優(yōu)化A*算法的OPEN列表數(shù)據(jù)結(jié)構(gòu)來改善搜索效率,但無法實(shí)現(xiàn)動(dòng)態(tài)避障。姜媛媛[9]利用垂距限值法剔除改進(jìn)后路徑上的冗余節(jié)點(diǎn)。Wang Xindong等[10]對啟發(fā)函數(shù)進(jìn)行指數(shù)衰減加權(quán),提高了A*算法的計(jì)算效率。Zhang Jing等[11]將機(jī)器人轉(zhuǎn)向成本加入到評價(jià)函數(shù)中,縮短了機(jī)器人的搜索路徑長度。

針對傳統(tǒng)A*算法冗余點(diǎn)多、規(guī)劃效率低、折點(diǎn)多等問題,本文首先優(yōu)化A*算法的搜索鄰域至5×5,將16個(gè)搜索方向舍去7個(gè)保留9個(gè),并對啟發(fā)函數(shù)增加權(quán)重系數(shù),提高了路徑搜索效率;然后將路徑中共線的節(jié)點(diǎn)和多余的拐點(diǎn)進(jìn)行刪除,解決了冗余點(diǎn)多的問題,并減少了路徑長度;最后將融合動(dòng)態(tài)窗口法,在滿足全局路徑最優(yōu)的前提下完成實(shí)時(shí)動(dòng)態(tài)避障操作。

1 改進(jìn)A*算法

1.1 傳統(tǒng)的A*算法

A*算法的實(shí)質(zhì)是啟發(fā)式搜索算法,可以實(shí)現(xiàn)全局路徑運(yùn)算[12],A*算法的評價(jià)函數(shù)為

F(n)=G(n)+H(n)

(1)

傳統(tǒng)A*算法面臨著以下弊端:

(1)搜索方向較少,搜索節(jié)點(diǎn)較多,算法的搜索效率低;

(2)A*算法規(guī)劃的全局路徑中多余的共線節(jié)點(diǎn)和拐點(diǎn),影響了機(jī)器人運(yùn)動(dòng)路線的連貫性。

針對上述問題,對A*算法進(jìn)行如下具體改進(jìn)措施。

1.2 搜索鄰域、方向的優(yōu)化

傳統(tǒng)A*算法采用3×3搜索領(lǐng)域,搜索方向?yàn)?個(gè),如圖1所示,n1～n8為搜索方向,深色柵格為當(dāng)前節(jié)點(diǎn)位置,淺色柵格為鄰域節(jié)點(diǎn)位置。傳統(tǒng)的A*算法由于搜索領(lǐng)域和搜索方向較少導(dǎo)致搜索效率較低,路徑規(guī)劃軌跡由于路徑轉(zhuǎn)折點(diǎn)較大導(dǎo)致平滑度不夠。

為了提高搜索效率,本文提出優(yōu)化搜索方向策略。首先將3×3搜索鄰域擴(kuò)展至5×5搜索鄰域,搜索方向也從8個(gè)變成16個(gè),如圖2所示;然后對16個(gè)搜索方向加以舍棄,舍去7個(gè)搜索方向,保留9個(gè)搜索方向;將當(dāng)前節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)間的連線方向,與圖2中n5方向的相對角度設(shè)為γ,其角度γ與保留的9個(gè)方向和舍去的7個(gè)方向之間的對應(yīng)關(guān)系如表1所示。經(jīng)過上述搜索鄰域和搜索方向的優(yōu)化,提高了算法的運(yùn)算效率和路徑軌跡平滑度。

由于FT2232D并沒有使用默認(rèn)的設(shè)置，因此必須外掛EEPROM并對其進(jìn)行配置。出于成本考慮，選用3線串行EEPROM存儲(chǔ)器AT93C46。將ORG管腳懸空，由于內(nèi)部上拉電阻的存在，AT93C46將被配置為16位的存儲(chǔ)器結(jié)構(gòu)。FT2232D與USB插座的信號管腳連接時(shí)，USBDP、USBDM兩個(gè)輸入端電阻的阻值必須相等，典型值為27 Ω，而且必須是1%精度的電阻，否則可能造成輸入阻抗不匹配從而使電路無法正常工作。電路晶振Y2兩端的2個(gè)電容的電容值也必須完全相等，其典型值為18 pF。

圖2 5×5擴(kuò)展搜索鄰域圖

表1 夾角與保留、舍去方向的對應(yīng)關(guān)系

1.3 A*算法啟發(fā)函數(shù)的優(yōu)化

A*算法的搜索效率和精度與啟發(fā)函數(shù)密切相關(guān),本文將啟發(fā)函數(shù)改為如式(2)所示。

F(n)=G(n)+ε·H(n)

(2)

當(dāng)前位置離目標(biāo)位置較遠(yuǎn)時(shí),H(n)的估計(jì)值小于實(shí)際值,導(dǎo)致搜索節(jié)點(diǎn)過多,為了增加算法計(jì)算效率,此時(shí)應(yīng)當(dāng)增大ε的值;當(dāng)前位置靠近目標(biāo)位置時(shí),H(n)的估計(jì)值逐漸接近實(shí)際值,但當(dāng)估計(jì)值超過實(shí)際值時(shí)會(huì)得不到路徑的最優(yōu)解,此時(shí)應(yīng)當(dāng)減小ε的值,從而得到最優(yōu)路徑。

1.4 冗余節(jié)點(diǎn)刪除策略

經(jīng)過上述優(yōu)化,搜索路徑還是存在冗余節(jié)點(diǎn),影響了機(jī)器人的跟隨性,因此引入冗余點(diǎn)刪除策略,刪除一些冗余節(jié)點(diǎn),只保留必要的拐點(diǎn)。具體刪測策略如下:

(1)遍歷搜索路徑中的節(jié)點(diǎn),刪除共線的節(jié)點(diǎn)。當(dāng)前節(jié)點(diǎn)與后節(jié)點(diǎn)在同一搜索方向上時(shí),刪除后節(jié)點(diǎn),即圖2中黑色柵格為需刪去的節(jié)點(diǎn)。

(2)刪除多余的拐點(diǎn)。設(shè)路線中的節(jié)點(diǎn)為{Xk|k=1,2,…,n},連接X1點(diǎn)與Xk點(diǎn),若X1Xk與障礙物的距離小于所設(shè)定安全間距,則連接X1Xk-1,刪除1～k的節(jié)點(diǎn),再從點(diǎn)X2出發(fā)重復(fù)上述步驟,直至遍歷完全段節(jié)點(diǎn)。

2 動(dòng)態(tài)窗口算法

動(dòng)態(tài)窗口法是一種局部動(dòng)態(tài)路徑規(guī)劃算法,實(shí)現(xiàn)過程主要包括3個(gè)部分:速度采樣、軌跡預(yù)測和軌跡評估[12]。機(jī)器人的速度在一個(gè)速度區(qū)間內(nèi),在這個(gè)區(qū)間內(nèi)可以產(chǎn)生很多條路徑軌跡,通過適合的方法選擇一條最優(yōu)的路徑。

2.1 機(jī)器人運(yùn)動(dòng)模型

建立機(jī)器人的運(yùn)動(dòng)模型,模擬機(jī)器人的動(dòng)作軌跡信息[13]。機(jī)器人初始狀態(tài)下的位姿信息為(x0,y0,θ0),速度信息為(v0,w0),設(shè)機(jī)器人在下一段時(shí)間間隔Δt內(nèi)勻速運(yùn)動(dòng),速度信息為(v′,w′),則機(jī)器人的動(dòng)態(tài)窗口模型為

(3)

式中(xn,yn,θn)為機(jī)器人在下一時(shí)刻的位姿信息。

2.2 速度采樣

速度空間中有多組線速度和角速度,由于一定的約束條件的影響,限制了機(jī)器人的速度生成范圍[14]。約束條件如下:

機(jī)器人的最大、最小速度約束為

Vm={(v,ω)|v∈[vmin,vmax]∩ω∈[ωmin,ωmax]}

(4)

式中:vmin,vmax為機(jī)器人的最小和最大線速度;ωmin,ωmax為機(jī)器人的最小和最大角速度。

機(jī)器人電機(jī)加減速度約束為

(5)

機(jī)器人的安全約束:機(jī)器人與障礙物發(fā)生碰撞之前能夠安全制動(dòng)的速度為

(6)

綜上所述,機(jī)器人的速度區(qū)間應(yīng)滿足如下條件:

vr=vm∩vd∩va

(7)

2.3 評價(jià)函數(shù)

根據(jù)機(jī)器人運(yùn)動(dòng)模型和速度范圍模擬出若干軌跡后,通過評價(jià)函數(shù)選擇最優(yōu)的軌跡。為了克服動(dòng)態(tài)窗口法易陷入局部最優(yōu)的缺陷,提出優(yōu)化評價(jià)函數(shù)。優(yōu)化的評價(jià)函數(shù)為

G(v,w)=σ[αhead(v,ω)+βdist(v,ω)+γvel(v,ω)]

(8)

式中:head(v,ω)為預(yù)測軌跡終點(diǎn)方位與目標(biāo)方位的夾角差;dist(v,ω)為軌跡至最近障礙物處的距離;vel(v,ω)為當(dāng)前速度大小的評價(jià)函數(shù);σ為平滑函數(shù);α,β,γ為以上3項(xiàng)的權(quán)重。

3 仿真實(shí)驗(yàn)及分析

3.1 改進(jìn)A*算法仿真實(shí)驗(yàn)分析

利用控制變量的方法對比分析本文改進(jìn)的A*算法的優(yōu)越性。在同一環(huán)境地圖條件下,分別用傳統(tǒng)A*算法、改進(jìn)搜索鄰域、方向和啟發(fā)函數(shù)的A*算法、刪除共線節(jié)點(diǎn)的A*算法、刪除多余拐點(diǎn)的A*算法進(jìn)行路徑規(guī)劃仿真實(shí)驗(yàn),此優(yōu)化過程是層層遞進(jìn)的。如圖3所示,仿真模型是30×30的二維柵格圖,起點(diǎn)為(30,1),終點(diǎn)為(1,30);A*算法擴(kuò)展鄰域至5×5、優(yōu)化搜索方向、優(yōu)化啟發(fā)函數(shù)后,機(jī)器人規(guī)劃的路徑長度減小,路徑平滑度提高,如圖3(b)所示;刪除共線節(jié)點(diǎn)和多余拐點(diǎn)后,節(jié)點(diǎn)個(gè)數(shù)明顯下降,如圖3中(c)、(d)所示,灰色表示原來的節(jié)點(diǎn),黑色表示保留下來的點(diǎn)。

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

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

(d)刪除多余節(jié)點(diǎn)圖3 傳統(tǒng)算法與改進(jìn)算法的路徑對比

重復(fù)10組仿真實(shí)驗(yàn)后,對機(jī)器人的路徑規(guī)劃長度和路徑搜索時(shí)間取平均值,結(jié)果如圖4所示。圖4中的算法優(yōu)化從改進(jìn)啟發(fā)函數(shù)和鄰域開始是層層遞進(jìn)的,即優(yōu)化搜索方向是在改進(jìn)啟發(fā)函數(shù)和鄰域的基礎(chǔ)上進(jìn)行的,依此類推,最后優(yōu)化的是刪除多余拐點(diǎn)。由柱狀對比圖可得:傳統(tǒng)的A*算法規(guī)劃的路徑長度、搜索時(shí)間最長;本文在對A*算法進(jìn)行改進(jìn)啟發(fā)函數(shù)、擴(kuò)展鄰域、優(yōu)化搜索方向、刪除共線節(jié)點(diǎn)和多余節(jié)點(diǎn)后,較傳統(tǒng)A*算法搜索時(shí)間平均減少了65.805%,路徑長度平均減少了4.967%,極大地提高了A*算法的規(guī)劃效率。

圖4 改進(jìn)算法結(jié)果對比柱

3.2 融合算法仿真實(shí)驗(yàn)分析

動(dòng)態(tài)窗口算法雖然可以實(shí)時(shí)地根據(jù)機(jī)器人探測的信息進(jìn)行局部動(dòng)態(tài)避障,但無法達(dá)到全局路徑規(guī)劃的需求[15],因此本文采取融合算法,將經(jīng)過改進(jìn)的A*算法與動(dòng)態(tài)窗口法進(jìn)行融合,在確保全局路徑規(guī)劃效果最優(yōu)的前提下,同時(shí)進(jìn)行局部路徑規(guī)劃,從而確保機(jī)器人能夠?qū)崟r(shí)地規(guī)避路徑中的動(dòng)態(tài)障礙物。

將改進(jìn)的A*算法、改進(jìn)A*算法結(jié)合動(dòng)態(tài)窗口法,對同一柵格地圖進(jìn)行仿真實(shí)驗(yàn)。地圖信息與圖3一致,并設(shè)定6個(gè)障礙物,通過設(shè)置障礙物的運(yùn)動(dòng)速度和運(yùn)動(dòng)方向,來模擬機(jī)器人面對隨機(jī)動(dòng)態(tài)障礙物時(shí)是否具有動(dòng)態(tài)避障的能力。動(dòng)態(tài)窗口法技術(shù)參數(shù):機(jī)器人最大線速度為1 m/s,最大角速度為0.349 1 rad/s,最大線加速度為2 m/s2,最大角加速度為0.872 7 rad/s。評價(jià)函數(shù)的參數(shù)為:α=0.1,β=0.05,γ=0.1,向前模擬軌跡的時(shí)間周期為2.0 s。融合算法仿真結(jié)果如圖5所示。

(a)融合算法

(b)準(zhǔn)備避障

(c)完成避障

(d)到達(dá)目標(biāo)節(jié)點(diǎn)圖5 融合算法動(dòng)態(tài)避障路徑規(guī)劃圖

如圖5所示,細(xì)曲線表示由改進(jìn)的A*算法所規(guī)劃的路徑軌跡,粗曲線表示由融合算法所規(guī)劃的機(jī)器人運(yùn)動(dòng)軌跡,黑色方塊和灰色方塊分別表示靜態(tài)障礙物和動(dòng)態(tài)障礙物。改進(jìn)的A*算法融合動(dòng)態(tài)窗口法后能夠規(guī)劃出從起始點(diǎn)到達(dá)目標(biāo)點(diǎn)的路徑,保證了全局路徑是最優(yōu)的;在路徑中設(shè)置移動(dòng)的障礙物,可以實(shí)現(xiàn)機(jī)器人的局部動(dòng)態(tài)避障,且路徑平滑度較好,符合機(jī)器人運(yùn)動(dòng)規(guī)律的跟隨性。

機(jī)器人在運(yùn)動(dòng)的過程中,線速度變化如圖6所示,姿態(tài)變化如圖7所示。機(jī)器人在進(jìn)行局部路徑規(guī)劃躲避動(dòng)態(tài)障礙物時(shí),線速度減小的同時(shí)弧度也會(huì)減小;當(dāng)機(jī)器人行進(jìn)到圖5(b)中時(shí)(控制節(jié)點(diǎn)個(gè)數(shù)在300～400之間),障礙物較多且聚集,機(jī)器人有明顯的減速過程,導(dǎo)致規(guī)劃時(shí)間會(huì)有所增加;實(shí)時(shí)輸出機(jī)器人的控制參數(shù),可以調(diào)節(jié)機(jī)器人的運(yùn)動(dòng)狀態(tài),有利于機(jī)器人的自動(dòng)反饋控制。綜上所述,本文改進(jìn)的A*算法融合動(dòng)態(tài)窗口算法在保證全局路徑最優(yōu)的前提下,可以實(shí)現(xiàn)局部路徑動(dòng)態(tài)避障操作,且實(shí)時(shí)性和路徑平滑性較好。

圖6 機(jī)器人線速度

圖7 機(jī)器人位姿變化圖

4 結(jié)論

本文將A*算法的搜索鄰域擴(kuò)展至5×5,同時(shí)將路徑搜索方向從16個(gè)優(yōu)化至9個(gè),并優(yōu)化了啟發(fā)函數(shù),引入了冗余點(diǎn)刪除方法,極大提升了算法的搜索效率,也優(yōu)化了算法的搜索路徑;通過動(dòng)態(tài)窗口法實(shí)現(xiàn)局部路徑規(guī)劃,克服了機(jī)器人在全局路徑規(guī)劃中不能動(dòng)態(tài)避障的缺陷。通過實(shí)驗(yàn)可得:

(1)本文在對A*算法進(jìn)行改進(jìn)啟發(fā)函數(shù)、擴(kuò)展鄰域、優(yōu)化搜索方向、刪除共線節(jié)點(diǎn)和多余節(jié)點(diǎn)后,與傳統(tǒng)A*算法相比,路徑規(guī)劃時(shí)間平均減少了65.805%,路徑長度平均減少了4.967%,極大提高了算法的搜索效率,且機(jī)器人的路徑平滑度也有所改善。

(2)改進(jìn)的A*算法融合動(dòng)態(tài)窗口法使得機(jī)器人進(jìn)行路徑規(guī)劃時(shí),在確保全局路徑最優(yōu)的前提下能夠進(jìn)行局部動(dòng)態(tài)避障,通過仿真實(shí)驗(yàn)設(shè)置動(dòng)態(tài)障礙物驗(yàn)證了融合算法的有效性。