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

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

0 引言

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

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

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

1 改進A*算法

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

A*算法的實質是啟發(fā)式搜索算法,可以實現全局路徑運算[12],A*算法的評價函數為

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

(1)

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

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

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

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

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

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

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

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

圖2 5×5擴展搜索鄰域圖

表1 夾角與保留、舍去方向的對應關系

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

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

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

(2)

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

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

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

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

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

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

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

2.1 機器人運動模型

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

(3)

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

2.2 速度采樣

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

機器人的最大、最小速度約束為

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

(4)

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

機器人電機加減速度約束為

(5)

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

(6)

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

vr=vm∩vd∩va

(7)

2.3 評價函數

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

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

(8)

式中:head(v,ω)為預測軌跡終點方位與目標方位的夾角差;dist(v,ω)為軌跡至最近障礙物處的距離;vel(v,ω)為當前速度大小的評價函數;σ為平滑函數;α,β,γ為以上3項的權重。

3 仿真實驗及分析

3.1 改進A*算法仿真實驗分析

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

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

(b)改進的A*算法

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

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

圖4 改進算法結果對比柱

3.2 融合算法仿真實驗分析

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

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

(a)融合算法

(b)準備避障

(c)完成避障

(d)到達目標節(jié)點圖5 融合算法動態(tài)避障路徑規(guī)劃圖

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

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

圖6 機器人線速度

圖7 機器人位姿變化圖

4 結論

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

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

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