基于邊緣采樣UKF濾波的捷聯慣導初始對準方法

李方能，許江寧，亓洪標

（海軍工程大學 電氣工程學院，武漢 430033）

基于邊緣采樣UKF濾波的捷聯慣導初始對準方法

李方能，許江寧，亓洪標

（海軍工程大學 電氣工程學院，武漢 430033）

設計了基于四元數的捷聯慣導非線性初始對準模型，同時指出該模型僅僅是姿態誤差四元數和速度誤差的非線性函數，而對于慣性器件誤差而言則是線性的。針對該模型的部分線性特性，設計了基于邊緣采樣的 UKF濾波算法，該算法僅對狀態量中的非線性子集進行采樣，因此對于部分線性模型而言，該算法在不損失濾波精度的前提下能夠有效降低算法計算量。仿真及車載實測數據實驗表明所研究的初始對準模型和相應的濾波算法是有效的，而且較傳統方法具有明顯的計算量方面的優勢；在達到相同對準精度的前提下，所設計算法的計算量較傳統算法降低了52%。

捷聯慣導；初始對準；UKF；邊緣采樣

作為一種航位推算系統(Dead-Reckoning System)，捷聯慣導系統初始信息直接關系到其接下來的導航定位精度[1]。因此必須為捷聯慣導系統提供初始姿態、速度和位置信息，而這正是由初始對準所完成。通常情況下，初始對準一般是先進行粗對準，然后進行精對準。粗對準一般在靜止條件下通過解析的方式進行，目的是獲取系統粗略初始姿態信息以滿足系統對姿態的小角度誤差要求。小角度的姿態誤差可以保證經典的慣導誤差方程成立，并以此誤差方程為初始對準模型進行精對準。精對準則在外部觀測量的輔助下，采用最優估計的方法進行[2]。然而，在很多情況下粗對準的精度很難保證基于線性慣導誤差方程精對準的小角度假設條件，這主要由于以下兩個方面引起：首先在初始對準的過程中，載體可能工作在具有各種噪聲的動態環境中，如系泊狀態下的艦船會受到各類風浪的干擾，而作為粗對準必要條件的地球自轉角速率可能被湮沒在這些高頻的外部干擾中，從而不能完成正常的粗對準。第二是由于捷聯慣導系統的傳感器性能太差。例如對于基于微慣性測量單元（MEMS）的捷聯慣導系統而言，其陀螺的常值漂移一般在1～10 (°)/h之間，甚至更高，而地球的自轉角速度為15 (°)/h，因此低精度陀螺儀不能有效地測得地球自轉角速度，從而不能完成正常的粗對準。因此，對于低于戰術級的慣導系統或是工作在惡劣動態環境下，無法實現解析式粗對準，從而不能完成傳統的二階段初始對準，必須進行非線性初始對準[3-6]。

進行非線性初始對準首先要建立非線性慣導誤差方程。由于在慣導系統中姿態有不同的表示方法，如歐拉角法、方向余弦矩陣法、四元數法，因此存在不同的姿態誤差方程，其中歐拉角法因其物理意義明確、誤差方程推導直接而得到廣泛的應用。建立基于歐拉角誤差方程首先需要確定導航坐標系和計算導航坐標系之間的方向余弦矩陣，此方向余弦矩陣的建立是通過三個軸向以失準角順序旋轉得到的。然而在大失準角條件下，不同的旋轉順序得到的方向余弦矩陣是不同的，而傳統方法都是采用某一特定旋轉次序，因此大失準角條件下的歐拉角誤差方程是不嚴格的。四元數相對于歐拉角具有以下突出優點：第一，不存在奇異性；第二，姿態矩陣可表示為四元數的代數形式；第三，在大角度條件下，基于四元數建立的姿態矩陣不涉及具體轉動次序[7-8]。因此，本文擬建立基于四元數來的大失準角姿態誤差方程用以非線性初始對準。

無味卡爾曼濾波（unscented Kalman filter, UKF）因其精度、計算量、適應性等方面的突出優點，在實際系統中得到了越來越廣泛的應用[9-10]。由于非線性初始對準模型是一個高維模型，將UKF應用到非線性初始對準中會產生較大的計算負擔，從而影響其實時應用[3-4]。對于UKF算法而言，影響其計算量的核心是sigma點采樣計算。為了減小UKF算法的計算量，Julier設計了不同的簡化UKF算法，但是他所設計的簡化UKF算法都是以犧牲濾波精度為前提的，而且是通用方法，并未考慮具體實際應用模型。在捷聯慣導初始對準問題中陀螺漂移一般都是假設為常值誤差來進行估計，因此其對應的狀態方程是線性的，從這一點來講我們所研究的姿態估計模型是部分線性的。因此能否考慮只對模型中的非線性狀態部分使用 UKF算法而對線性部分使用卡爾曼濾波算法，以期達到不損失濾波精度的前提下減小算法計算量的目的[6,11-14]。本文擬從邊緣概率密度的角度對該問題展開研究，分析指出基于四元數的非線性初始對準模型本質上是部分線性模型，其中的姿態誤差四元數和速度誤差是非線性函數。本文將基于邊緣采樣的無味變換（unscented transformation, UT）應用到該問題中。仿真及實測數據實驗表明，所研究算法在不損失濾波精度的前提下，能夠有效降低算法計算量，因此特別適合于實時應用。

1 基于四元數的非線性對準模型

記慣性坐標系為i系，地理坐標系（或導航坐標系）為n系，地球坐標系為e系，載體坐標系為b系；同時為了推導對準誤差模型，引入計算導航坐標系n′系。

1.1 捷聯慣導誤差模型

當不考慮任何誤差時，速度的理想值由下式確定：

在實際系統中由于存在各種外界干擾和誤差，真實的速度方程為：

式中，I3×3表示3維單位矩陣，δL和δh分別為緯度和高度誤差，具體表達式將在后文給出。在計算式(3)時忽略了重力矢量誤差。在初始對準過程中加速度計的誤差一般只考慮常值零偏▽b以及隨機噪聲，因此公式(4)中的 δfb可以用▽b替代。式(1)～(4)中的可以寫成式(1)中的四元數形式，因此式(4)可進一步表示為：

理想情況下，基于四元數的載體姿態運動學方程為：

由于外界的各種干擾及器件誤差，真實的姿態四元數運動學方程為：

式中，

式(11)中第二行到第三行的計算過程考慮了如下關系式：

在初始對準過程中陀螺儀的器件誤差一般只考慮陀螺漂移 εb和隨機游走，因此式(11)中的可以用代替：

1.2 非線性初始對準濾波模型

在初始對準過程中一般需要對陀螺儀漂移和加速度計零偏進行估計補償，因此需要建立相應的狀態方程，具體形式如下：

選取系統狀態為：

結合捷聯慣導的非線性誤差方程，可得到任意失準角條件下初始對準的系統方程為：

f（x）由式(5)(15)～(17)所確定，其緊湊形式為：

式(18)中所選的狀態是一種一般形式的量，具體使用時可根據應用場合進行相應簡化，如對于路用及海用捷聯慣導，天向速度分量及對應的加速度計常值零偏分量可以忽略。

在實際應用中一般選取速度誤差作為系統觀測量，即

2 非線性初始對準模型的部分線性框架

式(21)中的模型是非線性的，因此需要采用非線性濾波算法進行狀態估計。一般而言，非線性濾波算法都是將狀態看成一個整體，只要狀態中任意一子集是非線性的，則將整個狀態模型整體看成是非線性的。但是通過仔細分析可以發現，很多實際應用的非線性模型僅僅是狀態中某一子集的非線性函數，而對于其它子集而言則是線性的，對于這類模型我們一般稱之為部分線性模型。下面我們將分析指出式(21)中所存在的部分線性框架。

假設慣導更新的時間間隔為dt，則式(21)對應的離散形式為：

從式(25)可以看出，基于四元數的非線性初始對準模型僅僅是姿態誤差四元數和速度誤差的非線性函數，對于慣性器件誤差而言則是線性的。對于這種部分線性模型，可以通過邊緣概率密度的概念設計相應的簡化濾波算法，下節將給出基于UKF的簡化算法。

3 基于邊緣采樣的簡化UKF算法設計

對于部分線性系統，我們可以考慮只對狀態中非線性部分的狀態采樣UKF濾波，而對線性部分則直接應用卡爾曼濾波。此處我們引入邊緣無味變換的概念（Marginalized unscented transform, MUT）用以解決該類問題。

對于一狀態量x，其可分解為非線性和線性兩部分，即 x =[aT,bT]T，那么一部分線性函數g是指它可寫為如下形式：

式中， x ∈Rn， g(·)是任意一非線性函數，它可以是狀態方程，也可以是量測方程； a ∈Rna， b∈Rnb并且na+ nb= n；ψ是a的非線性函數；γ是a的函數，可以是線性，也可以是非線性。在常規的UKF算法中，該類問題需要對全狀態進行sigma點采樣，即在每一步濾波中計算2 n+1個sigma點。考慮到該類問題在模型上的特殊性，我們可以引入邊緣概率密度的思想，只對其中非線性部分進行采樣，即每一步濾波中只需要計算2 na+1個sigma點，而且這種方法不損失濾波算法的精度。下面我們簡要給出基于邊緣概率密度思想的MUT算法流程。

假設狀態服從高斯分布如下：

根據高斯分布的特性及矩陣運算法則可得：

那么傳遞后的均值可通過下式計算：

式中，

對于式(32)中的問題我們可以采用常規 UT算法計算其均值。由式(32)我們可以計算狀態非線性部分a對應的sigma點極其均值：

具體采樣方法可參考Julier相關文獻。那么經式(32)傳遞后的sigma點為：

由UT算法可得傳遞后的均值和方差分別為：

從上面的算法流程可以看出，在MUT算法中只需要對狀態的非線性部分進行采樣，因此可以有效降低算法計算量。具體到慣性系初始對準問題中，只需要對姿態和速度部分進行采樣，則相應的計算量可降低50%。將MUT替換傳統UKF中的UT即可得到基于邊緣采樣的簡化UKF算法（MUKF），該過程比較直接，具體過程此處不再給出。

4 實驗驗證

4.1 仿真實驗

為驗證算法的有效性，設計了靜基座條件下的仿真實驗。仿真實驗中使用的慣性器件精度分別如下：陀螺隨機常值漂移為 0.01 (°)/h，陀螺白噪聲為 0.03 (°)/h，加速度計隨機常值零偏為100 μg，加速度計量測白噪聲為500 μg。速度量測噪聲為0.01 m/s。濾波時間步長取為0.1 s，仿真總時間長度為600 s，并對慣導的速度、姿態進行反饋校正。對準精度用慣導反饋校正后的姿態與真實姿態的誤差進行評估。實驗中方位失準角服從[0°60°]上的均勻分布，兩個水平失準角服從 [- 15°15°]上的均勻分布。進行100次蒙特-卡洛仿真實驗并計算三個失準角的平均估計誤差和對應的3σ，結果分別如圖1～圖3所示。圖中紅線表示UKF，藍線表示MUKF；實線表示均值，虛線表示均方差。從圖中可以看出，兩種方法的精度基本相當，而且都能實現對較大初始失準角的有效估計，估計誤差均值在對應的3σ內，從而說明相應的算法是有效的。同時本文針對非線性初始對準模型設計的 MUKF算法由于在每次濾波中只需要傳統 UKF算法的接近一半數量的sigma點，因此計算量僅僅為傳統算法的50%。

圖1 俯仰角估計誤差Fig.1 Estimation error for the pith angle

圖2 橫滾角估計誤差Fig.2 Estimation error for the roll angle

圖3 航向角估計誤差Fig.3 Estimation error for the yaw angle

4.2 車載實驗

為了進一步驗證所研究算法的有效性，設計了相應的車載實驗。實驗設備主要包括激光捷聯慣導、里程計、GPS接收機等，其中激光陀螺漂移約為 0.007 (°)/h，加速度計零偏約為 5×10-5g，捷聯慣導的更新率為125 Hz；GPS更新率為1 Hz。實驗中選取300 s的車載數據進行初始對準算法驗證。300 s實驗數據對應的姿態基準如圖4所示。

圖4 車載對準實驗參考姿態Fig.4 Reference attitude in the experiment

圖6 橫滾角估計誤差Fig.6 Estimation error for the roll angle

圖7 航向角估計誤差Fig.7 Estimation error for the yaw angle

5 結 論

研究了基于四元數的非線性初始對準問題，分析指出了基于四元數的非線性初始對準模型本質上是一部分線性模型，即該模型僅僅是姿態誤差四元數和速度誤差的非線性函數，而對于器件誤差而言則是線性的。針對該模型的這一特性，設計了一種基于邊緣采樣的UKF濾波算法，該算法只對部分線性模型中狀態的非線性子集進行采樣，在不損失濾波精度的前提下能夠有效降低算法計算量。仿真實驗及車載實測數據實驗表明該算法能夠達到傳統 UKF濾波算法相同的濾波精度，但是計算量幾乎降低52%。

（

）：

[1] Titterton D H, Weston J L. Strapdown inertial navigation technology[M]. 2nd Edition. London: the Institute of Electrical Engineers, 2004.

[2] 劉錫祥，徐曉蘇，李天旦，劉義亭，王立輝. 基于數據存儲與循環解算的 SINS 快速對準方法[J]. 中國慣性技術學報，2013，21(6)：715-720.

LIU Xi-xiang, XU Xiao-su, LI Tian-dan, LIU Yi-ting, WANG Li-hui. Fast alignment method for SINS based on stored data and loop calculation[J]. Journal of Chinese Inertial Technology, 2013, 21(6): 715-720.

[3] 嚴恭敏，嚴衛生，徐德民. 簡化 UKF 濾波在 SINS 大失準角初始對準中的應用[J]. 中國慣性技術學報，2008，16(3)：253-264.

YAN Gong-min, YAN Wei-shen, XU De-min. Application of simplified UKF in SINS initial alignment for large misalignment angles[J]. Journal of Chinese Inertial Technology, 2008, 16(3): 253-264.

[4] Zhou Ben-chun, Cheng Xiang-hong. Robust UKF algorithm in SINS initial alignment[J]. Journal of Southeast University (English Edition), 2011, 27(1): 56-60.

[5] Ali J. Initial orientation of inertial navigation system realized through nonlinear modeling and filtering[J]. Measurement, 2011, 44: 793-801.

[6] Chang Lu-bin, Hu Bai-qing, Li An, Qin Fang-jun. Strapdown inertial navigation system alignment based on marginalized unscented Kalman filter[J]. IET Science, Measurement and Technology, 2013, 7(2): 128-138.

[7] 房建成，韓曉英，楊鳳英.一種高精度機載POS雙位置對準方法[J]. 中國慣性技術學報，2013，21(3)：318-323.

FANG Jian-cheng, HAN Xiao-ying, YANG Feng-ying. Two-position alignment method of airborne position and orientation system[J]. Journal of Chinese Inertial Technology, 2013, 21(3): 318-323.

[8] 夏家和，秦永元，趙長山. 適用于低精度慣導的非線性對準方法研究[J]. 儀器儀表學報，2009，30(8)：1618-1622.

XIA Jia-he, QIN Yong-yuan, ZHAO Chang-shan. Study on nonlinear alignment method for low precision INS [J]. Chinese Journal of Scientific Instrument, 2009, 30(8): 1618-1622.

[9] Julier S J, Uhlmann J K. Unscented filtering and nonlinear estimation[J]. Proc. IEEE, 2004, 92(3): 401-422.

[10] Chang Lu-bin, Hu Bai-qing, Li An, Qin Fang-jun. Transformed unscented Kalman filter[J]. IEEE Transactions on Automatic Control, 2013, 58(1): 252-257.

[11] Chang Lu-bin, Hu Bai-qing, Chang Guo-bin, Li An. Marginalized iterated unscented Kalman filter[J]. IET Control Theory & Applications, 2012, 6(6): 847-854.

[12] Morelande M R, Ristic B. Smoothed state estimation for nonlinear Markovian switching systems[J]. IEEE Transactions on Aerospace and Electronic Systems, 2008, 44(4): 1309-1324.

[13] Chang Guo-bin. Marginal unscented Kalman filter for crosscorrelated process and observation noise at the same epoch [J]. IET Radar, Sonar and Navigation, 2013, 8(1): 54-64.

[14] Chang Guo-bin. Loosely coupled INS/GPS integration with constant lever arm using marginal unscented Kalman filter[J]. The Journal of Navigation(in press). doi: 10.1017/S0373463313000775.

Initial alignment of strapdown inertial navigation system based on marginalized unscented Kalman filter

LI Fang-neng, XU Jiang-ning, QI Hong-biao
(Department of Electrical Engineering, Naval University of Engineering, Wuhan 430033, China)

A quaternion-based nonlinear model is designed for the initial alignment of a strapdown inertial navigation system(SINS) with large initial errors. It is pointed out that the model has only the nonlinear function relation with quaternion-based misalignment error and the velocities error, whereas it has linear relation with the errors of inertial sensors. In view of this partially linear characteristic, a unscented Kalman filter(UKF) algorithm with marginalized-sampling-based unscented transformation(UT) is designed, which only samples the nonlinear subset of state variables, and can effectively reduce the computation burden without losing filtering accuracy. Simulations and car-mounted experiments demonstrate that the marginalized UT-based Kalman filter can achieve at least a comparable performance to the traditional UT-based Kalman filter with a significantly lower expense. Compared with the traditional algorithm, the investigated method can reach the same alignment precision but with 52% reduced computational burden.

SINS; initial alignment; unscented Kalman filter; marginalized sampling

聯 系 人：許江寧（1964—），男，教授，博士生導師。E-mail：xujiangning@hotmail.com

1005-6734(2014)05-0612-07

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

U666.1

A

2014-05-14；

2014-09-17

國家自然科學基金（61304241，61374206）

李方能（1978—），男，講師，從事慣性技術及應用研究。E-mail：leefangneng@163.com