基于雙回路擴展卡爾曼濾波的慣性平臺連續自標定

王 琪，汪立新，秦偉偉，沈 強

(火箭軍工程大學，西安710025)

?

基于雙回路擴展卡爾曼濾波的慣性平臺連續自標定

王 琪，汪立新，秦偉偉，沈 強

(火箭軍工程大學，西安710025)

為解決傳統的慣性平臺連續自標定中，由于系統非線性強、狀態向量維數大引起的濾波收斂速度慢、對濾波初始條件敏感等問題，研究了一種雙回路擴展卡爾曼濾波方法。首先給出了平臺連續自標定的誤差模型；然后根據加速度計誤差與導航誤差之間的關系，對加速度計輸出進行預濾波得到加速度計輸出誤差；同時通過分析陀螺儀誤差在平臺連續自標定過程中的傳播特性，將耦合在加速度計輸出誤差中的陀螺儀誤差解耦；最后以陀螺儀誤差和加速度計輸出為觀測量，建立了陀螺儀和加速度計的擴展卡爾曼濾波方程，分別對陀螺儀和加速度計誤差系數進行標定，實現雙回路擴展卡爾曼濾波。仿真結果表明，該方法能夠在900 s內以低于0.05%的相對誤差標定出所有的平臺誤差系數，并且對濾波初始條件不敏感，可以有效地擴展連續自標定方法的實際應用。

連續自標定；擴展卡爾曼濾波；慣性平臺；誤差模型；誤差解耦

0 引 言

制導工具誤差是影響制導武器命中精度的主要因素，而慣性儀表的誤差是制導工具誤差中的主要成分，因此在使用時必須進行補償。根據陀螺儀和加速度計在長期穩定性測試中一次啟動隨機誤差遠小于多次啟動誤差的特性，提出了平臺射前自標定[1]。傳統的多位置自標定方法是一種“靜態”的標定方法，由于其誤差模型相對簡單，特別是激勵安裝誤差的地球自轉角速度相對較小，導致多位置自標定中對于安裝誤差的激勵不夠充分，標定精度較低[2-3]。

針對多位置自標定的缺點，Jackson[4]提出了連續自標定方法。慣性平臺連續自標定方法的基本原理如下：慣性平臺在外力矩的作用下以角速度ωc(稱為加矩角速度)轉動，在地球自轉角速度、加矩角速度以及重力加速度的激勵下，加速度計輸出中包含有陀螺儀誤差、加速度計誤差、安裝誤差和平臺對準誤差等全部誤差信息。以加速度計輸出為觀測量，以平臺對準誤差方程為動力學模型，采用最優濾波算法估計平臺誤差系數和對準誤差[5]。文獻[6]系統地介紹了平臺連續自標定的誤差建模、可觀測性分析、參數辨識等內容；文獻[7-8]針對大失準角情況下傳統的失準角誤差模型會產生較大誤差的問題，建立了框架角誤差模型；文獻[9-10]分別建立了以加速度計輸入軸和陀螺儀輸入軸為基準的平臺坐標系，使所有的安裝誤差變得可觀，提高了標定精度。

但是由于平臺連續自標定的誤差模型復雜、誤差系數較多[11]，因此在建立濾波模型時，會存在狀態向量維數大、系統非線性強等特點，這會導致在濾波過程中收斂速度慢、對濾波初始條件比較敏感等問題，從而限制了連續自標定方法的實際應用。針對以上問題，本文研究了一種基于雙回路擴展卡爾曼濾波的慣性平臺連續自標定方法，以提高連續自標定方法的適用性。

1 坐標系定義

首先給出平臺中的坐標系定義，假設平臺由三個單自由度積分陀螺儀與三個擺式加速度計組成，如圖1所示。

圖1 平臺陀螺儀和加速度計安裝示意圖Fig.1 Installation diagram of gyros and accelerometers in platform

設陀螺儀I軸繞其O、S軸的安裝誤差為Δoi、Δsi(i=x,y,z)，如圖2所示。則平臺坐標系(p)到陀螺儀敏感軸坐標系(g)的轉換矩陣為[12]

(1)

圖2 陀螺儀安裝誤差Fig.2 Gyro-platform geometry

如圖3所示，設加速度計I軸繞其P、O軸的安裝誤差為θpi、θoi(i=x,y,z)，則p系到加速度計敏感軸坐標系(a)的轉換矩陣為[12]

(2)

圖3 加速度計安裝誤差Fig.3 Accelerometer-platform geometry

此外，定義一個理想計算坐標系(c)：計算坐標系是一個理想的正交坐標系，作為平臺連續轉動的參考。在不考慮任何漂移的情況下，計算坐標系與平臺坐標系重合。

2 連續自標定誤差模型

2.1 姿態誤差方程

(3)

(4)

將式(4)代入式(3)，有

(5)

(6)

對式(6)左乘Φn，有

(7)

利用列向量φ、ω與其反對稱矩陣Φ、Ω之間的性質Ωφ+Φω=0，式(7)可變換為

(8)

對式(5)右乘φn，并將式(8)代入可得

(9)

對式(9)左乘(Φn)-1并省略高階項，整理可得失準角誤差方程為

(10)

綜上所述，p系相對I系的轉動角速度為

(11)

將式(11)代入式(10)可得平臺連續自標定的姿態誤差方程為

(12)

2.2 陀螺儀誤差模型

陀螺儀的誤差與其三軸上的比力有關，其誤差模型為[15]

i = x,y,z

(13)

陀螺儀三軸上的比力如下式所示

(14)

將式(14)代入式(13)可得陀螺儀誤差模型為

(15)

式中：

2.3 加速度計輸出模型

加速度計的輸出與加速度計三軸敏感到的比力有關，加速度計三軸上的比力為

(16)

加速度計輸出模型為[15]

(17)

式中：ka0i為加速度計的零偏；ka1i為加速度計一次項誤差系數；同理，未考慮加速度計二次項誤差系數；εai為測量隨機誤差。

將式(16)代入式(17)并忽略加速度計誤差系數相乘形成的二階項，可得加速度計輸出模型為

Za=Cφ+D1ka+D2θ+h(ka,φ)+gc+εa

(18)

式中：

3 誤差解耦

為實現雙回路濾波，本文選擇陀螺儀誤差和加速度計輸出兩個觀測量來分別對陀螺儀和加速度計誤差系數進行辨識。因此需要將耦合在加速度計誤差中的陀螺儀誤差進行解耦。為此首先分析陀螺儀誤差在平臺連續自標定中對加速度計輸出的影響。

3.1 陀螺儀誤差傳播分析

圖4 陀螺儀誤差對加速度計輸出的影響Fig.4 Effect on accelerometers’ output of gyros’ error

由此可知，只有沿Xp和Zp軸方向(即水平方向)的陀螺儀誤差會引起加速度計誤差，而沿Yp軸方向(即天向)的陀螺儀誤差不會對加速度計誤差產生影響。

3.2 誤差解耦

根據第3.1節得到的結論，可以采取如下的解耦方法從加速度計誤差中得到陀螺儀誤差：

(19)

yL的東向和北向分量即為陀螺儀濾波回路的量測更新，即有

(20)

式中：e和n分別為東向和北向的3×1的單位向量，由n系到c系的方向余弦組成。

(21)

4 加速度計輸出預濾波

在平臺連續自標定中，加速度計輸出是唯一的輸出量，因此需要對加速度計輸出進行預濾波來得到加速度計誤差。加速度計誤差ΔZa與導航速度誤差Δv有如下關系[18]

(22)

式中：Δv0為初始誤差。

速度誤差可由加速度計輸出減去理論輸出得到。理論上加速度計測量的是重力加速度在p系的投影，而在不考慮誤差的情況下，p系與c系重合，因此加速度計理論輸出為重力加速度在c系的投影，由此可得速度誤差為[18]

Δv=Za-gc

(23)

需要注意的是，式(23)中的Za是加速度計輸出轉換成的比力值。

設濾波周期為T，加速度計輸出離散周期為ΔT，則式(22)的離散形式可以寫為

(24)

式中：j∈[-N,N]，N=T/ΔT。

根據最小二乘法可解得加速度計誤差分量為

(25)

5 雙回路擴展卡爾曼濾波

通過第3節的誤差解耦和第4節的加速度計輸出預濾波，得到的陀螺儀誤差可以作為陀螺儀濾波回路的量測更新，同時將加速度計輸出作為加速度計濾波回路的量測更新，就可以實現雙回路卡爾曼濾波，從而極大地減小濾波過程中的計算量和加快濾波收斂速度。

5.1 陀螺儀濾波方程

(26)

式中：T為濾波周期，k表示第k個濾波周期。

(27)

5.2 加速度計濾波方程

(28)

對式(18)進行離散化可得觀測方程為

Cφk+1+D1kak+1+D2θk+1+

h(ka,k+1,φk+1)+gc,k+1+εa,k+1

(29)

在得到陀螺儀和加速度計的濾波方程后，就可以利用擴展卡爾曼濾波分別對陀螺儀和加速度計的誤差系數進行辨識，從而實現基于雙回路擴展卡爾曼濾波的慣性平臺連續自標定。

擴展卡爾曼濾波的基本方程如式(30)所示，式中：Qk是系統噪聲協方差矩陣，Rk為量測噪聲協方差矩陣，F和H分別為fe(X)和he(X)對X的偏導數。

(30)

6 仿真分析

根據前文推導的濾波方程，對本文方法進行了仿真驗證。首先設置仿真初始條件如下：

1)在連續自標定開始之前，平臺處于空間穩定的工作狀態，即始終跟蹤當地水平面；

2)濾波周期為T=0.25 s；

3)加速度計量測噪聲為1×10-6m/s2；

4)濾波初值設置為0。

由于平臺誤差系數的量值相對較小，因此定義如下的相對誤差來表示誤差系數的標定精度。

(31)

陀螺儀誤差系數標定結果如圖5所示，加速度計誤差系數標定結果如圖6所示。從圖中可以看出，本文方法可以在900s內標定出所有的陀螺儀和加速度計誤差系數，而且相對誤差都小于0.05%，驗證了方法的有效性。同時可以注意到，部分誤差系數，特別是安裝誤差的濾波值在濾波穩定后會出現周期性的波動，在圖中表現為一些周期性的“尖刺”，這是因為當加速度計接近水平位置時，加速度計輸出中噪聲比很大，從而會造成濾波結果偏離真值，在實際操作中甚至有可能導致濾波結果發散，這種情況是需要避免的。具體做法是：當加速度計接近水平位置時，斷開陀螺儀和加速度計濾波回路的量測更新，待加速度計離開水平位置一定角度后，再重新獲取量測更新值，繼續進行濾波。

圖5 陀螺儀誤差系數標定結果Fig.5 Calibration results of gyros’ error coefficients

為驗證本文方法濾波收斂速度快、對初始條件不敏感的優點，將本文方法與文獻[6]中的方法進行了仿真對比。設置了如下不同的濾波初始條件：

(1)加速度計量測噪聲設置為1×10-6m/s2，濾波初值設置為0；

(2)加速度計量測噪聲設置為1×10-6m/s2，濾波初值設置為X0/2(X0為仿真真值)；

(3)加速度計量測噪聲設置為1×10-6m/s2，濾波初值設置為X0+N(0,1)·X0(N(0,1)為服從均值為0、方差為1的正態隨機數)；

(4)加速度計量測噪聲設置為1×10-4m/s2，濾波初值設置為0。

圖6 加速度計誤差系數標定結果Fig.6 Calibration results of accelerometers’ error coefficients

不同初始條件下各參數的濾波收斂速度見表1，由于標定參數較多，表中只給出了所有參數收斂的最終時間。

表1 不同初始條件下的濾波收斂時間Table 1 Convergence time of different filter initial condition

從表1可以看出，本文方法在初始條件(1)、(2)、(3)下的濾波收斂速度明顯快于文獻[6]方法。在標定精度方面，本文方法在三種初始條件下的標定相對誤差都低于0.05%，而文獻[6]方法的相對誤差都在0.1%～1%之間。在初始條件(4)下，隨著量測噪聲的增大，文獻[6]方法中的陀螺儀安裝誤差系數Δsy、Δsz、Δox和Δoz不能有效的收斂到真值，而本文方法的標定結果基本不受影響。驗證了本文方法濾波收斂速度快、對初始條件不敏感的優點。

7 結 論

針對傳統的連續自標定方法中，由于系統非線性強、狀態向量維數大，會引起濾波收斂速度慢、對初始條件敏感等問題，本文研究了一種基于雙回路擴展卡爾曼濾波的慣性平臺連續自標定方法，以陀螺儀誤差和加速度計輸出為觀測量，將陀螺儀和加速度計分為兩個回路進行濾波，從而降低濾波方程中的狀態向量維數和系統的非線性。仿真結果表明，該方法可以加快連續自標定的濾波收斂速度和降低對濾波初始條件的敏感度，有效地提高連續自標定方法的適用性。

[1] 鐘萬登．液浮慣性器件[M]．北京：宇航出版社，1994：348-350，371-372．

[2] 楊立溪．慣性平臺誤差快速自標定技術[J]．中國慣性技術學報，2000，8(4)：1-4．[Yang Li-xi. Rapid autocalibration for the errors of inertial platform [J]. Journal of Chinese Inertial Technology, 2000, 8(4): 1-4.]

[3] 孟衛鋒，袁愛紅，賈天龍，等．高精度慣性平臺十六位置自標定方案[J]．中國慣性技術學報，2015，23(2)：150-155．[Meng Wei-feng, Yuan Ai-hong, Jia Tian-long, et al. 16-position self-calibration of high-accuracy inertial platform [J]. Journal of Chinese Inertial Technology, 2015, 23(2): 150-155.]

[4] Jackson A D. Continuous calibration and alignment techniques for an all-attitude inertial platform [C]. AIAA Guidance and Control Conference, Key Biscayne, Florida, USA, August 20-22, 1973.

[5] Cao Y, Cai H, Zhang S F, et al. A new continuous self-calibration scheme for a gimbaled inertial measurement unit [J]. Measurement Science and Technology, 2012, 23(1): 385-394.

[6] 楊華波．慣性測量系統誤差標定及分離技術研究[D]．長沙：國防科學技術大學研究生院，2008．[Yang Hua-bo. Research on error calibration and separation for inertial measurement systems [D]. Changsha: Graduate School of National University of Defense Technology, 2008.]

[7] 曹淵，張士峰，楊華波，等．慣導平臺誤差快速自標定方法研究[J]．宇航學報，2011，32(6)：1281-1287．[Cao Yuan, Zhang Shi-feng, Yang Hua-bo, et al. Research on rapid self-calibration method for inertial platform [J]. Journal of Astronautics, 2011, 32(6): 1281-1287.]

[8] 丁智堅，蔡洪，楊華波，等．浮球式慣性平臺連續翻滾自標定自對準方法[J]．國防科技大學學報，2015，37(3)：148-154．[Ding Zhi-jian, Cai Hong, Yang Hua-bo, et al. Continuous self-calibration and self-alignment method for floated inertial platform [J]. Journal of National University of Defense Technology, 2015, 37(3): 148-154.][9] 許永飛，張士峰．慣導平臺自標定中安裝誤差可觀性分析[J]．中國慣性技術學報，2015，23(5)：575-579．[Xu Yong-fei, Zhang Shi-feng. Observability analysis of IMU’s misalignment angles in self-calibration for inertial platform [J]. Journal of Chinese Inertial Technology, 2015, 23(5): 575-579.]

[10] 連丁磊，夏剛，王汀，等．基于陀螺儀敏感軸系的平臺連續翻滾自標定算法[J]．導彈與航天運載技術，2013，2：24-29．[Lian Ding-lei, Xia Gang, Wang Ting, et al. Self-calibration algorithm of inertial navigation platform continuous tumble based on gyro sensitive shaft system [J]. Missiles and Space Vehicles, 2013, 2:24-29.]

[11] 楊華波，蔡洪，張士峰，等．高精度慣性平臺誤差自標定自對準技術[J]．宇航學報，2006，27(4)：600-604．[Yang Hua-bo, Cai Hong, Zhang Shi-feng, et al. Continuous calibration and alignment techniques for a high precision inertial platform [J]. Journal of Astronautics, 2006, 27(4): 600-604.]

[12] Chana K Y, Rajakarunaa N, Engelkeb U, et al. Alignment parameter calibration for IMU using the Taguchi method for image deblurring [J]. Measurement, 2015, 65: 207-219.

[13] 曹淵，張士峰，楊華波，等．一種新的慣性平臺快速連續旋轉自對準方法[J]．兵工學報，2011，32(12)：1468-1473．[Cao Yuan, Zhang Shi-feng, Yang Hua-bo, et al. Fast self-alignment method for inertial platform through continuous rotation [J]. Acta Armamentarii, 2011, 32(12): 1468-1473.]

[14] Drora G M, Itzhack Y B I. Unified approach to inertial navigation system error modeling [J]. Journal of Guidance Control and Dynamics, 1992, 15(3): 648-653.

[15] 包為民，申功勛，李華濱．慣性平臺在系統中多位置翻滾自標定方法[J]．北京航空航天大學學報，2011，37(4)：462-465．[Bao Wei-min, Shen Gong-xun, Li Hua-bin. Investigation on inertial platform multi-position rolling self-calibration [J]. Journal of Beijing University of Aeronautics and Astronautics, 2011, 37(4): 462-465.]

[16] 王琪，汪立新，秦偉偉，等．基于D-最優的慣性平臺自標定方案設計[J]．中國慣性技術學報，2016，24(5)：583-588．[Wang Qi, Wang Li-xin, Qin Wei-wei, et al. Design of inertial platform self-calibration scheme based on D-optimal theory [J]. Journal of Chinese Inertial Technology, 2016, 24(5): 583-588.]

[17] 任磊，杜建邦，王美娥．旋轉慣導中加速度計尺寸效應誤差分析及補償[J]．航空學報，2013，34(6)：1424-1435．[Ren Lei, Du Jian-bang, Wang Mei-e. Error analysis and compensation of size effect in INS with IMU rotation [J]. Acta Aeronautica et Astronautica Sinica, 2013，34(6)：1424-1435.]

[18] 秦永元．慣性導航[M]．科學出版社，2006：249-253．

通信地址：陜西省西安市灞橋區同心路2號3304分隊(710025)

電話：15829247083

E-mail：wq050314@163.com

秦偉偉(1982-)，男，講師，主要從事高超聲速飛行器研究。本文通信作者。

通信地址：陜西省西安市灞橋區同心路2號3304分隊(710025)

電話：18502967140

E-mail：18502967140@163.com

Continuous Self-Calibration of Inertial Platform Based on Dual Extended Kalman Filter

WANG Qi, WANG Li-xin, QIN Wei-wei, SHEN Qiang

(Rocket Force University of Engineering, Xi’an 710025, China)

In view of the problems of slow convergence and sensitivity to initial conditions in the traditional continuous self-calibration of an inertial platform because of the nonlinearity of the system and the large number of dimensions of the state vector, the dual extended Kalman filter is researched in this paper. Firstly, the error models of the continuous self-calibration are given. Then the output error of the accelerometer is pre-filtered according to the relationship between the accelerometer error and the navigation velocity error. At the same time, the gyro error is decoupled from the output error of the accelerometer by analyzing its propagation characteristics in the continuous self-calibration. At last, the dual extended Kalman filter is carried out by using the gyro error and the accelerometer output as the observations respectively to establish the filter equations of the gyro and the accelerometer, which can reduce the nonlinearity of the system and the number of the dimensions of the state vector. Simulation results indicate that the continuous self-calibration method based on the dual extended Kalman filter can calibrate all the error coefficients of the inertial platform with relative error lower than 0.05% within 900s and is not sensitive to the initial conditions, which is beneficial to the application of the continuous self-calibration of the inertial platform.

Continuous self-calibration; Extended Kalman filter; Inertial platform; Error model; Error decoupling

2017-03-13;

2017-04-19

國家自然科學基金青年基金(61503390，61503392);陜西省自然科學基金(2015JQ6213);航空科學基金(201501U8001)

U666.12

A

1000-1328(2017)06-0621-09

10.3873/j.issn.1000-1328.2017.06.009

王 琪(1991-)，男，博士生，主要從事慣性系統及儀器研究。