一種改進的RRT路徑規劃算法*

王稷堯，袁鋒偉

（南華大學機械工程學院，衡陽 421001）

0 引言

隨著智能機器人不斷發展，路徑規劃問題研究越來越深入。路徑規劃指機器人在當前環境中按照一定的標準，搜索出一條從起始狀態點到目標狀態點，能夠繞開障礙物的最優或次優路徑[1]。根據其原理不同，路徑規劃算法有人工勢場法[2]、神經網絡法[3]、遺傳算法[4]等。其中，以RRT算法為代表的采樣規劃算法，通過一個初始點作為根節點，采用隨機采樣的方式增加樹結構的分支，當分支節點包含目標節點或進入目標區域，即可快速生成一條由樹節點生成的從初始點到目標點的路徑，具有概率完備性，在多障礙物的復雜場景應用較多[5]。

北京工業大學的阮曉鋼等[6]，提出了一種基于信息增益與RRT思想相結合的機器人環境探索策略，增強機器人對未知環境的探索降，低傳統RRT算法固有的盲目性。昆明理工大學的胡兵等[1]，在可變步長的隨機樹生長過程中，引入雙向生長策略，利用雙向生長特性，提高路徑搜索效率，解決了最優路徑與低效率間的矛盾。林依凡等[7]提出了一種無碰撞檢測RRT*運動規劃方法，增強了在復雜環境中的路徑規劃能力。四川大學的仲健寧等[8]提出一種基于多種啟發式策略和強化節點機制改進的高效RRT*路徑規劃算法，該算法提出強化節點機制，擴大節點蘊涵的信息，提高節點利用率。廣東工業大學的何兆楚等[9]利用人工勢場法進行局部路徑規劃，當陷入局部極小值時，使用改進的快速擴展隨機算法自適應地選擇臨時目標點，使搜索過程跳出局部極小值點；當機械臂逃離局部極小值點時，切換回人工勢場法進行規劃。

基于以上的研究分析，根據RRT算法隨機性、無序性的特點，對RRT算法進行選點方式、算法優化、碰撞檢測等方面的改進，提出一種新的路徑規劃算法。本研究以傳統RRT算法與勢場法相結合，對RRT算法加入勢場法、新節點生成策略、狹窄空間算法等，對RRT算法進行進一步優化。由于勢場的影響，檢測到的障礙物會產生斥力場，因此節點生成時函數值過大，會直接將其剔除，因此改進后的RRT算法也不再需要重復進行碰撞檢測，縮短了整體的運算時間。

1 問題分析

1.1 RRT算法原理

經典RRT算法是從狀態空間一初始點出發，通過隨機采樣擴展增加新節點，生成一個隨機擴展樹，當隨機樹中的新節點包含目標點或進入目標區域時，初始點到目標點間至少形成一條以隨機樹新節點組成的路徑[10]。

以二維空間中的RRT算法為例，假設在二維狀態空間C中，引入初始點xinit和目標點xgoal，使得xinit根據快速拓展隨機樹T進行隨機樹的逐步擴建，最終到達xgoal，其過程如圖1所示。

圖1 節點生成策略

狀態空間C中，障礙物所在的區域為障礙區Xob(XobX)，其他可通行的區域為通行區Xpa(XpaX)。在工作過程中，RRT算法會從初始點xinit開始逐步通過隨機采樣擴展結點，首先它會選定任意一個父節點，之后根據一個隨機的方向和規定的步長生成一個最近的臨時節點xrand，根據xrand所在位置判定其在Xob(XobX)還是Xpa(XpaX)。若其在障礙區，則該xrand無效：若其在通行區，則可生成一新的父節點xnew。這一過程會一直重復，直到xnew進入目標點xgoal的鄰域范圍Xgoal(XgoalX)，表明已成功找到目標點xgoal，進行逆追隨找到連接xinit與xgoal的最短路徑，完成路徑規劃。其偽代碼如表1所示。

表1 RRT algorithm

隨機樹T生長過程中，函數RANDOM_STATE（）會從空間中選取一點；根據函數NEAREST_COMPARE（），會在空間中拓展出一點；經過函數STEER（），SELECT_INPUT（），NEW_STATE（）判定后得到隨機樹上的新節點；之后collisionchecking（）函數會對其進行碰撞檢測，若滿足條件則生成新節點；最后進行鄰域檢測findnear_neighbor（），若滿足則路徑規劃成功；經過路徑比較函數MINPATH（）輸出最優路線。

1.2 實際問題分析

經過實驗測試，傳統的RRT算法依然存在著很多問題。

（1）規劃路徑成功率較低，在測試實驗中，設定最大節點生成個數為1 000個，在圖2實驗中，成功率為70%；在圖3實驗中，成功率僅僅為50%。若想增高成功率，可以設定更多的節點數和減小節點步長，但無疑會加大工作量，延長整體的工作時間。

圖2 路徑規劃1

圖3 路徑規劃2

（2）所規劃路徑質量較低，在兩種地圖中，RRT算法雖然都成功完成了路徑規劃任務，但所得到的路徑是非常不理想的，與最優路徑之間還存在著一定的距離，同時所規劃路徑存在過大的轉角，在實際工作過程中，會產生一定程度的震蕩，降低整個工作過程的穩定性。

（3）在狹窄空間中，RRT算法容易陷入無解情況，如圖3所示，由于算法生成策略的隨機性，在狹窄的通路中，當父節點離障礙物較近時，新生成的節點很大概率落入障礙區中，導致選點失敗陷入死循環。

針對于以上的問題，本文提出了一種RRT結合人工勢場軌跡規劃的算法。改進RANDOM_STATE（）選點函數，加入引力場和斥力場綜合作用影響和狹窄概率算法，使得生成受到勢場和概率算法的綜合影響。由于勢場的加入，障礙物生成的斥力場會影響到隨機點生成，使得隨機點不會生成在障礙區中，因此改進后的算法不再需要進行碰撞檢測函數collisionchecking（），提高了工作效率。引力場與斥力場的相互作用，使得整個空間中的勢場連續性變化，加之勢場法中對于機器人轉角因素的考慮，減小了路徑規劃中的過大轉角，增強了工作過程的平穩性。在狹窄空間中，狹窄概率算法的加入使得路徑搜索的成功率增大，在面臨復雜地形時整個算法的表現更加出色。

2 算法的改進

2.1 勢場法的引入

針對傳統RRT算法隨機性過大、規劃路徑質量較低的問題，本文提出一種基于傳統RANDOM_STATE（）選點函數的增益求解函數。

在傳統的勢場法中，針對于機器人中的路徑規劃問題，經常使用如下的引力函數和斥力函數：

式中：Ua(Pn)為機器人在位置Pn時所受到的引力；Ka為引力系數；Pg為目標點所在位置。

式中：Ur為斥力函數；Kr為斥力系數；ln為機器人各個連桿與障礙物之間的距離；l0為整個斥力場的作用范圍。

根據機器人各個連桿與障礙物之間的距離，就可以很輕松的知道機器人與障礙物是否發生了碰撞，因此在優化選點函數時，可以將兩者的距離差作為條件編入函數，以此省略了之后的collisionchecking（）碰撞檢測函數。

對于任意時刻，機器人受到的總勢能為引力勢能與斥力勢能的總和，即：

2.2 選點函數的改進

在RRT算法中，傳統的選點函數如圖4所示，以xnew父節點為中心，初始設定的步長為半徑，得到子節點的出生圓。之后根據隨機方向rand，得到一個新的子節點xrand，再進行碰撞檢測，若滿足工作要求則子節點生成成功。

圖4 傳統原理

從其工作原理中可看出，方向的確定是完全隨機的，而且每次生成新的子節點之后都要進行碰撞檢測，雖然拓展性較好，但針對于機器人的復雜工作環境來看，其工作效率很低，選點方式也并不科學。

結合勢場法的思想，如圖5所示，根據實際工作環境設定工作區域，將其分為若干個區域，計算每個區域中的綜合增益，根據增益得到每個區域方向的生成概率，完成最終的子節點生成。綜合增益可表示為：

圖5 改進原理

式中：k0為初始增益系數；X0為初始增益，其計算已在經典算法中得出；ku為勢場增益系數；Xui為勢場增益。k0、ku可以根據實際的工作過程進行取值。Xui可表示為：

式中：k?為條件系數，若步長圓內存在障礙物，則取值為0，否則為1；Di為各個區域范圍；U(θ,r)為總勢能函數，其中θ、r可根據實際工作過程進行取值。

根據每部分的增益結果，可得到各區域隨機方向生成的概率為：

改進后的選點函數由于勢場的引導作用，省去了很多無效點的生成和碰撞檢測，使得路徑規劃效率更高，節約了時間。新生成父、子節點之間的轉角不會過大，使得整個路徑更光滑，也更接近理想路徑。

2.3 加入狹窄概率函數

針對于RRT算法在狹窄環境中易陷入無解的情況，有針對性的加入一種狹窄概率函數，其偽代碼如表2所示。

表2 狹義算法

隨機選點函數COLLISIONCHECKING_NEW在工作過程中，狹窄概率函數會同時使用監測函數MONITOR監測新生成的xnew節點，當xnew與D中已經生成的節點之間的距離和位置條件，滿足judge函數時，則會將工作區域個數進行更細劃分，以此找到可能存在的狹窄通路。

RRT算法方向選擇的隨機性和勢場法容易陷入局部死循環的特性，都決定了其對于狹窄路徑規劃存在著弊端。狹窄概率算法很好地解決了狹窄通路難以尋找的問題，使得算法的整體魯棒性增強，優化了路徑規劃過程。

3 實驗驗證

3.1 實驗一

將RRT算法與RRT結合人工勢場軌跡規劃的算法進行仿真實驗，驗證算法的實際工作情況。如圖6~7所示，本次實驗采用Matlab R2016b數學仿真軟件進行，設置多組地圖進行測試。實驗一的部分參數如表3所示。

表3 實驗一參數

圖6 傳統算法

圖7 改進算法

實驗過程中，黑色圖形為隨機設置的障礙物，紅色圓點為初始點xinit，綠色圓點為目標點xgoal，隨機樹生成的節點為藍色圓點，每步之間的距離用藍色線段相連，其中找到的最優路徑用紅色路線標出。

實驗數據對比如表4所示。實驗中的部分數據如表中所示，通過實驗數據可明顯看出，改進后的算法成功率有所提高，在200次實驗中僅有6次沒有成功找到目標點，成功率提高了13%。同時，所產生的節點個數大大減少，改進后算法生成的節點數減少了大約86.22%，因此整體工作效率也得到了很大提高，平均工作時間減少了大約67.6%，同時改進后的算法所找到的路徑質量較高，接近理想路徑的數量更多。

表4 算法比較

綜上所述，改進后的算法在普通環境中能更好地實現路徑規劃，無論是從成功率、節點數還是工作時間上都得到了一定改善，接下來仍要繼續測試在一些特殊條件中算法的表現如何。

3.2 實驗二

在本組實驗中，針對于算法的狹窄空間路徑規劃能力進行測試，如圖8~9所示。采用Matlab R2016b數學仿真軟件進行，建立了整體地形較為狹窄的空間，設置多個復雜、易誘導路徑，模擬實際的工作過程。部分實驗參數如表5所示。

表5 實驗二參數

圖8 傳統算法

在對算法進行了200次模擬后，結果如表6所示。普通RRT算法成功率僅為47%，且所用平均節點數約為909個，實驗時間較長。而改進后的算法成功率為63.5%，增加了16.5%，平均節點數減少了大約13.3%，平均時間減少了大約22.1%。

圖9 改進算法

表6 實驗二結果

因為設置地圖環境較為復雜，且設定步數也僅為1 000步，所以兩種算法成功率均較低，但是也可明顯看出改進后的算法對于路徑的規劃表現更佳，從成功率和規劃效率上有了一定的優化，尤其在地圖的狹窄部分，由于選點函數和狹窄概率函數的影響，使得選點判斷更合理，所選擇路徑更優。

4 結束語

設計一種新的選點函數，通過區域化增益的方式，隨機生成新的子節點，通過節點選擇判定后產生，可以有效減少無效點，提高選點效率。

針對特殊狹窄空間，彈性地改變選點判定范圍，更有利于尋找正確路徑，提高整體路徑規劃效率。

本文研究提出的RRT結合人工勢場軌跡規劃的算法，經過模擬實驗測試，相對于傳統的RRT算法，改進后的算法擁有更好的路徑規劃能力，能有效改善路徑規劃中路徑的選擇問題。