基于平滑變結構卡爾曼濾波的機載組合導航算法*

高春雷 趙 賓,2

1.南京航空航天大學金城學院，南京 211156； 2.南京航空航天大學自動化學院，南京 211106

0 引言

捷聯慣導系統(SINS)具有短期精度高、自主性高、抗干擾能力強等優勢，但受其工作原理所限，誤差隨時間逐漸積累，長期穩定性較差[1]；全球衛星導航系統(GNSS)具有長期精度高的優點，然而在使用時易受到環境和人為干擾，定位、測速精度嚴重下降，無法保證持續導航定位的能力[2]。SINS和GNSS的信息具有互補性，將二者結合構成組合導航系統可實現優勢互補[3]，實現無人機的自主導航。

在SINS/GNSS組合導航中，常采用卡爾曼濾波器實現對導航誤差的最優估計和修正?？柭鼮V波器是一種最優估計技術，但是其要求系統的數學模型精確，且系統噪聲和量測噪聲均服從高斯分布，否則濾波的精度將下降甚至發散[4]。

為克服卡爾曼濾波器對噪聲的嚴格高斯假設，國內外學者提出了高斯和濾波器，利用高斯和對非高斯概率分布進行建模[5]，但是此方法增加了濾波器的計算復雜度。

近年來，滑?？刂频乃枷氡粡V泛應用到控制領域。文獻[6]提出了基于解耦的變結構自適應滑?？刂品椒ǎ瑢崿F了火箭全向發射下的姿態穩定；文獻[7]提出一種新型的平滑估計器，解決了微小衛星主動繞飛時經典卡爾曼濾波算法存在的估計值抖動和反轉問題。

采用滑?？刂频牟呗?，通過系統先驗知識設定狀態估計的邊界，可用來提高對模型失配和非高斯噪聲魯棒性。平滑變結構(Smooth Variable Structure Filter, SVSF)算法就是基于此思路，采用滑模控制概念，通過變結構的增益，使得預測狀態逐步接近真實狀態[8]，對于系統模型失配以及噪聲不符合高斯分布的情況具有較好的魯棒性和穩定性。

本文以機載捷聯慣導系統為應用對象，將滑?？刂频乃枷胍氲絊INS/GNSS組合導航中，研究基于卡爾曼的平滑變結構濾波器，克服常規卡爾曼濾波算法對系統模型和噪聲特性的限制，提高組合導航的魯棒性。

1 平滑變結構卡爾曼濾波算法

如圖1所示隨時間變化的系統狀態的軌跡，基于SVSF理論，估計狀態是通過系統的不確定模型得到的，通過SVSF增益的選取，可使得估計狀態軌跡朝著系統狀態軌跡逼近，直至進入到存在子空間以內，然后估計狀態被限制在存在子空間內時，沿著系統狀態軌跡來回的切換[9-11]。存在子空間始終圍繞著真實的系統狀態軌跡，其寬度包含了干擾和不確定性。如果干擾和不確定性是有界的，就可以得到存在子空間寬度的界限。因此，SVSF算法對系統模型和噪聲模型不準確的情況具有較好的魯棒性和穩定性。

圖1 SVSF估計的基本思想

本文采用基于協方差的線性離散系統迭代方程，研究基于平滑變結構卡爾曼的組合導航濾波算法。

設線性離散系統具有如下的狀態方程以及量測方程：

(1)

式中：Xk表示k時刻的系統狀態向量(n維)；Zk表示k時刻的量測向量(m維)；Φk/k-1表示由k時刻到k+1時刻的一步狀態轉移矩陣，大小為n×n階；Wk表示k時刻的系統噪聲向量；Hk是k時刻的量測系數矩陣，大小為m×n階；Γk表示系統噪聲系數矩陣；Vk表示k時刻的量測噪聲向量(m維)。

平滑變結構卡爾曼濾波迭代方程如下：

1)通過系統模型計算一步預測狀態估計

(2)

2)計算一步預測狀態估計協方差矩陣

(3)

式中：Qk-1為系統噪聲協方差陣。

(4)

4)根據實際量測值和預測量測值，計算先驗量測誤差變量ez,k/k-1

(5)

5)根據噪聲信息和誤差信息計算SVSF增益

(6)

其中diag(v)表示以向量v的各元素作為主對角線元素，構造對角矩陣；|e|表示對向量e中的各元素取絕對值，得到與e維數相同的向量；“°”表示Hadamard乘積；S是對先驗量測誤差列向量ez,k/k-1中各元素進行飽和運算后得到的與ez,k/k-1維數相同的列向量，其各元素的飽和運算定義如下：

(7)

其中，ezi,k/k-1為先驗量測誤差列向量ez,k/k-1的第i個元素，ψkii為平滑有界寬度陣ψk的第i個對角線元素，二者相除構成飽和運算得到的列向量S的第i個元素。平滑有界寬度ψk可以減小由于濾波增益高頻切換引起的狀態估計的抖振，

(8)

式(8)中，Rk為量測噪聲協方差陣。

通過式(7)飽和運算，在有界寬度外，可保證估計的魯棒性和穩定性，在有界寬度內，可通過插值獲得平滑函數。

由式(6)可以看出，SVSF增益是一個函數，它與先驗量測誤差變量ez,k/k-1、后驗量測誤差變量ez,k-1/k-1、平滑有界寬度ψk、記憶因子γ(0<γ<1)以及量測矩陣Hk有關。

6)由一步預測狀態估計和SVSF增益計算狀態估計

(9)

7)計算狀態估計協方差矩陣

(10)

8)最后，更新后驗量測誤差變量ez,k/k

(11)

2 基于平滑變結構卡爾曼濾波的組合導航方案設計及系統模型

2.1 組合導航方案設計

以機載捷聯慣導系統為組合導航研究對象。在組合導航過程中，利用捷聯慣導系統解算的位置、速度與GNSS測得的位置、速度之差作為量測信息，用本文提出的平滑變結構卡爾曼濾波器對捷聯慣導系統的各項誤差信息進行估計，濾波過程中對慣導系統實時進行閉環校正。基于平滑變結構卡爾曼濾波的SINS/GNSS組合導航方案如圖2所示。

圖2 基于平滑變結構卡爾曼濾波的SINS/GNSS組合導航方案框圖

2.2 組合導航系統模型

1)組合導航系統狀態方程

通過對SINS系統性能和誤差源的分析，即可得到組合導航系統狀態方程：

(12)

式中：X(t)為系統狀態向量；Φ(t)為狀態轉移矩陣；Γ(t)為系統噪聲系數矩陣；W(t)為系統噪聲向量。

2) 組合導航系統量測方程

以捷聯慣導系統導航解算得到位置、速度和衛星導航系統測量得到位置、速度的差值作為量測信息，則SINS/GNSS組合導航濾波器量測方程為：

(13)

式中：H(t) 為量測系數矩陣，Zp(t)為位置量測向量，Vp(t)為位置量測噪聲向量，近似為白噪聲；Zv(t)為速度量測向量，Vv(t)為速度量測噪聲向量，近似為白噪聲。

對式(12)、(13)進行離散化處理，可得到式(1)形式的線性離散系統模型，通過平滑變結構卡爾曼濾波器即可進行濾波估計，實現機載SINS/GNSS組合導航。

3 基于平滑變結構卡爾曼濾波的組合導航算法仿真驗證分析

3.1 仿真條件

1)仿真參數設置見表1所示。

表1 仿真參數表

2)系統仿真時間600s,包含了爬升、平飛、加速、減速、轉彎的過程。

為驗證平滑變結構卡爾曼濾波算法的魯棒性，在100～200s設置測速干擾，假定受到干擾的GNSS速度量測噪聲服從以下非高斯分布[13]：

(14)

其中，σ1為理想情況下的GNSS量測噪聲標準差，仿真中設置為0.2；σ2為異常干擾噪聲標準差，通常取值較大，仿真中設置為σ2=50σ1；ε為擾動比例因子，仿真中設為0.3。

3.2 仿真分析

在上述仿真條件下，采用本文研究的平滑變結構卡爾曼濾波算法對機載組合導航系統進行導航解算，選擇僅采用卡爾曼濾波組合導航方法作為對比進行分析驗證。濾波過程中對慣導系統進行閉環校正，通過誤差校正后的導航參數與理想參數作差(即誤差值)對比分析。兩種濾波算法的組合導航位置誤差曲線、速度誤差曲線分別如圖3～4所示。

圖3 兩種濾波算法的位置誤差曲線

圖4 兩種濾波算法的速度誤差曲線

對兩種濾波方法的位置、速度誤差進行統計分析，兩種濾波方法的位置、速度誤差絕對值的均值以及誤差的標準差如表2所示。

表2 兩種濾波方法誤差絕對值的均值和誤差標準差

由圖3～4可以看出：在0～100s，GNSS速度量測噪聲尚未受到干擾時，本文研究的平滑變結構卡爾曼濾波器和常規卡爾曼濾波器的性能基本一致，均具有較好的精度；在100～200s，GNSS速度量測噪聲如式(14)所示，受到異常噪聲干擾，不再符合高斯分布，此時常規卡爾曼濾波精度下降，速度和位置誤差均受到較大影響。而平滑變結構卡爾曼濾波器采用變結構增益的思想，具有一定的魯棒性，速度精度優于常規卡爾曼濾波，位置精度有較大幅度的提升；在200s干擾結束后，常規卡爾曼濾波的估計誤差需要較長的收斂時間，而平滑變結構卡爾曼濾波的估計誤差很快收斂到正常情況下的精度。

從表2的統計結果也可以看出，在GNSS受擾、量測噪聲異常的情況下，本文研究的平滑變結構卡爾曼濾波算法在估計精度、魯棒性以及穩定性方面均優于常規卡爾曼濾波算法。

4 結論

為改善卡爾曼濾波算法性能、提高機載SINS/GNSS組合導航系統的魯棒性，研究了一種基于卡爾曼的平滑變結構濾波算法。在量測噪聲非高斯分布情況下，驗證該方案的可行性及組合導航對準性能。仿真驗證結果表明，在GNSS測速性能未受擾情況下，本文研究的平滑變結構卡爾曼濾波和常規卡爾曼濾波性能一致；在GNSS受到干擾、速度量測噪聲不符合高斯分布的情況下，平滑變結構卡爾曼濾波算法在估計精度、魯棒性以及穩定性方面均優于常規卡爾曼算法，且在干擾結束后能快速收斂到正常情況下的精度。平滑變結構卡爾曼濾波算法克服了常規卡爾曼濾波算法對系統模型和噪聲特性的限制，對工程應用具有重要的參考價值。