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

（1. 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096；2. 東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

隨著光纖陀螺技術(shù)的飛快發(fā)展，在航海領(lǐng)域光纖捷聯(lián)慣導(dǎo)系統(tǒng)也逐步得到應(yīng)用。目前通過(guò)工藝改進(jìn)來(lái)提高光纖陀螺精度的成本越來(lái)越高，因此為提高慣性系統(tǒng)的導(dǎo)航精度，通過(guò)軟件對(duì)捷聯(lián)慣性器件誤差進(jìn)行在線建模及補(bǔ)償具有現(xiàn)實(shí)意義。采用在線標(biāo)定方法提高導(dǎo)航精度是當(dāng)前導(dǎo)航領(lǐng)域研究的熱點(diǎn)之一。

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

文獻(xiàn)[9]提出了一種抗差自適應(yīng)模型預(yù)測(cè)濾波算法，并將其應(yīng)用于組合導(dǎo)航系統(tǒng)中，仿真結(jié)果表明該算法能夠有效抑制系統(tǒng)模型誤差及觀測(cè)異常對(duì)導(dǎo)航解的影響。文獻(xiàn)[10]提出了一種CCD星敏感器輔助陀螺的在線標(biāo)定方法，采用乘性誤差四元數(shù)構(gòu)造量測(cè)量，運(yùn)用了MPF+EKF的組合濾波算法，仿真結(jié)果表明，標(biāo)定精度和速度明顯優(yōu)于傳統(tǒng)方法，但是這篇文獻(xiàn)僅研究了陀螺的相關(guān)誤差參數(shù)，未涉及加速度計(jì)的相關(guān)誤差參數(shù)。

本文在研究模型預(yù)測(cè)濾波和 Sage-Husa自適應(yīng)濾波算法優(yōu)缺點(diǎn)的基礎(chǔ)上，提出一種新的MPF-AKF算法。該算法利用MPF算法實(shí)時(shí)估計(jì)系統(tǒng)模型誤差，對(duì)系統(tǒng)模型進(jìn)行修正，提高狀態(tài)方程的準(zhǔn)確度，克服了將模型誤差假設(shè)為高斯白噪聲的局限性，利用AKF算法減小量測(cè)噪聲對(duì)在線標(biāo)定結(jié)果的影響。這種組合濾波算法能夠提高狀態(tài)估計(jì)精度，降低狀態(tài)變量維數(shù)，提高系統(tǒng)實(shí)時(shí)性。系統(tǒng)仿真實(shí)驗(yàn)進(jìn)一步表明，這種方法在提高導(dǎo)航精度的同時(shí)滿足了工程應(yīng)用對(duì)實(shí)時(shí)性的要求。

1 模型預(yù)測(cè)濾波與自適應(yīng)卡爾曼組合濾波

1.1 模型預(yù)測(cè)濾波原理分析

模型預(yù)測(cè)濾波算法的基本思想是通過(guò)比較量測(cè)輸出與預(yù)測(cè)輸出來(lái)估計(jì)相應(yīng)的模型誤差，進(jìn)而修正濾波器狀態(tài)，實(shí)現(xiàn)對(duì)真實(shí)狀態(tài)量的估計(jì)。在非線性預(yù)測(cè)濾波器中，狀態(tài)估計(jì)和輸出估計(jì)由初始模型和待標(biāo)定的模型誤差給出：

離散的量測(cè)方程可由下式表示：

將輸出方程(3)進(jìn)行泰勒級(jí)數(shù)展開，如下式：

式中，

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

1.2 Sage-Husa自適應(yīng)卡爾曼濾波

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

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

Sage-Husa自適應(yīng)卡爾曼濾波算法雖然在計(jì)算濾波增益時(shí)考慮了實(shí)際的測(cè)量值，但是這將會(huì)導(dǎo)致計(jì)算量增大，工程實(shí)用性較差，所以在Sage-Husa自適應(yīng)卡爾曼濾波算法的基礎(chǔ)上進(jìn)行簡(jiǎn)化，將系統(tǒng)過(guò)程噪聲和量測(cè)噪聲的均值向量設(shè)為零，將過(guò)程噪聲方差陣設(shè)為常數(shù)，同時(shí)取消了的計(jì)算[10]。

1.3 MPF-AKF組合濾波流程

Sage-Husa自適應(yīng)卡爾曼濾波結(jié)合模型預(yù)測(cè)濾波得到系統(tǒng)狀態(tài)的一步預(yù)測(cè)狀態(tài)量為：

最優(yōu)濾波值為：

偏差為：

一步預(yù)測(cè)誤差方差陣為：

新的信息為：

采用 Sage-Husa自適應(yīng)卡爾曼濾波方法對(duì)量測(cè)噪聲方差陣進(jìn)行動(dòng)態(tài)估計(jì)：

卡爾曼濾波增益陣為：

系統(tǒng)的估計(jì)誤差方差陣為：

2 基于MPF-AKF的在線標(biāo)定方法設(shè)計(jì)

2.1 慣性器件誤差模型

由于安裝誤差在實(shí)驗(yàn)室標(biāo)定后基本不隨器件的老化發(fā)生變化，因此這里考慮的慣性器件誤差包括器件常值誤差和刻度因數(shù)誤差。

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

建立包含常值誤差和刻度因數(shù)誤差的加速度計(jì)輸出誤差模型：

2.2 在線標(biāo)定系統(tǒng)模型

2.2.1 系統(tǒng)狀態(tài)方程

選取東北天地理坐標(biāo)系為導(dǎo)航坐標(biāo)系，選取SINS導(dǎo)航誤差為狀態(tài)量，包括速度誤差、姿態(tài)誤差和位置誤差，建立狀態(tài)方程為：

2.2.2 系統(tǒng)量測(cè)方程

以計(jì)程儀提供的水平速度和平臺(tái)羅經(jīng)提供的航向角為高精度外部信息源，將 SINS輸出的水平速度、航向角與外部信息中的水平速度、航向角的差值作為量測(cè)量，建立量測(cè)方程，其中 SINS誤差方程表示的是計(jì)算機(jī)中的導(dǎo)航坐標(biāo)系與真實(shí)導(dǎo)航坐標(biāo)系之間的平臺(tái)誤差角、、，所以建立航向角誤差與平臺(tái)誤差角之間的關(guān)系為：

那么，系統(tǒng)量測(cè)方程可表示如下：

2.3 可觀測(cè)性

根據(jù)文獻(xiàn)[11]，以俯仰為例，進(jìn)行可觀測(cè)性說(shuō)明。

對(duì)式(33)進(jìn)行積分可得：

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

其他不同機(jī)動(dòng)方式下慣性器件誤差的可觀測(cè)性如表1所示。

表1 慣性器件誤差在不同機(jī)動(dòng)方式的可觀測(cè)性Tab.1 Observability of IMU errors in different maneuvers

3 仿真驗(yàn)證及分析

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

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

圖2 陀螺儀標(biāo)定結(jié)果Fig.2 Calibration results of gyroscopes

圖3 加速度計(jì)標(biāo)定結(jié)果Fig.3 Calibration results of accelerometers

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

表2 陀螺誤差模型參數(shù)估計(jì)結(jié)果Tab.2 Parameter estimation results of gyro error model

表3 加速度計(jì)誤差模型參數(shù)估計(jì)結(jié)果Tab.3 Parameter estimation results of accelerometer error model

根據(jù)已設(shè)定的軌跡設(shè)置不同的外部條件進(jìn)行仿真分析。對(duì)照組中系統(tǒng)的速度量測(cè)噪聲為1 m/s，姿態(tài)量測(cè)噪聲為1°，仿真結(jié)果如圖4~5所示。

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

圖4 對(duì)照組陀螺儀標(biāo)定結(jié)果Fig.4 Calibration results of contrasted gyroscopes

圖5 對(duì)照組加速度計(jì)標(biāo)定結(jié)果Fig.5 Calibration results of contrasted accelerometers

4 結(jié) 論

為了提高在線標(biāo)定的精度，本文采用“速度+航向角”匹配方式，并采用MPF-AKF組合濾波算法進(jìn)行在線標(biāo)定的仿真實(shí)驗(yàn)。仿真結(jié)果表明，本文提出的MPF-AKF組合濾波算法在理論上是可行的，濾波穩(wěn)定，能夠估計(jì)出誤差模型中的參數(shù)，滿足標(biāo)定精度要求，收斂速度和濾波精度均明顯優(yōu)于EKF，達(dá)到預(yù)期效果，同時(shí)MPF算法使系統(tǒng)維數(shù)降低，減少了計(jì)算時(shí)間。

（

）：

[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] 谷豐, 房建成. 非線性卡爾曼濾波方法的實(shí)驗(yàn)比較[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] 薛麗, 高社生. 自適應(yīng)Sage-Husa粒子濾波及其在組合導(dǎo)航中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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] 高社生, 宋飛彪, 姜微微. 抗差自適應(yīng)模型預(yù)測(cè)濾波及其在組合導(dǎo)航中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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星敏感器輔助光纖陀螺在線標(biāo)定技術(shù)[J]. 系統(tǒng)工程與電子技術(shù),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] 孫偉強(qiáng), 張禮偉. 激光陀螺慣組系統(tǒng)級(jí)標(biāo)定方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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.