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

馬宏陽(yáng)，程鵬飛，王潛心

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

?

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

馬宏陽(yáng)1，程鵬飛2，王潛心1

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

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

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

一、引 言

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

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

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

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

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

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

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

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

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

(1)

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

(2)

定義變量

(3)

(4)

方程y的均值由下式計(jì)算

(5)

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

(6)

其中令非線(xiàn)性方程部分為

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

(7)

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

(8)

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

(9)

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

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

(10)

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

(11)

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

(12)

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

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

(1) 時(shí)間更新(預(yù)測(cè))

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

(13)

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

(14)

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

(15)

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

(16)

(17)

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

(18)

(2) 量測(cè)更新

計(jì)算協(xié)方差

(19)

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

(20)

濾波增益矩陣為

(21)

(22)

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

(23)

綜合上述時(shí)間更新和量測(cè)更新，UKF濾波過(guò)程如圖1所示。

圖1 UKF算法流程

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

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

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

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

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

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

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

(24)

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

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

(25)

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

為了方便計(jì)算，將誤差模型離散化為

(26)

式中，dt為時(shí)間間隔。

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

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

(27)

(28)

(29)

(30)

(31)

(32)

考慮一般非線(xiàn)性模型

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

(33)

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

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

(34)

相應(yīng)的觀(guān)測(cè)方程為

Y=δV+R

(35)

式中，R=[rxry]T，為觀(guān)測(cè)噪聲。

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

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

四、試驗(yàn)與分析

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

圖3 速度誤差比較

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

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

五、結(jié)束語(yǔ)

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

[1] 陸海勇, 趙偉, 賀榮光, 等.捷聯(lián)慣導(dǎo)初始對(duì)準(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] 項(xiàng)冬.高精度機(jī)載INS/GPS組合導(dǎo)航關(guān)鍵算法研究[D].北京: 中國(guó)測(cè)繪科學(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

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

10.13474/j.cnki.11-2246.2015.0202

2014-03-06

馬宏陽(yáng)(1991—)，男，碩士生，研究方向?yàn)镚NSS數(shù)據(jù)處理。E-mail：mahongyangcm@163.com

P228.4

：B

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