基于ST-EKF與濾波增益約束的加速度計(jì)微g級標(biāo)定

郭 航，吳文啟，王茂松，崔加瑞

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

0 引言

在系統(tǒng)級標(biāo)定中考慮加速度計(jì)組件的非線性誤差，精確標(biāo)定包括二次項(xiàng)誤差、振擺誤差在內(nèi)的各項(xiàng)誤差，將有效提高導(dǎo)航精度。尤其，當(dāng)捷聯(lián)慣性導(dǎo)航系統(tǒng)慣性測量單元(Inertial Measurement Unit，IMU)處于大水平傾角情況下，二次項(xiàng)誤差、振擺誤差將成為影響加速度計(jì)組件輸出精度的重要誤差源，對二次項(xiàng)誤差、振擺誤差的精確標(biāo)定與補(bǔ)償將極大提高大水平傾角下慣導(dǎo)系統(tǒng)的導(dǎo)航精度。

慣性測量單元的系統(tǒng)級標(biāo)定主要是建立包含待估計(jì)誤差系數(shù)在內(nèi)的誤差模型，通過將如速度誤差、位置誤差等導(dǎo)航輸出誤差作為觀測量對慣性器件的誤差參數(shù)進(jìn)行估計(jì)。文獻(xiàn)[1]指出了系統(tǒng)級標(biāo)定相對于分立式標(biāo)定的優(yōu)勢，表明系統(tǒng)級標(biāo)定不需要依賴高精度轉(zhuǎn)臺，適用于自標(biāo)定與外場標(biāo)定，因而得到國內(nèi)外諸多學(xué)者的深入研究。文獻(xiàn)[2]～文獻(xiàn)[6]對慣性測量單元的系統(tǒng)級標(biāo)定技術(shù)進(jìn)行了相關(guān)研究，文獻(xiàn)[2]提出了一種基于逆向?qū)Ш降碾p軸旋轉(zhuǎn)慣導(dǎo)系統(tǒng)自標(biāo)定方法，但是只完成了慣性測量單元部分誤差參數(shù)的系統(tǒng)級標(biāo)定；文獻(xiàn)[3]針對雙軸旋轉(zhuǎn)激光慣導(dǎo)系統(tǒng)提出了一種在線自標(biāo)定方法，但是沒有對IMU振擺誤差進(jìn)行標(biāo)定；文獻(xiàn)[4]考慮了內(nèi)外桿臂誤差，針對激光捷聯(lián)慣導(dǎo)系統(tǒng)提出了一種高階Kalman濾波的系統(tǒng)級標(biāo)定方法；文獻(xiàn)[5]提出了一種八位置的系統(tǒng)級標(biāo)定方法，考慮了溫度的影響，但只估計(jì)了慣性測量元件的線性誤差；文獻(xiàn)[6]總結(jié)了慣性測量元件的各種標(biāo)定方法以及標(biāo)定技術(shù)的發(fā)展方向。可以看出，文獻(xiàn)鮮少同時(shí)考慮將加速度計(jì)組件的二次項(xiàng)誤差、振擺誤差作為狀態(tài)變量進(jìn)行估計(jì)，一方面是考慮到加速度計(jì)組件振擺誤差的弱可觀性，會(huì)導(dǎo)致通過傳統(tǒng)擴(kuò)展 Kalman濾波(Extended Kalman filter，EKF)算法所估計(jì)出的結(jié)果不準(zhǔn)確；另一方面僅僅標(biāo)定出加速度計(jì)的線性誤差或進(jìn)一步標(biāo)定出加速度計(jì)的二次項(xiàng)誤差即可滿足一般慣性導(dǎo)航的實(shí)際精度需求。為了提高狀態(tài)變量估計(jì)的準(zhǔn)確性，國內(nèi)外學(xué)者也做了許多研究工作，例如，文獻(xiàn)[7]為解決動(dòng)態(tài)環(huán)境下濾波的局部可觀測性與誤差協(xié)方差矩陣估計(jì)不一致問題，提出了一種狀態(tài)變換擴(kuò)展Kalman濾波(State Transformation Extended Kalman Filter，ST-EKF)算法；文獻(xiàn)[8]為實(shí)現(xiàn)濾波增益矩陣與濾波狀態(tài)估計(jì)可觀測性的一致性，從而提高定位精度與姿態(tài)跟蹤精度而提出了Schmidt-Kalman濾波(Schmidt Kalman Filter)；文獻(xiàn)[9]為解決傳統(tǒng)Kalman濾波器對協(xié)方差估計(jì)不一致造成對弱可觀或不可觀狀態(tài)變量的錯(cuò)誤估計(jì)問題，引入了可觀測約束Kalman濾波等。

本文通過三個(gè)方面來實(shí)現(xiàn)對加速度計(jì)組件的微g級精確標(biāo)定：1)推導(dǎo)并建立包含標(biāo)度因數(shù)誤差、安裝誤差、零偏、二次項(xiàng)誤差、振擺誤差在內(nèi)的加速度組件誤差模型；2)將1)中建立的誤差模型中所有誤差參數(shù)作為狀態(tài)變量構(gòu)建基于狀態(tài)變換擴(kuò)展Kalman濾波(ST-EKF)的系統(tǒng)方程，以導(dǎo)航解算速度作為量測值，激勵(lì)出待估計(jì)誤差參數(shù)；3)為進(jìn)一步解決濾波狀態(tài)協(xié)方差陣計(jì)算誤差問題，提高待估計(jì)誤差參數(shù)的準(zhǔn)確性，采用以Schmidt濾波為基礎(chǔ)對濾波增益矩陣進(jìn)行約束的方法，通過調(diào)整濾波增益矩陣K的值，使得在編排的23個(gè)非傾斜位置上估計(jì)出標(biāo)度因數(shù)誤差、安裝誤差、零偏、二次項(xiàng)誤差，在12個(gè)傾斜位置上估計(jì)出振擺誤差。相比于傳統(tǒng)的基于擴(kuò)展Kalman濾波的系統(tǒng)級標(biāo)定算法，狀態(tài)變換擴(kuò)展Kalman濾波(ST-EKF)通過將濾波器狀態(tài)轉(zhuǎn)移矩陣中的比力項(xiàng)變換為當(dāng)?shù)刂亓铀俣龋鉀Q了轉(zhuǎn)臺在翻滾時(shí)狀態(tài)轉(zhuǎn)移矩陣計(jì)算的不準(zhǔn)確對濾波精度的影響。根據(jù)系統(tǒng)級標(biāo)定過程中狀態(tài)的實(shí)際可觀測性對濾波增益進(jìn)行約束，避免了對弱可觀或不可觀狀態(tài)的錯(cuò)誤估計(jì)，使參數(shù)估計(jì)更加準(zhǔn)確。

1 一種基于ST-EKF與增益約束的加速度組件系統(tǒng)級標(biāo)定方法

使用Kalman濾波算法對加速度計(jì)組件進(jìn)行各項(xiàng)參數(shù)的估計(jì)，應(yīng)用于實(shí)際系統(tǒng)長時(shí)間使用后需要進(jìn)行精確標(biāo)定的場景。由于捷聯(lián)慣導(dǎo)在出廠前已經(jīng)經(jīng)過了一次標(biāo)定，于是再標(biāo)定時(shí)的各誤差將都是微小量。

取靜止?fàn)顟B(tài)下慣導(dǎo)解算的速度與實(shí)際速度的差作為觀測量，觀測方程如下所示

于是得到觀測矩陣H

式(7)、式(8)中，Z(t)為觀測向量，ν為隨機(jī)白噪聲，其他符號與上文中的定義保持一致。

系統(tǒng)級標(biāo)定采用多位置靜止標(biāo)定，標(biāo)定過程通過陀螺模值來區(qū)分靜止數(shù)據(jù)段與翻滾數(shù)據(jù)段。

理論上，弱可觀誤差狀態(tài)估計(jì)的準(zhǔn)確性更加依賴于狀態(tài)協(xié)方差陣計(jì)算的準(zhǔn)確性。傳統(tǒng)的基于EKF的系統(tǒng)級標(biāo)定算法由于對狀態(tài)協(xié)方差陣P的計(jì)算不夠準(zhǔn)確，從而不適用于對弱可觀狀態(tài)的精確估計(jì)。然而，基于ST-EKF的系統(tǒng)級標(biāo)定算法將導(dǎo)航誤差方程中的比力項(xiàng)用重力項(xiàng)替代[10]，可更準(zhǔn)確計(jì)算出狀態(tài)協(xié)方差陣P。 同時(shí)，將ST-EKF與濾波增益約束相結(jié)合，進(jìn)一步避免了在非傾斜位置上對振擺誤差的錯(cuò)誤估計(jì)，同時(shí)在其他誤差狀態(tài)估計(jì)結(jié)束后，通過斜置IMU使弱可觀的振擺誤差最終得到準(zhǔn)確估計(jì)。

由振擺誤差的來源機(jī)理可知，當(dāng)IMU處于非傾斜位置時(shí)，將激勵(lì)不出振擺誤差，因此如果整體估計(jì)，將會(huì)由于協(xié)方差矩陣的計(jì)算誤差造成在非傾斜位置上估計(jì)出不準(zhǔn)確的振擺誤差系數(shù)。于是借鑒Schmidt濾波器的原理，調(diào)整與狀態(tài)變量對應(yīng)的濾波增益變化，在非傾斜位置(式(9))和傾斜位置(式(10))上設(shè)置濾波增益矩陣

式(9)、式(10)中，Kk為tk時(shí)刻的濾波增益矩陣，為根據(jù)狀態(tài)可觀測性借鑒Schmidt-Kalman濾波約束后的tk時(shí)刻的濾波增益矩陣。從而在非傾斜位置上只估計(jì)出標(biāo)度因數(shù)誤差、安裝誤差、零偏、二次項(xiàng)誤差，在傾斜位置上只估計(jì)振擺誤差而不改變已經(jīng)估計(jì)出來的其他各項(xiàng)誤差參數(shù)。

判斷加速度組件是否處于傾斜位置，可以通過各軸輸出比力值與當(dāng)?shù)刂亓铀俣饶Ｖ档拇笮砼袛唷Ｔ趯?shí)驗(yàn)室內(nèi)標(biāo)定的加速度計(jì)尺寸效應(yīng)參數(shù)不會(huì)變化，再次標(biāo)定時(shí)不考慮該誤差系數(shù)的變化。

2 實(shí)驗(yàn)驗(yàn)證與分析

2.1 實(shí)驗(yàn)設(shè)備與條件

為驗(yàn)證本文提出的一種基于ST-EKF與濾波增益約束系統(tǒng)級標(biāo)定算法的有效性，需借助SGT-3三軸慣導(dǎo)測試轉(zhuǎn)臺實(shí)現(xiàn)對慣導(dǎo)系統(tǒng)的位置編排。為減少溫度對慣性器件各項(xiàng)誤差的影響，在進(jìn)行標(biāo)定實(shí)驗(yàn)之前充分預(yù)熱，保證慣導(dǎo)系統(tǒng)溫度基本穩(wěn)定后，再開展標(biāo)定實(shí)驗(yàn)。

實(shí)驗(yàn)選擇某型光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)，加速度計(jì)與陀螺的指標(biāo)參數(shù)如表1、表2所示。

表1 某型慣性導(dǎo)航系統(tǒng)加速度計(jì)指標(biāo)參數(shù)Table 1 Performance parameters of accelerometer in the inertial navigation system

表2 某型慣性導(dǎo)航系統(tǒng)陀螺指標(biāo)參數(shù)Table 2 Performance parameters of gyroscope in the inertial navigation system

2.2 實(shí)驗(yàn)內(nèi)容與分析

將光纖慣導(dǎo)系統(tǒng)固定在三軸慣導(dǎo)測試轉(zhuǎn)臺上，通過編排23＋12位置估計(jì)出加速度計(jì)組件的各項(xiàng)誤差參數(shù)：23個(gè)非傾斜位置估計(jì)出加速度計(jì)組件的標(biāo)度因數(shù)誤差、安裝誤差、常值零偏、二次項(xiàng)誤差系數(shù)，12個(gè)傾斜位置估計(jì)出加速度計(jì)組件的振擺誤差系數(shù)。通過6個(gè)大水平傾角位置的重力殘差驗(yàn)證了標(biāo)定結(jié)果的正確性以及估計(jì)振擺誤差的必要性，最后對比不同補(bǔ)償模型時(shí)水平姿態(tài)角變化情況下純慣導(dǎo)解算的位置誤差，再一次驗(yàn)證了估計(jì)結(jié)果的正確性和估計(jì)振擺誤差的必要性。

標(biāo)定實(shí)驗(yàn)所需的三軸慣導(dǎo)測試轉(zhuǎn)臺如圖1所示。圖1中，光纖慣導(dǎo)系統(tǒng)被固定在轉(zhuǎn)臺上。

圖1 三軸慣導(dǎo)測試轉(zhuǎn)臺實(shí)物圖Fig.1 Physical map of three axis inertial navigation test turntable

23＋12位置編排如圖2、圖3所示。

圖2 系統(tǒng)級標(biāo)定實(shí)驗(yàn)非傾斜23位置編排Fig.2 Twenty-three non-tilting positions and rotations in systematic calibration experiments

圖3 系統(tǒng)級標(biāo)定實(shí)驗(yàn)傾斜12位置編排Fig.3 Twelve tilt positions in systematic calibration experiments

圖2、圖3給出了基于IMU的位置編排。圖2中，每個(gè)位置分別給出了當(dāng)前IMU軸系的方位、旋轉(zhuǎn)軸以及旋轉(zhuǎn)方向與角度，k＝0，1，2，…，n，本文實(shí)驗(yàn)選擇k＝0。圖3則給出了估計(jì)加速度計(jì)振擺誤差的12個(gè)傾斜位置，符號?代表垂直向內(nèi)，符號☉代表垂直向外。

基于上述的位置編排，區(qū)分開傾斜或者非傾斜位置，僅在靜態(tài)數(shù)據(jù)段以速度為零作為觀測量進(jìn)行濾波估計(jì)，得到加速度計(jì)的各項(xiàng)誤差參數(shù)的估計(jì)值，開展了三組重復(fù)性實(shí)驗(yàn)，結(jié)果如表3～表7所示。

表3 加速度計(jì)組件標(biāo)度因數(shù)標(biāo)定結(jié)果Table 3 Calibration results of accelerometer assembly scale factor

表4 加速度計(jì)組件安裝誤差標(biāo)定結(jié)果Table 4 Calibration results of accelerometer assembly installation error

表5 加速度計(jì)組件二次項(xiàng)誤差標(biāo)定結(jié)果Table 5 Calibration results of accelerometer assembly quadratic term error

表6 加速度計(jì)組件零偏標(biāo)定結(jié)果Table 6 Calibration results of accelerometer assembly bias

表3～表7給出了三次重復(fù)性實(shí)驗(yàn)中所有誤差參數(shù)的估計(jì)值：標(biāo)度因數(shù)誤差的極差約為7×10－6；安裝誤差的極差約為0.9″，二次項(xiàng)誤差的極差約為9μg／g2，零偏的極差為2.4μg，振擺誤差的極差約為12μg／g2。從統(tǒng)計(jì)結(jié)果中可以看出，基于ST-EKF與濾波增益約束的系統(tǒng)級標(biāo)定算法對誤差參數(shù)的估計(jì)具有較好的重復(fù)性。

在23＋12位置編排中，分別選擇6個(gè)傾斜位置與6個(gè)非傾斜位置來開展驗(yàn)證實(shí)驗(yàn)。

表8和圖4給出了IMU傾斜時(shí)不同標(biāo)定模型下的重力殘差模值統(tǒng)計(jì)結(jié)果和示意圖。

表8 IMU傾斜時(shí)不同標(biāo)定模型下的重力殘差模值統(tǒng)計(jì)結(jié)果Table 8 Statistical results of gravity residual modulus under different calibration models when IMU tilts

圖4 IMU傾斜時(shí)不同標(biāo)定模型下的重力殘差模值圖Fig.4 Diagram of gravity residual modulus under different calibration models when IMU tilts

由表8和圖4可知，標(biāo)定了非線性誤差項(xiàng)的加速度計(jì)組件具有更高的輸出精度。尤其，當(dāng)對加速度計(jì)組件的振擺誤差進(jìn)行標(biāo)定后，在大水平傾角下與對加速度計(jì)補(bǔ)償線性／二次項(xiàng)誤差相比，重力殘差模值的均值從12.68μg減小到7.65μg，均方差從13.38μg減小到6.71μg；而與僅補(bǔ)償線性誤差相比，重力殘差模值的均值從8.11μg減小到7.65μg， 均方差從24.12μg減小到6.71μg。

表9和圖5給出了IMU非傾斜時(shí)不同標(biāo)定模型下的重力殘差模值統(tǒng)計(jì)結(jié)果和示意圖。

表9 IMU非傾斜時(shí)不同標(biāo)定模型下的重力殘差模值統(tǒng)計(jì)結(jié)果Table 9 Statistical results of gravity residual modulus under different calibration models when IMU is not tilted

圖5 IMU非傾斜時(shí)不同標(biāo)定模型下的重力殘差模值圖Fig.5 Diagram of gravity residual modulus under different calibration models when IMU is not tilted

由表9和圖5可知，補(bǔ)償線性／二次項(xiàng)誤差與補(bǔ)償線性／二次項(xiàng)／振擺誤差的重力殘差模值的均值與均方差基本一致，且都要優(yōu)于僅補(bǔ)償線性誤差的情況。

表10和圖6給出了基于ST-EKF與EKF兩種不同系統(tǒng)級標(biāo)定算法對誤差參數(shù)估計(jì)的統(tǒng)計(jì)結(jié)果和示意圖。

表10 基于EKF與ST-EKF系統(tǒng)級標(biāo)定重力殘差模值的統(tǒng)計(jì)結(jié)果對比Table 10 Comparison of statistical results based on EKF and ST-EKF systematic calibration gravity residual modulus

圖6 EKF與ST-EKF系統(tǒng)級標(biāo)定重力殘差模值對比圖Fig.6 Comparison of gravity residual modulus in systematic calibration based on EKF and ST-EKF

由表10和圖6可知，基于ST-EKF的系統(tǒng)級標(biāo)定算法對誤差參數(shù)的估計(jì)具有更高的精度。相比于基于EKF的系統(tǒng)級標(biāo)定算法，基于ST-EKF的系統(tǒng)級標(biāo)定算法估計(jì)出來的參數(shù)補(bǔ)償加速度計(jì)后，其重力殘差模值的均值為 7.65μg，均方差為6.71μg，而基于EKF的系統(tǒng)級標(biāo)定算法其值分別為 11.39μg和 8.39μg。

表11和圖7給出了有無濾波增益約束的基于ST-EKF系統(tǒng)級標(biāo)定算法對誤差參數(shù)估計(jì)的統(tǒng)計(jì)結(jié)果和示意圖。

表11 有無濾波增益約束的ST-EKF系統(tǒng)級標(biāo)定重力殘差模值的統(tǒng)計(jì)結(jié)果對比Table 11 Comparison of statistical results based on ST-EKF systematic calibration gravity residual modulus with and without filter gain constraint

圖7 有無濾波增益約束的ST-EKF系統(tǒng)級標(biāo)定重力殘差模值對比圖Fig.7 Comparison of gravity residual modulus in systematic calibration based on ST-EKF with and without filter gain constraint

由表11和圖7可知，有無濾波增益約束的基于ST-EKF的系統(tǒng)級標(biāo)定算法對線性誤差、二次項(xiàng)誤差、振擺誤差進(jìn)行估計(jì)，并對加速度計(jì)補(bǔ)償后在大水平傾角下的重力殘差模值進(jìn)行對比，結(jié)果表明：有濾波增益約束的基于ST-EKF的系統(tǒng)級標(biāo)定算法標(biāo)定精度更高。在有濾波增益約束下，加速度計(jì)經(jīng)過補(bǔ)償后，重力殘差模值的均值為7.65μg，均方差為6.71μg；而在增益不進(jìn)行約束的情況下，其均值為10.34μg，均方差為7.96μg。

圖8為姿態(tài)角變化曲線，圖9、圖10為不同標(biāo)定模型下北向和東向位置誤差對比曲線。

圖8 姿態(tài)角變化曲線Fig.8 Change curves of attitude angles

圖9 不同標(biāo)定模型下北向位置誤差對比曲線Fig.9 Comparison curves of north position error under different calibration models

圖10 不同標(biāo)定模型下東向位置誤差對比曲線Fig.10 Comparison curves of east position error under different calibration models

由圖8～圖10可知，當(dāng)慣導(dǎo)系統(tǒng)處于大水平傾角時(shí)，將基于ST-EKF與濾波增益約束系統(tǒng)級標(biāo)定算法估計(jì)出的各項(xiàng)誤差參數(shù)通過線性／二次項(xiàng)／振擺誤差標(biāo)定模型補(bǔ)償加速度計(jì)的輸出后，其北向和東向位置的最大定位誤差得到減少。具體而言，對比僅標(biāo)定線性誤差，東向與北向最大位置誤差分別從 192.40m、96.72m 減小到 74.64m、65.44m；對比標(biāo)定線性／二次項(xiàng)誤差，東向與北向最大位置誤差則分別由173.70m、65.49m減少到74.64m、65.44m，結(jié)果證明了在大水平傾角變化下標(biāo)定二次項(xiàng)誤差與振擺誤差對提高慣導(dǎo)系統(tǒng)導(dǎo)航精度的必要性以及論文所提算法的有效性。

3 結(jié)論

隨著對導(dǎo)航定位、測姿精度的要求越來越高，對慣性器件誤差來源與標(biāo)定算法的研究也越來越深入。在高精度標(biāo)定中，當(dāng)慣導(dǎo)系統(tǒng)處于傾斜位置時(shí)，加速度計(jì)二次項(xiàng)誤差和振擺誤差對慣導(dǎo)系統(tǒng)導(dǎo)航精度的影響不容忽略，本文因此建立了包括二次項(xiàng)誤差和振擺誤差在內(nèi)的加速度計(jì)組件誤差模型，并利用ST-EKF和濾波增益約束解決了狀態(tài)轉(zhuǎn)移矩陣計(jì)算不準(zhǔn)確以及弱可觀非線性誤差項(xiàng)估計(jì)不準(zhǔn)的問題，提出了一種基于ST-EKF與濾波增益約束的系統(tǒng)級標(biāo)定算法，并通過重復(fù)性實(shí)驗(yàn)與對比實(shí)驗(yàn)驗(yàn)證了該算法在大水平傾角變化下的精度優(yōu)勢。最后，在大水平傾角變化下給出了線性、線性／二次項(xiàng)與線性／二次項(xiàng)／振擺誤差標(biāo)定模型下的東向與北向位置誤差變化曲線，曲線表明經(jīng)過線性／二次項(xiàng)／振擺誤差模型補(bǔ)償后的慣導(dǎo)系統(tǒng)具有更高的導(dǎo)航精度。