基于迭代無(wú)跡卡爾曼濾波的小型無(wú)人機(jī)目標(biāo)定位方法

唐大全,柳向陽(yáng),鄧偉棟,丁鵬程

(海軍航空大學(xué),山東 煙臺(tái) 264001)

在現(xiàn)代軍用領(lǐng)域中,無(wú)人機(jī)常用于偵察監(jiān)視、目標(biāo)打擊、作戰(zhàn)效能評(píng)估等[1],對(duì)地面目標(biāo)定位是偵察監(jiān)視的重要需求之一,其目的為求取目標(biāo)在大地坐標(biāo)系下的三維坐標(biāo)[2]。目前,高精度的無(wú)人機(jī)目標(biāo)定位已成為國(guó)內(nèi)外科研工作者的研究熱點(diǎn)。

基于測(cè)距信息等有源定位方式,需要無(wú)人機(jī)有較大的載荷功率發(fā)射信息信號(hào),小型無(wú)人機(jī)受功率載荷的限制,主要采用無(wú)源的定位方法,主要采集目標(biāo)的圖像,通過(guò)攝像機(jī)的內(nèi)外參數(shù)及共線方程,計(jì)算目標(biāo)的位置信息[3-4]。探測(cè)角度的唯方位測(cè)量方法是這類定位的常用方法,側(cè)向傳感器是獲取這一信息的重要渠道。解決這類問(wèn)題一般用非線性的濾波方法。

擴(kuò)展卡爾曼濾波(Extended Kalman Filter, EKF)是解決非線性問(wèn)題的方法之一,核心思想是將非線性方程在預(yù)測(cè)處進(jìn)行一階泰勒展開(kāi)并取近似項(xiàng),線性化之后再進(jìn)行卡爾曼濾波,這樣不可避免地存在偏差,當(dāng)系統(tǒng)非線性較強(qiáng)和誤差較大時(shí),濾波可能發(fā)散[5]。無(wú)跡卡爾曼濾波[6-7](Unscented Kalman Filer, UKF)是基于無(wú)跡變換(Unscented Transformation, UT),使用變換后的統(tǒng)計(jì)量來(lái)估計(jì)狀態(tài)的均值及方差,能夠避免線性化帶來(lái)的誤差,由于無(wú)跡變化對(duì)于協(xié)方差估計(jì)的不足,測(cè)量誤差較大時(shí),存在收斂速度慢和收斂易發(fā)散的問(wèn)題[8]。針對(duì)以上問(wèn)題,本文在UKF算法基礎(chǔ)上,提出了一種迭代UKF算法,不同于基于sigama采樣點(diǎn)和基于高斯牛頓法的迭代法的UKF[9-10]估計(jì)目標(biāo)狀態(tài),該方法通過(guò)極大似然函數(shù)確定迭代條件,對(duì)目標(biāo)定位的量測(cè)環(huán)節(jié)進(jìn)行迭代更新,縮小了量測(cè)誤差,提高了迭代UKF算法的收斂速度和跟蹤精度。

1 移動(dòng)目標(biāo)定位的原理

小型無(wú)人機(jī)的目標(biāo)定位系統(tǒng)一般包括GPS定位系統(tǒng)、慣性測(cè)量單元和光電成像平臺(tái)等。光電成像平臺(tái)上一般有穩(wěn)定伺服機(jī)構(gòu)、攝像機(jī)云臺(tái)和攝像機(jī)等部件。由于小型無(wú)人機(jī)功率載荷有限,激光測(cè)距儀等部件一般不能安裝在無(wú)人機(jī)上。無(wú)人機(jī)定位時(shí),將目標(biāo)鎖定在成像平面的視場(chǎng)中心,通過(guò)測(cè)量光心相對(duì)于目標(biāo)的高低角和方位角,結(jié)合無(wú)人機(jī)的位姿信息,解算出目標(biāo)位置。能夠精準(zhǔn)地對(duì)無(wú)人機(jī)定位和測(cè)量無(wú)人機(jī)與目標(biāo)的相對(duì)角度是解決這類問(wèn)題的關(guān)鍵。

1.1 視線方位角的計(jì)算

無(wú)人機(jī)目標(biāo)定位就是基于攝影測(cè)量、圖像處理和信息處理等技術(shù),通過(guò)對(duì)無(wú)人機(jī)偵察以及與圖像處理相關(guān)的遙測(cè)數(shù)據(jù)的處理與分析,提取目標(biāo)精確三維坐標(biāo)的過(guò)程[11]。在目標(biāo)定位過(guò)程中,首先圖像傳感器采集圖像,光電成像平臺(tái)穩(wěn)定伺服系統(tǒng)驅(qū)動(dòng)CCD攝像機(jī)光軸對(duì)準(zhǔn)目標(biāo),使目標(biāo)位于成像靶面中心。定義視線方位角為在無(wú)人機(jī)地理坐標(biāo)系中,采用東北天坐標(biāo)系,視線高低角α為視線與無(wú)人機(jī)Z軸正向(向上)之間的夾角,視線水平角β為視線在水平面的投影與x軸正向(東向)之間的夾角,如圖1所示,利用測(cè)角傳感器測(cè)量這些角度值。在無(wú)人機(jī)的定位過(guò)程中,攝像機(jī)的光軸指向與無(wú)人機(jī)的姿態(tài)角共同決定了目標(biāo)視線方位角。

視場(chǎng)中心點(diǎn)在攝像機(jī)坐標(biāo)系下為tp(0,0,f),f為攝像機(jī)焦距。設(shè)無(wú)人機(jī)的偏航角ψ、俯仰角θ、橫滾角γ和攝像機(jī)坐標(biāo)系下光軸的方位ρ和高低角φ,可以得到無(wú)人機(jī)地理坐標(biāo)系下的坐標(biāo)為

(1)

(2)

圖1 目標(biāo)定位示意圖

1.2 狀態(tài)方程的建立

無(wú)人機(jī)對(duì)目標(biāo)進(jìn)行定位,需要圍繞目標(biāo)飛行一段距離,測(cè)量各時(shí)刻目標(biāo)相對(duì)于無(wú)人機(jī)的角度信息,根據(jù)角度信息建立相應(yīng)的量測(cè)方程,估計(jì)目標(biāo)的位置。

xk+1=Φk+1,kxk+wk

(3)

zk=h(xk)+vk

(4)

wk～N(0,Qk)

上式中,在系統(tǒng)方程(3)和量測(cè)方程(4)中,wk,vk同時(shí)滿足:

Qk,Rk分別為系統(tǒng)噪聲序列的方差陣和量測(cè)噪聲序列的方差陣,假定都是正定矩陣。量測(cè)方程為

(5)

2 目標(biāo)定位的迭代無(wú)跡卡爾曼濾波算法

標(biāo)準(zhǔn)的UKF算法受數(shù)值計(jì)算舍入誤差、可觀測(cè)性和觀測(cè)噪聲大等因素的影響產(chǎn)生不穩(wěn)定、收斂速度慢的問(wèn)題。迭代無(wú)跡卡爾曼濾波是對(duì)無(wú)跡卡爾曼濾波的改進(jìn),能夠克服無(wú)跡卡爾曼濾波收斂速度慢和收斂精度不高的缺點(diǎn)[12-13]。

2.1 迭代UKF的原理步驟

1)濾波初始化

2)計(jì)算sigama樣本點(diǎn)集

(6)

3)時(shí)間更新

(7)

(8)

4)量測(cè)更新

產(chǎn)生新的采樣點(diǎn):

(9)

量測(cè)更新:

(10)

(11)

(12)

(13)

(14)

(15)

(16)

Pk,j=Pk,j-1-Kk,jPzz,k,jKk,j

(17)

5)確定迭代條件

對(duì)于濾波估計(jì)問(wèn)題,測(cè)量更新的目的就是利用已有的測(cè)量信息估計(jì)到準(zhǔn)確的狀態(tài)量和協(xié)方差矩陣?？紤]到獨(dú)立的隨機(jī)向量,服從正態(tài)分布,即:

(18)

式(19)可以構(gòu)成一個(gè)新的變量:

(19)

由式(19)和式(20)可得

(20)

那么y的似然函數(shù)為

(21)

n和m分別為狀態(tài)向量和量測(cè)向量的維數(shù),那么對(duì)狀態(tài)y的極大似然估計(jì)為

y*=arg max[L(y)]

(22)

比較式(22),也就是求

y*=arg min[q(x)]

(23)

其中

(24)

當(dāng)

q(xj+1)

(25)

成立時(shí),可以看到q(xj)時(shí)呈收斂趨勢(shì),也就是q(xj+1)比q(xj)更加接近最優(yōu)解。

由式(24)、(25)可得

(26)

于是可得

(27)

其中:

(28)

重新進(jìn)入步驟4),完成量測(cè)更新。當(dāng)j不滿足這個(gè)條件時(shí),進(jìn)入下一步。

2.2 目標(biāo)狀態(tài)估計(jì)仿真流程

目標(biāo)狀態(tài)估計(jì)就是利用無(wú)人機(jī)自身的狀態(tài)信息,融合:測(cè)向傳感器的測(cè)量信息,對(duì)地面目標(biāo)狀態(tài)的估計(jì)。具體流程如下:

Step1:鎖定目標(biāo)圖像,測(cè)量位于光軸中心位置的目標(biāo)的測(cè)量角(ρ,φ),結(jié)合無(wú)人機(jī)位置,利用公式(5)得到量測(cè)量z(k-1)。

Step3:從Step1開(kāi)始,開(kāi)始下一次估計(jì)。

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

為了驗(yàn)證算法的性能,進(jìn)行N次蒙特卡羅實(shí)驗(yàn)。采用均方誤差(Root Mean Square Error,RMSE)來(lái)表示 目標(biāo)定位與跟蹤的精度,定位為

(29)

仿真實(shí)驗(yàn)1:無(wú)人機(jī)做盤旋上升飛行,目標(biāo)靜止。設(shè)無(wú)人機(jī)飛行半徑100 m,圓心坐標(biāo)為(500,600,400),初始航高z=400 m,盤旋上升飛行速度vz=1 m/s,運(yùn)動(dòng)角速度為0.02 rad/s,無(wú)人機(jī)自身定位服從均值為零,方差為10的正態(tài)分布;目標(biāo)真實(shí)坐標(biāo)為(350,450)。此時(shí)目標(biāo)初始狀態(tài)x0=[330,480,0,0],靜止目標(biāo)Q為零矩陣,無(wú)人機(jī)視軸角誤差為1°,記為精度1,目標(biāo)定位的均方誤差如圖2a所示,當(dāng)無(wú)人機(jī)視軸角誤差為0.1°,記為精度2,目標(biāo)定位的均方誤差如圖2b所示。

圖2 仿真情景1的目標(biāo)定位均方誤差圖

圖3 仿真情景2的目標(biāo)定位均方誤差圖

視軸觀測(cè)角誤差是影響目標(biāo)定位精度的主要因素,當(dāng)觀測(cè)誤差較小,觀測(cè)精度較高時(shí),各算法都有較好的定位精度;觀測(cè)精度較小誤差比較大時(shí),可以看到UKF和EKF定位精度相當(dāng),IUKF的收斂速度和定位精度略微優(yōu)于前兩者。在非線性較弱的條件下,UKF和IUKF的優(yōu)勢(shì)不明顯。

仿真實(shí)驗(yàn)2目標(biāo)做勻速直線運(yùn)動(dòng),無(wú)人機(jī)對(duì)目標(biāo)的仿真情況。設(shè)無(wú)人機(jī)飛行半徑100 m,圓心坐標(biāo)為(500,600,400),初始航高z=400 m,盤旋上升飛行速度vz=1 m/s,運(yùn)動(dòng)角速度為0.09 rad/s,無(wú)人機(jī)自身定位服從均值為零,方差為10的正態(tài)分布;目標(biāo)初始起點(diǎn)為(350,450),以v=[3.9, 3.9]的速度運(yùn)動(dòng),即x0=[330, 480, 0, 3.9, 3.9, 0]。無(wú)人機(jī)視軸角誤差為1°,記為精度1,目標(biāo)定位的均方誤差如圖3a所示,當(dāng)無(wú)人機(jī)視軸角誤差為0.1°,記為精度2,目標(biāo)定位的均方誤差如圖3b所示。

從目標(biāo)運(yùn)動(dòng)的跟蹤定位圖中可以看出,各種算法都可以很好地跟蹤運(yùn)動(dòng)目標(biāo);在觀測(cè)精度高和觀測(cè)精度低的情況下,IUKF都能很好地跟蹤運(yùn)動(dòng)目標(biāo),并且算法的收斂速度和收斂精度都明顯好于前兩種算法,能夠達(dá)到快速穩(wěn)定準(zhǔn)確跟蹤運(yùn)動(dòng)目標(biāo)的效果。

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

本文提出了一種基于無(wú)跡卡爾曼濾波的無(wú)人機(jī)目標(biāo)定位方法。利用小型無(wú)人機(jī)的光電成像平臺(tái)將目標(biāo)鎖定在成像平面中心,鑒于小型無(wú)人機(jī)載荷能力有限的情況,測(cè)量無(wú)人機(jī)與目標(biāo)的視軸偏角,以此構(gòu)建目標(biāo)定位的模型。無(wú)跡卡爾曼濾波是解決非線性問(wèn)題的常用方法,由于計(jì)算誤差和系統(tǒng)參數(shù)分配出現(xiàn)濾波發(fā)散和收斂速度慢等問(wèn)題,迭代無(wú)跡卡爾曼濾波能夠克服系統(tǒng)收斂速度慢和收斂發(fā)散的問(wèn)題,提高收斂的效率。仿真實(shí)驗(yàn)表明,迭代無(wú)跡卡爾曼濾波能夠提高濾波收斂速度,抑制濾波發(fā)散,具有一定的實(shí)用價(jià)值。