陸皖麟,雷景森,邵 炎
(66133部隊, 北京 100043)
路徑規(guī)劃技術(shù)是移動機器人研究的熱點問題,對它的研究為控制機器人自主行駛奠定基礎(chǔ)[1]。路徑規(guī)劃的定義可以表示為:設(shè)定起始點、目標點后,依據(jù)規(guī)劃算法搜尋從起點至目標點同時可以避障的一條最佳路線。依照機器人的環(huán)境情況將運動路徑列為兩類:一類是基于地圖信息已知的全局路徑規(guī)劃,此類路徑規(guī)劃環(huán)境中的信息包括障礙物和可行駛區(qū)域全部已知;另外一類是根據(jù)相機、慣性導(dǎo)航、里程計等獲取實時信息的局部路徑規(guī)劃,局部路徑規(guī)劃是處在障礙物信息未知的環(huán)境[2]。常用的全局路徑規(guī)劃算法有[3]:A*算法、Dijkstra算法、粒子群算法、遺傳算法等。A*算法最早由Nilsson提出來的啟發(fā)式算法,它的核心是對目標點進行不斷搜尋,從而取得機器人的避障路徑。通過對狀態(tài)空間中搜索點進行評價,取得最佳節(jié)點,然后依據(jù)此位置節(jié)點繼續(xù)進行搜尋,一直到找到目標點為止。該算法具有編程方法相對簡單,參數(shù)設(shè)置較少,搜索路徑效率高[4]的特點。
A*算法是獲取最短路徑比較有效的搜索方法,也是典型的啟發(fā)式算法。A*算法一般用于靜態(tài)全局規(guī)劃,其核心表達式為
f(n)=g(n)+h(n)
(1)
其中:f(n)表示從起點開始經(jīng)過當(dāng)前節(jié)點到目標節(jié)點的路徑長度;g(n)表示起點至當(dāng)前節(jié)點n的路徑長度,此時g(n)已經(jīng)是機器人從起始點到當(dāng)前點的最短距離;h(n)是從狀態(tài)節(jié)點n到目標狀態(tài)的估計代價函數(shù)距離。由于g(n)是實際距離,為了在靜態(tài)地圖可以搜尋到最優(yōu)解,選擇代價h(n)類型非常重要,f(n)的值取決于h(n)的大小。下面描述算法搜索節(jié)點過程,如圖1所示,綠色小格表示起始點位置,紅色小格表示目標點位置,藍色小格周圍的8個點表示可以設(shè)定為當(dāng)前狀態(tài)空間的子節(jié)點候選點,設(shè)小格的一條邊長度為10,式(1)簡寫為F=G+H,G表示起始點至當(dāng)前狀態(tài)的曼哈頓距離,H是當(dāng)前狀態(tài)節(jié)點至目標節(jié)點估計代價函數(shù)值,這里代價函數(shù)H的計算使用的是歐式距離,起始點周圍8個格子F、G、H各數(shù)值見圖,由圖可知2號小格F最小,將其作為接下來要處理的點,然后檢查2號周圍的小格的G、H值,黑色方格是障礙物,在計算G、H值時忽略其不可通行,確定2號的待選小格是1號、3號。由于1號、3號是父節(jié)點(起始點)的8個相鄰節(jié)點、3號節(jié)點周圍沒有可選新的相鄰節(jié)點,故從起始點選擇待處理節(jié)點是1號而不是2號、3號。按照此思路,4號節(jié)點是1號節(jié)點的子節(jié)點。
圖1 A*算法搜索節(jié)點
在已構(gòu)建的地圖中,設(shè)起始點坐標節(jié)點為S(Sx,Sy),當(dāng)前節(jié)點C(Cx,Cy),目標節(jié)點的坐標T(Tx,Ty),若使用相對簡單的歐式距離估價函數(shù)值可表示為
(2)
下面將以偽代碼的形式描述A*算法流程,首先設(shè)置各項參數(shù),start:機器人路徑規(guī)劃的出發(fā)點;goal:機器人路徑規(guī)劃的目標點位置;g(n):n節(jié)點到start點的實際移動距離;h(n):n節(jié)點到達目標點的理論移動距離;openlist:搜尋節(jié)點的過程中保存未檢測的節(jié)點列表;closelist:已經(jīng)被檢測的節(jié)點保存至該列表;comaFrom:保存子節(jié)點和父節(jié)點,最終生成的路徑在此列表中。使用偽代碼為:
把起點加入到openlist
do
{
搜尋openlist中f(n)值最小的節(jié)點,它表示當(dāng)前節(jié)點
加入至closelist
對當(dāng)前節(jié)點相鄰的所有候選節(jié)點
if(是障礙物點‖在closelist中)
{
繼續(xù)搜尋下一個相鄰候選節(jié)點
}
if(openlist不存在此節(jié)點)
{
將此候選節(jié)點加入openlist,更新父節(jié)點,計算它的f(n)、g(n)、h(n)值
}
if(候選節(jié)點已經(jīng)在openlist內(nèi))
{
if(使用g(n)值作為評判標準,來檢測新的路徑,判斷是否有更低的g(n))
{
將它的父節(jié)點換為當(dāng)前節(jié)點,計算當(dāng)前節(jié)點的f(n)、h(n)的值
}}
使用comaFrom保存父節(jié)點
}while(目標節(jié)點在openlist里面,表示路徑規(guī)劃成功)
若openlist列表是空值,表示沒有可尋找的路徑。
最后把comaFrom保存的所有父節(jié)點從起點開始以此連接起來,該路線就是A*算法所求的路徑。
傳統(tǒng)A*算法路徑規(guī)劃程序結(jié)構(gòu)如圖2所示。
盡管傳統(tǒng)A*算法已經(jīng)成為移動機器人導(dǎo)航中廣泛使用的路徑規(guī)劃算法,但是算法規(guī)劃出來的路徑存在拐點多,轉(zhuǎn)折次數(shù)多的問題,在實際環(huán)境中不利于機器人的行駛[5]。因此在研究A*算法的基礎(chǔ)上提出一種適宜機器人行駛改進的A*算法。
圖2 傳統(tǒng)A*算法路徑規(guī)劃程序框圖
為解決A*算法在實際路徑規(guī)劃中不能求最短路徑,Anthony Stentz提出基于插值方法的D*方法[6],使路徑變得平滑,但采用插值的方法使計算量增大,實時性變差。遺傳算法、神經(jīng)網(wǎng)絡(luò)算法等智能算法也用于路徑規(guī)劃,但對于靜態(tài)地圖而言智能算法花費時間較多。為優(yōu)化A*算法的規(guī)劃路徑,減少路徑的長度,針對A*算法的3個方面加以改進:
1) 設(shè)定openlist訪問時間閾值
傳統(tǒng)A*算法在當(dāng)前節(jié)點擴展子節(jié)點時,每次都要把所有候選節(jié)點擴展完畢,這種方法在復(fù)雜環(huán)境下可能存在一定的無效搜索,將會耗費一定的時間,因此,在傳統(tǒng)A*算法的基礎(chǔ)上,每次檢測openlist表中最先插入的節(jié)點在經(jīng)過一定時間閾值N后,判斷是否得到擴展,若未擴展,則將此節(jié)點作為最高級優(yōu)先擴展,在一定程度可以避免陷入局部最小值,解決無法規(guī)劃出全局路徑問題。
2) Floyd算法優(yōu)化
Floyd算法借鑒動態(tài)規(guī)劃的思路搜尋最短路徑,是用來解決兩點之間的最優(yōu)距離問題的算法[7]。使用Floyd算法和A*算法結(jié)合的方法以減少路徑長度,符合實際應(yīng)用需求。Floyd算法原理如圖3所示。
圖3 Floyd算法原理
L(A,D)為A、D兩點間的距離,如圖3,A、D存在障礙物,可設(shè)L(A,D)=+∞,R(A,D)=A->D
B點是A點和D之間已規(guī)劃出的節(jié)點:
若
L(A,B)+L(B,D) (3) 則 L(A,D)=L(A,B)+L(B,D) (4) R(A,D)=A->B->D (5) 在B、D線段上插入點C: 若 L(A,C)+L(C,D) (6) 則 L(A,D)=L(A,C)+L(C,D) (7) R(A,D)=A->C->D (8) 去除C點,A到D的優(yōu)化路徑表示為一段優(yōu)化后的弧形路徑R(A,D)。采取Floyd算法優(yōu)化A*算法規(guī)劃的路徑能夠刪除多余的點,更加適合移動機器人行駛。 3) 路徑光滑處理 A*算法規(guī)劃的路徑存在許多尖銳的拐點和折點,這種拐點不符合移動機器人的平穩(wěn)運動。為了滿足移動機器人在規(guī)劃的路徑上能夠平穩(wěn)移動,需要對規(guī)劃的路徑進行平滑處理[8],用平滑的曲線來代替尖銳點。其原理框圖如圖4。 圖4 平滑處理原理框圖 (9) 過直線BC方程為 (10) 由圖可知 (11) 設(shè)圓弧半徑 |p1r|=|p2r|=R (12) 由式可得 |p1B|=|p2B|=R·cot(δ/2) (13) 設(shè) (14) 由A,B點坐標和式得p1和p2點坐標: p1·x=ε1·x1+(1-ε1)·x2 p1·y=ε1·y1+(1-ε1)·y2 p2·x=(1-ε2)·x2+ε2·x3 p2·y=(1-ε2)·y2+ε2·y3 (15) 過p1點和點p2分別做直線AB和BC的垂線,其交點為。則圓弧方程為: (x-ry)2+(y-ry)2=R2 (16) 那么軌跡A->P1->P2->C的方程為: (17) 通過以上方法可以在拐點、折點處,采取Floyd算法去除多余的點,縮短機器人行駛路徑,然后再平滑算法處理路徑,從而減少拐點的彎折程度,有利于機器人轉(zhuǎn)向行駛以及避障操作[9]。A*算法改進后的程序流程框圖如圖5所示。 圖5 改進A*算法路徑規(guī)劃程序流程框圖 為驗證改進后的A*算法效果,仿真使用MATLAB2016b進行編程。在MATLAB中采用GUI設(shè)計A*算法圖形化驗證界面,如圖6所示,在guide里面設(shè)置兩個按鈕,一個命名為A*算法路徑規(guī)劃,另外一個命名為生成障礙物,它們會自動生成.m文件,使用GUI可編輯文本設(shè)置起點坐標、目標點坐標,分別在A*算法路徑規(guī)劃和生成障礙物按鈕生成的.m文件導(dǎo)入相關(guān)的代碼,修改對應(yīng)Function函數(shù)的參數(shù),生成圖7的GUI界面,點擊生成障礙物按鈕,會出現(xiàn)長和寬為32×32,且周圍有一定障礙物的地圖,地圖上一個柵格單位長為1。 圖6 仿真界面GUI設(shè)計 圖7 A*算法驗證界面 構(gòu)建仿真環(huán)境地圖,地圖中障礙物隨機設(shè)置,同時在GUI編輯文本中隨機設(shè)置起點坐標為(3,3),目標點隨機設(shè)置為(29,22)。仿真功能就是實現(xiàn)從起點(3,3)至(29,22)尋找一條最優(yōu)路徑。實驗一采用傳統(tǒng)A*算法進行路徑規(guī)劃,路徑仿真結(jié)果見圖8(a);實驗二僅設(shè)定openlist訪問時間閾值,進行路徑規(guī)劃,路徑仿真結(jié)果見圖8(b);實驗三在實驗二基礎(chǔ)上增加Floyd和平滑處理算法,路徑仿真結(jié)果見圖8(c)。 圖8 仿真路徑規(guī)劃效果 分析對比實驗結(jié)果可知改進后的算法路徑總長度縮短了,拐點和折點數(shù)量有所降低,提高了路徑平滑度,更加適合移動機器人的運動。統(tǒng)計實驗數(shù)據(jù)如表1:改進后規(guī)劃時間略有增加,這是因為增加了Floyd算法和平滑算法。但規(guī)劃時間相差不是很大,在可以接受范圍內(nèi),這是由于對算法做了改進,在開啟列表(openlist)增加了一個閾值N,提高了尋找子節(jié)點的效率。 表1 仿真實驗數(shù)據(jù) 1) 基于靜態(tài)環(huán)境下A*算法,為了提高路徑規(guī)劃的實時性并且減少搜索節(jié)點數(shù)量,提出了在A*算法的openlist加入判斷閾值N,若搜索節(jié)點的次數(shù)超過N,判斷最初加入的節(jié)點是否擴展,未擴展設(shè)它為最高級擴展節(jié)點,該方法能夠縮短規(guī)劃時間。 2) 針對傳統(tǒng)A*算法搜索路徑存在的拐點多、路徑彎折程度大等問題利用Floyd算法進行優(yōu)化,然后進行路徑平滑處理:用圓弧曲線代替折點使其適合機器人運動。 3) 改進后的A*算法與傳統(tǒng)A*算法的MATLAB仿真結(jié)果對比分析可知,兩者規(guī)劃時間差別不大,但改進后的路徑更加平滑,路徑長度更短,更利于機器人的行駛。4 仿真實驗與分析
5 結(jié)論