基于IMU/ODO預積分的多傳感器即插即用因子圖融合方法

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

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

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

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

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

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

1 慣性/輪速里程計預積分模型

本文中，機體系記為b，輪速里程計系記為o。導航坐標系采用東北天坐標系，記為w。慣性/輪速里程計狀態傳播方程被建模為如下形式：

其中，

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

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

其中，

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

連續時刻的誤差狀態方程為：

其中，Ft預積分誤差的狀態轉移矩陣，Gt為噪聲矩陣，為預積分誤差狀態向量，nt為噪聲向量。對于預積分量測的雅克比矩陣與協方差矩陣，可以如下方式進行推算：

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

對于預積分量測的修正方程，可以表達為如下形式：

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

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

因此，可以構建慣性/輪速里程計預積分殘差為：

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

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

在優化過程中，狀態向量包括滑動窗口的n+1個時刻機體的位置、姿態、速度、加速度計零偏、陀螺零偏、輪速里程計標度因數以及慣性傳感器與輪速里程計的安裝誤差。

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

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

在圖1中，xk和xk+1分別為tk和tk+1時刻無人車的運動狀態，tk和tk+1時刻為優化時刻，每兩個優化時刻的時間間隔是相等的。為t1∈ [tk,tk+1]時刻的姿態類量測信息，為t2∈ [tk,tk+1]時刻的位置類量測信息。為t1時刻機體相對于bk系的預積分旋轉變化量，為t2時刻機體相對于bk系的預積分位置變化量。經過分類的導航量測信息可以通過如下的預積分公式構建量測模型。

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

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

3 試驗驗證

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

表1 IMU 性能參數Tab.1 IMU sensors specifications

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

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

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

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

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

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

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

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

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

4 結 論

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