黃輝

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

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

1 前言

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

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

2 規(guī)劃空間設(shè)計

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

圖1 典型換道場景

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

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

圖2 笛卡爾坐標系與曲線坐標系轉(zhuǎn)化關(guān)系

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

3 主、被動換道軌跡規(guī)劃

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

圖3 換道軌跡規(guī)劃系統(tǒng)框架

3.1 換道軌跡采樣

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

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

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

3.2 最優(yōu)換道軌跡生成

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

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

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

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

與[6]中基于采樣優(yōu)化的軌跡規(guī)劃方法不同的是，為了實現(xiàn)主動換道的目的，設(shè)計了換道距離因子項加入到成本函數(shù)中：

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

將上述成本項總結(jié)為1個成本向量，同時匹配相應(yīng)的權(quán)重，得到完整的成本函數(shù)：

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

4 試驗分析

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

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

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

圖5 被動換道過程可視化

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

圖6 主動換道過程可視化

圖7 主、被動換道規(guī)劃軌跡

圖8 橫、縱向Jerk量變化

5 結(jié)束語

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

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

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