梁新宇，吳建德，黃國勇，孫 磊

LIANG Xinyu1，2,WU Jiande1，2,HUANG Guoyong1，2,SUN Lei1，2

1.昆明理工大學 信息工程與自動化學院，昆明 650500

2.云南省礦物管道輸送工程技術(shù)研究中心，昆明 650500

1.Faculty of Information Engineering&Automation,Kunming University of Science and Technology,Kunming 650500,China

2.Engineering Research Center for Mineral Pipeline Transportation,Kunming 650500,China

1 引言

組合導航系統(tǒng)就是采用數(shù)據(jù)融合技術(shù)將各導航子系統(tǒng)以適當方式組合起來，取長補短，以達到提高系統(tǒng)精度和改善系統(tǒng)可靠性等目的。目前，工程上組合導航采用的數(shù)據(jù)融合方法主要是卡爾曼濾波（Kalman Filter，KF）和擴展卡爾曼濾波（Extended Kalman Filter，EKF）[1]。EKF以對非線性函數(shù)進行泰勒級數(shù)展開且只保留一階項的線性化方法，進而應用線性卡爾曼濾波框架進行遞推估計，因此EKF存在一階線性化近似精度偏低，需要計算雅克比矩陣以及要求非線性函數(shù)連續(xù)可微等自身無法克服的理論局限性[2]。為此，Julier等人基于“近似非線性函數(shù)的概率密度分布易于近似非線性函數(shù)本身”這一思路，提出了以UT變換來逼近最優(yōu)狀態(tài)估計框架中非線性狀態(tài)后驗分布的估計方法——無跡卡爾曼濾波（Unscented Kalman Filter，UKF）[3]，在系統(tǒng)非線性程度較強的情況下，相比于傳統(tǒng)的EKF，UKF數(shù)值穩(wěn)定性較強，濾波精度較高。但對于高維系統(tǒng)，UKF易出現(xiàn)協(xié)方差陣非正定的情況，導致濾波發(fā)散[4]。基于球面徑向規(guī)則的容積卡爾曼濾波（Cubature Kalman Filter，CKF）是UKF的特殊形式[2]，是2009年由加拿大學者Arasaratnam等人[5-6]提出的貝葉斯近似非線性濾波算法，應用球面徑向容積準則來逼近最優(yōu)框架中的狀態(tài)后驗分布，進行相應的數(shù)值積分運算。在系統(tǒng)處于任何非線性程度下，球面徑向容積規(guī)則都能以較高精度逼近非線性系統(tǒng)函數(shù)的狀態(tài)后驗均值及協(xié)方差，因此，CKF的精度高于通常的EKF，尤其適用于系統(tǒng)非線性程度較強的狀態(tài)估計問題，且無需計算非線性函數(shù)的雅可比矩陣。由于CKF中各積分點權(quán)值相同且為正數(shù)，因此其數(shù)值穩(wěn)定性優(yōu)于UKF[7]，更適用于解決高維組合導航系統(tǒng)的狀態(tài)估計問題，且CKF能夠更精確地保留系統(tǒng)的一二階矩信息，具有更高的濾波精度[8]。然而CKF在迭代過程中由于計算機的舍入誤差等原因易出現(xiàn)協(xié)方差陣的非正定性，進而導致計算發(fā)散的問題，且在系統(tǒng)非線性程度較強的情況下，由于系統(tǒng)狀態(tài)模型及噪聲統(tǒng)計特性的不確定性，也會對濾波效果產(chǎn)生嚴重影響，導致系統(tǒng)魯棒性降低，甚至出現(xiàn)濾波故障。

近年來，許多學者針對CKF存在的濾波發(fā)散的問題進行了大量的研究。其中，文獻[9]在標準CKF的基礎(chǔ)上，引入了矩陣的QR分解，采用平方根迭代的思想，結(jié)合強跟蹤濾波的理論框架，提出了一種自適應強跟蹤容積卡爾曼濾波算法，有效地改善了CKF的濾波性能；文獻[10]則將H∞魯棒濾波思想應用于標準的CKF，提出了一種基于CKF的新型魯棒濾波算法，并用于列車組合定位中，驗證了該結(jié)合方法對于改善濾波性能的有效性；文獻[11]針對實際應用中，由于約束條件γ的選取較小而導致Riccati不等式無解的情況，采用奇異值分解的方法，放寬了對約束條件γ的選取，在一定程度上改善了濾波的穩(wěn)定性；文獻[12]基于新息協(xié)方差匹配原理構(gòu)建的魯棒CKF，通過定義數(shù)據(jù)質(zhì)量檢測函數(shù)，根據(jù)測量數(shù)據(jù)質(zhì)量選擇魯棒CKF或標準CKF作為子系統(tǒng)的最優(yōu)濾波算法，仿真結(jié)果表明，該方法在一定程度上提高了導航子系統(tǒng)的魯棒性。

對此，本文基于簡化的CKF濾波算法，引入數(shù)值穩(wěn)定性較強的奇異值分解方法，對系統(tǒng)狀態(tài)協(xié)方差陣進行分解迭代，保證了算法中矩陣計算的數(shù)值穩(wěn)定性；并結(jié)合H∞濾波思想，基于矩陣不等式的理論，將約束條件γ的自適應選取方法用于該算法中，提出了一種H∞魯棒自適應CKF算法，在系統(tǒng)狀態(tài)模型及噪聲統(tǒng)計特性不確定的情況下，有效改善了濾波算法的穩(wěn)定性，提高了系統(tǒng)的魯棒性及其自適應能力。

2 簡化的SVD-CKF算法

2.1 簡化的CKF算法

在GNSS/INS組合導航系統(tǒng)中，觀測方程一般是線性的，而在標準的CKF算法中，觀測方程是非線性的，非線性的量測更新計算復雜度高，不利于在硬件設(shè)備上實現(xiàn)。因此，當標準CKF算法用于線性觀測系統(tǒng)時，將線性的觀測方程帶入CKF算法中，經(jīng)過簡化，可有效降低計算復雜度，便于算法在硬件設(shè)備中的實現(xiàn)。

故針對GNSS/INS組合導航，考慮如下離散時間非線性-線性動態(tài)系統(tǒng)：

式中，xk∈Rn為n×1維的系統(tǒng)狀態(tài)向量，zk∈Rm為m×1維的量測向量，f(?)為非線性函數(shù)，Hk為線性量測陣。wk-1和vk分別為狀態(tài)系統(tǒng)和觀測系統(tǒng)的均值為0，協(xié)方差為Qk-1和Rk的高斯白噪聲，且相互獨立。

當觀測方程為線性時，標準CKF算法中的量測更新部分可簡化為如下形式：

由上可知，經(jīng)過簡化的量測更新部分都退化為標準卡爾曼濾波的表達式，得到了簡化的CKF算法，該方法是針對組合導航系統(tǒng)觀測方程為線性的特性進行簡化，在濾波精度上并未降低，滿足組合導航的要求。

2.2 簡化的SVD-CKF算法

標準CKF算法經(jīng)過多次遞推，隨著濾波步數(shù)的增加，計算的舍入誤差逐漸累積，易使系統(tǒng)狀態(tài)協(xié)方差陣Pk失去正定性甚至失去對稱性，進而導致濾波發(fā)散。奇異值分解具有良好的數(shù)值穩(wěn)定性，用其進行系統(tǒng)狀態(tài)協(xié)方差陣的分解迭代，能有效提高濾波算法的穩(wěn)定性，改善組合導航非線性動態(tài)系統(tǒng)的濾波性能[13]。

基于奇異值分解且觀測方程為線性時，簡化的SVD-CKF算法步驟如下：

（1）計算點集和權(quán)值。三階球面徑向準則對應的點集和權(quán)值為：

其中，i=1,2,…,m，m表示基本容積點個數(shù)，三階球面徑向容積準則，容積點數(shù)是狀態(tài)維數(shù)的2倍，即m=2n，n為狀態(tài)維數(shù)；[1]表示完整全對稱點集，以n=2為例，[1]i表示如下點集中的第i個元素：

（2）時間更新。對簡化的CKF算法中的協(xié)方差陣進行奇異值分解，取平方根[14]，即：

計算基本容積點：

計算狀態(tài)傳播容積點：

計算狀態(tài)預測值：

計算狀態(tài)預測協(xié)方差陣：

（3）量測更新。對狀態(tài)預測協(xié)方差陣進行奇異值分解，即：

計算量測預測值：

簡化的CKF算法通過狀態(tài)預測協(xié)方差陣Pk/k-1求解增益矩陣Kk和系統(tǒng)狀態(tài)協(xié)方差陣Pk，而在SVDCKF算法中這樣求解不利于分解后的協(xié)方差矩陣迭代，故采用以下形式求解系統(tǒng)狀態(tài)協(xié)方差陣Pk和增益矩陣Kk[14]。

計算系統(tǒng)狀態(tài)協(xié)方差陣：

計算增益矩陣：

對系統(tǒng)狀態(tài)協(xié)方差陣Pk進行奇異值分解，得：Pk=，對進行cholesky分解，得則增益矩陣Kk的求解形式可進一步寫成[15]：

計算狀態(tài)更新值：

3 H∞魯棒自適應CKF算法

3.1 H∞濾波基本原理

H∞濾波針對系統(tǒng)模型及噪聲統(tǒng)計特性的不確定性，而引入H∞范數(shù)思想，使得從干擾輸入到濾波誤差輸出的H∞范數(shù)最小化，進而使系統(tǒng)在最壞干擾情況下的估計誤差實現(xiàn)最小[16]。

定義如下代價函數(shù)[17]：

其中，wk，vk表示系統(tǒng)噪聲和觀測噪聲，均為白噪聲且相互獨立，Qk，Rk為對應方差；x0為狀態(tài)初值，方差為P0為其估計值。

其余各項與此類似。

通常情況下，最優(yōu)H∞濾波問題難以求解得到解析形式的解[17]，可以尋求次優(yōu)迭代算法，設(shè)計一門限值γ，滿足，即如下Riccati不等式[18]：

其中，Pk為協(xié)方差矩陣，Hk為觀測矩陣，Lk為系數(shù)矩陣，取單位陣。在H∞濾波器中，約束條件γ控制狀態(tài)估計在最不利條件下的估計誤差，γ越小，系統(tǒng)的魯棒性越強；γ越大，H∞濾波的特性越接近于標準的卡爾曼濾波[18]。

3.2 約束條件γ的自適應選取

約束條件γ對濾波精度和魯棒性都有重要影響。通常參數(shù)γ根據(jù)工程實踐經(jīng)驗初始設(shè)置為固定值，從而使濾波器性能具有一定的保守性，無法適應組合導航系統(tǒng)應用環(huán)境可能存在的變化，不能保證在估計誤差較小的同時系統(tǒng)仍具有較強的魯棒性[19]。因此，需對γ的取值進行自適應優(yōu)化。

根據(jù)矩陣不等式相關(guān)理論及H∞魯棒濾波約束條件γ與rk之間存在的反比關(guān)系，確定γ值的自適應選取條件[19]。rk為濾波新息向量，如下式：

定理1[19]設(shè)A和B為2個n階Hermite矩陣，A＞0，B≥0，則。這里 λ1(A)表示 A的最大特征值。

根據(jù)定理1，由上述Riccati不等式得：γ2＞λ1(A)，其中定義，進而得γ的值為：

式中，β＞0為相關(guān)系數(shù)，與系統(tǒng)的實際情況有關(guān)，需通過實驗來確定。確定β后，系數(shù)α的值就僅與濾波新息相關(guān)。

圖1 H∞魯棒自適應CKF算法流程圖

3.3 H∞魯棒自適應CKF算法

當H∞濾波存在時，系統(tǒng)狀態(tài)協(xié)方差陣Pk滿足如下的遞推形式[20]：

用H∞濾波系統(tǒng)狀態(tài)協(xié)方差陣Pk的遞推式，替換簡化的SVD-CKF算法中的Pk，并進行約束條件γ值的自適應選取，進而計算Pk的更新式，即可構(gòu)成H∞魯棒自適應CKF算法，流程圖如圖1所示。

該算法針對線性觀測方程組合導航系統(tǒng)，對標準CKF算法量測更新部分進行簡化，降低了計算復雜度；同時，采用數(shù)值穩(wěn)定性較強的SVD方法，對系統(tǒng)狀態(tài)協(xié)方差矩陣進行分解迭代，改善了算法穩(wěn)定性，但也額外增加了運算復雜度；進而，結(jié)合H∞濾波理論，并對約束條件進行了自適應選取，其中存在一系列矩陣求逆等運算過程，同樣也增加了算法的運算復雜度。在系統(tǒng)狀態(tài)模型及噪聲統(tǒng)計特性不確定的情況下，有效改善了濾波算法的穩(wěn)定性，提高了系統(tǒng)的魯棒性及其自適應能力，總體上提升了組合導航系統(tǒng)的濾波精度。

4 數(shù)值仿真

假設(shè)固定翼飛機做機動飛行，先后完成爬升、變速、平飛和轉(zhuǎn)彎等飛行狀態(tài)。初始位置為東經(jīng)107.002°，北緯29.498°，高度300 m；初始速度為50 m/s，方向正北。其中，陀螺常值漂移為0.1（°/h），一階馬爾可夫過程相關(guān)時間為3 600 s，加計常值誤差為1.0－4g，一階馬爾可夫過程相關(guān)時間為1 800 s。GNSS水平位置誤差均方根為10 m，高度誤差均方根20 m，速度誤差均方根0.1 m/s。INS初始水平位置誤差為50 m，高度誤差100 m，初始速度誤差0.5 m/s。GNSS采樣周期為1 s，INS采樣周期為0.02 s，濾波周期為1 s，仿真時間1 200 s。

濾波過程以慣導系統(tǒng)為主系統(tǒng)，用線性的GNSS系統(tǒng)對其進行閉環(huán)修正，采用標準CKF算法、H∞魯棒CKF算法和H∞魯棒自適應CKF算法分別進行仿真實驗。

系統(tǒng)狀態(tài)向量xk為：

式中，φE,φN,φU為INS輸出的姿態(tài)角，vE,vN,vU為東北天方向的速度，l,λ,h 為東北天方向的位置，εbx,εby,εbz為陀螺隨機漂移。

狀態(tài)方程為：

其中，f(?)為非線性函數(shù)，由捷聯(lián)慣導力學編排方程和姿態(tài)誤差方程得到，wk-1為系統(tǒng)噪聲。

量測向量zk為：

式中，vEG,vNG,vUG為GNSS輸出的東北天方向的速度，lG,λG,hG為東北天方向的位置。

觀測方程為：

其中，Hk=[I6×606×6]，vk為觀測噪聲。

實驗中，模擬飛機飛行過程如圖2。

圖2 真實航跡及算法導航航跡

圖3～圖5為三種算法的對比仿真結(jié)果，對比三種算法可以看出，H∞魯棒自適應CKF算法在姿態(tài)角度及東北天方向速度、位置的解算上均具有較高的精度，且相比于標準CKF算法和H∞魯棒CKF算法，能夠在特殊條件下表現(xiàn)出較好的穩(wěn)定性和魯棒性。其中，在姿態(tài)角度誤差及東北天方向速度誤差的對比實驗中，雖然三種算法精度相當，但當系統(tǒng)在700～800 s時間內(nèi)發(fā)生了爬升、轉(zhuǎn)彎等一系列強非線性飛行狀態(tài)時，標準CKF算法穩(wěn)定性差，出現(xiàn)了較大的波動，而H∞魯棒自適應CKF算法則表現(xiàn)出了較強的濾波穩(wěn)定性及魯棒性；在東北天方向位置誤差的比較中，H∞魯棒自適應CKF算法也表現(xiàn)出了同樣的效果，且總體精度優(yōu)于其他兩種算法。統(tǒng)計三種算法的運行時間，其中標準CKF算法運行時間為16.055 6 s，H∞魯棒CKF算法運行時間為14.071 2 s，H∞魯棒自適應CKF算法運行時間為13.455 5 s。由此可見，H∞魯棒自適應CKF算法的運算復雜度并未增加。

對比三種算法的均方根誤差（RMSE），同樣可以看出H∞魯棒自適應CKF算法相比于其他兩種算法的優(yōu)勢所在。其中，表1為三種算法東北天方向速度與位置RMSE比較。

圖3 姿態(tài)角度誤差比較

圖4 東北天方向速度誤差比較

圖5 東北天方向位置誤差比較

5 結(jié)束語

本文算法針對線性觀測方程組合導航系統(tǒng)，在對標準CKF算法量測更新部分進行簡化的基礎(chǔ)上，采用數(shù)值穩(wěn)定性較強的奇異值分解方法，分解系統(tǒng)狀態(tài)協(xié)方差矩陣，優(yōu)化迭代過程；同時，結(jié)合H∞濾波理論，并進行約束條件γ值的自適應選取，進而計算系統(tǒng)狀態(tài)協(xié)方差陣的更新式。在系統(tǒng)狀態(tài)模型及噪聲統(tǒng)計特性不確定的情況下，保證了濾波精度和算法迭代的穩(wěn)定性。

將該算法應用于GNSS/INS組合導航系統(tǒng)的數(shù)值仿真實驗。結(jié)果表明，當系統(tǒng)發(fā)生大幅度爬升、轉(zhuǎn)彎等一系列強非線性飛行狀態(tài)時，本文算法在保證了濾波精度和算法穩(wěn)定性的同時，有效提高了系統(tǒng)的魯棒性及其自適應能力。

由于在約束條件β的自適應選取部分，相關(guān)系數(shù)β的值需根據(jù)系統(tǒng)的實際情況通過實驗來確定。因此，研究實現(xiàn)γ值自適應選取的方法，進一步提高本文算法的自適應能力，是下一步需要解決的問題。

表1 三種算法東北天方向速度與位置RMSE比較

參考文獻：

[1]趙琳，王小旭，丁繼成，等.組合導航系統(tǒng)非線性濾波算法綜述[J].中國慣性技術(shù)學報，2009，17（1）：46-52.

[2]王小旭，潘泉，黃鶴，等.非線性系統(tǒng)確定采樣型濾波算法綜述[J].控制與決策，2012，27（6）：812-801.

[3]Julier S J，Uhlmann J K，Durrant-Whyte H F.A new method for the nonlinear transformation of means and covariances[J].IEEE Transactions on Automatic Control，2000，45（3）：477-482.

[4]Liu Xing，Jiang Shoushan.Research on target tracking based on unscented Kalman filter[J].Sensors&Transducers，2013，153（6）：13-21.

[5]Arasaratnam I，Haykin S.Cubature Kalman filters[J].IEEE Transactions on Automatic Control，2009，54（6）：1254-1269.

[6]Arasaratnam I，Haykin S，Hurd R T.Cubature Kalman filtering for continuous-discrete system：Theory and simulations[J].IEEE Transactions on Sigal Processing，2010，58（10）：4977-4993.

[7]崔乃剛，張龍，王小剛，等.自適應高階容積卡爾曼濾波在目標跟蹤中的應用[J].航空學報，2015，36（12）：3885-3895.

[8]Dai H D，Dai S W，Cong Y C，et al.Performance comparison of EKF/UKF/CKF for the tracking of ballistic target[J].Telkomnika Indonesian J of Electrical Engineering，2012，10（7）：1692-1699.

[9]趙利強，羅達燦，王建林，等.自適應強跟蹤容積卡爾曼濾波算法[J].北京化工大學學報，2013，40（3）：98-103.

[10]劉江，蔡伯根，唐濤，等.基于CKF的GNSS/INS列車組合定位魯棒濾波算法[J].交通運輸工程學報，2010，10（5）：102-107.

[11]張秋昭，張書畢，劉志平，等.基于奇異值分解的魯棒容積卡爾曼濾波及其在組合導航中的應用[J].控制與決策，2014，29（2）：341-346.

[12]徐樹生，林孝工.基于魯棒CKF的多傳感器全信息融合算法[J].電機與控制學報，2013，17（2）：90-97.

[13]高社生，王建超，焦雅琳.自適應SVD-UKF算法及在組合導航的應用[J].中國慣性技術(shù)學報，2010，18（6）：737-741.

[14]孫磊，黃國勇，李越.改進的強跟蹤SVD-UKF算法在組合導航中的應用[J].計算機工程與應用，2017，53（10）：225-229.

[15]張友民，戴冠中，張洪才.基于SVD的推廣卡爾曼濾波及其在飛行狀態(tài)和參數(shù)估計中的應用[J].控制理論與應用，1996，13（1）：106-114.

[16]Dai H，Li J X，Jin H M.Application of robust kalman filtering to integrated navigation based on inertial navigation system and dead reckoning[C]//International Conference on Artificial Intelligence and Computational Intelligence，Sanya，2010.

[17]侯代文，殷福亮，陳喆.基于sigma點H∞濾波的說話人跟蹤方法[J].信號處理，2009，25（3）：374-378.

[18]朱英，梁彥，楊猛，等.自適應補償H∞濾波器在組合導航中的應用[J].火力與指揮控制，2010，35（10）：15-19.

[19]劉曉光，胡靜濤，王鶴.基于自適應H∞濾波的組合導航方法研究[J].儀器儀表學報，2014，35（5）：1013-1021.

[20]李兆銘，楊文革，丁丹，等.基于SVD的多終端實時定軌自適應魯棒CKF算法[J].儀器儀表學報，2016，37（3）：490-496.