基于卡爾曼濾波的GPS彈道測(cè)量誤差消除方法

楊小會(huì)，霍鵬飛，王 超

（機(jī)電動(dòng)態(tài)控制重點(diǎn)實(shí)驗(yàn)室，陜西 西安 710065）

0 引言

全球定位系統(tǒng)（Global Positioning System，簡(jiǎn)稱GPS）具有全天候、高精度、快速實(shí)時(shí)定位的優(yōu)點(diǎn)，在國(guó)外被廣泛應(yīng)用于射程修正引信中［1］。隨著我國(guó)北斗二代衛(wèi)星導(dǎo)航定位系統(tǒng)的建成，這種基于衛(wèi)星定位的彈道修正引信的實(shí)現(xiàn)將成為可能。

目前我國(guó)只能利用GPS中的粗測(cè)距碼（C／A碼）進(jìn)行定位，GPS測(cè)量值存在較大隨機(jī)誤差。傳統(tǒng)上利用慣性導(dǎo)航系統(tǒng)（Inertial Navigation System，簡(jiǎn)稱INS）與GPS優(yōu)勢(shì)互補(bǔ)的特點(diǎn)，將兩者組合成GPS／INS組合導(dǎo)航系統(tǒng)［2］，以提高整個(gè)系統(tǒng)的導(dǎo)航精度及性能；或者采用最小二乘估計(jì)［3］，降低GPS測(cè)量隨機(jī)誤差。

由于彈道修正引信工作環(huán)境惡劣，引信空間有限以及要求成本低等原因，在彈道修正引信中使用GPS／INS組合導(dǎo)航系統(tǒng)不太可能，而最小二乘濾波僅考慮了量測(cè)值，沒(méi)有考慮系統(tǒng)動(dòng)態(tài)模型，在系統(tǒng)表現(xiàn)為強(qiáng)非線性時(shí)，估計(jì)精度很差，甚至濾波發(fā)散，完全不可用［4］。為此提出了一種基于卡爾曼濾波的GPS測(cè)量誤差消除方法。

1 卡爾曼濾波

在非線性領(lǐng)域，卡爾曼濾波將非線性狀態(tài)方程和測(cè)量方程在狀態(tài)估計(jì)時(shí)刻進(jìn)行泰勒展開，忽略二階及二階以上的高階項(xiàng)，將得到的一階近似項(xiàng)作為原狀態(tài)方程和測(cè)量方程的近似表述形式。濾波公式［3］如下。

初始化方程：

時(shí)間更新方程：

量測(cè)更新方程：

2 卡爾曼濾波誤差消除方法

卡爾曼濾波誤差消除方法首先利用彈道動(dòng)態(tài)模型對(duì)彈丸狀態(tài)進(jìn)行預(yù)估，再利用GPS量測(cè)值對(duì)狀態(tài)預(yù)估值進(jìn)行校正，最終估計(jì)出誤差最小的運(yùn)動(dòng)狀態(tài)。圖1為卡爾曼濾波誤差消除原理圖。

圖1 卡爾曼濾波誤差消除原理Fig.1 Principles of eliminating errors based on kalman filter

圖1中，卡爾曼濾波算法見(jiàn)式（1）—式（9），狀態(tài)方程見(jiàn)2.1節(jié)中彈道方程，量測(cè)方程見(jiàn)2.2節(jié)中量測(cè)模型。

由圖1和卡爾曼濾波式（1）—式（9）可以看出，卡爾曼濾波有兩個(gè)明顯的信息更新過(guò)程：時(shí)間更新過(guò)程和量測(cè)更新過(guò)程。時(shí)間更新過(guò)程完成狀態(tài)預(yù)估，量測(cè)更新過(guò)程用來(lái)計(jì)算對(duì)時(shí)間更新值的修正量。

時(shí)間更新過(guò)程由式（3）、式（4）完成，式（3）完成了利用彈道動(dòng)態(tài)模型對(duì)彈丸狀態(tài)進(jìn)行預(yù)估，式（4）定量地描述了這種預(yù)估的質(zhì)量?jī)?yōu)劣。

量測(cè)更新過(guò)程由濾波算法其余公式完成，量測(cè)更新過(guò)程用來(lái)計(jì)算對(duì)時(shí)間更新值的修正量，該修正量由濾波增益 （Kk）、量測(cè)與狀態(tài)的關(guān)系（Hk）以及具體的量測(cè)值（Zk）所確定。濾波增益（Kk）是使估計(jì)的均方誤差陣達(dá)到最小的最佳增益陣，它通過(guò)時(shí)間更新的質(zhì)量?jī)?yōu)劣（Pk｜k－1）與量測(cè)信息的質(zhì)量?jī)?yōu)劣（Rk）自動(dòng)確定對(duì)量測(cè)值和狀態(tài)預(yù)估值的利用比例，以達(dá)到估計(jì)誤差最小的運(yùn)動(dòng)狀態(tài)的目的。

2.1 狀態(tài)模型

考慮到射程修正引信彈載控制器DSP的解算實(shí)時(shí)性及彈道辨識(shí)精度，采用質(zhì)點(diǎn)彈道模型作為狀態(tài)模型［5］。狀態(tài)方程如下：

式中，θ表示彈道傾角，v表示彈丸速度，x表示地面坐標(biāo)系中的射程，y表示地面坐標(biāo)系中的射向，c表示彈道系數(shù)，g表示重力加速度。

氣壓函數(shù)：

式中，R1為空氣常數(shù)，p0n表示標(biāo)準(zhǔn)氣象條件的地面大氣壓強(qiáng)。

空氣阻力函數(shù)：

cx0n（Ma）采用43年阻力定律，Ma表示馬赫數(shù)。Ma表示聲速，k＝1.404，τ表示虛溫函數(shù)，vr表示彈丸相對(duì)于空氣的運(yùn)動(dòng)速度。vr＝表示標(biāo)準(zhǔn)氣象條件的地面虛溫，wx表示彈道縱風(fēng)，可測(cè)量得到。

2.2 GPS量測(cè)模型

GPS測(cè)量采用的坐標(biāo)系是以地球質(zhì)心為原點(diǎn)的大地坐標(biāo)系［6］，即經(jīng)度λ、緯度φ和大地高h(yuǎn)，則量測(cè)模型如下：

式中，x，y，z分別為地面坐標(biāo)系中的射程、射向和橫偏。vE為東向速度，vN為北向速度，vU為天向速度；α為射向，以真北為零，順時(shí)針為正；λ0、φ0和h0為發(fā)射點(diǎn)位置。

在WGS－84坐標(biāo)系中，地球橢圓長(zhǎng)軸半徑為Re＝6 378 137m，地球橢圓短軸半徑為Rp＝6 356 752m。

記量測(cè)方程為z＝ h （x）＋R，假設(shè)量測(cè)噪聲為相互獨(dú)立的不相關(guān)零均值白高斯噪聲R～ （0 ，q）。

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

以某型底凹榴彈為試驗(yàn)平臺(tái)，采用彈載GPS進(jìn)行彈道測(cè)量，跟蹤雷達(dá)全彈道跟蹤彈道，遙測(cè)裝置實(shí)時(shí)保存GPS測(cè)量值。采用上述基于擴(kuò)展卡爾曼濾波的GPS測(cè)量值誤差消除方法對(duì)其中一發(fā)遙測(cè)到的GPS測(cè)量值進(jìn)行濾波估計(jì)。

3.1 直接彈道測(cè)量精度評(píng)估法

以跟蹤雷達(dá)外測(cè)值為基準(zhǔn)，分別計(jì)算GPS濾波估計(jì)值和GPS測(cè)量值的誤差，并在全彈道（彈丸出炮口后第7～48s）對(duì)彈丸射程和射高誤差值進(jìn)行了統(tǒng)計(jì)，結(jié)果見(jiàn)表1。

表1 GPS測(cè)量誤差統(tǒng)計(jì)值Tab.1 Statistic of GPS measurement errors

由表1可以看出：經(jīng)過(guò)擴(kuò)展卡爾曼濾波誤差消除后的GPS測(cè)量精度比未經(jīng)誤差消除的GPS測(cè)量精度可以提高近一倍。

3.2 間接彈道測(cè)量精度評(píng)估法

同一發(fā)彈丸的GPS實(shí)測(cè)數(shù)據(jù)，對(duì)經(jīng)擴(kuò)展卡爾曼濾波消除誤差后的GPS測(cè)量值和未經(jīng)誤差消除的GPS測(cè)量值分別采用相同的算法進(jìn)行落點(diǎn)預(yù)測(cè)，并將各自預(yù)測(cè)到的落點(diǎn)與彈丸實(shí)測(cè)落點(diǎn)進(jìn)行對(duì)比，以評(píng)估預(yù)測(cè)精度，進(jìn)而評(píng)價(jià)擴(kuò)展卡爾曼濾波誤差消除方法的有效性。

對(duì)誤差消除后的GPS測(cè)量值及未經(jīng)誤差消除的GPS測(cè)量值，分別選取彈丸出炮口后第30～37s時(shí)段的數(shù)據(jù)進(jìn)行二次曲線擬合。采用不同時(shí)刻的擬合值作為彈道初始諸元解算質(zhì)點(diǎn)彈道，預(yù)測(cè)落點(diǎn)，采用彈丸實(shí)測(cè)落點(diǎn)評(píng)估預(yù)測(cè)精度。預(yù)測(cè)誤差見(jiàn)表2。

表2 預(yù)測(cè)誤差Tab.2 Errors of prediction

由表2可以看出經(jīng)擴(kuò)展卡爾曼濾波誤差消除后的預(yù)測(cè)誤差相對(duì)于未經(jīng)誤差消除的預(yù)測(cè)誤差可以提高50%以上的射程預(yù)測(cè)精度。

4 結(jié)論

本文提出一種GPS測(cè)量誤差消除方法，該方法采用擴(kuò)展卡爾曼濾波結(jié)合外彈道模型對(duì)GPS測(cè)量值進(jìn)行濾波估計(jì)，以達(dá)到消除GPS測(cè)量誤差的目的。實(shí)測(cè)數(shù)據(jù)分析結(jié)果表明：結(jié)合彈道模型，采用卡爾曼濾波進(jìn)行最優(yōu)估計(jì)可有效消除GPS測(cè)量誤差；經(jīng)擴(kuò)展卡爾曼濾波誤差消除后的預(yù)測(cè)誤差相對(duì)于未經(jīng)誤差消除的預(yù)測(cè)誤差可以提高50%以上的射程預(yù)測(cè)精度。

［1］洪元軍，雷亞茹.國(guó)外彈道修正引信發(fā)展研究［J］.探測(cè)與控制學(xué)報(bào)，2001，23（4）：1－5.HONG Yuanjun，LEI Yaru.The current development of foreign trajectory correction fuze［J］.Journal of Detection and Control，2001，23（4）：1－5.

［2］周坤芳，孔鍵.INS輔助GPS接收機(jī)及抗干擾能力分析［J］.航空電子技術(shù)，2009，40（2）：1－3.ZHOU Kunfang，KONG Jian.INS－assisted gps receiver and analysis of its influence on anti－jamming capability［J］.Avionics Technology，2009，40（2）：1－3.

［3］申強(qiáng)，葛靦，張冀興，等.一種GPS彈道辨識(shí)方法的精度仿真分析［J］.北京理工大學(xué)學(xué)報(bào)，2009，29（2）：100－102.SHEN Qiang，GE Mian，ZHANG Jixing，et al.Analysis on the precision of a gps－based trajectory identification method by simulation［J］.Transactions of Beijing Institute of Technology，2009，29（2）：100－102.

［4］秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理［M］.西安：西北工業(yè)大學(xué)出版社，1998.

［5］韓子鵬.彈箭外彈道學(xué)［M］.北京：北京理工大學(xué)出版社，2008.

［6］董緒榮，張守信，華仲春.GPS／INS組合導(dǎo)航定位及其應(yīng)用［M］.長(zhǎng)沙：國(guó)防科技大學(xué)出版社，1998.