自適應(yīng)抗差CKF在艦載導(dǎo)彈武器初始對準(zhǔn)中的應(yīng)用

牟宏偉,韓 磊,李昂陽

（1．中國運(yùn)載火箭技術(shù)研究院，北京 100076；2．首都航天機(jī)械有限公司，北京 100076）

1 引言

艦載導(dǎo)彈武器系統(tǒng)軍事上得到了廣泛應(yīng)用。初始對準(zhǔn)技術(shù)是導(dǎo)彈武器系統(tǒng)的關(guān)鍵技術(shù)之一，初始對準(zhǔn)時(shí)間會(huì)影響發(fā)射控制流程時(shí)間，制約武器系統(tǒng)的快速反應(yīng)能力，初始對準(zhǔn)精度會(huì)影響導(dǎo)航初值誤差，制約武器系統(tǒng)的精確打擊能力。由于艦載發(fā)射平臺面對的使用環(huán)境復(fù)雜且多變，常規(guī)的初始對準(zhǔn)技術(shù)具有很大的局限性，導(dǎo)彈武器系統(tǒng)很難完成快速、高精度的初始對準(zhǔn)。

捷聯(lián)慣性導(dǎo)航系統(tǒng)(Strapdown Inertial Navigation System，SINS)對導(dǎo)航結(jié)果的影響巨大[1-2]。初始對準(zhǔn)則為在導(dǎo)航開始前確定載體初始姿態(tài)的過程，在很大程度上影響著慣性導(dǎo)航精度。SINS靜基座初始對準(zhǔn)通常采用標(biāo)準(zhǔn)卡爾曼濾波器(Kalman Filter，KF)，濾波模型采用小失準(zhǔn)角假設(shè)下的線性誤差模型。但是，在某些特殊場合，短時(shí)粗對準(zhǔn)后，姿態(tài)誤差可能并不滿足小角度假設(shè)，此時(shí)基于線性模型假設(shè)的精對準(zhǔn)很難收斂到對準(zhǔn)極限精度或理想的對準(zhǔn)精度。

因此，大方位失準(zhǔn)角初始對準(zhǔn)是SINS 的關(guān)鍵技術(shù)[3-4]。針對非線性誤差模型的特點(diǎn)，目前基于Spherical-Radial Cubature 準(zhǔn)則的容積卡爾曼濾波(CKF)應(yīng)用較為廣泛。該算法避免了非線性模型的線性化處理，彌補(bǔ)了EKF、UKF等傳統(tǒng)非線性濾波算法的不足[4]。

但是，如果觀測量存在粗差，且無法得到準(zhǔn)確的噪聲特性，會(huì)使得濾波估計(jì)性能受到了很大限制。針對上述問題，本文提出了自適應(yīng)抗差CKF算法，能夠有效提高濾波的穩(wěn)定性和自適應(yīng)能力，提高了艦載導(dǎo)彈武器系統(tǒng)的初始對準(zhǔn)精度。

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

其中，sφi和cφi分別表示sin(φi)和cos(φi)(i=x，y，z)。

假設(shè)n'系相對于n系的角速度為，則歐拉平臺誤差角微分方程為：

2.1 誤差方程

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

速度誤差方程為：

2.2 濾波模型

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

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

其中，Gw為過程噪聲輸入陣，f(x(t)，t)和GW的描述見式(5)；h(t)為量測陣，且有h(t)=[02×3I2×202×5]；w(t)為過程噪聲，v(t)為量測噪聲，均服從高斯分布v(t)～N(0，R)。

3 抗差Kalman濾波算法

在初始對準(zhǔn)中，粗差將對估計(jì)結(jié)果產(chǎn)生影響[4-5]。傳統(tǒng)Kalman 濾波不能對粗差進(jìn)行有效處理[4-6]，針對上述問題，本文利用抗差Kalman 濾波算法，并結(jié)合抗差估計(jì)理論，其中，穩(wěn)健估計(jì)的研究更為成熟，較為實(shí)用的是穩(wěn)健M 估計(jì)。本文在穩(wěn)健M估計(jì)基礎(chǔ)上，通過等價(jià)權(quán)原理，轉(zhuǎn)化為最小二乘形式。如何選擇權(quán)因子是抗差Kalman濾波的關(guān)鍵，本文采用權(quán)函數(shù)代替觀測噪聲協(xié)方差陣來減小或消除粗差的影響。

根據(jù)上述方式，參考文獻(xiàn)[6]，假設(shè)原觀測值權(quán)為Pi，則其等價(jià)權(quán)，權(quán)因子如下：

其中，vi為觀測量Z的殘差為vi的均方差，，為vi的權(quán)倒數(shù)；方差因子σ0可據(jù)得到；根據(jù)文獻(xiàn)，一般選擇k0與k1可分別取1．5～2．5，3．0～5．0。

4 自適應(yīng)Kalman濾波算法

不準(zhǔn)確的噪聲特性通常會(huì)使濾波不穩(wěn)定，甚至發(fā)散，因此，自適應(yīng)濾波技術(shù)被廣泛研究[6-9]。其中，Sage-Husa次優(yōu)無偏極大后驗(yàn)(MAP)噪聲估值器因計(jì)算簡單、原理清晰等優(yōu)點(diǎn)被廣泛應(yīng)用[10-11]。但是Sage-Husa 噪聲估值器不能同時(shí)估計(jì)系統(tǒng)噪聲和量測噪聲，否則會(huì)導(dǎo)致濾波發(fā)散[11-13]。在實(shí)際環(huán)境中，量測噪聲可由傳感器的物理特性得到，但由于測量儀器精度、外界干擾等因素影響，系統(tǒng)噪聲很難準(zhǔn)確得到[14-16]。針對上述問題，結(jié)合協(xié)方差匹配判據(jù)，并引入自適應(yīng)衰減因子，抑制消除自適應(yīng)濾波的發(fā)散現(xiàn)象，提高了穩(wěn)定性和自適應(yīng)能力。

4.1 系統(tǒng)噪聲估計(jì)方法

本文利用Sage-Husa對不準(zhǔn)確的系統(tǒng)噪聲qk和Qk進(jìn)行估計(jì)，參考文獻(xiàn)[9]，qk和Qk的公式如下：

其中，dk-1=(1-b)(1-bk)，一般選取0．95＜b＜0．99 為遺忘因子，為經(jīng)驗(yàn)值，且b的取值越大，噪聲統(tǒng)計(jì)變化較快。

4.2 濾波發(fā)散的抑制

Sage-Husa 估值器通常會(huì)出現(xiàn)發(fā)散現(xiàn)象，因此，參考文獻(xiàn)[9]，本文同樣利用協(xié)方差匹配判據(jù)，由式判斷是否發(fā)散，如果發(fā)散則按照下式修正Pk/k-1，以抑制發(fā)散。其中，S≥1為可調(diào)系數(shù)為殘差序列，即。

其中，0＜ρ≤1 為衰減系數(shù)，通常取0．95 左右，ρ的取值越大，越突出當(dāng)前殘差向量的影響。

5 自適應(yīng)抗差CKF濾波算法研究

自適應(yīng)抗差CKF可以同時(shí)兼顧觀測粗差和系統(tǒng)噪聲引起的誤差，不僅能夠克服觀測粗差的影響，而且可以抑制系統(tǒng)噪聲未知或時(shí)變引起濾波發(fā)散的問題。

假設(shè)非線性系統(tǒng)的狀態(tài)方程和量測方程為：

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

在CKF濾波的基礎(chǔ)上，結(jié)合上述自適應(yīng)Kalman濾波和抗差Kalman濾波，可推導(dǎo)自適應(yīng)抗差CKF算法如下：

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

(2) 時(shí)間更新

(3) 量測更新

假設(shè)原觀測值權(quán)為，則等價(jià)權(quán)。且有權(quán)因子：

發(fā)散判斷：

根據(jù)式(28)判斷發(fā)散，如果發(fā)散則按式(29)～式(32)修正Pk/k-1，不發(fā)散則進(jìn)入下一步。

對于系統(tǒng)噪聲不準(zhǔn)確和觀測粗差，自適應(yīng)抗差CKF算法均有針對性的解決方法：由Sage-Husa自適應(yīng)濾波在線估計(jì)系統(tǒng)噪聲Qk，并抑制濾波發(fā)散現(xiàn)象；利用等價(jià)權(quán)系數(shù)調(diào)節(jié)觀測噪聲誤差的影響程度，最終得到所需的最優(yōu)和。

6 仿真研究

結(jié)合本文提出的自適應(yīng)抗差CKF和SINS誤差模型，濾波數(shù)值仿真條件如下。

系統(tǒng)狀態(tài)初始估計(jì)值X(0)=0；初始失準(zhǔn)角φx=φy=1°，φz=10°；初始速度誤差為0．1m/s；陀螺儀常值漂移為0．02°/h，隨機(jī)噪聲為0．01°/h；加速度計(jì)零偏為1×10-4g，隨機(jī)噪聲為5×10-5g；GPS速度測量誤差為0．1m/s；則初始方差陣P(0)、系統(tǒng)噪聲陣Q和量測噪聲陣R分別為：

P(0)=diag{(1°)2，(1°)2，(10°)2，(0．1m/s)2，(0．1m/s)2，

(0．02°/h)2，(0．02°/h)2，(0．02°/h)2，(100μg)2，(100μg)2}

Q=diag(0．01°/h)2，(0．01°/h)2，(0．01°/h)2，

(50μg)2，(50μg)2，0，0，0，0，0}

R=diag{(10m)2，(10m)2，(0．1m/s)2，(0．1m/s)2}

(1) 在某段時(shí)間內(nèi)，速度觀測量增加3m/s粗差，且增大濾波采樣頻率，分別比較CKF和抗差CKF兩種非線性濾波方法，仿真時(shí)間300s。仿真結(jié)果見圖1～圖3。

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

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

從圖1、圖2 和表1 可以看出，對水平失準(zhǔn)角，抗差CKF估計(jì)精度更高，而且誤差曲線相對平緩，收斂速度也較快；從圖3 和表1 可以看出，對方位失準(zhǔn)角，抗差CKF 估計(jì)精度和收斂速度優(yōu)于CKF，而且沒有較多的毛刺，濾波更穩(wěn)定。

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

表1 基于CKF和抗差CKF的仿真結(jié)果

(2) 在仿真條件(1)的基礎(chǔ)上，假設(shè)實(shí)際系統(tǒng)噪聲為Q'=10Q，分別比較抗差CKF和自適應(yīng)抗差CKF兩種濾波方法，仿真時(shí)間300s。仿真結(jié)果見圖4～圖6。

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

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

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

從圖4～圖6和表2可以看出，兩種濾波算法對東向失準(zhǔn)角的估計(jì)精度差不多，對北向失準(zhǔn)角和方位失準(zhǔn)角的估計(jì)，自適應(yīng)抗差CKF能夠保持較高的估計(jì)性能。

表2 抗差CKF和自適應(yīng)抗差CKF的仿真結(jié)果

7 試驗(yàn)驗(yàn)證

為了驗(yàn)證自適應(yīng)抗差CKF濾波算法實(shí)際初始對準(zhǔn)中的可行性，以某次SINS 實(shí)測數(shù)據(jù)為基礎(chǔ)，進(jìn)行離線分析，試驗(yàn)設(shè)備的安裝見圖7，基于自適應(yīng)抗差CKF 濾波算法和CKF濾波算法的方位角對準(zhǔn)結(jié)果見表3。

圖7 試驗(yàn)設(shè)備安裝圖

表3 基于CKF和自適應(yīng)抗差CKF的對準(zhǔn)結(jié)果

從表3中的試驗(yàn)結(jié)果可知，自適應(yīng)抗差CKF濾波算法的對準(zhǔn)精度更高，很好地解決了量測信息異?；蛳到y(tǒng)噪聲特性不準(zhǔn)確帶來的誤差，提高了初始對準(zhǔn)精度。

表3 基于CKF和自適應(yīng)抗差CKF的對準(zhǔn)結(jié)果

8 結(jié)束語

本文研究了自適應(yīng)抗差CKF 濾波算法，該算法可以同時(shí)兼顧觀測粗差和系統(tǒng)噪聲不準(zhǔn)確引起的誤差。通過數(shù)值仿真和實(shí)驗(yàn)驗(yàn)證，在觀測信息異?；蛳到y(tǒng)噪聲特性未知的情況下，自適應(yīng)抗差CKF 能夠有效實(shí)現(xiàn)大方位失準(zhǔn)角初始對準(zhǔn)，提高了失準(zhǔn)角的估計(jì)精度和收斂速度，增強(qiáng)了濾波算法的穩(wěn)定性和自適應(yīng)能力。