基于改進RRT 算法的狹長空間無人車輛路徑規劃

張俊豪，潘樹國，高旺，郭芃，王萍，胡鵬

(東南大學儀器科學與工程學院,南京 210096)

0 引言

近年來，得益于電子通信技術、智能控制技術和智能計算技術等先進科技領域研究的突飛猛進，無人駕駛技術在世界各國得到快速發展.其中，路徑規劃是實現無人駕駛的重要技術之一，也是無人駕駛車輛能否安全并有效地抵達目的地的關鍵.

路徑規劃是指在規定區域內規劃出一條從起點到目標點的最優解路徑，且要保證與障礙物無碰撞，并盡可能保證路徑短、平滑度高、規劃時間短[1].目前，路徑規劃算法可以分為傳統路徑規劃算法和智能仿生路徑規劃算法兩類，其中傳統路徑規劃算法有A*(A star)算法、快速搜索隨機樹算法(rapidly-exploring random trees，RRT)、人工勢場算法、動態A*算法(dynamic A*，D*)等；智能仿生路徑規劃算法有蟻群算法、粒子群算法、遺傳算法等.在這些算法中，RRT算法是一種基于采樣的算法，因其具有概率完備性的特點而被廣泛應用于機器人路徑規劃中.

近年來，針對RRT 算法收斂速度慢、隨機性較大的缺點，許多學者都提出了改進RRT 算法的措施.Karaman等[2]提出 RRT*算法，該算法引入了重新布線的思想，通過重新選取路徑的父節點，使得得到的路徑最短；彭君[3]提出了基于變權重勢場的改進RRT 算法和基于模糊邏輯的改進RRT*算法，前者通過采用KD-Tree 算法(k-dimensional tree)和變權重的人工勢場法的規劃策略，解決了RRT 算法在路徑規劃時收斂速度慢和避障效果較差的問題，后者在RRT*算法的基礎上加入了模糊邏輯策略，提高了路徑質量；Zucker等[4]提出了MP-RRT 算法，該算法通過偏置采樣分布并重新使用先前規劃迭代中的分支來實現動態環境中的實時規劃；Otte等[5]提出RRTX算法，它在整個導航過程中對相同的搜索圖進行了改進，以處理不可預測的移動障礙物.

現有的算法大多聚焦于改進各個算法本身，專注于對算法進行優化和結合，但卻忽略了實際情況中環境和無人車輛本身的約束.在狹長空間中，可行區域狹小，隨機樹很容易陷入局部困境，從而導致規劃效率和成功率較低，影響無人車輛的正常運行.另外，在狹長空間中，無人車輛的尺寸不可忽視，這對于路徑則有著更高的要求.

在已有研究成果的基礎上，結合RRT 算法本身的缺陷以及無人車輛和環境的約束，本文提出了一種改進RRT 算法.該算法首先通過目標動態概率采樣策略，將終點信息加入了RRT 算法隨機采樣的過程里，減少了RRT 算法隨機采樣的盲目性；之后利用人工勢場對RRT 算法中的隨機樹進行引導，使其更容易接近終點；然后算法考慮了傳統RRT 算法中步長固定導致的規劃效率低的問題，以一種動態步長代替原有的固定步長，提高了規劃效率；最后，算法考慮了實際情況中路徑的轉角限制和車輛的碰撞限制，提高了路徑的平滑度和安全性.

1 傳統人工勢場引導下的RRT 算法

傳統的RRT 算法是一種基于采樣的路徑規劃算法，以一個初始的根節點為起點，通過隨機采樣的方法在空間搜索，然后添加子節點來不斷擴展隨機樹.當目標點進入隨機樹后，隨機樹擴展立即停止，此時能找到一條從起始點到目標點的路徑.

人工勢場法(artificial potential field，APF)是一種人為的在地圖中添加一個特殊的場來引導無人車輛從起點到終點的路徑規劃算法.

RRT 算法最大的優點在于其搜索能力強，能夠在時間充足的情況下搜索整個空間，從而得到一條可行路徑，但其搜索的隨機性也帶來了搜索效率低的缺陷.而APF 則擁有目標導向的作用，將兩者結合，人工勢場能夠在一定程度上引導隨機樹向目標點擴展，同時RRT 算法也能在一定程度上解決APF 容易陷入局部最優解的情況.

圖1 為人工勢場引導下的RRT 算法(APF-RRT算法).

圖1 APF-RRT 算法示意圖

其算法流程圖如圖2 所示.

2 算法優化及改進

2.1 改進RRT 算法

2.1.1 自適應目標概率采樣

在傳統的RRT 算法中，雖然隨機樹能夠保證在時間充足的情況下搜索出一條可行路徑，但這樣的搜索具有盲目性，會使得隨機樹產生許多無用的節點，另外，當隨機樹擴展到目標點附近時，由于搜索的隨機性，會出現多次擴展都無法擴展至目標點范圍的情況，降低了規劃效率.為解決這樣的問題，本文采用自適應目標概率采樣的方法為隨機樹增加目標導向性，使得其能夠快速擴展到目標點附近.傳統的目標概率采樣策略的做法，在采樣階段，以一定的概率Pgoal選用目標點Xgoal作為隨機采樣點Xrand，即

式中：Xrandom為原算法隨機產生的采樣點；P為0～1的一個隨機值.

傳統的目標概率采樣策略能夠很好地對隨機樹產生一定的目標引導作用，但是，當地圖中的障礙物較多且較為密集，尤其是狹長路段時，直接使用傳統的目標概率采樣策略不僅不會提高規劃效率，反而會出現陷入局部困境的問題.為了解決這樣的問題，本文采用目標動態概率采樣的方法來應對障礙物密集的情況.具體的做法是將原有的固定的概率Pgoal調整為自適應的值，即當隨機樹擴展到障礙物密集的區域時則降低Pgoal，使得隨機樹能夠更好地逃離狹長區域.即

式中：niter為迭代次數；nvoid為無效節點的個數(即與障礙物發生碰撞的節點個數)；Pmax為概率采樣閾值的最大值(一般為0.1～0.3).

2.1.2 動態步長

在RRT 算法中，由于步長的固定性，導致算法不能適應地圖中的各種情況，從而使得生成路徑的平滑度和效率較低.當步長較大時，生成路徑的效率較高，當產生路徑的平滑度較低，并且當遇到狹長區域時會陷入局部困境從而提高迭代次數；當步長較小時，在遇到狹長區域時能夠快速逃離障礙物區域并且生成路徑較為平滑，但規劃效率較低.因此，為了解決這種矛盾，本文采用一種動態步長的方法，使得步長隨著障礙物的密集程度進行變化，能夠有效解決大步長和小步長之間的矛盾，提高RRT 算法適應各種環境的能力，從而使得其能夠更好地應用于狹長區域.具體的動態步長 λ為

式中：Sobs為所有障礙物與上述矩形相交部分的面積；Stotal為以Xrand和Xnear為對角線且長和寬平行于坐標軸的矩形面積；λori為設置的初始最大步長.示意圖如圖3 所示.

圖3 動態步長策略示意圖

其中，以Xrand和Xnear為對角 線的矩 形面積即為Stotal，紅色區域的面積即為Sobs.通過這樣的動態步長，能夠讓隨機樹在狹長空間減少步長，從而快速逃離該區域；在空曠的區域，步長會隨之增大，從而提高規劃效率.

2.2 路徑約束條件

常見的路徑規劃算法大多關注于算法的優化和結合，而忽略了實際情況中的一些動力學約束條件，這些約束條件是讓無人駕駛車輛能夠正常運作的關鍵條件，因此并不能忽略.例如實際無人車輛不能作為質點進行處理，而必須考慮其本身大小，另外實際無人車輛的底盤有最大轉角的限制，過大的路徑轉角會使得無人車輛花費更多的動力甚至出現無法轉向的情況，因此本節將從碰撞約束和路徑轉角約束兩個方面展開.

2.2.1 碰撞約束

安全性是路徑規劃的基本要求之一，為了讓生成的路徑能夠使得無人車輛安全地抵達終點，必須對路徑點進行碰撞約束.常用的碰撞約束是判斷以Xrand和Xnear為端點和線段與障礙物是否相交來完成的，但是這樣方法的前提是將無人車輛看作質點，然而實際情況中如狹長空間中，無人車輛的大小無法忽略，常用的碰撞約束方法則不適用.

本文考慮了無人車輛的大小，將其視為一個矩形，在每產生一個Xnew節點之后，對其進行碰撞檢測，若以Xnew為中心的矩形與障礙物有相交的情況，則舍棄該節點.但是，當步長較大時，會出現圖4 所示的情況.

圖4 步長較大時兩節點連線與障礙物相交

這種情況的Xnew和Xnear節點均滿足碰撞約束，但是，他們的連線與障礙物發生的相交.為了減少這種情況，取Xnew和Xnear的中點Xmid，對Xmid同樣進行一次碰撞檢測，即當Xnew和Xmid均通過碰撞檢測時才認為Xnew是符合碰撞約束的，從而提高路徑的安全性.

2.2.2 路徑轉角約束

路徑的轉角約束對于無人車輛也是十分有必要的，這樣不僅能夠減少無人車輛轉向所消耗的動力，也能提高路徑的平滑度.路徑的轉角如圖5 所示.

圖5 路徑轉角

其中，Xparent為Xrand的父節點，θ 為轉角，根據余弦定理，有

式中，lXnear,Xparent、lXnear,Xnew和lXnew,Xparent分別表示三個節點之間的距離.當產生Xnew時，若 θ 的值大于所允許的最大轉角限制 θmax，則舍棄該節點，從而保證轉角約束.

2.2.3 限制區域隨機轉向

上述的路徑轉角約束對于無人車輛的作用不可忽視，但是，這樣的約束對于規劃效率帶來了很大的困難，由于RRT 算法的隨機性，滿足轉角約束的節點較少，這就導致每次規劃都將花費較多的迭代次數，大大降低了規劃效率，這在狹長的空間尤為明顯.狹長空間由于可行區域較窄，轉角的限制會使得隨機樹陷入局部困境.

為了解決這樣的問題，本文提出了限制區域隨機轉向的策略.即當產生的Xnew不滿足轉角約束的時候，在以Xnear為頂點，2θmax為張角的范圍內隨機選擇一個方向，在這個方向以步長產生新的Xnew，這樣產生的Xnew滿足轉角限制，因此只需要進行碰撞檢測即可.示意圖如圖6 所示.

圖6 限制區域隨機轉向策略示意圖

其中，Xold為不符合轉角約束的節點，θrand為0～θmax隨機選取的角度.隨機轉向的策略為隨機樹在障礙物密集或狹長的空間提供了更多擴展到可行區域的機會.

2.3 算法總流程

圖7 為算法整體流程.

圖7 改進RRT 算法流程圖

算法的具體步驟如下：

1)設置起點Xstart，終點Xgoal，可行區域O，障礙物rect，初始最大步長 stepori 等參數；

2)在可行區域O內采用自適應目標概率采樣得到Xrand；

3)遍歷隨機樹上的所有節點，計算得出距離Xrand最近的節點Xnear；

4)根據動態步長策略得到新的步長step；

5)計算Xnear所受的引力場和斥力場的合力F；

6)計算Xnear指向Xrand的單位 向量并和F的單位向量進行矢量疊加得到新的向量Fnew；

7)以Fnew方向，step 為步長擴展生成新的節點Xnew；

8)若Xnew不滿足碰撞約束，則舍棄Xnew；若Xnew滿足碰撞約束且滿足轉角限制，則將Xnew作為新的樹節點加入到隨機樹中；若Xnew滿足碰撞約束但不滿足轉角限制，采用限制區域隨機轉向策略產生新的Xnew，若新的Xnew不滿足碰撞約束，則舍棄Xnew；若新的Xnew滿足碰撞約束則將Xnew作為新的樹節點加入到隨機樹中；

9)重復步驟2)～8)直到新的樹節點與終點的距離小于設定的閾值，之后回溯樹節點得到路徑.

3 仿真實驗結果及分析

本文利用Python 語言，基于Windows10 下的PyCharm Community (Edition 2023.1)編寫程序.分別在簡單環境、密集障礙物環境以及狹長空間三種情況下對現有算法和改進RRT 算法進行仿真對比分析.為了減少結果的隨機性，在三種不同環境下分別運行不同算法各100次，取平均迭代次數、平均路徑長度、平均規劃時間、成功率作為路徑規劃的評價指標.另外，選用20×20 的矩形作為仿真地圖，并在仿真地圖中根據不同環境設置不同障礙物進行測試.

3.1 參數設置

考慮初始設置參數對實驗結果的影響，采用不同的初始最大步長(0.5、1 和1.5)和不同的最大迭代次數(2 000、5 000 和10 000)對約束條件下的改進RRT算法和APF-RRT 算法在狹長空間進行測試從而選取適合的初始參數.

首先固定最大迭代次數為10 000，測試不同的初始最大步長對結果的影響(為了減少隨機性，對不同算法分別運行100 次取平均值)，結果如圖8 所示.

圖8 不同初始最大步長下算法的實驗結果折線圖

結果表明，隨著步長的增大，兩種算法平均迭代次數和規劃時間呈下降趨勢，并且在步長為0.5 時成功率有所下降，而平均路徑長度則變化較小.因此，選用1.5 作為初始最大步長能同時保證算法的成功率和效率，減少其對算法性能的影響.

其次，固定初始最大步長為1.5，測試不同的最大迭代次數對結果的影響，結果如圖9 所示.

圖9 不同最大迭代次數下算法的實驗結果折線圖

結果表明，當最大迭代次數為2 000 和5 000時，兩種算法的成功率較低，因此選用10 000 作為最大迭代次數來減少其對其算法性能影響.

3.2 簡單環境

由圖10 可知，在簡單環境下，傳統的APF-RRT算法得到路徑質量較差，會出現路徑貼合障礙物的情況發生，這在實際中是不允許的，而在約束條件下的APF-RRT 算法和改進RRT 算法均生成了合理、安全的路徑.由表1 可知，在沒有約束條件時，改進RRT算法相較于APF-RRT 算法在平均迭代次數、平均路徑長度和平均規劃時間方面分別降低了34.99%、4.89%、40.67%；在有約束條件時，改進RRT 算法相較于APF-RRT 算法在在上述指標里分別降低了44.51%、2.23%、14.49%.

表1 簡單環境下不同算法實驗結果數據

3.3 復雜障礙物環境

如圖11 所示，在復雜障礙物的情況下，傳統的APF-RRT 算法依然產生了貼合障礙物的情況.由表2可知，在沒有約束條件時，改進RRT 算法相較于APF-RRT 算法在平均迭代次數、平均路徑長度和平均規劃時間方面分別降低了16.13%、4.05%、2.72%；在有約束條件時，改進RRT 算法相較于APF-RRT 算法在在上述指標里分別降低了11.99%、5.09%、0.85%.

表2 復雜障礙物環境下不同算法實驗結果數據

圖11 復雜障礙物環境下不同算法所得路徑圖

3.4 狹長空間

在狹長空間中，改進RRT 算法能夠在自適應目標概率采樣策略和動態步長策略下快速通過狹長區域，提高了規劃效率.首先由圖12 可知，在增加了約束條件后，路徑質量得到明顯提升，并且減少了路徑貼合障礙物的情況.由表3 可知，在沒有約束條件時，改進RRT 算法相較于APF-RRT 算法在平均迭代次數、平均路徑長度方面分別降低了35.73%、0.1%；在有約束條件時，改進RRT 算法相較于APF-RRT 算法在上述指標分別降低了33.09%、0.06%，另外在平均規劃時間方面也降低了6.44%.

表3 狹長空間下不同算法實驗結果數據

圖12 狹長空間下不同算法所得路徑圖

當空間更加狹窄的時候，改進RRT 算法更能體 現其優勢，如圖13 所示.

圖13 狹長空間(極端狹窄)下不同算法所得路徑圖

由表4 可知，在約束條件下，RRT 和傳統APFRRT 算法的規劃成功率極低，其中RRT 算法甚至在100 次規劃無一次成功，而改進RRT 算法則有著55%的成功率.這說明當隨機樹進入狹長空間時，自適應目標概率采樣策略降低了采樣概率，同時，動態步長策略降低采樣步長，從而使得隨機樹能夠更好地逃離狹長區域，完成路徑規劃.

表4 狹長空間(極端狹窄)下不同算法實驗結果數據

4 結束語

為了解決RRT 算法在狹長空間路徑規劃時可能出現的隨機性過大、迭代次數較大的問題，本文提出了改進RRT 算法，其中自適應目標概率采樣策略和動態步長策略有效改善了在障礙物復雜或狹長空間中隨機樹容易陷入局部困境的問題，提高了規劃效率.尤其在狹長空間的情況下，自適應目標概率采樣策略和動態步長策略能夠很好地引導隨機樹逃離障礙物區域，使得隨機樹快速到達終點附近.

另外，本文還考慮了實際情況中的路徑約束，對路徑進行了碰撞約束和轉角約束，并針對轉角約束會導致迭代次數激增的問題提出了一種限制區域內隨機轉向的策略，這種策略能夠有效降低迭代次數，提高規劃速度.

最后，通過仿真實驗，驗證了改進RRT 算法在簡單環境、復雜障礙物環境、狹長空間中應用的有效性.在狹長空間中，改進RRT 算法在約束條件下相較于傳統APF-RRT 算法迭代次數降低了33.09%，路徑長度減少了0.06%，規劃時間減少了6.44%，另外在極端狹窄的情況下，改進RRT 算法相較于傳統APFRRT 算法規劃成功率從2%提高到了55%，提升效果十分明顯.此外在簡單環境和復雜障礙物環境下該算法均有提升.