改進RRT算法的室內移動機器人路徑規劃

劉紫燕，張 杰

貴州大學 大數據與信息工程學院，貴陽550025

1 引言

隨著科學技術的不斷發展，導航技術已成為機器人領域的一個研究熱點，而路徑規劃算法是實現移動機器人導航的重要保障[1]。路徑規劃是指機器人按照一定的準則，在環境中搜索一條從起始點到目標點、可以避開障礙物的最優路徑[2]。傳統的路徑規劃算法有人工勢場法、模擬退火算法、模糊控制法、神經網絡算法等[3]，但這些算法的計算復雜度較大，在真實場景下的應用存在缺陷。遺傳算法、蟻群算法、粒子群算法等智能仿生學算法需要大量的迭代來求解最優路徑，因此收斂速度慢，而且極易陷入局部最優值[4-5]。

快速擴展隨機樹（Rapidly-exploring Random Tree，RRT）由于具有快速高效的優點，近幾年在移動機器人路徑規劃中被廣泛采用[6]。RRT 算法通過隨機采樣，把搜索導向空白區域，從而搜索到一條起點到終點的路徑，適合解決復雜環境下多自由度機器人的路徑規劃問題[7]。但是由于傳統RRT 算法擴展點具有隨機性，使得算法搜索過于平均、規劃結果偏離最優解[8]。針對以上缺陷，文獻[9]提出的RRT2connect算法同時從起點和終點生長兩棵隨機樹，在搜索效率方面有了顯著提高，但在組態空間存在窄道時其性能會顯著下降。Karaman和Frazzoli[10]提出了RRT*算法，用以改進由RRT 算法產生非最優解的問題，但存在收斂速度慢、結果不穩定的缺點。同時莫棟成等人[11]提出了任意時間RRT算法，通過減少搜索節點數來提高搜索效率，該算法的缺點是啟發函數沒有達到最優。劉成菊等人[12]提出了動態RRT算法，并將其應用于NAO 機器人，該算法能適應復雜障礙物環境，但是生成的路徑并非理想平滑曲線。Wang[13]提出基于學習的LM-RRT（Learning-based Multi-RRT）算法，用于窄通道中的機器人路徑規劃，提高了局部探索能力。Du等人[14]將RRT 算法和人工勢場法相結合，提出一種啟發式采樣方法，具有更高的采樣效率和收斂速度。Chang 等人[15]提出了SARRT（Struc‐ture-Aware Rapidly-exploring Random Tree）算法，通過構建溫度場使隨機樹生長偏向于目標區域，取得良好的效果，然而求解擴散方程計算成本昂貴，耗費了大量時間。

為了解決上述問題，本文結合目標偏向策略和氣味擴散法對RRT算法加以改進，并對生成的路徑進行優化處理，從而提高算法效率和路徑質量。通過基于ROS的仿真實驗和實驗室環境下的Turtlebot2 實驗，證實了改進RRT算法的優越性、實用性和可靠性。

2 機器人運動模型

機器人運動模型如圖1 所示，可將運動視為通過旋轉平移兩種方式完成從一點到另一點的操作。假設機器人沿曲線運動，t 時刻機器人的線速度為vt，角速度為ωt。在(t-1,t]時刻內（時間間隔為Δt），機器人以r=v/ω為半徑的圓形軌跡上移動，其瞬時速度為：

圖1 機器人運動模型

令t-1 時刻機器人的位姿Xt-1=(xt-1,yt-1,θt-1)T，Δt 時間內以速度常量(v,ω)T進行運動，則t 時刻機器人的位姿為：

機器人所有時刻的位姿構成了機器人實際行走的路徑。

3 基本RRT算法

RRT 算法由Lavalle 于1998 年提出，它是一種多維空間的高效規劃方法[16]。RRT 算法復雜度小，可直接應用于非完整約束系統的規劃。設機器人的位姿空間為C，初始節點為Qstart，目標節點為Qgoal，機器人最小步長為s，初始隨機樹為T。算法以Qstart為根節點，在位姿空間C 中隨機產生采樣點Qrand，然后遍歷T 中的所有節點，尋找離Qrand最近的節點Qnear作為擴展節點。此時判斷Qnear與Qrand的連接線是否與障礙物相交，若相交則在位姿空間C 中重新生成隨機采樣點Qrand；否則，機器人由Qnear向Qrand運動最小步長s到達新節點Qnew，同時將新節點Qnew添加到擴展樹T 中。重復上述步驟，當機器人到達目標節點Qgoal或超過最大迭代次數時算法結束。此時，由目標節點Qgoal回溯到初始節點Qstart，即可得到路徑。RRT算法偽代碼如下[17]：

4 改進RRT算法

4.1 目標偏向策略

由基本RRT算法原理可知，其產生新節點的方式是在機器人狀態空間中隨機采樣，導致搜索過于平均，浪費了大量計算時間和資源，算法效率低。而且算法輸出的路徑質量不高，偏離最優解。為了克服上述缺陷，高效地規劃出移動機器人在復雜環境下的最優路徑，本文在基本RRT 算法的框架中引入一種目標偏向策略。即提前設定一個偏向概率閾值pbias，在隨機選取擴展節點時利用均勻概率分布產生一個隨機概率p，若p ＞pbias，則Qrand在機器人狀態空間中隨機產生，否則Qrand=Qgoal。通過這種方式既保留了原算法的隨機特性，又使得算法收斂速度加快，從而提升搜索效率。

在基本RRT算法中，規劃問題的起始狀態點是樹的根節點，但目標狀態點在搜索過程中并沒有被考慮在內，只是被動地等待隨機樹擴展到目標區域。目標偏向策略將目標點的作用考慮在內，使得隨機樹以一定概率朝向目標點生長，改變了節點擴展方式，避免了原算法的無方向生長。當隨機概率p ＜pbias時，探索樹朝目標點生長；當p ＞pbias時，探索樹在狀態空間中隨機選擇采樣點進行生長。探索樹的目標偏向性和隨機延展性受隨機概率p 的影響，p 越大隨機樹朝目標點生長的趨勢越強，隨機性越弱。反之，p 越小隨機樹在整個狀態空間中選擇擴展點的機會越大，隨機樹的發散性也就越強，適用于復雜環境下的路徑規劃[18]。不論隨機樹是否朝目標點生長，當路徑與障礙物發生碰撞時都會放棄當前節點重新產生采樣點，這種節點擴展方式使得隨機樹在避開障礙物的同時朝向目標點生長，保證了生成路徑的準確性。目標偏向RRT算法仍具有概率完備性，在理論上總能在地圖中搜索到一條可行路徑，且規劃路徑優于RRT 算法生成路徑。RRT 算法與目標偏向RRT 路徑生成對比如圖2 所示，紅線表示規劃路徑，藍點表示采樣節點。由圖2 可知，基于目標偏向策略的RRT 算法采樣點減少，可大大縮短路徑長度和規劃時間，生成路徑也較為平整光滑，實驗結果證明了目標偏向策略的有效性和準確性。

在目標偏向RRT中，概率閾值pbias的選取會影響算法規劃性能。通過實驗分析對概率閾值pbias的取值進行研究。環境地圖如圖2（a）所示，起點位置坐標為S(1,1)，終點位置坐標為G(90,90)。選取不同的概率閾值，在每一個概率閾值pbias下分別進行10 次路徑規劃。將pbias=0.05 作為第一組實驗數據，pbias=0.1 作為第二組實驗數據，之后以0.1為間隔進行實驗。以路徑長度、搜索時間作為評價指標，記錄10 次實驗結果的平均值。仿真實驗結果如圖3所示。

圖2 RRT與目標偏向RRT對比

從圖3（a）可知，隨著概率閾值pbias逐漸增大，平均路徑長度整體上呈下降趨勢。表明pbias越大，隨機樹向終點生長的趨勢愈加明顯，使得路徑更為直接，減少了無效搜索，縮短了路徑長度。從圖3（b）可知，路徑平均搜索時間整體上呈現出先降低再上升的趨勢，pbias取0.2用時最短。表明pbias較小時，RRT 算法偏向終點擴展的趨勢小，隨機樹會向其他空白區域作無效搜索，導致搜索時間變長。而當pbias較大時，隨機樹會以較大概率朝終點生長，使得擴展節點選擇單一。所以算法會花費較多時間避開障礙物，增加了路徑搜索時間。

圖3 概率閾值pbias 對算法性能指標影響

由于本文采用目標偏向策略主要目的是減少搜索時間，因而在后續實驗中選取概率閾值pbias=0.2。

4.2 基于氣味擴散法的路徑搜索

氣味擴散法靈感來源于機器人主動嗅覺目標搜索[19]，也稱為氣味源搜索。機器人利用傳感器檢測環境中的氣味濃度，朝濃度增大方向行進并搜索出氣味源的位置。假設機器人的終點為氣味源，機器人所處環境為二維地圖，且地圖中的障礙物區域阻斷了氣味擴散，即氣味不能以障礙物為介質傳播。在地圖中，氣味源會呈輻射狀地向周圍擴散，離終點越遠氣味濃度越低，反之，氣味濃度越高。由于障礙物阻斷了擴散，氣味只能在地圖中的自由區域內完成擴散，而自由區域內與終點距離相同位置上的氣味濃度應大致相同。由此可在二維地圖上構建氣味素濃度場，終點氣味濃度最高且氣味濃度只與終點位置和環境狀況有關。無論機器人起點處于地圖中的什么位置，只要沿著濃度增大的方向移動一定能到達終點，從而完成路徑規劃任務。

先將二維地圖柵格化，排除障礙柵格，自由空間內每一個柵格可作為一個氣味擴散單元。氣味擴散采用四鄰域模型，如圖4 所示。在四鄰域模型中，氣味從當前柵格朝上下左右四個方向作擴散[20]，直至氣味素擴散到自由空間中每一個柵格。將路徑規劃的終點作為氣味源，即終點位置所處柵格為最高濃度，按照四鄰域模型逐漸向外擴散，每擴散一次，氣味濃度值減小一個等級，然后記錄每個柵格的氣味素濃度，當擴散完成后地圖中的每個柵格都會分配到相應氣味濃度值。氣味擴散法可用下面的表達式表示：

式（3）中，i 為擴散批次，{O(i)}表示第i 批擴散柵格的氣味濃度集合，{O(i+1)}為i+1批擴散柵格的氣味濃度集合。O(1)表示終點所在柵格的氣味濃度，將其值設為M。因為擴散中同一批次柵格的氣味濃度相等，所以將它們稱之為“等濃度集合”，氣味的擴散是按照等濃度集合進行的[21]，每擴散一次，氣味濃度值減1，如果此柵格已經標記有氣味濃度值則放棄擴散。當每個自由柵格都標記好氣味濃度值時擴散完成。

圖4 機器人運動方向

由氣味擴散法的原理可知，終點氣味濃度最高，其值為M，所有柵格的濃度都由終點擴散而來。柵格氣味濃度大小只與終點位置和環境狀況有關，而與起始位置無關。每一個柵格都只能向上下左右四個柵格做擴散，而且任一柵格的氣味濃度值都從比它濃度大1 的柵格擴散而來。所以任取一個濃度為X(X ＜M)的柵格，周圍一定至少有一個柵格的濃度為X+1，依此類推：氣味濃度為X+1的柵格周圍一定至少有一個濃度為X+2的柵格，追溯到最后一定能找出濃度為M 的柵格，即是到達終點。將每個柵格的氣味濃度值作為此柵格的代價，由此把二維地圖轉化為基于濃度的代價地圖。因此，從起始柵格出發，每次向比當前柵格代價高的柵格移動，必然能找到一條通往終點位置的可行路徑。這是將氣味擴散法應用到RRT 算法中的重要基礎。由氣味擴散法生成的代價地圖如圖5 所示。機器人所處環境為8×10的柵格圖，圖中橫向為x軸，縱向為y 軸，終點坐標為（1，1），起點坐標為（8，10），黑色區域為障礙物，白色區域為自由空間。設終點濃度值為30，可從圖中看出經過氣味擴散法處理后自由空間中每個柵格對應的代價值。機器人的一個可行路徑為（8，10）—（8，9）—（7，9）—（7，8）—（6，8）—（6，7）—（6，6）—（5，6）—（5，5）—（4，5）—（3，5）—（2，5）—（1，5）—（1，4）—（1，3）—（1，2）—（1，1）。

圖5 氣味擴散法示意圖

本文提出的改進RRT 算法在擴展節點過程中引入氣味擴散法，可準確有效地指導隨機樹的生長：在RRT算法生成新節點時根據柵格代價來確定接受或是舍棄，使得隨機樹朝柵格代價增大的方向生長，從而到達終點，克服了原算法偏離最優解的缺陷。由于氣味擴散法生成的代價地圖中每個柵格周圍都存在比當前柵格代價高的柵格，使得隨機樹趨向于目標點生長，提高了路徑質量。改進算法不但采用氣味擴散法改善擴展節點的選取，而且保留了原算法的隨機特性，從而避免了陷入局部最優解。

4.3 改進RRT算法流程

改進RRT 算法采用目標偏向策略和氣味擴散法來改善擴展節點的選取，加快了算法收斂速度，從而解決了隨機點分布過于平均的缺點。算法流程如下：

步驟1 環境建模：對地圖進行柵格化，利用氣味擴散法得到與柵格地圖相對應的代價地圖。

步驟2 初始化探索樹T。

步驟3 生成隨機點Qrand，設定一個偏向概率閾值pbias，在隨機選取擴展節點時利用均勻概率分布產生一個隨機概率p，若p ＞pbias，則Qrand在機器人狀態空間中隨機產生，否則Qrand=Qgoal。

步驟4 選擇樹的生長點：

步驟5 求探索樹的新節點Qnew：

步驟6 判斷Qnew所在柵格代價是否比Qnear所在柵格代價高，若是則轉步驟7，否則返回步驟3。

步驟7 更新探索樹T，判定Qnew是否為探索失敗節點，若不是，則Qnew加入到T 中。否則，放棄Qnew，返回步驟3。

步驟8 判斷是否到達目標點。若‖ Qgoal-Qnew‖＜s，則到達目標，否則，返回步驟3，其中s 為機器人最短行走距離。

步驟9 從目標節點逆向回溯至探索樹的根節點，并返回規劃路徑。

4.4 基于三次B樣條曲線的路徑平滑

通過改進RRT算法輸出的路徑由折線段構成，且路徑長度不是最短，需要作進一步平滑處理。常用的路徑平滑方法有多項式插值、曲線擬合、B樣條插值等。高次插值易在區間兩端產生震蕩，造成較大的逼近誤差，稱為龍格現象。為了使生成路徑逼近原路徑，平滑過程中應避免龍格現象。采用龍格函數測試各種路徑平滑方法的逼近性能，如圖6所示。從圖中可看出，三次曲線擬合的效果最差，所得圖形與原曲線相差甚遠；多項式插值在中間部分插值效果較好，對于兩端的插值結果非常不理想；而B 樣條曲線給出了光滑的插值曲線，逼近效果理想，穩定性好，收斂性強，能很好地回避龍格現象。

由于B 樣條曲線能避免龍格現象，具有連續性和局部性[2]等優點，且能生成曲率連續的平滑路徑，本文采用B 樣條曲線對路徑進行平滑處理。由于B 樣條曲線局部平滑性好，不能在縮短路徑方面取得理想效果，所以在采用B 樣條曲線對路徑平滑處理前先對路徑作預處理。預處理的目的是縮短路徑，以便后續平滑處理。

假設利用改進RRT 算法進行路徑規劃的輸出結果由點序列S={( x1,y1),(x2,y2),… ,(xn,yn)}構成，對點序列S進行預處理，預處理后路徑規劃的點序列為要使路徑長度縮短，必須滿足以下兩個條件：

圖6 三種方法逼近性能比較

條件（1）的作用是使得平滑后的所有路徑點與對應原始路徑點的距離和最小；條件（2）保證了平滑后的點序列中前一個路徑點與后一個路徑點的距離和最小。所以路徑最短的目標函數為：

式（6）中α、β 為權重因子，且滿足α+β=1。路徑平滑的實質即是求上式的最優解，求解最優解的方法采用梯度下降法（gradient descent），即通過多次迭代，調整P(i)使M 取得最小值，使得路徑變短。通過式（6）得到的路徑P 仍不夠平滑，需對P 作進一步處理。

為了使路徑變為理想的平滑曲線，采用三次B 樣條曲線對P 作進一步優化處理。三次B 樣條曲線和控制點間的關系如公式（7）所示：

式（7）中t ∈[0,1]，Pi、Pi+1、Pi+2、Pi+3為控制點。最后依次連接全部三階B 樣條曲線段即可生成最終路徑。

5 實驗結果及分析

分別在仿真環境和真實環境下進行實驗。將本文改進RRT 算法與基本RRT、SARRT 等算法進行對比，驗證提出算法的優越性和正確性。仿真實驗硬件平臺為筆記本電腦，真實環境實驗平臺為配備筆記本電腦的Turtlebot2。計算機配置為Ubuntu 操作系統，處理器為i3-3110M，主頻2.4 GHz，運行內存為4 GB。

5.1 仿真實驗

在ROS 的Stage 仿真平臺中選取三種不同場景進行實驗，圖7（a）～（c）分別為地圖1、地圖2、地圖3。三種地圖均為10 m×10 m 的迷宮地圖，地圖大小為200×200像素。

圖7 三種地圖

為了方便處理、提高運行效率，把地圖劃分為40×40的柵格。改進RRT與SARRT[13]算法的相同點是都會在隨機樹生長前對地圖進行預處理，利用擴散方法構造代價地圖，把柵格代價大小作為產生新節點的依據。以地圖2 為例，對兩種算法的性能進行比較。圖8（a）為改進RRT 算法得到的代價地圖，圖8（b）為SARRT 算法得到的代價地圖。在圖8 中，目標點用三角形標記，起點用圓形標記；紅色區域表示高代價柵格，藍色區域表示低代價柵格。觀察發現，圖8（b）左中右三個區域每個區域的顏色大致一樣，表明SARRT 的擴散結果過于平均，因而局部指導能力較差。與圖8（b）相比，圖8（a）顏色層次分明，更能準確反映地圖中自由空間的結構。改進RRT 的擴散過程采用氣味擴散法，該方法局部性能優越，在復雜環境下的指向性更好，能對隨機樹的生長作精確指導。SARRT 的擴散過程采用菲克第二定律，計算復雜，且需要多次迭代才能在環境中完成擴散，因此耗費更多時間。由于氣味擴散法計算簡單，使得改進算法在效率和速度上得到極大的提升。氣味擴散完成后開始執行路徑規劃任務，分別在地圖1、地圖2、地圖3下進行實驗。設定搜索步長s=0.30 m，偏向目標概率pbias=0.2，路徑最短目標函數式（6）中的權重因子α=0.5，β=0.5。

圖8 改進RRT與SARRT代價地圖對比

地圖1 的環境比較簡單，設定機器人起點坐標為（2，2），終點坐標為（2，9）。圖9（a）～（c）分別為基本RRT、SARRT 和本文提出的改進RRT 算法在地圖1 中的輸出結果。

圖9 地圖1中三種算法路徑規劃結果比較

地圖2的場景比地圖1復雜，設定機器人起點為（2，2），終點為（9，2）。圖10（a）～（c）分別為基本RRT、SARRT和本文提出的改進RRT算法在地圖2中的輸出結果。

圖10 地圖2中三種算法路徑規劃結果比較

地圖3 的場景最為復雜，設定機器人起點坐標為（1，1），終點坐標為（9，2）。在地圖3 中，機器人可以選擇不同的通道到達終點，路徑長度與機器人選擇的通道密切相關。圖11（a）～（c）分別為基本RRT、SARRT 和本文提出的改進RRT算法在地圖3中的輸出結果。

圖11 地圖3中三種算法路徑規劃結果比較

由圖9 和圖10 知，地圖1 和地圖2 的共同特點是機器人只有一個通道到達終點，在此種情況下改進RRT算法表現最優。由圖11 知，基本RRT 的路徑沒有方向性，機器人可能會選擇距離最長的通道，導致路徑長度增大。而SARRT 和改進RRT 的路徑具有方向性，機器人會選擇距離最短的通道到達終點，因而路徑長度縮短，路徑質量更高。比較圖11（b）和（c）可知，與SARRT 相比，改進RRT的路徑長度更短，路徑平滑度得到提升，所以改進RRT算法的性能優于SARRT。

由于RRT算法具有隨機性，為了客觀評價算法的優劣，在三種地圖場景中分別對三種算法進行20 次路徑規劃。設定三種算法的搜索步長s=0.30 m，改進RRT 算法的偏向目標概率pbias=0.2，路徑最短目標函數式（6）中的權重因子α=0.5，β=0.5。將路徑長度、搜索時間和采樣節點數作為三種算法的評價指標，記錄20 次實驗的平均值。地圖1、地圖2、地圖3 仿真實驗數據如表1、表2、表3所示。

表1 地圖1仿真實驗數據對比

表2 地圖2仿真實驗數據對比

表3 地圖3仿真實驗數據對比

通過圖9～11和表1～3中的仿真實驗數據可知，與基本RRT 和SARRT 相比，改進RRT 算法的搜索時間更短，在效率和速度上得到極大提升，而且路徑長度縮短、路徑更為平滑，使得路徑質量得到顯著改善，證明改進RRT算法在復雜環境下進行路徑規劃具有優良的性能。

5.2 Turtlebot2實驗

圖12 改進RRT算法實驗場景

為了測試實際場景下改進RRT算法的性能，將算法應用到基于ROS的移動機器人Turtlebot2上。本文的實驗場景為實驗室，如圖12（a）所示。圖12（b）為實驗室的整體環境圖。首先用無線控制器控制Turtlebot2掃描室內環境，利用gmapping 算法構建2 維地圖，地圖尺寸為576×512 像素。然后在建好的地圖上進行路徑規劃實驗，并將實驗結果與基本RRT和SARRT進行對比。

在ROS 的Rviz 可視化工具中顯示二維地圖和路徑規劃結果，基本RRT 算法的輸出結果如圖13（a）所示，SARRT算法的輸出結果如圖13（b）所示，改進RRT算法的輸出結果如圖13（c）所示。圖中的黑色區域表示障礙物，白色區域為機器人運動的自由空間。

圖13 實驗室環境下三種算法輸出結果

表4 列出了三種算法在相同起始點下的路徑長度、搜索時間和采樣節點數。由實驗數據可知，改進RRT算法優于SARRT 和基本RRT。與基本RRT 相比，改進RRT的路徑長度大大縮減；與SARRT相比，改進RRT的搜索速度更快。考慮到算法各方面的性能，認為改進RRT算法能在真實環境下生成最優路徑。

表4 實驗室環境下三種算法實驗數據對比

實驗結果證明，結合了目標偏向策略和氣味擴散法的改進RRT 算法能高效完成復雜環境下的移動機器人路徑規劃。相比于基本RRT 和SARRT 算法，改進RRT算法優化了節點擴展方式，因此搜索速度更快，路徑質量更優。

6 結束語

基本RRT 算法不能滿足真實環境下移動機器人路徑規劃的實際需求。為了提高路徑規劃的效率和質量，對RRT算法進行改進，在原算法的基礎上引入目標偏向策略和氣味擴散法，利用基于濃度場的成本地圖精確指導隨機樹的生長，從而選擇最優采樣節點，提高搜索效率。為了使生成路徑是理想平滑曲線，采用基于三次B樣條曲線的路徑平滑方法對生成的路徑進行優化，進一步改善了路徑質量。不同環境下的仿真實驗表明，改進RRT算法能顯著提高搜索效率和避障能力，而且路徑長度更短、路徑質量更優，尤其在復雜環境下表現出優良的性能。將改進RRT 算法應用到Turtlebot2 上并在真實環境下進行實驗，結果表明改進RRT 算法可滿足實際需求，能高效解決移動機器人在復雜環境下的路徑規劃問題。