王更生，張 翔

（華東交通大學(xué)信息工程學(xué)院，南昌330013）

改進(jìn)的GNSS/INS列車(chē)組合定位PF算法?

王更生，張 翔

（華東交通大學(xué)信息工程學(xué)院，南昌330013）

針對(duì)列車(chē)組合定位融合估計(jì)的非線性問(wèn)題，結(jié)合GNSS/INS（Global Navigation Satellite System/Inertial Navigation System）列車(chē)組合定位當(dāng)前發(fā)展的方向，給出了一般無(wú)跡粒子濾波（UPF）的基本過(guò)程。針對(duì)傳統(tǒng)PF（Particle Filter）缺乏量測(cè)信息且計(jì)算量大而難以應(yīng)用于GNSS/INS組合導(dǎo)航中的問(wèn)題，提出一種新的IUPF（Iterative Unscented Particle Filter）算法。它通過(guò)將IKF（Iterative Kalman Filter）的思想融入U(xiǎn)KF（Unscented Kalman Filter），得到比較健壯的迭代無(wú)跡Kalman濾波（IUKF），由IUKF（Iterative Unscented Kalman Filter）產(chǎn)生的分布與真實(shí)的后驗(yàn)分布有更大的支撐重疊區(qū)域，提高了精度。并且運(yùn)用全局采樣到PF中，結(jié)合當(dāng)前最新觀測(cè)值對(duì)粒子集整體采用一次IUKF來(lái)產(chǎn)生建議性分布，減少了計(jì)算量。仿真模擬結(jié)果表明，IUPF與一般的UPF、PF相比，精度更高，算法計(jì)算量更小。

列車(chē)組合定位；GNSS/INS組合系統(tǒng)；PF濾波；迭代無(wú)跡粒子濾波；全局采樣；建議性分布

1 引 言

基于GNSS與INS等傳感器融合構(gòu)成的列車(chē)組合定位系統(tǒng)，能夠有效提高整個(gè)系統(tǒng)的容錯(cuò)能力、位置信息可信度以及時(shí)間和空間覆蓋范圍，是當(dāng)前列車(chē)測(cè)速定位技術(shù)的重要發(fā)展方向。在GNSS/INS列車(chē)組合定位過(guò)程中，很重要的一步就是多個(gè)傳感器的信息融合，傳統(tǒng)的做法廣泛采用卡爾曼濾波方案。后來(lái)提出的EKF（Extended Kalman Filter）及UKF等又因?yàn)楦髯阅Ｐ偷南拗?，無(wú)法精確描述系統(tǒng)模型，很難適用于組合導(dǎo)航中強(qiáng)非線性、非高斯的濾波環(huán)境。粒子濾波PF（Particle Filter）在處理非線性、非高斯時(shí)變系統(tǒng)的參數(shù)估計(jì)和狀態(tài)濾波問(wèn)題上具有獨(dú)特優(yōu)勢(shì)［1］，因此在GNSS/INS組合導(dǎo)航應(yīng)用中有較大應(yīng)用潛力。粒子濾波中建議分布選擇的好壞決定著采樣后粒子集分布是否合理［2］，選擇一個(gè)合適的建議分布，可以使粒子集在重要性采樣后很好地覆蓋狀態(tài)的后驗(yàn)概率密度區(qū)域。將IKF的思想融入U(xiǎn)KF，就可以得到比較健壯的迭代無(wú)跡Kalman濾波。由IUKF產(chǎn)生的分布與真實(shí)的后驗(yàn)分布有更大的支撐重疊區(qū)域，所以IUKF比UKF估計(jì)的更精確，并且運(yùn)用全局采樣到IUPF中，能夠以較小的計(jì)算代價(jià)對(duì)GNSS/INS狀態(tài)進(jìn)行較高精度的估計(jì)。

2 傳統(tǒng)UPF的基本原理

針對(duì)PF（Particle Filter）［3］無(wú)法很好地逼近后驗(yàn)概率的問(wèn)題，Merwe等人提出使用UKF產(chǎn)生PF的重要性分布，稱(chēng)為Unscented粒子濾波器（UPF）。UKF產(chǎn)生的重要性分布與真實(shí)狀態(tài)概率密度分布的支撐集重疊部分較大，估計(jì)精度更高。

UPF算法充分利用了系統(tǒng)模型并結(jié)合了最新觀測(cè)值，使粒子狀態(tài)更加接近樣本真實(shí)的后驗(yàn)分布，可以有效抑制粒子的退化。但是，經(jīng)過(guò)若干次循環(huán)后，只有個(gè)別粒子具有較大的權(quán)值，不能有效地表示后驗(yàn)分布，從而對(duì)狀態(tài)的估計(jì)基本起不到作用。

3 基于全局采樣的IUPF算法

進(jìn)行PF及其擴(kuò)展算法時(shí)，重要性密度函數(shù)的選擇很重要。UKF是一種非常有效的非線性濾波方法，它利用狀態(tài)方程和量測(cè)方程真實(shí)的非線性模型，然后選擇一系列確切的sigma點(diǎn)去逼近概率密度函數(shù)。以往的研究表明，UPF在利用UKF算法對(duì)每個(gè)粒子產(chǎn)生建議分布時(shí)，包含最近的觀測(cè)信息，可以比較接近后驗(yàn)概率密度函數(shù)，對(duì)濾波精度有很大提高［5］。但是傳統(tǒng)的UPF在每一次濾波過(guò)程中都需要迭代N次UKF算法，分別去估計(jì)這N個(gè)粒子的均值和方差，然后以這N個(gè)高斯分布作為每個(gè)粒子的建議分布。如此一來(lái)，算法的計(jì)算量大大增加。

IUPF算法具體步驟如下：

步驟1：初始化

步驟2：計(jì)算粒子集的均值和方差

步驟3：利用IUKF預(yù)測(cè)和更新時(shí)刻粒子集的均值和方差

（1）擴(kuò)展系統(tǒng)的狀態(tài)向量

（3）時(shí)間更新

（4）量測(cè)更新

其中，zi，k｜k－1表示zk｜k－1的第i個(gè)列向量。

步驟4：重要性采樣并計(jì)算重要性權(quán)值

（1）重要性采樣

（2）計(jì)算重要性權(quán)值

并進(jìn)行歸一化處理。

步驟5：輸出階段

輸出一組帶權(quán)值的粒子，并得到相關(guān)的狀態(tài)估計(jì)。

步驟6：循環(huán)控制

令k＝k＋1，若算法不結(jié)束就轉(zhuǎn)到步驟2。

上述描述的IUPF，既有非線性分布統(tǒng)計(jì)量計(jì)算精度高的優(yōu)點(diǎn)（UKF主要的優(yōu)點(diǎn)），又能夠比較精確地表達(dá)基于觀測(cè)量和控制量的后驗(yàn)概率分布（PF的主要優(yōu)點(diǎn)）。并且，在采樣環(huán)節(jié)對(duì)粒子集整體進(jìn)行采樣，然后再融入IKF的思想到UKF中，對(duì)粒子集整體作一次IUKF算法，結(jié)合樣本最新的觀測(cè)值即可給出全局的建議分布。由IUKF產(chǎn)生的重要性分布與真實(shí)狀態(tài)概率密度函數(shù)的支集重疊部分更大，估計(jì)精度更高。同時(shí)，在IUPF算法過(guò)程中，只需要用到上一時(shí)刻的粒子集均值和方差，再結(jié)合當(dāng)前時(shí)刻的觀測(cè)值，故省去了標(biāo)準(zhǔn)PF算法中重采樣這一環(huán)節(jié)，從而也就改進(jìn)了濾波效率。

4 IUPF算法仿真實(shí)現(xiàn)

在采集和諧號(hào)列控系統(tǒng)的樣本數(shù)據(jù)后，設(shè)置以下仿真條件并用Matlab R2010b仿真實(shí)驗(yàn)：

（1）陀螺隨機(jī)常值飄移為［0.01，0.01，0.01］rad/h；

（2）陀螺一階馬爾卡夫飄移為［0.000001，0.000001，0.000001］rad/h；

（3）加速度計(jì)一階馬爾卡夫飄移為［0.0001，0.000001，0.000001］g；

（4）陀螺一階馬爾卡夫相關(guān)時(shí)間為400s；

（5）加速度計(jì)一階馬爾卡夫相關(guān)時(shí)間為600s；

（6）GPS數(shù)據(jù)刷新率為1Hz；

（7）GPS狀態(tài)誤差為0.05rad；

（8）GPS速度誤差為5m/s；

（9）GPS位置誤差為：水平15m，天向15m。

利用以上仿真條件，分別對(duì)同一組樣本數(shù)據(jù)采用傳統(tǒng)的UPF算法和改進(jìn)的IUPF算法進(jìn)行GNSS/INS列車(chē)組合定位系統(tǒng)的仿真驗(yàn)證。圖1－圖3即UPF和IUPF濾波分別應(yīng)用在GNSS/INS列車(chē)組合定位系統(tǒng)上，東向、北向和天向3個(gè)方向的位置誤差。從圖中可以看出：采用UPF算法得到的列車(chē)位置誤差基本上在［－4，4m］區(qū)間，而采用IUPF算法得到的位置誤差降低在［－1，1m］區(qū)間上。該算法使GNSS/INS列車(chē)組合定位系統(tǒng)的精確性得到了極大提高，為列車(chē)的科學(xué)調(diào)度和安全運(yùn)行提供技術(shù)支撐［9］。圖4為在不同粒子數(shù)下一次迭代濾波的平均時(shí)間，實(shí)驗(yàn)結(jié)果表明IUPF濾波時(shí)間比傳統(tǒng)的UPF明顯少。表1列出了傳統(tǒng)UPF和IUPF兩種算法的性能對(duì)比。

圖1 UPF和IUPF東向位置誤差

表1 UPF和IUPF算法性能比較

圖2 UPF和IUPF北向位置誤差

圖3 UPF和IUPF天向位置誤差

圖4 單步濾波時(shí)間

5 結(jié)束語(yǔ)

在IUKF和PF的基礎(chǔ)上，引入基于IUKF設(shè)計(jì)重要性密度函數(shù)的全局采樣IUPF算法，結(jié)合兩者優(yōu)點(diǎn)，又加入新的采樣環(huán)節(jié)。與傳統(tǒng)UPF相比算法效率提高不少，在GNSS/INS列車(chē)組合定位系統(tǒng)的定位精確性上有了很大增加，具有重要的工程實(shí)用價(jià)值。

［1］ 劉先省，胡振濤，金勇.基于粒子優(yōu)化的多模型粒子濾波算法［J］.電子學(xué)報(bào)，2010，38（2）：301－307.

Liu Xiansheng，Hu Zhentao，Jin Yong.Multiple model particle filter algorithm based on particle swarm optimization［J］.Chinese Journal of Electronics，2010，38（2）：301－307.

［2］ Wang Fasheng，Lin Yuejin.Improving particle filterwith a new sampling strategy［C］.//Proceedings of the 4th International Conference on Computer Science＆Education，Nanning，China：National Research Council of Computer Education in Colleges＆Universities 2009：407－412.

［3］ 胡士強(qiáng)，敬忠良.粒子濾波原理及應(yīng)用［M］.北京：科學(xué)出版社，2010.

Hu Shiqiang，Jing Zhongliang.The principle and application of particle filter［M］.Beijing：Science Press，2010.

［4］ 王旭陽(yáng)，王志勇.一種自適應(yīng)免疫優(yōu)化的無(wú)跡粒子濾波器［J］.計(jì)算機(jī)工程與應(yīng)用，2013：49（4）：231－235.

Wang Xuyang，Wang Zhiyong.An adaptive immune optimization unscented particle filter［J］.Computer engineering and applications，2013：49（4）：231－235.

［5］ Julier SJ，Uhlmann JK.Unscented filtering and nonlinear estimation［J］.Proceedings of the IEEE，2004，92（3）：401－422.

［6］ 席志紅，付存利.一種基于UPF.的改進(jìn)粒子濾波算法［J］.計(jì)算機(jī)仿真，2014，31（2）：336－339.

Xi Zhihong，F(xiàn)u Cunli.An improved particle filter algorithm based on UPF［J］.Computer simulation，2014，31（2）：336－339.

［7］ 詹武平，肖同林，吳艷琴.改進(jìn)的迭代粒子濾波法在GPS數(shù)據(jù)處理中的應(yīng)用［J］.遙測(cè)遙感，2011，32（2）：39－43.

Zhan Wuping，Xiao Lin，Wu Yanqin.Application of iterative method of improved particle filter in GPS data processing［J］.Remote sensing，2011，32（2）：39－43.

［8］ 郭文艷，韓崇昭，雷明.迭代無(wú)跡Kalman粒子濾波的建議分布［J］.清華大學(xué)學(xué)報(bào)（自然科學(xué)版），2007，47（S2）：1866－1869.

Guo Wenyan，Han Chongzhao，Lei Ming.The proposal distribution iterated unscented particle filter Kalman［J］.Journal of Tsinghua University（NATURAL SCIENCE EDITION），2007，47（S2）：1866－1869.

［9］ 董健康，安東.自適應(yīng)卡爾曼濾波在組合定位中的應(yīng)用研究［J］.計(jì)算機(jī)技術(shù)與發(fā)展，2011，21（10）：11－14.

Dong Jiankang，An dong.Study on Application of［J］.Technology and development in integrated positioning in adaptive Kalman filter，2011，21（10）：11－14.

Im proved Algorithm of PF for GNSS/INS Integrated Train Positioning

Wang Gengsheng，Zhang Xiang
（College of Information，East China Jiao Tong University，Nanchang 330013，China）

Aiming at the fusion estimation problems of nonlinearity in integrated train positioning，combining with GNSS/INS（Global Navigation Satellite System/Inertial Navigation System）of the current development of integrated train positioning direction，the basic process of the unscented particle filter（UPF）is given in this paper.The traditional PF（Particle Filter）is lack ofmeasurement information and the amount of calculation is large and difficult to be applied to GNSS/INS integrated navigation，so a new IUPF（iteration unscented particle filter）is proposed.It integrates IKF（Iterative Kalman Filter）thought into UKF（Unscented Kalman Filter）to get the comparison robust IUKF（iterative unscented Kalman filter），the distribution generated by the IUKF to the true posterior distribution with support overlap area larger，and improves the accuracy.By using a global sampling to PF，the overall use of an IUKF on the particle set，combiningwith the new observation to generate the proposal distribution，reduces the amount of calculation.The simulation results show that the proposed algorithm，comparing with the ordinary UPF and PF，has higher accuracy and less calculation algorithm.

Integrated positioning；GNSS/INSgroup system；Particle Filter（PF）；Iteration unscented particle filter（IUPF）；Global sampling；Proposal distribution

10.3969/j.issn.1002－2279.2015.06.011

TP13

A

1002－2279（2015）06－0040－04

國(guó)家自然科學(xué)基金資助項(xiàng)目（61461019）

王更生（1964－），男，湖南人，教授，主研方向：計(jì)算機(jī)應(yīng)用，全球衛(wèi)星定位系統(tǒng)，數(shù)據(jù)挖掘。

張翔（1990－），男，湖北省孝感市人，碩士研究生在讀，主研方向：全球衛(wèi)星定位系統(tǒng)，計(jì)算機(jī)應(yīng)用。

2015－03－12