黃輝 隗寒冰
(重慶交通大學(xué),重慶 400074)
主題詞:軌跡規(guī)劃 駕駛特征 成本優(yōu)化 逆強(qiáng)化學(xué)習(xí)
自動(dòng)駕駛車(chē)輛需要在換道軌跡規(guī)劃時(shí)反映擬人化需求,以提高行駛平順性及乘坐舒適性,避免換道過(guò)程中引起駕乘人員不適。因此,學(xué)習(xí)駕駛員軌跡特征在自動(dòng)駕駛車(chē)輛換道軌跡設(shè)計(jì)中具有十分重要的現(xiàn)實(shí)意義。
國(guó)內(nèi)外學(xué)者對(duì)車(chē)輛軌跡規(guī)劃問(wèn)題已進(jìn)行了廣泛研究[1]。規(guī)劃方法按照原理可大致分為圖搜索、數(shù)值優(yōu)化、插值法和采樣法。目前,基于軌跡采樣與成本函數(shù)優(yōu)化相結(jié)合的方法作為主流的軌跡規(guī)劃方法被廣泛應(yīng)用于自動(dòng)駕駛車(chē)輛。如M.Werling等[2]以軌跡采樣與成本函數(shù)優(yōu)化相結(jié)合的方法規(guī)劃軌跡,實(shí)現(xiàn)各換道場(chǎng)景的軌跡生成。該方法規(guī)劃的換道軌跡基本滿足換道行為功能需求,但忽略了駕駛員駕駛行為特征因素[3]。
在換道軌跡規(guī)劃中考慮駕駛員換道特征,即擬人化換道軌跡規(guī)劃,是近年來(lái)研究的熱點(diǎn)。如黃晶[4]等運(yùn)用K均值聚類(lèi)法將駕駛風(fēng)格分類(lèi),結(jié)合舒適性約束及換道行為數(shù)據(jù)進(jìn)行不同駕駛風(fēng)格換道軌跡規(guī)劃,但忽略了軌跡曲線特征。Schnelle 等[5]提出了組合駕駛員模型和確定駕駛員期望路徑的方法,對(duì)駕駛員模型和所需路徑參數(shù)進(jìn)行優(yōu)化,僅考慮了駕駛員的橫向控制,無(wú)法反映駕駛員的速度特征及行駛軌跡曲率特征。X.He 等[6]提出從自然駕駛數(shù)據(jù)中學(xué)習(xí)仿人軌跡規(guī)劃方法,以softmax函數(shù)建立軌跡的概率模型,但該方法存在奇異值問(wèn)題與學(xué)習(xí)特征上的局限。
機(jī)器學(xué)習(xí)方法被用來(lái)從人類(lèi)演示的數(shù)據(jù)中學(xué)習(xí)成本函數(shù)或參數(shù)設(shè)置,研究成果同樣應(yīng)用于自動(dòng)駕駛。Abbeel等[7]通過(guò)逆強(qiáng)化學(xué)習(xí)策略學(xué)習(xí)駕駛特征。Ziebart等[8]為了解決逆強(qiáng)化學(xué)習(xí)中存在的奇異值問(wèn)題,將逆強(qiáng)化學(xué)習(xí)擴(kuò)展到基于最大熵的方法。但這些研究都限于全局路徑規(guī)劃問(wèn)題,而且更偏向于駕駛員路徑選擇的離散空間問(wèn)題。Kuderer 等[9]運(yùn)用最大熵逆強(qiáng)化學(xué)習(xí)策略學(xué)習(xí)軌跡特征后應(yīng)用到局部軌跡規(guī)劃中,在學(xué)習(xí)過(guò)程中通過(guò)最大似然求解二維空間中六自由度無(wú)窮軌跡的最大熵模型,該方法存在計(jì)算量龐大的弊端。
為生成反映駕駛員特征的換道軌跡,本文開(kāi)展基于最大熵逆強(qiáng)化學(xué)習(xí)策略的換道軌跡特征學(xué)習(xí)方法研究,以軌跡采樣方法為基礎(chǔ),對(duì)學(xué)習(xí)過(guò)程中的計(jì)算進(jìn)行簡(jiǎn)化,通過(guò)軌跡采樣與成本優(yōu)化相結(jié)合的方式規(guī)劃換道軌跡。
軌跡采樣與成本優(yōu)化相結(jié)合的方法主要思路是依據(jù)設(shè)計(jì)的成本函數(shù)篩選采樣軌跡以生成最優(yōu)軌跡。為了平衡軌跡規(guī)劃的效率、舒適性、安全性,成本函數(shù)項(xiàng)權(quán)重需要基于專(zhuān)家經(jīng)驗(yàn)設(shè)計(jì),調(diào)試過(guò)程繁雜,具有一定的局限性。為解決目前存在的弊端,生成與駕駛員換道特征相匹配的軌跡,本文通過(guò)逆強(qiáng)化學(xué)習(xí)方法從駕駛員自然換道軌跡數(shù)據(jù)中迭代學(xué)習(xí)成本函數(shù)權(quán)重,并基于軌跡采樣的方法優(yōu)化學(xué)習(xí)過(guò)程中的迭代計(jì)算,使自動(dòng)駕駛車(chē)輛能夠規(guī)劃出與人類(lèi)駕駛員駕駛特征相近的換道軌跡?;谀鎻?qiáng)化學(xué)習(xí)的換道軌跡規(guī)劃流程如圖1所示。
基于逆強(qiáng)化學(xué)習(xí)的換道軌跡規(guī)劃由軌跡規(guī)劃、駕駛特征建模及逆強(qiáng)化學(xué)習(xí)過(guò)程組成。在某一軌跡規(guī)劃周期τ中,軌跡規(guī)劃部分首先依據(jù)換道規(guī)劃起點(diǎn)、終點(diǎn)的車(chē)輛坐標(biāo)及其關(guān)于時(shí)間的一階微分及二階微分狀態(tài),通過(guò)軌跡采樣的方式,生成備選換道軌跡序列TS=(Ts1,Ts2,Ts3,…,Tsn),駕駛特征建模部分采集駕駛員換道軌跡離線坐標(biāo)點(diǎn),以多項(xiàng)式曲線擬合方式構(gòu)建駕駛員換道軌跡TD=(TD1,TD2,TD3,…,TDn),依據(jù)設(shè)計(jì)的軌跡特征函數(shù)向量f(TD)計(jì)算駕駛員換道軌跡各成本項(xiàng)的經(jīng)驗(yàn)特征值。為解決軌跡分布中存在的奇異值問(wèn)題[8],將軌跡特征函數(shù)向量f(T)與備選軌跡序列TS依據(jù)最大熵原理構(gòu)建軌跡的最大熵模型pMaxEnt,其中T為任意軌跡集輸入,再依據(jù)最大熵模型求解各備選軌跡的期望特征值Ep[f]。通過(guò)梯度變化迭代調(diào)整軌跡特征函數(shù)中的各成本項(xiàng)權(quán)重向量系數(shù)λ,使得經(jīng)驗(yàn)特征值與期望特征值Ep[f]盡可能接近:
圖1 基于逆強(qiáng)化學(xué)習(xí)的換道軌跡規(guī)劃流程
當(dāng)期望特征值Ep[f]與經(jīng)驗(yàn)特征值的差值收斂于最小值時(shí),便得到表征駕駛員軌跡特征的權(quán)重向量λImi。依據(jù)λImi分配各成本函數(shù)項(xiàng)權(quán)重比,重新調(diào)整駕駛員特征成本函數(shù)CImi,最后通過(guò)CImi篩選采樣軌跡序列,以生成與駕駛員換道特征相匹配的換道軌跡。
對(duì)自動(dòng)駕駛車(chē)輛局部軌跡規(guī)劃問(wèn)題進(jìn)行簡(jiǎn)化,同時(shí)便于軌跡特征函數(shù)定義,采用曲線坐標(biāo)系為參考坐標(biāo)系。在結(jié)構(gòu)化道路下進(jìn)行軌跡規(guī)劃時(shí),曲線坐標(biāo)系通常以道路中心線作為參考線,將駕駛軌跡規(guī)劃問(wèn)題分解為沿道路中心線方向及其切線的法向量方向進(jìn)行規(guī)劃處理,圖2所示為笛卡爾坐標(biāo)系與曲線坐標(biāo)系的轉(zhuǎn)化關(guān)系。
圖2 笛卡爾坐標(biāo)系與曲線坐標(biāo)系的轉(zhuǎn)化關(guān)系
不同于笛卡爾坐標(biāo)基準(zhǔn)(x,y),曲線坐標(biāo)系是以車(chē)輛位置與參考線投影點(diǎn)P之間的距離d為縱坐標(biāo),以參考線起始點(diǎn)到投影點(diǎn)P的曲線長(zhǎng)度s為橫坐標(biāo)建立曲線坐標(biāo)系基準(zhǔn)(s,d)[10]。坐標(biāo)系之間的映射關(guān)系與轉(zhuǎn)化關(guān)系表示為:
式中,x為笛卡爾坐標(biāo)系下的車(chē)輛位置坐標(biāo)向量;s(t)、d(t)分別為t時(shí)刻車(chē)輛位置到參考線Lref上投影點(diǎn)的弧長(zhǎng)距離與法向距離;nr(s(t))為投影點(diǎn)的單位法向量;r(s(t))為投影點(diǎn)上的位置向量。
為保證換道軌跡曲率、速度、加速度上的連續(xù)性及軌跡邊界條件的完整性,采用五次多項(xiàng)式曲線擬合換道軌跡。根據(jù)峰值加速度準(zhǔn)則,五次多項(xiàng)式函數(shù)比其他路徑函數(shù)更具舒適性優(yōu)勢(shì)[11]。在任意t時(shí)刻,換道軌跡可以表示為:
式中,a0~a5與b0~b5分為d、s方向上五次多項(xiàng)式函數(shù)的系數(shù)。
當(dāng)換道軌跡的起點(diǎn)、終點(diǎn)狀態(tài)確定時(shí),可依據(jù)五次多項(xiàng)式換道軌跡推導(dǎo)出其一階微分及二階微分狀態(tài)。起點(diǎn)狀態(tài)由換道時(shí)的縱向速度及偏離道路中線距離決定,終點(diǎn)狀態(tài)依據(jù)換道任務(wù)的縱向偏移距離及目標(biāo)車(chē)速定義。在一個(gè)規(guī)劃周期τ中,五次多項(xiàng)式的系數(shù)可由起點(diǎn)、終點(diǎn)狀態(tài)及其一階、二階微分狀態(tài)求解得到,規(guī)劃周期τ由規(guī)劃起點(diǎn)時(shí)刻ts與規(guī)劃終點(diǎn)時(shí)刻te決定。
在每個(gè)規(guī)劃周期中,依據(jù)s、d方向起始狀態(tài)及終點(diǎn)狀態(tài)的采樣,依據(jù)五次多項(xiàng)式函數(shù)便可規(guī)劃出一系列備選換道軌跡序列集TS。
通過(guò)采樣方法生成備選換道軌跡集后,還需定義合理的成本函數(shù)用于備選換道軌跡篩選。本文的研究目標(biāo)是從駕駛員換道軌跡中學(xué)習(xí)駕駛軌跡特征,因此,成本函數(shù)應(yīng)為考慮更加全面且能表征駕駛員特征的成本函數(shù)項(xiàng),同時(shí)為逆強(qiáng)化學(xué)習(xí)過(guò)程需求定義合理的特征函數(shù)??紤]如下成本函數(shù)項(xiàng):
在整個(gè)換道軌跡中,以軌跡曲線函數(shù)在任意一點(diǎn)的三階微分量表征換道過(guò)程中的沖擊度特征J,整個(gè)換道軌跡s、d方向上的沖擊度特征成本CJ定義為:
以軌跡曲線在某一點(diǎn)上的二階微分量表征換道過(guò)程中的加速度特征a,整個(gè)換道軌跡s、d方向上的加速度特征成本Ca定義為:
以換道軌跡曲線上某一點(diǎn)處的曲率為曲率特征κ,整個(gè)換道軌跡上的曲率特征成本Cκ定義為:
以換道過(guò)程中s方向?qū)嶋H速度與期望速度偏差量vdev定義特征成本Cvdev:
以換道軌跡偏離道路中線Lref(τ)的距離ddev定義特征成本Cddev:
通過(guò)將設(shè)計(jì)的各成本項(xiàng)整合,同時(shí)分配權(quán)重系數(shù)便構(gòu)成完整的成本函數(shù)COpt:
式中,λ N為每個(gè)成本項(xiàng)的系數(shù)權(quán)重向量;N為成本函數(shù)的最大指數(shù)系數(shù);C=(CJ(s(t)),CJ(d(t)),Ca(s(t)),Ca(d(t)),Cκ(s(t),d(t)),Cvdev(s(t)),Cddev(d(t)))為成本項(xiàng)向量。
本文通過(guò)逆強(qiáng)化學(xué)習(xí)的方法學(xué)習(xí)得到權(quán)重向量系數(shù)λ,實(shí)現(xiàn)各成本項(xiàng)還原駕駛員特性分配。依據(jù)設(shè)計(jì)的成本函數(shù)定義逆強(qiáng)化學(xué)習(xí)中的特征函數(shù)向量f:
式中,fJ(s(t))、fJ(d(t))分別為s、d方向上的沖擊度特征函數(shù);fa(s(t))、fa(d(t))分別為s、d方向上的加速特征函數(shù);fκ(s(t),d(t))為s、d方向上的曲率特征函數(shù);fvdev(s(t))為s方向上的速度偏差特征函數(shù);fddev(d(t))為d方向上的距離偏差特征函數(shù)。
依據(jù)駕駛員多次自由換道情況下的離線換道軌跡點(diǎn),擬合出一系列換道軌跡曲線TD=(TD1,TD2,TD3,…,TDn),依據(jù)特征函數(shù)向量f計(jì)算出逆強(qiáng)化學(xué)習(xí)過(guò)程中駕駛員軌跡經(jīng)驗(yàn)特征值:
逆強(qiáng)化學(xué)習(xí)的目的是生成與演示結(jié)果相似的軌跡,以特征值為相似性的評(píng)判度量。逆強(qiáng)化學(xué)習(xí)的關(guān)鍵是找到預(yù)期的參數(shù)λ使得期望特征與觀察到的經(jīng)驗(yàn)特征相匹配。以采樣軌跡為基礎(chǔ),構(gòu)建軌跡的最大熵概率模型pMaxEnt,優(yōu)化問(wèn)題的最大熵模型有如下形式:
式中,Z(λ)為滿足歸一化條件的正規(guī)化因子,歸一化條件為:
將λTf(TS)解釋為成本函數(shù),與軌跡規(guī)劃部分的成本函數(shù)COpt相對(duì)應(yīng),可以理解為駕駛員更有可能選擇成本較低的軌跡進(jìn)行換道軌跡規(guī)劃。權(quán)重向量參數(shù)λ的解析解通常無(wú)法計(jì)算,但可以計(jì)算關(guān)于λ的拉格朗日函數(shù)的梯度,該梯度是期望值與經(jīng)驗(yàn)特征值的差值:
通過(guò)對(duì)演示軌跡TD求對(duì)數(shù)似然函數(shù)得到:
求其關(guān)于λ的一階微分為:
式中,pλ(T)為λ權(quán)重下關(guān)于任意軌跡輸入的概率。
通過(guò)基于采樣的軌跡生成方法,可以簡(jiǎn)捷地計(jì)算出期望特征值:
選擇合適的采樣軌跡數(shù)量可以優(yōu)化逆強(qiáng)化學(xué)習(xí)過(guò)程中的計(jì)算量。基于特征匹配的最大熵分布問(wèn)題等價(jià)于假設(shè)指數(shù)族分布時(shí)訓(xùn)練數(shù)據(jù)的最大似然問(wèn)題,從而將目標(biāo)轉(zhuǎn)化為找到符合預(yù)期的一組演示的特征分布。找到期望的分布意味著計(jì)算參數(shù)權(quán)重向量λ,使得期望特征值與經(jīng)驗(yàn)特征值相匹配。參數(shù)向量λ無(wú)法求解,但可以根據(jù)這些參數(shù)計(jì)算梯度,從而應(yīng)用基于梯度的優(yōu)化。梯度調(diào)整優(yōu)化過(guò)程通過(guò)計(jì)算特征函數(shù)的全微分,同時(shí)控制步長(zhǎng)使函數(shù)到達(dá)極小值。對(duì)于部分特征項(xiàng)不可解析的計(jì)算梯度,可采用數(shù)值積分及解析導(dǎo)數(shù)相結(jié)合的方式計(jì)算。曲率特征梯度的近似求解公式為:
式中,Δt為在規(guī)劃周期τ內(nèi)的采樣周期。
基于逆強(qiáng)化學(xué)習(xí)方法學(xué)習(xí)駕駛員換道軌跡特征的權(quán)重分配算法流程如圖3所示。
通過(guò)逆強(qiáng)化學(xué)習(xí)過(guò)程學(xué)習(xí)到權(quán)重向量λ后,依據(jù)λ為成本函數(shù)COpt各成本項(xiàng)分配權(quán)重篩選備選軌跡,便實(shí)現(xiàn)了包含駕駛員特征的換道軌跡生成。
為驗(yàn)證本文提出的基于逆強(qiáng)化學(xué)習(xí)的換道軌跡規(guī)劃的有效性,在“行遠(yuǎn)”自動(dòng)駕駛車(chē)輛試驗(yàn)平臺(tái)[12]上開(kāi)展試驗(yàn),分別執(zhí)行駕駛員換道軌跡點(diǎn)采集與實(shí)時(shí)換道軌跡規(guī)劃及跟蹤控制任務(wù)。駕駛員換道軌跡點(diǎn)處理及逆強(qiáng)化學(xué)習(xí)過(guò)程均離線完成。試驗(yàn)平臺(tái)硬件配置如圖4 所示:感知系統(tǒng)由雙目攝像頭及激光雷達(dá)構(gòu)成;定位系統(tǒng)配置差分GPS用于實(shí)現(xiàn)厘米級(jí)車(chē)輛定位與導(dǎo)航;所有算法部署在車(chē)載工控機(jī)上以實(shí)現(xiàn)車(chē)輛的感知、定位、決策、規(guī)劃及控制。
圖3 學(xué)習(xí)駕駛員換道特征分配權(quán)重算法流程
圖4 “行遠(yuǎn)”自動(dòng)駕駛車(chē)輛試驗(yàn)平臺(tái)
以重慶交通大學(xué)校園試驗(yàn)區(qū)作為試驗(yàn)場(chǎng)景分別進(jìn)行駕駛員1、駕駛員2 換道軌跡點(diǎn)的采集工作。試驗(yàn)場(chǎng)地如圖5a 所示,其為標(biāo)準(zhǔn)單向雙車(chē)道場(chǎng)地。駕駛員進(jìn)行自由換道操作,僅收集駕駛員從右向左的換道軌跡,且駕駛員每次換道僅跨越1 個(gè)車(chē)道位置??紤]校園試驗(yàn)場(chǎng)場(chǎng)景,規(guī)定駕駛員理想換道車(chē)輛速度為35 km/h。為了提高軌跡的隨機(jī)性,不預(yù)先告知駕駛員采集任務(wù),僅告知其需控制車(chē)速在35 km/h附近自由換道操作。
共采集到2名駕駛員有效換道軌跡20條,平滑濾波處理后駕駛員換道軌跡如圖5b、圖5d所示。2名駕駛員均具有3~5 年駕齡,能夠穩(wěn)定執(zhí)行換道操作,依據(jù)換道軌跡集邊界定義駕駛員換道區(qū)域如圖5c、圖5e 所示。在曲線坐標(biāo)系下,依據(jù)換道軌跡點(diǎn)橫、縱坐標(biāo)以及每個(gè)軌跡點(diǎn)的時(shí)間序列,擬合s、d方向上的五次多項(xiàng)式函數(shù),并依據(jù)式(11)計(jì)算出駕駛員各換道特征經(jīng)驗(yàn)特征值,統(tǒng)計(jì)結(jié)果如表1所示。相比于駕駛員換道軌跡縱向相關(guān)特征,橫向相關(guān)特征更為顯著。
圖5 校園試驗(yàn)場(chǎng)及駕駛員換道軌跡
將駕駛員換道軌跡經(jīng)驗(yàn)特征值代入逆強(qiáng)化學(xué)習(xí)過(guò)程中進(jìn)行迭代學(xué)習(xí),如圖6 所示,基于梯度下降的權(quán)重系數(shù)調(diào)整策略導(dǎo)致特征值差值曲線出現(xiàn)微小振幅,而非單調(diào)下降。在經(jīng)過(guò)接近25次迭代后,2位駕駛員的經(jīng)驗(yàn)特征值向量與期望特征值向量的差值基本收斂。由于軌跡規(guī)劃基于采樣的方式完成,僅能在軌跡集中篩選出最接近駕駛特征的某條軌跡,導(dǎo)致特征差值不為零。依據(jù)學(xué)習(xí)得到的權(quán)重向量λ,對(duì)規(guī)劃系統(tǒng)的成本函數(shù)權(quán)重進(jìn)行重新分配調(diào)整,依據(jù)調(diào)整更新后的成本函數(shù)篩選實(shí)時(shí)規(guī)劃的備選軌跡。
表1 駕駛員換道特征值
圖6 特征值差值迭代過(guò)程
圖7所示為成本權(quán)重更新前、后實(shí)時(shí)軌跡規(guī)劃對(duì)比情況。從圖7中可以看出:未調(diào)整權(quán)重情況下實(shí)時(shí)車(chē)輛備選規(guī)劃軌跡序列大部分偏離2 位駕駛員的換道軌跡區(qū)域;學(xué)習(xí)駕駛員1 換道特征后,車(chē)輛實(shí)時(shí)規(guī)劃的換道軌跡基本包含在駕駛員1換道區(qū)域內(nèi),部分偏離駕駛員2換道區(qū)域;學(xué)習(xí)駕駛員2換道特征后,車(chē)輛規(guī)劃的換道軌跡基本包含在駕駛員2換道軌跡區(qū)域內(nèi)。
如圖8、圖9 所示,將成本權(quán)重更新前、后換道軌跡特征對(duì)比情況以百分率形式表現(xiàn),即以駕駛員特征為基準(zhǔn)。自動(dòng)駕駛車(chē)輛局部軌跡規(guī)劃系統(tǒng)在均等權(quán)重比下,s方向上的特征偏離基準(zhǔn)較小,能較好地匹配,而在d方向上,調(diào)整前特征偏離基準(zhǔn)較大。學(xué)習(xí)駕駛員特征后,特征偏離情況降低,向駕駛員特征靠近。對(duì)比調(diào)整前、后,表征乘員舒適度的縱向特征J與加速度特征a指標(biāo)明顯降低,且趨近于人類(lèi)駕駛員換道操作水平,改善了換道過(guò)程中的乘員舒適性體驗(yàn)。
將駕駛員換道軌跡曲線及換道特征進(jìn)行對(duì)比,結(jié)合成本權(quán)重更新前、后的試驗(yàn)結(jié)果可以發(fā)現(xiàn):在均等成本函數(shù)權(quán)重下生成的軌跡部分偏離駕駛員換道軌跡區(qū)域,相對(duì)于駕駛員換道軌跡曲線,曲率變換更為劇烈,換道軌跡曲線特征除縱向特征外,較駕駛員各特征相差較遠(yuǎn);通過(guò)逆強(qiáng)化學(xué)習(xí)方法更新成本權(quán)重后的規(guī)劃軌跡基本包含在駕駛員換道軌跡內(nèi),換道軌跡曲線各特征與駕駛員特征相近,更能反映駕駛員主觀感受。
圖7 成本權(quán)重更新前、后軌跡規(guī)劃對(duì)比
圖8 學(xué)習(xí)駕駛員1特征對(duì)比
圖9 學(xué)習(xí)駕駛員2特征對(duì)比
本文采用軌跡采樣及成本優(yōu)化相結(jié)合的軌跡規(guī)劃方法設(shè)計(jì)了自動(dòng)駕駛車(chē)輛軌跡規(guī)劃算法??紤]換道規(guī)劃過(guò)程中的駕駛員主觀感受,運(yùn)用最大熵逆強(qiáng)化學(xué)習(xí)策略從駕駛員換道軌跡中學(xué)習(xí)軌跡特征分配,并將學(xué)習(xí)得到的權(quán)重向量用于成本函數(shù)權(quán)重分配,依據(jù)更新后的成本函數(shù)在采樣軌跡中篩選匹配,最終生成與駕駛員換道軌跡特征相近的換道軌跡。試驗(yàn)結(jié)果表明,本文開(kāi)發(fā)的基于逆強(qiáng)化學(xué)習(xí)的換道軌跡能較好地包含在駕駛員換道區(qū)域內(nèi),且軌跡特征與駕駛員軌跡特征相近,即滿足擬人化駕駛需求。