趙衛(wèi)東 蔣 超

(安徽工業(yè)大學電氣與信息工程學院 安徽 馬鞍山 243032)

0 引 言

隨著自動化和智能化技術的發(fā)展，移動機器人在生產(chǎn)和生活中的應用越來越廣泛。移動機器人的路徑規(guī)劃問題是移動機器人研究的核心問題之一。所謂移動機器人路徑規(guī)劃[1]，就是當移動機器人處于已知或未知的環(huán)境中，其根據(jù)自身傳感器反饋的信息來感知環(huán)境，從而避開障礙物自行規(guī)劃出一條最優(yōu)的安全運行路線。移動機器人路徑規(guī)劃研究最早始于20世紀60年代[2]。時至今日，移動機器人路徑規(guī)劃可分為傳統(tǒng)路徑規(guī)劃方法和現(xiàn)代路徑規(guī)劃方法。傳統(tǒng)的路徑規(guī)劃方法包括Dijkstra算法[3]、A*算法[4]、D*算法[5]等，現(xiàn)代路徑規(guī)劃算法包括蟻群算法[6]、遺傳算法[7]、神經(jīng)網(wǎng)絡算法[8]、模糊路徑算法[9]等。相較于蟻群算法等智能算法，傳統(tǒng)算法環(huán)境建模簡單，易于實現(xiàn)。

在眾多傳統(tǒng)算法中，A*算法是一種基于柵格地圖的路徑規(guī)劃算法，相比于主要應用在環(huán)境未知的動態(tài)場景的D*算法，對于環(huán)境已知的靜態(tài)場景A*算法能更加有效地求得最優(yōu)路徑[10]。同時，相比于Dijkstra算法，A*算法采用了啟發(fā)式的搜索方式，大幅度降低了搜索節(jié)點的數(shù)量，從而極大地提高了搜索效率。但是，A*算法存在以下問題：其規(guī)劃出的全局路徑中易出現(xiàn)轉折點，而考慮到機器人的運動學限制，路徑中存在過多的轉折點對于機器人的運動控制是不利的。

本文重點研究了A*算法生成的全局路徑平滑處理以及提高該算法實時性的有效手段，采用兩階段搜索算法，通過選擇合適的估價函數(shù)，指導搜索朝著最有希望的方向前進，以期獲得最優(yōu)路徑。

1 傳統(tǒng)的A*算法

A*算法是比較常用的移動機器人路徑規(guī)劃算法，它是一種啟發(fā)式搜索方法。A*算法中的評價函數(shù)定義為：

f(n)=s(n)+h(n)

(1)

式中：n表示機器人在路徑中的當前位置節(jié)點；s(n)代表機器人從起始節(jié)點到當前節(jié)點的實際花費代價和；h(n)是啟發(fā)函數(shù)，表示機器人從當前節(jié)點到目標節(jié)點的距離估計代價。通過估價函數(shù)來引導其搜索方向，傳統(tǒng)A*算法使用曼哈頓距離或者歐氏距離來定義啟發(fā)函數(shù)。曼哈頓距離表示兩個點在標準坐標系上的絕對軸距總和，距離計算方向僅分為橫向和縱向，計算簡單但對實際距離存在明顯的高估。歐氏距離表示兩個點在坐標系中的直線最短距離，是對實際距離很好的描述，所以采用歐氏距離來構建啟發(fā)函數(shù)性能較好。即：

(2)

式中：nx、ny分別為當前節(jié)點n在直角坐標系中的橫坐標和縱坐標；g是目標節(jié)點；gx、gy分別為目標節(jié)點g在直角坐標系中的橫坐標和縱坐標。

A*算法采用八鄰域搜索，如圖1所示。通過中心節(jié)點向附近8個鄰域擴散，然后通過每個鄰域的評價函數(shù)值的大小來選擇路徑方向，運動方向的角度也被限定為這8個方向。因此，規(guī)劃出來的路徑會有很多的轉折點，而對于移動機器人自身機械結構所決定的運動限制來說，這些轉折點明顯不符合運動學原理，所以A*算法在實際工程應用中效果并不理想。

圖1 八鄰域搜索

2 改進的A*算法

A*算法折點過多，機器人運動軌跡不平滑的原因有兩種：1)采用八鄰域的搜索方式，其子節(jié)點與父節(jié)點之間的實際移動代價也是按照八鄰域方向計算的，這就造成了采用歐氏距離描述的兩點之間實際距離所估計的代價低于實際移動代價而導致算法擴展多余節(jié)點。2)評價函數(shù)只考慮到起點到當前節(jié)點和當前節(jié)點到終點的代價，并沒有考慮到機器人自身的運動學約束的代價。因此，本文首先改進A*算法的評價函數(shù)，實現(xiàn)一級搜索，然后對一級搜索的結果進行二級平滑處理，最終通過兩級搜索的方式實現(xiàn)對路徑的平滑糾正的目的。

2.1 優(yōu)化的啟發(fā)函數(shù)

根據(jù)A*算法的八鄰域搜索方向：橫向、縱向、對角線方向，把A*算法啟發(fā)函數(shù)hE(n)采用的歐氏距離分解成這三個方向的組合，這樣能夠更準確地表示柵格地圖上任意兩點的實際移動代價，并使算法盡量處于僅尋得最佳路徑而不擴展其他擴展點的理想情況。優(yōu)化A*的啟發(fā)函數(shù)為：

(3)

優(yōu)化的啟發(fā)函數(shù)相對于使用曼哈頓距離的啟發(fā)函數(shù)不會高估到目標點的移動代價，也不會像使用歐氏距離的啟發(fā)函數(shù)那樣低估移動代價。

2.2 角度約束函數(shù)

A*算法作為一種經(jīng)典的路徑規(guī)劃方法，通過結合機器人的幾何特征對地圖上的障礙物進行輪廓膨脹，把機器人簡化為一個質點，從而達到簡化計算的目的，所以A*忽略了機器人實體自身的運動特性，導致算法規(guī)劃出的路徑機器人執(zhí)行難度較大，甚至無法執(zhí)行。另外，在實際應用中，機器人的路徑實施過程依賴于其傳感器定位，路徑存在過多的轉向需求(折點)也會令定位傳感器的觀測數(shù)據(jù)產(chǎn)生累計誤差，使機器人在地圖中自定位逐漸不準確，甚至位置丟失，而引入角度約束可以有效地抑制這些問題。

綜合機器人自身的運動約束，機器人行進方式一般為區(qū)域性向前，不會產(chǎn)生大幅度的轉向、頻繁且長時間的后退行為[11]，在路徑規(guī)劃的第一階段搜索過程中引入角度約束，使轉向行為具有一定的代價來抑制路徑轉折點的產(chǎn)生，即評價函數(shù)變?yōu)椋?/p>

f(n)=s(n)+h(n)+a(n)

(4)

新的評價函數(shù)在原有的基礎上增加了一個新的約束，該約束為機器人相對上一運動方向的轉角約束a(n)，其約束函數(shù)為：

(5)

式中：k為轉角約束系數(shù)，通過新的評價函數(shù)對當前節(jié)點不同轉角的子節(jié)點的評價可以確定k的取值范圍，在此范圍內(nèi)，k越大，角度約束的效果就越明顯；θn為當前時刻機器人的朝向角；θl為上一時刻機器人朝向角；l為單格步長。當機器人上一時刻轉角與當前時刻相同時，新的約束函數(shù)即為0，當機器人轉角越大，則約束越大。圖2和圖3分別給出了A*算法和本文改進算法的評價函數(shù)搜索方式。

圖2 傳統(tǒng)算法評價函數(shù)

圖3 改進算法的評價函數(shù)

A*的評價函數(shù)在八鄰域搜索時并沒有考慮到機器人偏航角問題，步長與角度沒有關聯(lián)，容易出現(xiàn)偏航角折點。改進后的A*算法添加了角度約束，轉偏航角度越大，則總步長越長，抑制了折點的產(chǎn)生，相比于傳統(tǒng)算法對路徑的平滑性方面的考慮上有較為明顯的提升。

2.3 二次搜索

通過上述改進后，規(guī)劃路徑中仍然存在較多冗余點，為了獲得更優(yōu)路徑，對上述路徑進行二次搜索，剔除不必要的節(jié)點，在保證減少折點個數(shù)的同時又縮短了總路徑的長度。具體操作步驟如下：

步驟一提取上一步驟規(guī)劃的全局路徑節(jié)點集P(N)，P(N)包含整個路徑的節(jié)點。

步驟二從起點節(jié)點P(1)向后遍歷P(N)中的所有節(jié)點，篩選出所有角度約束函數(shù)a(n)≠0的節(jié)點定義為折點，提取這些折點組成的節(jié)點集T(M)。

步驟三遍歷折點節(jié)點集T(i)，i=1,2,…，m，找到當前節(jié)點T(i)相鄰的前后節(jié)點T(i-1)和節(jié)點T(i+1)，連接這兩個節(jié)點，判斷兩點連線是否穿過障礙物；若沒有，則剔除節(jié)點T(i)。

步驟四遍歷所有節(jié)點集T(m)，剔除所有冗余折點，得到最終節(jié)點集P(L)，P(L)作為最終的全局路徑節(jié)點集。

采用15×15的柵格地圖對上述算法進行測試與仿真，仿真效果如圖4-圖6所示，其中：圖4是優(yōu)化啟發(fā)函數(shù)的A*算法的仿真結果；圖5是添加了角度約束的第一階段搜索結果；圖6是經(jīng)過二次搜索后的最終結果。圖中三角形標志代表路徑起點，叉號標志代表路徑終點。由圖中可知，傳優(yōu)化啟發(fā)函數(shù)的A*算法出現(xiàn)5個折點，15個擴展點，總轉折角度為675°。由圖5可知，對優(yōu)化啟發(fā)函數(shù)的A*算法加了角度約束的A*算法只出現(xiàn)了2個折點，也是15個擴展點，總轉折角度為270°，相對于傳統(tǒng)A*，在總擴展點不變的前提下轉折角度明顯降低。由圖6可知，采用Two-Stage(二級搜索)A*算法只出現(xiàn)了1個折點，擴展節(jié)點減少到6個，路徑累計轉折角度為121.45°。

圖4 優(yōu)化啟發(fā)函數(shù)的A*算法規(guī)劃仿真

圖5 添加角度約束的A*算法規(guī)劃仿真

圖6 兩階段A*算法規(guī)劃仿真

算法不同優(yōu)化階段的仿真結果如表1所示。改進的兩階段A*算法通過引入新的約束函數(shù)以及剔除冗余折點的方式，相比于優(yōu)化啟發(fā)函數(shù)的A*算法，在路徑平滑性上有明顯提升并顯著減少了路徑點個數(shù)，累計轉折角度相比于傳統(tǒng)算法減小了82%，路徑點個數(shù)相比于傳統(tǒng)算法降低了60%，路徑的總長度相比于傳統(tǒng)算法縮短了2.89%。

表1 算法不同優(yōu)化階段的仿真結果

3 實 驗

為了驗證改進A*算法在移動機器人路徑規(guī)劃中的實用性，將改進A*算法應用到如圖7所示的自主搭建的機器人平臺。該機器人平臺配備三個全向輪，可以進行全方位的運動控制，其處理器采用Intel i3-4010U，采用德國SICK TIM561激光雷達安裝于機器人底盤前部用以獲取外界環(huán)境信息，其最遠探測距離可達10 m。該實驗平臺基于ROS[12]操作系統(tǒng)，系統(tǒng)框架如圖8所示。利用自適應蒙特卡洛方法進行機器人定位，最后通過move_base程序模塊實現(xiàn)全局和局部路徑規(guī)劃，其輸入為機器人激光雷達點云、編碼器里程數(shù)據(jù)，輸出為機器人各個驅動輪的實時控制速度信息。

圖7 實驗平臺

圖8 改進后的ROS系統(tǒng)框架

實驗采用ROS系統(tǒng)中GMapping[13-14]分別在兩種常見的現(xiàn)實場景中建立地圖并進行全局路徑規(guī)劃。

測試場景1為狹長的走廊環(huán)境，地圖大小為10 m×10 m，測試場景及其建立的地圖如圖9所示。測試場景2為空間復雜度較高的大廳環(huán)境，地圖大小為12 m×8 m，測試場景及建立的地圖如圖10所示。建立出的地圖柵格分辨率為0.05 m×0.05 m，實驗設置機器人最大速度為0.5 m/s，最大轉角速度為0.6 rad/s。

圖9 實物測試場景1及建立的地圖

圖10 實物測試場景2及建立的地圖

同一機器人在同一地圖中取相同的起始點和目標點的情況下場景1測試效果圖如圖11所示，場景2測試效果圖如圖12所示。圖中圓點表示機器人起點，白色區(qū)域中的曲線表示機器人的規(guī)劃路徑?？梢钥闯?，改進后的算法規(guī)劃的路徑上轉折點更少，降低了不必要的路徑彎曲程度，整體上縮短了機器人所需要實現(xiàn)的位移。同時改進后的算法減小了機器人在軌跡上偏航方向上的航向震蕩，使機器人運動過程更加平穩(wěn)。

(a)A*算法 (b)改進A*算法

(a)A*算法 (b)改進A*算法

實物測試結果如表2所示。相比傳統(tǒng)A*算法，改進算法在擴展點個數(shù)上明顯降低。場景1中算法擴展點總個數(shù)降低了266個，規(guī)劃路徑的總長度減少了1.89 m，算法規(guī)劃過程耗時增加了0.028 s；場景2中算法擴展點總個數(shù)降低了181個，規(guī)劃路徑的總長度減少了0.92 m，算法規(guī)劃過程耗時增加了0.48 s。雖然改進算法較優(yōu)化啟發(fā)函數(shù)的A*算法耗時增加，但可以生成最優(yōu)路徑，同時在很大程度上縮減了路徑長度，方便了機器人的運動控制，使路徑執(zhí)行過程耗時明顯減少。其在實驗場景1下路徑執(zhí)行過程耗時減少了6.73 s；在實驗場景2下路徑執(zhí)行過程耗時減少了2.46 s。在實驗設定的兩種常見的測試場景中，改進的A*算法明顯優(yōu)于傳統(tǒng)A*算法。

表2 兩種場景的實物測試結果

4 結 語

為了解決A*算法在機器人全局路徑規(guī)劃中產(chǎn)生過多折點的問題，本文提出了一種兩階段搜索A*算法，其采用添加角度約束的代價評價策略進行路徑搜索，進而對搜索結果進行線形優(yōu)化。理論和實驗結果表明，本文算法的啟發(fā)函數(shù)能夠更準確地估計路徑代價，讓第一階段搜索的效果接近最理想狀態(tài)，減少了算法運算量，并加以角度約束抑制路徑轉折點的產(chǎn)生，不僅使路徑考慮到了機器人運動約束，還盡量壓縮了第二階段冗余折點的計算量。再通過第二階段搜索對路徑線形地進一步優(yōu)化，在保證生成滿足機器人運動限制的最優(yōu)路徑的同時顯著縮短規(guī)劃的路徑長度。該算法在較大面積的環(huán)境中優(yōu)化效果更佳顯著。將改進的A*算法應用于自主搭建的機器人平臺上的實驗表明，兩階段搜索的改進A*算法優(yōu)化效果明顯，且抑制了累計轉角所引起的傳感器累計誤差，優(yōu)化了機器人路徑執(zhí)行過程中的自定位，保證了路徑執(zhí)行的可靠性，滿足實際應用要求。