沈顯慶, 馬志鵬, 孫啟智, 王 賀
(黑龍江科技大學(xué) 電氣與控制工程學(xué)院, 哈爾濱 150022)
路徑規(guī)劃是機(jī)器人自主導(dǎo)航領(lǐng)域的關(guān)鍵問題之一[1]。相較于經(jīng)典路徑規(guī)劃算法如Dijkstra算法[2]、人工勢(shì)場(chǎng)法[3]和蟻群算法[4]等,A*算法具有搜索時(shí)間短、運(yùn)行速度快,不易陷入“早熟”等優(yōu)點(diǎn)。但傳統(tǒng)A*算法的計(jì)算量會(huì)隨著地圖增大而急劇增加,從而導(dǎo)致算法搜索效率降低、規(guī)劃路徑不平滑等問題。對(duì)此,楊璐等[5]提出了A*算法的二次規(guī)劃策略,提高計(jì)算效率,但并未考慮AGV的體積和轉(zhuǎn)角,實(shí)用性不強(qiáng)。A.Botea等[6]提出HPA*算法,提高了算法的搜索速度,但規(guī)劃出的不是最佳路徑。王維等[7]通過對(duì)A*算法的評(píng)價(jià)函數(shù)進(jìn)行改進(jìn),減少算法運(yùn)行時(shí)間,但普適性不好。F.Duchon等[8]提出了基于A*算法的跳點(diǎn)搜索策略,減小了算法的計(jì)算量,但并未解決路徑中的拐點(diǎn)問題?;谏鲜龇治?,筆者提出了一種改進(jìn)A*算法,對(duì)傳統(tǒng)A*算法的子節(jié)點(diǎn)選擇進(jìn)行優(yōu)化,提高算法的尋優(yōu)速度,在此基礎(chǔ)上,對(duì)Floyd-Warshall算法進(jìn)行雙向平滑處理,改善路徑質(zhì)量,通過Matlab平臺(tái)進(jìn)行仿真研究,驗(yàn)證所提算法的正確性及可行性。
柵格法環(huán)境建模是將三維運(yùn)動(dòng)空間抽象為非離散的二維平面,并將其分割成大小相等的若干網(wǎng)格[9],其中每個(gè)網(wǎng)格都具有二進(jìn)制參數(shù),利用0-1矩陣來判斷網(wǎng)格是否被障礙物覆蓋。當(dāng)網(wǎng)格內(nèi)有障礙物時(shí)賦值為1,用黑色表示;當(dāng)網(wǎng)格內(nèi)無障礙時(shí)賦值為0,用白色表示。其對(duì)應(yīng)關(guān)系如圖1所示。
圖1 柵格地圖對(duì)應(yīng)關(guān)系Fig. 1 Raster map correspondence
假設(shè)機(jī)器人R在有限個(gè)障礙物的二維平面區(qū)域A內(nèi)移動(dòng),以A的左下角為坐標(biāo)原點(diǎn),水平方向?yàn)閤軸,垂直方向?yàn)閥軸,構(gòu)建直角坐標(biāo)系xOy。xmax、ymax分別為x、y軸方向上的最大值。以機(jī)器人的步長(zhǎng)ρ對(duì)坐標(biāo)區(qū)域進(jìn)行劃分,每行、每列的柵格數(shù)為
nx=xmax/ρ,
ny=ymax/ρ。
對(duì)于坐標(biāo)區(qū)域的任一柵格,都有確定的坐標(biāo)及相應(yīng)的序號(hào)與之對(duì)應(yīng),如圖2所示。定義S={1,2,…,25}為柵格序號(hào)集,以坐標(biāo)區(qū)域的左上角為原點(diǎn),由左向右、自上至下,對(duì)二維平面區(qū)域A進(jìn)行編號(hào),坐標(biāo)與序號(hào)之間的關(guān)系為
圖2 柵格坐標(biāo)與序號(hào)關(guān)系Fig. 2 Grid coordinate and serial number relationship
xi=((i-1)modnx)+1,
yi=ny-ceil(i/nx)+1,
式中:i——第i個(gè)序號(hào);
mod——取余運(yùn)算;
ceil——進(jìn)一取整運(yùn)算;
nx、ny——每行、每列的柵格數(shù);
xi、yi——序號(hào)i所處柵格的中心橫坐標(biāo)和縱坐標(biāo)。
A*算法是一種啟發(fā)式搜索算法[10],通過啟發(fā)函數(shù)計(jì)算周圍8個(gè)柵格節(jié)點(diǎn)的估價(jià)值,選擇代價(jià)值最小的節(jié)點(diǎn)作為下一步搜索的父節(jié)點(diǎn),并繼續(xù)搜索這一父節(jié)點(diǎn)周圍相鄰的柵格節(jié)點(diǎn),不斷循環(huán)直到搜索到目標(biāo)節(jié)點(diǎn),可在保證得到最優(yōu)路徑的前提下,提高算法的搜索效率。A*算法的代價(jià)函數(shù)為
f(m)=g(m)+h(m),
式中:f(m)——節(jié)點(diǎn)m的代價(jià)函數(shù);
g(m)——起始節(jié)點(diǎn)S與節(jié)點(diǎn)m的代價(jià);
h(m)——節(jié)點(diǎn)m與目標(biāo)節(jié)點(diǎn)T的啟發(fā)式估值。
g(m)一般是固定數(shù)值,相當(dāng)于Dijkstra算法中到起始節(jié)點(diǎn)的最優(yōu)路徑。h(m)是A*算法的關(guān)鍵,直接影響著算法的求解速度和搜索效率。h(m)應(yīng)滿足的條件為
h(m)≤h*(m),
式中,h*(m)——節(jié)點(diǎn)m到目標(biāo)點(diǎn)T的真實(shí)最小代價(jià),h(m)與h*(m)差距不能過大。
同時(shí)為了減小A*算法的計(jì)算量,選用Euclidean距離來近似替代啟發(fā)函數(shù)為
式中:(xm,ym)——節(jié)點(diǎn)m所處柵格的中心坐標(biāo);
(xT,yT)——目標(biāo)節(jié)點(diǎn)T所處柵格的中心坐標(biāo)。
傳統(tǒng)A*算法在規(guī)劃路徑時(shí),將機(jī)器人當(dāng)作一個(gè)質(zhì)點(diǎn),忽略了機(jī)器人的體積和運(yùn)動(dòng)特性,因此,規(guī)劃出的路徑是理想路徑[11],可能會(huì)產(chǎn)生規(guī)劃路徑斜穿障礙物頂點(diǎn)的情況,存在路徑安全性問題,規(guī)劃路徑風(fēng)險(xiǎn)如圖3所示。
圖3 規(guī)劃路徑風(fēng)險(xiǎn)Fig. 3 Planning path risk
當(dāng)機(jī)器人從柵格A移動(dòng)到柵格B時(shí),考慮到機(jī)器人具有一定的體積,因此,在紅點(diǎn)處可能會(huì)發(fā)生碰撞,使機(jī)器人損壞。
對(duì)此,文中通過對(duì)A*算法子節(jié)點(diǎn)的擴(kuò)展順序進(jìn)行改進(jìn),降低在實(shí)際運(yùn)動(dòng)過程中與障礙物發(fā)生碰撞的風(fēng)險(xiǎn),相鄰節(jié)點(diǎn)優(yōu)先分組,如圖4所示。設(shè)當(dāng)前節(jié)點(diǎn)為O,周圍相鄰節(jié)點(diǎn)分別為A、B、C、D、E、S、W、N。
圖4 相鄰節(jié)點(diǎn)優(yōu)先級(jí)分組Fig. 4 Priority grouping of adjacent nodes
將這些鄰域節(jié)點(diǎn)進(jìn)行等級(jí)劃分,其中,E、S、W、N四個(gè)節(jié)點(diǎn)為高級(jí)組,將A、B、C、D四個(gè)節(jié)點(diǎn)為普通組。在子節(jié)點(diǎn)的生成過程中,首先搜索高級(jí)組中的子節(jié)點(diǎn),再根據(jù)表1所示規(guī)則生成普通組的子節(jié)點(diǎn)。
表1 普通組子節(jié)點(diǎn)生成規(guī)則
雖然對(duì)A*算法子節(jié)點(diǎn)的選擇進(jìn)行優(yōu)化,可以提高規(guī)劃路徑的安全性,但并未克服規(guī)劃路徑轉(zhuǎn)折次數(shù)多,路徑不平滑等問題[12]。針對(duì)這些問題,在優(yōu)化選擇子節(jié)點(diǎn)的基礎(chǔ)上,將雙向平滑理念引入到Floyd-Warshall算法中對(duì)A*算法進(jìn)行改進(jìn),同時(shí),設(shè)置障礙物的安全距離,避免碰撞風(fēng)險(xiǎn)。
Floyd-Warshall算法[13]是一種簡(jiǎn)單且廣泛使用的動(dòng)態(tài)規(guī)劃算法,其不需要設(shè)定相應(yīng)的參數(shù)和估計(jì)函數(shù),通過建立兩點(diǎn)之間路徑長(zhǎng)度的二維數(shù)組來計(jì)算最短路徑。將雙向平滑理念引入到Floyd-Warshall算法中,即在優(yōu)化正向路徑的基礎(chǔ)上,加入目標(biāo)點(diǎn)T到起始點(diǎn)S的反向優(yōu)化。具體改進(jìn)步驟如圖5所示。
圖5 改進(jìn)Floyd-Warshall算法優(yōu)化路徑 Fig. 5 Improved Floyd-Warshall algorithm to optimize path
Step1對(duì)路徑中同一直線上的中間冗余節(jié)點(diǎn)進(jìn)行刪除,僅保留起始點(diǎn)S、拐點(diǎn)和目標(biāo)點(diǎn)T。刪除冗余節(jié)點(diǎn)后的路徑為S→p1→p2→p3→T。
Step2從起始點(diǎn)S開始,在保留節(jié)點(diǎn)pi、pj之間每q步取一節(jié)點(diǎn),判斷取的節(jié)點(diǎn)與上一路徑節(jié)點(diǎn)之間有無障礙物。若障礙物存在,則路徑節(jié)點(diǎn)不變;若障礙物不存在,則計(jì)算障礙物與節(jié)點(diǎn)pi、pj之間連線的距離。
障礙物距離如圖6所示,假設(shè)節(jié)點(diǎn)O所在的灰色柵格為障礙物柵格,其坐標(biāo)為(xO,yO);SP為規(guī)劃的路線,S點(diǎn)坐標(biāo)為(xS,yS),P點(diǎn)坐標(biāo)為(xP,yP);障礙點(diǎn)O到SP的距離為dOB,與規(guī)劃路線SP相交于點(diǎn)A,距離為dOA;路線SP與x軸之間的夾角為β。
圖6 障礙物距離Fig. 6 Obstacle distance
由S、P及O三點(diǎn)坐標(biāo)可推導(dǎo)出dOB為
dOB=dOAcosβ,
根據(jù)柵格的大小將路徑安全距離設(shè)置為d,當(dāng)d>dOB時(shí),該路徑可選擇,否則該路徑不可選擇,加入安全距離后的路徑為S→p*3→T。
Step3自目標(biāo)點(diǎn)T反方向取點(diǎn)判斷,其判別方法與Step2相同。反向優(yōu)化后的路徑為S→p*2→T。
Step4輸出路徑,算法結(jié)束。
為驗(yàn)證上述理論分析及改進(jìn)A*算法的有效性,在Matlab 2018a[14]實(shí)驗(yàn)平臺(tái)下分別對(duì)傳統(tǒng)的A*算法、優(yōu)化選擇子節(jié)點(diǎn)改進(jìn)的A*算法和雙向Floyd-Warshall改進(jìn)的A*算法進(jìn)行2組仿真對(duì)比,以避免實(shí)驗(yàn)結(jié)果的偶然性。其中d=0.8 m,q=0.1 m,柵格大小為1 m。黑色柵格為障礙物區(qū)域,占地圖總面積分別為19.5%和13.5%,灰色柵格為遍歷的節(jié)點(diǎn)區(qū)域,圓形形狀代表起始位置,三角形狀代表目標(biāo)位置。兩組柵格地圖路徑規(guī)劃結(jié)果如圖7、8所示。同時(shí)對(duì)比3種算法在兩種不同地圖下的路徑長(zhǎng)度和搜索時(shí)間,結(jié)果如表2所示。
表2 3種算法的路徑長(zhǎng)度和搜索時(shí)間數(shù)據(jù)對(duì)比
由表2的對(duì)比數(shù)據(jù)可得,優(yōu)化選擇子節(jié)點(diǎn)改進(jìn)的A*算法與傳統(tǒng)的A*算法路徑長(zhǎng)度相差不大,但前者搜索效率更高,搜索時(shí)間平均減少了35.44%;雙向Floyd-Warshall改進(jìn)的A*算法在優(yōu)化選擇子節(jié)點(diǎn)的基礎(chǔ)上刪除了路徑中的冗余節(jié)點(diǎn),轉(zhuǎn)折次數(shù)較傳統(tǒng)的A*算法平均減少了37.5%,路徑長(zhǎng)度和搜索時(shí)間平均減少了1.8%及41.06%,提高了路徑規(guī)劃性能。
由圖7~8的仿真波形可得,傳統(tǒng)A*算法所規(guī)劃的路徑存在斜穿障礙物頂點(diǎn)的情況,普適性較差。改進(jìn)后的A*算法在設(shè)置安全距離后,規(guī)劃路徑與障礙物的距離始終大于d,避免了移動(dòng)機(jī)器人距障礙物過近而發(fā)生碰撞的風(fēng)險(xiǎn),提高了路徑的安全性。雖然,優(yōu)化選擇子節(jié)點(diǎn)改進(jìn)的A*算法較傳統(tǒng)A*算法轉(zhuǎn)折次數(shù)有所增加,但尋優(yōu)時(shí)間顯著降低,僅為傳統(tǒng)A*算法的64.56%,控制效果較好。雙向Floyd-Warshall改進(jìn)的A*算法拐點(diǎn)和折點(diǎn)數(shù)量較傳統(tǒng)A*算法有所減少,規(guī)劃的路徑更加平滑,在實(shí)際應(yīng)用中,可以減少機(jī)器人運(yùn)動(dòng)過程中的轉(zhuǎn)動(dòng)角度,有效提高移動(dòng)機(jī)器人運(yùn)動(dòng)的平穩(wěn)性和工作效率,具有一定的實(shí)用價(jià)值。
圖7 第1組柵格地圖路徑規(guī)劃結(jié)果Fig. 7 First group of grid map path planning results
圖8 第2組柵格地圖路徑規(guī)劃結(jié)果Fig. 8 Second group of grid map path planning results
(1)為提高A*算法的搜索效率及規(guī)劃路徑的安全性,提出在A*算法的基礎(chǔ)上對(duì)子節(jié)點(diǎn)的擴(kuò)展順序進(jìn)行優(yōu)化,將8個(gè)領(lǐng)域節(jié)點(diǎn)劃分為高級(jí)組和普通組,縮短尋優(yōu)時(shí)間,避免規(guī)劃路徑斜穿障礙物頂點(diǎn)。
(2)針對(duì)A*算法搜索路徑拐點(diǎn)個(gè)數(shù)多、路徑不平滑等問題,對(duì)Floyd-Warshall算法進(jìn)行雙向平滑處理,在優(yōu)化正向路徑的基礎(chǔ)上,添加反向優(yōu)化,從而減少轉(zhuǎn)角次數(shù),改善路徑質(zhì)量。
(3)改進(jìn)后的A*算法在搜索時(shí)間、路徑長(zhǎng)度和轉(zhuǎn)折次數(shù)方面均優(yōu)于A*算法,路徑的曲率變化連續(xù),更加符合機(jī)器人的運(yùn)動(dòng)控制,具有一定的現(xiàn)實(shí)意義。