王 聘，田 敬，吳 鵬，王洋洋

（1.海軍裝備部駐連云港地區(qū)軍事代表室，江蘇 連云港 222061；2.江蘇自動(dòng)化研究所，江蘇 連云港 222061）

粒子濾波算法是解決非線性／非高斯動(dòng)態(tài)系統(tǒng)狀態(tài)估計(jì)問題最主要的方法，并成功應(yīng)用于多目標(biāo)跟蹤［1］、故障診斷、飛行器姿態(tài)估計(jì)［2］、機(jī)器人導(dǎo)航［3］以及信號(hào)處理［4］等眾多領(lǐng)域。但是，粒子退化問題，以及由其所導(dǎo)致的對(duì)突變狀態(tài)的追蹤失效是目前粒子濾波所面臨的主要問題，為此，學(xué)者們進(jìn)行了大量研究并提出了多種改進(jìn)方法：EPF［5］、UPF［6］等，雖然這些算法能夠在一定程度上解決粒子退化的問題，但對(duì)于帶有突變狀態(tài)的系統(tǒng)，還是會(huì)產(chǎn)生較大的誤差。針對(duì)帶有突變狀態(tài)的系統(tǒng)，周東華提出了強(qiáng)跟蹤濾波STPF算法［7］，在卡爾曼濾波框架下引入次優(yōu)漸消因子，提高了處理突變系統(tǒng)時(shí)的濾波精度，但仍不能夠較好地處理似然函數(shù)位于狀態(tài)空間尾部的情況。

針對(duì)上述問題，本文提出了改進(jìn)強(qiáng)跟蹤粒子濾波算法ISTPF，該算法將強(qiáng)跟蹤算法與粒子濾波算法相結(jié)合，實(shí)現(xiàn)高效采樣和強(qiáng)跟蹤功能。該算法既具有較強(qiáng)處理突變系統(tǒng)的能力，又能夠很好地解決似然函數(shù)位于狀態(tài)空間尾部的問題，實(shí)現(xiàn)了高效采樣粒子濾波。最后將ISTPF算法應(yīng)用到緊組合導(dǎo)航模型中，仿真結(jié)果證實(shí)ISTPF算法能夠得出更精確的導(dǎo)航結(jié)果，具有一定的可行性及優(yōu)越性。

1 改進(jìn)強(qiáng)跟蹤粒子濾波算法

改進(jìn)強(qiáng)跟蹤粒子濾波算法通過運(yùn)行一次強(qiáng)跟蹤濾波，從而得出一個(gè)修正項(xiàng)并用于修正粒子濾波中粒子樣本，將粒子推向高似然區(qū)域［8］，即得出一個(gè)新的重要性概率密度函數(shù)，然后從中抽樣，該過程將當(dāng)前的量測(cè)信息考慮了進(jìn)去，故所抽取的粒子樣本更接近后驗(yàn)分布，最終的計(jì)算精度得到提高。

考慮如下的非線性系統(tǒng)：

其中xt和zt分別為狀態(tài)向量和量測(cè)向量，f（·）和h（·）是已知的連續(xù)非線性函數(shù)，vt－1和wt分別為統(tǒng)計(jì)特性已知的系統(tǒng)噪聲和量測(cè)噪聲，濾波的最終目的就是根據(jù)所有量測(cè)值z(mì)＝{z1z2…zt}遞推求出當(dāng)前時(shí)刻狀態(tài)值xt。

進(jìn)而對(duì)粒子進(jìn)行加權(quán)求得狀態(tài)估計(jì)值

由此可見，重要性概率密度函數(shù)至關(guān)重要，它代表了粒子分布，同時(shí)影響著各個(gè)粒子的權(quán)重。在改進(jìn)強(qiáng)跟蹤粒子濾波中，對(duì)重要性概率密度函數(shù)進(jìn)行改進(jìn)，通過運(yùn)行一步強(qiáng)跟蹤EKF，獲得一個(gè)修正項(xiàng)

其中，Kt為t時(shí)刻濾波增益，γt為t時(shí)刻新息。此修正項(xiàng)考慮了量測(cè)信息，在減弱老數(shù)據(jù)對(duì)當(dāng)前濾波值影響的基礎(chǔ)上，使得粒子濾波在抽樣階段便權(quán)衡了量測(cè)信息，所抽取粒子更加接近真實(shí)值。

求出修正項(xiàng)Ct后，通過更新系統(tǒng)方程（1）便可得到修正后的狀態(tài)轉(zhuǎn)移方程：

因此，將重要性概率密度函數(shù)定義為pv（xtf（xt－1）－Ct），從中抽取粒子并計(jì)算出其權(quán)重，得出狀態(tài)估計(jì)值。經(jīng)此重要性概率密度函數(shù)所抽取粒子能夠更好地遍布整個(gè)函數(shù)，且更多地集中在高似然區(qū)域，有效地描述了系統(tǒng)函數(shù)，同時(shí)高似然區(qū)域粒子所占權(quán)重更大，最終經(jīng)這些高質(zhì)量粒子加權(quán)所得狀態(tài)估計(jì)值便更為準(zhǔn)確。

由上可見，給定狀態(tài)初始值x1和狀態(tài)方差初始值P1，便可遞推求出相應(yīng)時(shí)刻狀態(tài)估值＾xt。

2 緊組合導(dǎo)航模型

緊組合模型是將衛(wèi)星導(dǎo)航設(shè)備接收機(jī)的偽距測(cè)量值和偽距率測(cè)量值，與利用慣性導(dǎo)航輸出計(jì)算出的相應(yīng)偽距、偽距率的值進(jìn)行比較，得到的差值形成濾波器（ISTPF）的測(cè)量輸入值，經(jīng)組合導(dǎo)航濾波器，生成系統(tǒng)的誤差估計(jì)值，這種估計(jì)值可在每次測(cè)量更新后對(duì)慣導(dǎo)系統(tǒng)進(jìn)行修正，以提高慣性導(dǎo)航的精度。緊組合導(dǎo)航具體過程如圖1所示。

圖1 緊組合模型示意圖

1）求解載體位置速度和姿態(tài)。

根據(jù)慣性器件（陀螺和加計(jì)）測(cè)得的數(shù)據(jù)按照導(dǎo)航方程（7）解算出載體的速度，進(jìn)而得出載體的位置；按照姿態(tài)更新方程（8）解算出載體的姿態(tài)［9-10］。

地固系下的導(dǎo)航方程：

姿態(tài)更新的四元數(shù)算法

2）求解位置速度和姿態(tài)修正量

根據(jù)導(dǎo)航方程可以推導(dǎo)出的地固系下誤差狀態(tài)方程（將陀螺零偏、加計(jì)零偏、接收機(jī)鐘差、鐘差漂移誤差作為待估計(jì)值）為式（9）

接收機(jī)鐘差和鐘差漂移誤差采用以下模型進(jìn)行描述：

結(jié)合衛(wèi)星導(dǎo)航設(shè)備接收機(jī)測(cè)得的偽距、偽距率和慣導(dǎo)設(shè)備解算出的載體位置速度，以及星歷信息（衛(wèi)星位置和速度），可得濾波的量測(cè)方程為式（13）

其中，Ri、d Ri分別為衛(wèi)星導(dǎo)航設(shè)備所得第i顆衛(wèi)星的偽距和偽距率，(xIyIzI)、 (VxIVyIVzI)分別為慣導(dǎo)設(shè)備解算所得載體位置和速率， (xis yis zis)、(VisxVisyVisz)分別為第i顆衛(wèi)星的位置和速率。

用衛(wèi)星導(dǎo)航設(shè)備接收機(jī)提供的星歷數(shù)據(jù)、慣導(dǎo)設(shè)備計(jì)算得到的載體位置和速度，計(jì)算出衛(wèi)星相應(yīng)于載體的偽距RI和偽距率˙RI，把RI和˙RI與衛(wèi)星導(dǎo)航設(shè)備測(cè)量的偽距RG和偽距率˙RG相比較作為濾波器的量測(cè)值，即采用ISTPF濾波器，以公式（9）作為系統(tǒng)方程、公式（13）作為量測(cè)方程進(jìn)行濾波計(jì)算，得出狀態(tài)誤差的估計(jì)值。

3）載體位置速度和姿態(tài)修正

將2）中濾波所得狀態(tài)誤差的估計(jì)值按照式（14）和式（15）來修正1）中SINS解算出的載體狀態(tài)。

直接用濾波值修正載體位置和速度：

通過修正方向余弦矩陣修正載體的姿態(tài)：

其中，

通過上述步驟，則完成了緊組合導(dǎo)航一個(gè)周期的解算。

3 仿真及分析

濾波初值選取及相關(guān)參數(shù)設(shè)定：狀態(tài)初值X＝017×1，狀態(tài)方差P中位置取10 m、速度取0.1 m／s、姿態(tài)取0.3°、陀螺漂移20°／h、加計(jì)零漂3 mg、接收機(jī)鐘差0.5 m、接收機(jī)鐘漂0.05 m／s，系統(tǒng)噪聲方差Q中加計(jì)噪聲3 mg、陀螺噪聲2°／h、接收機(jī)鐘差噪聲0.5 m、接收機(jī)鐘漂噪聲0.05 m／s，量測(cè)噪聲R中偽距測(cè)量噪聲7 m、偽距率測(cè)量噪聲1 m／s。ISTPF濾波中粒子數(shù)設(shè)定為N＝1 000，利用計(jì)算機(jī)進(jìn)行仿真，仿真結(jié)果如下。

3.1 不同滾轉(zhuǎn)角速度下組合導(dǎo)航仿真

初始位置為緯度L＝34.25°、經(jīng)度λ＝108.97°、高度h＝100.44 m，初始速度為VN＝7.5 m／s、VU＝0 m／s、VE＝12.990 4 m／s，初始姿態(tài)為偏航角ψ＝60°、俯仰角θ＝30°。即載體在初始位置處以15 m／s的速度及30°的俯仰角朝著北偏東60°方向做勻速直線運(yùn)動(dòng)，圖2～4為滾轉(zhuǎn)角速度ωroll＝720°／s情況下載體位置、速度、姿態(tài)的誤差（緊組合導(dǎo)航所得結(jié)果與給定真值之差），表1為不同滾轉(zhuǎn)角速度下載體位置、速度、姿態(tài)誤差的統(tǒng)計(jì)值。

表1 不同滾轉(zhuǎn)角速度下9個(gè)參數(shù)誤差的均值和方差

圖2 ωroll＝720°／s情況下載體位置誤差圖

圖3 ωroll＝720°／s情況下載體速度誤差圖

圖4 ωroll＝720°／s情況下載體姿態(tài)誤差圖

3.2 不同加速度下組合導(dǎo)航仿真

初始位置為緯度L＝34.25°、經(jīng)度λ＝108.97°、高度h＝100.44 m，初始速度為VN＝0、VU＝0、VE＝0，初始姿態(tài)為偏航角ψ＝60°、俯仰角θ＝30°、滾轉(zhuǎn)角γ＝0°。即載體在初始位置處以不同的加速度及30°的仰角朝著北偏東60°方向由靜止做勻加速直線運(yùn)動(dòng)，且偏航角和俯仰角保持不變，滾轉(zhuǎn)角速度保持不變?chǔ)豶oll＝720°／s，圖5～7為加速度a＝0.4 m／s2情況下載體的位置、速度、姿態(tài)誤差，表2為不同加速度情況下載體位置、速度、姿態(tài)誤差的統(tǒng)計(jì)值。

圖6 a＝0.4 m／s2情況下載體速度誤差圖

圖7 a＝0.4 m／s2情況下載體姿態(tài)誤差圖

表2 不同加速度情況下9個(gè)參數(shù)誤差的均值和方差

由圖2～4及表1可以直觀地看出，當(dāng)載體做勻速運(yùn)動(dòng)時(shí)，在給定滾轉(zhuǎn)角速度范圍內(nèi)，緊組合導(dǎo)航解算結(jié)果誤差均較小，且不同滾轉(zhuǎn)角速度對(duì)解算精度影響差別不大，位置誤差不超過0.3 m，速度誤差不超過0.05 m／s，姿態(tài)誤差不超過0.05°；由圖5～7及表2可以得出，當(dāng)載體做加速運(yùn)動(dòng)時(shí)，在給定的加速度范圍內(nèi)，緊組合導(dǎo)航解算結(jié)果誤差均較小，不同加速度對(duì)組合導(dǎo)航位置、速度及姿態(tài)的精度影響差別不大。位置誤差不超過0.3 m，速度誤差不超過0.02 m／s，姿態(tài)誤差不超過0.05°。

4 結(jié)束語

本文將強(qiáng)跟蹤濾波算法與粒子濾波算法相結(jié)合，采用通過強(qiáng)跟蹤濾波產(chǎn)生修正項(xiàng)，并以此修正粒子濾波中粒子樣本的思想，提出了改進(jìn)強(qiáng)跟蹤粒子濾波算法，提高了算法的精度，并將ISTPF濾波器應(yīng)用于車輛緊組合導(dǎo)航中，得出較高的導(dǎo)航精度，且所得誤差方差較小，即該算法具有較高的穩(wěn)定性，能夠滿足導(dǎo)航定位定姿的要求，具有較強(qiáng)的實(shí)用性及優(yōu)越性。