基于改進(jìn)迭代擴(kuò)展卡爾曼濾波的3星時頻差測向融合動目標(biāo)跟蹤方法

曲志昱 王超然 孫 萌

(哈爾濱工程大學(xué) 哈爾濱 150001)

(先進(jìn)船舶通信與信息技術(shù)工業(yè)和信息化部重點實驗室 哈爾濱 150001)

1 引言

星載無源定位系統(tǒng)因其具有探測距離遠(yuǎn)、監(jiān)控范圍廣、不受地域國界影響、隱蔽性能好等優(yōu)勢，在電子對抗、偵察監(jiān)視等領(lǐng)域[1–3]發(fā)揮著越來越重要的作用。近年來，隨著星載電子偵察技術(shù)的發(fā)展，其定位體制日趨多樣化[4]，定位目標(biāo)也從最初的地面固定目標(biāo)延伸至空域運(yùn)動目標(biāo)[5]。傳統(tǒng)星載時頻差無源定位算法應(yīng)用于高速運(yùn)動目標(biāo)時，常忽略目標(biāo)的運(yùn)動速度，這會導(dǎo)致頻差定位結(jié)果出現(xiàn)較大偏差[6]，另外，高速運(yùn)動的目標(biāo)一般都位于高空而非地表，但傳統(tǒng)星載定位普遍利用假設(shè)高程的球面模型或橢球面模型，即通過假設(shè)目標(biāo)高程為0的方式將地球表面視作定位面進(jìn)而實現(xiàn)目標(biāo)定位[7]。因此，對于搭載于高速運(yùn)動平臺(如戰(zhàn)機(jī)、導(dǎo)彈等)上的輻射源目標(biāo)而言，這些算法將不再適用。

文獻(xiàn)[8]提出一種基于高程值估計的3星時頻差運(yùn)動輻射源定位方法，使用解析法實現(xiàn)了動目標(biāo)的測速和定位，但精度較低，且隨著輻射源高程的增加，定位、測速的誤差會越來越大，文獻(xiàn)[9]研究一種基于擴(kuò)展卡爾曼濾波(Extended Kalman Filter,EKF)的時頻差聯(lián)合定位跟蹤方法，改善了高速目標(biāo)的速度帶來的測量誤差大的問題，但仍將目標(biāo)高程假設(shè)為已知量參與跟蹤計算；文獻(xiàn)[10]提出一種基于時差頻差的多站多次定位跟蹤濾波方法，但該算法應(yīng)用于星載3維定位時至少需要4個觀測站，這將大大提升探測成本和衛(wèi)星組網(wǎng)難度；文獻(xiàn)[11]給出一種高精度測向輔助的3星時差定位算法，該算法通過引入方位角信息，使定位目標(biāo)不再局限于地面輻射源，但由于缺乏對目標(biāo)速度的直接觀測，因此算法對動目標(biāo)定位跟蹤的效果較差。針對上述問題，本文在3星時頻差定位系統(tǒng)基礎(chǔ)上，提出了一種利用主星1維測向信息的改進(jìn)迭代擴(kuò)展卡爾曼濾波(Iterative Extended Kalman Filter,IEKF)融合跟蹤算法，算法在EKF方法中加入迭代，后采用LM(Levenberg-Marquardt)方法[12]對迭代過程的狀態(tài)更新進(jìn)行優(yōu)化，并給出了迭代終止條件。仿真結(jié)果表明，算法可以實現(xiàn)對未知高程運(yùn)動目標(biāo)的跟蹤、定位和測速，且性能優(yōu)于傳統(tǒng)EKF和IEKF算法。

2 時差頻差測向融合跟蹤模型

3星融合體制定位系統(tǒng)由1顆主星和2顆輔星構(gòu)成，具體形式如圖1所示，其中，α為地球自轉(zhuǎn)時間角，f為真近心角(真近點角)，Ω為升交點赤經(jīng)，w為近地點幅角，i為軌道傾角。系統(tǒng)的主星可通過干涉儀測得目標(biāo)輻射源到達(dá)主星的方位角信息(Direction Of Arrival,DOA)，并可以測得輻射源信號到達(dá)該衛(wèi)星與兩個輔星的到達(dá)時間差(Time Difference Of Arrival,TDOA)與到達(dá)頻率差(Frequency Difference Of Arrival,FDOA)信息。

圖1 TDOA-FDOA-DOA跟蹤定位系統(tǒng)示意圖

設(shè)置衛(wèi)星軌道為圓軌道，定義星體坐標(biāo)系原點為衛(wèi)星質(zhì)心，X軸指向衛(wèi)星前進(jìn)方向，Z軸指向地球質(zhì)心，Y軸與另外兩軸成右手關(guān)系。為簡化討論，假設(shè)衛(wèi)星姿態(tài)角為0，天線陣面安裝角也為0，即主星的天線陣面坐標(biāo)系與定義的主星星體坐標(biāo)系重合。

衛(wèi)星定位中，描述輻射源坐標(biāo)時常采用WGS-84系(ECEF坐標(biāo)系)，但參數(shù)的測量卻是在衛(wèi)星的天線陣面上進(jìn)行的，因此為方便分析，應(yīng)通過坐標(biāo)系轉(zhuǎn)換將輻射源和兩顆輔星的位置轉(zhuǎn)換至主星的天線陣面坐標(biāo)系下(本文中即主星星體坐標(biāo)系)。ECEF系至主星的星體坐標(biāo)系間轉(zhuǎn)換方式為

其中，(xe,ye,ze)為輻射源或衛(wèi)星在ECEF坐標(biāo)系下的坐標(biāo)，(xt,yt,zt)為其在主星的星體坐標(biāo)系下的坐標(biāo)，(xe0,ye0,ze0)為主星在ECEF坐標(biāo)系下的坐標(biāo)，而(xt0,yt0,zt0)為主星在其本體坐標(biāo)系下的坐標(biāo)(0,0,0)，因此可在計算中省略，R1為ECEF坐標(biāo)系到ECI坐標(biāo)系的轉(zhuǎn)換矩陣，忽略對其影響較小的章動和歲差，可由仿真時刻儒勒日計算出地球自轉(zhuǎn)時間角α得 到；R2為ECI坐標(biāo)系到主星的星體坐標(biāo)系的轉(zhuǎn)換矩陣，由軌道根數(shù)f,Ω,w,i得到。由式(1)可知，星體坐標(biāo)系至ECEF坐標(biāo)系的轉(zhuǎn)換關(guān)系為

設(shè)置輻射源為一高程未知的等高程勻速運(yùn)動目標(biāo)，在k時刻，其狀態(tài)方程為

其待定的位置和速度分別為(xk,yk,zk)和(vxk,vyk,vzk)，則該時刻輻射源運(yùn)動狀態(tài)矢量Xk=[xk,yk,zk,vxk,vyk,vzk]T,A為狀態(tài)轉(zhuǎn)移矩陣，定義為

其中，Δt為采樣周期，wk-1為相應(yīng)的過程噪聲，其對應(yīng)協(xié)方差為Qk-1。

方程組式(7)前3式為測量方程，其中，c為光速，tn,k和fn,k分別表示輔星n在k時刻與主星的時差(TDOA)、頻差(FDOA)觀測量；θk為主星的1維干涉儀測得的k時刻到達(dá)角觀測量，為簡化討論，這里假設(shè)該角為來波方向與ECEF系的X軸方向的夾角，eX與eY為主星的星體系坐標(biāo)軸的單位矢量，eX=[1,0,0]T,eY=[0,1,0]T。在跟蹤過程中，輻射源始終做等高程勻速運(yùn)動，因此其速度和位置矢量應(yīng)滿足正交性，由此得出方程組第4式。dtk,dfk,dθk,dOk為相互獨(dú)立的高斯白噪聲，其中，dtk,dfk,dθk分別代表時差、頻差和角度的觀測誤差，dOk為目標(biāo)位置與速度的正交約束誤差。

3 改進(jìn)IEKF跟蹤算法

3.1 濾波初值計算方法

工程上，擴(kuò)展卡爾曼濾波器(EKF)是一種應(yīng)用廣泛的非線性濾波器，利用EKF對輻射源目標(biāo)的位置和速度進(jìn)行濾波跟蹤是解決此類問題的常用方式，對于EKF而言，一個理想的濾波初值可以帶來更小的截斷誤差和更快的收斂速度，初始狀態(tài)選擇不當(dāng)會導(dǎo)致濾波器發(fā)散，因此濾波初值選取的優(yōu)劣對濾波結(jié)果有著直接影響。由定位方程組式(7)可知，時差和角度觀測方程與目標(biāo)速度無關(guān)，不會由于運(yùn)動目標(biāo)速度引入誤差。因此，在研究運(yùn)動目標(biāo)時，首先融合TDOA和DOA兩種定位信息來得到濾波初值。

濾波初值的計算在主星的星體坐標(biāo)系下進(jìn)行，首先設(shè)主星星體坐標(biāo)系下輻射源初始位置為ub,t=[xb,t,yb,t,zb,t]，3 顆衛(wèi)星初始位置為sb,n=[xb,n,yb,n,zb,n]，其中n=0時表示主星初始位置，其在星體坐標(biāo)系中的坐標(biāo)為(0,0,0),n=1,2時分別表示兩顆輔星初始位置。設(shè)主星與目標(biāo)輻射源的距離為r0，輔星與目標(biāo)輻射源的距離為rn,n=1,2，則有

在主星的星體系下，有如式(9)測量方程

其中，tn,0分別表示輔星n在初始時刻與主星的時差(TDOA)觀測量；θ0為主星的1維干涉儀測得的初始時刻到達(dá)角觀測量(來波方向與主星星體坐標(biāo)系X軸夾角)，dt0,dθ0分別代表初始時刻時差和角度的觀測誤差。

由方程組式(9)中的第1式可得

聯(lián)立定位方程組式(9)第2式與式(10)可得

其中，n=1或2，進(jìn)而有

將式(12)和方程組式(9)第2式代入式(8)，可得到關(guān)于r0的一元二次方程

由式(13)解得r0的兩個根，去除鏡像模糊值后，將得到的解代入式(12)和定位方程組式(9)第2式，解出目標(biāo)在主星星體系下的初始定位坐標(biāo)，將該位置坐標(biāo)由式(2)坐標(biāo)轉(zhuǎn)換至ECEF坐標(biāo)系下，即得到濾波初值u0。對于協(xié)方差矩陣，分別對式(2)及定位方程組式(9)的兩式求解全微分，可得到初始位置估計的協(xié)方差矩陣

其中，d iag為以相應(yīng)元素為對角線元素的對角陣，σt表示時差的測量精度，σθ表示方位角的測量精度，矩陣M為

目標(biāo)運(yùn)動狀態(tài)此時未知，故將初始速度置為0，可得濾波初始值為(,0,0,0)，初始協(xié)方差矩陣為

其中，vmax為目標(biāo)速度可能達(dá)到的最大值。

3.2 基于融合定位體制的EKF算法

卡爾曼濾波是一種可以對線性系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計的算法，但融合體制3星定位系統(tǒng)是一個非線性系統(tǒng)[13]，為解決這一問題，本文首先給出了融合定位系統(tǒng)的擴(kuò)展卡爾曼濾波(Extended Kalman Filter,EKF)方法，其基本思想是將非線性系統(tǒng)線性化，對非線性函數(shù)的泰勒展開式進(jìn)行1階線性化截斷，忽略其高階項，從而將非線性問題轉(zhuǎn)化為線性，然后進(jìn)行卡爾曼濾波。

首先設(shè)系統(tǒng)狀態(tài)方程和觀測方程分別為

其中，A是狀態(tài)轉(zhuǎn)移矩陣，狀態(tài)轉(zhuǎn)換函數(shù)h指代k個時間采樣點下狀態(tài)量Xk與觀測量Zk之間的函數(shù)關(guān)系，wk-1為過程噪聲，Rk為測量噪聲。

EKF過程如下：

目標(biāo)狀態(tài)預(yù)測

預(yù)測協(xié)方差矩陣

卡爾曼增益矩陣計算

更新目標(biāo)狀態(tài)

更新協(xié)方差矩陣

3.3 改進(jìn)的迭代卡爾曼濾波(IEKF)算法

EKF算法應(yīng)用在非線性系統(tǒng)時，會產(chǎn)生2次項以上的截斷誤差，尤其在線性模型誤差較大時，可能導(dǎo)致濾波結(jié)果誤差較大。IEKF算法在EKF的基礎(chǔ)上，在測量更新階段加入多次迭代，重復(fù)對觀測方程進(jìn)行線性化，最終得到更為準(zhǔn)確的濾波結(jié)果，由文獻(xiàn)[14]可定義其代價函數(shù)

IEKF的迭代過程可視為利用高斯-牛頓方法迭代求解式(24)的極小值點，但由于線性化帶來的截斷誤差等因素，往往只能求出其非零極值點，即等價于將高斯-牛頓方法應(yīng)用于小殘差問題的迭代優(yōu)化。對于此類問題，IEKF算法存在著不穩(wěn)定性，主要表現(xiàn)為對狀態(tài)的觀測更新不能保障誤差的一致減小，對協(xié)方差陣估計值比真實值偏低，進(jìn)而影響對觀測信息的有效利用。為解決這一問題，本文算法利用L-M方法調(diào)整預(yù)測協(xié)方差陣，以保證算法具有全局收斂性，算法核心是在每次迭代過程中使用阻尼因子μi對協(xié)方差預(yù)測矩陣進(jìn)行修正，以修正后的協(xié)方差矩陣進(jìn)行觀測的迭代更新[15–17]。具體過程如下：

目標(biāo)狀態(tài)預(yù)測

預(yù)測協(xié)方差矩陣

迭代開始

L-M方法修正協(xié)方差矩陣

卡爾曼增益矩陣計算

更新目標(biāo)狀態(tài)

更新協(xié)方差矩陣

當(dāng)代價函數(shù)隨迭代次數(shù)增加而小于一定限度時，即認(rèn)為收斂已至極值點附近，迭代終止條件可寫為

其中，ε是預(yù)先設(shè)置的門限值。

整個過程的具體流程圖如圖2所示。

圖2 改進(jìn)IEKF流程示意圖

4 仿真結(jié)果與分析

利用融合定位體制對輻射源進(jìn)行定位時，衛(wèi)星的編隊方式將對定位結(jié)果產(chǎn)生影響[18]，設(shè)3顆衛(wèi)星軌道高度均為800 km，星間距100 km；目標(biāo)初始位置為(28.73°E,8.61°N)，高程為10 km，目標(biāo)運(yùn)動狀態(tài)方程如式(5)所示，做速度為400 m/s勻速直線運(yùn)動。觀測時長為100 s，觀測時間間隔為1 s，時差測量誤差為20 ns，多普勒頻率測量誤差為3 Hz，測向誤差為0.25°。圖3、圖4分別給出了3星同軌和2星同軌1星異軌時的融合定位體制的GDOP圖，圖中的誤差單位均為km。由圖中可見在融合定位體制中，同軌3星的定位誤差情況優(yōu)于2星同軌1星異軌情況，這是由于本算法的主要誤差來自測角誤差，而3星同軌的編隊方式在主星星體系下聯(lián)立時差方程求解輻射源位置時所受測角誤差影響相對三角構(gòu)型要小得多，因此雖然對于時差測量來說等腰三角構(gòu)型的誤差情況要優(yōu)于同軌3星構(gòu)型[19]，但綜合考慮總體誤差，3星同軌編隊方式依然更優(yōu)。

圖3 3星同軌GDOP分布

圖4 2星同軌1星異軌GDOP分布

綜合考慮衛(wèi)星傳感器的測量參數(shù)誤差和覆蓋范圍情況，本文的算法更適合3星同軌編隊的應(yīng)用場景。圖5—圖8仿真分析了3星TDOA/FDOA系統(tǒng)的EKF跟蹤方法、3星TDOA/DOA系統(tǒng)的EKF跟蹤方法以及本文的融合跟蹤系統(tǒng)EKF方法對于未知高程的運(yùn)動目標(biāo)位置、速度的跟蹤性能，并將融合跟蹤系統(tǒng)的EKF方法與改進(jìn)IEKF方法進(jìn)行了比較。

圖5 目標(biāo)定位跟蹤結(jié)果

圖6 目標(biāo)高程跟蹤結(jié)果

圖7 目標(biāo)位置估計誤差

圖8 目標(biāo)速度估計誤差

從圖5、圖6可以看出，在未知高程的情況下，本文所提融合定位算法可以實現(xiàn)運(yùn)動輻射源目標(biāo)的跟蹤定位。從圖7可以看出，本文融合定位系統(tǒng)的定位精度優(yōu)于3星TDOA/DOA定位系統(tǒng)，且改進(jìn)IEKF的融合跟蹤方法定位精度略高于EKF算法。而由于對目標(biāo)高程估計誤差大，3星TDOA/FDOA定位性能明顯不如其他算法，且其誤差會隨著目標(biāo)高程增加而變得更大，4種算法收斂速度都較為緩慢，這是時差或頻差的測量方程相對于目標(biāo)位置的強(qiáng)非線性所導(dǎo)致的，在跟蹤時加入迭代能一定程度上緩解這一問題。由圖8可以看出，由于缺乏對速度參數(shù)的直接觀測，3星TDOA/DOA定位系統(tǒng)對于速度的跟蹤誤差較大，而加入FDOA觀測量后，對速度的跟蹤性能明顯提升。而由于定位初值偏差較大，3星TDOA/FDOA定位系統(tǒng)收斂速度明顯不如本文提出的融合定位系統(tǒng)。

5 結(jié)束語

本文針對未知高程的運(yùn)動目標(biāo)跟蹤問題提出了一種基于3星TDOA/FDOA/DOA的融合定位方法，并針對傳統(tǒng)EKF方法定位跟蹤精度低、收斂慢的問題，提出了改進(jìn)的IEKF融合跟蹤方法。首先利用3星時差信息和主星1維測向信息得到濾波初值及其協(xié)方差陣，再利用改進(jìn)的IEKF結(jié)合3星頻差信息進(jìn)行濾波跟蹤。仿真結(jié)果表明，在未知高程信息的情況下，算法可以實現(xiàn)運(yùn)動目標(biāo)的高精度位置跟蹤，并能獲得目標(biāo)的高精度速度估計。