改進(jìn)粒子濾波的無(wú)人機(jī)航跡預(yù)測(cè)方法*

曾 潤(rùn)， 田 杰， 江 虹， 王智寧

(1.西南科技大學(xué) 信息工程學(xué)院，四川 綿陽(yáng) 621010；2.中國(guó)工程物理研究院 電子工程研究所，四川 綿陽(yáng) 621900)

0 引 言

隨著無(wú)人機(jī)(unmanned aerial vehicle,UAV)技術(shù)與應(yīng)用的快速發(fā)展，無(wú)人機(jī)正成為很多場(chǎng)景執(zhí)行任務(wù)的最佳選擇[1,2]。對(duì)無(wú)人機(jī)航跡的準(zhǔn)確預(yù)測(cè)是保障無(wú)人機(jī)安全、有效飛行的重要基礎(chǔ)，無(wú)人機(jī)航跡預(yù)測(cè)在無(wú)人機(jī)跟蹤[3]、事故預(yù)警[4]、航跡規(guī)劃[5]等方面發(fā)揮著重要的作用。

模型濾波是無(wú)人機(jī)航跡預(yù)測(cè)的重要方法。文獻(xiàn)[6]指出，在現(xiàn)有狀態(tài)估計(jì)算法中，粒子濾波(particle filtering,PF)算法是基于遞推貝葉斯(Bayes)濾波估計(jì)的一種近似最優(yōu)估計(jì)方法，更適用于無(wú)人機(jī)這類(lèi)具有非線性運(yùn)動(dòng)特征的系統(tǒng)模型。如文獻(xiàn)[7]基于粒子濾波通過(guò)對(duì)無(wú)人機(jī)的俯仰角、橫滾角、偏航角的預(yù)測(cè)間接實(shí)現(xiàn)對(duì)無(wú)人機(jī)的航跡預(yù)測(cè)；文獻(xiàn)[8]提出一種結(jié)合粒子濾波與長(zhǎng)短期記憶(long short-term memory,LSTM)網(wǎng)絡(luò)的AGV(automated guided vehicle)位置持續(xù)估計(jì)算法。但傳統(tǒng)粒子濾波算法存在權(quán)值退化、樣本貧化等問(wèn)題[9]。為此文獻(xiàn)[10]使用無(wú)跡卡爾曼濾波(unscented Kalman filtering,UKF)產(chǎn)生建議密度函數(shù)，提出一種改進(jìn)的無(wú)跡卡爾曼粒子濾波(unscented Kalman particle filtering)算法，一定程度上提高了預(yù)測(cè)精度。但由于UKF算法的無(wú)跡變換(unscented transform,UT)需要對(duì)協(xié)方差矩陣做Cholesky分解，這要求矩陣必須是正定的，對(duì)于無(wú)人機(jī)這類(lèi)多維狀態(tài)模型在矩陣計(jì)算中很有可能迭代為非正定，導(dǎo)致算法在運(yùn)行過(guò)程中極易崩潰。文獻(xiàn)[11]將粒子濾波與神經(jīng)網(wǎng)絡(luò)相結(jié)合，一定程度上增加了粒子的多樣性。但算法復(fù)雜度太高達(dá)不到實(shí)時(shí)性要求，不適用于無(wú)人機(jī)航跡預(yù)測(cè)。

針對(duì)以上問(wèn)題，本文基于擴(kuò)展卡爾曼濾波(extended Kalman filtering,EKF)結(jié)合權(quán)重優(yōu)化思想[12]與數(shù)據(jù)融合思想，提出一種基于數(shù)據(jù)融合的擴(kuò)展粒子濾波(data-fusioned extended particle filtering,DFEPF)算法，建立航跡預(yù)測(cè)模型，并仿真對(duì)比三種常用非線性濾波方法，驗(yàn)證本文改進(jìn)方法對(duì)無(wú)人機(jī)航跡預(yù)測(cè)的有效性。

1 航跡預(yù)測(cè)系統(tǒng)模型

獲取到每個(gè)觀測(cè)站傳感器的預(yù)測(cè)結(jié)果后，本文使用標(biāo)準(zhǔn)線性最小方差加權(quán)融合規(guī)則來(lái)計(jì)算最佳融合估計(jì)值，如式(1)所示

(1)

式中ai為權(quán)重系數(shù)，計(jì)算公式為式(2)

(2)

X(k+1)=φX(k)+w(k)

(3)

其中，φ為狀態(tài)轉(zhuǎn)移矩陣。假設(shè)觀測(cè)站SN的坐標(biāo)位置為(xN,yN,zN)，則可得到該觀測(cè)站相對(duì)于無(wú)人機(jī)的距離dN和該觀測(cè)站相對(duì)于無(wú)人機(jī)的俯仰角α與橫向角β，建立系統(tǒng)觀測(cè)方程

Z(k)=h(X(k))+v(k)

(4)

h(X(k))=

2 改進(jìn)粒子濾波算法分析

2.1 改進(jìn)粒子濾波算法原理

粒子濾波是非線性遞推Bayes估計(jì)問(wèn)題的一種實(shí)現(xiàn)方法，其存在嚴(yán)重的粒子退化問(wèn)題，若干次迭代之后大部分粒子的權(quán)值小到可忽略不計(jì)，使權(quán)重更新失效。本文分別從推薦密度函數(shù)、重采樣兩個(gè)步驟對(duì)傳統(tǒng)粒子濾波算法進(jìn)行改進(jìn)。

首先,在重要性采樣階段中采用EKF算法融合最新的觀測(cè)值Zk為每個(gè)粒子計(jì)算其均值與協(xié)方差，通過(guò)以該均值與方差為參數(shù)的正態(tài)分布來(lái)近似建議密度函數(shù)，即式(5)。建議密度函數(shù)生成詳細(xì)步驟見(jiàn)2.2節(jié)。式(5)如下

(5)

(6)

2.2 改進(jìn)粒子濾波算法步驟

2)重要性采樣：使用EKF算法計(jì)算粒子均值與協(xié)方差(包含預(yù)測(cè)步)：

(7)

(8)

(9)

4)求粒子期望得到k時(shí)刻預(yù)測(cè)結(jié)果，返回步驟(2)直到完成所有迭代采樣。

3 仿真分析

3.1 航跡預(yù)測(cè)性能分析

設(shè)粒子數(shù)N=200，觀測(cè)站SA(100,200,0),SB(800,800,0),SC(100,900,0)。觀測(cè)噪聲協(xié)方差PR1=diag(1002,0.12,0.12)，PR2=diag(1102,0.12,0.12)，PR3=diag(1202,0.12,0.12)。由式(1)、式(2)得多觀測(cè)站DFEPF算法的預(yù)測(cè)結(jié)果

(10)

采用式(11)所示均方根(root mean squre,RMS)誤差衡量預(yù)測(cè)偏差。對(duì)比傳統(tǒng)算法單觀測(cè)站預(yù)測(cè)結(jié)果、每個(gè)采樣點(diǎn)RMS偏差、無(wú)人機(jī)多個(gè)預(yù)測(cè)狀態(tài)與真實(shí)值的偏差均值得到如圖1～圖3所示結(jié)果。式(11)如下

Xreal(2,i))2+(Xfilter(3,i)-Xreal(3,i))2)-1/2/M

(11)

圖1 預(yù)測(cè)偏差(粒子數(shù)N=200)

圖2 預(yù)測(cè)參數(shù)偏差對(duì)比(粒子數(shù)N=200)

圖3 不同噪聲方差預(yù)測(cè)誤差對(duì)比

圖4可看出，DFEPF相比其它算法能夠更好地預(yù)測(cè)出無(wú)人機(jī)真實(shí)狀態(tài)；由圖1可看出，DFEPF相比其它算法每個(gè)采樣點(diǎn)的預(yù)測(cè)偏差更小；由圖2可看出，DFEPF算法比EKF、UKF、PF算法多個(gè)參數(shù)的預(yù)測(cè)偏差均值都要小。對(duì)比4種算法的不同濾波參數(shù)如表1所示。

圖4 多種算法預(yù)測(cè)結(jié)果對(duì)比(粒子數(shù)N=200)

表1 多種算法不同濾波參數(shù)對(duì)比

從表1不難看出，通過(guò)EKF算法協(xié)助重要性采樣，并引入權(quán)重漂移思想的DFEPF算法的有效粒子Neff數(shù)相比基本粒子濾波的有所提高，有效削弱了粒子退化，提高了預(yù)測(cè)精度。由仿真結(jié)果可粗略統(tǒng)計(jì)出：DFEPF算法比EKF、UKF、PF的均方根偏差均值整體上分別減小了48.46 %,41.92 %,40.22 %；平均有效粒子Neff相比基本粒子濾波算法增多63.72 %。

3.2 航跡預(yù)測(cè)算法穩(wěn)定性與時(shí)間復(fù)雜度分析

設(shè)置不同的噪聲方差Rc，粒子數(shù)N，將采樣次數(shù)增大到M=400。計(jì)算RMS預(yù)測(cè)誤差如圖3所示。記錄粒子數(shù)N=200時(shí)，各種濾波算法的單步預(yù)測(cè)時(shí)間如圖5所示。

圖5 單步預(yù)測(cè)時(shí)間對(duì)比

由圖3可看出：當(dāng)N=200，Rc=5R或Rc=8R時(shí)，當(dāng)Rc=5R，N=200或N=400時(shí)，基本PF算法均會(huì)在不同的時(shí)間開(kāi)始發(fā)散，而DFEPF算法始終處于收斂的狀態(tài)。多次取不同噪聲方差Rc與粒子數(shù)目N實(shí)驗(yàn)，總結(jié)出本文提出的DFEPF相比基本PF算法更加穩(wěn)定；DFEPF算法由于涉及生成建議密度函數(shù)、重采樣、數(shù)據(jù)融合等步驟，相比傳統(tǒng)非線性濾波算法時(shí)間復(fù)雜度略高，但整體上仍可達(dá)到無(wú)人機(jī)狀態(tài)監(jiān)測(cè)的實(shí)時(shí)性要求[15]。

4 結(jié) 論

本文針對(duì)無(wú)人機(jī)非線性航跡難以預(yù)測(cè)的問(wèn)題，提出數(shù)據(jù)融合的改進(jìn)粒子濾波航跡預(yù)測(cè)方法。仿真試驗(yàn)表明：改進(jìn)算法較EKF、UKF、PF算法的均方根偏差均值整體上分別減小48.46 %,41.92 %,40.22 %；平均有效粒子較基本PF算法增多63.72 %；較基本PF算法更加穩(wěn)定；較傳統(tǒng)非線性濾波算法更耗時(shí)，但能夠滿足航跡預(yù)測(cè)實(shí)時(shí)性要求，可為三維航跡預(yù)測(cè)研究提供參考。未來(lái)的工作將針對(duì)重采樣與重要性采樣等步驟進(jìn)一步優(yōu)化。