基于改進LGM-BRRT＊的移動機器人路徑規劃算法

謝志長，嚴華

（四川大學電子信息學院，成都610065）

0 引言

隨著移動機器人在核設施、宇宙探索、救援任務等領域應用的快速增長，移動機器人在給定的工作環境中如何選擇行走路徑、避免碰撞障礙物、快速到達指定目標的路徑規劃問題成為移動機器人研究熱點之一。傳統的路徑規劃算法有：人工勢場法、BUG 算法、A*算法、可視圖法等。人工勢場法是一種成熟的局部避障算法，但是容易陷入死鎖現象[1]，存在局部最小問題。為了解決這一問題，Kavraki 在文獻[2-3]中提出PRM算法，但是該算法不適合在未知環境下做路徑規劃[2]。BUG 算法運行時間長，容易陷入局部最優等問題[4]。A*算法在復雜情況下計算量急劇增長，效率低下[5]。RRT（Rapidly-exploring Random Tree）算法是一種單查詢方法[6]，能夠解決大多數運動規劃問題，并且它的改進具有更好的性能。例如：B-RRT[7-8]、RRT-connect[9]、目標偏向RRT[10]。然而，這些算法的局限性在于它們沒有考慮路徑成本，因此無法保證最佳解決方案。

為了解決這個問題，Karaman 等人引入了RRT 的最佳變體RRT*[11]。RRT*首先找到一條初始路徑，然后通過重選父節點，重布線優化路徑，這使得RRT*漸進最優，這意味著當樣本數無限大時，可以保證收斂到最優解。但是由于RRT*執行的是純隨機探索，它的收斂速度非常地慢。

為了克服RRT*算法收斂速度慢的問題，RRT*及其變體在最近幾年得到了廣泛的研究。文獻[12]提出了B-RRT*算法，通過兩棵樹交替擴展，提高了算法的收斂速度；文獻[13]提出的RP-RRT*算法，通過融入人工勢場法優化采樣過程，在勢力場的作用下執行目標偏差采樣，能夠在較短時間內生成較優路徑；文獻[14]提出包圍盒頂點引導的BNM 算法，通過障礙物包圍盒頂點引導路徑擴展，該方法能夠產生接近最優的無碰撞路徑；文獻[15]提出本地引導多個B-RRT*（LGMBRRT*）算法，該算法通過引入橋接測試和基于本地的新穎搜索策略解決了RRT*收斂速度低下的問題。

LGM-BRRT*算法雖然解決了RRT*算法效率低下的問題，但是在選擇本地橋接點時可能采用對目標方向無用的橋接點，缺乏導向型，耗費時間。因此，本文引入本地橋接點目標導向策略避免獲取無用的橋接點，并在擴展新節點時引入目標偏向策略，進一步提升收斂速度。在此基礎上，采用改進的去冗余處理策略和三次B 樣條采樣擬合路徑，以獲取更優的路徑。最后，在不同環境中與RRT*、B-RRT*和LGM-RRT*進行了對比。仿真實驗結果表明改進算法具有更好的收斂性、穩定性與有效性。

1 背景知識

定義1：根據文獻[15]給出的路徑規劃問題定義：X∈Rd表 示 地 圖 范 圍，Xobs∈X代 表 障 礙 物 區 間，Xfree?XXobs表示無障礙物區間。Xinit表示起始點，Xgoal表示目標點。路徑規劃問題就是在Xfree區間找到一條從起始點Xinit到目標點Xgoal的可通行路徑。

1.1 RRT＊算法介紹

RRT 算法首先初始化樹T，并將起始節點Xinit添加到該樹，在無障礙物空間中生成一個隨機節點Xrand，并在樹T 中找到最近的節點Xnearest。然后，沿Xnearest到Xrand的路線擴展一步得到新節點Xnew，如果T 中最近的節點Xnearest與新節點Xnew之間的路徑無障礙物，則將新節點Xnew插入到樹T 中，重復此過程n 次，直到找到完整的可行路徑為止，如果在n 次迭代后仍未生成可行路徑，則意味著規劃失敗。算法原理如圖1 所示。由于RRT 算法以隨機方式快速生成一條可行的路徑，無法獲得漸進最優的路徑，RRT*通過在添加新節點時重選父節點和重布線減少路徑成本，獲得盡可能最優的路徑。在添加新節點時，相較于RRT 選擇最近的鄰居作為父節點，RRT*選擇鄰域內代價最小的節點作為父節點。在這基礎上，如果用新節點替換父節點能夠獲得更小代價，并且它們之間的路徑沒有沖突，則重新布線。這些改進幫助RRT*找到接近最優的路徑，但代價是執行時間更長。

圖1 RRT算法原理

1.2 LGM-BRRT＊算法介紹

通過前述分析，RRT*最大的問題是時間消耗大，所以加快收斂速度是RRT*的主要改進方向。Tahir 等人[12]提出B-RRT*算法提高RRT*的收斂速度。BRRT*使用兩棵樹Ta和Tb交替擴展，所以在擴展過程中，需要檢查兩棵樹是否已經連接，若已連接，則記錄當前路徑，若無連接，則進行下一次迭代。如果迭代沒有結束，則將不斷優化現有路徑。然而B-RRT*在面對狹窄通道等復雜地圖環境時仍然采用簡單的隨機擴展，需要更多的迭代次數才能從狹窄通道中搜索出可行路徑。

Shu 等人[14]提出的LGM-RRT*算法，引入本地橋接點引導樹策略找出地圖中的狹窄通道，快速搜索出可行路徑，很好地適應了復雜地圖環境。LGM-RRT*需要提前對地圖進行預處理，找出地圖中的狹窄通道點。橋接法[16]是識別狹窄通道的有效方法之一，它在障礙物空間Xobs中隨機生成兩個端點qf和qs組成的一個線段，若線段中點qm位于自由空間Xfree中，這樣的線段稱為一個橋，如圖2 所示。顯然，在狹窄區域建造一座“橋梁”要比在寬闊自由空間建造一座“橋梁”要容易得多[17]。最后，通過聚類分析方法獲得每個狹窄通道的唯一識別點[17-18]。Zhong[19]提出正交測試法以避免在拐角附近建造“橋梁”，如圖3 所示。

圖2 橋接法原理（實線表示成功，虛線表示失敗）

圖3 拐角附近的“橋梁”

B-RRT*和LGM-BRRT*對地圖一的規劃結果如圖4-5 所示。從圖中可以看出LGM-BRRT*算法的路徑更短，但存在無效的本地點引導。原因在于LGMBRRT*只采用了一個歐氏距離范圍來限制選擇本地點引導，距離小于設定閾值時就觸發本地點引導，但是此時的引導可能已經完全偏離目標方向。

圖4 BRRT*地圖一規劃結果

圖5 LGM-BRRT*地圖一規劃結果

2 改進的LGM-BRRT＊算法

本節將介紹一種加入目標方向約束的本地點引導策略，消除LGM-BRRT*的冗余引導，以幫助LGMBRRT*快速規劃初始路徑。同時，采樣點加入目標偏向，以削弱采樣的隨機性，加速找到最佳路徑。最后，再做相應的路徑優化策略。

2.1 本地點目標約束

LGM-BRRT*本地橋接點引導觸發的條件是距離小于設定閾值Ltri。在此基礎上，改進算法加入角度閾值θtri約束方向，當新增節點和本地橋接點的距離小于Ltri并且和目標點構成的角度小于θtri時，觸發本地引導。加入角度閾值θtri約束方向后，可以避免冗余的本地點引導，能夠快速到達目標點，提高收斂速度。

如圖6 所示，在二維地圖環境中，假設目標為Pgoal(xg,yg) ，本地橋接點為Plocal(xl,yl) ，新擴展節點為Ptemp(xt,yt)，這三點構成的角度為θ。

θ的角度公式為：

圖6 約束角度示意圖

2.2 目標偏向采樣

LGM-RRT*算法擴展新節點時，是在自由空間中生成一個隨機點Prand(xr,yr)，然后在擴展樹中找到離隨機點最近的節點擴展一個步長。這種擴展方式隨機性太強，當地圖存在較小出口時，從出口向外擴展成功的概率相應減小，使得從出口向外擴展的時間更長。因此，改進算法加入目標方向上的引導能夠提升擴展效率。如圖7 所示，往隨機方向與目標方向的合成向量方向擴展一個步長。其中Pgoal(xg,yg) 為目標點，Pnearest(xn,yn)為擴展樹上最近的點，λ1為合成系數，新擴展點的坐標為Ptemp(xt,yt)。

圖7 目標偏向采樣示意圖

擴展新節點函數算法1 所示，generateNewNode 根據公式（2）、公式（3）、公式（4）生成Ptemp點，如果Pnearest到Ptemp點的路徑之間無障礙物，將Ptemp點加入樹T 中。

算法1：Expand（xnearest，xrand）

2.3 使用本地點約束和目標偏向采樣策略

設置本地點目標約束和目標偏向采樣策略后，改進算法的擴展策略如算法2 所示，稱起始點向目標點擴展的樹為Ta，稱目標點向Ta擴展的樹為Tb。

算法2：改進LGM-BRRT*

2.4 路徑優化處理

使用以下三種策略進行路徑優化處理。

（1）考慮路徑機器人寬度的膨脹檢測

碰撞檢測時，對機器人路徑進行膨脹處理。膨脹后的機器人路徑將向外擴張長度為λ2Rrt（Rrt為機器人半徑）。此時兩個路徑節點之間機器人路徑可看做是長條矩形，如圖8 所示。

圖8 碰撞檢測示意圖

（2）改進去冗余處理策略

從起始點后的第二個節點開始，依次向前遍歷，若當前節點與起始點不發生碰撞且該兩點與其中間點不在同一直線上，則將中間點刪除；否則以中間點為起始點重新向下遍歷。如圖9 所示，start 到step4 之間有三個路徑節點，去除step2 可以減小路徑長度。而使用傳統策略，去除的是step1 和step3，這樣只減少了路徑節點，而并未縮短路徑。

圖9 路徑平滑操作

（3）使用路徑控制點的曲線擬合處理

在曲線擬合之前對原路經進行插值處理，當路徑節點與左右兩點的路徑長度大于最小障礙物包圍盒寬度時，則在該路徑節點中間靠近路徑節點位置插入控制節點，插值之后在使用三次B 樣條曲線擬合路徑，以避免直接采用曲線擬合會導致擬合后路徑在轉彎處偏離原路經而產生碰撞。

3 實驗與分析

為了驗證改進算法的有效性，在虛擬機Ubuntu 14.04（內存2GB，硬盤25GB，處理器2 核）搭建機器人操作系統（Robots On System，ROS），在該環境下實現改進算法，并在ROS 可視化工具（3D visualization tool for ROS，RVIZ）中顯示完整的路徑規劃結果。用于安裝虛擬機的計算機配置為：（處理器：Intel Core i7-4710HQ 2.50 GHz CPU，內存：8GB，系統類型64 位操作系統）。

環境地圖設置成200×200，起點坐標（3，3），目標位置（197，197），擴展步長設置為3，當樹和目標點之間的距離小于步長時，則認為樹已擴展到目標。當兩棵樹之間的距離小于3 時，則認為兩棵樹已連接。設定距離閾值Ltri=25，角度閾值θtri=45，設置路徑寬度為機器人半徑的1.5 倍，即λ2=1.5。不同參數設置對RRT 算法的效率有不同的影響，但是為了便于比較，在模擬測試中設置了相同的值。

在地圖一中，存在很多障礙物，包括9 個狹窄出口，這使得擴展難度增加。圖10、圖11、圖12 和圖13分別顯示了在不同迭代次數下RRT*、B-RRT*、LGMBRRT*與改進LGM-BRRT*的在地圖一環境下的仿真實驗結果。在圖10 中，當迭代次數增加到1000 次時，RRT*才能規劃出一條路徑，這是四種算法所需的最大迭代次數。而改進LGM-B-RRT*算法僅迭代了150次就規劃出了路徑，此時RRT*還未找到第二個狹窄出口，B-RRT*兩棵樹尚未相交。對比LGM-BRRT*，改進的LGM-BRRT*擴展樹枝更加收攏，具有目標性，無效擴展少。從圖10、圖11、圖12 和圖13 的處理過程可以看出，改進的LGM-BRRT*比RRT*、B-RRT*和LGM-BRRT*更早進入收斂狀態。為進一步觀察改進LGM-BRRT*算法的穩定性，在地圖二中進行了實驗，圖14 展示了在地圖二下部分迭代次數的仿真結果。可以看出，在不同的地圖環境下，該算法依然能在較少迭代次數下獲得很好的仿真結果，進一步證明了本文所提算法的有效性。

圖10 RRT*的地圖一仿真結果（從左到右依次是迭代次數150、300、500、1000、1500）

圖11 B-RRT*的地圖一仿真結果（從左到右依次是迭代次數150、300、500、1000、1500）

圖12 LGM-BRRT*的地圖一仿真結果（從左到右依次是迭代次數150、300、500、1000、1500）

圖13 改進LGM-BRRT*的地圖一仿真結果（從左到右依次是迭代次數150、300、500、1000、1500）

圖14 改進LGM-BRRT*在地圖二下的仿真結果（從左到右依次是迭代次數150、300、500、1500、2500）

為了觀察每種算法在不同迭代次數中的規劃能力，對每種算法進行迭代次數為150、300、500、1000、1500、2000、2500、3000 的實驗，統計十次規劃成功的次數。圖15 顯示了隨著迭代次數的增加，四種算法在不同環境中的成功次數的統計。從圖15 可以看出，改進LGM-BRRT*最先進入收斂狀態。除此之外，路徑成本也是度量規劃算法的重要依據，為了避免單項測試的偶然性，在每個迭代次數下進行十次實驗，并將十次實驗的平均路徑成本作為最終路徑成本值。圖16 繪制了在不同迭代次數下的路徑成本統計圖。從圖16 可以看出，RRT*、Bi-RRT*、LGM-BRRT*和改進LGMBRRT*算法的路徑成本隨著迭代次數的增加而降低。當迭代次數相同時，改進LGM-BRRT*路徑成本比其他三種算法要小。

綜合以上實驗與分析可以證明，改進LGM-BRRT*算法運行結果收斂更快，路徑成本更低，適合狹窄通道地圖路徑規劃。

圖15 四種算法在不同迭代下成功次數統計

圖16 四種算法在不同迭代下路徑成本統計

4 結語

為了解決LGM-BRRT*算法存在選擇本地橋接點時缺乏導向性導致算法效率低的問題，提出了改進LGM-BRRT*算法，通過引入本地橋接點目標導向策略，并在擴展新節點時引入目標偏向提高收斂速度。在ROS 仿真驗證了改進策略的有效性。但是，算法需要對地圖做一定的預處理找出本地引導點，而在實際環境中地圖是未知的，需要融合實時避障策略進行處理。