基于多項(xiàng)式擬合的擴(kuò)展卡爾曼濾波算法

吳漢洲 宋衛(wèi)東 徐敬青

摘要：彈道修正彈內(nèi)的彈載計(jì)算機(jī)必須實(shí)時(shí)對(duì)衛(wèi)星定位接收機(jī)獲取的彈丸狀態(tài)數(shù)據(jù)進(jìn)行濾波降噪，用于預(yù)測(cè)彈丸落點(diǎn)，傳統(tǒng)濾波方法濾波時(shí)間長(zhǎng)，濾波實(shí)時(shí)性差，提出一種基于多項(xiàng)式擬合的方法。通過適當(dāng)降低衛(wèi)星定位接收機(jī)數(shù)據(jù)更新頻率，并用多項(xiàng)式擬合插值出的數(shù)據(jù)代替數(shù)據(jù)更新時(shí)間間隔內(nèi)的彈丸狀態(tài)數(shù)據(jù)。仿真實(shí)驗(yàn)表明，該算法在不降低濾波效果的前提下，較普通擴(kuò)展卡爾曼濾波時(shí)間降低7/8，提高了濾波實(shí)時(shí)性，對(duì)于彈道修正彈關(guān)鍵技術(shù)的研究提供了重要參考。同時(shí)該方法可推廣應(yīng)用到其他濾波算法當(dāng)中，具有很強(qiáng)的可移植性。

關(guān)鍵詞：濾波算法；多項(xiàng)式擬合；彈道修正彈；衛(wèi)星定位數(shù)據(jù)；濾波誤差

中圖分類號(hào)：TP301 文獻(xiàn)標(biāo)志碼：A

Abstract： The data acquired by the satellite positioning receiver in the trajectory correction projectile must be filtered in realtime to predict the point. The calculation of traditional filtering method is timeconsuming， and is difficult to meet the requirements of realtime filtering. A kind of extended Kalman filtering algorithm based on polynomial fitting was proposed. The data of projectile flight in the time interval was replaced by the fitting interpolation data. In this way the filter frequency could be reduced. Simulation results show that the computation time of the proposed method can be reduced by 7/8 compared to traditional extended Kalman filtering without reducing the filtering precision， and the realtime performance is improved. This method can provide important reference for the research of key technology of trajectory correction projectile. At the same time， the method can be applied to other filtering algorithms， and has a strong portability.

Key words：filtering algorithm； polynomial fitting； trajectory correction projectile； satellite positioning data； filtering error

0 引言

隨著科技的發(fā)展，代表科技進(jìn)步顯著成果之一的電子元器件的發(fā)展經(jīng)歷了從無到有、從簡(jiǎn)單到復(fù)雜的過程，并不斷向集成化、小型化、低成本等多方向發(fā)展。隨著軍事發(fā)展，對(duì)電子元器件額外提出的高抗過載能力問題也得到很好的解決[1]，這大大促進(jìn)了彈道修正彈的發(fā)展。彈道修正彈內(nèi)部裝有衛(wèi)星定位系統(tǒng)，能夠?qū)崟r(shí)獲取彈丸位置、速度等信息，利用該信息計(jì)算預(yù)測(cè)的彈丸落點(diǎn)，通過比較預(yù)測(cè)落點(diǎn)與目標(biāo)點(diǎn)距離形成修正指令，修正彈丸飛行彈道，從而使彈丸飛向目標(biāo)。其中衛(wèi)星數(shù)據(jù)準(zhǔn)確度直接影響到預(yù)測(cè)落點(diǎn)的準(zhǔn)確度，由于衛(wèi)星定位系統(tǒng)自身系統(tǒng)誤差，其定位數(shù)據(jù)不能滿足直接用來預(yù)測(cè)彈丸落點(diǎn)的要求，需要對(duì)定位數(shù)據(jù)進(jìn)行降噪濾波，為滿足濾波實(shí)時(shí)性、準(zhǔn)確性要求，基于狀態(tài)空間法的卡爾曼濾波理論[2-3]被廣泛應(yīng)用。其中應(yīng)用于非線性系統(tǒng)的主要為擴(kuò)展卡爾曼濾波（Extended Kalman Filtering， EKF）。

EKF算法主要通過對(duì)非線性系統(tǒng)進(jìn)行泰勒展開，忽略高階項(xiàng)來實(shí)現(xiàn)非線性系統(tǒng)的線性化。但這種方法處理強(qiáng)非線性系統(tǒng)時(shí)，會(huì)因?qū)ο到y(tǒng)描述不準(zhǔn)確而濾波效果不好。對(duì)此有關(guān)研究者提出了二階EKF算法[4-6]，即保留泰勒展開的一、二階項(xiàng)，忽略更高階項(xiàng)，但該方法造成了較大的運(yùn)算量，影響了運(yùn)算速度。也有研究者提出通過反復(fù)迭代狀態(tài)估計(jì)的方法[7-9]優(yōu)化濾波效果，但該方法同樣存在運(yùn)算量增大的問題。

本文通過適當(dāng)降低濾波頻率，減少濾波次數(shù)的方法來整體提升濾波速率，并基于多項(xiàng)式擬合的方法在濾波點(diǎn)之間插值，補(bǔ)償定位數(shù)據(jù)更新慢的不足，仿真實(shí)驗(yàn)表明該方法能夠在不擴(kuò)大濾波誤差的情況下，明顯提高濾波效率。

1 彈道模型的選擇

彈道修正彈內(nèi)的彈載計(jì)算機(jī)進(jìn)行濾波時(shí)，必須以彈道模型為濾波基礎(chǔ)，彈道模型越精確濾波算法濾波效果越好，但是彈道模型越精確就意味著彈道模型越復(fù)雜，本文以某型100mm一維彈道修正榴彈的彈道為仿真對(duì)象，鑒于彈道濾波實(shí)時(shí)性、快速性的特點(diǎn)，本文以彈道模型較簡(jiǎn)單的二維質(zhì)點(diǎn)彈道[10]建立其彈道模型：

2.2 基于多項(xiàng)式擬合的擴(kuò)展卡爾曼濾波

在工程應(yīng)用中，彈道修正彈內(nèi)卡爾曼濾波算法是根據(jù)全球定位系統(tǒng)（Global Positioning System， GPS）接收數(shù)據(jù)的頻率進(jìn)行循環(huán)濾波的，這樣造成了兩個(gè)問題：1） 如果GPS的數(shù)據(jù)更新頻率太快，則為了保證濾波實(shí)時(shí)性，要求濾波時(shí)間盡可能地短，這對(duì)于簡(jiǎn)單系統(tǒng)或者簡(jiǎn)單濾波算法來說一般還能滿足，對(duì)于復(fù)雜系統(tǒng)、復(fù)雜濾波算法，則會(huì)由于每次濾波時(shí)間太長(zhǎng)，失去濾波實(shí)時(shí)性；2）如果降低濾波頻率，給濾波計(jì)算留出時(shí)間，則彈丸在進(jìn)行落點(diǎn)預(yù)測(cè)時(shí)（落點(diǎn)預(yù)測(cè)頻率與GPS數(shù)據(jù)更新頻率相等）會(huì)因每次預(yù)測(cè)落點(diǎn)相隔時(shí)間太長(zhǎng)，失去最佳修正時(shí)機(jī)，造成較大的落點(diǎn)偏差，大大影響射彈精度。

本文提出在保證一定濾波精度基礎(chǔ)上適當(dāng)降低GPS數(shù)據(jù)更新頻率，從而降低濾波頻率，對(duì)于GPS數(shù)據(jù)更新之間的時(shí)間間隔里通過最小二乘擬合的方式擬合插值出新的更新數(shù)據(jù)[10]。以射程x為例加以說明，為簡(jiǎn)化插值算法在短時(shí)間內(nèi)可認(rèn)為彈丸速度及位置數(shù)據(jù)變化符合某一元二次拋物線某一段曲線，因此進(jìn)行數(shù)據(jù)擬合時(shí)最少可僅用三個(gè)點(diǎn)，設(shè)已濾波降噪得到三個(gè)點(diǎn)（x1，x2，x3），利用這三個(gè)點(diǎn)求出式（19）中拋物線未知系數(shù)a、b、c的值，其中時(shí)間t就取彈載計(jì)算機(jī)內(nèi)時(shí)鐘每個(gè)點(diǎn)出現(xiàn)時(shí)刻：

利用該表達(dá)式，可求出上一次濾波時(shí)間與下一濾波時(shí)間之間更小時(shí)間間隔內(nèi)的x值，彌補(bǔ)了GPS數(shù)據(jù)更新頻率低的不足。

設(shè)每一次對(duì)GPS數(shù)據(jù)的濾波處理耗時(shí)Tm，設(shè)tk表示該時(shí)刻要對(duì)GPS數(shù)據(jù)進(jìn)行第k次濾波，圖1為一次完整的基于多項(xiàng)式擬合的擴(kuò)展卡爾曼濾波流程。由于每次擬合插值計(jì)算相對(duì)于濾波計(jì)算耗時(shí)很少，故忽略該部分計(jì)算耗時(shí)。

3 仿真實(shí)驗(yàn)

3.1 仿真背景設(shè)置

彈載計(jì)算機(jī)測(cè)得的彈丸位置及速度信息是基于WGS84協(xié)議下的地心地球固連坐標(biāo)系（ECEF）內(nèi)，在調(diào)用該數(shù)據(jù)時(shí)需要將該坐標(biāo)下的數(shù)據(jù)通過坐標(biāo)變化矩陣轉(zhuǎn)換到彈載計(jì)算機(jī)上的地面坐標(biāo)系內(nèi)[11]，本文假設(shè)已對(duì)GPS測(cè)量原始數(shù)據(jù)進(jìn)行了坐標(biāo)變化[12]。具體實(shí)現(xiàn)過程為，通過建立該彈丸6D剛體彈道模型，解算出一條彈道，獲得的彈道數(shù)據(jù)包括彈丸不同時(shí)刻位置、速度信息，以該彈道數(shù)據(jù)為原始數(shù)據(jù)，加上由仿真軟件產(chǎn)生的高斯白噪聲誤差即為模擬GPS定位數(shù)據(jù)，誤差范圍設(shè)置如表1所示。本文以一條射角為35°仿真彈道為基礎(chǔ)， 濾波算法對(duì)整條彈道都進(jìn)行濾波降噪，取彈載計(jì)算機(jī)GPS數(shù)據(jù)更新頻率為1Hz，對(duì)GPS更新時(shí)間間隔插值9次，相當(dāng)于GPS數(shù)據(jù)更新頻率變成了10Hz（彈載計(jì)算機(jī)GPS數(shù)據(jù)更新常用頻率）。

從圖2、圖4可看出，雖然基于插值擬合的EKF算法濾波頻率只有1Hz，但仍能保持較好的濾波效果，與濾波頻率為10Hz的普通EKF算法兩者濾波效果相當(dāng)；圖3、圖5中GPS定位誤差曲線與以10Hz頻率濾波誤差曲線重合，說明普通EKF算法對(duì)誤差較小的vx、vy濾波效果不明顯，而基于插值擬合的EKF算法濾波誤差出現(xiàn)較大的波動(dòng)，波動(dòng)幅值與GPS定位誤差幅值相當(dāng)，說明其對(duì)vx、vy濾波效果也不明顯；表2為不同算法濾波后數(shù)據(jù)方差表，從表中可看出，以10Hz濾波頻率的普通EKF算法與1Hz濾波頻率的基于插值擬合的EKF算法對(duì)x、y都具有較好的濾波效果，兩者對(duì)vx、vy濾波效果都不明顯。表3列出了基于Matlab軟件分別建立EKF模型和基于多項(xiàng)式擬合的EKF模型，并在不同仿真條件下對(duì)同一條模擬GPS彈道數(shù)據(jù)進(jìn)行濾波所用時(shí)間，從表中可看出，1Hz濾波頻率的基于插值擬合的EKF算法較濾波頻率較1Hz濾波頻率的普通EKF算法濾波時(shí)間增加14.8%，但僅是濾波頻率為10Hz普通EKF濾波時(shí)間的1/8，整體濾波速率提升明顯。綜上所述，基于插值擬合的EKF算法在不影響濾波效果情況下，濾波時(shí)間上大大縮短。

4 結(jié)語

在彈道修正彈研發(fā)過程中，彈載計(jì)算機(jī)必須實(shí)時(shí)對(duì)衛(wèi)星接收到的彈丸位置、速度等信息進(jìn)行濾波降噪，傳統(tǒng)濾波方法濾波時(shí)間較長(zhǎng)，跟不上衛(wèi)星定位接收機(jī)數(shù)據(jù)更新頻率，大大影響了濾波的實(shí)時(shí)性，本文提出適當(dāng)降低衛(wèi)星定位接收機(jī)數(shù)據(jù)更新頻率，利用多項(xiàng)式插值擬合的方式，插值擬合出衛(wèi)星定位接收機(jī)數(shù)據(jù)更新時(shí)間間隔內(nèi)的彈丸位置、速度信息。仿真實(shí)驗(yàn)表明，該方法在不降低濾波效果的前提下，能夠明顯降低濾波時(shí)間，提高彈丸位置、速度信息更新的實(shí)時(shí)性，對(duì)于彈道修正彈關(guān)鍵技術(shù)的研究提供了重要方法。同時(shí)該方法可移植性強(qiáng)，可推廣應(yīng)用到其他濾波算法當(dāng)中。

參考文獻(xiàn)：

[1]GRACE J. GPS guidance system increases projectile accuracy [J]. IEEE Aerospace and Electronic Systems Magazine， 2000， 15（6）： 15-17.

[2]KALMAN R E. A new approach to linear filtering and prediction theory [J]. Journal of Basic Engineering， 1960， 82（1）： 35-45.

[3] KALMAN R E， BUCY R S. New results in linear filtering and prediction theory [J]. Journal of Basic Engineering， 1961， 83（1）： 95-108.

[4]JAWINSKI A H. Filtering for nonlinear dynamical systems [J]. IEEE Transactions on Automatic Control， 1966， 11（4）： 765-766.

[5]范煒，李勇.近似二階擴(kuò)展卡爾曼濾波方法研究[J].空間控制技術(shù)與應(yīng)用，2009，35（1）：30-35. （FAN W， LI Y. A method of approximate secondorder extended Kalman filter [J]. Aerospace Control and Application， 2009， 35（1）： 30-35.）

[6]孟真，閻躍鵬，于進(jìn)勇.基于二階泰勒展開的擴(kuò)展卡爾曼濾波測(cè)頻算法[J].江蘇大學(xué)學(xué)報(bào)（自然科學(xué)版），2010， 31（5）：564-569. （MENG Z， YAN Y P， YU J Y. Extended Kalman filter for frequency measurement based on twoorder Taylor expansion [J]. Journal of Jiangsu University （Natural Science Edition）， 2010， 31（5）： 564-569.）

[7]BELL B M， CATHEY F W. The iterated Kalman filter update as a GaussNewton method[J]. IEEE Transactions on Automatic Control， 1993， 38（2）： 294-297.

[8]朱建峰.擬線性最優(yōu)平滑濾波在指令控制——維彈道修正彈上的應(yīng)用研究[D].南京：南京理工大學(xué)，2012：32-34. （ZHU J F. Application research of quasilinear optimum smoothing filtering to onedimensional ballistic in command control [D]. Nanjing： Nanjing University of Science and Technology， 2012： 32-34.）

[9]歐陽廣帥，周晶.基于卡爾曼濾波的高精度彈道濾波算法研究[J].電子測(cè)量技術(shù)，2014，37（11）：16-19，24. （OUYANG G S， ZHOU J. High accuracy ballistic filtering algorithm based on the Kalman filter [J]. Electronic Measurement Technology， 2014， 37（11）： 16-19， 24.）

[10]梁民贊，黃子豪，曹占啟.曲線擬合與卡爾曼濾波器的濾波精度評(píng)估[J].無線電工程，2013，43（3）：36-39. （LIANG M Z， HUANG Z H， CAO Z Q. Accuracy assessment of filtering based on curve fitting and Kalman filter [J]. Radio Engineering of China， 2013，43（3）： 36-39.）

[11]邱致和，王萬義.GPS原理與應(yīng)用[M].北京：電子工業(yè)出版社，2002：10-36. （QIU Z H， WANG W Y. GPS Principle and Application[M]. Beijing： Publishing House of Electronics Industry， 2002： 10-36.）

[12]申強(qiáng)，葛靦，彭博，等.基于GPS彈道測(cè)量的卡爾曼濾波參數(shù)估計(jì)算法[J].北京理工大學(xué)學(xué)報(bào)，2009，29（12）：1048-1051. （SHEN Q， GE M， PENG B， et al. Parameters estimation algorithm by Kalman filtering based on GPS measurement for projectile trajectory [J]. Transactions of Beijing Institute of Technology， 2009， 29（12）： 1048-1051.）