趙 偉，吳子英

西安理工大學 機械與精密儀器工程學院，西安710048

移動機器人的路徑規(guī)劃是求解機器人起始點與目標點之間的位置序列[1]。在動態(tài)環(huán)境中，移動機器人既要做到實時感知周圍環(huán)境無碰撞抵達目標點，還要做到路徑質量盡可能高。這就要求動態(tài)環(huán)境下移動機器人的路徑規(guī)劃需要結合全局路徑規(guī)劃與局部路徑規(guī)劃。移動機器人的路徑質量主要涉及規(guī)劃時間、路徑長度、折轉次數(shù)、軌跡平滑度、障礙物距離等方面。其中規(guī)劃時間與路徑長度影響機器人的移動效率，折轉次數(shù)與軌跡平滑度影響機器人運動連貫性，障礙物距離決定機器人移動的安全性。根據(jù)對環(huán)境信息的掌握情況，移動機器人的路徑規(guī)劃可以分為全局路徑規(guī)劃與局部路徑規(guī)劃，常用的局部路徑規(guī)劃算法為動態(tài)窗口法[2]、粒子群算法[3]、遺傳算法[4]等。常用的全局路徑規(guī)劃算法有A*算法[5-6]、Dijkstra算法[7]、蟻群算法[8]等。

動態(tài)窗口法常用于移動機器人的局部路徑規(guī)劃，通過對機器人速度空間的約束來實現(xiàn)對局部環(huán)境避障計算。傳統(tǒng)的動態(tài)窗口法存在兩個缺陷：面對諸如“凹”形與“C”形障礙物的時候，容易陷入局部最優(yōu)無法繼續(xù)前行；規(guī)劃路徑長度較長，無法做到全局路徑最優(yōu)。針對動態(tài)窗口法的兩大缺陷，程傳奇等人構造了顧及全局的最優(yōu)路徑評價函數(shù)，使得動態(tài)窗口法在選取軌跡時更加側重全局最優(yōu)[9]；卞永明等人定義了新的轉點評價子函數(shù)，解決了動態(tài)窗口法容易陷入“C”形障礙物且容易繞行濃密障礙物的問題[10]；華洪等人受到動態(tài)窗口算法的啟發(fā)提出了一種多重A*算法下的動態(tài)避障策略，在局部路徑規(guī)劃中，基于云的協(xié)作定位系統(tǒng)，使用卷積層和ConLSTM層的組合算法確定動態(tài)避障與局部路徑的軌跡關系，使得局部避障的轉角與規(guī)劃時間大大降低[11]。A*算法是一種有效的全局路徑規(guī)劃算法，其顯著的優(yōu)點在于計算時間快，求得的路徑長度較短，但同時也存在著折轉次數(shù)較多、路線平滑度較低等問題。針對A*算法的問題，勞彩蓮等人先從A*規(guī)劃出的軌跡獲得一系列點，通過斜率判斷運動方向，將相同運動方向的中間點刪除，只留下關鍵轉折點，減少了路徑折轉次數(shù)[12]。陳嬌等人通過從A*規(guī)劃軌跡的起點開始做直線，依次連接路徑點，判斷直線是否通過障礙物來確定關鍵轉折點，刪除冗余點，降低了路徑折轉次數(shù)[13]?；眲?chuàng)鋒等人將傳統(tǒng)A*算法的3×3搜索鄰域擴展到7×7搜索鄰域，優(yōu)化了路徑的搜索角度，減少了路徑轉折以及避免了過大轉折角的出現(xiàn)[14]，但是較多的搜索方向增加了運行內存的消耗。

針對動態(tài)環(huán)境中移動機器人既要動態(tài)避障又要做到全局最優(yōu)的路徑規(guī)劃需求，本文提出了雙層優(yōu)化A*算法與動態(tài)窗口法融合的動態(tài)路徑規(guī)劃算法。針對傳統(tǒng)動態(tài)窗口法面對“凹”形與“C”形障礙物容易陷入局部最優(yōu)無法前行，以及規(guī)劃路徑無法做到全局最優(yōu)的缺陷，通過引入雙層優(yōu)化A*算法來使動態(tài)窗口法的規(guī)劃軌跡更貼近全局最優(yōu)。首先通過對傳統(tǒng)A*算法的一層優(yōu)化獲得關鍵轉折點路徑；再通過二層優(yōu)化獲得轉折點數(shù)量更少的全局路徑規(guī)劃；最后通過設計評價函數(shù)，引入“路徑評價子函數(shù)”將雙層優(yōu)化A*算法與動態(tài)窗口法結合為融合算法。分別在靜態(tài)環(huán)境與動態(tài)環(huán)境中對融合算法進行仿真實驗，實驗結果驗證了本文算法對移動機器人運動動態(tài)避障與全局最優(yōu)的雙重要求。

1 全局路徑規(guī)劃

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

傳統(tǒng)A*算法是一種有效的全局路徑規(guī)劃算法，其路徑規(guī)劃運算過程可以通過以下流程來表示：首先初始化地圖信息，包括起點、終點、障礙物等，創(chuàng)建openlist與closelist 兩個節(jié)點列表，并將起點放到openlist 中，設定起點為父節(jié)點。建立一個循環(huán)，首先判斷終點是否在closelist 表中，若終點不在closelist 中，則計算當前父節(jié)點周圍領域的八個子節(jié)點：若子節(jié)點為障礙物無法達到，則忽略它；若子節(jié)點已經在closelist中，忽略它；若子節(jié)點可以到達且不在openlist 中，則將子節(jié)點添加到openlist 中；若子節(jié)點已經在openlist 中，檢查其節(jié)點的代價值是否比當前節(jié)點小；之后對openlist 表中的節(jié)點按照代價值大小排序，將代價值最小的子節(jié)點從openlist表中刪除并添加到closelist中，之后跳轉其上，設定該點為父節(jié)點，繼續(xù)判斷終點是否存在于closelist 中重復上述循環(huán)。若openlist表為空表，則表示無路徑到達終點；若終點存在于closelist中，則表示找到路徑并結束循環(huán)，將closelist表中的坐標點按照起點到終點的順序依次連接得到所求路徑。

傳統(tǒng)的A*算法的代價值由兩部分組成：子節(jié)點到起點的累積代價與子節(jié)點到終點的累積代價[15]。A*算法的代價值函數(shù)可以表示為：

式中，F(xiàn)(n)表示當前子節(jié)點的總代價；G(n)表示子節(jié)點到起點的累積代價；H(n)表示子節(jié)點到終點的累積代價。

本文采用雙維靜態(tài)地圖模型，因此任意兩點(x1,y1)、(x2,y2)之間的代價值d采用歐式距離來計算：

傳統(tǒng)A*算法在求解障礙物較多的地圖模型時，得到的路徑折轉點較多，頻繁的折轉使得機器人在實際尋路流程中經常出現(xiàn)卡頓、卡死等狀況，較多的折轉點還使得機器人每次只能行駛較短的一段直線路段，無法用較大的加速通過，變相地增加了尋路耗時。為了解決A*算法轉折點較多、路徑長度過長等問題，本文采用雙層全局優(yōu)化。首先通過第一層優(yōu)化來提取關鍵折轉點，刪除大量的冗余點；在第一層優(yōu)化的基礎上進行第二層優(yōu)化，將路徑轉折點降到最低，得到折轉點數(shù)量最小的一條優(yōu)化路徑。如圖1 所示為傳統(tǒng)A*算法在場景1 中的尋路求解結果，其中黃色方格代表起點，藍色方格代表終點，黑色方格代表邊界與障礙物，紅色方格代表路徑轉折點，用黑色線段依次連接起點、路徑點、終點得到算法求得的路徑規(guī)劃。

圖1 傳統(tǒng)A*算法全局規(guī)劃Fig.1 Global planning of traditional A* algorithm

1.2 一層全局優(yōu)化

在傳統(tǒng)A*算法求得全局路徑規(guī)劃之后，首先提取路徑轉折拐點，對路徑轉折拐點進行篩選剔除，刪除冗余拐點，具體篩選剔除策略如圖2所示。

圖2 一層優(yōu)化A*算法示意圖Fig.2 Schematic diagram of one-level optimization A* algorithm

（1）將傳統(tǒng)A*算法求得的路徑點坐標Nodei(i=1,2,…)按照從起點到終點的順序依次存儲在坐標集path中；將地圖的邊界與障礙物等不可到達區(qū)域存儲在集合Obstacle中。

（2）將起點與終點放置到坐標集Fpo（First path optimization）中，并設定起點為當前開始點NodeS；之后按順序依次計算路徑點Nodei與開始點NodeS兩點連線的斜率ki。

（3）若ki ＋1=ki，則表示點NodeS、Nodei與Nodei ＋1三點共線，忽略該路徑點Nodei，繼續(xù)計算ki ＋2；若ki ＋2=ki，則忽略路徑點Nodei ＋1，繼續(xù)計算ki ＋3…。

（4）若存在ki ＋j≠ki，其中j=1,2,…,則對路徑點Nodei ＋j進行條件判斷：若從NodeS指向Nodei ＋j的連線不經過Obstacle區(qū)域，則忽略該路徑點Nodei ＋j，繼續(xù)按照步驟（3）往下循環(huán)計算ki ＋j ＋1…；若從NodeS指向Nodei ＋j的連線經過Obstacle區(qū)域，則路徑點Nodei ＋j－1為關鍵轉折點，將路徑點Nodei ＋j－1放置到坐標集Fpo中，并將路徑點Nodei ＋j設置為新的當前開始點NodeS，將路徑點Nodei ＋j ＋1重置為Nodei，重復（2）—（3）—（4）的循環(huán)過程，直到抵到終點結束。

（5）將坐標集Fpo中的路徑點按照從起點到終點的順序依次連接，得到第一層全局優(yōu)化的路徑規(guī)劃結果。

按照場景1 的柵格地圖對傳統(tǒng)的A*算法進行第一層路徑優(yōu)化，其優(yōu)化結果如圖3所示。通過對比可以看出，傳統(tǒng)A*算法求得的路徑轉折點數(shù)為7個，優(yōu)化后的路徑轉折點為3個，路徑轉折點數(shù)量有效地減少。

圖3 一層優(yōu)化A*算法路徑規(guī)劃Fig.3 Path planning of one-level optimization A* algorithm

1.3 二層全局優(yōu)化

通過第一層的全局優(yōu)化，A*算法求得的路徑中的拐點數(shù)有效地減少了，提高了路徑的運動連貫性且減短了路徑總長。雙層全局優(yōu)化是在第一層全局優(yōu)化的基礎上進一步的優(yōu)化，通過第二層的全局優(yōu)化，可以將A*算法求得的路徑轉折點進一步降低。

雙層全局優(yōu)化的策略是將一層優(yōu)化后的路徑按照轉折點分成若干個相連的路徑段，通過判斷某一路徑段兩端的路徑段能否在非障礙物相交來優(yōu)化路徑，其具體策略如圖4、圖5所示。

圖4 二層優(yōu)化A*算法示意圖Fig.4 Schematic diagram of two-level optimization A* algorithm

圖5 雙層優(yōu)化A*算法示意圖Fig.5 Schematic diagram of bilevel optimization A* algorithm

（1）將一層優(yōu)化后的路徑按照從起點NodeS到終點NodeE的順序，以Fpo集合中的路徑轉折點Nodei為分割點，分割為若干個首位相連的路徑段Segmi(i=1,2,…)創(chuàng)建路徑坐標集Spo（Second path optimization），將起點與終點放置到Spo中。

（2）按順序依次選定路徑段Segmi兩端的路徑段Segmi－1與Segmi ＋1，若Segmi－1不存在，則說明路徑段Segmi的左端為起點，忽略Segmi，繼續(xù)選定Segmi ＋1兩端的路徑段；若Segmi ＋1不存在，則說明Segmi右端點為終點，計算結束。

（3）將路徑段Segmi－1與路徑段Segmi ＋1無限延長，若延長后兩直線無交點，則忽略路徑段Segmi，將Nodei添加到集合Spo中，之后繼續(xù)計算Segmi ＋1的兩端直線是否相交；若延長后兩直線有交點，設其交點為NodeC，并對交點NodeC進行條件判斷。

（4）若交點NodeC存在于集合Obstacle中，即交點為障礙點，忽略路徑段Segmi，將Nodei添加到集合Spo中，繼續(xù)計算Segmi ＋1；若交點NodeC不存在于集合Obstacle中，且從交點NodeC到Segmi兩端點Nodei－1與Nodei的連線均不通過Obstacle區(qū)域，則認為路徑段Segmi為可刪除路徑段，將NodeC添加到集合Spo中，之后選定計算Segmi ＋2…,重復（2）—（3）—（4）的循環(huán)過程，直到抵到終點結束。

（5）將坐標集Spo中的路徑點按照從起點到終點的順序依次連接，得到雙層全局優(yōu)化的路徑規(guī)劃結果。

按照場景1 的柵格地圖對傳統(tǒng)的A*算法進行雙層路徑優(yōu)化，其優(yōu)化結果如圖6 所示。通過對比可以看出，傳統(tǒng)A*算法求得的路徑轉折點數(shù)為7個，第一層優(yōu)化后轉折點數(shù)為3個，雙層優(yōu)化后的路徑轉折點為2個，路徑轉折點數(shù)量進一步地減少。

圖6 雙層優(yōu)化A*算法路徑規(guī)劃Fig.6 Path planning of bilevel optimization A* algorithm

2 局部路徑規(guī)劃

動態(tài)窗口法常用于移動機器人的局部路徑規(guī)劃，如圖7所示。傳統(tǒng)的動態(tài)窗口法存在兩個缺陷：面對諸如“凹”形與“C”形障礙物時，容易陷入局部最優(yōu)無法繼續(xù)前行；規(guī)劃路徑長度較長，無法做到全局路徑最優(yōu)。

圖7 傳統(tǒng)動態(tài)窗口法路徑規(guī)劃Fig.7 Path planning of traditional dynamic window method

針對傳統(tǒng)動態(tài)窗口法存在的問題，需要通過引入全局規(guī)劃來引導機器人走出陷入局部最優(yōu)的“凹”形和“C”形區(qū)域且更加接近全局最優(yōu)。因此本文引入雙層優(yōu)化后的A*算法來與動態(tài)窗口算法相融合。在雙層優(yōu)化A*算法求得全局規(guī)劃后，通過動態(tài)窗口法來實現(xiàn)局部路徑規(guī)劃。雙層優(yōu)化后的A*算法可以求得一條節(jié)點較少、總路程較短的全局路徑規(guī)劃結果。這條路徑雖然已經比傳統(tǒng)A*算法有所提升，但依然存在一些問題：在拐點處路徑距離障礙物較近。距離障礙物較近的問題，使得機器人在實際運行中拐點處容易與障礙物發(fā)生碰撞，因此需要在動態(tài)窗口法中對障礙物距離進行約束來保證障礙物距離。動態(tài)窗口法的避障策略其實是機器人運動的速度空間的帶約束的優(yōu)化問題，通過捕捉預測機器人的實時運動狀態(tài)來預測行進軌跡，再通過評價函數(shù)來選取一條總代價最小的軌跡作為實際路徑。

2.1 機器人運動模型

動態(tài)窗口法實時預測時間間隔Δt內機器人速度空間與狀態(tài)空間的變化，根據(jù)評價函數(shù)來確定最優(yōu)軌跡[16]。

移動機器人的速度空間V=(νt,ωt)包含機器人的線速度νt與角速度ωt，機器人的狀態(tài)空間(xt,yt,yawt,νt,ωt)包含機器人的橫縱坐標(xt,yt)、朝向yawt、線速度νt與角速度ωt，移動機器人運動變化時間間隔為Δt。移動機器人的運動模型可以表達為：

2.2 速度采樣

機器人的速度空間內存在無窮組速度對(ν,ω)，但是在對機器人進行實時速度采樣時，機器人在速度空間受到多方面的約束，其約束主要如下：

（1）最大、最小速度約束

移動機器人的線速度與角速度是有限的，存在最大值與最小值，速度空間的最大值與最小值構成動態(tài)窗口算法的最大范圍Vs：

（2）最大加、減速度約束

移動機器人的加速度受制于電機的輸出扭矩，存在最大加、減速度，即在速度采樣間隔Δt內，速度的變化是有限的。在采樣間隔Δt內允許的速度變化范圍Va：

（3）制動距離約束

機器人的移動過程中不允許與障礙物發(fā)生干涉碰撞，這就要求在最大減速度下，機器人在當前速度狀態(tài)下抵到最近障礙物時速度必須可以減為0。安全制動條件下的速度空間Vd：

機器人的速度需要同時滿足三個約束，即機器人允許的速度空間V為三個約束空間的交集：

將速度空間V按照速度的最大分辨率進行離散化，得到速度的一系列離散采樣點。將每個速度采樣點代入評價函數(shù)中，根據(jù)評價函數(shù)選取最優(yōu)的軌跡點作為下一時刻的運動軌跡。

2.3 評價函數(shù)

通過設計評價函數(shù)，在速度空間選取一條代價最小的軌跡作為移動機器人的實際軌跡[16]，既要保證路徑與障礙物保持一定的距離，還要做到路徑盡可能地接近雙層優(yōu)化A*算法求得的全局路徑，即要求完成動態(tài)避障的同時可以快速逼近終點。評價函數(shù)Cost(ν,ω) 的定義為：

式中，α、β、γ、σ為各項加權系數(shù)；Heading(ν,ω)為方位角評價子函數(shù)，當前機器人的運動朝向角度與目標點角度之差；Dist(ν,ω)為障礙物距離評價子函數(shù)，計算機器人預測點位置與最近障礙物之間的距離；Path(ν,ω)為路徑評價子函數(shù)，計算動態(tài)窗口法預測的Δt之后的預測點與雙層優(yōu)化A*算法求得全局軌跡的最小距離；Goal(ν,ω)為代價評價子函數(shù)，計算預測點到目標終點的代價值。

當評價函數(shù)Cost(ν,ω)最小時，即認為預測點為最優(yōu)的軌跡點。各子函數(shù)的具體函數(shù)表達式如下：

各項加權系數(shù)α、β、γ、σ：加權系數(shù)決定了動態(tài)窗口法在選擇最優(yōu)軌跡時不同子函數(shù)的優(yōu)先級程度。本算法的四個子函數(shù)中，加權系數(shù)的優(yōu)先級最高的為β，即優(yōu)先考慮障礙物距離。雙層的A*優(yōu)化算法雖然減少了轉折點的數(shù)量，但是在某些地圖場景中會增加路徑長度，因此可以通過加權系數(shù)的關系對這一問題進行修復。令γ ＜σ，則算法會優(yōu)先選擇總代價較小的路徑。

方位角評價子函數(shù)Heading(ν,ω)：

式中，θg為機器人指向目標點與水平線的夾角；θt為機器人行進方向與水平線的夾角。θg與θt的差值越小說明與目標點的方位差越小。

障礙物距離評價子函數(shù)Dist(ν,ω)：

其中，R為機器人的實際底盤半徑，由于大部分機器人的底盤不完全是規(guī)則的圓形，且機器人的移動與定位存在一定的誤差，為了保證機器人移動的安全性，在機器人底盤半徑R基礎上增加0.2R的安全距離來保證其運行的安全性。do為機器人到最近障礙物的距離。(xo,yo)為障礙物的坐標。當機器人到障礙物的距離大于機器人底盤半徑R時一般不會發(fā)生碰撞，此時子函數(shù)Dist(ν,ω)值為0，即不發(fā)生碰撞時不考慮障礙物距離評價；當機器人到障礙物的距離小于或等于R時，機器人發(fā)生碰撞的可能性較大，此時Dist(ν,ω)值隨著距離的減小而增大，即距離越近發(fā)生碰撞的影響越大。

路徑評價子函數(shù)Path(ν,ω)：

其中，(xa,ya)為雙層優(yōu)化A*算法求得的全局規(guī)劃路徑的節(jié)點坐標。Path(ν,ω)越小說明越接近全局路徑規(guī)劃，反之則越偏離全局路徑規(guī)劃。

代價評價子函數(shù)Goal(ν,ω)：

其中，(xg,yg)為目標終點的坐標。Goal(ν,ω)越小，機器人移動到目標終點所需的代價越小，機器人越靠近目標終點。

3 算法融合

移動機器人在獲取到全局靜態(tài)環(huán)境信息之后，首先通過傳統(tǒng)的A*算法求得全局路徑規(guī)劃，在此全局規(guī)劃的基礎上進行雙層路徑優(yōu)化。首先通過第一層路徑優(yōu)化降低轉折點數(shù)量，減短路徑長度，再進一步進行二層優(yōu)化，使得路徑的轉折點進一步降低，得到最終的全局路徑規(guī)劃。全局路徑規(guī)劃完成后開始對機器人的速度進行采樣，利用動態(tài)窗口法進行實時局部路徑規(guī)劃，達到局部路徑最優(yōu)與動態(tài)避障。融合雙層優(yōu)化的A*全局路徑規(guī)劃與動態(tài)窗口法的局部路徑規(guī)劃，保證了機器人路徑規(guī)劃的全局最優(yōu)性。用融合算法對上述地圖場景進行路徑規(guī)劃的結果如圖8所示。

圖8 融合算法路徑規(guī)劃Fig.8 Path planning of fusion algorithm

融合算法的具體流程如圖9所示。

圖9 融合算法流程圖Fig.9 Flow chart of fusion algorithm

4 仿真實驗

4.1 仿真環(huán)境描述

為了驗證融合算法的有效性，本文采用Python3.6為實驗仿真平臺。在Python3.6中進行四種算法的路徑點計算，將輸出的路徑點坐標導入到Matlab中進行繪圖與路徑長度計算。設置四種不同的二維靜態(tài)柵格地圖，其中地圖1、2 為非對稱的隨機分布地圖，地圖3、4 為中心對稱式分布地圖，在四組地圖中分別對傳統(tǒng)A*算法、一層優(yōu)化A*算法、二層優(yōu)化A*算法以及融合算法進行仿真實驗，對四種算法求得路徑進行數(shù)據(jù)統(tǒng)計與比較。設置動態(tài)障礙物的動態(tài)地圖，在動態(tài)環(huán)境中對雙層優(yōu)化A*算法、傳統(tǒng)動態(tài)窗口法與融合算法進行仿真實驗與數(shù)據(jù)統(tǒng)計分析。

仿真環(huán)境為二維靜態(tài)柵格地圖，由25×25（不包含邊界）個相同大小的單元格組成。其中黑色方格代表障礙物，白色方格代表可行路徑點，黃色方格代表移動機器人起點，紅色方格代表路徑轉折點，藍色方格代表目標終點，黑色連續(xù)線條代表算法求得的路徑規(guī)劃，地圖模型采用相同的起點（1，24）與終點（24，1）。

融合算法中的動態(tài)窗口法的相關參數(shù)設置為：起始坐標（1，24），目標點（24，1），初始方位角－π/2，初始線速度0 m/s，初始角速度0 rad/s，最大線速度1 m/s，最大角速度20 rad/s，最大線加速度0.2 m/s2，最大角加速度50 rad/s，線速度分辨率0.01 m/s，角速度分辨率1 rad/s，時間分辨率0.1 s，預測周期間隔2 s，動態(tài)障礙物起始坐標為[13，1]，按照3.6 m/s的速度沿Y軸正方向勻速移動。

4.2 仿真結果

對傳統(tǒng)A*算法、一層優(yōu)化A*算法、二層優(yōu)化A*算法以及融合算法在四種不同的二維靜態(tài)網格地圖上進行仿真實驗，其仿真結果如圖10、圖11、圖12、圖13所示。

圖10 場景1仿真結果Fig.10 Simulation results of scenario 1

圖11 場景2仿真結果Fig.11 Simulation results of scenario 2

圖12 場景3仿真結果Fig.12 Simulation results of scenario 3

圖13 場景4仿真結果Fig.13 Simulation results of scenario 4

對雙層優(yōu)化A*算法、傳統(tǒng)動態(tài)窗口法及融合算法在二維動態(tài)網格地圖中進行仿真實驗，并在路徑規(guī)劃過程中捕捉四個時刻的規(guī)劃狀態(tài)，其仿真結果如圖14、圖15、圖16所示。

圖14 雙層優(yōu)化A*算法動態(tài)仿真結果Fig.14 Dynamic simulation results of bilevel optimization A* algorithm

圖15 傳統(tǒng)動態(tài)窗口法動態(tài)仿真結果Fig.15 Dynamic simulation results of traditional dynamic window method

圖16 融合算法動態(tài)仿真結果仿真結果Fig.16 Dynamic simulation results of fusion algorithm

對上述四組二維靜態(tài)環(huán)境地圖仿真結果進行數(shù)據(jù)統(tǒng)計如下：

由表1可以看出：雙層優(yōu)化A*算法提供了較高質量的全局路徑規(guī)劃，其優(yōu)化主要表現(xiàn)在路徑轉折點數(shù)量與路徑長度上。在四個不同場景的地圖中，一層優(yōu)化A*算法轉折點平均優(yōu)化率為34.3%，雙層A*優(yōu)化算法路徑轉折點平均優(yōu)化率為62.4%，優(yōu)化幅度較為明顯。在路徑長度優(yōu)化比例上優(yōu)化幅度較小，其中一層優(yōu)化A*算法平均優(yōu)化率為3.1%，雙層優(yōu)化A*算法平均優(yōu)化率為－3.4%，融合算法平均優(yōu)化率為1.8%?？梢钥闯鲭p層優(yōu)化A*算法在大幅降低路徑轉折點數(shù)量的同時，使得路徑長度小幅度變長，再通過融合算法的加權系數(shù)來修正路徑長度，最后獲得的融合算法在較少轉折的基礎上路徑長度再次減少。

表1 四組靜態(tài)地圖仿真結果Table 1 Four groups of map simulation results

由動態(tài)仿真結果與表2可以看出：雙層優(yōu)化A*算法提供了轉折點較少、路徑長度較短的高質量全局路徑規(guī)劃，但是在t3時刻，機器人與障礙物的距離為－0.43 m，發(fā)生碰撞，碰撞點之后的路徑規(guī)劃也失去了參考價值；傳統(tǒng)動態(tài)窗口法在t1時刻遇到第一個“L”形障礙物時，因為局部最優(yōu)而選擇了較遠的路程，同時在t4時刻面對“凹”形障礙物陷入局部最優(yōu)無法繼續(xù)前行抵達的終點；融合算法在t1時刻面對“L”形障礙物時選擇了更加貼近雙層優(yōu)化A*算法的最優(yōu)全局路徑規(guī)劃，在t2時刻融合算法的動態(tài)窗口檢測到即將可能發(fā)生碰撞的動態(tài)障礙物，在t3時刻機器人進行了動態(tài)避障，在t4時刻機器人完成動態(tài)避障之后繼續(xù)按照雙層優(yōu)化A*算法的最優(yōu)全局規(guī)劃抵到終點?？梢钥闯鋈诤纤惴鎸討B(tài)障礙物時，可以保證機器人與障礙物的安全距離并且實現(xiàn)實時動態(tài)避障，同時路徑規(guī)劃貼近全局最優(yōu)，在完成動態(tài)避障的同時僅比雙層優(yōu)化A*算法的最優(yōu)全局規(guī)劃多出了2%的路徑長度，滿足了移動機器人在動態(tài)環(huán)境中既要實現(xiàn)動態(tài)避障抵達終點，又要做到全局最優(yōu)的路徑規(guī)劃需求。

表2 動態(tài)地圖仿真結果Table 2 Dynamic map simulation results

5 結束語

動態(tài)環(huán)境中機器人既要安全規(guī)避動態(tài)障礙物抵到終點，同時又需要全局規(guī)劃路徑質量盡可能高，針對移動機器人在動態(tài)環(huán)境中的路徑規(guī)劃需求，本文進行了以下研究：

（1）提出了雙層優(yōu)化A*算法與動態(tài)窗口法結合的移動機器人路徑規(guī)劃算法，實現(xiàn)了動態(tài)避障與全局最優(yōu)的雙重要求。

（2）一層優(yōu)化A*算法采用節(jié)點斜率計算與篩除，有效減少了路徑轉折次數(shù)并小幅度降低了路徑長度。

（3）二層優(yōu)化A*算法通過延長路徑獲得交點的方法，大幅度降低了路徑轉折次數(shù)，將路徑轉折次數(shù)降到了最低，但是小幅度增加了路徑長度。

（4）融合算法在動態(tài)窗口法中通過引入“路徑規(guī)劃子函數(shù)”與設置障礙物安全距離，使得移動機器人在面對諸“C”“凹”形障礙物時選擇全局最優(yōu)路徑，同時安全距離保證了機器人移動的安全性。