基于濾波和優(yōu)化的慣性/里程計(jì)組合導(dǎo)航精度對(duì)比分析

王茂松,崔加瑞,劉若辰

（國(guó)防科技大學(xué)智能科學(xué)學(xué)院,長(zhǎng)沙 410073）

0 引言

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

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

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

1 慣性/里程計(jì)因子圖建模方法

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

接下來給出慣性/里程計(jì)組合導(dǎo)航的因子圖模型,如圖1所示。

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

圖1中,圓形框?yàn)橄到y(tǒng)歷史的n個(gè)狀態(tài)變量,即因子圖中的變量節(jié)點(diǎn)xi（i=1,2,…,n）;黑色方框?yàn)橐蜃庸?jié)點(diǎn)?（?）,包括量測(cè)信息節(jié)點(diǎn)?ODO、?IMU和先驗(yàn)節(jié)點(diǎn)x0。

1.1 預(yù)積分因子模型

預(yù)積分因子模型將兩次觀測(cè)間隔內(nèi)的加速度計(jì)和陀螺儀的數(shù)據(jù)進(jìn)行融合,使其能夠在整個(gè)狀態(tài)進(jìn)行重新線性化的過程中保持不變,因此避免了重復(fù)計(jì)算,減低了計(jì)算量。預(yù)積分因子節(jié)點(diǎn)建模為如下殘差函數(shù)

式（2）中,xk-1為模型在時(shí)刻tk-1的狀態(tài),預(yù)測(cè)函數(shù)hIMU（·）利用時(shí)刻tk-1到時(shí)刻tk的加速度計(jì)和陀螺儀增量數(shù)據(jù)遞推出下一觀測(cè)時(shí)刻的載體位姿,表示協(xié)方差為Σi的變量間的馬氏距離。Σi實(shí)質(zhì)上是加速度計(jì)和陀螺儀噪聲強(qiáng)度的函數(shù),協(xié)方差值應(yīng)謹(jǐn)慎設(shè)置,因?yàn)樗绊懙阶畲蠛篁?yàn)（Maximum A Posteriori,MAP）估計(jì)的結(jié)果。

1.2 里程計(jì)速度約束因子模型

為了和Kalman 濾波器中速度觀測(cè)的形式相一致,本文將里程計(jì)的脈沖進(jìn)行差分得到里程計(jì)的速度,將當(dāng)前時(shí)刻里程計(jì)速度作為速度約束因子節(jié)點(diǎn),第m個(gè)里程計(jì)因子節(jié)點(diǎn)的殘差函數(shù)如下

式（3）中,hODO（·）為將導(dǎo)航系下的速度狀態(tài)轉(zhuǎn)換為里程計(jì)坐標(biāo)系下的速度狀態(tài),Rv為里程計(jì)的速度噪聲。

2 基于因子圖優(yōu)化和Kalman 濾波的慣性/里程計(jì)組合導(dǎo)航算法對(duì)比

2.1 基于因子圖優(yōu)化的算法

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

為了評(píng)估因子圖優(yōu)化算法和Kalman 濾波器算法,本文采用GTSAM 開源因子圖庫(kù)[9]進(jìn)行算法設(shè)計(jì),利用ISAM2 算法進(jìn)行增量遞推,優(yōu)化算法使用LM 算法,稀疏矩陣分解方法使用具有良好數(shù)值穩(wěn)定性的QR 分解算法。以下為L(zhǎng)M 算法的迭代計(jì)算公式

LM 的思想為使用Jacobi 矩陣的平方近似Hessian 矩陣并且添加阻尼項(xiàng)使其可逆。式（5）中,hlm為L(zhǎng)M 算法的變量迭代更新方向,α為更新步長(zhǎng),J為Jacobi 矩陣,f為因子圖模型構(gòu)造的非線性最小二乘函數(shù)。通過調(diào)節(jié)LM 算法的μ可以使其轉(zhuǎn)化為Gauss-Newton 法與梯度下降法。

ISAM2 算法解決了增量?jī)?yōu)化的問題,其思路是將因子圖轉(zhuǎn)化為Bayes 樹,當(dāng)系統(tǒng)添加一個(gè)新的因子時(shí),僅影響包含相關(guān)變量的團(tuán)和其父節(jié)點(diǎn)。受影響的團(tuán)可以被重新組合為新的團(tuán)結(jié)構(gòu),并形成新的子樹,而不必更新整個(gè)Bayes 樹,以此實(shí)現(xiàn)了增量?jī)?yōu)化過程。

2.2 基于Kalman 濾波器的算法

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

系統(tǒng)變量如下

式（6） 中,?n、δvn?、δrn、εb、Δb、δkODO、δαy-ODO、δαz-ODO分別為當(dāng)?shù)貙?dǎo)航系下的姿態(tài)失準(zhǔn)角、狀態(tài)變換速度誤差、位置誤差、陀螺儀和加速度計(jì)零偏以及里程計(jì)標(biāo)度因數(shù)誤差和安裝角誤差。在里程計(jì)坐標(biāo)系m系下的觀測(cè)方程如下

式（7）中,×表示將三維矢量轉(zhuǎn)換為對(duì)應(yīng)的反對(duì)稱矩陣,其他的相關(guān)參數(shù)可進(jìn)一步參考文獻(xiàn)[11]。

利用Kalman 濾波預(yù)測(cè)過程和更新過程的5 個(gè)公式即可完成對(duì)于當(dāng)前狀態(tài)的最優(yōu)估計(jì)。

時(shí)間更新公式為

量測(cè)更新公式為

2.3 算法流程對(duì)比

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

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

通過上述的算法流程對(duì)比可以看出: 基于Kalman 濾波器的狀態(tài)估計(jì)是一種遞推算法,其利用歷史信息得到當(dāng)前狀態(tài)的最優(yōu)估計(jì);而因子圖優(yōu)化算法是一種批量估計(jì)算法,其能夠利用歷史信息得到所有狀態(tài)的最優(yōu)估計(jì)。在線性Gauss 的假設(shè)下,兩者在最大后驗(yàn)估計(jì)的意義下都能夠?qū)崿F(xiàn)對(duì)當(dāng)前狀態(tài)的最優(yōu)估計(jì);但是在非線性非Gauss 系統(tǒng)中,因子圖優(yōu)化的主要優(yōu)勢(shì)在于對(duì)歷史狀態(tài)的重新估計(jì),在重新估計(jì)的過程中,相當(dāng)于對(duì)原系統(tǒng)的線性化點(diǎn)進(jìn)行重新線性化,因此最小化系統(tǒng)整體的非線性誤差,這也是因子圖優(yōu)化算法魯棒性和相對(duì)較高后處理精度的來源。

3 試驗(yàn)與分析

3.1 地面無人平臺(tái)配置及試驗(yàn)軌跡

為了對(duì)基于Kalman 濾波和基于因子圖優(yōu)化的慣性/里程計(jì)組合導(dǎo)航算法進(jìn)行對(duì)比,本文基于如圖3所示的陸用無人平臺(tái)進(jìn)行數(shù)據(jù)采集和算法驗(yàn)證,采用的平臺(tái)為松靈機(jī)器人SCOUT2.0 移動(dòng)機(jī)器人底盤,平臺(tái)上主要配置的設(shè)備包括光纖陀螺慣性導(dǎo)航系統(tǒng)、STIM300 慣性導(dǎo)航系統(tǒng)、輪式里程計(jì)以及衛(wèi)星接收機(jī)。

圖3 陸用慣性/里程計(jì)組合導(dǎo)航無人系統(tǒng)Fig.3 Diagram of land SINS/ODO integrated navigation unmanned system

本文中的試驗(yàn)采用光纖陀螺捷聯(lián)慣性導(dǎo)航系統(tǒng)和衛(wèi)星速度位置進(jìn)行松組合后再進(jìn)行RTS（Rauch Tung Striebel）平滑的結(jié)果作為參考基準(zhǔn),本試驗(yàn)中的陸用無人平臺(tái)傳感器配置參數(shù)如表1～表3所示。

表1 光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)參數(shù)Table 1 Parameters of FOG-SINS

表2 STIM300 參數(shù)Table 2 Parameters of STIM300

表3 輪式里程計(jì)參數(shù)Table 3 Parameters of wheeled odometer

試驗(yàn)地點(diǎn)位于湖南省長(zhǎng)沙市內(nèi),試驗(yàn)整體持續(xù)時(shí)長(zhǎng)為1520s,起點(diǎn)和終點(diǎn)相同,試驗(yàn)軌跡如圖4所示。

圖4 地面無人平臺(tái)試驗(yàn)軌跡衛(wèi)星圖Fig.4 Satellite trajectory of ground unmanned platform test

3.2 試驗(yàn)結(jié)果及分析

為保證相對(duì)的公平性,因子圖模型和Kalman濾波器在狀態(tài)估計(jì)過程中將安裝角、標(biāo)度因數(shù)誤差提前進(jìn)行了補(bǔ)償,并使用相同的慣性器件和里程計(jì)速度噪聲參數(shù)。

對(duì)含有稀疏位置觀測(cè)的慣性/里程計(jì)組合導(dǎo)航算法和僅有速度約束的慣性/里程計(jì)組合導(dǎo)航算法進(jìn)行驗(yàn)證,得到的試驗(yàn)結(jié)果如圖5、圖6和表4所示。

圖5 不同位置觀測(cè)間隔的慣性/里程計(jì)組合導(dǎo)航優(yōu)化算法結(jié)果Fig.5 Optimization algorithm results of SINS/ODO integrated navigation with different position measurement intervals

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

表4 基于Kalman 濾波器（無位置觀測(cè)）和因子圖優(yōu)化（含稀疏位置約束）的慣性/里程計(jì)組合導(dǎo)航結(jié)果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 為位置量測(cè)出現(xiàn)的時(shí)間間隔,數(shù)值的變化表征了觀測(cè)信息密度程度不同,時(shí)間間隔越長(zhǎng)表示觀測(cè)信息密度逐漸稀疏。此試驗(yàn)結(jié)果表明,基于Kalman 濾波器的慣性/里程計(jì)能夠在無位置觀測(cè)的條件下達(dá)到較高的定位精度,其整體誤差小于3m,水平定位誤差的均方差小于2m;而在沒有位置信息的條件下,僅依靠慣性/里程計(jì)因子圖優(yōu)化方法不能達(dá)到理想的效果,而隨著位置觀測(cè)稀疏程度的下降,慣性/里程計(jì)組合導(dǎo)航精度又能夠得到顯著提升。限制因子圖優(yōu)化定位精度的挑戰(zhàn)有如下幾個(gè): 1）采用因子圖優(yōu)化技術(shù)的概率圖模型建模理論較為復(fù)雜,相比而言,基于遞推方法的Kalman 濾波器理論簡(jiǎn)單,故因子圖模型的建模難度較Kalman 濾波器大;2）優(yōu)化算法容易陷入局部最優(yōu)解,當(dāng)缺乏約束信息時(shí),其批量?jī)?yōu)化結(jié)果反而會(huì)將未約束的變量“帶偏”,由此導(dǎo)致的整體解算結(jié)果精度會(huì)下降,這點(diǎn)顯著體現(xiàn)在增加位置約束后位置精度提升上,此處的位置約束可以理解為常規(guī)Kalman 濾波器當(dāng)中的位置觀測(cè)。

另一方面,在狀態(tài)估計(jì)的平滑性以及魯棒性上,當(dāng)含有稀疏位置觀測(cè)時(shí),因子圖優(yōu)化算法表現(xiàn)得比Kalman 濾波算法更好。在圖6中能夠看出,基于Kalman 濾波器的算法其導(dǎo)航結(jié)果含有水平位置誤差尖峰,分別出現(xiàn)在200s、600s～700s、900s～1000s、1100s 等處;相對(duì)而言,因子圖優(yōu)化算法出現(xiàn)的尖峰較小,軌跡比較平滑。

4 討論和結(jié)論

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