基于迭代無跡卡爾曼濾波的小型無人機目標定位方法

唐大全,柳向陽,鄧偉棟,丁鵬程

(海軍航空大學,山東 煙臺 264001)

在現代軍用領域中,無人機常用于偵察監視、目標打擊、作戰效能評估等[1],對地面目標定位是偵察監視的重要需求之一,其目的為求取目標在大地坐標系下的三維坐標[2]。目前,高精度的無人機目標定位已成為國內外科研工作者的研究熱點。

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

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

1 移動目標定位的原理

小型無人機的目標定位系統一般包括GPS定位系統、慣性測量單元和光電成像平臺等。光電成像平臺上一般有穩定伺服機構、攝像機云臺和攝像機等部件。由于小型無人機功率載荷有限,激光測距儀等部件一般不能安裝在無人機上。無人機定位時,將目標鎖定在成像平面的視場中心,通過測量光心相對于目標的高低角和方位角,結合無人機的位姿信息,解算出目標位置。能夠精準地對無人機定位和測量無人機與目標的相對角度是解決這類問題的關鍵。

1.1 視線方位角的計算

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

視場中心點在攝像機坐標系下為tp(0,0,f),f為攝像機焦距。設無人機的偏航角ψ、俯仰角θ、橫滾角γ和攝像機坐標系下光軸的方位ρ和高低角φ,可以得到無人機地理坐標系下的坐標為

(1)

(2)

圖1 目標定位示意圖

1.2 狀態方程的建立

無人機對目標進行定位,需要圍繞目標飛行一段距離,測量各時刻目標相對于無人機的角度信息,根據角度信息建立相應的量測方程,估計目標的位置。

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

(3)

zk=h(xk)+vk

(4)

wk～N(0,Qk)

上式中,在系統方程(3)和量測方程(4)中,wk,vk同時滿足:

Qk,Rk分別為系統噪聲序列的方差陣和量測噪聲序列的方差陣,假定都是正定矩陣。量測方程為

(5)

2 目標定位的迭代無跡卡爾曼濾波算法

標準的UKF算法受數值計算舍入誤差、可觀測性和觀測噪聲大等因素的影響產生不穩定、收斂速度慢的問題。迭代無跡卡爾曼濾波是對無跡卡爾曼濾波的改進,能夠克服無跡卡爾曼濾波收斂速度慢和收斂精度不高的缺點[12-13]。

2.1 迭代UKF的原理步驟

1)濾波初始化

2)計算sigama樣本點集

(6)

3)時間更新

(7)

(8)

4)量測更新

產生新的采樣點:

(9)

量測更新:

(10)

(11)

(12)

(13)

(14)

(15)

(16)

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

(17)

5)確定迭代條件

對于濾波估計問題,測量更新的目的就是利用已有的測量信息估計到準確的狀態量和協方差矩陣。考慮到獨立的隨機向量,服從正態分布,即:

(18)

式(19)可以構成一個新的變量:

(19)

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

(20)

那么y的似然函數為

(21)

n和m分別為狀態向量和量測向量的維數,那么對狀態y的極大似然估計為

y*=arg max[L(y)]

(22)

比較式(22),也就是求

y*=arg min[q(x)]

(23)

其中

(24)

當

q(xj+1)

(25)

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

由式(24)、(25)可得

(26)

于是可得

(27)

其中:

(28)

重新進入步驟4),完成量測更新。當j不滿足這個條件時,進入下一步。

2.2 目標狀態估計仿真流程

目標狀態估計就是利用無人機自身的狀態信息,融合:測向傳感器的測量信息,對地面目標狀態的估計。具體流程如下:

Step1:鎖定目標圖像,測量位于光軸中心位置的目標的測量角(ρ,φ),結合無人機位置,利用公式(5)得到量測量z(k-1)。

Step3:從Step1開始,開始下一次估計。

3 仿真結果及分析

為了驗證算法的性能,進行N次蒙特卡羅實驗。采用均方誤差(Root Mean Square Error,RMSE)來表示 目標定位與跟蹤的精度,定位為

(29)

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

圖2 仿真情景1的目標定位均方誤差圖

圖3 仿真情景2的目標定位均方誤差圖

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

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

從目標運動的跟蹤定位圖中可以看出,各種算法都可以很好地跟蹤運動目標;在觀測精度高和觀測精度低的情況下,IUKF都能很好地跟蹤運動目標,并且算法的收斂速度和收斂精度都明顯好于前兩種算法,能夠達到快速穩定準確跟蹤運動目標的效果。

4 結束語

本文提出了一種基于無跡卡爾曼濾波的無人機目標定位方法。利用小型無人機的光電成像平臺將目標鎖定在成像平面中心,鑒于小型無人機載荷能力有限的情況,測量無人機與目標的視軸偏角,以此構建目標定位的模型。無跡卡爾曼濾波是解決非線性問題的常用方法,由于計算誤差和系統參數分配出現濾波發散和收斂速度慢等問題,迭代無跡卡爾曼濾波能夠克服系統收斂速度慢和收斂發散的問題,提高收斂的效率。仿真實驗表明,迭代無跡卡爾曼濾波能夠提高濾波收斂速度,抑制濾波發散,具有一定的實用價值。