封 碩，吉現(xiàn)友，程 博，韓啟東，于少偉

1.長安大學 工程機械學院，西安 710064

2.長安大學 運輸工程學院，西安 710064

移動機器人路徑規(guī)劃的相關工作種類繁多，具體可分為兩大類，全局路徑規(guī)劃和局部路徑規(guī)劃。全局路徑規(guī)劃算法中，A*算法因其快速的計算速度廣泛應用于機器人的路徑劃化問題。對于路徑不發(fā)生變化且路徑中不存在動態(tài)障礙物的情況，機器人只需嚴格遵循全局規(guī)劃的路徑即可。然而，當環(huán)境是動態(tài)的且隨時可能發(fā)生變化時需要借助局部路徑規(guī)劃，以保證機器人及時避開障礙物。其中，局部路徑規(guī)劃算法主要有人工勢場法、動態(tài)窗口法等。動態(tài)窗口法通過在速度空間中采樣多組速度，并模擬在這些速度組下機器人在一定時間內(nèi)的移動軌跡，通過評價函數(shù)對這些軌跡進行打分，從而選擇出最優(yōu)軌跡的速度組，驅(qū)使機器人移動。由于動態(tài)窗口法可以直接獲得機器人在接下來一定時間內(nèi)的速度信息[1]。因此，可以通過獲取動態(tài)障礙物的運動信息，并且與機器人的預測信息進行融合判斷，從而實現(xiàn)機器人在運動時及時避開障礙物，避免發(fā)生碰撞。

對于A*算法的改進，文獻[2-6]分別從不同角度對傳統(tǒng)A*算法進行了優(yōu)化處理，主要成果可歸類為從問題的各個方向上減少了傳統(tǒng)貪心算法帶來的路徑轉(zhuǎn)折點過多、轉(zhuǎn)折角度過大等問題。國內(nèi)路徑研究人員針對障礙物位置不發(fā)生變化時的路徑規(guī)劃算法做出了大量研究，但是對于復雜動態(tài)環(huán)境下的機器人路徑規(guī)劃與導航研究比較少，比如：文獻[7]針對機器人在不同類型障礙物環(huán)境下的路徑規(guī)劃問題，提出基于神經(jīng)網(wǎng)絡的改進粒子群優(yōu)化算法，可以應用于靜態(tài)和動態(tài)障礙物環(huán)境，但是該方法需要對神經(jīng)網(wǎng)絡進行初始化，無法做到及時處理。文獻[8]針對人工勢場法在路徑規(guī)劃中出現(xiàn)的目標點不可達、轉(zhuǎn)折次數(shù)多及路線較長的問題，提出了一種動態(tài)環(huán)境下移動機器人全局路徑規(guī)劃的改進A*勢場算法。文獻[9]針對在局部動態(tài)路徑規(guī)劃中人工勢場法出現(xiàn)的局部最小值和目標不可達等問題，提出一種改進的人工勢場法。文獻[10]提出了一種適用于局部動態(tài)環(huán)境的路徑規(guī)劃的多重A*算法，并結合反應性和慎思型避障控制策略的優(yōu)點進行實時避障。文獻[11]對現(xiàn)有路徑規(guī)劃算法進行了分類，并根據(jù)每種算法的特點將不同算法相互結合運用，以解決動態(tài)環(huán)境下機器人的路徑規(guī)劃問題。文獻[12]為了實現(xiàn)AGV 實時動態(tài)避障，將A*和DWA 兩種算法融合，進行在線實時規(guī)劃路徑，設計了一種基于全局最優(yōu)路徑的圓滑路徑曲線。經(jīng)過仿真，提出的算法在路徑長度、機器人平均轉(zhuǎn)折角度、運行時間等都大大減少。文獻[13]針對動態(tài)環(huán)境下UGV 的正常工作，提出了動態(tài)障礙物窗口法和動態(tài)障礙物樹窗口法，確保了UGV 在工作中可以安全的避開障礙物和環(huán)境安全。為了解決利用動態(tài)窗口法進行機器人避障時可能會撞到一些機器人軌跡附近，但不在軌跡上的障礙物的問題，文獻[14]提出了一種區(qū)域動態(tài)窗口法，通過對傳統(tǒng)動態(tài)窗口法的評價函數(shù)進行修改，最終得到一個可以安全避開軌跡附近障礙物的算法。為了提高機器人運動的智能性，文獻[15-16]提出了一種行人特征模型以識別機器人導航中行人的特征，從而及時避免與行人發(fā)生碰撞。

上述文獻對傳統(tǒng)A*算法和動態(tài)窗口法從不同角度進行了優(yōu)化，但是未考慮機器人在復雜多變的、高度動態(tài)的環(huán)境下機器人的路徑規(guī)劃問題。針對以上問題，提出了一種融合機器人與障礙物運動信息的改進動態(tài)窗口法來解決機器人在動態(tài)環(huán)境下的局部路徑規(guī)劃問題，并且與優(yōu)化A*算法相結合來實現(xiàn)全局最優(yōu)路徑規(guī)劃。通過實驗驗證，該算法與傳統(tǒng)的動態(tài)窗口法相比，具有更好的動態(tài)避障能力、路徑更加符合避讓規(guī)則，同時避免了陷入局部最優(yōu)解的問題。

1 優(yōu)化A＊算法

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

A*算法是一種快速、有效求解最短路徑的啟發(fā)式搜索算法。其主要思想為，從起點開始對周圍的八個點進行搜索，計算周圍點到當前點的實際代價值以及周圍點到目標點的估計代價值，通過選擇總代價值最小的點作為下一個節(jié)點，直到終點。其代價函數(shù)如下：

式中，F(xiàn)(n)是節(jié)點n的代價函數(shù)，G(n)是在狀態(tài)空間中從初始節(jié)點到第n個節(jié)點的實際代價，H(n)是從節(jié)點n到目標節(jié)點的估計代價。其中H(n)可分為有曼哈頓距離、歐氏距離和切比雪夫距離，本文算法重點比較所規(guī)劃路徑的長度，故采取歐氏距離度量節(jié)點之間的代價，歐氏距離計算公式如下：

式中，(xs,ys)為當前點的位置坐標，(xg,yg)為目標點的位置坐標。

1.2 優(yōu)化A＊算法

傳統(tǒng)的A*算法只能對當前點周圍的八個點進行搜索，從而導致利用A*算法求解的路徑中存在大量的冗余轉(zhuǎn)折點。為了解決該問題，本文提出一種轉(zhuǎn)折點剔除算法，將傳統(tǒng)A*算法求解的路徑冗余點剔除，從而得到一條長度較短、更平滑的最優(yōu)路徑（如圖1）。

圖1中紅色曲線為優(yōu)化后的路徑，藍色表示傳統(tǒng)A*算法路徑，黑色柵格表示障礙物。

其優(yōu)化的具體步驟如下：

（1）獲取傳統(tǒng)A*算法求解的全局路徑點集合P。用Pi表示第i個路徑點。新建一個只包含起始點和終點的集合Pn。

（2）令i=1，m=2。判斷第i個路徑點Pi與Pi+m路徑點之間是否存在障礙物。

（3）若存在障礙物，則將路徑點Pi加入到Pn集合，令i=i+1。繼續(xù)判斷第i個路徑點Pi與Pi+m路徑點之間是否存在障礙物。若存在則返回第（3）步，若不存在則返回第（4）步。

（4）若不存在，則剔除Pi與Pi+m中間的路徑點，令m=m+1。判斷第i個路徑點Pi與Pi+m路徑點之間是否存在障礙物。若存在則返回第（3）步，若不存在則返回第（4）步。直到終點。

（5）依次連接集合Pn中的路徑點，得到一條剔除冗余轉(zhuǎn)折點的最優(yōu)路徑。

2 優(yōu)化動態(tài)窗口法

2.1 傳統(tǒng)動態(tài)窗口法

動態(tài)窗口法是機器人局部路徑規(guī)劃中常用的一種路徑算法。該算法采樣多組速度（包括：線速度、加速度、角速度、角加速度），并模擬出這些速度下機器人下一段時間內(nèi)的位置移動軌跡。然后，通過評價函數(shù)對這些軌跡進行評價處理，選擇最優(yōu)速度組，驅(qū)使機器人移動。傳統(tǒng)動態(tài)窗口法的評價函數(shù)主要分為三部分，具體表達式如下：

其中，heading(v,w)是方位角評價函數(shù)，用來評價機器人在當前設定的采樣速度下，達到模擬軌跡末端時的朝向和目標之間的角度差距。velocity(v,w)是速度評價函數(shù)，用來評價當前軌跡的速度大小。dist(v,w)為距離評價函數(shù)，表示機器人在當前軌跡上與最近的障礙物之間的距離。如果在這條軌跡上沒有障礙物，則設定為一個常數(shù)。α、β、γ為權重參數(shù)，ρ為歸一化參數(shù)。

在傳統(tǒng)動態(tài)窗口法中主要存在以下缺陷：

（1）由于缺少全局規(guī)劃，機器人一旦面臨“L”型、“C”型等半封閉障礙物區(qū)間時，會導致評價函數(shù)失靈，無法選擇正確路徑。

（2）在障礙物發(fā)生移動的動態(tài)環(huán)境下，機器人只有在距離障礙物很近時才能觸發(fā)判斷函數(shù)，但是由于機器人與障礙物皆在移動，處于評價函數(shù)的機制問題，機器人無法做到安全、及時的避障。

針對動態(tài)窗口法在復雜動態(tài)環(huán)境中存在的兩大問題，提出了一種融合障礙物運動信息的改進算法。

2.2 優(yōu)化動態(tài)窗口法

2.2.1 優(yōu)化動態(tài)避障功能

傳統(tǒng)的動態(tài)窗口法在障礙物固定不動的環(huán)境下具有很好的魯棒性，可實現(xiàn)機器人的局部避障。但是當環(huán)境中存在高度動態(tài)的障礙物時，傳統(tǒng)的動態(tài)窗口法將不在具備可信度，無法做到及時避障、且容易陷入局部最優(yōu)解。但是，傳統(tǒng)動態(tài)窗口法的評價函數(shù)結構清晰，具有很好的可擴展性。因此本文通過對傳統(tǒng)評價函數(shù)進行擴展，在原來的基礎上新增一項可動態(tài)避障的評價函數(shù)(dym_dist(v,w))。

通過對dym_dist(v,w)函數(shù)的定義，使得機器人在感知到障礙物時可以根據(jù)速度差和距離大小的綜合判斷來選擇下一時刻的速度組。經(jīng)擴展后的優(yōu)化動態(tài)窗口法的評價函數(shù)如式（5）所示：

式中，δ為權重參數(shù)。

對于動態(tài)環(huán)境下的移動障礙物，由于不同的障礙物其速度不同，因此其單位時間內(nèi)可運動的距離也有所不同，為了實現(xiàn)更加智能的避障，將不同速度的障礙物賦予其不同大小的危險區(qū)域供機器人參考（如圖2）。速度大的障礙物，其在單位時間內(nèi)的運動距離大，從而危險區(qū)域更大，反之則更小。

對于圖2 中的安全行駛決策，在機器人行駛過程中，原則上不應該影響到動態(tài)障礙物（行人、車輛等動態(tài)因素）的移動，要自主的根據(jù)障礙物移動信息進行預測、判斷，選擇路徑。

主要算法流程步驟為：

步驟1 根據(jù)機器人預測軌跡與障礙物危險區(qū)域，判斷兩者是否即將相遇。如果是，則進行步驟2。如果沒有，則按照傳統(tǒng)動態(tài)窗口法運動。

步驟2 計算每一個速度組下機器人的預測位置，將預測位置觸及障礙物危險區(qū)域的速度組拋棄。

步驟3 對剩余的速度組進行安全駛離判斷。計算機器人當前移動方向與障礙物到機器人連線方向的叉乘在坐標軸Z軸方向的正負。如果為負，則機器人應該往左行駛，反之則向右行駛。

步驟4 根據(jù)機器人的移動方向，給定新增評價函數(shù)的權重參數(shù)。對于符合安全駛離移動方向的預測軌跡，dym_dist(v,w)取正值，反之，取負值。

步驟5 計算所有軌跡的評分，選擇最大評分軌跡驅(qū)使機器人移動。

其中，步驟3的具體算法如圖2所示，假設機器人預測軌跡已經(jīng)進入障礙物危險區(qū)域，此時需要判斷機器人如何行駛。用W表示Pnr與Pr的叉乘在坐標軸z方向的正負（式（6）），如果為負則機器人向左行駛（圖2左圖），反之向右行駛（圖2右圖）。

式中，Pr表示機器人速度方向、Po表示障礙物速度方向、Pnr表示障礙物與機器人連線方向。

圖2 中，灰色圓形表示機器人，紅色圓形表示障礙物，綠色圓形表示危險區(qū)域，紅色表示障礙物運動方向，藍色表示機器人運動方向，黑色表示障礙物與機器人連線方向。

2.2.2 優(yōu)化局部最優(yōu)缺陷

針對動態(tài)窗口法易陷入局部最優(yōu)和成功率低等缺點，將動態(tài)窗口法與優(yōu)化A*算法做融合，進一步優(yōu)化動態(tài)窗口法的評價函數(shù)，從而繼續(xù)對路徑進行局部優(yōu)化。按照各個路徑點之間的相對距離大小，對A*算法計算出的路徑點進行插值，使得只有I個路徑點的原始路徑增加到J(J＞I)。然后，設置一個閾值M，計算機器人起始位置到所有路徑點的距離，找到距離大于M的最近路徑點，設置為臨時目標點。同時，從路徑點集合中剔除距離小于M的路徑點。執(zhí)行優(yōu)化DWA 算法，計算機器人當前位置到剩余路徑點的距離，找到距離大于M的最近路徑點，替換原來的臨時目標點，直到終點。

式中，Heading( )v,w為機器人與臨時目標點之間的方位角函數(shù)，用來評價機器人在當前設定的采樣速度下，達到模擬軌跡末端時的朝向和臨時目標之間的角度差。

最終得到機器人在復雜動態(tài)環(huán)境下的基于全局最優(yōu)路徑的動態(tài)避障路徑規(guī)劃的流程圖（如圖3所示）。

3 實驗分析

為了驗證算法的可行性，在MATLAB 中對該算法進行實驗驗證。采用柵格法將環(huán)境劃分為24×24 的柵格地圖，其中黑色表示障礙物，白色表示非障礙物。實驗中用到的機器人速度信息如表1 所示。算法參數(shù)如表2、表3所示。

表1 移動機器人機械特性Table 1 Mechanical characteristic of mobile robot

表2 動態(tài)窗口法的基本參數(shù)Table 2 Basic parameters of dynamic window approach

表3 評價函數(shù)參數(shù)Table 3 Evaluation function parameter

以（4，4）為起點、（20，18）為終點，分別采用傳統(tǒng)A*算法和優(yōu)化A*算法進行路徑規(guī)劃，如圖4所示，其中紅色曲線表示傳統(tǒng)A*算法，綠色曲線表示優(yōu)化A*算法。從圖4中可以得到，傳統(tǒng)A*算法的轉(zhuǎn)折點有9個，路徑長度為22.728 m。優(yōu)化A*算法僅有5個，路徑長度為21.367 m，相較于傳統(tǒng)算法優(yōu)化后的路徑在轉(zhuǎn)折點上減少了4個，路徑長度減少了1.361（5.99%）m，且路徑更加平滑。

如圖5所示，以（4，4）為起點，（12，12）為終點，在其間加入L 型障礙物，通過傳統(tǒng)動態(tài)窗口法進行路徑規(guī)劃。從圖5 可以得到傳統(tǒng)的動態(tài)窗口法在進行路徑規(guī)劃時容易陷入局部最優(yōu)解，特別是當導航路線中存在U、L型等凹形障礙物時尤為明顯。同時，對于環(huán)境中存在動態(tài)障礙物時，由于障礙物在實時移動，僅通過傳統(tǒng)動態(tài)窗口法進行路徑規(guī)劃容易出現(xiàn)碰撞等危險，因為只有當障礙物距離機器人很近時才會進行避障。圖6 為僅采用傳統(tǒng)動態(tài)窗口法面對動態(tài)障礙物時的路徑規(guī)劃路線，由圖可知當機器人開始準備避障時，已經(jīng)與障礙物發(fā)生碰撞。

對于存在動態(tài)障礙物的環(huán)境，將優(yōu)化A*算法與優(yōu)化動態(tài)窗口法進行融合，利用優(yōu)化A*算法得到的全局最優(yōu)路徑作為動態(tài)窗口法的臨時目標路徑指示，如圖7所示，紅色曲線表示優(yōu)化A*算法求解的全局最優(yōu)路徑，紅色圓形表示動態(tài)障礙物，綠色圓形表示危險區(qū)域，藍色箭頭表示障礙物移動方向。優(yōu)化動態(tài)窗口法以臨時目標點（temp goal）作為行駛目標，驅(qū)使機器人盡可能地按照全局最優(yōu)路徑行駛，避免陷入局部最優(yōu)。由圖8、9、10、11、12可知，機器人在預測軌跡觸及障礙物危險區(qū)域時開始執(zhí)行遠離決策，通過預先設置的評價函數(shù)，將機器人速度信息與障礙物速度信息進行分析，從而決定機器人在下一時刻的運動狀態(tài)，安全規(guī)避動態(tài)障礙物。其中，從圖8、10可知，當機器人遇到動態(tài)障礙物時，機器人一致性的向左行駛，從而在不阻礙障礙物運動的條件下安全的避開障礙物，同時盡可能地按照全局最優(yōu)路徑進行行駛，最終得到一條既考慮全局最優(yōu)路徑也考慮動態(tài)障礙物的實時避障路徑（如圖12）。圖13為機器人在面臨動態(tài)障礙物時的速度變化圖。從圖13中可知，每當新的障礙物出現(xiàn)在預測范圍內(nèi)時，機器人速度會適當降低，但是在確定正確的行駛方向后又迅速回到最大值，以保證機器人的運動速度。

針對優(yōu)化的動態(tài)窗口法和傳統(tǒng)動態(tài)窗口法，進行多次仿真實驗，得到其碰撞次數(shù)、行駛時間、路徑長度、平均速度的對比表。由表4可知，優(yōu)化算法的碰撞次數(shù)降低了95%，行駛時間降低了46%，路徑長度降低了37%，平均速度提高了13%?？偟膩碚f，優(yōu)化算法可以大幅度減少與動態(tài)障礙物的碰撞次數(shù)，同時提升機器人運動速度、減少路徑長度。

表4 優(yōu)化算法的性能提升Table 4 Performance improvement of optimization algorithm單位：%

4 結束語

針對傳統(tǒng)動態(tài)窗口法在面對動態(tài)障礙物時容易出現(xiàn)碰撞，環(huán)境復雜時容易陷入局部最優(yōu)等問題，以及傳統(tǒng)A*算法在路徑上存在冗余轉(zhuǎn)折點的問題，提出了一種融合動態(tài)障礙物運動信息的路徑規(guī)劃算法。通過與傳統(tǒng)A*算法作比較可以得到，剔除冗余轉(zhuǎn)折點后的優(yōu)化算法在路徑上更加平滑、長度更短。與傳統(tǒng)動態(tài)窗口法做比較可以得到，優(yōu)化后的算法在面對動態(tài)障礙物時，通過采用快速駛離決策可以在不干涉障礙物移動的前提下安全有效地避開動態(tài)障礙物。同時，通過將優(yōu)化A*算法的最優(yōu)路徑作為局部路徑規(guī)劃的臨時目標，解決了動態(tài)窗口法容易陷入局部最優(yōu)解的問題。最終得到一種可以既考慮全局最優(yōu)路徑也考慮動態(tài)障礙物的實時避障路徑規(guī)劃算法。