何虔恩，高鐘毓，吳秋平

（清華大學 精密儀器系，北京 100084）

基于未補償偏置濾波器的慣導動態初始對準

何虔恩，高鐘毓，吳秋平

（清華大學 精密儀器系，北京 100084）

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

慣性導航系統；初始對準；動態條件下；Kalman濾波；未補償偏置濾波

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

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

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

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

1 系統誤差模型及對準誤差分析

1.1 系統誤差模型

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

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

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

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

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

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

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

ψN, ψE——平臺失準角的北向和東向分量，滿足如下關系式：

式中：λ——相對地理經度，初始時刻為0；

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

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

式中：S10、Δσ?、Δγ——平臺失準角初值；

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

Δd22——陀螺g2項漂移系數誤差。

1.2 對準誤差分析

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

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

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

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

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

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

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

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

表 1 陀螺漂移誤差在對準誤差中的貢獻Tab.1 Alignment errors caused by gyroscope drift errors

2 未補償偏置濾波器設計

2.1 帶偏置結構的誤差模型

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

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

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

其余元素為零；

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

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

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

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

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

2.2 對準濾波器設計

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

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

時間傳播：

濾波增益：

測量修正：

Lk遞推計算：

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

3 試驗結果

3.1 對準結果

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

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

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

圖 1 動態初始對準初值誤差收斂曲線Fig.1 Convergence curves of initial errors for initial alignment under dynamic conditions

3.2 導航結果

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

圖2 歸一化經緯度誤差Fig.2 Normalized errors of longitude and latitude

以上試驗結果與前述理論分析相吻合，在一定程度上驗證了未補償偏置濾波法應用在慣導動態初始對準中的有效性。

4 結 論

針對動態環境下隨機干擾大、慣性儀表誤差可觀測性弱問題，采用未補償偏置濾波器實現慣性導航系統的初始對準。定量分析了陀螺漂移誤差對降維濾波器精度的影響，在此基礎上給出帶偏置結構的對準誤差模型，并設計了7維未補償偏置濾波器。動態試驗結果表明，未補償偏置濾波器能有效對付動態隨機干擾和慣性儀表誤差的影響，其對準速度比直接降維濾波器快一倍以上，僅需40 min使對準精度達標，具有較強的工程應用價值。

值得注意的是，限于快速性要求，動態初始對準無法解決慣性儀表逐次啟動重復性誤差導致系統長時間導航精度下降的問題。因此，在導航階段必須采取有關技術措施，如重調，對慣性儀表誤差進行標校，以保證系統長時間導航精度達標。

（References）：

[1] 高鐘毓. 慣性導航系統技術[M]. 北京: 清華大學出版社, 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

總裝“十二五”預研項目（51309030401）

何虔恩（1985—），男，博士后，研究方向為導航系統與控制。E-mail：heqianen2005@126.com

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