改進RRT 的復雜障礙物環境路徑規劃算法研究

牛旭，張志安

（南京理工大學機械工程學院，江蘇南京 210094）

路徑規劃，即根據規劃空間中的信息，規劃出一條最優路徑[1]。對于復雜障礙物環境下或者高維空間下的路徑規劃問題，常見的算法有RRT 算法和PRM 算法[2-4]。而對于多維度空間或復雜環境的路徑規劃，RRT 算法不需要規劃空間，其泛用性更好[5-6]。但是，基本的RRT 算法在規劃路徑時也存在一些缺點，由于采樣過程的隨機性，搜索路徑存在具有擴展隨機性大，冗余節點多，收斂速度慢，導致搜索效率低等問題。

在后續研究中，文獻[7]提出了漸進最優RRT*算法，通過重新選擇父節點和重布線思想來優化路徑。文獻[8]提出了Quick-RRT*算法，通過擴大RRT*算法動態調整父節點的范圍，提高其搜索效率。文獻[9]在復雜環境下利用A*作為引導域限制RRT 的無效搜索提高其采樣效率。文獻[10]提出了一種Informed-RRT*的變體，利用目標偏置引導策略搜索目標點，利用橢球形子集細化路徑，從而找到最優路徑。文獻[11]引入了權重系數，增加了樹向目標點方向的擴展分量，文獻[12]等在RRT 中引入了引力系數，增加樹目標點的擴展分量，引導擴展偏向目標點，提高其迭代效率。

但對于樹擴展隨機性問題的改進研究，大都單一考慮了如何讓樹偏向目標點方向進行擴展，而不考慮環境障礙所帶來的影響。對此，該文提出了一種改進的RRT 算法：引入自適應步長目標偏置，結合勢場力引導隨機樹向目標點的生長，借助區域定性降低障礙物附近的搜索區域，再對路徑關鍵點剪枝，利用B 樣條曲線完成對路徑平滑優化，得到一條最優路徑。

1 算法原理

1.1 RRT算法原理

路徑規劃通常被視為在狀態空間Χ中搜索從初始狀態xinit到目標區域xgoal∈X或目標狀態xgoal的連續路徑[13]。該算法采用隨機擴展的方法在狀態空間中構造從初始節點到目標節點的樹。以狀態空間的起點xinit作為根節點，在搜索空間中隨機選取一個點xrand。如果這個點在可行區域內，它將遍歷T，在這個隨機點上尋找最接近的節點n。通過碰撞檢測，當xrand與xnear之間的距離比設定的擴展步長小時，xrand將作為葉節點xnear加入到隨機樹中；而當xrand與xnear之間的距離大于設定的擴展步長時，則在xrand與xnear之間的線段上，以xnear擴展設定步長的點xnew，將其加入到隨機展開樹中，將與xnew最近的節點稱為xnew的父節點。重復上述迭代過程，直到目標節點變為葉節點或超過設定的迭代次數時則搜索結束。從目標節點回溯到起點，即可獲得規劃的路徑。節點擴展的原理如圖1 所示。

圖1 RRT算法節點擴展原理圖

傳統RRT 算法偽代碼如下所示：

1.2 人工勢場法（APF）

APF 是一種局部路徑規劃算法[14-15]。假設機器人x在目標引力分量和障礙物斥力分量的共同作用下運動。其總勢場U(x)可以表示為:

式中，Utarget(x)是目標點作用于機器人x的引力勢場，Uobstacle(x)是障礙物對機器人的斥力勢場。

其勢場力F(x)可以表示為:

同樣地，在式（2）中，Ftarget(x)是目標點對于機器人x的引力作用力，Fobstacle(x)是障礙物對機器人x的斥力作用力，而F(x)則是其合力。

但是人工勢場法在路徑規劃中存在以下的缺點：

1）當運動空間障礙物較多時，路徑規劃的成功率會降低；

2）當障礙物與目標的距離小于一定閾值時，容易發生振蕩，使其無法到達目標點；

3）當F(x)所受合力在勢場中為0 時，將會陷入局部最小值，陷入局部振蕩階段。

2 改進RRT路徑規劃算法

2.1 自適應步長目標偏置

由于恒定步長的設定，無效搜索路徑冗余繁多，使得算法的收斂速度慢；對狹窄區域或存在復雜障礙物區域的探索能力不足的情況，由此引入改進的自適應步長策略。當距離障礙物較遠時，以大步長擴展，增加對所處未知環境的包容性，而當其進入障礙物影響域內，為了增加其對復雜環境的探索能力，選擇以小步長擴展，如式（3）所示：

引入自適應步長來消除遠離目標區域的無用搜索區域，引導隨機樹可能向目標點生長，大大減少了基本RRT 算法的迭代次數和節點數。

特殊情況下，針對xnear位于目標點鄰域λ內時，步長，以目標點偏置導向，高效迭代。

2.2 引入引力場與斥力場

2.2.1 引入引力分量

在自適應步長目標偏置算法中引入人工勢場的概念，增加隨機節點的重力分量，控制隨機樹向目標點生長，降低隨機性。原理圖如圖2 所示。其主要的核心思想是在每個新生成的節點中加入引力分量。

圖2 添加引力分量構造

在構造時，引力函數G(n)在每個新節點n中引入，以改變生長方向的合力。它可以表示為:

式中，F(n)代表隨機樹在搜索過程中確定新節點的函數，Rd(n)代表最新節點的隨機生長函數，G(n)代表添加的目標引力函數。

xnew節點的隨機生長函數如式（5）所示:

目標引力函數G(n)如式（6）所示：

式（5）、（6）中，ρ是步長，g為引力增益系數，xgoal為目標位置矢量，|xgoal-xnear|表示節點間集合距離的絕對值。

2.2.2 引入斥力分量

在自適應目標偏置RRT 算法中引入APF 中障礙物斥力場的概念，引導局部隨機樹朝向遠離障礙物生長。RRT 算法增加斥力分量的展開圖如圖3 所示。其主要的核心思想是對局部隨機樹搜索展開過程中產生的每個新節點引入斥力分量。

圖3 添加斥力分量構造

在構造時，障礙物排斥函數T(n)，在生成的新節點n處引入，可以表示為：

式中，F(n)代表隨機樹在搜索過程中確定新節點n的函數，Rd(n)代表最新節點的隨機生長函數，和T(n)是添加的障礙物斥力函數。

障礙物斥力函數T(n)如下：

式中，krep為斥力增益系數，p(x)為隨機節點到障礙物的最短距離，r0為節點上障礙物的影響范圍，xobstacle為障礙物的斥力矢量。

2.3 改進人工勢場法

傳統的勢場為了充分利用勢場的優勢，其斥力系數遠遠大于引力系數，會出現在目標位置附近由于障礙物的斥力過大而無法到達目標位置的問題。針對上述問題，如圖4 所示，利用多區域的劃分，重新構造目標斥力函數為：

式中，rsta為障礙物的安全距離（障礙物膨脹后）。

既要保障斥力勢場的效果明顯，又要兼顧附近目標位置的需求，利用劃分定性，在R0(R0=2r0)內的點所受斥力作用為無窮大，而當其處于Rsta(Rsta=2rsta)臨界區時，斥力由斥力系數定性。

2.4 復雜障礙物環境下逃離局部最小值

機器人在整個勢場中的運動是隨機的，當其處于復雜障礙物環境下時，由于引入勢場的概念，勢場引導會出現合勢場為0 的點，使得節點擴展處于緩慢，振蕩循環的狀態；通過連續五次相鄰路徑節點間的角度變化均處于±5°誤差范圍內，判定其處于局部振蕩階段；

由圖5 可知，處于復雜環境（三個近距離障礙物1，2，3）的勢場，具體實現：從點A擴展到點I結束，當計算第i個夾角時，需要利用i-1 和i+1 的節點坐標，用余弦定理進行角度計算；求出點A、B和點C之間的角度，以及點B、C和點D之間的角度，以此類推，其計算公式如式（10）所示：

為了引導其逃離局部最小值，該文引入局部定向剪枝概念，對路徑關鍵點進一步擇優剪枝選取。如圖5 所示，當機器人從點A開始陷入局部振蕩時，則連接點A與點B、C、D、E、F、G、H、I之間的線段進行碰撞判定，直到完成路徑迭代：A→H→I。

2.5 B樣條曲線的擬合優化

B 樣條是樣條曲線的一種特殊表現形式，同時也是貝塞爾曲線的一般形式，其具有凸包性、幾何不變性、局部支撐性、非負性等優點[16]。n次B 樣條曲線表達式為：

式中，Pi為第i段控制點的方程，Fi,n為n次B 樣條的基函數，其公式為：

而常見的B 樣條曲線的是二次和三次B 樣條曲線，因為二次B 樣條曲線同樣不需要經過首尾的節點，所以B 樣條曲線基函數選擇為三次樣條曲線。

為了保證沿路徑運動時的速度平穩性，減少突變處，對圖5 進行三次樣條擬合優化，從a到目標點的平滑曲線如圖6 所示。

圖6 三次樣條曲線擬合優化

3 仿真實驗與結果分析

為了驗證改進后的算法能否在空間的起始點更高效地搜索目標點并生成路徑，利用Matlab 搭建仿真環境，完成改進RRT 算法的仿真實驗。在該研究中，只考慮二維空間中的情況，模擬地圖的尺寸為1 000×1 000（無量綱）。為了方便比較，統一了各算法的仿真參數。具體參數如下：起始節點坐標為(0,0)，目標節點坐標為(999,999)，起始點和目標點分別用不同形狀標記，擴展步長為30。

3.1 標準RRT 與自適應步長（AS-RRT）算法實驗結果分析

標準RRT 與自適應步長(AS-RRT)算法實驗結果分別如圖7、8 所示。

圖7 標準RRT實驗結果圖

圖8 自適應步長目標偏置RRT實驗結果圖

從圖7-8 可以看出，標準RRT 算法在搜索過程中沒有自適應步長偏置引導，路徑是隨機生成的。而對于改進的自適應步長偏置算法，引入大步長加大在無障礙物影響的搜索區域的范圍，而引導隨機樹在多障礙物影響域引入小步長增加環境遍歷包容性；大大減少了基本RRT 算法在多障礙物域內搜索的迭代次數和擴展的節點數。

3.2 APF-RRT和APF-ASRRT算法實驗結果分析

APE-RRT 和APF-ASRRT 算法實驗結果分別如圖9、10 所示。觀察兩圖發現，通過引入引力系數kp，其值為0.000 01，路徑可以更嚴格地限制在起始節點和目標節點之間的區域內，與自適應步長RRT 算法相比，減小了搜索面積。由于障礙物斥力場的存在（斥力系數krep為200 000），對距離障礙物不同距離范圍內的斥力定性，保障了障礙物附近有效點的搜索面積。因此，可以發現多障礙物之間的路徑幾乎是一條線，不僅使搜索路徑與障礙物保持適當的距離，而且保證多障礙物環境定性區域搜索的完備性；但是由于勢場的缺陷，路徑會出現局部振蕩階段。

圖9 APF-RRT實驗結果圖

圖10 APF-ASRRT實驗結果圖

3.3 剪枝優化和三次樣條曲線結果分析

APF-ASRRT 算法的剪枝優化和三次樣條擬合實驗結果分別如圖11、12 所示。

圖11 APF-ASRRT算法剪枝優化實驗結果圖

在圖11-12 兩幅圖中，從圖11 看出針對局部振蕩階段的震蕩缺陷，剪枝擇優選取路徑關鍵點，綠色表示算法在二維空間中剪枝后的路徑。圖12 中黑色路徑即是該三次樣條曲線擬合出最優可行路徑。從這兩幅圖中可以看出，剪枝優化與曲線擬合路徑保障環境的實施可行性。

圖12 APF-ASRRT算法三次樣條擬合實驗結果圖

3.4 復雜環境RRT算法和改進RRT算法實驗結果分析

復雜環境標準RRT 算法和改進RRT 算法實驗結果分別如圖13、14 所示。

圖13 復雜環境標準RRT實驗結果圖

為了增加對復雜障礙物環境的對比，將算法部署到多障礙物復雜環境，從圖13 和圖14 中可以看出，該算法仍能保證對路徑的搜索穩定性。

圖14 復雜環境改進RRT結果圖

3.5 RRT算法改進實驗系列對比分析

研究中，改進的AS-RRT 算法、改進的APF RRT算法和基本算法在相同系數的前提下，對50 次仿真的迭代次數、搜索時間和路徑長度進行了對比，計算模擬實際的平均搜索時間、平均路徑長度和平均迭代次數，如表1 所示。

表1 仿真實驗結果對比

從表1 中可以看出，AS-RRT 算法與標準RRT 算法相比，路徑搜索時間減少了10.16%，路徑長度減少了9.15%。而改進的APF-RRT 算法進一步減少了52.1%的迭代次數和92.1%的計算時間。在路徑長度上，由于斥力場的限制，APF-RRT 改進算法的路徑長度并沒有減少。在多障礙物環境下，由改進的APF-ASRRT 算法結果可以看出，該算法在保證了對同一路徑搜索的同時，搜索時間和迭代次數分別顯著減少68.5%和20%。

4 結論

該文研究基于RRT 算法結合人工勢場的概念改進算法，提出一種多障礙物路徑尋優的策略。仿真實驗證明，該混合算法明顯改善了基本RRT 算法的不足。通過引入自適應步長偏置策略和人工勢場概念，有效地減少了路徑搜索過程中的迭代次數和運行時間，減少了路徑的隨機性和偏差，并且對初步規劃出的路徑進行剪枝擇優和三次樣條B 曲線平滑處理。該算法對RRT 進行多處改進，對于多障礙物環境的路徑規劃，具有一定的參考意義。

雖然在對多障礙物的表面做規則量化處理，但在實際的環境中，往往是不規則的多邊形，因此未來的研究可針對不規則的障礙物進行研究。