編者按：運動規划作為一種具有多個非線性約束的高維優化問題，需要消耗大量的計算資源，並且由於場景的復雜性和實時性，不是總能快捷簡單的得到最優軌跡，這對規划算法的魯棒性和實用性有着較高的要求。本文提出了一種軌跡監控策略，同時進行換道軌跡規划和監控軌跡規划，在前者未能規划出軌跡或者規划超時，系統將以監控軌跡作為輸出，保證車輛換道的穩定性和可行性。為滿足車輛運動約束以及實時性要求，將采用基於B樣條的方法來規划連續曲率路徑。基於RRT的方法可以作為保持算法完備性的補充。然后，監控軌跡主要遵循無碰撞要求，通過減速規划以保持車輛實時運動的穩定性。
本文譯自《A Novel Robust Lane Change Trajectory Planning Method for Autonomous Vehicle》，文章出自會議IV2019，作者為來自同濟大學智能汽車研究所的熊璐教授、曾德全博士等。
摘要：本文提出了一種新穎的自動駕駛車輛的換道軌跡規划方法。由於難以准確地獲得其他車輛的運動軌跡，自動駕駛車輛的運動軌跡不能總是快速簡單的得到。此外，運動規划作為一種具有多個非線性約束的高維優化問題，需要大量計算資源才能找到正確的解決方案。為此，我們提出了一種軌跡監控策略，同時進行換道軌跡規划和監控軌跡規划，保證車輛換道的穩定性和可行性。在前者未能規划出軌跡或者規划超時，則系統將以監控軌跡作為輸出。為滿足車輛運動約束以及實時性要求，將采用基於B樣條的方法來規划連續曲率路徑。基於RRT的方法可以作為保持算法完備性的補充。然后，監控軌跡主要遵循無碰撞要求，通過減速規划以保持車輛實時運動的穩定性。結果表明，基於B樣條的方法和基於RRT的方法都可以產生曲率連續路徑並滿足運動限制，但兩者都有可能超時。特別是，隨着環境變得更加復雜，成功率也面臨挑戰。
1. 引言
近幾十年來，自動駕駛技術作為一種前景廣闊的技術取得了良好的發展，並已經有大量的技術產品投入應用[1-3]。 與人類相比，智能駕駛系統由於其快速操作，超出視覺范圍的感知和准確的決策，有望提高駕駛安全性，舒適性，交通效率和能源經濟性[4-6]。
為了安全快速地將智能車輛從當前位置導航到目標點，必須設計一種用於生成軌跡的魯棒性高的運動規划方法[7]。如文獻 [1-2]所述，運動規划架構主要包括基於圖搜索的規划方法，基於采樣的方法，插值曲線規划法和數值優化法。Dijkstra [8]和A * [9]是基於圖表搜索的較常見的規划方法。先將環境離散化之后，通過遍歷網格找到最短路徑，無論網格是相同大小[10]還是可變粒度[11]。盡管智能車中使用的算法有很多變種，如D * [12]，ARA * [13]，AD* [14]，但仍然很難選擇網格分辨率並跟蹤由連接網格組成的路徑[15]。與離散的環境方法不同，基於采樣的規划方法（如RRT [16]）可以通過在連續空間中擴展節點直到達到目標位置來產生有效路徑。應用於智能汽車的算法的最新變體是RRT * [17]和CL-RRT [18]，但是，此類規划的結果不是最優的，生成的路徑曲折並且由於隨機采樣的原因，使得車輛難以直接進行軌跡跟蹤。插值曲線規划方法能夠實現曲率連續性，如多項式曲線[19]，回旋曲線[20]，貝塞爾曲線[21]和B樣條[22]，它需要在一組給定的路徑點基礎上來擬合出一條新的路徑。由於自動駕駛車輛的場景是實時動態的，因此難以遍歷所有環境以獲得路徑點。通過構建障礙約束，起點約束，目標點約束，模型約束和優化目標函數（例如，最短路徑和最舒適等約束），數值優化[23-25]可以在沒有路點的情況下規划最佳平滑軌跡。然而，這種方法本質上是一種具有多個非線性約束的高維優化問題，需要大量資源才能找到正確的解決方案。文獻[26-27]表明，為了產生滿足多目標約束的泊車軌跡，CPU時間通常不小於300ms，甚至40000ms。此外，還有規划失效的可能性[15]。
為了提高運動規划算法的魯棒性，避免無路可走的困境，我們提出了一種軌跡監控策略，同時產生換道軌跡和監控軌跡。如果前者沒有產生安全軌跡或超時，則監控軌跡將作為結果輸出。否則，輸出是換道軌跡。為了滿足車輛運動的約束和實時規划的要求，將采用基於B樣條的方法來規划車道變換的連續曲率路徑。並且基於RRT的方法作為保持車道變換計划算法的完整性的補充。然后，監控軌跡主要遵循無碰撞要求，通過減速規划以保持車輛穩定性。本文的其余部分結構如下：在第二部分，描述了一種軌跡監控的策略。第三部分描述了基於B樣條的換道路徑規划方法。第四部分描述了基於RRT算法的換道軌跡規划方法。第五部分描述了速度規划方法。第六部分實現並分析了典型車道變換場景的軌跡規划算法。最后，第七部分介紹了本文的結論。
通常情況下，難以准確地掌握周圍車輛的運動軌跡，這意味着自動駕駛車輛的換道軌跡並不總是容易快速獲得。而且，運動規划的本質是一種具有多個非線性約束的高維優化問題，需要大量的資源才能找到正確的解。因此，規划並不是總能夠得到正確的路徑。為了避免碰撞，有必要建立一個安全策略，就像車輛組件的功能安全一樣，可以在必要時提供最安全的軌跡。因此，我們提出了一種軌跡監控策略，如圖1所示，用於車道變換場景。如果駕駛任務保持不變且當前軌跡安全，則應保持當前軌跡。相反，換道軌跡和監控軌跡將同時開始規划。如圖2所示，通過基於B樣條和基於RRT的方法生成車道變換軌跡（No.2的淺藍色曲線），並且在減速時生成監視軌跡（No.3的黃線） 。如果前者沒有生成安全軌跡或超時，則監監控軌跡將作為結果輸出。否則，輸出的是換道軌跡。
圖1 軌跡監控策略
圖2 換道場景1
盡管監控軌跡具有與AEB類似的效果，但它比AEB更“智能”。如圖3所示，當車輛改變車道時，路邊防撞樁進入車輛感知區域（最短距離將小於車道寬度），其已達到觸發AEB的條件。但是，車輛執行的車道變換軌跡應該是安全的，並且AEB不應該介入。
圖3 換道場景2
如果監控軌跡的減速度為αx，則為直線行駛
其中αmin是車輛最小加速度，V0是初始速度，Vg是目標速度，L0g是減速長度。
如果轉彎半徑為R，對於直線駕駛而言
其中μ是路面附着系數。
2. 基於B樣條曲線的換道軌跡規划方法
A. 問題定義
一般來說，路徑規划問題可以被定義為典型的兩點邊值問題，
其中f(x)是目標函數，通常用最短時間或者路程來描述代價，gi(x)代表起始點和目標點的約束，hj(x)代表安全距離約束。
B.曲率約束
為了滿足車輛轉向執行器的限制，生成的路徑應該滿足連續曲率，峰值曲率和最大曲率的約束，采用參數化的三次B樣條曲線來規划路徑，如圖4所示。
圖4 三次B樣條曲線的控制點
圖4中具有4個控制點
和7個參數節點
的三次B樣條曲線能夠被定義為
其中Bi,j代表B樣條曲線的基函數，可以通過deBoox-Cox公式遞推得到。
為了滿足起點和目標點的約束，頂部和底部的端點需要設置為三個節點，因此參數節點的矢量將設置為[0,0,0,0,0.5,1,1,1,1], 然后，通過等式（6）分別描述通過三次B樣條生成的路徑點的表達式。
曲率表達式如式（7），
由控制點構成的控制線段如圖4所示。為了滿足車輛的峰值曲率約束，控制線的長度和夾角受式（8）[28]的約束。
C.安全距離約束
基於b樣條曲線的凸特征，可以通過控制段與障礙物之間的距離來表示生成路徑的安全檢測。典型情況如圖5所示。
圖5 安全檢測
根據自我車輛的位置，存在三個階段發生碰撞風險，其中需滿足各自的安全約束條件。假設自動車輛的避障是通過強制轉向而不是改變速度來實現的，其目的在於使乘客感到舒適。 因此，詳細安全性檢測如下：
⑴第一個控制段L0的縱向安全距離
⑵第二個控制段L1的橫向安全距離
⑶第三個控制段L2的橫向安全距離
其中，lfba是自車從后軸到頭部的長度，w是自車寬度，Δlog是橫向安全距離，Vego是自車速度。
最后，路徑規划這樣一個兩點邊值問題可以被定義為;
其中
代表起始點，
代表目標點。
公式（1）和（10）說明智能車輛的路徑規划是具有多個約束的線性規划問題。但是，約束主要是非線性的。處理這類問題主要有三種方法：離線數據集方法，凸優化和仿生智能優化算法。由於智能車輛的環境難以窮盡，許多離線數據存儲，提取和匹配需要大量的計算資源，離線數據集方法的應用受到限制。由於凸優化方法要求域是凸的，環境中的所有障礙必須用廣義仿射集的形式表示，它加劇了環境地圖的處理要求，使其難以應用於高速動態環境中。與上述求解方法相比，仿生智能優化算法由於對定義域，函數組成和優化函數的環境描述要求較低，非常適合解決高維非線性優化問題，並已廣泛應用於運動規划中[29]。經過不斷的嘗試，我們最終選擇了蟻群算法來解決路徑規划問題。
由於由公式（10）建立的優化問題嚴格地限制了曲率，並且蟻群解算法的局部最小值問題使得算法失去其完備性。因此，當基於B樣條的算法沒有解決方案但路徑存在時，添加基於RRT的算法以盡可能地找到路徑。當然，為了加快算法的求解速度，引入了一些措施來減少采樣的盲目性，如高斯采樣，自適應采樣空間和最近鄰搜索策略。同時，為了解決原始RRT規划結果包含大量曲折路徑導致曲率不連續的問題，采用樣條曲線的平滑處理方法使規划結果能夠由智能車輛執行。
A.高斯采樣策略
隨機采樣函數是基本RRT的傳統采樣方法，這意味着配置空間中每個點的擴展概率相等。 但是，考慮到汽車最小轉彎半徑的限制，機車前方的擴展概率應高於后方。因此，高斯采樣將與實際駕駛行為更加一致。如圖6所示，采樣節點位於扇形的陰影區域。
圖6 高斯采樣概率分配
根據公式：
其中（Sx,Sy）是采樣節點，（X0,Y0）是參考節點，（r,θ）是高斯參數，可通過式(14)得到。
其中（r0,θ0）是偏移參數，（σr,σθ）代表高斯標准差，（rrand,θrand）是隨機參數。
B.自適應采樣空
采樣空間越小，到目標點的擴展越快。但是，這可能使得無法在復雜環境中擴展到目標點。為了提高算法對環境的適應性，在公式（15）中定義的偏移角θ中引入成功因子ε(ε>0)。
成功因子 可以根據環境的復雜性，以及先前擴展的成功率和擴展的數量自適應調整。公式（15）采用了近似的方法，因為最大特征空間是矩形圖而不是360度圓。
圖7 自適應采樣空間
C.最近鄰域搜索方法
放入RRT樹的新節點應該在采樣后鏈接到RRT路徑樹中最近的節點。通過鄰域搜索算法尋找最近的節點，並使用經典的歐幾里德距離作為度量。然而，由於車輛最大轉向角的約束，兩個節點之間的轉向角也應被視為較小的角度，意味着更平滑的路徑。如圖8所示，根據歐幾里德距離，節點n1是新采樣節點n的最近節點。然而，從節點 到節點n1的轉向角更大，導致路徑s1連接 和n1更為尖銳。為了使路徑更利於車輛跟蹤，由式（16）定義的傳統度量函數被修改為式（17）。
圖8 度量示意圖
4. 速度規划
在路徑生成后，應根據邊界約束（如起始速度和目標速度），執行器約束（如最大速度和最大加速度）和穩定性約束（最大側向加速度）計算速度曲線，如下所示：
利用三次多項式曲線描述速度曲線：
首先，基於起始速度和目標速度，速度分布如圖9（a）所示。其次，根據圖9（b）所示的最大側向加速度限制（虛線），可以得到如圖9（c）所示的速度曲線。最后，結合加速度極限，可以得到最終速度曲線，如圖9（d）所示。
圖9 速度規划
5. 實驗結果和分析
模擬在C ++ 11 / Linux中進行，並在Jetson TX2上執行，Jetson TX2在HMP Dual Denver 2/2 MB L2+QuadARM®A57/ 2 MB L2上運行。車輛參數列於表I.
表1 車輛參數
A.基於B樣條曲線的換道軌跡規划
車道變換的場景1如圖10所示，起點為（0,0,0,15），目標點為（25,3.5,0,10）。靜止障礙物位於（20,0.7），長4.5米，寬1.4米。軌跡的曲率是連續的，如圖11所示，其最大值為0.05729 / m（小於0.08 / m的限制），曲率率小於0.02 / m2，如圖12所示。
圖10 換道方案1
圖11 換道方案1規划路徑曲率
圖12 換道方案1規划路徑曲率變化率
如圖13所示，速度分布是連續的，最小加速度小於2.20 m / s2（滿足小於4.9 m / s2的限制）。為了更好地分析計划的時間性能，我們總共進行了1000次滾動計算。雖然軌跡生成的平均持續時間是59.1ms（小於我們100ms的目標），但是仍然有足夠的時間超過100ms，甚至高達250ms，如圖15所示。並且如圖16所示，生成持續時間的概率為9％超過100ms。但車道變換的所有輪廓意味着基於B樣條的方法可以產生連續的曲率並滿足運動的限制。
圖13 換道方案1速度曲線
圖14 換道方案1加速度曲線
圖15 換道方案1的規划時間
圖16 換道方案1的規划時間分布
B. 基於RRT的換道軌跡規划
車道變換的場景2如圖17所示，起點為（0,0,0,15），目標點為（25,3.5,0,10）。RRT的停止標准是 並且最大迭代次數不超過1500.靜態障礙物位於（17,0.7），長度為4.5m，寬度為1.4m。
圖17 換道場景2
圖18 換道方案2規划路徑曲率
圖19 換道方案2規划路徑曲率變化率
圖20 換道方案2的速度規划
圖21 換道方案2的加速度曲線
圖22 換道方案2的規划時間
圖23 換道方案2的成功規划時間
如圖18所示，軌跡的曲率是連續的，其最大值為0.050 / m（小於0.08 / m的限制），曲率率小於0.033 / m2，如圖19所示。如圖20所示，速度分布是連續的，最小加速度非常接近4.9 m / s2的限制。如圖22所示，所有軌跡生成的平均持續時間為139.99ms（超過我們100ms的目標），並且有足夠的時間超過100ms，甚至高達3500ms。如圖23所示，對於成功規划，所有軌跡生成的平均持續時間為143.37ms（超過我們100ms的目標），並且有超過100ms的多種時間，甚至高達3000ms。
為了更好地分析計划的時間性能，我們總共進行了10000次滾動計算。並且如圖24所示，規划時間僅有21％的概率滿足小於100ms的要求。產生小於200ms的軌跡的可能性為92％。但是在規划成功的前提下，時間小於100ms的次數減少到15％，如圖25所示，盡管在200ms內規划完成的概率只有92%，但是成功率達到了91％。此外，所有基於RRT的方法的換道曲線可以產生連續的曲率並滿足運動的限制。
圖24 換道方案2的所有規划時間分布
圖25 換道方案2的成功規划時間分布
圖26 換道方案2的成功率
6結論
為了提高運動規划算法的魯棒性，避免了無路可走的困境，本文提出了一種新的自動駕駛車輛換道軌跡規划方法。我們提出了一種軌跡監控策略，在換到過程中保持穩定性的同時生成換道軌跡和監控軌跡。如果前者沒有規划出安全軌跡或者規划超時，則監控軌跡將作為結果輸出。否則，輸出是換道軌跡。為了滿足車輛運動和實時要求的約束，將采用基於B樣條的方法來規划換道的連續曲率路徑。並且基於RRT的方法作為保證換道規划算法的完備性的補充。然后，監視軌跡主要遵循無碰撞要求，其計算減速度以保持車輛穩定性。結果表明，基於B樣條的方法和基於RRT的方法都可以產生連續曲率並滿足運動限制，但兩者都有可能超時。特別是，隨着環境變得更加復雜，成功率也面臨挑戰。未來的工作將集中在真實的車輛實施和測試上。如果我們努力研究更強大的功能安全策略，收獲可能不小。