李璞, 楊濤, 牟宏偉

(1.國防科技大學(xué), 湖南 長沙 410073; 2.中國運(yùn)載火箭技術(shù)研究院, 北京 100076)

捷聯(lián)慣性導(dǎo)航系統(tǒng)(strapdown inertial navigation system,SINS)的核心是依托內(nèi)部慣性器件開展持續(xù)數(shù)據(jù)采集和累計(jì),通過積分方法進(jìn)行導(dǎo)航結(jié)果計(jì)算,且系統(tǒng)的初始值對(duì)于導(dǎo)航最終結(jié)果的影響非常大。在開始工作之前,SINS需要利用初始對(duì)準(zhǔn)來確定載體平臺(tái)的起始姿態(tài),且起始姿態(tài)的測(cè)量精度對(duì)于最終的定位精度具有重大影響。對(duì)于載體平臺(tái)是靜基座的情況,假定系統(tǒng)失準(zhǔn)角較小,系統(tǒng)誤差模型符合線性特性,SINS一般采取卡爾曼濾波器(Kalman filter,KF)來進(jìn)行初始對(duì)準(zhǔn)。然而,在某些惡劣或復(fù)雜工作場(chǎng)景下,基于粗對(duì)準(zhǔn)后的平臺(tái)姿態(tài)誤差存在不滿足小失準(zhǔn)角假設(shè)的可能,在這種情況下線性精對(duì)準(zhǔn)方法往往無法獲取較高精度的對(duì)準(zhǔn)結(jié)果,進(jìn)而直接影響最終的導(dǎo)航精度。面對(duì)上述情況,諸多學(xué)者對(duì)基于非線性濾波算法和誤差模型的方法進(jìn)行了研究。例如,劉海鵬介紹了擴(kuò)展卡爾曼濾波器(extended Kalman filter,EKF)和無跡卡爾曼濾波器(unscented Kalman filter,UKF)在初始對(duì)準(zhǔn)方位失準(zhǔn)角大角度問題中的解決方法,并設(shè)計(jì)了相應(yīng)的仿真評(píng)估測(cè)試。基于測(cè)試結(jié)果,可以發(fā)現(xiàn)UKF比EKF更適應(yīng)大失準(zhǔn)角度的情況;龍瑞則介紹了中心差分卡爾曼濾波器(central difference Kalman filter,CDKF)在初始對(duì)準(zhǔn)方位失準(zhǔn)角大角度問題中的解決方法,并基于仿真測(cè)試結(jié)果驗(yàn)證了該方法的高效性;Jamshaid針對(duì)航向大失準(zhǔn)角初始對(duì)準(zhǔn)問題,也提出了比EKF 和 UKF 精度更高的二階分離插值濾波器(second-order divided difference filter,DDF2)[1-4]。

然而,上述傳統(tǒng)的非線性濾波器卻具有一定的缺點(diǎn),如EKF由于復(fù)雜的導(dǎo)數(shù),很難解析雅可比矩陣,且對(duì)于高度非線性化的系統(tǒng),濾波效果并不好;UKF存在算法精度不高、穩(wěn)定性差、對(duì)動(dòng)態(tài)適應(yīng)性低等缺點(diǎn)[5-7]。針對(duì)上述方法缺點(diǎn),基于Spherical-Radial Cubature準(zhǔn)則的容積卡爾曼濾波(CKF)在近段時(shí)間被廣泛應(yīng)用,它可以適應(yīng)非線性誤差模型,相比于傳統(tǒng)非線性濾波算法有所改進(jìn)。

此外,如果衛(wèi)星信號(hào)不穩(wěn)定,觀測(cè)量也會(huì)存在粗差,進(jìn)而影響濾波算法的估計(jì)效果。同時(shí),多數(shù)情況下無法得到準(zhǔn)確的干擾特性,使得濾波估計(jì)性能受到了很大限制。針對(duì)上述問題,本文基于自適應(yīng)抗差理論設(shè)計(jì)了一種改進(jìn)CKF算法。具體而言,通過穩(wěn)健M估計(jì)和改進(jìn)Sage-Husa次優(yōu)無偏極大后驗(yàn)估值器，使改進(jìn)CKF算法具有更強(qiáng)的濾波穩(wěn)定性和更好的自適應(yīng)性。

1 SINS大方位失準(zhǔn)角誤差模型

(1)

(2)

式中

根據(jù)文獻(xiàn)[1],可得SINS姿態(tài)誤差方程為

(3)

速度誤差方程為

(4)

(5)

假設(shè)系統(tǒng)狀態(tài)量為

陀螺及加速度計(jì)的白噪聲為

速度和位置信息為

則系統(tǒng)狀態(tài)方程和量測(cè)方程可以表示為

(6)

式中:Gw是干擾的輸入矩陣,f(x(t),t)和Gw之間的關(guān)系如公式(5)所示;h(t)是量測(cè)陣,且有h(t)=[02×3I2×202×5];v(t)是量測(cè)干擾,w(t)是過程干擾,這2種都服從于v(t)～N(0,R)分布形式。

2 抗差Kalman濾波算法

在SINS/GPS初始對(duì)準(zhǔn)中,粗差將對(duì)最終估計(jì)結(jié)果產(chǎn)生影響,個(gè)別較大的粗差會(huì)使結(jié)果偏差惡化[2-3,8]。傳統(tǒng)Kalman濾波不能對(duì)粗差進(jìn)行有效處理[9-11]。為了解決上述難題,本文提出了一種基于抗差Kalman的SINS濾波算法。穩(wěn)健M估計(jì)在抗差估計(jì)理論中十分實(shí)用,所以本文的算法便是以這種估計(jì)方法為基石,同時(shí)為了轉(zhuǎn)化為最小二乘形式,算法還使用了等加權(quán)原理?？共頚alman濾波的關(guān)鍵是權(quán)因子的選擇,而觀測(cè)干擾協(xié)方差陣的方法經(jīng)常會(huì)使粗差的影響權(quán)重變大。因此,本文采用權(quán)函數(shù)來取代上述方法從而減小或者消除這種影響。

wi=

(7)

3 自適應(yīng)抗差CKF濾波算法

3.1 基于自適應(yīng)Kalman濾波算法設(shè)計(jì)

不準(zhǔn)確的干擾特性通常會(huì)使濾波不穩(wěn)定,甚至發(fā)散,因此諸多學(xué)者對(duì)自適應(yīng)濾波技術(shù)進(jìn)行了研究[11-13]。在諸多算法中,從計(jì)算復(fù)雜度、算法復(fù)雜度等多角度進(jìn)行對(duì)比,Sage-Husa次優(yōu)無偏極大后驗(yàn)(MAP)噪聲估值器具有更低的計(jì)算復(fù)雜度和算法復(fù)雜度,因此被廣泛研究[14]。然而,Sage-Husa噪聲估值器不能同時(shí)估計(jì)系統(tǒng)和量測(cè)受到的干擾,否則會(huì)導(dǎo)致濾波發(fā)散[15]。在實(shí)際環(huán)境中,量測(cè)受到的干擾可由傳感器的物理特性得到,但由于測(cè)量儀器精度、外界干擾等因素影響,系統(tǒng)受到的干擾則很難準(zhǔn)確獲得。為了解決上述問題,同時(shí)提高系統(tǒng)的自適應(yīng)能力,本文以協(xié)方差匹配為基礎(chǔ)設(shè)計(jì)了自適應(yīng)衰減因子。

1) 系統(tǒng)干擾估計(jì)算法

本文利用改進(jìn)的Sage-Husa對(duì)不易計(jì)算的系統(tǒng)所受干擾qk和Qk進(jìn)行估計(jì)

式中,dk-1=(1-b)(1-bk),0.95

2) 濾波發(fā)散抑制算法

(10)

(11)

(12)

(13)

式中:0<ρ≤1是衰減系數(shù),一般取值為0.95左右;ρ的值越大,當(dāng)前殘差向量的影響越大。

3.2 基于自適應(yīng)抗差的改進(jìn)設(shè)計(jì)

不準(zhǔn)確的系統(tǒng)干擾會(huì)給濾波收斂帶來影響,為解決該問題,本文利用自適應(yīng)抗差理論對(duì)容積卡爾曼濾波(CKF)進(jìn)行了改進(jìn)優(yōu)化。

首先,對(duì)于非線性系統(tǒng)來說,設(shè)定其狀態(tài)方程和量測(cè)方程為

(14)

式中,wk-1和vk為互不相關(guān)均值為零的高斯白噪聲,且方差陣分別為Q和R。

本文提出的基于自適應(yīng)抗差CKF算法的改進(jìn)初始對(duì)準(zhǔn)方法如下:

step1 容積點(diǎn)和權(quán)值計(jì)算

(15)

step2 時(shí)間更新

step3 量測(cè)更新

(26)

(27)

step4 發(fā)散判斷

(28)式是發(fā)散的判斷依據(jù),如果根據(jù)公式判斷發(fā)散就按照(29)～(32)式修正Pk/k-1,如果不發(fā)散就繼續(xù)下一步。

4 驗(yàn)證與分析

4.1 仿真測(cè)試與驗(yàn)證

針對(duì)本文提出的基于自適應(yīng)抗差CKF算法的改進(jìn)初始對(duì)準(zhǔn)方法,設(shè)計(jì)對(duì)應(yīng)的仿真測(cè)試方法,并對(duì)其濾波性能進(jìn)行測(cè)試。

首先,對(duì)仿真參數(shù)進(jìn)行設(shè)定,如表1所示。

表1 仿真參數(shù)表

根據(jù)上述參數(shù),系統(tǒng)初始方差矩陣P(0)、系統(tǒng)所受干擾矩陣Q和量測(cè)所受干擾矩陣R分別為：

P(0)=diag{(1°)2,(1°)2,(10°)2,(0.1 m/s)2,

(0.1 m/s)2,(0.02°/h)2,(0.02°/h)2,(0.02°/h)2,

(1×10-4g)2,(1×10-4g)2}

Q=diag{(0.01°/h)2,(0.01°/h)2,(0.01°/h)2,

(5×10-5g)2,(5×10-5g)2,0,0,0,0,0};

R=diag{(10 m)2,(10 m)2,(0.1 m/s)2,

(0.1 m/s)2}

1) 在某段時(shí)間內(nèi),速度觀測(cè)量增加3 m/s粗差,且在對(duì)濾波采樣時(shí)進(jìn)行頻率增加,設(shè)置仿真時(shí)間為300 s,分別比較2種方法。仿真結(jié)果見圖1～3。

根據(jù)圖1和圖2,CKF算法收斂時(shí)間依次為241.355,18.675 s,誤差精度絕對(duì)值依次為0.026°,0.008°;本文提出的抗差CKF算法收斂時(shí)間依次為0.001,0.001s,誤差精度絕對(duì)值依次為0.003°,0.002°,則本文方法在收斂時(shí)間上具有明顯優(yōu)勢(shì),在誤差精度方面也相比未加入抗差的算法精度高。根據(jù)圖3,在方位失準(zhǔn)角估計(jì)方面,CKF算法收斂時(shí)間為228.425 s、誤差精度為0.471°;抗差CKF收斂時(shí)間為45.355 s、誤差精度為0.017°,表明抗差CKF具有更高的估計(jì)精度和更快的收斂速度,并且濾波更穩(wěn)定。

圖1 東向失準(zhǔn)角估計(jì)誤差曲線 圖2 北向失準(zhǔn)角估計(jì)誤差曲線 圖3 方位失準(zhǔn)角估計(jì)誤差曲線

2) 基于第1)組仿真條件,假定Q′=10Q,分別比較加入抗差和未加入抗差的2種方法,仿真時(shí)間300 s。仿真結(jié)果見圖4～6。

圖4 東向失準(zhǔn)角估計(jì)誤差曲線 圖5 北向失準(zhǔn)角估計(jì)誤差曲線 圖6 方位失準(zhǔn)角估計(jì)誤差曲線

根據(jù)圖4至6,CKF算法收斂時(shí)間依次為0.006,250.148,140.635 s,誤差精度依次為0.005°,0.031°,0.114°;本文提出的抗差CKF算法收斂時(shí)間依次為0.001,0.001,38.112 s,誤差精度依次為0.002°,0.002°,0.065°,則對(duì)于東向失準(zhǔn)角,抗差CKF和自適應(yīng)抗差CKF估計(jì)精度相差不大;對(duì)于北向失準(zhǔn)角和方位失準(zhǔn)角,加入抗差之后的算法的估計(jì)精度明顯低于同時(shí)加入自適應(yīng)和抗差的算法。

針對(duì)2次測(cè)試結(jié)果,最終收斂的誤差精度抗差CKF算法更高,收斂速度更快,能夠更好滿足當(dāng)前對(duì)失準(zhǔn)角估計(jì)的實(shí)際使用需求。

4.2 實(shí)物測(cè)試與驗(yàn)證

在實(shí)物測(cè)試中,選取某次SINS實(shí)測(cè)數(shù)據(jù),為了驗(yàn)證本文提出的算法的實(shí)際可行性,對(duì)數(shù)據(jù)進(jìn)行離線分析,得到了圖7的試驗(yàn)結(jié)果。

從圖7可以得知,在此次SINS試驗(yàn)中,自適應(yīng)抗差CKF算法收斂于-3.82′,CKF算法收斂于-5.11′,則自適應(yīng)抗差CKF算法性能更好。因此,本文提出的算法可以有效降低不準(zhǔn)確的系統(tǒng)干擾特性以及異常量測(cè)信息引起的測(cè)量誤差,改善了系統(tǒng)的初始對(duì)準(zhǔn)精度。

圖7 方位失準(zhǔn)角估計(jì)誤差曲線

5 結(jié) 論

本文提出了一種基于自適應(yīng)抗差CKF算法的改進(jìn)初始對(duì)準(zhǔn)方法,該方法用于解決初始對(duì)準(zhǔn)中失準(zhǔn)角過大情況下觀測(cè)粗差和不確定系統(tǒng)干擾造成的誤差影響。通過仿真實(shí)驗(yàn)和實(shí)物測(cè)試驗(yàn)證了本文方法的有效性及其在初始對(duì)準(zhǔn)中具有更強(qiáng)的濾波穩(wěn)定性、更高的濾波估計(jì)精度和更短的算法收斂時(shí)間,進(jìn)而顯著提升了初始對(duì)準(zhǔn)效率。