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

黃輝

（重慶交通大學(xué)，機(jī)電與車輛工程學(xué)院，重慶 400074）

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

1 前言

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

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

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

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

圖1 典型換道場景

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

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

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

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

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

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

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

3.1 換道軌跡采樣

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

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

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

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

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

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

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

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

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

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

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

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

4 試驗(yàn)分析

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

圖4“行遠(yuǎn)”自動駕駛汽車硬件配置

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

圖5 被動換道過程可視化

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

圖6 主動換道過程可視化

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

圖8 橫、縱向Jerk量變化

5 結(jié)束語

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

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

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