汪 馨，姚念民，譚國真

大連理工大學 計算機科學與技術(shù)學院，遼寧 大連 116024

移動機器人要想在特定環(huán)境下安全地完成任務，則既要做到實時感知周圍環(huán)境信息、無碰撞地到達目標點，也要使路徑質(zhì)量盡可能得高、使移動機器人盡可能快地沿著路徑平順地到達目標點。常用的全局路徑規(guī)劃算法包括：RRT（rapidly exploring random tree，RRT）算法[1-2]、A*算法[3-4]、遺傳算法[5]、蟻群算法[6]等。常用的局部路徑規(guī)劃算法有動態(tài)窗口法（dynamic window approach，DWA）[7-8]、人工勢場法[9]等。

全局路徑規(guī)劃無論是基于采樣的RRT算法，還是基于啟發(fā)式搜索的A*算法，都可以在相對短的時間內(nèi)搜索出一條從起點到終點的全局路徑，但是往往無法躲避局部障礙物，對局部動態(tài)環(huán)境的適應性不強，無法滿足在未知環(huán)境下避障的要求。局部路徑規(guī)劃DWA算法可以在線實時規(guī)劃，具有良好的避障能力，但是在大規(guī)模復雜環(huán)境中如果缺少中間點引導，則可能無法得到最優(yōu)路徑，甚至無法達到目標點。因此，結(jié)合兩者的優(yōu)勢，實現(xiàn)兩種算法融合是當前研究的熱點[10-11]。Li等人[12]擴展A*算法的搜索鄰域，將改進的A*算法與DWA算法融合，雖然保證了更流暢的路徑，但是前期規(guī)劃時間過長。勞彩蓮等人[13]對DWA的評價函數(shù)進行改進，將A*算法與改進的DWA算法進行融合，實現(xiàn)實時最優(yōu)路徑規(guī)劃。Wang等人[14]提出RRT與DWA相結(jié)合的算法，在動態(tài)約束條件下為機器人實現(xiàn)了有效和平滑的路徑，但是該算法僅考慮了靜態(tài)的障礙物。張杰[15]用3次B樣條曲線的路徑平滑方法對RRT*算法進行優(yōu)化處理，同時將DWA算法與改進RRT*相結(jié)合，對全局最優(yōu)目標函數(shù)進行設計，該混合算法提高了行駛速度。

利用啟發(fā)式搜索的全局規(guī)劃，要么犧牲了實時的效率，要么只將啟發(fā)式方法應用于搜索的某些方面而降低了效率。利用抽樣搜索的全局規(guī)劃，要么犧牲了實時的效率，要么通過在解決方案成本以外的指標上排序搜索而降低了效率[16]。

針對這些問題，本文引入BIT*這種將A*算法與RRT*算法相融合的快速規(guī)劃路徑算法[17]，該算法在超橢球子集中執(zhí)行排序搜索，可以找到更好狀態(tài)的集合，即更短的全局路徑[18]，但是其冗余節(jié)點會使得搜索成本增加，所以對路徑進行拉伸優(yōu)化，縮短路徑的同時減少節(jié)點數(shù)量，避免將無價值的節(jié)點和邊加入到生成樹中。將全局路徑點作為改進的DWA算法的中繼點，增加軌跡點評價函數(shù)，將動態(tài)因素考慮在內(nèi)、對距離評價函數(shù)進行細分，實現(xiàn)實時避障的任務。最后，通過對比仿真實驗驗證了融合算法的優(yōu)越性。

1 改進BIT＊算法的全局路徑規(guī)劃

1.1 問題描述

可行路徑是一條能夠避開障礙物且連接起點與終點的路徑。最優(yōu)路徑是要使成本函數(shù)（例如，路徑長度）最小化的可行路徑。由此可知，解決一個最優(yōu)規(guī)劃問題需要解決潛在的可行規(guī)劃問題[19]。

定義X?Rn表示路徑規(guī)劃問題的狀態(tài)空間，定義Xobs?X表示與障礙物碰撞的狀態(tài)，即有障礙物的區(qū)域。定義函數(shù)σ:[0,1]→Xfree表示一條無碰撞可行路徑，其中σ(0)=Xstart,σ(1)=Xgoal。定義S表示所有路徑集合，Sfree表示所有無碰撞可行路徑的集合，則最優(yōu)路徑規(guī)劃問題可以被描述為，尋找一條可行路徑σ*，使得成本函數(shù)c:Sfree→R≥0最小，即：

其中，R≥0表示非負實數(shù)的集合。

1.2 函數(shù)定義

定義cbest表示當前最優(yōu)路徑長度。定義搜索樹模型T=(V,E)，其中V?Xfree表示樹模型中的節(jié)點，E={(v1,v2)}表示樹模型中的連線。

定義函數(shù)c?(x,y)表示點x∈X和點y∈X構(gòu)成連線的歐拉距離，定義函數(shù)c(x,y)表示點x∈X和點y∈X在樹模型中的連線長度，如果連線于障礙物相交，則認為其成本無窮大，即c?(x,y)=∞或者c(x,y)=∞。

1.3 傳統(tǒng)BIT*算法主體模塊

由文獻[16]可知，可以將BIT*算法分為：批采樣點生成、連線、選邊、節(jié)點擴展以及修整模塊。

（1）采樣點生成。在橢圓區(qū)域中進行采樣，可以先在標準圓方程中采樣，再將采樣點旋轉(zhuǎn)平移到實際采樣區(qū)域。由圖1可知，長軸長度為cbest，表示當前狀態(tài)下最優(yōu)路徑長度，短軸長度為c2best-c2min，其中cmin表示起點與終點之間的歐拉距離。

圖1 生成采樣點區(qū)域Fig.1 Region to generate sample points

（2）連線選擇。隊列QE存儲以最優(yōu)移動成本遞增排列的連線(v,x)，隊列QV存儲以最優(yōu)移動成本遞增排列的節(jié)點v∈V，函數(shù)BestInQueue(QE)可以計算當前樹模型中的最優(yōu)連線，函數(shù)BestInQueue(QV)可以計算當前樹模型中最優(yōu)節(jié)點。函數(shù)BestValue(QE)可以計算當前樹模型中的連線的最優(yōu)移動成本，函數(shù)BestValue(QV)可以計算當前樹模型中節(jié)點的最優(yōu)移動成本。只有當節(jié)點v∈V的最優(yōu)移動成本小于樹模型中連線的最優(yōu)移動成本時，才將該節(jié)點進行連線擴展，避免在各節(jié)點上的遍歷預算，減少計算規(guī)模。

（3）連線操作。通過BestInQueue(QE)計算獲得QE中的最優(yōu)連線(vm,xm)，并將該連線從隊列QE中移除，以進行連線的下一步相關(guān)操作。

（4）節(jié)點擴展。首先，從Qv中彈出最優(yōu)節(jié)點v，然后獲取與節(jié)點v歐拉距離小于r的節(jié)點構(gòu)成采樣點xrand，當xrand中的節(jié)點x，滿足公式（3）時，將連線(v,x)加入QE。

（5）修剪操作。從采樣點中移除估計成本大于給定成本的節(jié)點，從已有節(jié)點中移除估計成本大于給定成本的節(jié)點，從已有連線中移除估計成本大于給定成本的連線。

1.4 改進BIT＊算法主體模塊

已知BIT*的收斂速度取決于橢圓區(qū)域的大小，而橢圓區(qū)域的大小取決于當前最優(yōu)路徑長度cbest，所以重新審視當前路徑，減少冗余點帶來的路徑成本。

改進路徑點過程如圖2所示，黑色實線是原始路徑。首先連接xstart和x3，當連線與障礙物不發(fā)生碰撞時，連接xstart與x4,x5,…,xn，當xstart與xn的連線與障礙物發(fā)生碰撞時，以發(fā)生碰撞的障礙物轉(zhuǎn)角點為中心，機器人的直徑長度為半徑做圓，得到點xstart與圓的切點，則成為下一路徑點，與xn,xn+1,…,xgoal相連，如圖3（a）（b），重復這個過程直到到達目標。

圖2 更新路徑點示意圖Fig.2 Updating waypoint

圖3 改進BIT*過程示意圖Fig.3 Process of improved BIT*

算法1為調(diào)整路徑函數(shù)的偽代碼，該算法對路徑點進行循環(huán)處理，直到到達目標點。第6行的NoCollision函數(shù)檢查兩個路徑點之間是否存在障礙物，第10行Collision函數(shù)找到距離Xn最近的發(fā)生碰撞的障礙物轉(zhuǎn)角點，第11行ContactPoint函數(shù)求出切點，即找到下一路徑點。

算法1調(diào)整路徑

顯然，利用三角形任意兩邊之和大于第三邊的原理，很容易證明改進后的路徑具有更少的節(jié)點和更短的路徑。

在圖3中，黑色實線是原始路徑，紅色實線表示改進后的新路徑。可以看出，新路徑的長度比原來的路徑短。

在圖3（c）中，黑色虛線橢圓是由傳統(tǒng)BIT*算法得到的下一個采樣區(qū)域的邊界，紅色虛線橢圓是改進后產(chǎn)生的邊界。可以看出紅色橢圓區(qū)域的采樣點更有可能縮短當前路徑，加快收斂速度。

2 改進DWA算法的局部路徑規(guī)劃

2.1 運動學模型

設在t時刻下，移動機器人的線速度為v(t)，角速度為ω(t)，朝向角度為θ(t)，坐標為(xt,yt)，變化時間間隔為Δt，則機器人運動學模型可表達為：

2.2 速度采樣

根據(jù)環(huán)境和機器人物理特性的限制，DWA算法可以將采樣速度限制在一定的空間內(nèi)，其在速度空間受到的主要約束如下：

（1）線速度和角速度的約束。線速度與角速度都存在最值，其速度對(v,ω)滿足的約束公式為：

其中，vmin、vmax分別為最小、最大線速度，ωmin、ωmax分別為最小、最大角速度。

（2）加速度和減速度的約束。對于移動機器人而言，加速度受制于電機的輸出扭矩，會存在最大加速度，以及最大減速度，具體約束為：

其中，vt表示t時刻的線速度，avdmax表示線速度中最大減速度，avimax表示線速度中最大加速度，ωt表示t時刻的角速度，awdmax表示角速度中最大減速度，awimax表示角速度中最大加速度。

（3）制動距離約束?？紤]到機器人的移動安全性，移動機器人能夠在不與障礙物發(fā)生碰撞的情況下停下來。具體制動約束如下：

其中，dist(vt,ωt)表示在速度(vt,ωt)時，移動機器人與障礙物的最近距離。

最終移動機器人的速度需要對以上三個約束空間取交集：V=Vs∩Vd∩Ve。

針對一系列滿足要求的速度采樣點，將其帶入評價函數(shù)，選取最優(yōu)軌跡點作為下一時刻的運動軌跡。

2.3 DWA算法評價函數(shù)

傳統(tǒng)DWA算法的評價函數(shù)包含方位評價、速度評價和距離評價三個方面，具體公式如下：

其中，Head(v,ω)為方位角評價函數(shù)，用來計算移動機器人在有限速度下到達模擬軌跡終點方向與目標方向之間的角度偏差；Vel(v,ω)為速度評價函數(shù)，用于計算當前預估的運行速度；Dist(v,ω)為距離評價函數(shù)，用于計算機器人預測點位置與最近障礙物之間的距離。α、β、γ為每一項的加權(quán)系數(shù)，評價函數(shù)G(v,ω)表示機器人在保持與障礙物的距離同時以較大的速度朝著目的地前進。

2.4 DWA算法評價函數(shù)優(yōu)化

傳統(tǒng)DWA算法容易陷入局部最優(yōu)，針對該問題，用改進的BIT*算法得到的全局路徑來引導DWA算法進行局部規(guī)劃。為更好接近全局規(guī)劃路徑，在評價函數(shù)中添加軌跡點評價函數(shù)Point(v,ω)，用于計算DWA算法在t時刻預測的軌跡點(xt,yt)與BIT*求得的全局路徑點(xg,yg)之間的最小距離。

針對移動障礙物問題，對傳統(tǒng)DWA算法中的距離評價函數(shù)進行改進，對距離評價函數(shù)進行細分，劃分為Dist_S(v,ω)和Dist_D(v,ω)這兩個距離評價項，分別用于評價移動機器人與靜態(tài)障礙物和動態(tài)障礙物的距離，對動、靜態(tài)障礙物分別設置不同的安全距離。

設靜態(tài)障礙物坐標為(xs,ys),Ds為t時刻的軌跡點距離靜態(tài)障礙物最近的距離。R為移動機器人的底盤半徑，不規(guī)則的移動機器人以最長底盤半徑為R，在此基礎上加入0.2R來保證與靜態(tài)障礙物之間的安全距離。Dist_S(v,ω)約束公式如下：

設動態(tài)障礙物坐標為(xi,yi),Dd為t時刻的軌跡點距離動態(tài)障礙物最近的距離。Vt表示移動機器人在t時刻的速度，S為機器人在時間間隔Δt內(nèi)運動的路徑，由于可能會存在變加速問題，所以將其擴展為1.2倍，來保證與動態(tài)障礙物之間的安全距離。Dist_D(v,ω)約束公式如下：

則，最終的DWA評價函數(shù)為：表示機器人在全局規(guī)劃的路徑基礎上保持與障礙物的距離，同時按照以較大的速度朝著目的地前進。

3 融合算法

改進的BIT*算法在靜態(tài)環(huán)境中能很好地完成全局路徑規(guī)劃，但是當出現(xiàn)未知障礙物時，就無法及時避障，其局部路徑規(guī)劃能力較弱。DWA算法所生成的路徑為局部最優(yōu)并非全局最優(yōu)，如果缺少全局指引，只有目的地一個方向指引，會容易陷入局部最優(yōu)，導致全局路徑變大，甚至全局路徑規(guī)劃失敗。

針對上述問題，首先用改進的BIT*算法在已知障礙物的地圖中規(guī)劃全局路徑，即在移動前，找到全局最優(yōu)路徑，然后將提取的關(guān)鍵轉(zhuǎn)折點作為移動機器人的中繼點。移動過程中，如果傳感器感知到未知障礙物阻擋了算法所規(guī)劃的路徑，則用改進的DWA算法根據(jù)更新的障礙物信息實時規(guī)劃新場景下的避障路徑。融合算法添加了軌跡點評價函數(shù)，避免局部規(guī)劃過分偏離，同時對未知障礙物分配更高的比重，減少已知障礙物對路徑的影響，融合算法流程圖如圖4所示。

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

4 仿真實驗

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

為了驗證改進算法的有效性，采用python開發(fā)的開源工具PathEnv。仿真環(huán)境為二維靜態(tài)地圖，設置為30×30的正方形，其中黑色格子代表障礙物，白色格子代表可移動區(qū)域，藍色格子代表起點，紅色格子代表終點。動態(tài)環(huán)境中，黃色格子代表未知靜態(tài)障礙物，紫色格子代表未知動態(tài)障礙物，紫色虛線代表動態(tài)障礙物的移動路徑。

地圖1為簡單地圖，起點（2，29），目標點（20，4），地圖2包含有窄通道，起點（6，20），終點為（23，10），地圖3比較復雜，包含有較多的窄通道，此環(huán)境下的實驗能體現(xiàn)算法優(yōu)劣，設置起點（2，29），目標點（20，4）。在三組地圖中分別對RRT*算法、BIT*算法、改進的BIT*算法以及融合算法進行仿真實驗，對四種算法求得的路徑進行統(tǒng)計與比較。在動態(tài)環(huán)境下對改進的BIT*算法、傳統(tǒng)DWA算法以及融合算法進行仿真實驗，統(tǒng)計數(shù)據(jù)進行分析比較。

相關(guān)運動學參數(shù)設置：初始方位角-π/2，最大線速度1.5 m/s，最小線速度0，即設置為不可倒退，最大角速度40 rad/s，最大線加速度為0.2 m/s，最大角加速度為50 rad/s，線速度分辨率為0.01 m/s，角速度分辨率為0.1 rad/s，采樣周期0.1 s，機器人半徑設置為0.3 m。

4.2 靜態(tài)環(huán)境仿真結(jié)果

對RRT*算法、BIT*算法、改進的BIT*算法以及融合算法在三組靜態(tài)地圖中對進行仿真實驗，其仿真結(jié)果如圖5、圖6、圖7。對上述三組二維靜態(tài)地圖仿真環(huán)境進行數(shù)據(jù)統(tǒng)計如表1。

表1 三組靜態(tài)地圖仿真結(jié)果Table 1 Three groups of map simulation results

圖5 地圖1仿真結(jié)果Fig.5 Simulation results of Map1

圖6 地圖2仿真結(jié)果Fig.6 Simulation results of Map2

圖7 地圖3仿真結(jié)果Fig.7 Simulation results of Map3

圖中藍綠色線段表示路徑擴展過程，即搜索樹的擴展過程，藍色顆粒表示隨機生成的采樣點，藍色虛線橢圓表示更新的采樣區(qū)域邊界，紅色實線表示算法最終規(guī)劃的路徑。

為進一步驗證融合算法在大規(guī)模環(huán)境下的有效性，格外增加地圖4，大小為60×60，起點（0，60），目標點（60，0），對A*算法、蟻群算法、改進的BIT*算法以及融合算法進行仿真實驗，其仿真結(jié)果如圖8，數(shù)據(jù)統(tǒng)計如表1。

其中，圖8（a）中灰色點區(qū)域表示已搜索區(qū)域，紅色實線表示算法最終規(guī)劃的路徑。

圖8 地圖4仿真結(jié)果Fig.8 Simulation results of Map4

由表1可知，改進的BIT*算法提供了質(zhì)量更高的全局規(guī)劃路徑，其優(yōu)化主要體現(xiàn)在轉(zhuǎn)折點個數(shù)以及路徑長度上。在前三個地圖中，改進BIT*算法的路徑轉(zhuǎn)折點較RRT*算法平均優(yōu)化84.60%，比傳統(tǒng)BIT*算法平均高39.40%，改進BIT*算法的路徑長度較RRT*算法平均優(yōu)化30.17%，比傳統(tǒng)BIT*算法平均高5.30%。在地圖4中，改進BIT*算法提供了更少的路徑轉(zhuǎn)折點和更短的全局路徑，在大規(guī)模地圖中也有可行性。

在簡單地圖1和大規(guī)模地圖4中，路徑長度優(yōu)化幅度較小，但在擁有狹窄通道的地圖2中，路徑長度優(yōu)化24.92%，尤其是包含更多狹窄通道的地圖3，其軌跡優(yōu)化比例為46.66%，可知改進的BIT*算法可以在更少的迭代次數(shù)下找到更優(yōu)的路徑，尤其是在擁有狹窄通道的復雜環(huán)境下。融合算法的路徑長度平均優(yōu)化率為21.01%，可以看出在使用改進BIT*算法和改進DWA算法的融合，在保證局部連貫性的同時也貼近了全局規(guī)劃，滿足移動機器人靜態(tài)環(huán)境下的路徑規(guī)劃需求。

4.3 動態(tài)環(huán)境仿真結(jié)果

在添加了未知動靜態(tài)障礙物的地圖3中分別對改進BIT*算法、改進動態(tài)窗口法以及融合算法進行仿真實驗，其仿真結(jié)果如圖9所示。

圖9 動態(tài)地圖仿真結(jié)果Fig.9 Dynamic simulation results of fusion algorithm

地圖中，黃色格子代表未知靜態(tài)障礙物，紫色格子代表未知動態(tài)障礙物，紫色虛線代表動態(tài)障礙物的移動路徑，紫色箭頭代表動態(tài)障礙物的移動方向。藍色虛線表示改進BIT*算法規(guī)劃的全局路徑，藍色星點為提取的關(guān)鍵路徑點，紅色實線表示實際運動路徑。

由仿真結(jié)果圖9（a）可知，改進的BIT*算法雖然提供了轉(zhuǎn)折點更少、路徑更短的全局規(guī)劃路線，但是在機器人移動后，如果在已規(guī)劃路徑上出現(xiàn)未知障礙物，則會與障礙物距離為負，即發(fā)生碰撞，那碰撞之后的路徑則無效，所以，改進的BIT*算法適合在機器人移動前對其進行全局規(guī)劃，但在移動過程中則無法解決未知障礙物問題。由仿真結(jié)果圖9（b）可知，改進的DWA算法，可以很好地避開未知動靜態(tài)障礙物，但是在沒有全局規(guī)劃的指引下，會陷入局部最優(yōu)，無法到達目標點。

由仿真結(jié)果圖9（c）可知，當碰到未知靜態(tài)障礙物時，融合算法可以在貼近全局規(guī)劃的同時進行有效避障；當未知障礙物阻擋過程性目標點時，機器人距離障礙物一定距離時會舍棄該點，更新下一目標點；當碰到未知動態(tài)障礙物時，融合算法可以進行有效避障。

總之，在改進的BIT*算法提供的全局規(guī)劃路徑下，融合算法滿足機器人既要避開障礙物又要全局路徑最優(yōu)的需求。

5 結(jié)束語

本文針對移動機器人在復雜環(huán)境下的動態(tài)路徑規(guī)劃問題做了以下研究：

（1）全局路徑規(guī)劃采用改進的BIT*算法，在傳統(tǒng)BIT*算法的基礎上對路徑進行拉伸優(yōu)化，對比傳統(tǒng)RRT*算法和BIT*算法，改進的BIT*算法可以在更短的迭代時間內(nèi)找到更優(yōu)的無碰撞路徑，且在大規(guī)模環(huán)境中也有可行性。

（2）局部路徑規(guī)劃采用改進的DWA算法，通過對距離函數(shù)進行改進的同時引入軌跡點評價函數(shù)，避免了局部規(guī)劃過分偏離，也減少了已知障礙物對路徑的影響，有效改善傳統(tǒng)DWA算法局部最優(yōu)的問題。

（3）融合算法將全局規(guī)劃的關(guān)鍵轉(zhuǎn)折點當作局部規(guī)劃的中間過程點，將全局規(guī)劃能力與實時避障能力相結(jié)合，保證路徑質(zhì)量的同時避開未知動靜態(tài)障礙物，滿足機器人路徑規(guī)劃需求，實現(xiàn)了更為有效的實時避障的全局路徑規(guī)劃。

本文主要在傳統(tǒng)算法基礎上進行改進，并與傳統(tǒng)算法進行對比，但目前已有提出將路徑規(guī)劃與強化學習等知識相結(jié)合的研究[20]，所以之后會將本文提出的算法與更前沿的知識相結(jié)合，在更大、更動態(tài)的仿真環(huán)境中進行實驗，得到更靈活的路徑規(guī)劃算法。