牟宏偉,韓 磊,李昂陽

（1．中國運載火箭技術研究院，北京 100076；2．首都航天機械有限公司，北京 100076）

1 引言

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

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

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

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

2 SINS大方位失準角誤差模型

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

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

2.1 誤差方程

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

速度誤差方程為：

2.2 濾波模型

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

陀螺及加速度計的白噪聲為：

其中，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濾波算法

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

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

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

4 自適應Kalman濾波算法

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

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

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

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

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

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

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

5 自適應抗差CKF濾波算法研究

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

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

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

在CKF濾波的基礎上，結合上述自適應Kalman濾波和抗差Kalman濾波，可推導自適應抗差CKF算法如下：

(1) 計算容積點和權值

(2) 時間更新

(3) 量測更新

假設原觀測值權為，則等價權。且有權因子：

發(fā)散判斷：

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

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

6 仿真研究

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

系統(tǒng)狀態(tài)初始估計值X(0)=0；初始失準角φx=φy=1°，φz=10°；初始速度誤差為0．1m/s；陀螺儀常值漂移為0．02°/h，隨機噪聲為0．01°/h；加速度計零偏為1×10-4g，隨機噪聲為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) 在某段時間內(nèi)，速度觀測量增加3m/s粗差，且增大濾波采樣頻率，分別比較CKF和抗差CKF兩種非線性濾波方法，仿真時間300s。仿真結果見圖1～圖3。

圖1 東向失準角估計誤差曲線

圖3 方位失準角估計誤差曲線

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

圖2 北向失準角估計誤差曲線

表1 基于CKF和抗差CKF的仿真結果

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

圖4 東向失準角估計誤差曲線

圖5 北向失準角估計誤差曲線

圖6 方位失準角估計誤差曲線

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

表2 抗差CKF和自適應抗差CKF的仿真結果

7 試驗驗證

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

圖7 試驗設備安裝圖

表3 基于CKF和自適應抗差CKF的對準結果

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

表3 基于CKF和自適應抗差CKF的對準結果

8 結束語

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