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

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

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

0 引言

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

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

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

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

SINS/GNSS/RA 彈載多源組合導(dǎo)航系統(tǒng)選取SINS 作為公共參考系統(tǒng)，構(gòu)建SINS/GNSS 和SINS/RA 兩個(gè)子濾波器，聯(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)方程和量測(cè)方程為［4］

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

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

1）子濾波器信息的時(shí)間更新

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

2）子濾波器信息的量測(cè)更新

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

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

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

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

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

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

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

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

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

2.1 SINS/GNSS 子濾波器

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

1）速度量測(cè)方程：

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

2）位置量測(cè)方程：

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

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

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

2.2 SINS/RA 子濾波器

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

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

為了獲得導(dǎo)彈實(shí)際所在位置的海拔高度，需要對(duì)測(cè)量值H′進(jìn)行補(bǔ)償，具體如下：

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

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

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

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

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

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

Hs與Hr作差，可得

式中：vh為誤差項(xiàng)，受導(dǎo)航誤差和RA 測(cè)量精度影響。其中，

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

其中，量測(cè)矩陣HRA為

3 仿真校驗(yàn)

3.1 仿真條件

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)束語(yǔ)

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