狀態(tài)變換卡爾曼濾波的進(jìn)一步解釋及應(yīng)用

王茂松，吳文啟，何曉峰，潘獻(xiàn)飛

（國防科技大學(xué)智能科學(xué)學(xué)院，長沙 410073）

如果捷聯(lián)慣性組合導(dǎo)航系統(tǒng)建模合理，并且傳感器均能良好工作的情況下，大部分基于擴(kuò)展卡爾曼濾波甚至其它非線性濾波的組合導(dǎo)航系統(tǒng)[1-3]均能較好的工作。但是，當(dāng)導(dǎo)航平臺處于不利的環(huán)境下時(shí)，如在準(zhǔn)靜態(tài)的環(huán)境下，即處于靜態(tài)的載體仍然受到振動(dòng)、風(fēng)動(dòng)等外部力的影響，一貫的捷聯(lián)慣性組合導(dǎo)航濾波方式很容易出現(xiàn)方差估計(jì)不一致的問題。例如，雖然在準(zhǔn)靜態(tài)環(huán)境下，東方向陀螺和水平方向加表是不可觀測的，即它們的方差在理論上應(yīng)該是保持不變的，但是由于振動(dòng)的存在，會(huì)使組合導(dǎo)航系統(tǒng)中它們的方差仍然會(huì)繼續(xù)減少，導(dǎo)致方差的理論值與實(shí)際估計(jì)值不符，進(jìn)而導(dǎo)致導(dǎo)航精度的下降甚至是濾波的發(fā)散[4]。

為了解決方差估計(jì)不一致的問題，學(xué)者們提出了一些方法論，如施密特卡爾曼濾波器[5]（Schmidt- Kalman Filter,SKF），將不可觀的狀態(tài)零化，也就是不可觀狀態(tài)對應(yīng)的方差不進(jìn)行更新。施密特卡爾曼濾波器實(shí)際上為次優(yōu)濾波器，但是，它可以保證不可觀狀態(tài)估計(jì)的一致性，避免估計(jì)發(fā)散，并成功的應(yīng)用于目標(biāo)追蹤[6]以及太空探測[7]。Huang 等人[8]指出，在EKF-SLAM 系統(tǒng)中，如果過程模型和量測模型的雅克比矩陣以最近的狀態(tài)估計(jì)進(jìn)行線性化，將會(huì)導(dǎo)致可觀測子空間的維度比實(shí)際的維度要高，進(jìn)而使得狀態(tài)空間中本來不可觀測的狀態(tài)進(jìn)行錯(cuò)誤的校正，產(chǎn)生不一致的問題。所以，作者提出了選擇合適的線性化點(diǎn)來使系統(tǒng)的可觀測性與實(shí)際相符的卡爾曼濾波方式，稱為觀測約束擴(kuò)展卡爾曼濾波（Observability Constraint Extended Kalman Filter,OCEKF），通過實(shí)驗(yàn)驗(yàn)證了所提出濾波方法的一致性。不變卡爾曼濾波（Invariant Extended Kalman Filter,IEKF）[9]是國外學(xué)者最新提出的可以應(yīng)用于組合導(dǎo)航系統(tǒng)中的濾波方法，核心內(nèi)容是基于李群李代數(shù)的理論，可以防止組合導(dǎo)航系統(tǒng)出現(xiàn)方差估計(jì)不一致的問題，在同時(shí)定位與構(gòu)圖領(lǐng)域亦得到應(yīng)用[10-11]。

文獻(xiàn)[12]指出導(dǎo)致捷聯(lián)慣性組合導(dǎo)航系統(tǒng)方差估計(jì)不一致問題的主要原因在于系統(tǒng)矩陣中的比力項(xiàng)，通過速度誤差狀態(tài)的嚴(yán)格變換，可以避免系統(tǒng)矩陣中含有比力項(xiàng)，進(jìn)而克服方差估計(jì)不一致的問題。實(shí)際上，速度誤差的狀態(tài)變換在本質(zhì)上與速度誤差狀態(tài)定義的坐標(biāo)系一致性有異曲同工之妙[13-15]。本文對文獻(xiàn)[12]中提出的速度誤差從坐標(biāo)系一致的角度作進(jìn)一步的解釋，并將其成果進(jìn)一步擴(kuò)展應(yīng)用于捷聯(lián)慣性導(dǎo)航的初始對準(zhǔn)和慣性/衛(wèi)星松組合導(dǎo)航。

1 NED 坐標(biāo)系基于ST-EKF 的慣性/衛(wèi)星松組合系統(tǒng)方程

當(dāng)?shù)貙?dǎo)航系下定義的速度微分方程為：

其中，fb為比力向量，gn當(dāng)?shù)刂亓κ噶?，為地球相對慣性系的角速率在導(dǎo)航系的投影，為北東地(NED)坐標(biāo)系相對地球運(yùn)動(dòng)產(chǎn)生的角速率在導(dǎo)航系的投影，∧代表將向量轉(zhuǎn)化為斜對稱矩陣，Cbn為姿態(tài)矩陣。

其中，φn和分別為姿態(tài)誤差和速度誤差向量，和的定義見文獻(xiàn)[12]，δfb為加速度計(jì)誤差向量。

新定義的速度誤差的微分方程的詳細(xì)推導(dǎo)如下：

其中，εb和?b分別為陀螺和加速度計(jì)的零偏；wg和wa分別為陀螺和加速度計(jì)誤差的白噪聲向量；是姿態(tài)誤差的微分：

陀螺儀和加速度計(jì)誤差可以建模為隨機(jī)游走過程：

根據(jù)雅克比恒等式可以得到：

顯然，從式(9)可以看出，新的速度誤差微分方程中不再含有比力項(xiàng)，而是由重力向量替代。由于IMU 在一般情況下的輸出都是比力增量而不是比力本身[13]，并且比力增量的值由于器件等原因會(huì)摻雜著量化噪聲，所以傳統(tǒng)基于EKF 的組合導(dǎo)航系統(tǒng)的系統(tǒng)矩陣的計(jì)算會(huì)被動(dòng)態(tài)環(huán)境下的比力fn嚴(yán)重影響，進(jìn)而導(dǎo)致方差不一致性。但是，新的系統(tǒng)矩陣中不再有比力項(xiàng)fn，而是被重力項(xiàng)gn所替代。新的基于狀態(tài)變換的卡爾曼濾波方式可以避免由于比力量化噪聲較大帶來的系統(tǒng)矩陣計(jì)算不精確的問題，因?yàn)閷σ话爿d體的當(dāng)?shù)鼐植繉?dǎo)航來說，gn的變化很小，幾乎為常值。

根據(jù)新定義的速度誤差向量，可以推出位置誤差微分方程為：

其中，

根據(jù)式(5)(6)(9)(10)可以推導(dǎo)出基于ST-EKF 的組合導(dǎo)航系統(tǒng)在NED 坐標(biāo)系下的慣導(dǎo)誤差狀態(tài)方程為：

其中，系統(tǒng)矩陣為F，慣導(dǎo)誤差狀態(tài)向量為x，噪聲轉(zhuǎn)移矩陣為G，過程噪聲向量為w，其對應(yīng)的協(xié)方差矩陣為Q。它們的定義如下：

2 NED 坐標(biāo)系基于ST-EKF 的慣性/衛(wèi)星松組合觀測方程

基于ST-EKF 的慣性/衛(wèi)星松組合觀測模型可以設(shè)計(jì)為δz=H x+υ，其中：

式(23)中的H矩陣是根據(jù)式(24)得到。

其中，δzv和δzr分別為慣導(dǎo)與衛(wèi)星之間的速度誤差和位置誤差，υv,3×1和υr,3×1為對應(yīng)的觀測白噪聲。由于卡爾曼濾波估計(jì)的是新的速度誤差狀態(tài)，所以，濾波之后速度狀態(tài)的更新應(yīng)該根據(jù)式(25)進(jìn)行：

姿態(tài)和位置的校正則與傳統(tǒng)的EKF 相同[13]。

3 實(shí)驗(yàn)結(jié)果與分析

3.1 單位置對準(zhǔn)實(shí)驗(yàn)

本節(jié)通過基座振動(dòng)環(huán)境的單位置精對準(zhǔn)實(shí)驗(yàn)來驗(yàn)證所提出的狀態(tài)變換卡爾曼濾波的有效性。慣性測量單元為激光陀螺IMU，性能指標(biāo)參數(shù)如表1所示。

表1 激光陀螺IMU 性能指標(biāo)參數(shù) Tab.1 Specifications of the RLG IMU

捷聯(lián)慣導(dǎo)系統(tǒng)放置在測試臺上靜態(tài)測試，時(shí)間為3600 s，慣性測量單元采樣率為200 Hz。為了驗(yàn)證擴(kuò)展卡爾曼濾波EKF 和狀態(tài)變換卡爾曼濾波ST-EKF在15 狀態(tài)情況下即狀態(tài)不完全可觀時(shí)的基座振動(dòng)環(huán)境下的對準(zhǔn)結(jié)果，對原始的比力測量數(shù)據(jù)加1 mm/s,10 Hz 的正弦線振動(dòng)。GPS 的觀測頻率為1 Hz，并在速度和位置觀測中加入速度誤差0.01 m/s(1σ)，位置誤差1 m(1σ)。EKF 和ST-EKF 的預(yù)測頻率和更新頻率均設(shè)置為1Hz。實(shí)際上，提高EKF 的預(yù)測頻率可以改善其估計(jì)精度，但是這種方案并不是完全有效的[12]。

為了對比15 狀態(tài)EKF 和15 狀態(tài)ST-EKF 的結(jié)果，以5 狀態(tài)（3 個(gè)姿態(tài)角誤差，2 個(gè)水平速度誤差） EKF精對準(zhǔn)的結(jié)果作為對比參考值，此時(shí)狀態(tài)是完全可觀的。5 狀態(tài)EKF、15 狀態(tài)EKF 和15 狀態(tài)ST-EKF均在3 min 解析粗對準(zhǔn)之后開始卡爾曼濾波精對準(zhǔn)。由于對準(zhǔn)過程中往往航向角是最難估計(jì)的，所以這里僅對比觀察航向角的收斂結(jié)果。EKF 和ST-EKF 的航向角估計(jì)結(jié)果如圖1所示。

圖1 單位置精對準(zhǔn)EKF 和ST-EKF 的航向角估計(jì)曲線 Fig.1 Yaw curves of EKF and ST-EKF in single-position fine alignment

圖1表明，在1800 s 以內(nèi)，5 狀態(tài)EKF、15 狀態(tài)EKF 和15 狀態(tài)ST-EKF 的航向角估計(jì)均正常。實(shí)際上5 狀態(tài)ST-EKF 的航向角估計(jì)結(jié)果與5 狀態(tài)EKF 結(jié)果類似，這里不再過多展示。但是，在1800 s 之后，15狀態(tài)EKF 所估計(jì)的航向角逐漸發(fā)散，最終在3600 s 時(shí)，15 狀態(tài)EKF 與5 狀態(tài)EKF 的航向角差為0.0247°，而15狀態(tài)ST-EKF與5狀態(tài)EKF的航向角差僅為0.0009°。

圖2 東向陀螺漂移誤差標(biāo)準(zhǔn)差曲線 Fig.2 Standard deviation curve of the eastward gyro bias error

圖2表明，15 狀態(tài)EKF 的估計(jì)中，東向陀螺漂移誤差的標(biāo)準(zhǔn)差是逐漸減少的，而理論上不應(yīng)該減少。準(zhǔn)靜態(tài)的基座振動(dòng)環(huán)境下，東向陀螺漂移是不可觀測的，而且東向陀螺漂移和航向角誤差是緊密相關(guān)的，由于東向陀螺漂移誤差的標(biāo)準(zhǔn)差進(jìn)行了錯(cuò)誤的估計(jì)，進(jìn) 而導(dǎo)致15 狀態(tài)EKF 的航向角在1800 s 之后發(fā)散。但是，15 狀態(tài)ST-EKF 具有較好的東向陀螺漂移誤差標(biāo)準(zhǔn)差估計(jì)特性，如圖2所示，保持在0.01 (°)/h 附近，所以東向陀螺漂移誤差的標(biāo)準(zhǔn)差保持特性使得ST-EKF 的航向角估計(jì)不會(huì)因?yàn)椴豢捎^的狀態(tài)產(chǎn)生錯(cuò)誤的校正。

盡管5 狀態(tài)EKF 在對準(zhǔn)過程中不會(huì)出現(xiàn)濾波不一致的問題，但是5 狀態(tài)卡爾曼濾波不適合對準(zhǔn)之后的組合導(dǎo)航過程，因?yàn)樾枰ㄟ^15 狀態(tài)組合導(dǎo)航濾波進(jìn)行初始對準(zhǔn)和在組合導(dǎo)航過程中估計(jì)陀螺儀、加速度計(jì)的零偏誤差，提高組合導(dǎo)航精度。當(dāng)長時(shí)間沿直線行駛或停車時(shí)，15 狀態(tài)組合導(dǎo)航濾波誤差狀態(tài)不完全可觀。傳統(tǒng)卡爾曼濾波在計(jì)算方差陣時(shí)，由于狀態(tài)轉(zhuǎn)移矩陣中的比力計(jì)算誤差，造成方差陣的計(jì)算誤差，使本來不可觀的等效東向陀螺漂移得到錯(cuò)誤的修正，從而造成圖2的現(xiàn)象。沿直線行駛或停車時(shí)間越長，現(xiàn)象越明顯。類似現(xiàn)象及描述見文獻(xiàn)[4]和[16]。而15狀態(tài)ST-EKF 適用于對準(zhǔn)之后直接進(jìn)行組合導(dǎo)航，不需要進(jìn)行濾波狀態(tài)的切換。

3.2 雙位置對準(zhǔn)實(shí)驗(yàn)

本節(jié)進(jìn)一步通過雙位置對準(zhǔn)實(shí)驗(yàn)來驗(yàn)證ST-EKF的對準(zhǔn)性能，包括光纖陀螺IMU 3 min 雙位置對準(zhǔn)實(shí)驗(yàn)和激光陀螺IMU 3 min 雙位置對準(zhǔn)實(shí)驗(yàn)。這里的雙位置是指IMU 在一個(gè)位置靜止80 s 采集數(shù)據(jù)，再繞天向軸在20 s 內(nèi)轉(zhuǎn)動(dòng)180°之后，在另一個(gè)位置采集數(shù)據(jù)80 s，目的是為了提高慣性器件誤差的可觀測性，提高對準(zhǔn)的精度。其中光纖陀螺的零偏優(yōu)于0.003 (°)/h，激光陀螺的零偏優(yōu)于0.01 (°)/ h，采樣頻率均為200 Hz。15 狀態(tài)EKF 和15 狀態(tài)ST-EKF 的預(yù)測頻率和更新頻率均設(shè)置為1 Hz。重復(fù)6 次實(shí)驗(yàn)，光纖陀螺IMU 和激光陀螺IMU 的對準(zhǔn)結(jié)果分別如表2和表3所示。

從表2可以看出，光纖陀螺IMU 的六次實(shí)驗(yàn)中，EKF 航向角估計(jì)的標(biāo)準(zhǔn)差為0.1170°，而ST-EKF 的航向角估計(jì)標(biāo)準(zhǔn)差為0.0238°，ST-EKF 的精度要遠(yuǎn)高于EKF 的雙位置對準(zhǔn)精度。從表3可以看出，激光陀螺IMU 實(shí)驗(yàn)中，EKF 航向角估計(jì)的標(biāo)準(zhǔn)差為0.0382°，而ST-EKF 的航向角估計(jì)標(biāo)準(zhǔn)差為0.0209°。

表2 光纖陀螺IMU 雙位置對準(zhǔn)實(shí)驗(yàn)的航向角估計(jì)結(jié)果 Tab.2 Estimation results of yaw angles in fiber gyro IMU's two-position alignment experiment

表3 激光陀螺IMU 雙位置對準(zhǔn)實(shí)驗(yàn)的航向角估計(jì)結(jié)果 Tab.2 Estimation results of yaw angles in laser gyro IMU's two-position alignment experiment

兩種不同的IMU 系統(tǒng)均表明，ST-EKF 具有更優(yōu)的雙位置對準(zhǔn)精度。這是由于在IMU 轉(zhuǎn)動(dòng)過程中EKF系統(tǒng)矩陣中的比力計(jì)算不準(zhǔn)確造成了方差陣的估計(jì)不準(zhǔn)確，影響航向角的估計(jì)精度。ST-EKF 則解決了這個(gè)問題，航向角估計(jì)精度較高，可以在對準(zhǔn)之后直接進(jìn)行組合導(dǎo)航，不需要進(jìn)行濾波狀態(tài)的切換。

3.3 激光陀螺IMU/衛(wèi)星組合導(dǎo)航跑車實(shí)驗(yàn)

3.1 節(jié)和3.2 節(jié)分別從單位置對準(zhǔn)和雙位置對準(zhǔn)實(shí)驗(yàn)驗(yàn)證了ST-EKF的優(yōu)越性能。本節(jié)通過激光陀螺IMU/衛(wèi)星動(dòng)態(tài)跑車松組合導(dǎo)航實(shí)驗(yàn)來驗(yàn)證ST-EKF 的性能。激光陀螺IMU 參數(shù)如表1所示，IMU 數(shù)據(jù)輸出頻率為100 Hz，GPS 輸出位置速度頻率為1 Hz。跑車軌跡的谷歌地球顯示如圖3所示。在組合導(dǎo)航過程中，將衛(wèi)星信號人為取多段進(jìn)行斷開90 s，觀察水平位置誤差的精度。EKF 和ST-EKF 的預(yù)測頻率和更新頻率均設(shè)置為1 Hz，4 h 組合導(dǎo)航的水平位置誤差如圖4所示。

由圖4可以看出，在衛(wèi)星信號斷開的6 個(gè)時(shí)間段內(nèi)，EKF 具有比ST-EKF 更大的最大水平位置誤差。同時(shí)，ST-EKF 的總體水平位置誤差曲線都在EKF 的水平位置誤差曲線下面。所以，對于導(dǎo)航級IMU，ST-EKF 具有比EKF 更優(yōu)的組合導(dǎo)航水平位置精度。

圖3 激光陀螺IMU/衛(wèi)星組合導(dǎo)航實(shí)驗(yàn)軌跡 Fig.3 Google curve of RLG-IMU/GNSS integration experiment

圖4 激光陀螺IMU/衛(wèi)星組合導(dǎo)航EKF 和ST-EKF 的水平位置誤差對比 Fig.4 Comparisons on horizontal position errors between EKF and ST-EKF of RLG-IMU/GNSS integration experiment

3.4 MEMS-IMU/衛(wèi)星組合導(dǎo)航跑車實(shí)驗(yàn)

為了進(jìn)一步驗(yàn)證ST-EKF 的組合導(dǎo)航性能，本節(jié)設(shè)計(jì)了STIM300 MEMS-IMU/衛(wèi)星組合導(dǎo)航實(shí)驗(yàn)。STIM300 MEMS-IMU 的性能參數(shù)如表4所示。

表4 STIM300 IMU 性能指標(biāo)參數(shù) Tab.4 Specifications of the STIM300 IMU

IMU 輸出頻率為200 Hz，GPS 輸出位置速度頻率為1 Hz。跑車實(shí)驗(yàn)時(shí)長為3600 s，跑車軌跡的谷歌地球顯示如圖5所示。EKF 和ST-EKF 的預(yù)測頻率和更新頻率均設(shè)置為1 Hz，在完全相同的初始參數(shù)設(shè)置條件下，EKF 和ST-EKF 的組合導(dǎo)航水平位置誤差如圖6所示。

圖5 MEMS-IMU/衛(wèi)星組合導(dǎo)航實(shí)驗(yàn)軌跡 Fig.5 Google curve of the MEMS-IMU/GNSS integration experiment

圖6表明，在多處斷開GPS 的情況下，EKF 的最大水平位置誤差達(dá)到了55 m，而ST-EKF 的水平位置誤差均能保持30 m 以下。所以，在3.3 節(jié)和3.4 節(jié)相應(yīng)實(shí)驗(yàn)中，ST-EKF 不僅適用于導(dǎo)航級IMU 與衛(wèi)星的組合導(dǎo)航，同時(shí)也適用于MEMS-IMU/衛(wèi)星組合導(dǎo)航。ST- EKF 比EKF 具有更好的動(dòng)態(tài)適應(yīng)性，導(dǎo)航精度更高。

圖6 MEMS-IMU/衛(wèi)星組合導(dǎo)航EKF 和ST-EKF的水平位置誤差對比 Fig.6 Comparisons on horizontal position errors between EKF and ST-EKF of MEMS-IMU/GNSS integration experiment

4 結(jié) 論

本文從速度誤差定義的坐標(biāo)系一致性角度，對狀態(tài)變換卡爾曼濾波的合理性作出了進(jìn)一步的解釋，并推導(dǎo)了捷聯(lián)慣導(dǎo)的初始對準(zhǔn)和組合導(dǎo)航的松組合模型。在狀態(tài)變換卡爾曼濾波的框架下，組合導(dǎo)航濾波的預(yù)測過程不需要以犧牲系統(tǒng)矩陣的高頻預(yù)測來提高濾波的精度，預(yù)測過程和更新過程可以同時(shí)執(zhí)行。同時(shí)，對準(zhǔn)過程和組合導(dǎo)航過程可以采用統(tǒng)一的15 狀態(tài)模型，不需要切換組合導(dǎo)航狀態(tài)，提高組合導(dǎo)航的精度和穩(wěn)定性。

激光陀螺IMU 的動(dòng)基座單位置對準(zhǔn)實(shí)驗(yàn)驗(yàn)證了狀態(tài)變換卡爾曼濾波具有比傳統(tǒng)擴(kuò)展卡爾曼濾波更好的航向估計(jì)穩(wěn)定性和精度。激光陀螺IMU 和光纖陀螺IMU 雙位置對準(zhǔn)實(shí)驗(yàn)亦驗(yàn)證了狀態(tài)變換卡爾曼濾波具有比傳統(tǒng)濾波更高的對準(zhǔn)精度。

激光陀螺IMU/衛(wèi)星組合導(dǎo)航實(shí)驗(yàn)表明，在多處衛(wèi)星信號斷開的情況下，狀態(tài)變換卡爾曼濾波比傳統(tǒng)的濾波方案定位精度更高。MEMS-IMU/衛(wèi)星組合導(dǎo)航實(shí)驗(yàn)驗(yàn)證了，在多處衛(wèi)星信號斷開的情況下，傳統(tǒng)卡爾曼濾波的最大水平位置誤差達(dá)到了55 m，而狀態(tài)變換卡爾曼濾波的水平位置誤差均在30 m 以下。