一種新型自適應模型預測組合濾波在在線標定中的應用

徐曉蘇，吳 梅，張 濤，李 瑤，王捍兵

（1. 微慣性儀表與先進導航技術教育部重點實驗室，南京 210096；2. 東南大學 儀器科學與工程學院，南京 210096）

隨著光纖陀螺技術的飛快發展，在航海領域光纖捷聯慣導系統也逐步得到應用。目前通過工藝改進來提高光纖陀螺精度的成本越來越高，因此為提高慣性系統的導航精度，通過軟件對捷聯慣性器件誤差進行在線建模及補償具有現實意義。采用在線標定方法提高導航精度是當前導航領域研究的熱點之一。

目前，在線標定一般采用卡爾曼濾波（Kalman Filter, KF）和擴展卡爾曼濾波（Extended Kalman Filter,EKF）算法進行參數估計。KF和EKF算法不但要求模型準確，而且要求系統噪聲和量測噪聲均為高斯白噪聲，這種做法會降低狀態估計精確度，影響濾波器的最優性，當系統是時變的且噪聲統計信息未知時，甚至可能導致濾波估計發散[1]。高精度濾波算法對提高標定精度有重要影響，因此近年來許多學者展開了各種先進濾波方法在在線標定中的應用研究，其中：無跡卡爾曼濾波（Unscented Kalman Filter, UKF）是一種典型的非線性變換估計方法，濾波精度比KF和EKF算法優越，但需要計算大量統計樣本，計算量過大，難以在工程實際中應用[2-3]；粒子濾波存在運算實時性不高、狀態初始概率如何選取的問題，使其距離工程應用還有一定差距[4]；Sage-Husa自適應濾波是近幾年發展起來的一種針對不確定系統過程噪聲方差陣和量測噪聲方差陣的經典算法[5-6]；模型預測濾波是一種基于最小模型誤差準則的實時非線性濾波算法，能夠實時對模型誤差進行估計，進而修正系統模型，并且克服了將模型誤差和系統噪聲假設為高斯白噪聲的局限[7-8]。

文獻[9]提出了一種抗差自適應模型預測濾波算法，并將其應用于組合導航系統中，仿真結果表明該算法能夠有效抑制系統模型誤差及觀測異常對導航解的影響。文獻[10]提出了一種CCD星敏感器輔助陀螺的在線標定方法，采用乘性誤差四元數構造量測量，運用了MPF+EKF的組合濾波算法，仿真結果表明，標定精度和速度明顯優于傳統方法，但是這篇文獻僅研究了陀螺的相關誤差參數，未涉及加速度計的相關誤差參數。

本文在研究模型預測濾波和 Sage-Husa自適應濾波算法優缺點的基礎上，提出一種新的MPF-AKF算法。該算法利用MPF算法實時估計系統模型誤差，對系統模型進行修正，提高狀態方程的準確度，克服了將模型誤差假設為高斯白噪聲的局限性，利用AKF算法減小量測噪聲對在線標定結果的影響。這種組合濾波算法能夠提高狀態估計精度，降低狀態變量維數，提高系統實時性。系統仿真實驗進一步表明，這種方法在提高導航精度的同時滿足了工程應用對實時性的要求。

1 模型預測濾波與自適應卡爾曼組合濾波

1.1 模型預測濾波原理分析

模型預測濾波算法的基本思想是通過比較量測輸出與預測輸出來估計相應的模型誤差，進而修正濾波器狀態，實現對真實狀態量的估計。在非線性預測濾波器中，狀態估計和輸出估計由初始模型和待標定的模型誤差給出：

離散的量測方程可由下式表示：

將輸出方程(3)進行泰勒級數展開，如下式：

式中，

根據式(4)和式(8)可得到模型誤差的求解公式：

1.2 Sage-Husa自適應卡爾曼濾波

帶遺忘因子的Sage-Husa自適應卡爾曼濾波與傳統卡爾曼濾波基本方程的差異主要在于，系統過程噪聲和量測噪聲的均值向量不為零。同時實時更新過程噪聲方差陣和量測噪聲方差陣，即：

在離散型卡爾曼濾波五個方程的基礎上增加如下公式：

Sage-Husa自適應卡爾曼濾波算法雖然在計算濾波增益時考慮了實際的測量值，但是這將會導致計算量增大，工程實用性較差，所以在Sage-Husa自適應卡爾曼濾波算法的基礎上進行簡化，將系統過程噪聲和量測噪聲的均值向量設為零，將過程噪聲方差陣設為常數，同時取消了的計算[10]。

1.3 MPF-AKF組合濾波流程

Sage-Husa自適應卡爾曼濾波結合模型預測濾波得到系統狀態的一步預測狀態量為：

最優濾波值為：

偏差為：

一步預測誤差方差陣為：

新的信息為：

采用 Sage-Husa自適應卡爾曼濾波方法對量測噪聲方差陣進行動態估計：

卡爾曼濾波增益陣為：

系統的估計誤差方差陣為：

2 基于MPF-AKF的在線標定方法設計

2.1 慣性器件誤差模型

由于安裝誤差在實驗室標定后基本不隨器件的老化發生變化，因此這里考慮的慣性器件誤差包括器件常值誤差和刻度因數誤差。

建立包含常值誤差和刻度因數誤差的光纖陀螺輸出誤差模型：

建立包含常值誤差和刻度因數誤差的加速度計輸出誤差模型：

2.2 在線標定系統模型

2.2.1 系統狀態方程

選取東北天地理坐標系為導航坐標系，選取SINS導航誤差為狀態量，包括速度誤差、姿態誤差和位置誤差，建立狀態方程為：

2.2.2 系統量測方程

以計程儀提供的水平速度和平臺羅經提供的航向角為高精度外部信息源，將 SINS輸出的水平速度、航向角與外部信息中的水平速度、航向角的差值作為量測量，建立量測方程，其中 SINS誤差方程表示的是計算機中的導航坐標系與真實導航坐標系之間的平臺誤差角、、，所以建立航向角誤差與平臺誤差角之間的關系為：

那么，系統量測方程可表示如下：

2.3 可觀測性

根據文獻[11]，以俯仰為例，進行可觀測性說明。

對式(33)進行積分可得：

式(34)表明了陀螺誤差在俯仰時的可觀測性。

其他不同機動方式下慣性器件誤差的可觀測性如表1所示。

表1 慣性器件誤差在不同機動方式的可觀測性Tab.1 Observability of IMU errors in different maneuvers

3 仿真驗證及分析

首先設計載體的行駛軌跡，盡量模擬真實運動環境，綜合設計了靜止、勻加速、勻速、上抬、俯仰、轉彎時伴有橫滾等多種機動方式。假設初始位置為東經118.78，北緯32.06，初始速度為0 m/s，初始航向角、俯仰角和橫滾角均為0(隨機設定)。利用 Matlab仿真生成行駛軌跡，如圖1所示。

基于圖1所示軌跡，對本文中所研究的在線標定方法進行仿真驗證。設光纖陀螺的常值誤差為0.1 (°)/h，隨機游走為0.001(°)/h1/2，刻度因數誤差為 100×10-6；加速度常值誤差為 50 μg，隨機游走為，刻度因數誤差為 100×10-6；系統的速度量測噪聲為0.1 m/s，姿態量測噪聲為0.1°。仿真結果如圖2~3和表2~3所示。

圖2 陀螺儀標定結果Fig.2 Calibration results of gyroscopes

圖3 加速度計標定結果Fig.3 Calibration results of accelerometers

由圖 2~3和表 2~3可以看出，采用MPF-AKF組合濾波方法標定后，陀螺常值誤差在0.001 (°)/h級別上，加速度計的常值誤差小于0.4μg，陀螺的刻度因數誤差小于6×10-6，加速度計的刻度因數誤差小于5×10-6，標定結果理想，且均收斂于設定值。與傳統EKF濾波相比，采用MPF-AKF組合濾波使得收斂速度更快，收斂精度更高，穩定性更好。

表2 陀螺誤差模型參數估計結果Tab.2 Parameter estimation results of gyro error model

表3 加速度計誤差模型參數估計結果Tab.3 Parameter estimation results of accelerometer error model

根據已設定的軌跡設置不同的外部條件進行仿真分析。對照組中系統的速度量測噪聲為1 m/s，姿態量測噪聲為1°，仿真結果如圖4~5所示。

由圖2~5可以看出，相較于傳統的EKF濾波方法，速度和姿態量測噪聲較大時，采用MPF-AKF組合濾波方法標定，所有IMU誤差的標定值均與設置值相差較小，標定精度和收斂速度受外部信息精度影響較小。

圖4 對照組陀螺儀標定結果Fig.4 Calibration results of contrasted gyroscopes

圖5 對照組加速度計標定結果Fig.5 Calibration results of contrasted accelerometers

4 結 論

為了提高在線標定的精度，本文采用“速度+航向角”匹配方式，并采用MPF-AKF組合濾波算法進行在線標定的仿真實驗。仿真結果表明，本文提出的MPF-AKF組合濾波算法在理論上是可行的，濾波穩定，能夠估計出誤差模型中的參數，滿足標定精度要求，收斂速度和濾波精度均明顯優于EKF，達到預期效果，同時MPF算法使系統維數降低，減少了計算時間。

（

）：

[1] Wu X J, Wang X L. A SINS/CNS deep integrated navigation method based on mathematiccal horizon reference[J]. Aircraft Engineer and Aerospace Technology, 2011, 29(8): 26-34.

[2] Moradi E. Design a low price and high performance sensor network using constrained-based unscented Kalman filter[C]//International Conference on Control Automation and Systems.2010: 124-129.

[3] 谷豐, 房建成. 非線性卡爾曼濾波方法的實驗比較[J]. 決策與控制, 2014, 29(8):1388-1393.Gu F, Fang J C. Experimental investigation and comparison of nonlinear Kalman filters[J].Control and Decision, 2014, 29(8): 1388-1393.

[4] Sawabe W. Application of particle filter to autonomous navigation system for outdoor environment[C]//2008 SICE Annual Conference.2008: 93-96.

[5] Narasimhappa M, Rangababu R. A modified Sage-Husa adaptive Kalman filter for denoising Fiber Optic Gyroscope signal[C]//2012 Annual IEEE India Conference. 2012: 643-1283.

[6] 薛麗, 高社生. 自適應Sage-Husa粒子濾波及其在組合導航中的應用[J]. 中國慣性技術學報, 2013, 21(1): 84-88.Xue L, Gao S S. Adaptive Sage-Husa particle filtering and its application[J]. Journal of Chinese Inertial Technology, 2013, 21(1):84-88.

[7] Crassidid J L, Markley F L. Predictive filtering for nonlinear system[J]. Journal of Guidance Control and Dynamics, 1997, 20(3):566-572.

[8] Sanaz M, Javad P, Mohanmmad R J, et al.Nonlinear model predictive control of a pH neutralization process based on Wiener-Laguerre model[J]. Chemical Engineering Journal, 2009, 146(3): 328-337.

[9] 高社生, 宋飛彪, 姜微微. 抗差自適應模型預測濾波及其在組合導航中的應用[J]. 中國慣性技術學報, 2011, 19(6): 701-705.Gao S S, Song F B, Jiang W W. Robust adaptive model predictive filtering algorithm and application to integrated navigation[J].Journal of Chinese Inertial Technology, 2011,19(6): 701-705.

[10] 高偉, 林星辰. CCD星敏感器輔助光纖陀螺在線標定技術[J]. 系統工程與電子技術,2012, 34(8): 1680-1683.Gao W, Lin X C. Ecperimental investigation and comparison of nonlinear Kalman filters[J].Control and Decision, 2014, 29(8): 1388-1393.

[11] 孫偉強, 張禮偉. 激光陀螺慣組系統級標定方法[J]. 中國慣性技術學報, 2016, 24(1): 9-13.Sun W Q, Zhang L W. Systematic calibration method for RLG inertial measurement unit[J].Journal of Chinese Inertial Technology, 2016,24(1): 9-13.