基于未補(bǔ)償偏置濾波器的慣導(dǎo)動態(tài)初始對準(zhǔn)

何虔恩，高鐘毓，吳秋平

（清華大學(xué) 精密儀器系，北京 100084）

基于未補(bǔ)償偏置濾波器的慣導(dǎo)動態(tài)初始對準(zhǔn)

何虔恩，高鐘毓，吳秋平

（清華大學(xué) 精密儀器系，北京 100084）

動態(tài)初始對準(zhǔn)是慣性導(dǎo)航系統(tǒng)（慣導(dǎo)）工程應(yīng)用的重要功能之一。針對動態(tài)環(huán)境下隨機(jī)干擾和弱可觀慣性儀表誤差導(dǎo)致對準(zhǔn)濾波器性能下降的問題，采用未補(bǔ)償偏置濾波器實(shí)現(xiàn)慣導(dǎo)系統(tǒng)的初始對準(zhǔn)。給出了帶高度阻尼的慣導(dǎo)水平通道誤差模型，根據(jù)最小二乘估計(jì)原理定量分析了陀螺漂移誤差對降維濾波器精度的影響，進(jìn)而推導(dǎo)出帶偏置結(jié)構(gòu)的對準(zhǔn)誤差模型，設(shè)計(jì)出基于水平位置誤差觀測的7維未補(bǔ)償偏置Kalman濾波器。動態(tài)試驗(yàn)結(jié)果表明，未補(bǔ)償偏置濾波器能有效提高慣導(dǎo)動態(tài)初始對準(zhǔn)性能，僅需40 min對準(zhǔn)精度即達(dá)標(biāo)，比直接降維濾波器快一倍以上，具有較強(qiáng)的工程應(yīng)用價(jià)值。

慣性導(dǎo)航系統(tǒng)；初始對準(zhǔn)；動態(tài)條件下；Kalman濾波；未補(bǔ)償偏置濾波

慣導(dǎo)是一種航位推算系統(tǒng)，其工作精度依賴于初始條件的設(shè)定精度。例如，為達(dá)到0.1 n mile/h的導(dǎo)航精度，初始水平和方位失準(zhǔn)角應(yīng)分別小于6"和30"，且初始速度誤差應(yīng)小于0.1 n mile/h。因此，為獲得高精度的性能，系統(tǒng)必須具有高精度的初始化[1]。其中，初始位置和速度通過全球衛(wèi)星定位系統(tǒng)等外部設(shè)備容易直接獲得，并且在水平速度阻尼條件下，初始位置和速度誤差的影響將逐漸衰減為零（經(jīng)過1～2個Schuler振蕩周期）；而初始水平角和方位角通常需要借助特定的算法間接計(jì)算出來，并且其誤差對導(dǎo)航精度的影響是長期的（高精度慣導(dǎo)通常是無方位阻尼的）。因此，系統(tǒng)初始對準(zhǔn)的主要任務(wù)在于獲得精確的初始水平角和方位角。

長航時高精度慣導(dǎo)是復(fù)雜的光機(jī)電系統(tǒng)，在長期工作過程以及動態(tài)環(huán)境中各種沖擊、振動的作用下，其零部件難免會出現(xiàn)故障，在故障修復(fù)后，要求系統(tǒng)能夠即刻恢復(fù)正常工作狀態(tài)。此時，動態(tài)初始對準(zhǔn)是系統(tǒng)不可或缺的功能之一，其基本要求是保精度、速度快。

慣導(dǎo)有光學(xué)對準(zhǔn)法、傳遞對準(zhǔn)法和自對準(zhǔn)法等初始對準(zhǔn)方法[1]。其中，自對準(zhǔn)法簡便、可操作性強(qiáng)，且適用于動基座，因而取得廣泛應(yīng)用。目前，自對準(zhǔn)主要采用基于現(xiàn)代控制理論的濾波法。現(xiàn)代濾波法采用狀態(tài)空間法描述系統(tǒng)誤差的動態(tài)特性，可同時估計(jì)初始失準(zhǔn)角和可觀測的慣性儀表誤差參數(shù)，并能最優(yōu)（在某種意義下，如方差最小）地過濾各種動態(tài)干擾。國內(nèi)外學(xué)者結(jié)合不同的應(yīng)用背景，有針對性地開發(fā)了各種濾波算法。例如，針對非線性問題，有擴(kuò)展Kalman濾波[2]、無跡Kalman濾波[3]和粒子濾波[4]等算法；針對隨機(jī)干擾，有魯棒濾波[5]、H∞濾波[6]和自適應(yīng)濾波[7]等算法。這些算法通常對慣性儀表誤差和測量偏置進(jìn)行精確建模，并將模型參數(shù)作為濾波器狀態(tài)給予估計(jì)（增廣狀態(tài)法）。然而，在動態(tài)條件下，由于風(fēng)浪、發(fā)動機(jī)振動和運(yùn)載體機(jī)動等多種因素的綜合作用，慣導(dǎo)所遭受的隨機(jī)干擾可能比靜態(tài)條件下大2倍以上。此時，慣性儀表誤差和測量偏置相關(guān)狀態(tài)的可觀測性弱，增廣狀態(tài)法無法在短時間內(nèi)獲得其精確估計(jì)（根據(jù)慣導(dǎo)誤差傳播特性[1]和實(shí)踐經(jīng)驗(yàn)知，欲精確分離高精度慣性儀表誤差參數(shù)，濾波時間通常要超過12 h，這無法滿足動態(tài)初始對準(zhǔn)的快速性要求）；同時，將降低濾波收斂速度，并可能導(dǎo)致濾波器發(fā)散。若直接忽略慣性儀表誤差和測量偏置的影響（直接降維法），則可能引入較大的模型誤差，降低濾波器性能。針對這類矛盾，Zanetti R等[8-9]提出未補(bǔ)償偏置濾波器的思想：將偏置和噪聲的影響統(tǒng)一視為不確定源，并利用偏置結(jié)構(gòu)的特點(diǎn)有針對性地、實(shí)時地調(diào)整濾波器的過程和測量噪聲陣，使濾波器性能接近最優(yōu)（濾波計(jì)算得到的樣本協(xié)方差與狀態(tài)估計(jì)誤差協(xié)方差匹配）。

將未補(bǔ)償偏置濾波器思想應(yīng)用在慣導(dǎo)動態(tài)初始對準(zhǔn)中，旨在解決慣性儀表誤差（陀螺漂移誤差達(dá)到1×10-3(°)/h量級）和動態(tài)隨機(jī)干擾導(dǎo)致降維濾波器性能下降的問題。本文研究了慣性儀表誤差對失準(zhǔn)角初值估計(jì)精度的影響；在此基礎(chǔ)上，設(shè)計(jì)相應(yīng)的未補(bǔ)償偏置濾波器；最后，給出基于未補(bǔ)償偏置濾波器的動態(tài)初始對準(zhǔn)試驗(yàn)結(jié)果，并與增廣狀態(tài)法和直接降維法的結(jié)果進(jìn)行了比較，得到有關(guān)結(jié)論。

1 系統(tǒng)誤差模型及對準(zhǔn)誤差分析

1.1 系統(tǒng)誤差模型

引入高度阻尼后，慣導(dǎo)垂直通道的誤差可忽略不計(jì)，此時，對于低速巡航載體，水平通道的誤差方程可寫為[1]：

式中：δrN, δrE——位置誤差的北向和東向分量；

δvN, δvE——速度誤差的北向和東向分量；

ωD=-ωiesinL ，ωie和L分別為地球自轉(zhuǎn)角速率和本地地理緯度；

ω——Schuler振蕩角頻率，1.24×10-3rad/s；

sg——本地重力加速度值；

▽N, ▽E——比力誤差的北向和東向分量；

ψN, ψE——平臺失準(zhǔn)角的北向和東向分量，滿足如下關(guān)系式：

式中：λ——相對地理經(jīng)度，初始時刻為0；

S1, σ1, γ2——由地球坐標(biāo)系（e系）依次繞x、y和z軸旋轉(zhuǎn)到空間穩(wěn)定平臺坐標(biāo)系（P系）的三個Euler角；

ΔS1, Δσ1, Δγ2——平臺分別沿x、y和z方向的失準(zhǔn)角，考慮陀螺漂移誤差，經(jīng)推導(dǎo)可得其近似的解析表達(dá)式為：

式中：S10、Δσ?、Δγ——平臺失準(zhǔn)角初值；

Δε1x,Δε1y,Δ2z——陀螺常值漂移誤差；

Δd22——陀螺g2項(xiàng)漂移系數(shù)誤差。

1.2 對準(zhǔn)誤差分析

選擇水平位置誤差為觀測變量，并記觀測向量為

式中：ra——地球平均半徑。

對于低速運(yùn)載體，在對準(zhǔn)期間經(jīng)緯度變化為小量。簡單起見，考慮S1和σ1為小量的情況，此時，式(2)可簡化為：

另外，為滿足快速性要求，時間t通常較小。此時，式(3)可簡化為：

這里，已利用γ2≈ωiet+γ20和

將式(6)代入式(5)，再與式(4)一同代入方程(1)，整理后可得：

易見，式(8)等號右邊是關(guān)于時間t的二次多項(xiàng)式。在提供一定長度觀測數(shù)據(jù)的條件下，可分離出各項(xiàng)系數(shù)。若不考慮陀螺漂移誤差（采用直接降維法），則初始對準(zhǔn)將只包含式(8)第一等式的常值項(xiàng)、t1項(xiàng)以及第二等式的常值項(xiàng)和部分t1項(xiàng)（與Δσ?相關(guān)的部分），按照最小二乘估計(jì)原理，可推導(dǎo)出如下對準(zhǔn)結(jié)果（中低緯度地區(qū)）：

根據(jù)式(7)(9)可得陀螺漂移誤差在對準(zhǔn)誤差中的貢獻(xiàn)如表 1所示。可見，陀螺漂移誤差主要造成方位對準(zhǔn)誤差。由表 1知，Δδ10和Δε1ycosγ20-Δε1xsin γ20主要引起常值型方位對準(zhǔn)誤差，其它漂移系數(shù)誤差主要引起與對準(zhǔn)時間成正比的方位對準(zhǔn)誤差（t1型誤差）。由于失準(zhǔn)角初值本身是定常的，常值型的對準(zhǔn)誤差是無法消除的；而t1型的對準(zhǔn)誤差，通過采取一定的技術(shù)措施（如增廣狀態(tài)法），有可能減小甚至被消除。

表 1 陀螺漂移誤差在對準(zhǔn)誤差中的貢獻(xiàn)Tab.1 Alignment errors caused by gyroscope drift errors

2 未補(bǔ)償偏置濾波器設(shè)計(jì)

2.1 帶偏置結(jié)構(gòu)的誤差模型

根據(jù)第1節(jié)的討論（詳見式(9)和表1）知，為了提高對準(zhǔn)精度，陀螺漂移誤差組合參數(shù)δSt、δσ0和tγδ是值得考慮的。此時，根據(jù)式(3)和式(6)有：

式中：F(t)——7×7維系統(tǒng)矩陣，各個元素根據(jù)式(1)、式(2)和式(10)容易得到（限于篇幅，不一一列出）；

B(t)——7×3維偏置矩陣，第i行第j列元素Bij為（i=1,2,…,7，j=1,2,3）：

其余元素為零；

w(t)——7×1維狀態(tài)噪聲向量。

以水平位置誤差為觀測量，觀測方程可寫為

式中：H——2×7維觀測矩陣：

n(t)——2×1維觀測噪聲向量。

由第1.2節(jié)的討論知，基于水平位置誤差觀測，狀態(tài)x(t)是可觀的，因此定能設(shè)計(jì)出有效的濾波算法對其進(jìn)行估計(jì)。

2.2 對準(zhǔn)濾波器設(shè)計(jì)

令狀態(tài)噪聲w(t)和觀測噪聲n(t)為相互獨(dú)立的零均值Gauss白噪聲，且協(xié)方差陣滿足ε

δ為與w(t)和n(t)不相關(guān)的隨機(jī)常向量，且滿足

時間傳播：

濾波增益：

測量修正：

Lk遞推計(jì)算：

這里：Φk,k-1≈I+F（tk-1）·Δt ，I為7階單位陣，Δt為采樣周期；Bk≈B（tk）·Δt ；Qk≈Q（tk）·Δt ；Rk=R（tk）。

3 試驗(yàn)結(jié)果

3.1 對準(zhǔn)結(jié)果

某慣導(dǎo)系統(tǒng)在緯度為9o～23o的區(qū)域進(jìn)行了海上動態(tài)試驗(yàn)。利用在線記錄的系統(tǒng)試驗(yàn)數(shù)據(jù)和外參考數(shù)據(jù)（如：全球定位系統(tǒng)提供的經(jīng)緯度信息），對上述濾波算法進(jìn)行驗(yàn)證。這里，人為引入1×10-3(o)/h量級的陀螺漂移誤差。

圖1摘錄了某段試驗(yàn)數(shù)據(jù)（載體速度和加速度計(jì)隨機(jī)噪聲的幅值分別達(dá)0.2～0.6 m/s和0.4～1 m/s2）分別采用未補(bǔ)償偏置法（UBM—Uncompensated Bias Method）、直接降維法（DRM—Directly Reduced Method）和增廣狀態(tài)法（ESM—Extended States Method）的對準(zhǔn)結(jié)果。圖1中，ρ 表示允許的對準(zhǔn)誤差。

由圖1易見，未補(bǔ)償偏置濾波法的對準(zhǔn)速度和精度優(yōu)于直接降維法和增廣狀態(tài)法——水平和方位對準(zhǔn)僅分別進(jìn)行10 min和40 min即達(dá)到指標(biāo)要求。在陀螺漂移誤差和隨機(jī)干擾的影響下，直接降維法需要多一倍以上的時間才能使方位精度滿足要求（如δS10所示）；而增廣狀態(tài)法則由于陀螺漂移誤差組合參數(shù)的可觀測性差，對準(zhǔn)速度在三種方法中最慢。

圖 1 動態(tài)初始對準(zhǔn)初值誤差收斂曲線Fig.1 Convergence curves of initial errors for initial alignment under dynamic conditions

3.2 導(dǎo)航結(jié)果

初始對準(zhǔn)進(jìn)行1 h后轉(zhuǎn)入導(dǎo)航，三種對準(zhǔn)方法相應(yīng)的歸一化導(dǎo)航經(jīng)緯度誤差如圖2所示。易見，未補(bǔ)償偏置濾波法對應(yīng)導(dǎo)航誤差的24 h周期分量依次比直接降維法和增廣狀態(tài)法小1倍和3倍以上，這與前面的對準(zhǔn)結(jié)果是一致的。另外，經(jīng)度誤差的趨勢項(xiàng)和緯度誤差的24 h周期發(fā)散振蕩分量分別由赤道和極軸陀螺的常值漂移誤差（取決于陀螺逐次啟動的重復(fù)性精度）引起，這對系統(tǒng)長時間的工作精度具有致命的影響，必須采取一定的技術(shù)手段（如重調(diào)）來消除。

圖2 歸一化經(jīng)緯度誤差Fig.2 Normalized errors of longitude and latitude

以上試驗(yàn)結(jié)果與前述理論分析相吻合，在一定程度上驗(yàn)證了未補(bǔ)償偏置濾波法應(yīng)用在慣導(dǎo)動態(tài)初始對準(zhǔn)中的有效性。

4 結(jié) 論

針對動態(tài)環(huán)境下隨機(jī)干擾大、慣性儀表誤差可觀測性弱問題，采用未補(bǔ)償偏置濾波器實(shí)現(xiàn)慣性導(dǎo)航系統(tǒng)的初始對準(zhǔn)。定量分析了陀螺漂移誤差對降維濾波器精度的影響，在此基礎(chǔ)上給出帶偏置結(jié)構(gòu)的對準(zhǔn)誤差模型，并設(shè)計(jì)了7維未補(bǔ)償偏置濾波器。動態(tài)試驗(yàn)結(jié)果表明，未補(bǔ)償偏置濾波器能有效對付動態(tài)隨機(jī)干擾和慣性儀表誤差的影響，其對準(zhǔn)速度比直接降維濾波器快一倍以上，僅需40 min使對準(zhǔn)精度達(dá)標(biāo)，具有較強(qiáng)的工程應(yīng)用價(jià)值。

值得注意的是，限于快速性要求，動態(tài)初始對準(zhǔn)無法解決慣性儀表逐次啟動重復(fù)性誤差導(dǎo)致系統(tǒng)長時間導(dǎo)航精度下降的問題。因此，在導(dǎo)航階段必須采取有關(guān)技術(shù)措施，如重調(diào)，對慣性儀表誤差進(jìn)行標(biāo)校，以保證系統(tǒng)長時間導(dǎo)航精度達(dá)標(biāo)。

（References）：

[1] 高鐘毓. 慣性導(dǎo)航系統(tǒng)技術(shù)[M]. 北京: 清華大學(xué)出版社, 2012: 184-389. Gao Zhong-yu. Inertial navigation systems technology [M]. Beijing: Tsinghua University Press, 2012: 184-389.

[2] Fang J, Yang S. Study on innovation adaptive EKF for in-flight alignment of airborne POS[J]. IEEE Transactions on Instrumentation and Measurement, 2011, 60(4): 1378-1388.

[3] Lubin C, Baiqing H, AN L, et al. Strapdown inertial navigation system alignment based on marginalised unscented Kalman filter[J]. IET Science, Measurement & Technology, 2013, 7(2): 128-138.

[4] Mao B, Wu J. An adaptive particle filter for MEMS based SINS nonlinear initial alignment[C]//Proceedings of the 2010 International Conference on Information and Automation. 2010: 1504-1509.

[5] Sun F, Lan H, Yu C, et al. A robust self-alignment method for ship's strapdown INS under mooring conditions [J]. Sensors, 2013, 13(7): 8103-8139.

[6] Guo Lei, Cao Song-yin, Qi Chun-tang, et al. Initial alignment for nonlinear inertial navigation systems with multiple disturbances based on enhanced anti-disturbance filtering[J]. International Journal of Control, 2012, 85(5): 491-501.

[7] Fang J, Han X. In-flight alignment algorithm based on ADD2 for airborne POS[J]. Journal of Navigation, 2013, 66(2): 209-225.

[8] Zanetti R, Bishop R. Kalman filters with uncompensated biases[J]. Journal of Guidance Control and Dynamics, 2012, 35(1): 327-330.

[9] Hough M. Orbit Determination with improved covariance fidelity, including sensor measurement biases[J]. Journal of Guidance Control and Dynamics, 2011, 34(3): 903-911.

Initial alignment based on uncompensated bias filter for inertial navigation systems under dynamic conditions

HE Qian-en, GAO Zhong-yu, WU Qiu-ping
(Department of Precision Instruments, Tsinghua University, Beijing 100084, China)

The initial alignment of inertial navigation system(INS) under dynamic conditions plays a key role in the engineering applications. In this paper, an uncompensated bias filter is adopted in INS initial alignment to overcom the problem of alignment filter’s performance degradation due to random disturbances and inertial instrument errors of weak observability under dynamic conditions. An INS horizontal error model with altitude damped is presented, and the effect of gyro drift errors on accuracy of reduced alignment filter is analyzed quantitatively according to the principle of least squares estimation. Consequently, an alignment error model with bias structure is derived and a seven-dimension Kalman filter with uncompensated bias based on observations of horizontal position errors is designed. Results of dynamic experiments show that performance of INS initial alignment under dynamic conditions can be improved effectively by using the Kalman filter with uncompensated bias, in which the expected alignment accuracy is achived within 40 min, and the speed is at least one fold faster than that of direct dimension-reduction filters, showing great value in engineering applications.

inertial navigation system; initial alignment; dynamic condition; Kalman filter; uncompensated bias filtering

U666.1

A

1005-6734(2015)02-0184-05

10.13695/j.cnki.12-1222/o3.2015.02.009

2014-11-23；

2015-03-02

總裝“十二五”預(yù)研項(xiàng)目（51309030401）

何虔恩（1985—），男，博士后，研究方向?yàn)閷?dǎo)航系統(tǒng)與控制。E-mail：heqianen2005@126.com

聯(lián) 系 人：吳秋平（1972—），男，副研究員，博士生導(dǎo)師。E-mail：wuqiuping@mail.tsinghua.edu.cn