洪晉夫，劉錦昌

（1.山西省交通科學(xué)研究院，太原 030006；2.空軍工程大學(xué)防空反導(dǎo)學(xué)院，西安 710051）

0 引言

全球定位系統(tǒng)［1］具有全天候?qū)Ш?、?shí)時(shí)性強(qiáng)、定位精度高的優(yōu)點(diǎn)，但是也具有數(shù)據(jù)頻率較低，受衛(wèi)星信號(hào)強(qiáng)度影響大以及數(shù)據(jù)可靠性低的缺點(diǎn)。導(dǎo)航推算系統(tǒng)［2］是一種利用方位和速度信息計(jì)算汽車位置信息的自主導(dǎo)航系統(tǒng)，在較短時(shí)間里具有高精度、高頻率、高可靠性的特點(diǎn)，但是隨著時(shí)間的增長(zhǎng)，導(dǎo)航提算系統(tǒng)會(huì)隨著自身誤差的積累而出現(xiàn)發(fā)散現(xiàn)象，需要外部導(dǎo)航定位信息提供信息校準(zhǔn)，全球定位系統(tǒng)恰恰滿足導(dǎo)航推算的長(zhǎng)時(shí)間導(dǎo)航信息校準(zhǔn)要求。兩種系統(tǒng)的深度融合可以取長(zhǎng)補(bǔ)短，發(fā)揮自身優(yōu)勢(shì)，為汽車提供長(zhǎng)時(shí)間可靠精確導(dǎo)航定位信息。近年來(lái)這種組合導(dǎo)航定位系統(tǒng)［3-5］已經(jīng)逐漸成為理論研究的熱點(diǎn)問(wèn)題。

組合導(dǎo)航定位系統(tǒng)是一種高維非線性系統(tǒng)，對(duì)于組合導(dǎo)航定位系統(tǒng)的濾波算法的研究近年來(lái)也取得了很多成果。擴(kuò)展卡爾曼濾波［6-7］通過(guò)對(duì)非線性系統(tǒng)進(jìn)行一階泰勒展開(kāi)進(jìn)行局部線性化來(lái)進(jìn)行濾波，具有計(jì)算量適中，迭代估計(jì)方便的優(yōu)點(diǎn)，但是當(dāng)系統(tǒng)的非線性較高時(shí)會(huì)出現(xiàn)估計(jì)的誤差較大甚至發(fā)散的缺點(diǎn)，并且當(dāng)系統(tǒng)的維數(shù)較高并且較復(fù)雜時(shí)，計(jì)算雅克比矩陣的難度加大甚至無(wú)法求取，計(jì)算效率降低，無(wú)法滿足組合導(dǎo)航系統(tǒng)的要求。粒子濾波［8-13］具有對(duì)非線性高維系統(tǒng)適應(yīng)性強(qiáng)的特點(diǎn)，文獻(xiàn)［11］提出利用擴(kuò)展卡爾曼濾波和粒子濾波結(jié)合的擴(kuò)展卡爾曼粒子濾波算法，改善了擴(kuò)展卡爾曼濾波的性能，但是由于依然采用了線性化處理，濾波精度有待提高。無(wú)跡卡爾曼濾波［14-15］采用無(wú)跡變換預(yù)測(cè)下一時(shí)刻的值，避免了線性化的過(guò)程，提高了運(yùn)算精度。文獻(xiàn)［16］將無(wú)跡卡爾曼濾波與粒子濾波結(jié)合，用無(wú)跡卡爾曼濾波生成粒子濾波的采樣點(diǎn)，提出了無(wú)跡粒子濾波算法，提高了算法的精度。但是無(wú)跡粒子濾波同樣存在隨著系統(tǒng)維數(shù)提高，計(jì)算量上升很快，對(duì)于組合導(dǎo)航這種對(duì)實(shí)時(shí)性要求很高的系統(tǒng)，計(jì)算效率遠(yuǎn)遠(yuǎn)滿足不了要求。本文將約束方程引入無(wú)跡粒子濾波算法中，提出一種改進(jìn)的約束粒子濾波算法，約束方程的引入可以降低系統(tǒng)的維數(shù)，使算法的計(jì)算量大大降低，實(shí)時(shí)性提高，并且保證了運(yùn)算的精度，通過(guò)汽車導(dǎo)航定位系統(tǒng)的仿真，驗(yàn)證了算法的優(yōu)越性。

1 約束無(wú)跡粒子濾波算法

1.1 CUPF算法設(shè)計(jì)

非線性系統(tǒng)可以用下式表述：

式中，xk是k時(shí)刻狀態(tài)變量，zk是測(cè)量值，f（·）和h（·）為非線性函數(shù)，wk是過(guò)程噪聲，vk是量測(cè)噪聲。

構(gòu)造狀態(tài)量xk的約束方程為

其中，D與d分別是狀態(tài)變量的約束矩陣與約束向量。約束無(wú)跡粒子濾波算法過(guò)程如下：

1）初始值，令k=0，從先驗(yàn)概率函數(shù)p（x0i）中取一個(gè)樣本 x0i，i=0，1，2，…，2n。

2）狀態(tài)擴(kuò)維

其中，Q與R分別wk與vk的協(xié)方差矩陣。

3）令 k=1，2，…

（1）重要采樣

i=0，1，2，…，2n 時(shí)，采用無(wú)跡卡爾曼濾波更新粒子。

①粒子值

②一步預(yù)測(cè)

③更新值

（2）重采樣

Neff為有效粒子數(shù)，NT為有效粒子數(shù)閾值，如果Neff＜ NT，則進(jìn)行重采樣，重采樣之后新的粒子為，歸一化權(quán)值為。

（3）結(jié)果

4）約束值計(jì)算

可得CUPF濾波算法的狀態(tài)值。

2 CUPF在汽車導(dǎo)航中的應(yīng)用

2.1 汽車導(dǎo)航系統(tǒng)

2.1.1 組合系統(tǒng)狀態(tài)方程

式中，F(xiàn)是狀態(tài)轉(zhuǎn)移陣，U是控制陣，W是噪聲。

2.1.2 量測(cè)方程

汽車導(dǎo)航系統(tǒng)的量測(cè)值為

式中，xe和xn分別為全球定位系統(tǒng)顯示的東向和北向的位置，ω為電子羅盤(pán)顯示角速率，s為里程表顯示數(shù)。量測(cè)方程如式（31）

式中

2.2 約束方程

2.2.1 運(yùn)動(dòng)約束方程

根據(jù)運(yùn)動(dòng)學(xué)定理中位移、速度和加速度之間的關(guān)系可得汽車運(yùn)動(dòng)滿足下式

因此，運(yùn)動(dòng)約束為

式中，矩陣D1與向量d1分別是：

2.2.2 道路約束方程

汽車行駛過(guò)程中，必須在道路范圍內(nèi)運(yùn)動(dòng)，如果超出道路的范圍，必須對(duì)汽車的運(yùn)動(dòng)進(jìn)行約束才能使其在正確的范圍內(nèi)行駛，行駛范圍如下頁(yè)圖1所示。

圖1 汽車的行駛范圍

式中，de和 dn是已知道路的位置。（de，dn）是以 k-1時(shí)刻汽車的位置為圓心、以k-1至k時(shí)段內(nèi)汽車的位移為半徑的圓與道路邊線相交的位置，此位置最接近估值。由式（13）可得：

其中，矩陣D2和向量d2分別為

2.2.3 導(dǎo)航系統(tǒng)總約束方程

式（10）和式（14）構(gòu)成了汽車導(dǎo)航系統(tǒng)總約束方程，聯(lián)立式（10）和式（14）可得：

3 仿真與分析

汽車導(dǎo)航系統(tǒng)的仿真參數(shù)如表1所示。

表1 仿真參數(shù)設(shè)置

設(shè)道路中線過(guò)原點(diǎn)，沿正東北方向，路寬10 m。仿真總的時(shí)長(zhǎng)設(shè)為600 s。分別采用擴(kuò)展粒子濾波算法（EPF）和無(wú)跡粒子濾算法（UPF）算法及本文算法進(jìn)行仿真，粒子數(shù)選為50，仿真結(jié)果如圖2和圖3所示。

圖2 北向位置誤差

圖3 東向位置誤差

為了進(jìn)一步分析算法的性能，計(jì)算各種算法的均方根誤差RMSE大小，公式如下：

xRMSE表示算法估計(jì)的精度大小，均方根誤差越小表明算法的精度越高。3種算法分別進(jìn)行50次，將每次運(yùn)算的xRMSE取平均值得到算法的平均均方根誤差，將每次運(yùn)算的時(shí)間取平均值得到算法的平均運(yùn)行時(shí)間，表2列出了3種算法的平均均方根誤差和平均運(yùn)行時(shí)間。

表2 3種算法的性能分析

從圖2和圖3以及表1可看出，EPF濾波算法的穩(wěn)定性比較差，有很多跳變的點(diǎn)，算法的誤差較小，位置平均RMSE在3種算法中最大，在汽車導(dǎo)航系統(tǒng)中，對(duì)算法的穩(wěn)定性和誤差要求很高，EPF算法無(wú)法滿足汽車導(dǎo)航系統(tǒng)的要求。UPF濾波算法的精度較高，沒(méi)有跳變的點(diǎn)，穩(wěn)定性較高，但是算法的平均運(yùn)行時(shí)間在3種算法中最高，說(shuō)明算法的實(shí)時(shí)性較差，在汽車導(dǎo)航系統(tǒng)中，算法的實(shí)時(shí)性將關(guān)系到汽車導(dǎo)航系統(tǒng)的操控性，UPF算法無(wú)法滿足汽車導(dǎo)航系統(tǒng)的實(shí)時(shí)性要求。本文提出的CUPF算法的誤差最小，導(dǎo)航的精度最高，穩(wěn)定性也較好，算法的運(yùn)行時(shí)間最小，實(shí)時(shí)性較好，能夠滿足汽車導(dǎo)航系統(tǒng)的要求。通過(guò)比較，可以看出本文提出的算法在3種算法中性能最好，體現(xiàn)了算法的優(yōu)越性。

4 結(jié)論

針對(duì)現(xiàn)有各種濾波算法的不足，本文將約束方程引入無(wú)跡粒子濾波算法，提出一種約束無(wú)跡粒子濾波（Constraints Unscented Particle Filter，CUPF）算法，此算法既保持了無(wú)跡粒子濾波的高精度特點(diǎn)，又通過(guò)約束方程的引入降低了系統(tǒng)維度，提高了算法的實(shí)時(shí)性。分析了汽車導(dǎo)航系統(tǒng)中存在的約束方程，將此算法應(yīng)用到汽車導(dǎo)航系統(tǒng)中，仿真結(jié)果表明，本文提出的算法估計(jì)精度較高，算法的穩(wěn)定性較好，算法所用時(shí)間較短，運(yùn)算性能明顯高于擴(kuò)展粒子濾波和無(wú)跡粒子濾波算法，可以滿足汽車導(dǎo)航系統(tǒng)的要求。