量子PSO粒子濾波在DR/GPS組合導航系統中的應用*

趙國材，趙 力，宋春雷，劉志德

(1.遼寧工程技術大學電氣與控制學院，遼寧葫蘆島125105;2.北京理工大學自動化學院，北京100081)

0 引言

對車輛的全球范圍精確定位在軍用和民用領域均有很多應用。捷聯慣性導航系統(strapdown inertial navigation system，SINS)、航位推算(dead reckoning，DR)導航和全球定位系統(global positioning system，GPS)等都是目前比較常用的定位方式。GPS可以提供絕對的經緯度，是一種易用并且緊湊的導航系統。但由于城市中高樓和隧道對衛星信號的遮擋，GPS并不總是可用，另外還有多通道反射現象的存在［1］，因此，GPS不是最理想的導航方式，通常和慣性導航系統組成組合導航系統，DR導航系統就是其中的一種［2，3］。

DR系統是一種常用的車輛導航系統，它具有實時性好、短時間工作精度高的優點，其關鍵部分是其慣性單元，但是隨著時間的增長，行駛里程的增加，導航系統的誤差逐漸積累，并逐步發散。GPS衛星導航系統具有全天候、高精度等特點，且GPS誤差不存在累積效應，能夠較好地抑制DR導航系統的誤差。因此，DR/GPS是比較理想的組合方案。

上個世紀末，Gordon N提出了用大量樣本逼近狀態分布的粒子濾波(particlefilter，PF)［4］。PF 是一種基于遞推貝葉斯估計的濾波方法，但是PF存在著粒子退化、樣本貧化和計算量大等缺點，本文將量子粒子群優化(quantum particle swarm optimization，QPOS)算法［5］與 PF 相結合提出了QPSO-PF算法，改善了采樣過程，提高了PF的精度，改善了濾波的性能，并將新算法應用于DR/GPS組合導航系統中，跑車實驗驗證了新算法的有效性。

1 QPSO-PF算法

PSO算法模擬由簡單個體組成的群落與環境以及個體之間的互動行為，通過群體中個體之間的協作和信息共享來尋找最優解［6］。量子計算是利用量子系統的疊加性、并行性和量子糾纏等特性實現比經典計算更為高效的計算模式［7，8］。

本文提出的QPSO-PF算法使用量子位對粒子進行編碼，同時引入量子旋轉門與量子變異操作保持粒子集的多樣性，使粒子集有效逼近后驗概率密度函數，加速了粒子集的收斂，進而提高了PF的精度，從而取得了良好的濾波性能。

綜上所述，QPSO-PF算法流程如下:

式中 xk∈Rn為狀態向量，wk－1為系統噪聲，vk為量測噪聲。

式中 θij=2π× rand，i=1，2，…，m;j=1，2，…，n，rand 為(0，1)之間的隨機數，m是種群數。

4)計算每個粒子的適應度，計算得到粒子的個體極值位置和全局極值位置。

5)按照式(3)，式(4)實現粒子狀態更新，按照(5)實現粒子的變異。位置更新方程

政府必須回歸到市場經濟下政府的職能本位上來，將“經濟建設”的職能交還給市場，政府要履行好公共財政的基本職能。首先要正確地認識什么是真正的公共財政，就是怎么樣去定位政府與市場的關系，政府究竟應該做什么？過去我們經常講，政府包辦天下，政府做了太多管不好也管不了的事，今天“萬能政府”的影子還見諸各個行業，凡是國家權力干預的地方和行業必然是效率低下和壟斷程度很深的。我們的微觀經濟生活管得太深、管得太死，要回歸到政府的公共財政的建設上面，把資源配置和經濟建設的職能主體要交還給市場和私人部門。

式中 i=1，2，…，m;j=1，2，…，n。

速度更新方程

6)若滿足收斂條件或達到最大迭代次數，跳到步驟(7)，否則，返回步驟(4)。

7)按照式(6)計算對應粒子的重要性權值，歸一化權值

8)利用式(7)對xk進行估計;k=k+1，返回步驟(2)

2 陸用組合導航系統模型與實驗

2.1 導航系統組成與模型

陸用組合導航系統主要由方位保持儀、尋北儀、控制顯示器、航向指示器、高程計、里程計和GPS等部分組成，如圖1所示。

圖1 陸用組合導航系統組成Fig 1 Composition of land combined navigation system

陸用組合導航系統以DR導航系統為主要導航系統，以GPS作為輔助，實現組合導航。DR方法是根據航向和里程信息，推算得到車輛的位置信息，里程信息由里程計提供，航向信息由方位保持儀提供。設車輛的起始位置為(x0，y0)，Sn和θn是從n－1時刻到n時刻車輛行駛的路程和航向，則車輛n時刻的位置為

航向誤差體現在實時航向數據上，里程誤差體現在里程系數誤差上。設航向誤差為ζ，里程系數為k，里程系數誤差為ξ，車輪轉一周里程計輸出脈沖數為N，第i段距離內里程計脈沖數為ni，采樣間隔為500 ms，由式(8)可得

以DR系統與GPS在北向和東向的定位偏差vxn和vyn作為觀測量，則有量測方程為

2.2 里程系數與航向誤差辨識

設觀測向量為υn=［υxn，υyn］T，使用 QPSO-PF 方法，根據里程系數誤差和航向誤差的先驗概率分布產生兩組粒子，依照觀測向量υn對粒子分布進行調整。設觀測向量υn的范數為‖υn‖，如果粒子使范數‖υn‖計算數值較小，則認為該粒子出現機率較大;反之，則認為此粒子出現機率較小。依照粒子出現的機率對粒子分布重新調整。通過調整后的粒子對ζ和ξ進行估計。用估計結果對里程系數和航向進行修正，能夠抑制DR導航系統誤差的增長，提高導航定位精度。

里程系數誤差ξ主要與輪胎和地面狀況以及車輛負荷等因素相關。設用一組粒子{ξ1，0，ξ1，0，…，ξNp，0}描述里程系數誤差的初始概率分布p0(ξ)，Np為粒子數目，粒子初始分布為均勻分布。航向誤差主要來自于初始尋北誤差和陀螺儀隨時間慢慢積累的漂移。用一組粒子{ζ1，0，ζ2，0，…，ζNp，0}描述航向誤差的初始概率分布p0(ζ)，粒子初始分布為均勻分布。由式(9)，根據n時刻觀測向量vn對粒子分布進行調整。

里程系數誤差和航向誤差辨識的QPSO-PF具體步驟如下:

1)初始化隨機樣本:根據里程系數誤差和航向誤差的先驗概率分布 p0(ξ)和 p0(ζ)，抽取初始隨機樣本{ξ1，0，ξ1，0，…，ξNp，0}和{ζ1，0，ζ2，0，…，ζNp，0}。

2)采用QPSO-PF算法進行粒子更新:依照n時刻的觀測向量 vn對粒子分布進行調整?？紤]隨機樣本 ξ1，n－1，ξ2，n－1，…，ξNp，n－1和 ζ1，n－1，ζ2，n－1，…，ζNp，n－1，根據范數‖vn‖將樣本中出現機率較大的粒子進行復制;將出現機率較小的進行舍棄，得到新的隨機樣本 ξ1，n，ξ2，n，…，ξNp，n和 ζ1，n，ζ2，n，…，ζNp，n。

3)對參數進行估計:由 n 時刻粒子樣本 ξ1，n，ξ2，n，…，ξNp，n和 ζ1，n，ζ2，n，…，ζNp，n，可以計算

4)取n+1→n ，返回步驟(2)，并重復下去。

2.3 跑車實驗

跑車實驗從北京西三環某地出發，行駛里程約為24 km，所用時間約為30 min，跑車路線如圖2所示。圖3為跑車實驗中的DR/GPS組合導航，跑車過程中記錄DR和GPS數據，并進行離線濾波處理。

圖2 跑車路線圖Fig 2 The route of test

圖3 DR/GPS組合導航系統Fig 3 DR/GPS integrated navigation system

采用PF算法和QPSO-PF算法對記錄的跑車數據進行濾波。設粒子數目為500。估計里程系數誤差ξ時，在區間［k(1－15%)，k(1+15%)］內均勻產生粒子;估計航向誤差ζ時，在區間［－60，+60 mil］范圍內均勻產生粒子。PF算法和QPSO-PF算法對里程系數誤差濾波的結果如圖4所示;對航向誤差濾波的結果如圖5所示。

因為GPS信號是在400 s左右開始有效的，所以，對里程系數誤差和航向誤差的濾波是從400 s開始的。從圖4和圖5可以看出:采用QPSO-PF算法對里程系數誤差和航向誤差進行濾波，里程系數誤差估值^ξ約從－0.058×10－3m/Nr變化到0.019×10－3m/Nr;航向誤差估值^ζ約為－6.2～－1.7 mil。由于存在未標定的安裝誤差和初始尋北誤差及動態方位漂移誤差，航向誤差對DR導航系統誤差影響較大，使DR導航系統精度明顯下降，而里程系數誤差對DR導航系統誤差影響較小。

在跑車實驗中，車輛最終停留在一個標準點上，通過和標準點的數值比較，可以確定導航系統的定位精度。將PF算法和QPSO-PF算法估計出的里程系數誤差和航向誤差反饋到DR導航系統中，對里程系數和航向進行修正，修正的DR誤差如表1所示。采用PF算法對里程系數誤差和航向誤差修正后，DR誤差較大，定位精度約為0.171%;采用QPSO-PF算法修正后，DR導航系統獲得了較高的定位精度，約為0.125%。這表明，QPSO-PF算法能夠有效地抑制了DR導航系統誤差的增長，提高了組合導航系統的定位精度。

圖4 里程系數誤差估計對比曲線Fig 4 Contrastive curves of error estimation of mileage coefficient

圖5 航向誤差估計對比曲線Fig 5 Contrastive curves of error estimation of azimuth

表1 DR導航系統誤差對比Tab 1 Error contrast of DR navigation system

3 結論

本文將QPSO算法與PF算法相結合，提出一種QPSOPF算法。該算法充分利用了量子優化的疊加性、并行性和加速功能等特點，在QPSO的搜索尋優過程中，使用量子優化控制粒子的運動方向，防止粒子過分密集，保證了粒子集的多樣性。新算法有效地改善了粒子的分布，加速了粒子集的收斂，獲得了更快的收斂速度和全局尋優的能力，進而提高了PF的精度，取得了良好的濾波性能。將QPSO-PF算法應用到陸用DR/GPS組合導航系統中，根據GPS定位信息對DR導航系統中的航向誤差和里程系數誤差進行估計，并對里程系數和航向進行修正，跑車實驗驗證了該算法的有效性。

［1］Kaplan E D，Understanding GPS:Principles and applications［M］.Artech House，1996.

［2］Zhao L，Ochieng WY，Quddus M A，et al.An extended Kalman filter algorithm for integrating GPSand low cost dead reckoning system data for vehicle performance and emissions monitoring［J］.Journal of Navigation，2003，56(2):257－275.

［3］Zhao Y.Vehicle location and navigation systems［M］.Artech House，1997.

［4］Gordon N，Salmond D.Novel approach to nonlinear/non Gaussian Bayesian state estimation［J］.IEE Proc of Radar and Signal Processing，1993，140(2):107－113.

［5］Moore P，Venayagamoorthy G K.Evolving combinational logic circuits using a hybrid quantum evolution and particle swarm inspired algorithm［C］∥Proceedings of the 2005 NASA/DoD Conference on Evolvable Hardware，2005:97－102.

［6］紀 震，廖惠連，吳青華.粒子群算法及應用［M］.北京:科學出版社，2009.

［7］夏培肅.量子計算［J］.計算機研究與發展，2001，38(10):1153－1171.

［8］金乃高，殷福亮.量子進化粒子濾波算法及其在說話人跟蹤中的應用［J］.信號處理，2008，24(6):982－987.

［9］李士勇，李盼池.量子計算與量子優化算法［M］.哈爾濱:哈爾濱工業大學出版社，2009.