自動駕駛車輛主動換道軌跡規劃方法研究

黃輝

（重慶交通大學，機電與車輛工程學院，重慶 400074）

主題詞：自動駕駛車輛 換道軌跡 軌跡采樣 成本優化

1 前言

換道行為作為行駛過程中最棘手、最復雜的動作之一，在自動駕駛中受到廣泛關注。自動駕駛車輛如何在較好地完成換道操作任務的同時，滿足安全性、舒適性、平順性以及實時性要求，已經成為目前研究的熱點問題。近年來，國內外學者及研究機構對自動駕駛車輛的軌跡規劃問題進行深入研究，設計并開發了許多代表性技術。自動駕駛車輛軌跡規劃目前主要分為基于圖搜索、基于數值優化、基于軌跡采樣和插值的方法[1]。基于圖搜索的方法其基本原理是先對環境進行描述，依據相應約束條件限制，在規劃空間中擴展搜索并生成軌跡集，最后通過評價指標生成最優軌跡，其中具有代表性的方法為Dijkstra[2]、A*[3]算法。馬靜等人[4]通過對Dijkstra等基于圖搜索的規劃算法原理及性能進行詳細分析對比，得出各規劃方法的優劣排序，最后運用A*規劃算法進行軌跡規劃仿真試驗。Dijkstra算法環境適用性強，但離散性較大，且計算繁重，而A*算法相對計算輕便，但其啟發規則難以獲取，且隨著規劃空間維度提高，內存占用巨大，無法滿足實時自動駕駛。基于數值優化的方法其基本原理是需要將車輛約束及規劃空間約束結合，并轉化為2次凸優化問題，運用2次最優求解方法，迭代求解其目標函數規劃軌跡，其中具有代表性的方法為人工勢場法、基于模型預測控制的軌跡規劃算法。Ruan Y等人[5]通過人工勢場法原理構建規劃空間中的風險勢場并設計目標函數，通過求解目標函數以規劃最終軌跡。而人工勢場法可能會出現局部最優的情況，算法魯棒性較差。Cardoso等人[6]依據模型預測控制算法的原理設計了局部軌跡規劃系統，用于生成光滑軌跡的同時避障換道。基于軌跡采樣和插值的方法，其基本原理是在規劃空間內進行采樣，依據車輛的狀態信息，對變化中的車輛狀態進行插值，擬合得到相應函數曲線，用于構建車輛行駛軌跡。其中具有代表性的方法有多項式曲線、樣條曲線、貝塞爾曲線。McNaughton等人[7]依據車輛的終點狀態信息進行采樣，使用高階多項式曲線對其進行建模，描述車輛的行駛軌跡。而使用基于軌跡采樣和插值的方法用于車輛軌跡規劃會出現平順性、舒適性較差的缺陷，通常結合數值優化的方法用于彌補。Werling等人[8]采用軌跡采樣與優化相結合的方法在曲線坐標系下規劃軌跡，用于實現高速環境下生成安全、舒適的軌跡，但其方法僅能實現被動避障操作，靈活性較低，且在突發的緊急避障環境變化下會出現局部無解的情況。

為了克服上述方法的缺點，在基于軌跡采樣與優化相結合的方法基礎上，設計了主動換道因子，并構建為成本函數項結合到成本函數中去，在生成舒適、安全、光滑的軌跡同時，可依據主動換道因子的變化，實現主動換道操作。該方法既保證了采樣軌跡的完整性，同時又提高換道軌跡規劃的靈活性。最后基于“行遠”自動駕駛車輛[9]進行實車試驗驗證，試驗結果表明該方法具有合理性和可行性。

2 規劃空間設計

自動駕駛車輛的規劃層接受來自感知層與決策層的環境信息，基于當前環境與車輛狀態規劃出安全、舒適的行駛軌跡。當駕駛需求發生改變或駕駛環境出現變化時，出于安全性或舒適性的考慮，規劃層中的局部路徑規劃系統就會對局部軌跡進行重新規劃，實現主動或被動換道、跟車行駛以及靠邊停車駕駛行為。這里僅考慮典型換道情況，即主動換道與被動換道情況，在1次換道時域內，僅可進行1次換道操作，如圖1所示。

圖1 典型換道場景

局部路徑規劃系統通常是基于笛卡爾坐標系建立的，本研究采用曲線坐標系作為參考坐標系。在結構化道路上，車道的幾何形狀是幫助人類司機規劃軌跡的主要線索，一般情況下，駕駛員會根據車道的幾何形狀行駛。而曲線坐標系通常使用道路中線為參考線，將駕駛軌跡分解為與車道中線幾何形狀相關的2個方向進行規劃，進一步簡化局部軌跡規劃問題，方便分析駕駛軌跡特征指標，優化計算，且具有一定的仿人性。

如笛卡爾坐標系與曲線坐標系轉化關系圖2所示，相對于笛卡爾坐標基準(x,y)，從離線地圖中得到的車道的幾何形狀，建立曲線坐標基準(s,d)[10]。其中s是從參考線Lref的起始點到投影點P的曲線距離,即縱向位移，d為車輛位置到參考線Lref的橫向位移，用如下關系表示笛卡爾坐標系到曲線坐標系之間的映射：坐標系之間的轉化關系為：

圖2 笛卡爾坐標系與曲線坐標系轉化關系

在此坐標系轉換規律的基礎上，將笛卡爾坐標上規劃空間的所有狀態信息轉換為曲線坐標系下的狀態信息。在曲線坐標系下，局部規劃系統可以像人類駕駛員一樣，依據車道來規劃車輛的軌跡，從而在換道軌跡曲線擬合時更加直觀的去設計理解軌跡規劃，簡化處理相關坐標信息的工作，提高效率。

3 主、被動換道軌跡規劃

軌跡規劃算法的主要任務是在較好的跟蹤全局路徑同時保證安全性，生成平滑的換道軌跡。如圖3所示，所研究的換道軌跡規劃系統主要由軌跡采樣、成本函數、軌跡檢查、軌跡尋優4個部分組成。局部軌跡規劃系統首先接受來自定位系統與數字地圖的定位與道路幾何信息，轉化到曲線坐標系下進行軌跡生成與軌跡采樣；然后依據決策層提供的跟蹤要求設計成本函數進行軌跡評價，為每個采樣軌跡分配成本值；接著通過來自感知層的障礙物信息檢測各采樣軌跡是否超過規劃要求的約束，剔除不符合要求的軌跡；最后選擇軌跡成本量最低的軌跡作為最優軌跡輸出，用于控制層進行換道軌跡跟蹤控制。本研究在基于軌跡采樣與成本優化相結合的軌跡規劃方法上進行了改進，在成本函數中加入了主動換道因子，在規劃安全、舒適的軌道同時用于自動駕駛車輛主動換道軌跡規劃，彌補了常規規劃方法中僅能考慮車輛避礙而被動換道的缺陷。

圖3 換道軌跡規劃系統框架

3.1 換道軌跡采樣

在結構化道路上，如果沒有例外，自動駕駛汽車的理想行為是沿著道路行駛，根據環境的變化及需求采取變道和避障操作。在1個換道時域內，1組換道軌跡{L1,L2…Ln}是由對未來運動序列的潛在候選序列的描述生成。根據峰值加速度準則，5次多項式函數比其他路徑函數更具有乘客更好的舒適性優勢[11]，在本研究中使用了5次多項式函數規劃換道軌跡，即在s、d2個方向上以5次多項式函數擬合軌跡，如：

依據最優化Jerk量[12]的原則，s、d2個方向的5次多項式函數系數均可由每個采樣周期T、初始狀態終點狀態計算得到,其中角標0表示采樣周期起始點，角標T表示采樣周期終點：

其中Ci可以分別為或在各采樣周期內，都要進行5次多項式函數參數求解過程，便可得到采樣周期內2個方向上的規劃軌跡，即：

3.2 最優換道軌跡生成

在s、d2個方向上生成適宜的采樣軌跡后，再進行軌跡檢查操作，將不滿足最大速度、最大加速度以及最大曲率的軌跡進行剔除，同時考慮安全避障情況，篩選出避障安全區域外的換道軌跡，以實現被動換道避障的目的。這一方法的缺陷在于局部規劃系統為保證安全性的前提下，可能將舒適性、平順性優的換道軌跡剔除，靈活性較低，且在換道過程中會由于采樣軌跡稀疏的原因，會陷入局部無解的情況。為了克服這一缺陷，本節設計了主動換道因子。在經過剔除篩選過的軌跡之后，定義了合理的成本函數用于評估各采樣軌跡，評估軌跡優劣，同時將主動換道因子設計為成本函數項用于主動換道。下面詳細介紹了成本函數的各部分組成。

考慮舒適性層面，較高的加速度及加速度變化率對乘客舒適性的影響較大，運動軌跡對于時間t的3階微分量表示為Jerk量，用于表示舒適性。為了滿足乘客舒適性需求，分別在s、d2個方向上進行加速度a與Jerk量的成本函數項進行定義：

考慮效率層面，將2個方向上的采樣周期T定義為成本函數項，用于評價軌跡規劃的效率量：

考慮跟蹤精度層面，d方向上偏離曲線坐標系定義的參考線應盡可能小些，s方向上應考慮與決策層輸入的目標車速最為接近，分別以d方向上的橫向偏差與s方向上的速度跟蹤誤差為跟蹤精度評價量設計成本項：

與[6]中基于采樣優化的軌跡規劃方法不同的是，為了實現主動換道的目的，設計了換道距離因子項加入到成本函數中：

當局部規劃層接收到來自決策層主動換道信息時，依據所需的橫向移動距離，換道距離因子隨之改變，采樣軌跡中橫向偏移量與換道距離因子相近的成本價值增加，最終作為最優規劃軌跡輸出。

將上述成本項總結為1個成本向量，同時匹配相應的權重，得到完整的成本函數：

其中ξk為每個成本項的系數權重向量，K為成本函數的最大指數系數，C為成本項向量，f(T)為包含全部成本項的完整成本函數。

4 試驗分析

為了對所研究的換道軌跡規劃方法進行驗證，以C++程序設計語言開發設計了智能駕駛車輛局部軌跡規劃系統，在校園試驗區內將規劃算法部署到“行遠”智能駕駛汽車試驗平臺上進行算法驗證。“行遠”自動駕駛汽車的硬件配置如圖4中所示，試驗車搭載1個雙目攝像頭及2個16線激光雷達，用于組建車輛感知系統；以及采用厘米級的差分GPS用于車輛定位與導航，所有軟件、算法皆部署在車載工控機上以實現車輛的感知、定位、決策、規劃及控制，其硬件設備基本滿足試驗要求。

圖4“行遠”自動駕駛汽車硬件配置

試驗設計在標準雙車道場景上進行，分別進行被動避障換道與主動換道操作規劃，以工控機采集實時局部軌跡規劃情況。局部軌跡規劃系統在2個方向上采用等距采樣方式，每個規劃周期規劃5×12，即共60條規劃軌跡，且每個成本項皆匹配均等權重系數。

圖5 被動換道過程可視化

圖5、6為車載工控機上實時換道過程可視圖，圖7為轉化到車輛坐標系下主、被動換道軌跡規劃情況。基于原軌跡采樣與成本優化方法僅能通過剔除包含障礙物的規劃軌跡來實現被動避障換道，不能保證備選軌跡的完整性，為保證安全性的前提下，導致常規方法僅能選擇次優或次次優軌跡作為換道軌跡輸出。從圖8中可以看出，考慮主動換道因子的換道過程中，橫、縱向的規劃軌跡Jerk量相對與原方法皆有一定程度的降低。綜上所述，通過將主動換道因子考慮進成本函數的方法，可以使車輛在稍前的一段時間內規劃換道軌跡，同時保持規劃軌跡的完整性，且既能實現主、被動換道過程規劃，相對于原方法更具靈活性，換道軌跡更為平緩，乘客舒適性較優。

圖6 主動換道過程可視化

圖7 主、被動換道規劃軌跡

圖8 橫、縱向Jerk量變化

5 結束語

(1)針對自動駕駛汽車的局部軌跡規劃問題，分析了1種在曲線坐標系上基于軌跡采樣與成本優化相結合的方法。首先將定位相關信息轉化在曲線坐標系上，然后通過采樣的方法運用5次多項式函數擬合軌跡，生成備選軌跡集，再剔除包含障礙及不符合安全條件的軌跡，最后基于設計的成本函數對備選軌跡集進行選優，作為最優軌跡輸出。

(2)試驗結果表明，通過將設計的主動換道因子整合到成本函數中，可以彌補原方法僅能用于被動避障換道的缺陷，保證了備選軌跡集的完整性，提高了局部軌跡規劃系統的靈活性。

(3)試驗考慮為校園試驗場環境，相對于復雜的城市道路環境，對算法的實時性、安全性需要提出更高的要求。同時對于成本函數權重分配的問題將作為之后研究的重點，可用于不同場景或個性化駕駛軌跡規劃實現。