基于卡爾曼濾波的無人機(jī)測向定位算法研究

劉 暢，劉湘?zhèn)ィㄅ睿钭咏?/p>

（電子工程學(xué)院，合肥 230037）

0 引 言

利用無人機(jī)對地面雷達(dá)等目標(biāo)實(shí)施偵察定位，已經(jīng)成為一種常見的無人機(jī)作戰(zhàn)運(yùn)用方式。無人機(jī)測向定位時以2次或多次測向線的交點(diǎn)作為目標(biāo)位置進(jìn)行定位，但由于測向誤差等問題，定位精度一直不高。對目標(biāo)實(shí)施多次測向定位，理論上可以降低定位誤差，但是所需的定位次數(shù)多，定位時間長。文獻(xiàn)［1］通過對測角誤差的分析計算出了定位誤差，但對多次定位給定位誤差帶來的影響研究較少。

在解決目標(biāo)跟蹤問題時，文獻(xiàn)［2］、［3］依據(jù)卡爾曼濾波（KF）算法，按照時間序列預(yù)測目標(biāo)位置與速度，通過對多次測量結(jié)果的修正有效降低了跟蹤誤差。而且，對于跟蹤目標(biāo)時觀測平臺靜止、雷達(dá)目標(biāo)運(yùn)動，無人機(jī)測向定位時無人機(jī)運(yùn)動、雷達(dá)目標(biāo)靜止的問題，可以通過坐標(biāo)系轉(zhuǎn)化來解決。可見將多次測向定位結(jié)果融合以提高定位精度的方法切實(shí)可行。

但是，跟蹤目標(biāo)時所關(guān)心的是目標(biāo)當(dāng)前的位置信息，是一個點(diǎn)。但對目標(biāo)進(jìn)行定位時，由于有誤差，目標(biāo)可能的位置應(yīng)當(dāng)是一個區(qū)域。因此只用目標(biāo)跟蹤的方法計算目標(biāo)位置的估計值不能滿足對目標(biāo)定位的需求。文獻(xiàn)［4］在對通信電臺的定位中，提出了目標(biāo)可能位置概率橢圓的概念，提出了在一定概率內(nèi)，目標(biāo)可能的區(qū)域位置與大小形狀的計算方法。利用此方法可以更加直觀地計算出目標(biāo)所在的區(qū)域，而且便于理解。但是計算時，各測向線相互獨(dú)立，沒有通過預(yù)估與修正提高定位的精度。

基于以上分析，本文在無人機(jī)測角定位目標(biāo)雷達(dá)的背景下，將KF算法與目標(biāo)可能位置概率橢圓計算方法相結(jié)合，得到更加精確、直觀的定位結(jié)果。

1 目標(biāo)位置概率橢圓計算模型

無人機(jī)對目標(biāo)雷達(dá)實(shí)施多次測向定位，每次2條測向線相交的交點(diǎn)即為目標(biāo)雷達(dá)的位置。由于無人機(jī)本身在飛行，因此可將不同時刻無人機(jī)所得的測向線進(jìn)行交叉，即可對目標(biāo)雷達(dá)定位。令無人機(jī)以勻速飛行，每隔時間T對目標(biāo)雷達(dá)實(shí)施1次定位，共進(jìn)行N次定位，建立坐標(biāo)系如圖1所示。

圖1 無人機(jī)多次測向定位示意圖

目標(biāo)雷達(dá)位于 （xt，yt），無人機(jī)從 （x1，y1）位置開始實(shí)施測向定位，至位置 （xN，yN），期間所測得的方位角分別為θ1，…，θk，…，θN。Δθ1，Δθ2，…，ΔθN是方位角的測量誤差，σang為測角的均方根誤差。

令k時刻無人機(jī)與目標(biāo)的距離為Dk。若將N次測向結(jié)果進(jìn)行交會，無人機(jī)各時刻對目標(biāo)的測向偏差是相互獨(dú)立的，則概率最大時目標(biāo)可能位置的坐標(biāo)為［4］：

為了直觀地表示無人機(jī)測向定位時目標(biāo)可能的位置，采用目標(biāo)可能位置概率橢圓的算法，將目標(biāo)可能存在的區(qū)域依其存在的概率劃定區(qū)域。由計算得，目標(biāo)可能位置等概率點(diǎn)的周界為一橢圓，當(dāng)概率為p時，橢圓的長短半軸a0和b0分別為：

2 基于KF的目標(biāo)位置及概率橢圓計算模型

根據(jù)KF的動目標(biāo)跟蹤算法可知，測得目標(biāo)位置初值之后，再以測量值更新，可以得到目標(biāo)位置的時間序列預(yù)測，而且跟蹤精度會隨著時間的積累而提高。無人機(jī)對目標(biāo)雷達(dá)的測向定位與動目標(biāo)跟蹤的不同在于一個是觀測平臺在運(yùn)動，另一個是目標(biāo)在運(yùn)動。只要轉(zhuǎn)換坐標(biāo)系就可將2個問題劃歸為一類問題，因此KF算法可以用于提高無人機(jī)測向定位的精度。

在目標(biāo)雷達(dá)的可能位置概率橢圓計算過程中，認(rèn)為各次的測角誤差相互獨(dú)立，但服從同一零均值正態(tài)分布，因而多次測量對概率橢圓的面積減小程度有限。因此，首先利用KF算法，對目標(biāo)可能出現(xiàn)的方位角測量進(jìn)行時間序列預(yù)測，以減小測角的誤差，然后再計算目標(biāo)可能位置的概率橢圓，以實(shí)現(xiàn)對目標(biāo)較高精度的定位。

利用KF算法，可由角度測量值計算得到目標(biāo)的位置及其方差。將此作為KF算法的初值進(jìn)行計算，根據(jù)位置估計下一時刻的位置，得到位置方差的同時，可以計算得到測角方差。將KF算法求得的測角方差作為每個點(diǎn)的測角誤差，即可計算出目標(biāo)可能位置的概率橢圓，思路如圖2所示。

在圖1所給出的坐標(biāo)系下，當(dāng)無人機(jī)飛行的航向角為α?xí)r，無人機(jī)的坐標(biāo)為：

圖2 基于KF的目標(biāo)可能位置算法框圖

利用KF算法計算時，系統(tǒng)的狀態(tài)方程可表示為：

系統(tǒng)的初值為無人機(jī)對目標(biāo)前2次測向定位的結(jié)果，即：

無人機(jī)測向定位時的觀測量為目標(biāo)的方位角，因此系統(tǒng)的測量值為：

從上一時刻系統(tǒng)狀態(tài)點(diǎn)向當(dāng)前時刻測向線作垂線，交點(diǎn)即為當(dāng)前時刻目標(biāo)位置的測量值，如圖3所示。

則目標(biāo)位置的測量值為：

圖3 目標(biāo)位置測量值示意圖

經(jīng)KF算法計算后，可得各時刻的目標(biāo)位置估計值［5-6］，即 （xk，k），可計算 得當(dāng)前時刻目標(biāo) 方 位角的估計值：

而此時方位角的測量誤差為：

由于Dj已知，將式（14）、（15）代入式（1）、（8），即可求出當(dāng)前目標(biāo)可能位置出現(xiàn)的區(qū)域。

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

仿真中分別考慮直接利用測向線分析目標(biāo)可能位置以及利用KF算法對測量結(jié)果進(jìn)行分析后目標(biāo)的可能位置，并將2種方式的目標(biāo)位置測量誤差進(jìn)行比較。定位誤差可定義為［7］：

式中：k為測量次數(shù)。

設(shè)定無人機(jī)對目標(biāo)雷達(dá)進(jìn)行定位時，目標(biāo)雷達(dá)位于坐標(biāo)原點(diǎn) （0，0），無人機(jī)沿X軸正方向飛行，測角誤差σang＝0.003rad，目標(biāo)雷達(dá)始終處在無人機(jī)偵察范圍內(nèi)，不考慮目標(biāo)雷達(dá)的高度、無人機(jī)的飛行高度即信號截獲概率的影響。仿真條件如表1所示。

表1 仿真條件表

無人機(jī)每隔時間T（T＝1s）對目標(biāo)雷達(dá)進(jìn)行1次測向，定位誤差隨測量次數(shù)變化的曲線如圖4所示。測量所得的目標(biāo)可能位置如圖5、圖6所示，圖中的點(diǎn)為目標(biāo)位置觀測點(diǎn)，橢圓線為概率0.8時的目標(biāo)可能位置區(qū)域邊界。

分析仿真結(jié)果可得：

（1）由圖4可知，經(jīng)KF算法計算后的定位精度比直接通過測角定位所得的結(jié)果精確了1倍。前15次測角，KF方法可使定位誤差迅速降低，經(jīng)50次測角后，KF方法的定位誤差為200～300m，而直接測角定位的誤差為400～500m。

圖4 定位誤差隨測量次數(shù)變化曲線

圖5 經(jīng)KF計算得多目標(biāo)位置估計點(diǎn)及可能位置橢圓

圖6 直接測角所得目標(biāo)位置估計點(diǎn)及可能位置橢圓

（2）對比圖5、圖6可知，KF算法所得的目標(biāo) 位置觀測點(diǎn)在目標(biāo)真實(shí)位置周圍分布密集而且距離較近。相同測角次數(shù)下，目標(biāo)可能位置區(qū)域的面積也較小。經(jīng)50次測角后，KF算法所得的目標(biāo)可能位置區(qū)域?yàn)?.047km2，而直接測角所得的結(jié)果為0.068km2

（3）對比圖5、圖6中條件1、2可知，定位誤差隨無人機(jī)與目標(biāo)雷達(dá)間距離的變小而減小。當(dāng)無人機(jī)與目標(biāo)在Y軸方向上的距離從20km減小為10km時，該方向上觀測點(diǎn)的位置變得密集，而且50次測角后，目標(biāo)可能位置區(qū)域的面積也由0.047km2變?yōu)?.041km2。

（4）由圖5、圖6中條件1、3可知，無人機(jī)速度對精度的影響主要體現(xiàn)在定位誤差變化的速度上。當(dāng)無人機(jī)飛行速度從50m／s降為30m／s后，目標(biāo)可能存在區(qū)域在X方向的半軸的長度變化率變?yōu)樵瓉淼亩种弧６矣捎谒俣鹊臏p小，無人機(jī)與目標(biāo)間的距離相對較大，目標(biāo)可能位置區(qū)域也由0.047km2變?yōu)?.081km2。

4 結(jié)束語

利用KF算法對測角定位結(jié)果進(jìn)行分析與對直接測角定位點(diǎn)求重心的方法相比，同樣是利用迭代算法，前者的計算復(fù)雜度不高，但是定位精度以及收斂速度都較好。通過仿真可知，經(jīng)KF算法對目標(biāo)定位結(jié)果進(jìn)行分析，目標(biāo)位置的測量點(diǎn)在目標(biāo)真實(shí)位置周圍的分布更加緊密，目標(biāo)可能位置區(qū)域也更小。而且通過不同仿真方案的對比，可知無人機(jī)配置對定位精度的影響，對無人機(jī)的運(yùn)用具有一定的指導(dǎo)意義。

［1］ 呂義東，周銘.基于MATLAB的雙站交叉定位及誤差分析［J］.實(shí)驗(yàn)科學(xué)與技術(shù)，2008，6（6）：11-14.

［2］ 袁罡，陳鯨.基于UKF的單站無源定位與跟蹤算法［J］.電子與信息學(xué)報，2008，30（9）：2120-2123.

［3］ 司文健，平殿發(fā)，蘇峰，等.基于相位差變化率的機(jī)載無源定位研究［J］.艦船電子工程，2010，30（4）：76-79.

［4］ 曹志耀，邵國培，何俊，等.計算機(jī)作戰(zhàn)模擬系統(tǒng)［M］.北京：解放軍出版社，1999.

［5］ 彭丁聰.卡爾曼濾波的基本原理及應(yīng)用［J］.軟件導(dǎo)刊，2009，8（11）：32-34.

［6］ 郭福成，李宗華，孫仲康.無源定位跟蹤中修正協(xié)方差擴(kuò)展卡爾曼濾波算法［J］.電子與信息學(xué)報，2004，26（6）：917-922.

［7］ 姚天任，孫洪.現(xiàn)代數(shù)字信號處理［M］.武漢：華中科技大學(xué)出版社，1999.