一種基于聯(lián)邦濾波的SINS/GNSS/RA 彈載多源組合導航算法

李杰，梁玉琴，李昃雯，秦 碩，程遵堃

（上海航天控制技術(shù)研究所，上海 201109）

0 引言

彈載導航系統(tǒng)提供的位置、速度和姿態(tài)信息，能夠指引導彈按照既定的彈道飛行，從而對目標實現(xiàn)精確打擊。彈載導航系統(tǒng)需要工作在全天候、高動態(tài)環(huán)境中，目前多采用慣性/衛(wèi)星（Ship’s Inertial Navigation System/Global Navigation Satellite System，SINS/GNSS）組合導航的方式［1］。SINS 不需要接收外部信息，是一種完全的自主式導航系統(tǒng)，但由于其工作原理的特殊性，長時間單獨運行導航信息會逐漸發(fā)散［2］，因此需要以其他導航信息，如GNSS 提供的位置和速度等進行修正。然而GNSS接收機易受干擾，常常因為接收不到衛(wèi)星信號而導致丟星，無法提供組合導航融合信息，使得導彈的打擊精度受到影響。

單一的SINS/GNSS 組合導航系統(tǒng)難以滿足導彈精確打擊的需求，因此輔助傳感器的研發(fā)和選用極為迫切。在眾多高程測量傳感器中，雷達高度表（Radar Altimeter，RA）具有體積小、功耗低、精度高且測高范圍大等特點［3］，常用于航空航天設(shè)備中。因此選取雷達高度表為彈載導航提供信息源參與SINS/GNSS 信息融合，能夠極大地提升彈載導航系統(tǒng)的魯棒性和可靠性。

本文基于發(fā)射慣性系，構(gòu)建了SINS/GNSS/RA彈載多源組合導航系統(tǒng)聯(lián)邦濾波融合框架，并推導了數(shù)學模型，通過彈道仿真試驗驗證了該算法的正確性，為彈載多源組合導航系統(tǒng)的研究提供了思路。

1 聯(lián)邦卡爾曼濾波融合框架

SINS/GNSS/RA 彈載多源組合導航系統(tǒng)選取SINS 作為公共參考系統(tǒng)，構(gòu)建SINS/GNSS 和SINS/RA 兩個子濾波器，聯(lián)邦濾波算法結(jié)構(gòu)如圖1所示。

圖1 聯(lián)邦濾波算法結(jié)構(gòu)Fig.1 Structure diagram of the federated filter algorithm

聯(lián)邦濾波的狀態(tài)方程和量測方程為［4］

式中：F(k)、G(k)、X(k)、w(k)分別為k時刻狀態(tài)轉(zhuǎn)移矩陣、系統(tǒng)噪聲驅(qū)動矩陣、狀態(tài)矢量和系統(tǒng)狀態(tài)噪聲矢量；X(k+1)為系統(tǒng)k時刻到k+1 時刻狀態(tài)估計值；Zi(k)、Hi(k)和vi(k)為系統(tǒng)k時刻子濾波器i的量測矢量、量測矩陣和量測噪聲矩陣。

聯(lián)邦濾波信息融合方法主要包含4 個環(huán)節(jié)［5-6］：

1）子濾波器信息的時間更新

式中：Pi(k+1|k)、Qi(k+1|k)分別為子濾波器i狀態(tài)一步預測誤差估計協(xié)方差矩陣和一步預測系統(tǒng)噪聲方差矩陣。

2）子濾波器信息的量測更新

式中：Ki(k)、Ri(k)分別為子濾波器i增益矩陣和量測噪聲方差矩陣。

3）主濾波器信息融合，假設(shè)子濾波器的數(shù)量用N表示，則信息融合得到狀態(tài)量全局最優(yōu)估計和全局最優(yōu)估計協(xié)方差陣：

4）子濾波器信息分配，子濾波器信息Pi(k)、的分配按照如下原則進行：

式（5）中，信息分配系數(shù)βi(βi＞0)滿足信息守恒原理：

通過以上4 個環(huán)節(jié)，可以實現(xiàn)多個導航信息源測量值對狀態(tài)的最優(yōu)估計，從而得到最優(yōu)融合后的導航輸出信息。

2 發(fā)射慣性系下SINS/GNSS/RA 組合導航系統(tǒng)數(shù)學模型

由于聯(lián)邦濾波將SINS 作為公共參考系統(tǒng)，因此選取發(fā)射慣性系下SINS 的基本導航參數(shù)誤差和慣性儀器參數(shù)誤差共15 維誤差信息作為狀態(tài)變量［7］，用X(k)表示為

式中：φx、φy、φz為發(fā)射慣性系下姿態(tài)失準角；δVx、δVy、δVz和δx、δy、δz分別為發(fā)射慣性系下x、y、z3個軸向速度和位置誤差；εx、εy、εz和?x、?y、?z分別為彈體坐標系下陀螺儀常值漂移和加速度計常值偏置。

通過姿態(tài)失準角、速度、位置和慣性器件誤差傳播方程構(gòu)建式（1）中的狀態(tài)方程，F(xiàn)(k)、G(k)和w(k)的具體參數(shù)見文獻［7］，此處不再贅述。

2.1 SINS/GNSS 子濾波器

SINS/GNSS 子濾波器量測方程由3個軸向速度和3個軸向位置2部分量測方程構(gòu)成［8-12］，具體如下：

1）速度量測方程：

式中：Vsx、Vsy、Vsz為由SINS 解算的發(fā)射慣性 系下x、y、z3 個軸向速度值；Vgx、Vgy、Vgz為由GNSS 輸出的速度轉(zhuǎn)換到發(fā)射慣性系下投影在x、y、z3 個軸向上的分量；vv為GNSS 接收機速度測量誤差矢量；Hv為速度量測矩陣。

2）位置量測方程：

式中：Xs、Ys、Zs為由SINS解算的發(fā)射慣性系下x、y、z3 個軸向位置值；Xg、Yg、Zg為由GNSS 輸出的位置轉(zhuǎn)換到發(fā)射慣性系下投影在x、y、z3 個軸向上的分量；vp為GNSS 接收機位置測量誤差矢量；Hp為位置量測矩陣，

由式（8）和式（10），可得到SINS/GNSS 子濾波器的量測方程：

式中：ZGNSS、HGNSS和vGNSS分別為SINS/GNSS 子濾波器系統(tǒng)的量測矢量、量測矩陣和量測誤差矩陣。

2.2 SINS/RA 子濾波器

裝載有RA 的導彈在飛行過程中，以一定的頻率向地球表面發(fā)射雷達脈沖［13-25］，導彈上的雷達信號接收裝置接收到回波后，通過時間間隔Δt可以得到RA 的測量值H′［2］，具 體為

式中：c為電磁波傳遞速度。

為了獲得導彈實際所在位置的海拔高度，需要對測量值H′進行補償，具體如下：

式中：Δh為補償值，由當?shù)睾０蔚纫蛩卮_定。

發(fā)射慣性系下導彈位置如圖2 所示。圖2 中，Re0、Re分別為發(fā)射點和當前導彈所在位置的地球半徑。

圖2 發(fā)射慣性系下導彈位置Fig.2 Schematic diagram of the missile position under the launch inertial system

在發(fā)射慣性系即Ol-XlYlZl系下，位置矢量OlP與發(fā)射點水平面夾角用θo表示，滿足式（15）：

式中：x、y、z為導彈所處位置在發(fā)射慣性系下的坐標。

將坐標轉(zhuǎn)換為海拔高度值為

Hs與Hr作差，可得

式中：vh為誤差項，受導航誤差和RA 測量精度影響。其中，

綜上，構(gòu)建SINS/RA 子濾波器的量測方程為

其中，量測矩陣HRA為

3 仿真校驗

3.1 仿真條件

1）設(shè)計導彈為垂直發(fā)射，發(fā)射點初始位置：緯度32.0°，經(jīng)度118.0°，高度0 m；發(fā)射方位角為90°；主動段為70 s，全程飛行時間為357 s。

2）GNSS 采樣頻率為1 Hz，RA 的采樣頻率為10 Hz，SINS 采樣頻率為200 Hz；聯(lián)邦濾波融合周期為1 s。

3）陀螺儀常值漂移為0.5（°）/h，白噪聲為0.1（°）/h；加速度計常值偏置為1 mg，白噪聲為0.5 mg。GNSS 位置誤差為10 m，速度誤差為0.1 m/s。RA的測量誤差為30 m。

3.2 仿真結(jié)果及分析

通過上述條件生成彈道軌跡，如圖3 所示。

圖3 發(fā)射慣性系下彈道軌跡Fig.3 Ballistic trajectory in the launch inertial coordinate system

生成發(fā)射慣性系下SINS、GNSS 和RA 仿真數(shù)據(jù)后，利用聯(lián)邦濾波算法進行信息融合，得到發(fā)射慣性系下的位置誤差曲線，如圖4 所示。

圖4 發(fā)射慣性系下X、Y和Z 3 個方向位置誤差曲線Fig.4 Position error curves of the X-，Y-，and Zdirections in the launch inertial coordinate system

續(xù)圖4 發(fā)射慣性系下X、Y和Z 3 個方向位置誤差曲線Continue Fig.4 Position error curves of the X-，Y-，and Zdirections in the launch inertial coordinate system

由圖4 可知，本文所設(shè)計的SINS/GNSS/RA 多源組合導航系統(tǒng)使用聯(lián)邦濾波進行信息融合后，位置誤差能夠在短時間收斂，且波動幅度較小，能夠得到穩(wěn)定的導航位置信息。計算x、y、z3 個方向位置的均方根誤差分別為1.48、1.34 和1.03 m，由此可知，導航系統(tǒng)輸出的導航定位結(jié)果具有較高的精度。

本文基于聯(lián)邦濾波的彈載多源組合導航算法，旨在提高彈載導航系統(tǒng)的穩(wěn)定性和可靠性，在GNSS 失效的情況下，依然可以為導彈提供較為準確的導航定位信息。因此，設(shè)計仿真試驗，生成一組仿真數(shù)據(jù)，在導彈飛行250 s 時，對GNSS 施加干擾，使其處于丟星狀態(tài)，分別使用本文設(shè)計的SINS/GNSS/RA 多源組合導航系統(tǒng)與傳統(tǒng)的SINS/GNSS 組合導航系統(tǒng)進行信息融合，得到位置誤差對比曲線如圖5 所示。

由圖5 可知，在導彈飛行250 s 即GNSS 未受干擾之前，SINS/GNSS/RA 多源組合導航系統(tǒng)和SINS/GNSS 組合導航系統(tǒng)兩者誤差相近，都有較高導航定位精度；在導彈飛行250 s 之后，SINS/GNSS 組合導航系統(tǒng)x、y、z3 個方向位置誤差均快速發(fā)散，而SINS/GNSS/RA 多源組合導航系統(tǒng)誤差雖然也緩慢發(fā)散，但在短時間內(nèi)，依然可以為導彈提供較可靠的導航定位信息。

圖5 發(fā)射慣性系下位置誤差對比曲線Fig.5 Comparative position error curves in the launch inertial coordinate system

在GNSS 受到干擾后，SINS/GNSS 子濾波器失效，聯(lián)邦濾波主濾波器只融合了SINS/RA 的處理結(jié)果。分析SINS/RA 子濾波器的量測方程可知，SINS/RA 子濾波器能夠?qū)ξ恢谜`差進行估計，使得聯(lián)邦濾波信息融合后可以在一定程度上對導彈的位置進行修正，從而保證了導彈的打擊精度。

4 結(jié)束語

本文分析了導彈傳統(tǒng)SINS/GNSS 組合導航系統(tǒng)的缺陷，提出了基于聯(lián)邦濾波的SINS/GNSS/RA 多源組合導航算法。通過彈道仿真試驗，驗證了導航算法的正確性。在GNSS 受干擾的情況下，將本文設(shè)計的彈載SINS/GNSS/RA 多源組合導航系統(tǒng)與傳統(tǒng)SINS/GNSS 組合導航系統(tǒng)作對比，結(jié)果表明，本文設(shè)計的導航系統(tǒng)具有更高的魯棒性和可靠性，并依然能夠在一定時間范圍內(nèi)保證導彈的打擊精度。