楊璐寧, 劉正華, 溫 暖

(北京航空航天大學(xué)自動(dòng)化科學(xué)與電氣工程學(xué)院, 北京 100191)

0 引 言

隨著無人駕駛車輛的研究不斷深化,無人駕駛車輛對(duì)導(dǎo)航系統(tǒng)的要求逐漸嚴(yán)格[1]。其中,應(yīng)用最廣泛的導(dǎo)航系統(tǒng)為捷聯(lián)慣性導(dǎo)航系統(tǒng)(strapdown inertial navigation system, SINS)和全球衛(wèi)星定位系統(tǒng)(global positioning system, GPS)組成的組合導(dǎo)航系統(tǒng)[2]。在良好的工作環(huán)境下SINS/GPS組合導(dǎo)航能提高準(zhǔn)確的定位信息[3],但在山洞、隧道等惡劣環(huán)境下,GPS信號(hào)將受到影響[4],基于慣導(dǎo)、衛(wèi)導(dǎo)的組合導(dǎo)航方式退化為純慣導(dǎo)系統(tǒng)[5],致使導(dǎo)航誤差累積,定位信息發(fā)散[6],無法提供高精度的導(dǎo)航信號(hào)[7]。因此,為了保證在GPS中斷期間的導(dǎo)航可靠性,研究人員針對(duì)多元未知環(huán)境中的導(dǎo)航問題展開探索[8]。

為了獲得高性能的導(dǎo)航系統(tǒng),將傳統(tǒng)線性濾波估計(jì)器轉(zhuǎn)化為無跡卡爾曼濾波(unscented Kalman filter, UKF)估計(jì)器進(jìn)行組合導(dǎo)航[9],防止卡爾曼估計(jì)出現(xiàn)的較大偏差[10]。同時(shí),為了克服濾波器的發(fā)散性,Geiger等應(yīng)用自適應(yīng)卡爾曼濾波提高組合導(dǎo)航精度[11]?；趥鞲衅髟紨?shù)據(jù),可擴(kuò)展性強(qiáng),但不能從根本解決問題[12]。隨著人工智能的發(fā)展,Zhang提出了一種多層感知機(jī)提供偽GPS信號(hào)的求解方式[13]。但由于只利用末時(shí)刻導(dǎo)航信息造成導(dǎo)航信息浪費(fèi)現(xiàn)象[14],基于神經(jīng)網(wǎng)絡(luò)時(shí)間序列預(yù)測(cè)技術(shù)應(yīng)運(yùn)而生[15]。Fu等設(shè)計(jì)了一種混合遞歸神經(jīng)網(wǎng)絡(luò)框架估計(jì)無人船的位置[16]。然而,由于在訓(xùn)練期間引入額外輸入歷史數(shù)據(jù),模型變得更加復(fù)雜[17]。

高斯過程(Gaussian progress, GP)是一種典型的非參數(shù)貝葉斯模型[18],可將輸入空間之間的相關(guān)性投影到輸出空間來估計(jì)給定任意輸入的輸出均值和協(xié)方差[19],故GP在預(yù)測(cè)問題上引起廣泛關(guān)注[20]。Chen等[21]提出了一種GP/空間網(wǎng)格正交法(spare grid quadrature,SGQ)-卡爾曼濾波(Kalman filter,KF)的方法來處理測(cè)量中斷。但由于GP模型的能力受到核函數(shù)表達(dá)能力的限制[22],在GPS長(zhǎng)期中斷的情況下,GP模型的性能仍有提高的空間[23]。深度GP(deep GP, DGP)是一種基于多元GP的深度模型[24]。與GP不同的是GP只能代表一類有限函數(shù),而DGP是一種多層次模型[25],更適合于復(fù)雜的小樣本數(shù)據(jù)關(guān)系[26]。同時(shí)DGP能自動(dòng)防止過擬合且在輸出上能提供完整的概率分布[27],并對(duì)不確定性進(jìn)行合理量化[28]。簡(jiǎn)言之,DGP保留了GP原有的屬性[29],并克服了其局限性,更適合高度復(fù)雜的數(shù)據(jù)關(guān)系[30]。

本文針對(duì)多元未知環(huán)境中的無人駕駛車輛導(dǎo)航問題,設(shè)計(jì)了一種基于DGP的組合導(dǎo)航軌跡預(yù)測(cè)方法,有效解決了GPS長(zhǎng)時(shí)間失鎖的信息融合問題,極大提高了無人駕駛車輛在多元未知環(huán)境中的導(dǎo)航定位精度。本文的創(chuàng)新點(diǎn)主要包括:

(1) 提出了一種基于非參數(shù)模型DGP的DGP-UKF組合導(dǎo)航框架。采用DGP-UKF框架進(jìn)行軌跡預(yù)測(cè),即使數(shù)據(jù)已知先驗(yàn)信息較少時(shí),DGP能增加模型復(fù)雜程度,針對(duì)于數(shù)據(jù)量較少的情況有效防止過擬合,相對(duì)于深度神經(jīng)網(wǎng)絡(luò)極大提高了預(yù)測(cè)精度。同時(shí),對(duì)于GPS中斷時(shí)間較長(zhǎng)的軌跡預(yù)測(cè),DGP模型預(yù)測(cè)結(jié)果較深度神經(jīng)網(wǎng)絡(luò)誤差更小,具有更好的魯棒性和可靠性。

(2) 針對(duì)傳統(tǒng)深度神經(jīng)網(wǎng)絡(luò)可解釋性弱的問題,概率模型DGP能通過一種變分推理的方法求解出定位位置的置信區(qū)間,同時(shí)給出精確的證據(jù)下界計(jì)算方法,為基于深度學(xué)習(xí)模型的數(shù)據(jù)融合預(yù)測(cè)方法提供了嚴(yán)格的理論推導(dǎo)和可解釋性說明。

1 無人車導(dǎo)航定位建模

基于DGP的無人車多元未知環(huán)境的導(dǎo)航問題,就是在GPS信號(hào)可用時(shí),根據(jù)SINS與GPS信號(hào)之間的關(guān)系進(jìn)行DGP模型訓(xùn)練[31];在GPS信號(hào)失效時(shí),利用網(wǎng)絡(luò)模型對(duì)位置信息進(jìn)行預(yù)測(cè),再將預(yù)測(cè)的偽位置信息與SINS構(gòu)成組合導(dǎo)航模型。故無人車導(dǎo)航定位的建模應(yīng)包括位置解算和組合導(dǎo)航系統(tǒng)位置估計(jì)兩部分。

1.1 位置解算

在“東-北-天”(簡(jiǎn)稱E-N-U坐標(biāo)系)坐標(biāo)系下,無人駕駛車輛的位置計(jì)算如下:

(1)

在地理系的比力可由測(cè)量比力及旋轉(zhuǎn)矩陣計(jì)算得到。

(2)

--------------------

(3)

--------------------

1.2 GPS/INS組合導(dǎo)航位置估計(jì)

構(gòu)建組合導(dǎo)航系統(tǒng),首先建立15維的狀態(tài)方程與觀測(cè)方程為

(4)

式中:X為狀態(tài)向量;F為系統(tǒng)矩陣;G為系統(tǒng)噪聲陣;W為過程噪聲向量;Z為觀測(cè)向量;H為觀測(cè)矩陣;V為觀測(cè)噪聲向量。其中,狀態(tài)變量X選取為

(5)

(6)

(7)

(8)

實(shí)際無人車導(dǎo)航過程中應(yīng)用離散UKF,故將式(4)離散化:

(9)

設(shè)定初始化值為

(10)

根據(jù)無跡變換(unscented transform,UT)變換原理,可得到時(shí)間更新方程為

(11)

無跡卡爾曼量測(cè)更新如下:

(12)

濾波更新方程為

(13)

本節(jié)主要建立了無人車導(dǎo)航定位模型,針對(duì)位置解算和GPS/INS組合導(dǎo)航位置估計(jì)模型應(yīng)確定與車輛位置存在函數(shù)關(guān)系的參量,構(gòu)建非參信息融合GPS信號(hào)預(yù)測(cè)模型,為無人車導(dǎo)航定位提供精準(zhǔn)的導(dǎo)航信號(hào)。

2 GPS信號(hào)預(yù)測(cè)模型設(shè)計(jì)

本節(jié)重點(diǎn)介紹了基于DGP的無人車輛導(dǎo)航系統(tǒng)設(shè)計(jì),首先搭建基于GP的GPS預(yù)測(cè)模型,進(jìn)而將網(wǎng)絡(luò)疊加構(gòu)成DGP完成GPS信號(hào)預(yù)測(cè)網(wǎng)絡(luò)設(shè)計(jì)。

2.1 基于GP的GPS預(yù)測(cè)模型設(shè)計(jì)

(14)

式中:p(f,u;X,Z)表示高斯過程的先驗(yàn)概率;p(yi|fi)為似然函數(shù);p(f|u;X,Z)表示輸入為目標(biāo)點(diǎn)X及誘導(dǎo)點(diǎn)Z的u的條件下f的概率且滿足p(f|u;X,Z)=N(f|μ,Σ);p(u;Z)表示輸入為Z的u的概率且滿足p(u)=N(u|m(Z),k(Z,Z)),其中μ,Σ滿足:

[μ]i=m(xi)+α(xi)T(u-m(Z))

(15)

[Σ]ij=k(xi,xj)-α(xi)Tk(Z,Z)α(xj)

(16)

式中:α(xi)=k(Z,Z)-1k(Z,xi)。

針對(duì)于較大量數(shù)據(jù)時(shí)的高斯過程求解復(fù)雜,故采用一種變分推理法進(jìn)行求解。即通過求解KL(Kullback Leible)散度的最小值,尋找概率q(f,u)近似代替高斯過程的聯(lián)合概率密度p(y,f,u)。同時(shí)根據(jù)等價(jià)性原則,可將求解KL散度的最小值轉(zhuǎn)化為求解邊緣似然函數(shù)證據(jù)下界的最大值為

(17)

p(y,f,u)求解如式(17)所示,變分后驗(yàn)概率q(f,u)表達(dá)式如下:

q(f,u)=p(f|u;X,Z)q(u)

(18)

式中:q(u)滿足高斯分布且q(u)=N(u|m,S)。

由于后驗(yàn)變分的兩項(xiàng)都服從高斯分布,可將u邊緣化得到q(f|m,S;X,Z):

(19)

μm,Z(xi)=m(xi)+αT(xi)(m-m(Z))

(20)

ΣS,Z(xi,xj)=k(xi,xj)-αT(xi)(k(Z,Z)-S)α(xj)

(21)

變分后驗(yàn)邊緣概率fi取決于輸入xi,故可將邊緣概率q(fi|m,S;X,Z)改寫為

q(fi|m,S;X,Z)=q(fi|m,S;xi,Z)=
N(fi|μm,z(xi),ΣS,Z(xi,xi))

(22)

使用變分后驗(yàn)概率式(18)和證據(jù)下界式(17)可得到簡(jiǎn)化的證據(jù)下界:

(23)

2.2 基于DGP的GPS預(yù)測(cè)模型設(shè)計(jì)

為了進(jìn)一步提升GPS預(yù)測(cè)模型精度,彌補(bǔ)單層GP的模型缺陷。在本節(jié)中,將第2.1節(jié)中GP模型進(jìn)行疊加,構(gòu)建多層深度GP,根據(jù)GP的y,f,u的聯(lián)合概率分布式(18),類似地,可以得到DGP的聯(lián)合概率分布:

(24)

式中:l為第l個(gè)GP層。

DGP的層數(shù)增加時(shí),無法通過非線性協(xié)方差函數(shù)求得解析解,假設(shè)層與層之間的后驗(yàn)概率分布是可因式分解的,參考GP中的式(18)變分推理求得的DGP后驗(yàn)概率:

(25)

式中:q(Ul)為高斯分布且滿足q(Ul)=N(ml,Sl)。

類似單層的GP將每一層的誘導(dǎo)變量邊緣化,可以得到在層內(nèi)與層間完全耦類似單層的GP將每一層合的分布。

(26)

類似GP的證據(jù)下界式(17),推斷出深度GP的證據(jù)下界為

(27)

結(jié)合式(24)和式(25),并代入式(27),可得到簡(jiǎn)化的DGP證據(jù)下界為

(28)

總結(jié)GPS信號(hào)預(yù)測(cè)的兩個(gè)模型,繪制GP/DGP-KF算法流程圖如圖1所示。

圖1 GP/DGP-UKF組合導(dǎo)航框架流程圖

3 仿真校驗(yàn)與分析

3.1 數(shù)據(jù)集及實(shí)驗(yàn)環(huán)境

本文首先采用德國卡爾斯魯厄理工學(xué)院和豐田美國技術(shù)研究院聯(lián)合創(chuàng)辦的車輛導(dǎo)航數(shù)據(jù)集KITTI進(jìn)行實(shí)驗(yàn)[11],KITTI數(shù)據(jù)集包含了對(duì)駕駛車輛的GPS信號(hào)及慣性測(cè)量單元(inertial measurement unit, IMU)參數(shù),IMU包括三軸加速度計(jì)和陀螺儀。安裝在車輛中后輪軸中間上方,GPS接收機(jī)天線安裝在無人車輛車頂。GPS/IMU輸出頻率為100 Hz。軟件開發(fā)平臺(tái)為:Python3.7+Pytorch1.10.2。訓(xùn)練硬件平臺(tái)為CPU:i7-7700k。

3.2 GP/DGP/LSTM模型訓(xùn)練

為了進(jìn)一步驗(yàn)證DGP方法在GPS信號(hào)失鎖時(shí)信號(hào)預(yù)測(cè)的優(yōu)越性,本節(jié)利用GPS有效時(shí)的數(shù)據(jù)進(jìn)行訓(xùn)練,利用典型深度神經(jīng)網(wǎng)絡(luò)長(zhǎng)短期記憶(long and short term memory, LSTM)網(wǎng)絡(luò)進(jìn)行對(duì)比實(shí)驗(yàn),同時(shí)構(gòu)建GP和DGP模型進(jìn)行驗(yàn)證實(shí)驗(yàn)。由于IMU頻率較高,時(shí)間迭代次數(shù)較多,網(wǎng)絡(luò)層數(shù)較多會(huì)導(dǎo)致過擬合及訓(xùn)練速度較慢現(xiàn)象發(fā)生,因此將LSTM與DGP網(wǎng)絡(luò)層數(shù)設(shè)置為2。且為了驗(yàn)證DGP的優(yōu)勢(shì),保證所有參數(shù)盡量一致。3種模型的所有網(wǎng)絡(luò)參數(shù)如表1所示。將訓(xùn)練好的GP/DGP及對(duì)比LSTM模型離線保存,調(diào)用模型進(jìn)行在線預(yù)測(cè)驗(yàn)證,模型訓(xùn)練及預(yù)測(cè)的詳細(xì)過程如圖1所示。

表1 3種模型及模型參數(shù)

選取GPS信號(hào)中斷50 s數(shù)據(jù)集進(jìn)行在線測(cè)試驗(yàn)證,假定數(shù)據(jù)集中GPS數(shù)據(jù)中斷50 s,利用GP/DGP及LSTM模型進(jìn)行GPS數(shù)據(jù)在線預(yù)測(cè),并與數(shù)據(jù)集中真實(shí)GPS信號(hào)進(jìn)行比較,其表現(xiàn)結(jié)果如圖2～圖4所示。同時(shí),考慮模型在線預(yù)測(cè)時(shí)間,理論上GP模型在線預(yù)測(cè)運(yùn)算時(shí)間主要來自于式(15)和式(16)中矩陣k(Z,Z)求逆運(yùn)算。由式(19)可知,GP的在線預(yù)測(cè)運(yùn)算時(shí)間復(fù)雜度為O(n3);DGP模型時(shí)間復(fù)雜度為N×O(n3),其中n為誘導(dǎo)點(diǎn)數(shù)目,N為DGP模型深度;實(shí)際中測(cè)量所用硬件GP/DGP更新一次GPS預(yù)測(cè)值所需時(shí)間。由于GP/DGP兩種模型GPS更新速率較快,故測(cè)量更新100次GPS值所耗時(shí)間tGP=0.49 s,tDGP=0.95 s。由于GPS/IMU數(shù)據(jù)輸出頻率為100 Hz,故輸出100次GPS量測(cè)值共需1 s。根據(jù)真實(shí)時(shí)間測(cè)量可知DGP模型的GPS更新頻率高于GPS/IMU量測(cè)頻率,故DGP模型可以在GPS失效時(shí)與IMU量測(cè)信息構(gòu)成DGP-KF框架估計(jì)車輛導(dǎo)航信息。

圖3 GP模型測(cè)試效果圖

圖4 DGP模型測(cè)試效果圖

3.3 LSTM/GP/DGP-KF實(shí)驗(yàn)結(jié)果

將3種模型進(jìn)行保存,累加預(yù)測(cè)的經(jīng)緯度增量,可以得到無人車軌跡的經(jīng)緯度變化值,實(shí)驗(yàn)結(jié)果通過4種方法進(jìn)行對(duì)比分析:① 使用純IMU慣導(dǎo)輸出的數(shù)據(jù);② 使用LSTM-UKF模型進(jìn)行預(yù)測(cè);③ 使用GP-UKF模型進(jìn)行預(yù)測(cè);④ 使用DGP-UKF模型進(jìn)行預(yù)測(cè)。選擇采樣時(shí)間為50 s進(jìn)行軌跡預(yù)測(cè)如圖5所示。

圖5 3種模型預(yù)測(cè)軌跡與真實(shí)軌跡

使用交叉驗(yàn)證的方法重復(fù)實(shí)驗(yàn)5次,取平均值來驗(yàn)證模型最終擬合結(jié)果,將定位誤差值累積來度量比較3種定位方法的模型性能。同時(shí),將包含經(jīng)緯度信息的軌跡值轉(zhuǎn)換成東向和北向誤差如圖6所示。

圖6 3種模型預(yù)測(cè)東北方向誤差

由于DGP誤差較小,故截取部分采樣點(diǎn)觀測(cè)誤差,通過實(shí)驗(yàn)結(jié)果可以看出通過IMU直接解算的誤差值較大,在出發(fā)時(shí)間9 s后誤差率達(dá)到35.34%,相比于由IMU直接解算,采用3種模型可在不同程度上提高整個(gè)駕駛車輛過程中車輛導(dǎo)航定位的準(zhǔn)確性。

通過實(shí)驗(yàn)結(jié)果可以看出,INS直接輸出數(shù)據(jù)在出發(fā)后誤差迅速發(fā)散,相比于直接采用慣性元件結(jié)算出的導(dǎo)航信號(hào)。另外,3種對(duì)GPS信號(hào)進(jìn)行估計(jì)的方法效果更好。LSTM網(wǎng)絡(luò)無法根據(jù)模型擬合的置信區(qū)間來判斷模型的可靠性對(duì)模型參數(shù)進(jìn)行調(diào)整,故在擬合過程中無法保證所有預(yù)測(cè)值均在預(yù)測(cè)位置的置信區(qū)間帶中,模型定位會(huì)產(chǎn)生較大誤差。GP模型較DGP模型簡(jiǎn)單,擬合效果有限,無法達(dá)到最優(yōu)的預(yù)測(cè)效果。DGP網(wǎng)絡(luò)能增加模型復(fù)雜程度,針對(duì)于數(shù)據(jù)量較少的情況有效防止過擬合,同時(shí)能保證在車輛運(yùn)動(dòng)發(fā)生較大變化時(shí)預(yù)測(cè)模型仍在置信區(qū)間中,能有效提高精度,相較于LSTM網(wǎng)絡(luò)定位誤差率降低為40.19%。

選取同一起點(diǎn)的不同GPS信號(hào)中斷時(shí)長(zhǎng)進(jìn)行實(shí)驗(yàn),并將其轉(zhuǎn)化為東北方向的誤差變化其實(shí)驗(yàn)結(jié)果如表2所示。

表2 LSTM-UKF/GP-UKF/DGP-UKF3種模型誤差比較

將訓(xùn)練好的LSTM-UKF、GP-UKF、DGP-UKF模型保存,并設(shè)置GPS信號(hào)失效時(shí)間不同時(shí)間的對(duì)比實(shí)驗(yàn)驗(yàn)證模型GPS信號(hào)失鎖較長(zhǎng)時(shí)間時(shí)模型的預(yù)測(cè)能力。設(shè)置GPS分別信號(hào)失鎖100 s、200 s及500 s,此時(shí)3種模型定位誤差如表2所示?？梢钥闯?隨著GPS信號(hào)失鎖時(shí)間較長(zhǎng),LSTM模型無法利用模型不確定程度進(jìn)行修正,GP模型擬合能力有限,本文提出的DGP-UKF組合導(dǎo)航框架在GPS失效較長(zhǎng)時(shí)間內(nèi)能始終保持在較高精度。隨著GPS失鎖時(shí)間增大,模型誤差增大。直至GPS信號(hào)失鎖500 s時(shí),原有INS/GPS-UKF先驗(yàn)信息缺失最多。由于DGP能夠增加模型復(fù)雜程度,且LSTM模型擬合能力有限,故DGP-UKF較LSTM-UKF具有較好的預(yù)測(cè)效果。通過計(jì)算可知,相較于LSTM-UKF框架,DGP-UKF定位精度提高40.19%,相對(duì)于GP-UKF定位精度提高28.96%。由表1和表2可以看出,GP和DGP區(qū)別于LSTM為非參數(shù)模型,可以學(xué)習(xí)任意類型的函數(shù),提高訓(xùn)練精度。對(duì)于GPS中斷時(shí)間較長(zhǎng)的軌跡預(yù)測(cè),DGP-UKF模型也表現(xiàn)出較好的預(yù)測(cè)能力,較深度神經(jīng)網(wǎng)絡(luò)預(yù)測(cè)誤差更小,具有更好的魯棒性和可靠性。

4 結(jié) 論

本文針對(duì)無人車在多元未知環(huán)境中的軌跡預(yù)測(cè)問題,構(gòu)建了GPS信號(hào)的軌跡預(yù)測(cè)模型,實(shí)現(xiàn)了無人車在復(fù)雜環(huán)境中的精準(zhǔn)導(dǎo)航定位?？紤]DGP的非參數(shù)優(yōu)勢(shì),提出了一種基于DGP的組合導(dǎo)航軌跡預(yù)測(cè)方法,該方法具有抑制過擬合、精度高、可靠性好的特點(diǎn),通過仿真說明了提出的DGP-UKF算法可以有效解決無人車面對(duì)未知環(huán)境的導(dǎo)航問題。引入DGP方法進(jìn)行輔助導(dǎo)航在預(yù)測(cè)過程中計(jì)算定位軌跡置信區(qū)間,為基于深度學(xué)習(xí)的數(shù)據(jù)融合組合導(dǎo)航問題提供了嚴(yán)格的理論推導(dǎo)和可解釋性支持。通過交叉驗(yàn)證可以得到500 s信號(hào)中斷時(shí),DGP相較于純慣導(dǎo)解算精度提升97.32%,相較于LSTM精度提升40.19%,相較于GP精度提升28.96%,能夠?qū)崿F(xiàn)更精確的導(dǎo)航定位,具有更好的魯棒性和可靠性。