白師宇，賴際舟，呂 品，岑益挺，王炳清，黃 凱

（1.南京航空航天大學(xué)，南京 210096；2.陜西寶成航空儀表有限責(zé)任公司，寶雞 721000）

近年來隨著人工智能、機(jī)器視覺、傳感器等技術(shù)的快速發(fā)展，無人車得到了廣泛關(guān)注。在無人車的各類應(yīng)用中，車載導(dǎo)航系統(tǒng)發(fā)揮著重要的感知作用，為自動駕駛控制系統(tǒng)提供準(zhǔn)確的速度、位置、姿態(tài)等信息。導(dǎo)航系統(tǒng)的精度與可靠性是無人車安全行駛的重要保障。然而，現(xiàn)有的無人車高度依賴全球衛(wèi)星導(dǎo)航系統(tǒng)（Global Navigation Satellite System,GNSS），當(dāng)無人車行駛在高密度建筑物、峽谷等復(fù)雜環(huán)境或是受到電磁壓制及干擾時，衛(wèi)星信號質(zhì)量較低且容易消失，從而無法對無人車進(jìn)行定位[1,2]。

當(dāng)衛(wèi)星信號不可用時，可以通過集成多種傳感器的信息來獲得精確可靠的導(dǎo)航解決方案。目前，多傳感器融合技術(shù)已被深入研究并廣泛應(yīng)用于無人駕駛領(lǐng)域[3,4]。然而，目前的車載多源融合導(dǎo)航系統(tǒng)普遍基于“煙囪式”架構(gòu)，該種架構(gòu)是針對特定傳感器以及測量源而定制的，每當(dāng)導(dǎo)航系統(tǒng)需要更改或更新時，都會帶來巨大的成本，不具備對異類異步導(dǎo)航信息的“即插即用”能力。為了解決上述問題，許多研究者在濾波器框架的基礎(chǔ)上，形成了能夠?qū)ρ舆t異步量測信息進(jìn)行處理的即插即用融合方法[5-6]。但是，基于濾波的方法將狀態(tài)向量限制為最新狀態(tài)并邊緣化所有過去的信息，從而只能獲得次優(yōu)的結(jié)果。與基于過濾的方法不同，因子圖融合方法能夠保留過去的信息并進(jìn)行全局優(yōu)化，因此可以獲得比濾波器更高的估計精度[7-8]。文獻(xiàn)[9]提出了一種基于因子圖的傳感器最優(yōu)選擇方法，其利用計算資源、導(dǎo)航精度和可觀測性指標(biāo)來選擇傳感器的最優(yōu)子集。為了解決因子圖計算量大的問題，文獻(xiàn)[10]提出了一種用于無人車的滑動窗口因子圖融合方法以保證實時運行。文獻(xiàn)[11]評估了基于因子圖框架的魯棒優(yōu)化技術(shù)，結(jié)果表明基于因子圖的魯棒優(yōu)化算法可以在GNSS 受限環(huán)境中實現(xiàn)較優(yōu)的定位性能。但是，上述研究仍使用定制的融合解決方案進(jìn)行優(yōu)化，這不足以滿足無人車導(dǎo)航系統(tǒng)對于靈活性和可擴(kuò)展性的需求。

在優(yōu)化的過程中，需要利用預(yù)積分對慣性量測進(jìn)行處理以避免重復(fù)積分[12]。在面向車載導(dǎo)航的因子圖融合方法中，由于輪速里程計同樣具有遞推特性，需要構(gòu)建輪速里程計的預(yù)積分模型。在傳統(tǒng)的輪速里程計預(yù)積分模型中，通常將地面假設(shè)為平面進(jìn)行模型的推導(dǎo)。然而，當(dāng)車輛運行在室外環(huán)境中時，這種假設(shè)會引入較大的誤差。文獻(xiàn)[13]利用陀螺的數(shù)據(jù)將輪速里程計的測速進(jìn)行了分解，克服了平面假設(shè)的缺陷。文獻(xiàn)[14]則利用慣性測量單元（Inertial Measurement Unit，IMU）構(gòu)建了輪速里程計的預(yù)積分模型，并將輪速里程計的安裝誤差加入到因子圖中進(jìn)行優(yōu)化。但是以上方法都沒有考慮輪速里程計的標(biāo)度因數(shù)對導(dǎo)航定位的影響。盡管文獻(xiàn)[15]在輪速里程計預(yù)積分模型中考慮了標(biāo)度因數(shù)的影響，但是沒有將IMU 與輪速里程計之間的安裝誤差加入到因子圖中進(jìn)行優(yōu)化，因此會導(dǎo)致定位精度的下降。

本文提出了一種基于慣性/輪速里程計預(yù)積分的多源信息“即插即用”因子圖融合優(yōu)化方法?？紤]到輪速里程計標(biāo)度因數(shù)、慣性傳感器與輪速里程計之間安裝誤差的影響，推導(dǎo)并構(gòu)建了完整的慣性導(dǎo)航系統(tǒng)和輪速里程計的預(yù)積分模型，采用固定頻率進(jìn)行優(yōu)化解算，并利用預(yù)積分信息將無人車的運動狀態(tài)與優(yōu)化時刻之間的多源異步量測信息進(jìn)行關(guān)聯(lián)形成量測模型，加入到因子圖的目標(biāo)函數(shù)中，從而實現(xiàn)了一種即插即用式的因子圖導(dǎo)航融合與參數(shù)優(yōu)化方法。跑車測試結(jié)果表明，該方法可有效兼容多類導(dǎo)航傳感器信息。同時，相比基于卡爾曼濾波的多源信息融合方法，定位精度提升60%以上。

1 慣性/輪速里程計預(yù)積分模型

本文中，機(jī)體系記為b，輪速里程計系記為o。導(dǎo)航坐標(biāo)系采用東北天坐標(biāo)系，記為w。慣性/輪速里程計狀態(tài)傳播方程被建模為如下形式：

其中，

式(1)中，和分別為tk和tk+1時刻機(jī)體在導(dǎo)航坐標(biāo)系下的位置向量，和分別為tk和tk+1時刻機(jī)體在導(dǎo)航坐標(biāo)系下的速度向量，和為tk和tk+1時刻機(jī)體系相對于導(dǎo)航系的四元數(shù)。Δtk為時間間隔，為t時刻從機(jī)體系到導(dǎo)航系的旋轉(zhuǎn)矩陣，gw為導(dǎo)航系下的重力向量。和分別為t時刻加速度計與陀螺輸出，bat和bwt為t時刻加速度計與陀螺的零偏，na和nw分別為加速度計和陀螺的噪聲。和分別為tk和tk+1時刻輪速里程計在機(jī)體系下的位置向量，為t時刻輪速里程計系到機(jī)體系的旋轉(zhuǎn)矩陣，sot為t時刻輪速里程計的標(biāo)度因數(shù)，ns為輪速里程計的噪聲。

將式(1)中的狀態(tài)傳播模型投影到tk時刻的機(jī)體系bk下，可得：

其中，

式(4)的離散形式可以寫為：

連續(xù)時刻的誤差狀態(tài)方程為：

其中，F(xiàn)t預(yù)積分誤差的狀態(tài)轉(zhuǎn)移矩陣，Gt為噪聲矩陣，為預(yù)積分誤差狀態(tài)向量，nt為噪聲向量。對于預(yù)積分量測的雅克比矩陣與協(xié)方差矩陣，可以如下方式進(jìn)行推算：

其中，Q是噪聲的對角協(xié)方差矩陣。預(yù)積分量測在tk的雅克比矩陣與協(xié)方差矩陣的初值為：

對于預(yù)積分量測的修正方程，可以表達(dá)為如下形式：

其中，δbak和δbwk分別代表加速度計和陀螺的零偏誤差，δsok和分別代表輪速里程計標(biāo)度因數(shù)誤差以

及安裝角度誤差。和分別為預(yù)積分位置變化量誤差對應(yīng)δb和δb的雅克比矩陣，和b分別為預(yù)積分速度變化量誤差對應(yīng)δbak和的雅克比矩陣，為預(yù)積分旋轉(zhuǎn)變化量誤差對應(yīng)δb的雅克比矩陣，、和wk分別為預(yù)積分位置變化量誤差對應(yīng)、和的雅克比矩陣。上述雅克比矩陣均對應(yīng)tk+1時刻雅克比矩陣的子塊矩陣，可以從對應(yīng)位置中獲取。

因此，可以構(gòu)建慣性/輪速里程計預(yù)積分殘差為：

2 基于預(yù)積分的即插即用因子圖融合方法

隨著車載傳感器種類與數(shù)目的增加，當(dāng)前的定制型多傳感器融合方法不具備對異類異步導(dǎo)航信息的即插即用能力。因此，本文提出了一種基于預(yù)積分的即插即用因子圖融合方法。該方法通過設(shè)置固定的頻率進(jìn)行優(yōu)化解算，并利用預(yù)積分信息將無人車的運動狀態(tài)與優(yōu)化時刻之間的多源異步量測信息進(jìn)行關(guān)聯(lián)形成量測模型，加入到因子圖的目標(biāo)函數(shù)中，因此能夠自適應(yīng)兼容任何種類的導(dǎo)航信息并避免了基于傳感器頻率進(jìn)行優(yōu)化而導(dǎo)致的計算量增加的問題。

在優(yōu)化過程中，狀態(tài)向量包括滑動窗口的n+1個時刻機(jī)體的位置、姿態(tài)、速度、加速度計零偏、陀螺零偏、輪速里程計標(biāo)度因數(shù)以及慣性傳感器與輪速里程計的安裝誤差。

基于預(yù)積分的即插即用因子圖融合方法如圖1所示。

圖1 基于預(yù)積分的即插即用因子圖融合方法Fig.1 Plug-and-play factor graph fusion method based on preintegration

在圖1中，xk和xk+1分別為tk和tk+1時刻無人車的運動狀態(tài)，tk和tk+1時刻為優(yōu)化時刻，每兩個優(yōu)化時刻的時間間隔是相等的。為t1∈ [tk,tk+1]時刻的姿態(tài)類量測信息，為t2∈ [tk,tk+1]時刻的位置類量測信息。為t1時刻機(jī)體相對于bk系的預(yù)積分旋轉(zhuǎn)變化量，為t2時刻機(jī)體相對于bk系的預(yù)積分位置變化量。經(jīng)過分類的導(dǎo)航量測信息可以通過如下的預(yù)積分公式構(gòu)建量測模型。

因此，導(dǎo)航信息量測模型的殘差可以表示為：

將式(13)加入到因子圖的目標(biāo)函數(shù)中，可以表示為：

3 試驗驗證

本文開展了實際跑車測試，用于驗證所提出的算法。測試系統(tǒng)包括MEMS-IMU、雙天線RTK、輪速里程計、激光雷達(dá)、攝像頭以及百度Apollo 自動駕駛開發(fā)套件。IMU 的性能參數(shù)如表1所示，跑車測試平臺如圖2所示。在測試中，利用千尋基站服務(wù)提供的厘米級精度RTK 信息作為軌跡基準(zhǔn)。跑車測試軌跡如圖3所示。

表1 IMU 性能參數(shù)Tab.1 IMU sensors specifications

圖2 跑車測試平臺Fig.2 Experimental test platform

圖3 跑車測試軌跡Fig.3 Trajectory of experimental test

為了實現(xiàn)衛(wèi)星拒止環(huán)境下的高精度定位，我們首先利用三維激光雷達(dá)對跑車測試的場景進(jìn)行了構(gòu)圖，用于點云地圖匹配以提供無衛(wèi)星環(huán)境下的高精度位置信息。點云地圖的俯視圖如圖4所示。同時，在跑車過程中，我們也利用攝像頭對周圍的特征點進(jìn)行提取與匹配，通過視覺里程計實現(xiàn)對車體自身的狀態(tài)估計。視覺特征點提取與匹配如圖5所示。

圖4 點云地圖俯視圖Fig.4 Top view of point cloud map

圖5 視覺特征點提取與匹配Fig.5 Visual feature point extraction and matching

在整個測試過程中，雙天線RTK 的定位結(jié)果作為參考值，并沒有參與融合定位。利用慣性傳感器、輪速里程計、激光雷達(dá)點云地圖匹配以及視覺里程計信息完成對無人車的導(dǎo)航定位，并在不同的時間段根據(jù)導(dǎo)航信息的可用性，多源融合導(dǎo)航系統(tǒng)采用不同的導(dǎo)航傳感器配置方案。我們對卡爾曼濾波與本文所提出方法得到的定位結(jié)果進(jìn)行了對比，軌跡對比如圖6所示，可以看出利用本文所提出方法得到的融合定位結(jié)果優(yōu)于卡爾曼濾波融合結(jié)果。

圖6 融合軌跡對比Fig.6 Comparisons of the fusion trajectory

圖7和圖8分別為兩種方法的水平與高度誤差對比。其中IMU 代表慣性信息，ODO 代表輪速里程計信息，VO 代表視覺里程計信息、MAP 代表激光雷達(dá)點云地圖信息?？梢钥闯?，多源融合導(dǎo)航系統(tǒng)能夠兼容多種類型的導(dǎo)航信息。當(dāng)有高精度地圖絕對位置量測修正時，卡爾曼濾波與本文所提出的因子圖融合方法可以取得等效的定位精度。但是，當(dāng)缺乏高精度地圖修正而依靠慣性/輪速里程計/視覺里程計進(jìn)行位置推算時，由于本文所提出的因子圖融合方法可以對傳感器參數(shù)進(jìn)行迭代優(yōu)化，可以獲得比卡爾曼濾波更高的傳感器參數(shù)估計精度，因此有效抑制遞推過程的位置發(fā)散。在大約600 s～1200 s 的時間內(nèi)，卡爾曼濾波融合的水平位置誤差最大發(fā)散至103 m，高度誤差最大發(fā)散至27 m。而本文所提出方法的水平位置誤差最大發(fā)散至34 m，高度誤差最大發(fā)散至1 m，相比卡爾曼濾波融合，定位精度提升60%以上。

圖8 高度誤差對比Fig.8 Comparisons of the vertical position errors

4 結(jié) 論

在本文中，我們提出了一種基于慣性/輪速里程計預(yù)積分的多源信息“即插即用”因子圖融合優(yōu)化方法。首先推導(dǎo)并構(gòu)建了慣性導(dǎo)航系統(tǒng)和無人車輪速里程計的預(yù)積分模型，利用預(yù)積分信息將無人車運動狀態(tài)與多源異步量測信息進(jìn)行關(guān)聯(lián)并采用固定頻率解算，從而實現(xiàn)了一種即插即用式的因子圖導(dǎo)航融合與參數(shù)優(yōu)化方法。該方法避免了優(yōu)化過程中對慣性以及輪速里程計信息的重復(fù)積分以及基于傳感器頻率進(jìn)行優(yōu)化而導(dǎo)致的計算量增加的問題。論文開展了實際跑車測試，結(jié)果表明該方法可有效兼容多類導(dǎo)航傳感器信息。同時，相比基于卡爾曼濾波的多源信息融合方法，IMU/ODO/VO 定位精度提升60%以上。