面向非結構化環境的移動機器人路徑規劃*

朱偉龍,徐曉輝,宋 濤,李錫哲,汪 曙

(河北工業大學電子信息工程學院,天津 300401)

0 引言

隨著自動導航技術的發展,以移動機器人為基礎的現代機械裝備逐漸普及,相較于傳統人工作業方式,移動機器人開展生產作業效率更高,對工業經濟和人們生活水平的提高具有重要意義[1-2]。

在現代機械裝備領域中,路徑規劃是重要課題之一,大量學者提出多種優秀算法,傳統算法有以A*算法、禁忌搜索算法為代表的全局路徑規劃算法和以動態窗口算法、人工勢場算法為代表的局部路徑規劃算法,智能仿生算法路徑規劃有蟻群算法、粒子群算法、遺傳算法等[3-6]。

NYEONG等[7]針對D*算法在較大地圖內搜索節點冗余問題,提出使用自動聚類算法對地圖進行分割,以消除動態和部分已知環境中不必要的擴展節點。但這種方式為了得到較優的局部地圖,會增加時間開銷。BO等[8]提出了一種基于改進A*算法的完整復蓋路徑規劃算法,使用橫向遍歷與縱向遍歷相結合環境遍歷方法,在提高環境遍歷效率的同時縮短路徑長度,但會造成搜索節點冗余。KIM等[9]針對傳統DWA沒有兼顧障礙物的速度和方向問題,提出匹配移動節點與動態障礙物的速度和方向,劃分障礙物識別區域,提高DWA算法的環境適應性,但由于沒有全局路徑導向,容易陷入局部最優解。

龔鵬等[10]基于A*算法融合JPS搜索策略對子節點跳躍搜索,并基于Floyd算法對路徑進行平滑處理,提高算法路徑搜索效率,減小路徑總轉折角度,但未考慮局部路徑規劃,不具備動態避障能力。高鵬等[11]對果園移動機器人建圖定位和路徑規劃進行研究,減小A*算法的貪心程度避免得到路徑次優解,但改進后的A*算法存在轉彎角度大的問題。楊保海等[12]對人工勢場算法進行改進,重構識別路徑,引入障礙有關的邊界條件,經過反復迭代由各個局部最優解構成全局路徑,但節點搜索量較大。

上述學者提出的路徑規劃算法雖然表現出色,但存在一定的局限性,不能兼顧靜態與動態路徑規劃。提出改進A*算法與動態窗口法相結合路徑規劃算法,改進A*算法減少子節點遍歷次數,算法搜索效率得到提升,對回溯完成的路徑進行平滑處理,提高移動機器人作業時流暢程度,以改進A*算法得到的全局路徑為引導路線,引入DWA動態路徑規劃算法,避免陷入局部最優解,實現機器人全局路徑規劃和動態避障。

1 柵格地圖模型搭建

在解決具體的路徑規劃問題之前,需要對環境進行數學模型搭建。利用柵格法對移動機器人作業環境進行模擬,如圖1所示,使用直角坐標系對柵格地圖進行描述,每個柵格在坐標系中有對應的二維坐標(xi,yi)。為了方便描述柵格點,將二維矩陣下標轉換為線性索引,每一個柵格由線性索引值唯一標識,二維下標到線性索引的計算公式為:

圖1 非結構化環境柵格地圖

k=max(|xi|,|yi|)

(1)

(2)

式中:k為柵格橫縱坐標絕對值中的最大值,xi和yi值為單元格左下角的橫縱坐標值。將白色區域定義為無障礙可通行區域,黑色方形柵格定義為障礙物,圓形為起始位置,三角形為終點位置,五角星為中間引導點,移動機器人從S點出發,依次尋找G1、G2……G6、G,來找到終點位置,通過這種方式能夠保證移動機器人遍歷整個柵格地圖。

2 A*算法及DWA算法

2.1 傳統A*算法

(a)*算法是一種快速高效的啟發式算法,常用搜索靜態路網中的最短路徑。其核心是估價函數f(n),即從初始點經由節點n到目標點的距離,表達式如式(3)所示[13]。

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

(3)

g(n)=g(n-1)+step(n-1,n)

(4)

式中:n表示當前經過節點,step(n-1,n)表示進行一次路徑規劃的代價值,g(n)表示從起點到當前節點n的累計路徑代價函數,啟發函數h(n)表示當前節點n到目標點最佳路徑的估計距離[14]。主要算法流程分為4個步驟:

步驟1:更新起始點、終點、open列表、close列表等,并把起始點加入open列表中,計算對應的估價函數;

步驟2:判斷open列表狀態,如果為空則路徑搜索失敗,不為空則計算列表中估價函數值最小的點;

步驟3:判斷f(n)值最小的點是否為終點,如果是則構建回溯路徑,路徑規劃結束,否則將該點從open列表中移除,加入到close列表中,以該點為父節點搜索子節點,更新open列表;

步驟4:重復步驟2～步驟3,直至找到終點完成路徑規劃。

2.2 DWA算法

DWA算法是一種典型的動作采樣算法,在速度空間內對速度進行采樣。DWA算法差速移動機器人運動學模型如式(5)所示,對線速度和角速度進行多組采樣構成速度空間(v,ω),移動機器人速度約束如式(6)所示。

(5)

(6)

式中:v(t)和ω(t)分別表示移動機器人的線速度和角速度[15]。

從實際考慮,移動機器人的動力裝置存在力矩限制,在下一單位時間內的最大速度和角速度為:

(7)

式中:va、vb、vc分別表示最大線速度、最小線速度、當前線速度,ωa、ωb、ωc分別表示最大角速度、最小角速度以及當前角速度[16]。機器人的運動可以分解成直線運動與旋轉運動,路徑規劃中為避免與平臺與障礙物相撞定義最大減速安全距離dist(v,ω),添加制動距離約束:

(8)

3 改進A*算法

3.1 引入動態權重參數

根據移動機器人數據采集作業任務需求,對傳統A*算法估價函數進行優化,針對啟發函數引入動態權重參數,優化后的估價函數為:

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

(9)

(10)

式中:w(n)為估計代價h(n)當前的權重系數,l為當前點到目標點的距離,L為起始點到目標點的距離。傳統A*算法在尋找最小代價節點時,會根據排序后的估計代價值選擇最優的節點放進close列表中,引入權重系數w(n)的目的是提高h(n)對估價函數的影響,將靠近目標點的子節點加入close列表,減少A*算法搜索過程中子節點遍歷次數,提高算法搜索效率。

3.2 路徑平滑

(a)*算法改進另一項內容是對路徑進行平滑處理,該優化過程分為兩個步驟:①選擇較高鄰域節點搜索策略,減少路徑規劃過程中的冗余節點和拐點;②對回溯完成的路徑進行平滑處理。

低鄰域節點搜索策略規劃出的路徑拐點較多,移動機器人在作業過程中需要高頻次轉向,影響工作效率。如圖2a展示的4鄰域搜索策略,移動機器人只能沿著4個方向運動,每次轉向均為直角,導致移動機器人運動不夠流暢。高鄰域搜索策略能夠提供更多的運動方向,增加啟發函數的遍歷范圍,減少路徑中轉向次數。圖2b、圖2c展示了另外兩種搜索策略,在4鄰域搜索的基礎上分別增加4個、12個搜索搜索方向,父節點每次遍歷的子節點數目增加。

(a) 4鄰域 (b) 8鄰域 (c) 16鄰域

為進一步驗證不同鄰域搜索策略路徑規劃效果,在10*10柵格地圖中進行仿真實驗,如圖3所示。

(a) 4鄰域 (b) 8鄰域 (c) 16鄰域

圖4 融合算法流程圖

圖3中設定黑色圓形為起點,黑色三角為終點,定義方格單位長度為1 m,對比分析3種不同鄰域搜索策略數據,8鄰域與16鄰域搜索策略規劃出的路徑代價相近,低于4鄰域搜索策略規劃出的路徑長度。16鄰域搜索策略規劃出的路徑轉向次數最少,路徑更為流暢。

表1 不同鄰域搜索策略數據對比

使用高鄰域搜索策略得到轉折點相對更少的路徑,為了保證路徑更加平滑,基于梯度下降算法與SG濾波器對回溯完成的路徑進行曲線平滑處理。

梯度下降算法是最優化方法的基礎,常用來求曲線的極值。梯度指函數在某點的方向導數,將多點連接的函數定義為非線性函數y=f(x,θ),θ為偏置項參數。則平均損失函數為:

(11)

式中:L(f(xi,θ),yi)為路徑規劃節點第i個樣本的損失函數,▽θL(f(xi,θ),yi)為損失函數對θ的偏導,得到路徑曲線在第i個節點的梯度值。平均損失函數沿著梯度負方向變化減小,即:

θ-ε▽θL(f(xi,θ),yi)→θ

(12)

式中:ε為學習率,用來控制梯度下降的步長。梯度下降算法的本質是在close列表中數據的基礎上,經過迭代計算與參數調整,使優化前后目標點偏移程度最小以及相鄰序列距離最小。

(13)

式中:x(j)、y(j)分別為close列表中第j個點的橫、縱坐標值,xi(j)、yi(j)分別為迭代第i次對應的第j個點的橫、縱坐標值,(xi(j-1),yi(j-1))、(xi(j+1),yi(j+1))分別為(xi(j),yi(j)前一個與后一個坐標值,通過多次迭代與調整α、β值獲取最優的close列表。

SG濾波算法使用一元M階多項式對close列表中的每一個數據點鄰域內各點的數據進行擬合,廣泛地運用于數據流平滑除噪。對close列表中的節點進行梯度下降算法處理后,再進行濾波器處理,提高數據的平滑程度[17]。

4 算法融合

(a)*算法只考慮坐標信息,沒有將運動的偏航角信息融入的路徑規劃中,導致移動機器人路徑規劃缺少運動學約束,這是造成路徑不平滑的根本原因。先由全局路徑規劃算法規劃出一條全局路徑,然后局部路徑規劃算法將全局路徑分割成多條小段,再進行局部路徑規劃,將全局路徑分成多條局部路徑依次完成。該操作流程有兩點優勢:①在進行全局路徑規劃時能夠對地圖已經保存的障礙物進行避障;②在進行局部路徑規劃時對動態障礙物進行避障,全局路徑規劃和局部路徑規劃相互配合共同完成導航與避障功能。

5 仿真與實驗

5.1 A*算法優化仿真

針對A*算法的優化主要從引入動態權重參數與路徑平滑兩方面進行,為進一步驗證優化算法的有效性和可行性,在MATLAB平臺中建立10*10的柵格地圖,基于障礙物環境設置路徑起始點和終止點。

5.1.1 引入權重參數

如圖5和表2所示,權重參數w(n)取不同值時,路徑規劃的搜索節點數量和運行時間存在差異。傳統A*算法默認w(n)=1,基于式(9)在啟發函數中引入動態權重參數。實驗數據表明,引入動態權重參數之后A*算法運行時間明顯下降,運行時間從0.711 2 s下降到0.617 2 s,同時路徑拓展的子節點數目從35個下降到28個,算法搜索效率得到提升。

表2 w(n)取不同值對路徑的影響

(a) w(n)=1 (b) w(n)=1+l/L

5.1.2 路徑平滑

引入權重參數后,在高鄰域搜索A*算法基礎上融入梯度下降算法和SG濾波器。為得到相對較優的曲線,通過多組仿真實驗確定梯度下降算法學習率參數,選擇10*10的柵格地圖進行測試,在不同迭代次數、α、β值情況下進行仿真實驗,圖6為多組實驗的仿真結果。

圖6 梯度下降算法仿真結果

(a) α=0.25,β=0.50 (b) α=0.26,β=0.52

仿真結果表明,當α=[0.05,0.10,0.15,0.20,0.25,0.30,0.35],β=[0.1,0.2,0.3,0.4,0.5,0.0]時,隨著迭代次數增加,路徑長度收斂于14.078 3 m,相較于傳統A*算法15.650 3 m縮短了10.04%。α=0.40,β=0.8時迭代優化結果出現畸變,α=0.25,β=0.5與α=0.30,β=0.6兩組迭代收斂速度較快,在迭代15次后收斂到最優值。

在α∈[0.25,0.30],β∈[0.5,0.6]區間內選擇6組數據進行仿真實驗,記錄A*算法路徑長度(Length1=15.650 28 m)、A*算法+梯度下降算法長度(Length2)、A*算法+梯度下降算法+SG濾波器長度(Length3)、A*算法+梯度下降算法耗時(Timer2)、A*算法+梯度下降算法+SG濾波器耗時(Timer3)。如圖10所示,L1表示A*算法規劃路徑,L2表示A*算法+梯度下降算法規劃路徑,L3表示A*算法+梯度下降算法+SG濾波器規劃路徑。

如表3和表4所示,在迭代10次的情況下,α=0.27,β=0.54生成路徑最短,α=0.30,β=0.60用時最短,對比圖10d與圖10f,圖10f圖像有明顯波動,導致Length2與Length3數值變大。

表3 不同參數路徑對比 (m)

表4 不同參數時間對比 (s)

對比每一組實驗的路徑長度數據,經過SG算法處理的路徑長度Length3較于Length1平均減小16.82%,較于Length2平均減小7.62%。

5.2 融合算法仿真

5.2.1 全局靜態路徑規劃

為驗證融合算法的有效性,模擬30*30的非結構化環境地圖,在該柵格地圖上對高鄰域搜索A*算法、改進A*算法、融合算法、遺傳算法(GA)、蟻群算法(ACO)、多因素蟻群算法(文獻[18])進行仿真實驗,從起點依次經過引導點找到終點,相關算法關鍵參數如表5所示。

表5 部分關鍵參數設置

路徑規劃仿真實驗結果如圖8所示,算法迭代曲線對比如圖9所示。

(a) 16鄰域A*算法 (b) 改進A*算法

圖9 迭代曲線對比

移動機器人在中間點的引導下規劃出全局路徑,雖然使用了高鄰域搜索策略,但全局路徑轉折角度較大,如圖8a所示,該條件下路徑長度為208.600 2 m,如表6所示。引入梯度下降算法和SG濾波器的目的是得到相對平滑的全局路徑,在α=0.27,β=0.54的條件下,迭代10次后收斂,得到如圖8b所示的全局路徑,此時路徑長度為198.686 0 m,相較于高鄰域搜索A*算法路徑長度下降了4.75%。在靜態環境中,融合算法與高鄰域搜索A*算法效果相近,遺傳算法、蟻群算法與文獻[18]提出的多啟發因素蟻群算法規劃路徑長度較于高鄰域搜索A*算法路徑分別增加0.41%、5.18%、2.91%,且對比圖8中的路徑曲線,該3種算法規劃的路徑不夠平滑,路徑轉折角度較大。在該30*30非結構化環境中,本文提出的改進算法性能更優。

表6 不同算法路徑長度對比

5.2.2 全局動態路徑規劃

在改進A*算法的路徑中加入新增障礙物,在圖1基礎上增加隨機障礙物六角形,如圖10所示。

(a) 隨機障礙物區域1 (b) 隨機障礙物區域2

在改進A*算法的基礎上,引入DWA算法的目的是確保移動機器人擁有全局和局部路徑規劃的能力,在全局路徑規劃的基礎上躲避動態障礙物。圖10a、圖10b和圖10d描述了融合算法避開新增障礙物過程,在保證全局路徑最優的基礎上實現動態路徑規劃,有效規避3個隨機障礙物區域,且得到的路徑更加平滑,如圖10d所示。證明改機A*算法融合DWA算法在動態環境下路徑規劃的可行性。圖11為移動機器人動作過程中輸出的姿態角度、線速度以及角速度。

(a) 機器人姿態角度 (b) 機器人運行線速度

6 結束語

(1)針對A*算法引入權重參數,對估價函數進行優化提高子節點搜索效率,并在高鄰域搜索策略基礎上引入梯度下降算法和SG濾波算法對路徑進行平滑處理。

(2)針對移動機器人動態避障問題,在改進A*算法基礎上引入動態窗口法,將全局路徑劃分為多個局部路徑,分段完成多個局部路徑規劃。

(3)在MATLAB平臺對涉及到的算法進行仿真實驗,引入權重參數后路徑搜索效率得到提升,利用梯度下降算法與SG濾波算法進行曲線平滑后,全局路徑總拐角數量較少,確保移動機器人能夠流暢開展作業,在全局路徑隨機放置障礙物,移動機器人能夠實施動態避障,實驗結果表明了融合算法在路徑規劃中的有效性和可行性。