王佳軍 孟令軍 薛志凌 李曉宇

（中北大學(xué)儀器與電子學(xué)院 太原 030000）

1 引言

隨著科技水平的快速提高，機器人由最初的結(jié)構(gòu)簡單、功能單一發(fā)展為能夠適應(yīng)多樣化環(huán)境、執(zhí)行復(fù)雜任務(wù)的智能化機器人，廣泛應(yīng)用于物流業(yè)、制造業(yè)、服務(wù)業(yè)等領(lǐng)域，極大地提高了社會的生產(chǎn)力水平。而路徑規(guī)劃是實現(xiàn)自主導(dǎo)航的關(guān)鍵一步，也是機器人智能化的關(guān)鍵技術(shù)之一，是機器人以及自動駕駛領(lǐng)域的研究熱點。目前，機器人路徑規(guī)劃主要集中在三類算法：1）基于圖的算法，如：Dijkstra、A*算法［1］，D*算法［2］，D*Lite算法［3］等；2）隨機采樣類算法，如：快速隨即搜索樹法（RRT）［4］、概率路圖法（PRM）等；3）智能優(yōu)化算法：如蟻群算法、遺傳算法、神經(jīng)網(wǎng)絡(luò)等，此外，還有人工勢場法［5］，動態(tài)窗口法［6］等。

Dijkstra 算法基于廣度優(yōu)先遍歷，向四周盲目拓展，搜索節(jié)點過多。A*算法在Dijkstra 算法的基礎(chǔ)上引入啟發(fā)式函數(shù)，增強了搜索的目的性，在機器人全局路徑規(guī)劃中使用廣泛。但是該算法存在節(jié)點過于密集、軌跡不平滑等問題。文獻［7］利用跳點搜索理論，大大減小了搜索過程中節(jié)點的數(shù)量；文獻［8］引入電動車的能耗約束建立綜合估價函數(shù)，同時考慮中途充電站，規(guī)劃出能耗最優(yōu)的行車路徑；文獻［9］將傳統(tǒng)A*算法八角度拓展節(jié)點優(yōu)化為任意角度路線，減小了路徑長度，與蟻群算法進行融合實現(xiàn)多目標(biāo)點路徑規(guī)劃。

動 態(tài) 窗 口 法（Dynamic Window Approach，DWA）對復(fù)雜環(huán)境具有很強的適應(yīng)能力，實時結(jié)合機器人自身傳感器進行局部路徑規(guī)劃，能夠有效躲避障礙物且路徑為平滑曲線，但是該方法容易陷入局部最優(yōu)解，無法抵達目標(biāo)點。文獻［10］結(jié)合與全局路徑距離信息定義新的評價函數(shù)，能夠提前規(guī)避“C”型障礙物；文獻［11］結(jié)合自適應(yīng)思想使得機器人在通過不同密度障礙物區(qū)域時，運動參數(shù)能夠自主變化，機器人的速度與路徑更加合理；文獻［12］利用動態(tài)窗口法對可選避障速度進行速度約束，同時，增大了移動機器人可選避障速度區(qū)域。

本文將A*算法與動態(tài)窗口法結(jié)合，首先對A*算法產(chǎn)生的路徑節(jié)點進行預(yù)處理，剔除冗余節(jié)點，得到路徑坐標(biāo)點作為動態(tài)窗口算法的局部目標(biāo)點，直至最終目標(biāo)點，有效解決了單一算法在路徑規(guī)劃中存在的問題。

2 A*算法

2.1 環(huán)境構(gòu)建

在機器人工作過程中，需要通過自身傳感器感知環(huán)境，構(gòu)建一個機器人可以理解地表示所在環(huán)境的地圖模型。柵格法是經(jīng)典的環(huán)境建模方法，簡單高效，將機器人構(gòu)建的地圖劃分為大小相同、彼此相連的柵格，柵格帶有環(huán)境信息及機器人位置信息，機器人以柵格為基本單元進行移動和搜索，完成路徑規(guī)劃任務(wù)。如圖1所示，深色柵格表示環(huán)境中存在障礙物，機器人無法通過，淺色柵格表示允許搜索區(qū)域。

圖1 柵格地圖

2.2 A*算法基本原理

A*算法在搜索過程中假設(shè)環(huán)境不再改變，是求解最短路徑的有效方法。Dijkstra 盲目向四周搜索，而A*算法的改進在于結(jié)合當(dāng)前位置與目標(biāo)點的距離建立具有啟發(fā)性的搜索函數(shù)：

式中：g(n)為起始節(jié)點至當(dāng)前節(jié)點的實際距離代價；?(n)為當(dāng)前節(jié)點至目標(biāo)點的估計距離代價。

代價函數(shù)f(n)來進行節(jié)點的擴展和搜索，g(n)代表搜索的完整性，g(n)越大越趨向于搜索的廣度，但會降低搜索的效率，h（n）具有啟發(fā)性，h（n）越大，搜索的效率高，但通常難以規(guī)劃到最優(yōu)路徑。

A*算法沿著柵格地圖搜索的過程中，更新維護open list和close list兩個列表。Open存儲拓展點周圍節(jié)點，用于計算選擇最小代價節(jié)點作為下一個拓展點，close 存儲經(jīng)過的節(jié)點以及障礙物。A*算法具體流程如下：

1）初始化環(huán)境信息，包括障礙物、起始點、目標(biāo)點，以及open和close列表；

2）判斷open 列表是否為空或只有目標(biāo)點，若為真，結(jié)束搜索；

3）選取open 列表中評價函數(shù)最小的節(jié)點作為父節(jié)點，調(diào)用拓展函數(shù)，計算g（n），h（n），將此節(jié)點放入close列表；

4）將拓展出來的節(jié)點放入open 列表中，保存父節(jié)點；

5）跳至步驟2），直至找到終點；

6）保存的父節(jié)點信息，即為路徑關(guān)鍵節(jié)點。

3 動態(tài)窗口法

動態(tài)窗口法由機器人運動學(xué)方程推導(dǎo)而來，在預(yù)測時間dt內(nèi)，對機器人線速度和角速度采樣，形成速度空間(v,ω)，描繪出所有可能的運動軌跡，構(gòu)建評價函數(shù)對軌跡進行評價，從中選擇最合理的路徑。

首先，我們建立機器人的運動模型，機器人的運動軌跡由一系列的圓弧組成，在dt時間內(nèi)由速度對(v,ω)決定，運動模型為

依據(jù)上述機器人運動模型，在機器人的實際工作中，具有無窮多(v,ω)，需要根據(jù)環(huán)境和工作狀態(tài)添加約束，選擇出朝向目標(biāo)、躲避障礙物的(v,ω)。

出于安全考慮，對機器人行駛的速度范圍進行約束，定義Vs為機器人最大線速度和角速度，所以動態(tài)窗口所能搜索的最大范圍為

由于機器人電機所提供轉(zhuǎn)矩的限制，機器人在dt時間內(nèi)所能達到的實際速度受到一定的約束，即：

為了保證機器人在行駛過程中避免與障礙物發(fā)生碰撞，當(dāng)機器人檢測到環(huán)境中障礙物時，能夠以最大減速度停下來，即：

最終，由以上三個速度構(gòu)成動態(tài)窗口速度集合Vr：

經(jīng)過速度采樣后得到速度空間，如何在該空間內(nèi)選擇一條最優(yōu)的軌跡，需要我們設(shè)計適宜的評價函數(shù)，使得機器人朝向目標(biāo)快速前進，局部避開障礙物。評價函數(shù)設(shè)計為

式中：σ為平滑系數(shù)，α,β,γ為各子函數(shù)加權(quán)系數(shù)；heading（v，ω）表示當(dāng)前位置與全局之間的角度偏差，使機器人朝向目標(biāo)點方向；dist（v，ω）表示生成的速度軌跡至障礙物的最近距離，防止機器人發(fā)生碰撞；vel（v，ω）表示采樣軌跡中的速度大小，盡快抵達。

4 優(yōu)化與改進

A*算法根據(jù)評價函數(shù)，遍歷整個柵格地圖，得到一系列路徑點，導(dǎo)致路徑點過于密集，曲率不連續(xù)，關(guān)鍵拐點選取策略，剔除不相關(guān)的冗余節(jié)點，提升路徑的平滑度。設(shè)A→B→C 為路徑中三個節(jié)點的行駛路線，若A 與B 共線，B 與C 共線，則認為B節(jié)點為冗余節(jié)點，刪去該結(jié)點。

傳統(tǒng)評價函數(shù)從方向性、安全性、效率性方面充分評價速度空間內(nèi)最合理的速度。向方向子函數(shù)加入全局路徑信息約束：全局路徑距離當(dāng)前位置最近的節(jié)點之間的角度，新的方向函數(shù)保證機器人不斷跟隨新的目標(biāo)點前進。該改進方法有效地解決了動態(tài)窗口法中單一目標(biāo)點容易陷入局部最優(yōu)的缺陷，規(guī)劃出的路徑即為全局最佳路徑，改進后算法具體執(zhí)行步驟如下：

1）根據(jù)機器人傳感器構(gòu)建柵格地圖；

2）初始化A*及DWA參數(shù)；

3）A*算法進行搜索，獲取關(guān)鍵全局路徑點信息；

4）根據(jù)機器人運動模型，計算當(dāng)前動態(tài)窗口Vr；

5）根據(jù)機器人運動模型，計算預(yù)測時間dt內(nèi)運動軌跡點，同時根據(jù)評價函數(shù)對各軌跡點打分，分數(shù)最高者為下一個dt時間機器人的運動速度；

6）判斷機器人是否到達目標(biāo)點，若抵達，尋路成功，否則，返回第4）步，繼續(xù)搜索。

5 仿真與分析

為了驗證本文算法的有效性，在Win10 系統(tǒng)Matlab R2016a 環(huán)境下構(gòu)建柵格地圖進行仿真實驗。根據(jù)文獻［13］，機器人運動參數(shù)設(shè)置為Vmax=1.0m/s，ωmax=20°/s，V.max=0.2m/s2，ω?max=50°/s，速度分辨率和角速度分辨率分別為0.01m/s，1°/s。各評價子函數(shù)系數(shù)分別為0.05，0.4，0.1。起始點、目標(biāo)點分別為（1.5，2.5），（10.5，9.5）。

傳統(tǒng)A*算法規(guī)劃路徑如圖2所示，當(dāng)搜索鄰域由4鄰域變?yōu)?鄰域時，路徑節(jié)點數(shù)、路徑長度和轉(zhuǎn)彎角度有了明顯改善。實驗數(shù)據(jù)如表1所示?？梢姡S著搜索鄰域的增加，比如：24 鄰域［14］，48 鄰域［15］，運行時間沒有明顯增加，轉(zhuǎn)彎角度降低，路徑更加平滑。同時，坐標(biāo)（3，3），（6，5），（8，9）處距離障礙物距離過近，導(dǎo)致路徑的安全性降低。

表1 仿真實驗數(shù)據(jù)

圖2 全局最優(yōu)路徑及節(jié)點

圖3 全局最優(yōu)路徑及關(guān)鍵節(jié)點

由圖4 可以看出，傳統(tǒng)DWA 算法規(guī)劃路徑時會陷入局部最優(yōu)，尋路失敗；由圖5可知，與A*算法融合改進后得到新的路徑為平滑曲線，距離障礙物距離更加合理，提高機器人行駛的安全性。本文得到運動學(xué)參數(shù)結(jié)果圖6所示，速度和角速度連續(xù)變化，整體符合要求。在迭代次數(shù)為300 次通過稠密障礙物時，機器人方向出現(xiàn)小幅度震蕩，在今后的研究中還需要改進。

圖4 DWA路徑

圖5 改進型DWA路徑

圖6 速度曲線

6 結(jié)語

為了提高機器人在復(fù)雜動態(tài)環(huán)境下的表現(xiàn)，結(jié)合A*算法全局路徑最優(yōu)的特點和動態(tài)窗口法局部最優(yōu)特點，提出一種新的算法。仿真結(jié)果表明，相較于單一算法，混合算法克服了局部最優(yōu)問題，平滑度上有了巨大改進。但是在經(jīng)過稠密障礙物時，還需進一步改進。本文僅僅做了初步的研究和實驗，在今后還將在多目標(biāo)、實時性方面做更多的探索。