基于濾波和優化的慣性/里程計組合導航精度對比分析

王茂松,崔加瑞,劉若辰

（國防科技大學智能科學學院,長沙 410073）

0 引言

目前,基于因子圖優化的方法在多傳感器融合領域吸引了許多學者[1],其利用概率圖模型對狀態估計方程進行建模,利用非線性估計技術對狀態進行迭代估計,在理論上具有對歷史狀態的修正和平滑作用。但是,目前因子圖優化的研究多針對于慣性/衛星[2]或慣性/視覺組合[3],屬于多約束系統,而對慣性/里程計組合導航缺乏對比研究。因此,本文將對比因子圖優化技術和Kalman濾波器技術在慣性/里程計組合導航中的應用,并分析兩者的適用場景和優劣,以期給出有益的參考。

慣性/里程計組合導航可以不借助外界信息,如無線電信息,可以作為陸地車輛及機器人的自主導航實現方法,基于Kalman 濾波技術的組合導航方法已經有較多研究并具有較為成熟的工程應用: 嚴恭敏[4]對高精度慣性/里程計組合導航以及航位推算模型中的安裝角及桿臂誤差進行了精確建模和補償;Wu[5]針對慣性/里程計應用提出了一種自標定和初始對準技術;Ouyang 等[6]進一步對精確建模的慣性/里程計模型應用了多模型自適應估計算法提高定位精度。對比而言,因子圖將組合導航系統的聯合概率分布用概率圖模型表示,利用系統的稀疏性,具有即插即用的能力。Kaess等[7]在因子圖上應用了一種高效的增量推理算法,即ISAM2（Incremental Smoothing and Mapping）算法,使其性能接近于批量優化,進一步基于IMU 預積分的技術大幅縮減了需要優化的狀態量,使得因子圖優化的效率進一步提升。

本文首先介紹了基于因子圖的慣性/里程計組合導航建模方法;然后,對比了基于Kalman 濾波和因子圖優化的狀態估計算法,說明其在狀態估計原理上的差異;最后,基于陸用無人平臺進行了慣性/里程計組合導航試驗,對比了慣性/里程計組合導航在不同方法下的精度差異,分析了原因以及基于優化的慣性/里程計組合導航方法還需要解決的問題。

1 慣性/里程計因子圖建模方法

因子圖建模技術是一種利用概率圖模型進行建模的技術,因子圖F=（U,V,E） 是一種二分圖,包含兩種節點,即因子節點?i∈U、變量節點xj∈V以及連接兩者的邊eij∈E。連接變量節點?i的集合寫作N（?i）,Xi是其上的一個子集,Xi表示任意節點xj的集合,則因子函數集可以寫作?（X）,因子圖的全局函數可以寫作

接下來給出慣性/里程計組合導航的因子圖模型,如圖1所示。

圖1 基于因子圖的慣性/里程計組合導航模型Fig.1 Diagram of factor-graph-based SINS/ODO integrated navigation

圖1中,圓形框為系統歷史的n個狀態變量,即因子圖中的變量節點xi（i=1,2,…,n）;黑色方框為因子節點?（?）,包括量測信息節點?ODO、?IMU和先驗節點x0。

1.1 預積分因子模型

預積分因子模型將兩次觀測間隔內的加速度計和陀螺儀的數據進行融合,使其能夠在整個狀態進行重新線性化的過程中保持不變,因此避免了重復計算,減低了計算量。預積分因子節點建模為如下殘差函數

式（2）中,xk-1為模型在時刻tk-1的狀態,預測函數hIMU（·）利用時刻tk-1到時刻tk的加速度計和陀螺儀增量數據遞推出下一觀測時刻的載體位姿,表示協方差為Σi的變量間的馬氏距離。Σi實質上是加速度計和陀螺儀噪聲強度的函數,協方差值應謹慎設置,因為它影響到最大后驗（Maximum A Posteriori,MAP）估計的結果。

1.2 里程計速度約束因子模型

為了和Kalman 濾波器中速度觀測的形式相一致,本文將里程計的脈沖進行差分得到里程計的速度,將當前時刻里程計速度作為速度約束因子節點,第m個里程計因子節點的殘差函數如下

式（3）中,hODO（·）為將導航系下的速度狀態轉換為里程計坐標系下的速度狀態,Rv為里程計的速度噪聲。

2 基于因子圖優化和Kalman 濾波的慣性/里程計組合導航算法對比

2.1 基于因子圖優化的算法

為了得到最優狀態估計,基于因子圖的優化算法可以使用合適的優化算法來求得式（4）的最優估計結果,例如使用梯度下降法、Gauss-Newton 法和LM（Levenberg-Marquarelt）算法[8]等

為了評估因子圖優化算法和Kalman 濾波器算法,本文采用GTSAM 開源因子圖庫[9]進行算法設計,利用ISAM2 算法進行增量遞推,優化算法使用LM 算法,稀疏矩陣分解方法使用具有良好數值穩定性的QR 分解算法。以下為LM 算法的迭代計算公式

LM 的思想為使用Jacobi 矩陣的平方近似Hessian 矩陣并且添加阻尼項使其可逆。式（5）中,hlm為LM 算法的變量迭代更新方向,α為更新步長,J為Jacobi 矩陣,f為因子圖模型構造的非線性最小二乘函數。通過調節LM 算法的μ可以使其轉化為Gauss-Newton 法與梯度下降法。

ISAM2 算法解決了增量優化的問題,其思路是將因子圖轉化為Bayes 樹,當系統添加一個新的因子時,僅影響包含相關變量的團和其父節點。受影響的團可以被重新組合為新的團結構,并形成新的子樹,而不必更新整個Bayes 樹,以此實現了增量優化過程。

2.2 基于Kalman 濾波器的算法

本文中,Kalman 濾波器使用狀態變換Kalman濾波器（State-transform Extended Kalman Filter,STEKF）,采用考慮里程計的安裝角和標度因數誤差的模型,其相關算法見文獻[10]。此處給出主要STEKF 的算法公式:

系統變量如下

式（6） 中,?n、δvn?、δrn、εb、Δb、δkODO、δαy-ODO、δαz-ODO分別為當地導航系下的姿態失準角、狀態變換速度誤差、位置誤差、陀螺儀和加速度計零偏以及里程計標度因數誤差和安裝角誤差。在里程計坐標系m系下的觀測方程如下

式（7）中,×表示將三維矢量轉換為對應的反對稱矩陣,其他的相關參數可進一步參考文獻[11]。

利用Kalman 濾波預測過程和更新過程的5 個公式即可完成對于當前狀態的最優估計。

時間更新公式為

量測更新公式為

2.3 算法流程對比

基于Kalman 濾波算法和因子圖優化算法的流程圖如圖2所示。

圖2 Kalman 濾波器和因子圖優化算法流程圖Fig.2 Flowchart of Kalman filter and factor graph optimization algorithms

通過上述的算法流程對比可以看出: 基于Kalman 濾波器的狀態估計是一種遞推算法,其利用歷史信息得到當前狀態的最優估計;而因子圖優化算法是一種批量估計算法,其能夠利用歷史信息得到所有狀態的最優估計。在線性Gauss 的假設下,兩者在最大后驗估計的意義下都能夠實現對當前狀態的最優估計;但是在非線性非Gauss 系統中,因子圖優化的主要優勢在于對歷史狀態的重新估計,在重新估計的過程中,相當于對原系統的線性化點進行重新線性化,因此最小化系統整體的非線性誤差,這也是因子圖優化算法魯棒性和相對較高后處理精度的來源。

3 試驗與分析

3.1 地面無人平臺配置及試驗軌跡

為了對基于Kalman 濾波和基于因子圖優化的慣性/里程計組合導航算法進行對比,本文基于如圖3所示的陸用無人平臺進行數據采集和算法驗證,采用的平臺為松靈機器人SCOUT2.0 移動機器人底盤,平臺上主要配置的設備包括光纖陀螺慣性導航系統、STIM300 慣性導航系統、輪式里程計以及衛星接收機。

圖3 陸用慣性/里程計組合導航無人系統Fig.3 Diagram of land SINS/ODO integrated navigation unmanned system

本文中的試驗采用光纖陀螺捷聯慣性導航系統和衛星速度位置進行松組合后再進行RTS（Rauch Tung Striebel）平滑的結果作為參考基準,本試驗中的陸用無人平臺傳感器配置參數如表1～表3所示。

表1 光纖陀螺捷聯慣導系統參數Table 1 Parameters of FOG-SINS

表2 STIM300 參數Table 2 Parameters of STIM300

表3 輪式里程計參數Table 3 Parameters of wheeled odometer

試驗地點位于湖南省長沙市內,試驗整體持續時長為1520s,起點和終點相同,試驗軌跡如圖4所示。

圖4 地面無人平臺試驗軌跡衛星圖Fig.4 Satellite trajectory of ground unmanned platform test

3.2 試驗結果及分析

為保證相對的公平性,因子圖模型和Kalman濾波器在狀態估計過程中將安裝角、標度因數誤差提前進行了補償,并使用相同的慣性器件和里程計速度噪聲參數。

對含有稀疏位置觀測的慣性/里程計組合導航算法和僅有速度約束的慣性/里程計組合導航算法進行驗證,得到的試驗結果如圖5、圖6和表4所示。

圖5 不同位置觀測間隔的慣性/里程計組合導航優化算法結果Fig.5 Optimization algorithm results of SINS/ODO integrated navigation with different position measurement intervals

圖6 慣性/里程計組合導航優化算法（100s 稀疏位置約束）與Kalman 濾波算法（無位置約束）對比結果Fig.6 Comparison results of SINS/ODO integrated navigation optimization algorithm （100s sparse position measurement） and Kalman filtering algorithm （without position measurement）

表4 基于Kalman 濾波器（無位置觀測）和因子圖優化（含稀疏位置約束）的慣性/里程計組合導航結果Table 4 Results of SINS/ODO integrated navigation based on Kalman filter （without position measurement）and factor graph optimization （with position measurement of different frequencies）

圖5中的水平誤差為東向和北向合成誤差,其中的100s～500s 為位置量測出現的時間間隔,數值的變化表征了觀測信息密度程度不同,時間間隔越長表示觀測信息密度逐漸稀疏。此試驗結果表明,基于Kalman 濾波器的慣性/里程計能夠在無位置觀測的條件下達到較高的定位精度,其整體誤差小于3m,水平定位誤差的均方差小于2m;而在沒有位置信息的條件下,僅依靠慣性/里程計因子圖優化方法不能達到理想的效果,而隨著位置觀測稀疏程度的下降,慣性/里程計組合導航精度又能夠得到顯著提升。限制因子圖優化定位精度的挑戰有如下幾個: 1）采用因子圖優化技術的概率圖模型建模理論較為復雜,相比而言,基于遞推方法的Kalman 濾波器理論簡單,故因子圖模型的建模難度較Kalman 濾波器大;2）優化算法容易陷入局部最優解,當缺乏約束信息時,其批量優化結果反而會將未約束的變量“帶偏”,由此導致的整體解算結果精度會下降,這點顯著體現在增加位置約束后位置精度提升上,此處的位置約束可以理解為常規Kalman 濾波器當中的位置觀測。

另一方面,在狀態估計的平滑性以及魯棒性上,當含有稀疏位置觀測時,因子圖優化算法表現得比Kalman 濾波算法更好。在圖6中能夠看出,基于Kalman 濾波器的算法其導航結果含有水平位置誤差尖峰,分別出現在200s、600s～700s、900s～1000s、1100s 等處;相對而言,因子圖優化算法出現的尖峰較小,軌跡比較平滑。

4 討論和結論

本文對基于Kalman 濾波器和因子圖的慣性/里程計組合導航算法進行了算法的驗證和結果對比分析,得到的結論為: 在約束不充分的情況下,例如僅含有速度約束或位置觀測十分稀疏的條件下,基于Kalman 濾波器的可靠性和精度能夠優于基于因子圖優化的算法;隨著約束條件增多,如當位置約束（位置觀測）增多、衛星信號或地面路標信號較為豐富準確時,因子圖優化算法有比基于濾波算法更高的狀態估計精度表現。此外,如果因子圖優化算法能夠建立更為準確的模型,如在因子圖模型中考慮對里程計標度因數誤差、安裝角誤差的實時估計等,其性能將會有進一步提升。