范 旭，陳國光,王光志，趙軍強，楊智杰

(1 中北大學(xué) 機電工程學(xué)院， 太原 030051； 2.豫西工業(yè)集團有限公司， 河南 南陽 473000)

【彈道工程和火藥工程】

卡爾曼彈道濾波器抗野值方法研究

范 旭1，陳國光1,王光志2，趙軍強2，楊智杰1

(1 中北大學(xué) 機電工程學(xué)院， 太原 030051； 2.豫西工業(yè)集團有限公司， 河南 南陽 473000)

在彈道諸元參數(shù)最優(yōu)估計獲取過程中,為減少量測過程中可能會形成的野值對卡爾曼濾波過程所造成的不利影響，提出了一種基于多項式擬合的野值去除方法。該方法通過對濾波前期彈道數(shù)據(jù)的選取，進行擬合估計，獲得三方向位置擬合多項式。在此多項式的基礎(chǔ)上，以殘差作為判別依據(jù)，以三倍標準差作為判別標準，將病態(tài)點去除。仿真結(jié)果表明，該方法行之有效，可將野值去除，保證濾波快速收斂。

彈道濾波；卡爾曼濾波；野值去除；多項式擬合

對于彈道測量，其測量數(shù)據(jù)中包含著干擾或噪聲引起的誤差，直接用測量數(shù)據(jù)進行分析計算將會帶來很大的誤差，甚至是錯誤，而必須從量測數(shù)據(jù)中濾取所需要的信息，這種數(shù)據(jù)處理方法稱為濾波。目前工程技術(shù)中使用較多的是最小二乘濾波、最大似然估計和卡爾曼濾波，其中卡爾曼濾波在控制領(lǐng)域，在火箭、炮彈、導(dǎo)彈彈道和飛行狀態(tài)以及衛(wèi)星軌道觀測的數(shù)據(jù)處理上得到廣泛的應(yīng)用?？柭畛跆岢龅臑V波基本理論只適用于線性系統(tǒng)，并且要求量測也必須是線性的。如果模型是非線性的，通常在推導(dǎo)濾波方程時，增加線性化步驟。在狀態(tài)估計時，對系統(tǒng)方程在前一狀態(tài)值處做實時的線性泰勒近似；在預(yù)測步驟中，對量測方程在相應(yīng)的預(yù)測位置也進行線性泰勒近似；所得到的卡爾曼濾波被稱為擴展卡爾曼濾波。處理非線性模型的這一思想是很自然的，且濾波過程簡單有效[1]。

而量測系統(tǒng)的噪聲是真實存在且不可忽視的。在常規(guī)的濾波過程中，我們視系統(tǒng)噪聲為正態(tài)分布的高斯噪聲，雖然正態(tài)分布的規(guī)律使大部分的隨機數(shù)位于三倍標準差之內(nèi)。但在實際情況中，量測信號要經(jīng)過傳輸系統(tǒng)、數(shù)據(jù)采集轉(zhuǎn)換系統(tǒng)等環(huán)節(jié)，才能轉(zhuǎn)化為數(shù)字形式的目標信號，這樣不但會帶有大量的隨機噪聲而且有可能受到外界無規(guī)律的干擾產(chǎn)生一些與真實數(shù)據(jù)相差較大的野值，以至于影響濾波過程。為避免這一現(xiàn)象，對濾波過程的抗野值方法的研究勢在必行[2]。

1 外彈道濾波模型

彈道濾波即是根據(jù)在一段彈道上測得的彈箭飛行彈道數(shù)據(jù)(坐標，速度、加速度、姿態(tài)角和姿態(tài)變化率)，利用數(shù)學(xué)方法從這些數(shù)據(jù)中提取當(dāng)前飛行狀態(tài)及彈道數(shù)學(xué)模型中的參數(shù)的最優(yōu)估計。而彈道預(yù)測則是利用這種最優(yōu)估計和彈道數(shù)學(xué)模型計算彈道，求取在所需點的彈道參數(shù)。彈道濾波是彈道預(yù)測的基礎(chǔ)，而彈道預(yù)測是彈道修正的重要依據(jù)，所以濾波的收斂速度及準確度也是彈道修正的重要研究方向。

1.1 濾波狀態(tài)模型

地面炮位偵查雷達對空中飛行的彈箭進行跟蹤和探測，地面計算機對雷達實測到的彈道參數(shù)進行數(shù)據(jù)處理，獲得較準確的彈道參數(shù)，與目標坐標比較，得到彈道偏差，并由雷達向彈載計算機傳輸指令，使修正控制機構(gòu)實現(xiàn)一維或二維彈道修正。根據(jù)上述彈道修正彈的執(zhí)行過程，在雷達量測數(shù)據(jù)的濾波的過程中，需要用到彈道模型，考慮彈道參數(shù)獲取的快速性、實時性等，本文采用質(zhì)點彈道模型作為彈道濾波的狀態(tài)方程[3]：

(1)

則有系統(tǒng)狀態(tài)方程：

(2)

式中W(k)是均值為零的高斯白噪聲，且服從方差為Q的正態(tài)分布，W～N(0,Q)。

1.2 濾波量測方程

設(shè)雷達測量值為斜距r、方位角β和高低角ε，雷達坐標系為球坐標系，可得雷達測量值與地面坐標系的關(guān)系[4]：

β=arctan(z/x)

(3)

令量測變量為Z，即Z=(r,β,ε)T，則得量測方程為:

(4)

式中，V是雷達的量測噪聲，假定為零均值高斯白噪聲，且服從方差為R的正態(tài)分布，即V～N(0,R)，h(x)為三維矢量函數(shù)。

2 卡爾曼彈道濾波方程

卡爾曼濾波方程組分為兩大部分，第一部分為卡爾曼濾波方程，這一部分負責(zé)向下一時刻推算軌跡狀態(tài)；第二部分為卡爾曼濾波器的增益矩陣遞推公式，這一部分用于反饋先驗估計，并對預(yù)測進行修正[4]。

設(shè)k時刻的被估計狀態(tài)Xk受系統(tǒng)噪聲序列w驅(qū)動，驅(qū)動機理由下述狀態(tài)方程描述

Xk=Φk,k-1Xk-1+Γk-1Wk-1

(5)

量測方程：

Zk=HkXk+Vk

(6)

式中:Φk,k-1為tk-1時刻的一步轉(zhuǎn)移矩陣；Γk為系統(tǒng)噪聲驅(qū)動陣；Hk為量測陣；Vk為量測噪聲序列；Wk為系統(tǒng)激勵噪聲序列。

如果被估計狀態(tài)Xk滿足式(5)，對Xk的量測Zk滿足式(6)，系統(tǒng)噪聲Wk和量測噪聲Vk滿足式(6)，系統(tǒng)噪聲Wk方差陣Qk非負定，量測噪聲Vk方差陣Rk正定，k時刻的量測為Zk，則Xk的估計Xk按下述方程求解：

狀態(tài)的一步預(yù)測

(7)

量測預(yù)測方程：

(8)

(9)

濾波增益：

(10)

一步預(yù)測均方誤差矩陣：

(11)

估計均方誤差

Pk=(I-KkHk)Pk/k-1

(12)

3 野值的判別與處理

3.1 三方向量測位置分量的方差計算

設(shè)彈道時間為t，將量測數(shù)據(jù)rt,βt,εt由下式轉(zhuǎn)化為x,y,z三方向數(shù)據(jù)即xt,yt,zt。

xt=rt·cosβt·cosεt

yt=rt·sinεt

zt=rt·cosεt·sinβt

(13)

(14)

同理可求得δy，δz。

3.2 彈道軌跡的多項式擬合

在實際情況下，彈體的在空中的運動受到重力，空氣阻力及橫風(fēng)等外界因素影響，其運動軌跡為曲線[8]。在曲線擬合的過程中，可選擇線性回歸模型，指數(shù)類或冪指數(shù)類等，及其組合形式作為擬合算法的基礎(chǔ)，其中線性回歸模型即多項式擬合方法計算復(fù)雜度低，計算速度快。同時，分析真實的彈道曲線，可發(fā)現(xiàn)彈道曲線形狀與變化規(guī)律與拋物線類似，從實際情況與模型的相似性出發(fā)，可選擇多項式進行曲線擬合。從上述兩點分析，將采用線性回歸模型進行彈道擬合并分析。在曲線擬合的計算過程中，常用到最小二乘法，高斯消去法，三次樣條法和追趕法等方法，來對系數(shù)矩陣進行求解，進而得到多項式系數(shù)?；趶椀狼€的形狀和擬合彈道多項式計算的快速實時性，選取最小二乘法進行進行彈道多項式擬合。

設(shè)擬合多項式的為n次冪多項式，其表達形式為:

f(x)=A1·xn+A2·xn-1+…+An·x+An+1

(15)

取m次量測數(shù)據(jù)(xi,yi)(m>n)，根據(jù)最小二乘法估計準則可得目標函數(shù)：

(16)

因目標函數(shù)為實際問題，其極小值必然存在，由極小值存在條件：

(17)

將上述條件方程轉(zhuǎn)化為矩陣表達形式如下：

(18)

由數(shù)值分析方法可知，當(dāng)系數(shù)矩陣A非奇異時，則上式有唯一解向量x。x向量中的參數(shù)即A1、…、An+1，就是多項式各次冪的系數(shù)。在已知完整多項式及量測時間間隔T的情況下， 將xm=t+T代入，便可獲得預(yù)估位置分量xm+1,ym+1,zm+1。

3.3 野值的判別與處理

在綠波起始時刻t之前，分別取基準彈道數(shù)據(jù)和量測數(shù)據(jù)，數(shù)據(jù)長度為N。將量測數(shù)據(jù)段代入按上式(14)，求得三方向標準差δx，δy，δz。將量測數(shù)據(jù)與基準彈道數(shù)據(jù)比對，若三方向上任意方向上差值大于3δ，則將該數(shù)據(jù)點去除，即：

用剩余數(shù)據(jù)點進行多項式擬合，進而得到多項式全系數(shù)。

濾波開始后，利用基準多項式估計下一采樣時刻數(shù)據(jù)，以上述原則為判據(jù)。若量測數(shù)據(jù)在判據(jù)下非野值，則將其用于濾波，并替代數(shù)據(jù)段中最初時刻數(shù)據(jù)，重新擬合多項式用以下次估計。

若量測數(shù)據(jù)被判別為野值，則不將此點代入濾波過程，以多項式估計值作為新量測數(shù)據(jù)，

代替原始量測，進入濾波過程且多項式無需改變。

4 仿真校驗

為驗證野值去除方法的可行性和實用性，需進行計算機仿真計算。以某型火箭彈為例，通過6D彈道方程求解出彈道數(shù)據(jù)，將彈道數(shù)據(jù)轉(zhuǎn)化為極坐標的雷達量測數(shù)據(jù)。并通過隨機發(fā)生器產(chǎn)生均值為零的高斯白噪聲作為要引入的雷達噪聲，加入到原始量測數(shù)據(jù)，不定時間間隔加入較大誤差作為野值，產(chǎn)生受噪聲影響的雷達量測數(shù)據(jù)。

在仿真計算中，取雷達數(shù)據(jù)刷新間隔為0.1 s，徑向距離上的方差σr為10 m，方向角的噪聲方差σβ為0.001 5 rad，俯仰角的噪聲方差σε為0.001 5 rad，即量測噪聲矩陣R的主對角元素值。模擬在火箭彈發(fā)射3.5 s后雷達開始跟蹤目標測量，在量測數(shù)據(jù)在40 s，開始對彈道數(shù)據(jù)進行濾波處理，持續(xù)監(jiān)控20 s左右至60 s時停止濾波處理。

多項式的擬合精度受擬合數(shù)據(jù)點個數(shù)及擬合模型與實際軌跡契合度等方面影響。所以將從采用數(shù)據(jù)點的時間長度，多項式階數(shù)兩方面來分析其變化對多項式擬合精度的影響。

從圖1可看出所估位置的殘差隨著擬合多項式階數(shù)的增加先減少而后略有增加。雖然擬合數(shù)據(jù)時間長度不同，但一次多項式基本對應(yīng)殘差最大值，造成這種情況的原因在于直線模型與真實三方向軌跡情況存在很大差異，具有較大的模型誤差，擬合效果差。

圖1 多項式階數(shù)與擬合位置殘差的關(guān)系

由一次多項式至二次多項式時，擬合殘差迅速減小，證明擬合模型與真實軌跡狀況的匹配度較高。圖2表示了二次多項式擬合殘差與擬合數(shù)據(jù)長度的關(guān)系。

由二次多項式至五次多項式時，擬合殘差卻略有增長，原因在于擬合模型與真實軌跡匹配度逐漸減弱?；诰瓤紤]，將選取二階多項式作為擬合模型。

圖2 二次多項式擬合殘差與擬合數(shù)據(jù)長度的關(guān)系

從圖2可看出隨數(shù)據(jù)點數(shù)的增加，基于二次多項式的擬合殘差逐漸減小，而到使用濾波前五秒數(shù)據(jù)時殘差卻有所增長。其原因在于，當(dāng)數(shù)據(jù)時間長度在一秒和四點五秒之間時，隨著數(shù)據(jù)點的增多，真實彈道與拋物線的相似度逐漸提高，使模型誤差逐漸減小，殘差隨之下降。但真實彈道雖與拋物線相似，但其軌跡為非對稱性，并不是二次多項式完全契合的拋物線，所以當(dāng)數(shù)據(jù)時間長度再次增加時，這種差異性會逐漸凸顯，造成擬合殘差增大。所以擬合數(shù)據(jù)時間長度選取為4.5 s。

從圖3、圖4和圖5可看出存在野值時，若不去除，濾波過程波動較大，濾波收斂速度受到影響，大幅度了延長了濾波收斂時間?？梢娨爸档拇嬖趯V波過程的影響持續(xù)時間較長且波動較大。證明去除野值應(yīng)是彈道濾波過程必須考慮到的因素。

圖3 X方向是否去野值對濾波的影響

圖4 Y方向是否去野值對濾波的影響

圖5 Z方向是否去野值對濾波的影響

5 結(jié)論

本文針對雷達量測的火箭彈彈道數(shù)據(jù)，通過對濾波前期的量測數(shù)據(jù)進行坐標轉(zhuǎn)換，對所量測的三方向數(shù)據(jù)進行多項式擬合，進而估計濾波下一時刻的位置分量。以濾波前量測數(shù)據(jù)作為樣本，求取三方向位置分量方差，以此來作為量測系統(tǒng)的總體參數(shù)。選取擬合數(shù)據(jù)長度和擬合多項式階數(shù)作為變量，通過計算機仿真分析其對彈道參數(shù)擬合精度的影響，最后選取最優(yōu)組合確定其為彈道擬合模板。通過濾波仿真，驗證此種野值去除方法的可行性。由仿真結(jié)果可看出，文中所提出的去野值方法有效，簡單，計算速度快，符合實用性要求，可有效去除野值對濾波過程的影響，保證濾波過程平緩，無較大波動，使濾波快速收斂。

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

[2] 閆小龍,陳國光,楊東.抗野值卡爾曼濾波在火箭彈落點估計中的應(yīng)用[J].彈箭與制導(dǎo)學(xué)報,2016(3):94-98.

[3] 韓子鵬.彈箭外彈道學(xué)[M].北京：北京理工大學(xué)出版社，2014:74-77.

[4] 李巖,任睿,王旭剛.兩種卡爾曼濾波模型在修正彈彈道數(shù)據(jù)處理中的應(yīng)用比較[J].彈道學(xué)報,2011(1):27-30.

[5] 吳漢洲,宋衛(wèi)東,徐敬青.基于多項式擬合的擴展卡爾曼濾波算法[J].計算機應(yīng)用,2016(5):1455-1457,1463.

[6] 閆小龍,陳國光,白敦卓.自適應(yīng)卡爾曼濾波器在火箭彈落點估計中的應(yīng)用(英文)[J].Journal of Measurement Science and Instrumentation,2015(3):212-217.

[7] 余小琴,沈文苗.擴展卡爾曼濾波算法初值選取方法[J].聲學(xué)與電子工程,2012(1):12-13,17.

[8] 秦偉,田曉麗,劉超.基于卡爾曼濾波的導(dǎo)彈姿態(tài)最優(yōu)控制法[J].機械工程與自動化,2016(1):183-185.

[9] 戰(zhàn)杰. 一種外彈道測量數(shù)據(jù)的斑點型野值剔除方法[J].航天控制,2016,34(1):75-77+83.

[10] 卓寧. 靶場外彈道數(shù)據(jù)處理中野值點剔除方法[J].測試技術(shù)學(xué)報,2008(4):313-317.

[11] 李景熹,王宇,王樹宗,等.觀測值中野值的判別與處理方法仿真研究[J].微計算機信息,2006(13):140-142.

TheStudyofKalmanBallisticFilterforAdaptingtheOutliers

FAN Xu1, CHEN Guoguang1, WANG Guangzhi2, ZHAO Junqiang2, YANG Zhijie1

(1.The North University of China, Taiyuan 030051, China; 2.Yuxi Industries Group Co.,Ltd., Nanyang 473000, China)

In the acquisition process of the optimal ballistic parameter estimation,Kalman filter is an algorithm that the application and usability of project is strong.To reduse the adverse effects of the outliers that might be formed during the measurement on Kalman filtering process, a kind of wild value removed method based on polynomial fitting is proposed. The method use the early trajectory data of the filter to polynomial fit.On the basis of this polynomial, the residual as the criterion,triple std as the judge standard, wipe off the outliers. Simulation results show that,the method is effective,the outliers can be removed, the process of filtering can be fast convergence.

trajectory filter; Kalman filter; outliers removed; polynomial fit

2017-07-20；

2017-08-11

范旭，男，碩士研究生，主要從事智能彈藥、彈箭制導(dǎo)與控制研究; 陳國光，男，博士，教授，博士生導(dǎo)師，主要從事信息化武器系統(tǒng)研究。

10.11809/scbgxb2017.12.022

本文引用格式：范旭，陳國光,王光志，等.卡爾曼彈道濾波器抗野值方法研究[J].兵器裝備工程學(xué)報，2017(12):93-97.

formatFAN Xu, CHEN Guoguang, WANG Guangzhi,et al.The Study of Kalman Ballistic Filter for Adapting the Outliers[J].Journal of Ordnance Equipment Engineering,2017(12):93-97.

TJ765

A

2096-2304(2017)12-0093-05

(責(zé)任編輯唐定國)