復雜障礙物環境下基于轉向約束的智能汽車路徑規劃方法研究

姜 康，王 皓，陳佳佳

(合肥工業大學 汽車與交通工程學院，安徽 合肥 230009)

0 引 言

智能車輛研究的關鍵問題有：定位、感知、規劃、決策以及控制。路徑規劃是規劃的重要一環，在機器人領域又稱為運動規劃，是指用于將期望的運動任務分解成離散的運動，以滿足運動的限制，并可能優化運動的某些方面。智能車中的路徑規劃，是指生成一條從起點到終點的可行駛路徑，同時滿足時間、環境以及車輛動力學等約束[1]。

智能汽車可理解為一種多輪機器人，因此智能車路徑規劃算法與機器人領域相關算法一脈相承。常見有模糊規則法、遺傳算法、神經網絡、蟻群優化算法等，但這些算法的復雜度會隨著自由度增加呈指數級增長，并不適合解決復雜障礙物下的規劃問題。此外還有基于圖的算法例如A*算法[2]、D*算法、人工勢場法[3,14]等，這類算法雖滿足路徑規劃的實時性和最優性要求，但規劃的路徑并未考慮車輛的動力學特性，使規劃路徑不一定符合智能汽車的運動約束。

基于采樣空間的算法，搜索速度快且算法復雜度不會隨著自由度的增加轉向指數級增長，比較適合智能車的規劃問題。快速隨機搜索樹(rapidly-exploring random tree，RRT)作為此類算法的代表[4]，由LAVALLE提出，但是此算法存在3點缺陷：①在全局使用均勻采樣，雖保證了算法的概率完備性，卻導致算法的開銷過大，降低了收斂速度；②原始的最鄰近算法在存在復雜約束時算法可能多次失效，因而無法滿足實時性要求；③在全局的隨機采樣，導致生成路徑不平滑，無法滿足智能車的動力學約束。針對以上幾點不足，國內外的研究學者做出了一些改進。為了加快算法的收斂和搜索效率，S.M.LAVALLE提出了Bi-RRT[5]和 Goal-biasing RRT算法[6]；為了降低不準確的度量距離對RRT探索能力的影響，A.SHKOLNIK等[7]提出了RG-RRT(Reachability guided RRT)算法；由于隨著RRT算法采樣點趨向于無窮，收斂到最優解可能性趨于0; S. KARAMAN等[8]提出了漸進最優(Asymptotic optimality)的RRT*算法，但會導致算法的開銷變得十分龐大；為解決由于RRT算法的隨機采樣所導致的生成路徑不平滑問題，T.FRAICHARD等[9]采用了回旋曲線進行平滑處理，但由于回旋曲線沒有閉合形式解，無法滿足實時性的要求；B.LAU等[10]采用了5次貝塞爾曲線，但未考慮生成路徑的曲率連續性，因此無法滿足智能車的動力學約束；此外還有利用深度學習[11-12]訓練高回報點從而優化路徑，但可靠性和可解釋性還不是很高。

針對以上各類RRT衍生算法的不足，筆者就復雜障礙物環境下的智能汽車的路徑規劃問題，提出了一種基于改進的Bi-RRT算法。利用車輛的轉向約束進一步縮小采樣空間，結合Bi-RRT的快速搜索性使得路徑的生成更加迅速。并通過仿真實驗，驗證了該算法的有效性、實用性和適應性。

1 車輛運動學模型

針對機器人的傳統規劃算法均未考慮車輛的動力學模型，而對于智能汽車來說，車輛自身的動力學限制尤其重要，否則即使規劃好了路徑，車輛也不一定能跟隨預定生成的軌跡。采用常見的阿克曼轉向模型作為動力學模型，如圖1。

圖1 車輛動力學模型Fig. 1 Vehicle dynamics model

該模型具有三個自由度，(x,y)表示車輛在系統坐標系下的位置，θ代表車輛縱軸軸線與坐標系之間的夾角，φ為前輪轉角。假設車輛的速度為v，R為轉向半徑，k為轉向曲率，那么在任何運動瞬間，車輛滿足式(1)：

(1)

車輛模型受到動力學約束，故需要滿足式(2)：

(2)

式中：vmax為最大車速；φmax為最大前輪轉角；Kmax為最大轉彎曲率；Rmin為最小轉彎半徑。

2 改進的RRT算法

2.1 基本的RRT及Bi-RRT算法

基本的RRT算法是一種基于采樣的單查詢增量式樹狀結構算法。初始化時隨機樹T只包含一個初始節點qstart，隨后在狀態空間里隨機的選擇節點qrand，再遍歷T樹，找出距離最近的節點qnear，最后擴展函數從qnear向qrand擴展一段距離，生成一個新的節點qnew。隨后檢查新節點qnew是否與障礙物發生碰撞，若是則擴展函數返回空，放棄此次生成的qnew；否則將qnew添加至T樹。按照以上步驟反復迭代，直到新的節點qnear與目標點qgoal的距離小于一定的閾值，則代表T樹到達了目標點算法成功。若算法達到了最大循環次數依舊未能達到設定閾值，則算法失敗。最后，由目標點qgoal回溯到初始節點qstart，得到算法所規劃出的路徑。算法如圖2，具體搜索示意過程如圖3。

圖2 基本RRT算法Fig. 2 Basic RRT algorithm

圖3 基本的RRT算法示意Fig. 3 Schematic diagram of the basic RRT algorithm

雙向RRT算法的基本思想是在基本RRT上，從初始點和目標點同時構建隨機搜索樹。需要注意的是，第二棵樹的擴展方式與基本RRT算法的擴展方式略有不同，它會以第一棵樹生成的qnew代替qrand進行擴展，算法結束的標志即是兩棵樹相連。此外，考慮到兩棵樹的平衡性(即兩棵樹的節點數的多少)，需要交換次序選擇“小”的那棵樹進行擴展，使Bi-RRT算法較基本RRT算法更加有效。Bi-RRT搜索過程如圖4。基本的Bi-RRT算法示意如圖5。

圖4 基本Bi-RRT算法Fig. 4 Basic Bi-RRT algorithm

圖5 基本的Bi-RRT算法示意Fig. 5 Schematic diagram of the basic Bi-RRT algorithm

2.2 改進的Bi-RRT算法流程

通過對智能車規劃問題的分析及對基本RRT算法的介紹，筆者提出了一種在復雜障礙物環境下基于Bi-RRT改進的智能車規劃算法。具體算法步驟如圖6。算法的主要改進有兩點：①通過車輛的轉向約束限制了采樣空間，可以避免在整個采樣空間進行搜索，加快了算法的收斂速度；②在節點生成時，根據轉向約束限制節點的生成，使得生成的路徑更加符合車輛的動力學約束。具體改進的算法詳見2.3、2.4節。

圖6 改進的Bi-RRT算法Fig. 6 Improved Bi-RRT algorithm

2.3 基于轉向約束的概率目標偏向采樣策略

基本的RRT以及基本的Bi-RRT在隨機采樣的過程中，都是在全局范圍內的隨機搜索，這樣會導致有很多無謂的搜索而浪費計算資源，也使得路徑的生成更加緩慢。針對這一問題，筆者采在隨機樹每次的生長過程中，基于轉向約束限制采樣空間，從而避免了很多不必要的搜索，如圖7。由圖7可知，對于整個T樹而言，尋找最鄰近點時不必搜索全樹，而是考慮車輛自身轉向約束，先在生成的子樹中確定車輛允許的轉向區域，再從相應區域內尋找最近點。

圖7 基于轉向約束的搜索Fig. 7 Search based on steering constraints

為進一步提高搜索效率，使初始狀態和目標狀態更快相遇，在隨機數剛開始的擴展階段使用目標偏置策略——即根據隨機概率來決定qrand是目標點還是隨機點。設定一個目標偏向閾值qprob，在每次節點生長前得到一個0到1的隨機值p，當0

2.4 考慮轉向約束的最鄰近點擴展策略

RRT算法中的最鄰近點的搜索尤其重要，它不但決定了新節點的生長方向，對于全樹的收斂也非常重要。查找最近點的基礎是計算狀態空間內兩點之間的距離。計算距離最直觀的方法是使用歐氏距離，但對智能汽車來說，這樣的定義并不正確，如圖8所示，對于某車輛的Ⅰ狀態而言，下一時刻，存在Ⅱ、Ⅲ、Ⅳ這3種狀態。其中，Ⅱ向左旋轉了30°，Ⅲ與Ⅰ距離15 m，Ⅳ與Ⅰ距離40 m，由于車輛本身的動力學約束，車輛不能原地打轉也不能橫向平移，所以在歐式距離的定義下，離Ⅰ狀態最遠的Ⅳ狀態才是最近的搜索方向點。

圖8 最鄰近點判斷Fig. 8 Judgment of the nearest neighbor point

考慮車輛的動力學約束，筆者在尋找最近點時，首先基于車輛的轉向約束在目標偏置的基礎上進一步縮小采樣空間，然后利用K-D樹搜索出幾個歐式距離下的最近點，再考慮到車輛的動力學約束，使得到的最近點能夠符合最大曲率約束并保證曲率的連續性。如圖9，在選擇最鄰近節點時，雖然qi與qrand之間歐氏距離更近，但是由θi>θj，因此能夠使生成的路徑更加平滑并且能夠保證車輛具有良好的跟隨性，所以最終選擇qj作為最近鄰節點。筆者將隨機采樣點到鄰近點之間的度量函數定義如式(3)：

圖9 最鄰近節點選擇示意Fig. 9 Schematic diagram of the selection of the nearest node

Ddist=w1D+w2A

(3)

式中：D、A分別為歸一化后的歐式距離和角度；w1、w2是歐式距離和角度所占權重，可根據實際應用進行設定，據實驗的經驗數據設定w1=0.55，w2=0.45。dmax和θmax分別表示當前采樣區間內距離目標點的最遠距離和最大角度，di和θi分別表示當前采樣區間內第i個點距離目標點的距離和角度。D、A的值分別為：

(4)

3 后處理方法

由于RRT算法本身的隨機采樣性，生成的路徑通常非常曲折且極度不平滑，常常伴隨著抖動且包含很多不必要的節點，尤其是在存在大量形狀不規則且分布不均勻的障礙物的復雜環境中，過多的折點會使得智能車無法有效對路徑進行跟蹤，且頻繁轉向會影響乘坐舒適性，并加速車輛機械結構的磨損，這對路徑規劃來說是不可接受的。需對初始的RRT路徑進行剪枝和平滑化的后處理工作。

3.1 剪枝處理

根據車輛的最大曲率約束以及曲率的連續性，對全樹T中的安全節點進行刪除以達到剪枝的作用，如圖10。由圖10可知，從q1到q8，q10到q15都是安全的，因此可直接將q1和q8相連，q10與q15相連。若剪枝后的曲線不滿足最大曲率約束，則再插入必要的節點。程序仿真的剪枝前后的對比效果如圖11。

圖10 剪枝原理Fig. 10 Pruning principle

圖11 剪枝前后算法Fig. 11 Schematic diagram of the algorithm before and after pruning

3.2 碰撞檢測

在定義與障礙物的安全距離時，碰撞檢測是必不可少的重要步驟。采取通過環境地圖與車輛構型做卷積校驗的檢測方法進行碰撞檢測，將車輛大致構型為若干半徑為r的圓，如圖12。進行安全碰撞檢測時，需保證每個圓在環境地圖中所占據的柵格是安全的，只要有任意一個或幾個圓不安全，則需放棄此次生長并插入新的安全節點。

圖12 安全碰撞檢測Fig. 12 Safe collision detection

圖12中，a為車輛的長度，b為寬度，d為兩圓心之間的距離，r為圓的半徑，圓的相關參數為：

(5)

3.3 貝塞爾曲線平滑函數

貝塞爾曲線可以生成曲率連續的路徑，可以確保車輛能夠平穩沿著規劃路徑行進[13]。采用貝塞爾曲線對最終路徑進行擬合和平滑處理，n階貝塞爾曲線的如式(6)：

(6)

筆者采用具有兩個控制節點的三階貝塞爾曲線，如式(7)：

P(t)=(1-t)2P0+2t(1-t)P1+t2P2

(7)

式中：P0、P1、P2是指控制點的個數；t為一個在0到1的值，控制曲線的曲率。

4 仿真實驗

為了驗證算法的有效性、實時性和適應性，筆者進行了仿真實驗進行驗證。仿真實驗的全部算法用C++語言編寫，在搭載Intel? Core i7-8950H，16G內存的筆記本電腦上完成。設定車輛車頭朝向為箭頭所指方向，左下角為起始方向，右上角為目標方向，障礙物區域為黑色。圖13分別是基本RRT，Bi-RRT和改進后的Bi-RRT 3種算法分別在簡單障礙物及復雜障礙物情況下對同一規劃問題的求解結果。從仿真結果可以看出，改進后的Bi-RRT使得多余的節點生成數量大大減少，因此算法所需要的計算開銷明顯變少。而且生成路徑的曲率更加平緩，更符合車輛的動力學約束。

圖13 算法仿真效果對比Fig. 13 Comparison of algorithm simulation results

在復雜仿真環境下分別對3種算法進行了100次規劃，并記錄生成路徑所需的平均時間、生成的節點數目以及成功率，如表1。由表1可知，經過改進的Bi-RRT算法，在實時性以及節點數目的生成方面都有很大改進。此外，由于增加了約束，算法在成功率方面則略有下降，但依然能夠滿足實時性的要求。

表1 不同算法的時間、節點數目、成功率對比Table 1 Comparison of time, number of nodes and success rate ofdifferent algorithms

圖14(c)為3種算法生成路徑的曲率變化。由圖14可知，與筆者提到的基本RRT和基本Bi-RRT算法相比，筆者提出的算法所生成的路徑的曲率變化是更加平緩且平滑的，因此能夠滿足車輛的轉向約束。

圖14 歸一化路徑曲率對比Fig. 14 Comparison of normalized path curvature

綜合表1和圖14可以看出，改進后的Bi-RRT不論是在算法的搜索速度還是算法開銷上都有顯著的提升，搜索的路徑相對來說也更加平緩，這兩點在復雜障礙物環境下顯得尤為明顯。并且所生成的可執行路徑的曲率也非常符合車輛動力學要求，所以該算法是正確且有效的。

5 結 論

1)智能汽車一直是國內外的研究熱點，也是智能交通智慧出行的重要組成部分。路徑規劃在智能汽車研究問題中起著承上啟下作用，是感知與控制之間的橋梁。筆者提出的基于Bi-RRT改進的規劃算法，利用Bi-RRT的快速搜索性結合車輛的轉向約束，能夠實時有效的規劃出在復雜環境下智能汽車車輛的運動路徑并具有良好的跟隨行。

2)與傳統的RRT算法相比，由于利用車輛的動力學特性在節點生成時進行了約束，從而使得所生成的路徑的曲率能夠很好的滿足車輛的轉向要求。仿真實驗證明了算法的有效性、實時性和適應性。筆者所提出的算法不僅僅適用于無人駕駛汽車，也適用于其他的機器人路徑規劃問題。此外，該算法目前適用于低速場景下的路徑規劃問題，下一步將會嘗試解決高速場景下的路徑規劃問題，以便更好的滿足實際工程需要。