一種基于三維到達角的無偏偽線性卡爾曼濾波

林建軍,王驤予涵,班曉軍

(1.哈爾濱工業大學航天學院,哈爾濱 150000;2.哈爾濱工程大學航天與建筑工程學院,哈爾濱 150001)

0 引言

無源偵察定位技術具有作用距離遠、機動性強的優點,在軍事上存在著巨大的潛在價值。與有源定位不同,無源定位不需要向對方發射探測信號,具有隱蔽性良好、生存能力強的優點。因此,隨著電子干擾技術、反偵察技術的發展,其在軍事領域的研究價值日益凸顯。無源定位的方法較多[1-2],根據定位原理的不同可分為測向定位法、信號強度定位法、到達時間定位法、到達時間差定位法、多普勒頻率定位法等。機載快速定位技術是由搭載各式傳感器的無人駕駛飛行器(unmanned aerial vehicle,UAV)取代傳統的固定基站,對目標進行定位跟蹤的一項技術。根據UAV 的數量可以分為單站定位與多站定位。單站定位始終只有一架UAV 工作不需要受到其他限制,具有機動性強、靈活性高的優點,但單次測量時存在信息量不足,無法對目標運動狀態完全估計;而多站定位觀測信息雖然較豐富,對運動形式要求較少,但不同平臺之間的數據同步與數據融合都有著嚴格的限制,使得觀測系統的機動性和獨立性有所降低[3]。因此,單站定位技術具有更為巨大的研究潛力。

基于三維到達角對目標運動狀態進行估計的技術稱為3D-AOA(three-dimensional angle of arrival)目標跟蹤,諸多學者對該技術進行了研究,按照狀態估計方法大致可以分為非線性濾波法與偽線性濾波法。偽線性濾波方法是通過對具有非線性性質的角度量測方程進行偽線性化得到[4],具有計算復雜度低、初值不敏感的優點,文獻[5]曾對EKF、偽線性濾波以及粒子濾波在二維條件下的性能進行了比較,結果表明偽線性濾波方法與粒子濾波的性能接近,但其算法復雜度遠低于粒子濾波。但偽線性濾波在噪聲較大時,存在嚴重的有偏性問題[6],為此,Dogancay等[7-8]對偏差進行了分析并設計了誤差補償方法,并將輔助變量法也應用到偽線性濾波上。偏差補償偽線性卡爾曼濾波(bias-compensated pseudo-linear Kalman filter,BC-PLKF)與輔助變量偽線性卡爾曼濾波(instrumental variable pseudolinear Kalman filter,IV-PLKF)是其中比較有代表性的方法,但IV-PLKF受限于BC-PLKF,當目標出現機動時很可能在BC-PLKF時就出現發散現象,嚴重影響IV-PLKF性能;郭戈等[9]則對傳感器不確定性進行了考慮,對過程噪聲的不確定性進行了建模,提出了遞推輔助變量偽線性卡爾曼濾波(recursive instrumental variable Kalman filter,RIVKF);而Pang等[10]則考慮了觀測平臺自身位置估計誤差造成的影響,對該部分誤差進行了補償,文獻[11-12]則是采用了一種添加約束條件的方法來達到漸進無偏的效果。Huang等[13]將噪聲項與真值分離的方法適用于二維條件的具有無偏性的UB-PLKF(unbiased pseudolinear Kalman filter),從原理上實現了算法的無偏性,并通過仿真實驗表明該方法能夠較好地解決機動目標的跟蹤問題。

然而,在基于角度信息的目標跟蹤中,大多數方法適用于二維條件下的近距離目標,且研究對象主要為勻速運動目標,而對于大場景下的遠距離機動目標研究較少。本文將在此基礎上,將適用于二維場景下的UB-PLKF 方法推廣到三維條件下,并設計EKF 作為角度濾波器對其進行輔助得到3DUBKF(three dimensional unbiased Kalman filter),使其適用于遠距離場景下機動目標跟蹤,并保持較高的定位精度。

1 基于3D-AOA的目標定位模型

1.1 基于3D-AOA的運動模型

對于機動目標,其運動軌跡呈現為曲線形式,在文獻[14]就將目標軌跡假設為服從關于時間t的N次多項式,而根據樣條插值理論,可以選擇一次函數作為樣條插值的基函數,該情況正對應了目標勻速運動。因此,將勻速運動模型作為機動目標的運動模型具有一定的合理性。

定義目標在k時刻的位置用pk=[xT,k,yT,k,zT,k]T表示,速度vk=[vx,k,vy,k,vz,k]T,xk為目標在k時刻的狀態變量,其狀態空間模型可由如下形式表示

其中,xk=;Fk為k時刻的目標狀態轉移矩陣;wk表示k時刻的過程噪聲,其自相關矩陣,

其中,T表示采樣時間間隔;q表示噪聲的功率譜密度(m2/s3);I表示單位矩陣。

1.2 基于3D-AOA 的量測模型

基于3D-AOA 的目標跟蹤,其量測模型具有非線性。定義觀測平臺的位置uk= [xo,k,yo,k,zo,k]T,觀測平臺對目標的方位角與俯仰角的觀測方程可表示為

其中,δk=[nk,mk]T;nk,mk為相互獨立的高斯白噪聲,標準差分別為σθ,k,σφ,k;E{δkδTk}=Rk=。

該模型進行偽線性化可以得到新的系統狀態方程

式中

1.3 偽線性卡爾曼濾波

根據1.2節的結果,將卡爾曼濾波算法應用到該偽線性系統中,得到如下的偽線性卡爾曼濾波算法

(1)狀態預測階段

(2)更新階段

對卡爾曼濾波進行偏差分析,將式(12),(13),(14)代入到式(15)中,并利用矩陣求逆引理可以得到

令

于是,對式(17)求取期望可以得到偽線性卡爾曼濾波估計的偏差為

也就是說,對于偽線性卡爾曼濾波而言,其原理性偏差由Bk與Ck兩部分組成。Bk是由目標的過程噪聲引起,當過程噪聲較小時,該部分可近似為0,即E{Bk}≈0;而Ck是由于系統偽線性化使得量測矩陣Hk與ηk存在相關性引起的,二者之間的相關性不可忽略,是偽線性卡爾曼濾波存在有偏性的根本原因。Ck的大小與角測量噪聲大小有關,當噪聲較小時并不明顯,而當角測量噪聲增大時,將會快速增大使得狀態估計結果迅速發散。因此為了保證目標跟蹤的精度,必須對偏差進行處理。

2 3D-UBKF

2.1 3D-UBKF算法原理

為了消除偽線性卡爾曼濾波的偏差,目前的解決思路主要分為對偏差進行估計再補償以及消除量測矩陣Hk與ηk之間相關性兩種[7]。而根據偏差補償的解決思路,只適用于角測量為小噪聲的情況,當噪聲較大時去偏效果嚴重下降,如BC-PLKF;消除量測矩陣Hk與ηk之間相關性卻能從原理上解決算法的有偏性問題,如IV-PLKF。但是,該方法的輔助變量卻是通過BC-PLKF 進行構造,受限于BC-PLKF的性能。為了消除Hk與ηk之間相關性,本文通過將噪聲項從量測矩陣中分離出來,提出了一種適用于三維場景且具有無偏性能的3D-UBKF算法。然而,將該方法應用到遠距離場景下時,發現該方法易出現估計發散的情況,為此,本文還設計了EKF作為角度濾波器幫助3D-UBKF 來改善該問題。

在小噪聲條件下,假設cosnk≈cosmk≈1,sinnk≈nk,sinmk≈mk,將其代入偽線性量測矩陣Hk中

將式(22)與式(23)代入式(6)得到

式中

將式(24)兩邊同乘對角陣diag([1/mk1,1/mk2]),于是得到

據式(21)的結果可知,此時原理性偏差

由于ˉHθ,k與ˉHφ,k分別與角測量噪聲nk、mk無關,因此E{Ck}=0,即該算法具有無偏性。整理3D-UBKF算法如表1所示。

表1 基于3D-UBKF的目標跟蹤步驟Tab.1 Target tracking steps based on 3D-UBKF

在實際應用過程中,由于dk以及觀測真值θk,φk未知,無法直接得到Gk以及參數mk1,mk2,可以采用估計值代替,這個思想與輔助變量法具有一定的相似之處。而根據輔助變量法的收斂條件,需要保證估計矩陣^Gk與真值Gk具有較強的相關性,若直接采用該方法的角度估計值作為真值輸入,那么很容易在前期由于估計不夠準確而使得與真值Gk不相關,使得目標運動狀態估計發散。EKF 是一種不具有原理性偏差的非線性濾波算法,且計算量小,對角度的估計較穩定,因此,采用EKF方法后驗估計的方位角與俯仰角作為該算法的角度真值輸入,從而得到

于是,總結3D-UBKF 算法的工作流程如圖1所示。

圖1 3D-UBKF工作流程Fig.1 3D-UBKF workflow

2.2 基于EKF的角度估計

EKF適用于非線性高斯模型,應用十分廣泛。根據第一章的模型,可以得到該系統模型為

EKF采用泰勒公式展開將非線性模型近似為線性模型,將其應用到系統(35)中,對于表2中Jk可由式(36)得到

表2 基于EKF的角度估計步驟Tab.2 AOA estimation steps based on EKF

式中

3 仿真分析

本章通過仿真實驗對算法的性能進行測試,選擇EKF,CKF(cubature Kalman filter)[15],3DIVKF(3D-instrumental variable based Kalman filter)[8]作為對比對象,分別對具有單一運動模型的目標以及組合運動模型的目標(勻速運動模型+協同轉彎模型)進行了比較。

3.1 單一運動模型目標仿真分析

仿真場景一:仿真時間長720 s,采樣時間間隔0.2 s,蒙特卡羅仿真200次。觀測平臺的出發點為坐標原點,起始速度為(0 m/s,250 m/s,0 m/s)T。為了保證觀測平臺對目標具有較高的可觀性,過程中采用協同轉彎與勻速直線運動的組合形式,無天向運動,最終設計觀測平臺的第一階段的機動策略如表3所示,重復該過程9次。

表3 航行軌跡參數Tab.3 Navigation trajectory parameters

水平面上的機動軌跡如圖2所示,起始點為坐標原點。

圖2 觀測平臺X-Y 平面運動軌跡Fig.2 Observation platform X-Y plane motion trajectory

目標的起始點位置為(60 km,80 km,10 km)T,速度為(-330 m/s,-170 m/s,1.21 m/s)T,將過程噪聲Q中的過程譜密度噪聲設置為qx=2 m2/s3,qy=2 m2/s3,qz=0.2 m2/s3。另外,設置濾波器初始值。由于估計過程中假設無先驗信息,只能通過傳感器探測范圍來確定,將濾波器初值設為1.5倍真值,初始協方差設置為625,625,6.25,6.25,6.25]),觀測噪聲協方差矩陣R與過程噪聲矩陣Q此處均設置為真值,3D-IVKF中的閾值設置為3σ。

圖3比較了目標在做勻速直線運動條件下,在不同角測量誤差條件下的算法,其中,圖3(a)對比了不同算法在500～720 s時間內的絕對位置誤差(absolute position error,APE),該指標可一定程度反映算法的收斂速度,圖3(b)對比了采樣結束時不同算法的APE,此時目標與觀測平臺距離為342 km。通過圖3(a)與圖3(b)可以看出當角測量誤差從0.1°變化到0.5°時,各算法誤差逐漸增大,而3D-UBKF與3D-IVKF 方法的誤差變化較小,但角測量誤差從0.1°到0.25°的變化過程中,CKF與EKF方法的誤差均顯著低于其他兩種基于偽線性量測方程的方法;當角測量誤差達到0.3°時,由于3D-UBKF以及3D-IVKF 對角測量誤差相對不敏感,此時3DUBKF在500～720 s時間內的時均APE 達到最小,而3D-IVKF 的誤差則與EKF,CKF 十分接近,這說明此時3D-UBKF 的收斂速度較快;而當角測量誤差達到0.5°時,3D-UBKF 的算法優勢進一步擴大,此時從精度與收斂速度上均為最優。圖4展示了算法在0.5°角測量噪聲時的跟蹤效果。

圖3 目標勻速時算法在不同角測量誤差條件下的誤差對比圖Fig.3 Error comparison of algorithm under different angular measurement error conditions when the target speed is constant

圖4 勻速目標跟蹤態勢圖Fig.4 Situation map of uniform motion target tracking

因此,根據上述仿真結果可以看出,在勻速條件下,EKF,CKF的誤差指標接近,在角測量誤差小于0.3°時均表現出優異的定位跟蹤性能,但當角測量誤差大于0.3°時,對算法的收斂速度影響較大;基于偽線性量測方程推導的3D-IVKF 與3DUBKF在小噪聲條件下定位跟蹤性能上則稍微差一些,但收斂速度與跟蹤精度對角測量誤差相對不敏感,當角測量誤差達到0.5°時,3D-UBKF 能達到同時兼顧精度與收斂速度的目的。

3.2 組合運動模型目標仿真分析

仿真場景二:觀測平臺的運動軌跡與仿真場景一保持一致,目標的起始點位置為(60 km,80 km,10 km)T,初始速度為(-330 m/s,-170 m/s,1.21 m/s)T,過程噪聲Q中的過程譜密度噪聲設置為qx=2 m2/s3,qy=2 m2/s3,qz=0.2 m2/s3。在采樣前300 s保持勻速直線運動,之后z方向高度不變,產生在水平方向順時針旋轉的勻速圓周運動,角速度為1(°)/s,持續30 s,之后繼續保持勻速直線運動。濾波器參數設置保持不變。

圖5 比較了目標在機動條件下,不同算法在不同角量測誤差條件下的誤差表現,其中,圖5(a)對比了不同算法在500～720 s時間內的時均APE,圖5(b)對比了采樣結束時不同算法的APE,此時目標與觀測平臺之間距離為305 km。在目標發生機動的條件下,可以看到各算法的性能表現仍大致保持一致。通過圖5 可以看到,非線性濾波的EKF,CKF在角測量誤差小于0.3°時,性能十分接近,優于偽線性濾波的3D-IVKF 與3D-UBKF,但隨著角測量誤差增大,誤差的變化速度也高于這兩種算法。圖5(a)可以看到,角測量誤差從0.1°到0.25°的變化過程中,CKF與EKF方法的誤差較小;當角測量誤差達到0.3°時,3D-UBKF 的誤差指標開始與EKF,CKF接近;而角測量誤差為0.35°時,EKF,CKF的誤差指標則與3D-IVKF接近。再觀察角測量誤差為0.5°時各算法的誤差指標,可以看到,EKF,CKF的各項誤差指標均高于3D-IVKF 以及3DUBKF;其中,3D-UBKF達到了最佳的跟蹤效果,在整個角測量誤差增大的過程中,3D-UBKF的絕對位置誤差以及采樣結束時刻APE均幾乎不發生變化。圖6為角測量噪聲為0.5°時各算法的跟蹤情況。

圖5 目標機動時算法在不同角測量誤差條件下的誤差對比圖Fig.5 Error comparison of algorithm under different angular measurement error conditions during target maneuvering

圖6 機動目標跟蹤情況Fig.6 Maneuvering target tracking situation

根據算法對機動目標的仿真結果來看,在機動條件下,EKF,CKF在角測量誤差小于0.3°時仍能表現出優異的定位跟蹤性能,但當角測量誤差大于0.3°時,對算法的收斂速度影響較大;基于偽線性量測方程推導的3D-IVKF 與3D-UBKF 在小噪聲條件下,定位跟蹤性能上則稍微差一些,其中,3DUBKF由于結合了EKF 的后驗結果,在小噪聲條件下保持住了EKF 的優良性能,跟蹤精度略低于EKF,而明顯優于3D-IVKF;除此之外,由于偽線性濾波對角測量誤差相對不敏感,當角測量誤差達到0.3°～0.5°時,3D-UBKF 與3D-IVKF 的跟蹤精度逐漸超過EKF 與CKF,而在大噪聲條件下,3DUBKF又表現出優于3D-IVKF 的跟蹤性能,性能指標在各算法中最優。

進一步對各算法的運行效率進行統計,以EKF一次蒙特卡羅仿真的時間作為單位時間,將其他算法的一次蒙特卡羅仿真時間與之相除作為各算法的運行時間,得到各算法相對運行時間如表4所示?？梢钥闯?在小噪聲條件下,EKF 同時兼顧了運算速度快、定位估計精度高的優點。而基于偽線性濾波改進的3D-IVKF 與3D-UBKF 由于添加了其他運算,使得運行效率有所下降,但仍明顯優于點估計形式的CKF。3D-UBKF通過犧牲一定的運行速度和小噪聲條件下的定位估計精度,達到了運行速度相對較快,且對噪聲具有較強抗干擾能力的特點。

表4 算法相對運行時間對比Tab.4 Comparison of algorithm relative running times

4 結論

本文通過仿真實驗表明:

1)基于非線性濾波理論的EKF,CKF 在小噪聲條件下性能穩定,定位估計精度高,兩種算法效果十分接近;但噪聲較大時,這兩種算法均會出現目標跟蹤精度迅速下降的問題,對角測量噪聲敏感。本文提出的3D-UBKF 相比于EKF,CKF,具有更強的抗干擾能力。

2)基于偽線性濾波的3D-IVKF 與3D-UBKF均有效改善了普通3D-PLKF 的有偏性問題,具有對角測量誤差不敏感的優點,當角測量誤差較大時,具有比EKF,CKF 更加良好的目標跟蹤性能,尤其是本文提出的3D-UBKF,相比于3D-IVKF 具有更高的目標跟蹤精度。對于百公里級別的目標,當角測量誤差從0.1°變化到0.5°,算法在仿真時間結束時均能將絕對位置誤差降低至10 km 以內。

3)3D-UBKF由于利用了EKF 對角度測量值預先進行了處理,增大了運算量,其運行效率介于3D-IVKF與CKF之間,具有與EKF同一個量級的運行速度。

綜上所述,3D-UBKF 同時兼顧了目標定位跟蹤精度、抗干擾能力以及運行速度,可以為遠距離場景下的目標跟蹤提供有效方法。