趙國(guó)材，趙 力，宋春雷，劉志德

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

0 引言

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

DR系統(tǒng)是一種常用的車輛導(dǎo)航系統(tǒng)，它具有實(shí)時(shí)性好、短時(shí)間工作精度高的優(yōu)點(diǎn)，其關(guān)鍵部分是其慣性單元，但是隨著時(shí)間的增長(zhǎng)，行駛里程的增加，導(dǎo)航系統(tǒng)的誤差逐漸積累，并逐步發(fā)散。GPS衛(wèi)星導(dǎo)航系統(tǒng)具有全天候、高精度等特點(diǎn)，且GPS誤差不存在累積效應(yīng)，能夠較好地抑制DR導(dǎo)航系統(tǒng)的誤差。因此，DR/GPS是比較理想的組合方案。

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

1 QPSO-PF算法

PSO算法模擬由簡(jiǎn)單個(gè)體組成的群落與環(huán)境以及個(gè)體之間的互動(dòng)行為，通過群體中個(gè)體之間的協(xié)作和信息共享來尋找最優(yōu)解［6］。量子計(jì)算是利用量子系統(tǒng)的疊加性、并行性和量子糾纏等特性實(shí)現(xiàn)比經(jīng)典計(jì)算更為高效的計(jì)算模式［7，8］。

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

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

式中 xk∈Rn為狀態(tài)向量，wk－1為系統(tǒng)噪聲，vk為量測(cè)噪聲。

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

4)計(jì)算每個(gè)粒子的適應(yīng)度，計(jì)算得到粒子的個(gè)體極值位置和全局極值位置。

5)按照式(3)，式(4)實(shí)現(xiàn)粒子狀態(tài)更新，按照(5)實(shí)現(xiàn)粒子的變異。位置更新方程

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

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

速度更新方程

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

7)按照式(6)計(jì)算對(duì)應(yīng)粒子的重要性權(quán)值，歸一化權(quán)值

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

2 陸用組合導(dǎo)航系統(tǒng)模型與實(shí)驗(yàn)

2.1 導(dǎo)航系統(tǒng)組成與模型

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

圖1 陸用組合導(dǎo)航系統(tǒng)組成Fig 1 Composition of land combined navigation system

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

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

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

2.2 里程系數(shù)與航向誤差辨識(shí)

設(shè)觀測(cè)向量為υn=［υxn，υyn］T，使用 QPSO-PF 方法，根據(jù)里程系數(shù)誤差和航向誤差的先驗(yàn)概率分布產(chǎn)生兩組粒子，依照觀測(cè)向量υn對(duì)粒子分布進(jìn)行調(diào)整。設(shè)觀測(cè)向量υn的范數(shù)為‖υn‖，如果粒子使范數(shù)‖υn‖計(jì)算數(shù)值較小，則認(rèn)為該粒子出現(xiàn)機(jī)率較大;反之，則認(rèn)為此粒子出現(xiàn)機(jī)率較小。依照粒子出現(xiàn)的機(jī)率對(duì)粒子分布重新調(diào)整。通過調(diào)整后的粒子對(duì)ζ和ξ進(jìn)行估計(jì)。用估計(jì)結(jié)果對(duì)里程系數(shù)和航向進(jìn)行修正，能夠抑制DR導(dǎo)航系統(tǒng)誤差的增長(zhǎng)，提高導(dǎo)航定位精度。

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

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

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

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

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

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

2.3 跑車實(shí)驗(yàn)

跑車實(shí)驗(yàn)從北京西三環(huán)某地出發(fā)，行駛里程約為24 km，所用時(shí)間約為30 min，跑車路線如圖2所示。圖3為跑車實(shí)驗(yàn)中的DR/GPS組合導(dǎo)航，跑車過程中記錄DR和GPS數(shù)據(jù)，并進(jìn)行離線濾波處理。

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

圖3 DR/GPS組合導(dǎo)航系統(tǒng)Fig 3 DR/GPS integrated navigation system

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

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

在跑車實(shí)驗(yàn)中，車輛最終停留在一個(gè)標(biāo)準(zhǔn)點(diǎn)上，通過和標(biāo)準(zhǔn)點(diǎn)的數(shù)值比較，可以確定導(dǎo)航系統(tǒng)的定位精度。將PF算法和QPSO-PF算法估計(jì)出的里程系數(shù)誤差和航向誤差反饋到DR導(dǎo)航系統(tǒng)中，對(duì)里程系數(shù)和航向進(jìn)行修正，修正的DR誤差如表1所示。采用PF算法對(duì)里程系數(shù)誤差和航向誤差修正后，DR誤差較大，定位精度約為0.171%;采用QPSO-PF算法修正后，DR導(dǎo)航系統(tǒng)獲得了較高的定位精度，約為0.125%。這表明，QPSO-PF算法能夠有效地抑制了DR導(dǎo)航系統(tǒng)誤差的增長(zhǎng)，提高了組合導(dǎo)航系統(tǒng)的定位精度。

圖4 里程系數(shù)誤差估計(jì)對(duì)比曲線Fig 4 Contrastive curves of error estimation of mileage coefficient

圖5 航向誤差估計(jì)對(duì)比曲線Fig 5 Contrastive curves of error estimation of azimuth

表1 DR導(dǎo)航系統(tǒng)誤差對(duì)比Tab 1 Error contrast of DR navigation system

3 結(jié)論

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

［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］紀(jì) 震，廖惠連，吳青華.粒子群算法及應(yīng)用［M］.北京:科學(xué)出版社，2009.

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

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

［9］李士勇，李盼池.量子計(jì)算與量子優(yōu)化算法［M］.哈爾濱:哈爾濱工業(yè)大學(xué)出版社，2009.