一種改進的UKF算法在捷聯慣導初始對準中的應用

馬宏陽，程鵬飛，王潛心

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

?

一種改進的UKF算法在捷聯慣導初始對準中的應用

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

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

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

UT變換；UKF；初始對準；捷聯慣導；Sigma點

一、引 言

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

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

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

二、一種改進的UKF算法

1.傳統UT變換

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

2.改進的UT變換

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

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

(1)

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

(2)

定義變量

(3)

(4)

方程y的均值由下式計算

(5)

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

(6)

其中令非線性方程部分為

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

(7)

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

(8)

權陣Wi也相應地變為

(9)

將選取的狀態矢量通過非線性函數得

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

(10)

加權近似求解系統統計特性

(11)

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

(12)

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

3.基于改進UT變換的UKF算法過程

(1) 時間更新(預測)

由系統狀態方程對各個采樣的每一個Sigma點進行非線性變換，得到變換后的Sigma點集為

(13)

對變換后的Sigma點集進行加權處理，從而得到一步預測狀態為

(14)

同樣的，狀態的一步預測方差陣為

(15)

根據一步預測值，再次使用UT變換，產生新的Sigma點集

(16)

(17)

最后使用加權求和計算得到系統的預測觀測值

(18)

(2) 量測更新

計算協方差

(19)

可以得到系統量測變量的方差陣

(20)

濾波增益矩陣為

(21)

(22)

求解狀態后驗方差陣得

(23)

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

圖1 UKF算法流程

三、SINS初始對準非線性模型

1.初始對準原理

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

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

2.捷聯慣導初始對準誤差模型

捷聯慣導初始對準的任務是建立計算坐標系到導航坐標系之間的坐標轉換矩陣[5]。通過初始對準誤差模型，根據量測值估計出失準角，若不考慮垂直方向的影響，則濾波狀態為

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

(24)

圖2 初始對準流程

再根據速度誤差方程和姿態誤差方程建立的非線性初始對準模型如下[6]

(25)

式中，φx為東向失準角；φy為北向失準角；φz為方位失準角；δVx為東向速度誤差；δVy為北向速度誤差；L為當地緯度；ωie為地球自轉角速度；εx為陀螺東向漂移；εy為陀螺北向漂移；εz為陀螺天向漂移；Δ為加速度計偏置。

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

(26)

式中，dt為時間間隔。

可以看出，該誤差方程是條件線性的，結合式(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為上述狀態模型；f(x)為改進UKF方法非線性狀態方程；Q為過程噪聲

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

(34)

相應的觀測方程為

Y=δV+R

(35)

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

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

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

四、試驗與分析

分別用傳統的UKF方法與改進的MUKF方法處理數據，速度誤差與失準角偏差結果如圖3和圖4所示。

圖3 速度誤差比較

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

圖4 失準角誤差比較

五、結束語

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

[1] 陸海勇, 趙偉, 賀榮光, 等.捷聯慣導初始對準的UKF改進算法[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] 項冬.高精度機載INS/GPS組合導航關鍵算法研究[D].北京: 中國測繪科學研究院, 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

馬宏陽，程鵬飛，王潛心.一種改進的UKF算法在捷聯慣導初始對準中的應用[J].測繪通報，2015(7)：18-22.

10.13474/j.cnki.11-2246.2015.0202

2014-03-06

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

P228.4

：B

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