馬宏陽，程鵬飛，王潛心

(1.中國測繪科學(xué)研究院，北京 100830； 2.國家測繪產(chǎn)品質(zhì)量檢驗測試中心，北京 100830)

?

一種改進(jìn)的UKF算法在捷聯(lián)慣導(dǎo)初始對準(zhǔn)中的應(yīng)用

馬宏陽1，程鵬飛2，王潛心1

(1.中國測繪科學(xué)研究院，北京 100830； 2.國家測繪產(chǎn)品質(zhì)量檢驗測試中心，北京 100830)

Unscented卡爾曼濾波具有精度高、穩(wěn)定性好、實用性強(qiáng)等特點，因此UKF算法逐漸成為處理非線性濾波問題的有效方法和導(dǎo)航系統(tǒng)中數(shù)據(jù)處理與信息融合技術(shù)的研究熱點。但是UKF具有計算量大、效率低等缺點，因此限制了UKF在實時導(dǎo)航中的應(yīng)用。針對這一缺點，本文提出了一種改進(jìn)的UKF算法，該算法可以減少UT變換中Sigma點的計算數(shù)量，從而提高運(yùn)算效率；推導(dǎo)了改進(jìn)的算法公式，給出了適合該算法的初始對準(zhǔn)非線性模型，并分析了其精度，用實測數(shù)據(jù)進(jìn)行了驗證。結(jié)果顯示，改進(jìn)的UKF算法性能與傳統(tǒng)UKF相當(dāng)，但效率提升了40%左右。

UT變換；UKF；初始對準(zhǔn)；捷聯(lián)慣導(dǎo)；Sigma點

一、引 言

初始對準(zhǔn)是慣性導(dǎo)航的關(guān)鍵技術(shù)之一，對準(zhǔn)中常提到兩個指標(biāo)：對準(zhǔn)精度和對準(zhǔn)速度[1]。由于初始對準(zhǔn)誤差方程是非線性的，因此在小失準(zhǔn)角情況下，利用擴(kuò)展卡爾曼濾波(EKF)通?？梢缘玫奖葌鹘y(tǒng)卡爾曼濾波更優(yōu)的估值。但是EKF的本質(zhì)是非線性方程線性化，這樣舍棄了非線性的高階項，導(dǎo)致濾波結(jié)果為次優(yōu)，在某些情況下甚至?xí)l(fā)散，影響系統(tǒng)的正常工作[2]。20世紀(jì)90年代中期以來，人們逐漸拋棄了傳統(tǒng)的非線性模型Taylor展開近似的方法，采用非線性變換思想，產(chǎn)生了無跡卡爾曼濾波(UKF)[3]。

UKF以UT變換為基礎(chǔ)，采用卡爾曼濾波框架，具體采樣形式為確定性采樣，而非PF的隨機(jī)采樣，最常用的是(2n+1)個Sigma點的對稱采樣[4]。該濾波直接利用非線性模型，避免引入線性化誤差，并且采用的是確定性采樣，避免了PF的粒子點退化問題，提高了濾波精度。但是由于Sigma點計算量降低了算法的效率，限制了其在實時導(dǎo)航中的應(yīng)用。

針對UKF算法計算復(fù)雜的問題，本文提出一種改進(jìn)的UKF算法，該算法可以減少Sigma點計算量，從而提高算法效率。本文推導(dǎo)了改進(jìn)算法中核心的UT變換公式，給出了適合該算法的初始對準(zhǔn)誤差狀態(tài)方程并分析其精度，用實測數(shù)據(jù)對其進(jìn)行驗證，試驗結(jié)果表明，改進(jìn)的UKF算法性能與傳統(tǒng)UKF相當(dāng)，但效率提升了40%左右。

二、一種改進(jìn)的UKF算法

1.傳統(tǒng)UT變換

UKF過程可參考文獻(xiàn)[7—9]。由UT變換的過程可知，每一次運(yùn)算都需要(2n+1)個Sigma點經(jīng)過非線性函數(shù)，降低了運(yùn)算效率。本文給出一種改進(jìn)的UT變換算法，該算法可以減少Sigma點的計算量，從而提高UKF的效率。

2.改進(jìn)的UT變換

許多非線性方程是條件線性的。假設(shè)非線性方程y=f(x)為條件線性，將狀態(tài)x分為[aT,bT]T，這樣非線性方程可以寫成

y=f(x)=ψ(a)+γ(a)·b

(1)

式中，f(x)為任意非線性函數(shù)，并且na+nb=n；ψ是a的非線性方程；γ是a的非線性或線性方程。變換后狀態(tài)的均值和方差為

(2)

定義變量

(3)

(4)

方程y的均值由下式計算

(5)

由式(1)、式(5)可得

(6)

其中令非線性方程部分為

Φ(a)=ψ(a)+γ(a)·η(a)

(7)

式(6)是通過Φ(a)來計算y的均值，而式(7)可以用前面UT的方法進(jìn)行估計，計算Sigma點

(8)

權(quán)陣Wi也相應(yīng)地變?yōu)?/p>

(9)

將選取的狀態(tài)矢量通過非線性函數(shù)得

ξi=Φ(χi)i=0,1,…,2n

(10)

加權(quán)近似求解系統(tǒng)統(tǒng)計特性

(11)

由式(2)、式(3)、式(6)、式(7)及式(11)得y的方差Py為

(12)

從上面的過程可以看出，改進(jìn)的UT算法只需計算(2na+1)個Sigma點，這使計算量大大減小。該UT變換的思想是利用非線性函數(shù)中已有的線性化部分來減少整體的計算量，這與分解粒子濾波中僅對線性部分估計其邊緣后驗概率密度相似。

3.基于改進(jìn)UT變換的UKF算法過程

(1) 時間更新(預(yù)測)

由系統(tǒng)狀態(tài)方程對各個采樣的每一個Sigma點進(jìn)行非線性變換，得到變換后的Sigma點集為

(13)

對變換后的Sigma點集進(jìn)行加權(quán)處理，從而得到一步預(yù)測狀態(tài)為

(14)

同樣的，狀態(tài)的一步預(yù)測方差陣為

(15)

根據(jù)一步預(yù)測值，再次使用UT變換，產(chǎn)生新的Sigma點集

(16)

(17)

最后使用加權(quán)求和計算得到系統(tǒng)的預(yù)測觀測值

(18)

(2) 量測更新

計算協(xié)方差

(19)

可以得到系統(tǒng)量測變量的方差陣

(20)

濾波增益矩陣為

(21)

(22)

求解狀態(tài)后驗方差陣得

(23)

綜合上述時間更新和量測更新，UKF濾波過程如圖1所示。

圖1 UKF算法流程

三、SINS初始對準(zhǔn)非線性模型

1.初始對準(zhǔn)原理

在SINS/GNSS組合導(dǎo)航系統(tǒng)中，慣導(dǎo)需要進(jìn)行初始對準(zhǔn)，其主要工作是確定姿態(tài)矩陣的初始值，并利用濾波方法將初始失準(zhǔn)角估計出來，用于修正姿態(tài)矩陣，使系統(tǒng)工作時有正確的初始條件。初始對準(zhǔn)的精度會直接影響到系統(tǒng)的精度。因此初始對準(zhǔn)是慣性導(dǎo)航的重要階段，其精度和速度影響著慣導(dǎo)的工作性能，初始對準(zhǔn)分為粗對準(zhǔn)和精對準(zhǔn)過程。

精對準(zhǔn)的主要原理是根據(jù)粗對準(zhǔn)提供的初始姿態(tài)矩陣，利用慣性元件輸出信息，并用合適的濾波方法，將計算的導(dǎo)航坐標(biāo)系與真實導(dǎo)航坐標(biāo)系的失準(zhǔn)角估計出來，用來修正姿態(tài)矩陣，從而完成初始對準(zhǔn)，使計算坐標(biāo)系與真實坐標(biāo)系盡可能的重合。

2.捷聯(lián)慣導(dǎo)初始對準(zhǔn)誤差模型

捷聯(lián)慣導(dǎo)初始對準(zhǔn)的任務(wù)是建立計算坐標(biāo)系到導(dǎo)航坐標(biāo)系之間的坐標(biāo)轉(zhuǎn)換矩陣[5]。通過初始對準(zhǔn)誤差模型，根據(jù)量測值估計出失準(zhǔn)角，若不考慮垂直方向的影響，則濾波狀態(tài)為

x=[δVxδVyφxφyφzΔxΔyεxεyεz]

(24)

圖2 初始對準(zhǔn)流程

再根據(jù)速度誤差方程和姿態(tài)誤差方程建立的非線性初始對準(zhǔn)模型如下[6]

(25)

式中，φx為東向失準(zhǔn)角；φy為北向失準(zhǔn)角；φz為方位失準(zhǔn)角；δVx為東向速度誤差；δVy為北向速度誤差；L為當(dāng)?shù)鼐暥龋沪豬e為地球自轉(zhuǎn)角速度；εx為陀螺東向漂移；εy為陀螺北向漂移；εz為陀螺天向漂移；Δ為加速度計偏置。

為了方便計算，將誤差模型離散化為

(26)

式中，dt為時間間隔。

可以看出，該誤差方程是條件線性的，結(jié)合式(1)及式(26)得

a=[δVxδVyφxφyφz]T
b=[Δε]T

(27)

(28)

(29)

(30)

(31)

(32)

考慮一般非線性模型

xk=f(xk-1)+Qk-1

(33)

式中，xk為上述狀態(tài)模型；f(x)為改進(jìn)UKF方法非線性狀態(tài)方程；Q為過程噪聲

Q=[qδvxqδvyqφxqφyqφz00000]T

(34)

相應(yīng)的觀測方程為

Y=δV+R

(35)

式中，R=[rxry]T，為觀測噪聲。

通過上述誤差模型可知，利用MUT變換的UKF，每次遞歸只需計算11個Sigma點，與UT變換每次計算21個Sigma點比起來計算量大大減小。傳統(tǒng)的提高運(yùn)算效率的方法通常是忽略慣導(dǎo)陀螺儀和加速度計的儀器偏差，通常達(dá)不到理想的精度。另外一種提高效率的方法是SUKF(simplex UKF)，即利用較少的Sigma點進(jìn)行計算，但精度也因此受到損失。

由式(1)及式(27)—式(29)可以看出，本文提出的基于MUT變換方法的UKF在11個Sigma點的情況下也可以估計陀螺儀和加速度計的傳感器漂移，因為在SINS誤差模型中僅僅失準(zhǔn)角誤差方程和速度誤差方程是非線性的，而加速度計偏差和陀螺儀漂移是線性的。由于MUKF也是一種卡爾曼濾波模型，因此它對過程噪聲和測量噪聲會比較敏感，但是許多健壯噪聲矩陣的自適應(yīng)卡爾曼濾波方法同樣也可以應(yīng)用于MUKF，這些方法本文不再提及。

四、試驗與分析

分別用傳統(tǒng)的UKF方法與改進(jìn)的MUKF方法處理數(shù)據(jù)，速度誤差與失準(zhǔn)角偏差結(jié)果如圖3和圖4所示。

圖3 速度誤差比較

圖3和圖4分別是MUKF與UKF速度誤差與失準(zhǔn)角估計誤差比較，從圖中可以看出，速度誤差估計精度兩者相仿，但是MUKF收斂速度較慢。失準(zhǔn)角誤差估計中，北向失準(zhǔn)角誤差估計MUKF稍好，東向失準(zhǔn)角和方位失準(zhǔn)角兩者基本相同。本次試驗采用的電腦為Inter Core2雙核處理器，在計算效率上，MUKF解算時間為3.918 2 s，而UKF解算時間為6.899 4 s，MUKF的解算效率較UKF提高了40%左右，這是因為MUKF在每次非線性遞歸計算時僅需要計算11個Sigma點，而UKF需要計算21個。因此可以得出，慣性導(dǎo)航初始對準(zhǔn)中MUKF與UKF相比性能相似，但MUKF計算效率有了大幅提升。

圖4 失準(zhǔn)角誤差比較

五、結(jié)束語

本文提出了一種高計算效率的改進(jìn)UKF方法，并將它應(yīng)用于捷聯(lián)慣導(dǎo)初始對準(zhǔn)當(dāng)中。它是基于SINS初始對準(zhǔn)誤差模型的條件線性，只有速度誤差與失準(zhǔn)角誤差是非線性的。試驗結(jié)果顯示，MUKF與UKF相比有同等的精度，但MUKF計算效率更高，適用于實時導(dǎo)航中。

[1] 陸海勇, 趙偉, 賀榮光, 等.捷聯(lián)慣導(dǎo)初始對準(zhǔn)的UKF改進(jìn)算法[J].航空兵器, 2009(3): 17-21.

[2] HOVLAND G E, VON HOFF T P, GALLESTEY E A, et al.Nonlinear Estimation Methods for Parameter Tracking in Power Plants[J].Control Engineering Practice, 2005, 13(11): 1341-1345.

[3] JULIER S T, UHLMANN J K.Unscented Filtering and Nonlinear Estimation[J].Proceedings of the IEEE Aerospace and Electronic System, 2004, 92(3): 401-402.

[4] FAUBEL F, KLAKOW D.A Transformation-based Derivation of the Kalman Filter and an Extensive Unscented Transform[C] ∥IEEE/SP 15th Workshop on Statistical Signal Processing.Cardiff: IEEE, 2009: 161-164.

[5] 項冬.高精度機(jī)載INS/GPS組合導(dǎo)航關(guān)鍵算法研究[D].北京: 中國測繪科學(xué)研究院, 2013.

[6] DMITRIYEV S T, STEMPANOV O A, SHPEL S V.Nonlinear Filtering Methods Application in INS Alignment[J].IEEE Transactions on Aerospace and Electronic System, 1997, 33(1): 260-271.

[7] JULIER S J, UHLMANN J K, DURRANT-WHYTE H F.A New Approach for Filtering Nonlinear Systems[C] ∥Proceeding of the 1995 American Control Conference.Seattle, WA: IEEE, 1995: 1628-1632.

[8] JULIER S J, UHLMANN J K.A New Extension of the Kalman Filter to Nonlinear System[C] ∥The Proceeding of Aero Sense: The 11th International Symposium on Aerospace Defense Sensing.Orlando, FL: Simulation and Controls, 1997: 54-56.

[9] JULIER S T, UHLMANN T K, DURRANT-WHYTE H F.A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators[J].IEEE Transactions on Automatic Control, 2000, 45(3): 477-482.

Application of an Improved UKF Algorithm in SINS Initial Alignment

MA Hongyang，CHENG Pengfei，WANG Qianxin

馬宏陽，程鵬飛，王潛心.一種改進(jìn)的UKF算法在捷聯(lián)慣導(dǎo)初始對準(zhǔn)中的應(yīng)用[J].測繪通報，2015(7)：18-22.

10.13474/j.cnki.11-2246.2015.0202

2014-03-06

馬宏陽(1991—)，男，碩士生，研究方向為GNSS數(shù)據(jù)處理。E-mail：mahongyangcm@163.com

P228.4

：B

：0494-0911(2015)07-0018-05