一種用于無人車路徑規劃的改進雙向快速擴展隨機樹算法研究

2021-10-15 05:44:10劉光中時培成梁濤年

安徽工程大學學報 2021年4期

劉光中,時培成*,倪璇，梁濤年

(1.安徽工程大學汽車新技術安徽省工程技術研究中心，安徽蕪湖 241000；2.蕪湖伯特利汽車安全系統股份有限公司，安徽蕪湖 241009)

智能交通系統的興起給汽車工業的發展帶來新的活力，在智能交通系統發展過程中，最核心的部分就是無人駕駛車輛。路徑規劃作為無人駕駛汽車發展研究的關鍵技術之一，受到廣泛的關注和研究。路徑規劃過程就是車輛根據車身傳感器所接收到的信息，在即將行駛的環境空間中分析計算出一條既滿足車輛動力學又滿足幾何約束，又能在行進過程中不與障礙物發生碰撞的可行軌跡。

近年來基于快速擴展隨機樹(Rapidly Expanding Random Tree，RRT)的路徑規劃方法由于在高維空間中的卓越性能受到了廣大學者的青睞。該算法雖然本身具有概率完備性，且算法收斂速度較快，在具有障礙物的未知環境中可以很好地應用等優點，但也存有一些不容忽視的缺點：①為了找到可行路徑，算法通過不停地進行反復迭代來探索全部的未知環境，計算過程中會消耗大量的內存；②由于算法在環境空間中進行擴展生長時采樣比較隨機，導致隨機樹在路徑生成時沒有方向，規劃結束時會難以生成最優路徑；③由于算法的隨機性，會很大程度上計算出較為粗糙彎折的路徑，不適合無人車輛直接使用。

針對RRT算法所存在的不足，研究人員有針對性地做出了一些改進。Adiyatov O等提出的RRT算法，巧妙地解決了RRT算法中難以生成最優路徑的問題，但是會增加很大的計算量，使得路徑生成實時性大幅度降低。裴以建等在RRT的基礎上，提出一種新的離線路徑規劃算法，結合人工勢場法調整采樣區域，縮短了生成最優路徑的時間。康亮等提出的將滾動窗口和RRT相結合的算法，以滾動方式進行在線規劃，提高了算法對未知空間的探索能力，但是效率偏低。潘思宇等在RRT算法的基礎上進行了改進，同時引入節點啟發式采樣函數，在一定程度上提高了路徑規劃時的速度和質量。閆明亮等提出了一種新的采樣方法，提高了路徑規劃的效率，但是仍不能很好地解決采樣點隨機性較大的問題。

針對上述問題，研究提出一種新的改進Bi-RRT算法，在原始的RRT算法基礎上替換了原算法的采樣方式，同時生成兩個隨機采樣點，選擇兩者中距離目標點比較靠近的采樣點，以提高采樣效率，并加入目標引力思想，讓隨機樹更具有方向性；規劃過程中，兩棵樹將對方新生節點作為目標點，在提高方向性的同時，降低不必要節點的產生。通過對RRT和Bi-RRT算法進行仿真實驗驗證，表明研究算法擁有更小的時間代價、更少的迭代次數和更高的路徑質量。

1 基本RRT算法

1.1 單向RRT算法

RRT算法是一種采用增長式擴展樹的搜索算法，流程如圖1所示。它將所要規劃路徑的起點位置作為隨機樹的根節點，將終點位置作為隨機樹的目標節點。隨機樹從根節點出發，在空間中隨機采樣，使用隨機采樣函數生成隨機點，然后用最近點函數遍歷隨機樹中所有已生成節點，找到距離隨機樹中最近的節點，接著從最近點向隨機點方向擴展一個步長生成一個新的節點。通過碰撞檢測函數來判斷最近點向新節點擴展過程中是否與障礙物發生碰撞，若無碰撞，將新節點加入隨機樹中，反之，舍棄此新生節點，繼續重新采樣。直至新生節點與目標節點間的距離小于設定閾值，停止生長，返回規劃的路徑。

圖1 基本RRT算法

1.2 雙向RRT算法

雙向RRT算法(Bi-RRT)的擴展示意圖如圖2所示。Bi-RRT在自由空間中定義了兩棵樹，采用和基本RRT算法相同的節點生成方式，算法從起始點位置和目標點位置各生成一棵隨機樹相向擴展，根據產生的單個隨機點，兩棵樹交替擴展生成新節點，直到兩棵樹相遇或者新生節點間距離小于設定閾值則生成路徑。

圖2 Bi-RRT擴展示意圖

2 改進Bi-RRT算法

2.1 節點生成方式的改進與自適應概率采樣目標偏向策略

基本RRT算法節點生成方式單一，且生成的節點比較隨機，導致隨機樹擴展時沒有方向，降低了規劃速度。研究提出雙采樣點的方式來提高算法的搜索效率，在環境空間內同時生成兩個隨機采樣點，通過比較兩個采樣點到目標點的距離，選擇與目標點較近的點作為臨時采樣點，能夠快速減少待規劃路徑，縮短算法運行時間。

同時，針對算法的無方向性、容易陷入局部極小值問題，提出基于目標偏向的自適應概率采樣策略，目標引力思想可以使隨機樹的生長具有一定的方向性。改進Bi-RRT算法采用啟發式采樣，運行時通過隨機概率

(1>

≥0)來選擇擴展點。若設定參照概率

，當

時，選擇隨機點作為最終采樣點，當

時，選擇目標點作為最終采樣點，當目標點作為采樣點時能夠提高隨機樹的擴展效率。擴展樹節點

時，計算公式為

(1)

式中，

為擴展時隨機樹的步長;(

)表示兩向量單位化;||

||表示兩點間歐氏距離。加入目標引力思想的

計算公式為

(2)

式中，

表示向隨機采樣點方向擴展時的步長；

表示向目標點方向擴展時的步長；||

||為

和

間歐氏距離。上述目標偏向引力思想雖然可以引導隨機樹向目標方向生長，但是選取目標點作為隨機點的概率值是固定的，當算法陷入局部極小值時，向著目標擴展一般是無效的，反而會浪費大量的計算內存。因此，當以目標點方向擴展無效時，認為擴展樹陷入局部極小值，將概率

值設為0，讓算法努力通過隨機生長來逃出局部極小值，之后隨著隨機采樣次數增多，隨機樹跳出局部極小值的可能性逐漸變大，所以

值也會隨著擴展次數的增加而增加。隨機樹跳出局部極小值時的

值公式如下：

(3)

式中，

是未陷入局部極小值時的概率；

是形狀系數；

是最大嘗試次數。當

接近

時，

逐漸恢復為

。

2.2 改進Bi-RRT算法原理

本算法將起始位置

和目標位置

分別作為

、

兩棵隨機樹的根節點，兩棵樹進行相向生長，如圖3所示。

生長時，使用采樣Random_point函數同時生成兩個隨機采樣點

1、

2，用Near_to_Goal函數比較

1、

2與目標點位置

的距離。若知|

1，

|<|

|，選擇距離較小的

1作為臨時采樣點，結合目標引力思想，當

時，選擇

作為最終采樣點，當

時，選擇

作為最終采樣點來擴展得到新節點

。

擴展時，將

生成的新節點

作為目標點，新節點產生方式與

相同。接下來兩棵樹分別把對方上一步新生成的節點作為目標點，繼續向下擴展生長，一直到兩棵樹的新生節點間的距離小于設定步長，則連接兩節點生成規劃路徑。

圖3 改進Bi-RRT算法示意圖

2.3 改進Bi-RRT算法實現

提出的改進Bi-RRT算法實現過程如圖4所示。

(1)分別以起始位置

和目標位置

為

、

根節點，兩棵樹相向生長。(2)使用采樣Random_point函數同時生成兩個隨機采樣點

1、

2，用Near_To_Goal函數選擇距離目標較近點作為臨時采樣點。(3)結合目標引力思想，當

時，選擇

作為最終采樣點；當

時，選擇

作為最終采樣點。新生節點

會偏向最終采樣點。(4)通過碰撞檢測Collision Free函數來判斷節點

向節點

擴展過程中是否與障礙物發生碰撞，若無碰撞將

加入

中，反之，返回第(2)步。(5)

、

分別把對方上一步新生成的節點當作目標點，進行下一步生長。(6)兩隨機樹新生節點

間的距離小于設定步長，則生成路徑。

圖4 改進Bi-RRT算法流程圖

3 仿真驗證

為了驗證改進Bi-RRT算法的有效性、實時性和相對原始算法的優越性，繪制了復雜障礙物地圖和迷宮地圖如圖5所示。由圖5a可見，障礙物大小不一、形狀不規則且分布復雜，是為了充分模擬現實中復雜的環境。由圖5b可見，障礙物形狀規則，但是通路較多極具迷惑性，對算法的計算能力是一種極大的考驗。在這兩種環境地圖中進行傳統RRT和Bi-RRT算法仿真，并對規劃路徑及規劃效率作出對比分析。

仿真實驗均在Matlab R2015a編程環境下，電腦配置參數：CPU，Intel(R) Core(TM)i7-6700，3.4 GHz；運行內存為4 G。地圖尺寸大小為500*500，起始位置和目標位置坐標分別為[10,10]，[490,490]。

圖5 環境地圖

3.1 復雜障礙物地圖仿真分析

在圖5a中，將RRT算法同Bi-RRT算法和改進Bi-RRT算法分別運行30次，將3種算法在每次路徑規劃中產生的迭代次數、規劃時間和路徑長度記錄下來。各取3種算法的一次運行結果如圖6所示。由圖6可知，基本RRT算法因為具有概率完備性，在地圖中采用均勻隨機采樣策略，搜索盲目，無目的性，故產生了大量的無用節點，且路徑比較曲折；Bi-RRT算法相對RRT算法，采用兩棵樹相向生長的方法來提高探索速度，很大程度上減少了無用的節點；改進Bi-RRT算法相對前面兩種算法，減少了極大部分的無用節點，生成的路徑在三者中最優。3種算法在復雜障礙物地圖中成功搜索到路徑時迭代次數、所需時間和路徑長度的對比圖如圖7、圖8、圖9所示。

圖6 復雜障礙物地圖規劃比較

圖7 復雜障礙物地圖迭代次數對比圖8 復雜障礙物地圖運行時間對比

圖9 復雜障礙物地圖路徑長度對比

3.2 迷宮地圖仿真分析

在圖5b中，將RRT算法同Bi-RRT算法和改進Bi-RRT算法各自運行30次，記錄3種算法在每次路徑規劃中的迭代次數、規劃時間和路徑長度。3種算法的運行結果圖如圖10所示。由圖10可知，基本RRT算法在整個環境地圖上生成了大量的無用節點，路徑極其曲折，不夠平滑；Bi-RRT算法相對RRT算法，節點明顯減少，路徑彎折情況有所改善；改進Bi-RRT算法相對前兩種算法，所產生的無用節點數量最少，路徑最平順。3種算法在迷宮地圖中成功搜索到路徑時迭代次數、所需時間和路徑長度的對比圖分別如圖11、圖12、圖13所示。由圖11、圖12、圖13可知，改進Bi-RRT算法相對于基本RRT算法和Bi-RRT算法，在算法迭代次數、路徑規劃時間和規劃路徑長度上具有明顯優勢。

3個參數記錄值的平均值如表2所示。由表2中數據計算得出，改進Bi-RRT算法相對基本RRT算法和Bi-RRT算法，在平均迭代次數上，分別減少了73.8%，21.7%；在平均規劃用時上，分別縮短了81.2%，46.7%；在平均路徑長度上，分別降低了14.2%，7.7%。

表1 復雜環境地圖仿真實驗數據

表2 迷宮地圖仿真實驗數據

圖10 迷宮地圖規劃比較

圖11 迷宮地圖迭代次數對比圖12 迷宮地圖運行時間對比

圖13 迷宮地圖路徑長度對比

4 樣車實驗

為了驗證改進算法的可靠度，進行了樣車實驗如圖14所示。樣車配備了16線激雷達和單線激光雷達，兩種雷達可獲取融合后更為精確的環境信息。導入改進Bi-RRT算法后，由工控機對環境進行處理，并規劃出路徑。

圖14 樣車及主要部件

在獲取環境信息建立地圖模型后，改進的Bi-RRT算法規劃出的行駛路徑如圖15所示。運行過程如圖16所示。由圖16可知，在布置好的環境地圖中，小車從起始位置成功穿越障礙物到達終點，驗證了改進Bi-RRT算法所規劃路徑的可靠性。

圖15 規劃路徑展示

圖16 運行過程展示

5 結論

研究以傳統RRT算法為基礎，對Bi-RRT算法做出了一些改進。摒棄單個采樣的方法，同時生成兩個隨機采樣點，選擇兩個隨機點距離目標點較近的點作為采樣點，提高了采樣效率和采樣點的有效性；又結合目標引力思想來選擇最終采樣點，可使得路徑在生長過程中具有明顯的方向性，提高算法規劃最優路徑的效率；在算法運行過程中，通過選擇對方隨機樹生成的最新節點作為目標點，加快了路徑生成速度。在兩種環境地圖中對改進后的Bi-RRT算法進行仿真驗證，結果表明，改進Bi-RRT算法在迭代次數、路徑生成所需時間和路徑長度上相較于RRT、Bi-RRT兩種算法具有明顯優勢。樣車在搭建的實驗場景中成功規劃出了可行路徑，驗證了改進Bi-RRT算法的可靠性。