基于平滑變結(jié)構(gòu)卡爾曼濾波的機(jī)載組合導(dǎo)航算法*

高春雷 趙 賓,2

1.南京航空航天大學(xué)金城學(xué)院，南京 211156； 2.南京航空航天大學(xué)自動(dòng)化學(xué)院，南京 211106

0 引言

捷聯(lián)慣導(dǎo)系統(tǒng)(SINS)具有短期精度高、自主性高、抗干擾能力強(qiáng)等優(yōu)勢(shì)，但受其工作原理所限，誤差隨時(shí)間逐漸積累，長(zhǎng)期穩(wěn)定性較差[1]；全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)具有長(zhǎng)期精度高的優(yōu)點(diǎn)，然而在使用時(shí)易受到環(huán)境和人為干擾，定位、測(cè)速精度嚴(yán)重下降，無(wú)法保證持續(xù)導(dǎo)航定位的能力[2]。SINS和GNSS的信息具有互補(bǔ)性，將二者結(jié)合構(gòu)成組合導(dǎo)航系統(tǒng)可實(shí)現(xiàn)優(yōu)勢(shì)互補(bǔ)[3]，實(shí)現(xiàn)無(wú)人機(jī)的自主導(dǎo)航。

在SINS/GNSS組合導(dǎo)航中，常采用卡爾曼濾波器實(shí)現(xiàn)對(duì)導(dǎo)航誤差的最優(yōu)估計(jì)和修正。卡爾曼濾波器是一種最優(yōu)估計(jì)技術(shù)，但是其要求系統(tǒng)的數(shù)學(xué)模型精確，且系統(tǒng)噪聲和量測(cè)噪聲均服從高斯分布，否則濾波的精度將下降甚至發(fā)散[4]。

為克服卡爾曼濾波器對(duì)噪聲的嚴(yán)格高斯假設(shè)，國(guó)內(nèi)外學(xué)者提出了高斯和濾波器，利用高斯和對(duì)非高斯概率分布進(jìn)行建模[5]，但是此方法增加了濾波器的計(jì)算復(fù)雜度。

近年來(lái)，滑模控制的思想被廣泛應(yīng)用到控制領(lǐng)域。文獻(xiàn)[6]提出了基于解耦的變結(jié)構(gòu)自適應(yīng)滑模控制方法，實(shí)現(xiàn)了火箭全向發(fā)射下的姿態(tài)穩(wěn)定；文獻(xiàn)[7]提出一種新型的平滑估計(jì)器，解決了微小衛(wèi)星主動(dòng)繞飛時(shí)經(jīng)典卡爾曼濾波算法存在的估計(jì)值抖動(dòng)和反轉(zhuǎn)問(wèn)題。

采用滑模控制的策略，通過(guò)系統(tǒng)先驗(yàn)知識(shí)設(shè)定狀態(tài)估計(jì)的邊界，可用來(lái)提高對(duì)模型失配和非高斯噪聲魯棒性。平滑變結(jié)構(gòu)(Smooth Variable Structure Filter, SVSF)算法就是基于此思路，采用滑模控制概念，通過(guò)變結(jié)構(gòu)的增益，使得預(yù)測(cè)狀態(tài)逐步接近真實(shí)狀態(tài)[8]，對(duì)于系統(tǒng)模型失配以及噪聲不符合高斯分布的情況具有較好的魯棒性和穩(wěn)定性。

本文以機(jī)載捷聯(lián)慣導(dǎo)系統(tǒng)為應(yīng)用對(duì)象，將滑模控制的思想引入到SINS/GNSS組合導(dǎo)航中，研究基于卡爾曼的平滑變結(jié)構(gòu)濾波器，克服常規(guī)卡爾曼濾波算法對(duì)系統(tǒng)模型和噪聲特性的限制，提高組合導(dǎo)航的魯棒性。

1 平滑變結(jié)構(gòu)卡爾曼濾波算法

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

圖1 SVSF估計(jì)的基本思想

本文采用基于協(xié)方差的線性離散系統(tǒng)迭代方程，研究基于平滑變結(jié)構(gòu)卡爾曼的組合導(dǎo)航濾波算法。

設(shè)線性離散系統(tǒng)具有如下的狀態(tài)方程以及量測(cè)方程：

(1)

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

平滑變結(jié)構(gòu)卡爾曼濾波迭代方程如下：

1)通過(guò)系統(tǒng)模型計(jì)算一步預(yù)測(cè)狀態(tài)估計(jì)

(2)

2)計(jì)算一步預(yù)測(cè)狀態(tài)估計(jì)協(xié)方差矩陣

(3)

式中：Qk-1為系統(tǒng)噪聲協(xié)方差陣。

(4)

4)根據(jù)實(shí)際量測(cè)值和預(yù)測(cè)量測(cè)值，計(jì)算先驗(yàn)量測(cè)誤差變量ez,k/k-1

(5)

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

(6)

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

(7)

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

(8)

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

通過(guò)式(7)飽和運(yùn)算，在有界寬度外，可保證估計(jì)的魯棒性和穩(wěn)定性，在有界寬度內(nèi)，可通過(guò)插值獲得平滑函數(shù)。

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

6)由一步預(yù)測(cè)狀態(tài)估計(jì)和SVSF增益計(jì)算狀態(tài)估計(jì)

(9)

7)計(jì)算狀態(tài)估計(jì)協(xié)方差矩陣

(10)

8)最后，更新后驗(yàn)量測(cè)誤差變量ez,k/k

(11)

2 基于平滑變結(jié)構(gòu)卡爾曼濾波的組合導(dǎo)航方案設(shè)計(jì)及系統(tǒng)模型

2.1 組合導(dǎo)航方案設(shè)計(jì)

以機(jī)載捷聯(lián)慣導(dǎo)系統(tǒng)為組合導(dǎo)航研究對(duì)象。在組合導(dǎo)航過(guò)程中，利用捷聯(lián)慣導(dǎo)系統(tǒng)解算的位置、速度與GNSS測(cè)得的位置、速度之差作為量測(cè)信息，用本文提出的平滑變結(jié)構(gòu)卡爾曼濾波器對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)的各項(xiàng)誤差信息進(jìn)行估計(jì)，濾波過(guò)程中對(duì)慣導(dǎo)系統(tǒng)實(shí)時(shí)進(jìn)行閉環(huán)校正。基于平滑變結(jié)構(gòu)卡爾曼濾波的SINS/GNSS組合導(dǎo)航方案如圖2所示。

圖2 基于平滑變結(jié)構(gòu)卡爾曼濾波的SINS/GNSS組合導(dǎo)航方案框圖

2.2 組合導(dǎo)航系統(tǒng)模型

1)組合導(dǎo)航系統(tǒng)狀態(tài)方程

通過(guò)對(duì)SINS系統(tǒng)性能和誤差源的分析，即可得到組合導(dǎo)航系統(tǒng)狀態(tài)方程：

(12)

式中：X(t)為系統(tǒng)狀態(tài)向量；Φ(t)為狀態(tài)轉(zhuǎn)移矩陣；Γ(t)為系統(tǒng)噪聲系數(shù)矩陣；W(t)為系統(tǒng)噪聲向量。

2) 組合導(dǎo)航系統(tǒng)量測(cè)方程

以捷聯(lián)慣導(dǎo)系統(tǒng)導(dǎo)航解算得到位置、速度和衛(wèi)星導(dǎo)航系統(tǒng)測(cè)量得到位置、速度的差值作為量測(cè)信息，則SINS/GNSS組合導(dǎo)航濾波器量測(cè)方程為：

(13)

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

對(duì)式(12)、(13)進(jìn)行離散化處理，可得到式(1)形式的線性離散系統(tǒng)模型，通過(guò)平滑變結(jié)構(gòu)卡爾曼濾波器即可進(jìn)行濾波估計(jì)，實(shí)現(xiàn)機(jī)載SINS/GNSS組合導(dǎo)航。

3 基于平滑變結(jié)構(gòu)卡爾曼濾波的組合導(dǎo)航算法仿真驗(yàn)證分析

3.1 仿真條件

1)仿真參數(shù)設(shè)置見表1所示。

表1 仿真參數(shù)表

2)系統(tǒng)仿真時(shí)間600s,包含了爬升、平飛、加速、減速、轉(zhuǎn)彎的過(guò)程。

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

(14)

其中，σ1為理想情況下的GNSS量測(cè)噪聲標(biāo)準(zhǔn)差，仿真中設(shè)置為0.2；σ2為異常干擾噪聲標(biāo)準(zhǔn)差，通常取值較大，仿真中設(shè)置為σ2=50σ1；ε為擾動(dòng)比例因子，仿真中設(shè)為0.3。

3.2 仿真分析

在上述仿真條件下，采用本文研究的平滑變結(jié)構(gòu)卡爾曼濾波算法對(duì)機(jī)載組合導(dǎo)航系統(tǒng)進(jìn)行導(dǎo)航解算，選擇僅采用卡爾曼濾波組合導(dǎo)航方法作為對(duì)比進(jìn)行分析驗(yàn)證。濾波過(guò)程中對(duì)慣導(dǎo)系統(tǒng)進(jìn)行閉環(huán)校正，通過(guò)誤差校正后的導(dǎo)航參數(shù)與理想?yún)?shù)作差(即誤差值)對(duì)比分析。兩種濾波算法的組合導(dǎo)航位置誤差曲線、速度誤差曲線分別如圖3～4所示。

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

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

對(duì)兩種濾波方法的位置、速度誤差進(jìn)行統(tǒng)計(jì)分析，兩種濾波方法的位置、速度誤差絕對(duì)值的均值以及誤差的標(biāo)準(zhǔn)差如表2所示。

表2 兩種濾波方法誤差絕對(duì)值的均值和誤差標(biāo)準(zhǔn)差

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

從表2的統(tǒng)計(jì)結(jié)果也可以看出，在GNSS受擾、量測(cè)噪聲異常的情況下，本文研究的平滑變結(jié)構(gòu)卡爾曼濾波算法在估計(jì)精度、魯棒性以及穩(wěn)定性方面均優(yōu)于常規(guī)卡爾曼濾波算法。

4 結(jié)論

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