陰影匹配/粒子濾波自適應(yīng)加權(quán)組合定位算法

胡　輝，歐敏輝，顏瑜軍

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

?

陰影匹配/粒子濾波自適應(yīng)加權(quán)組合定位算法

胡輝，歐敏輝，顏瑜軍

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

針對(duì)城市峽谷場(chǎng)景中，傳統(tǒng)陰影匹配定位算法只能提高過(guò)街定位精度，不能提高沿街定位精度問(wèn)題，基于粒子濾波定位算法定位精度的分布特點(diǎn)，提出了陰影匹配/粒子濾波自適應(yīng)加權(quán)組合定位算法。該算法根據(jù)陰影匹配運(yùn)動(dòng)狀態(tài)下定位駐留時(shí)間特性和模板分值統(tǒng)計(jì)特性，計(jì)算自適應(yīng)加權(quán)因子，對(duì)陰影匹配和粒子濾波定位算法進(jìn)行自適應(yīng)加權(quán)，得到最終定位結(jié)果。試驗(yàn)結(jié)果表明：該算法與陰影匹配、粒子濾波、陰影匹配/卡爾曼濾波組合、陰影匹配/粒子濾波組合定位算法相比，分別在沿街方向平均誤差從5.04 m縮小到2.80 m，在過(guò)街方向平均誤差從4.87 m縮小到1.30 m，均方根誤差從6.82 m縮小到3.47 m。

陰影匹配；粒子濾波；自適應(yīng)加權(quán)；定位精度

0　引言

城市峽谷是一種高樓密集、高樓間距離短的城市環(huán)境，道路上空的可視衛(wèi)星分布在與道路平行的“條狀”天空中，導(dǎo)致誤差放大因子GDOP(Geometric Dilution of Precision)值一般都大于幾十，使得在此場(chǎng)景中GPS存在較大的定位誤差。在城市峽谷中，由于環(huán)境復(fù)雜，建筑物對(duì)衛(wèi)星信號(hào)的阻礙，以及行人和車(chē)輛對(duì)衛(wèi)星信號(hào)造成衰減，需要更好的方法進(jìn)行GPS定位處理提高定位精度。國(guó)內(nèi)外學(xué)者研究將粒子濾波PF(Particle filter)算法應(yīng)用于GPS傳統(tǒng)偽距定位解算的優(yōu)化，粒子濾波技術(shù)是利用統(tǒng)計(jì)學(xué)特性進(jìn)行建模，可以較好地解決模型上因近似非線性所帶來(lái)的主觀性誤差，隨著采樣粒子數(shù)的增大，逐漸趨向狀態(tài)的后驗(yàn)概率密度，在GPS動(dòng)態(tài)單點(diǎn)、高動(dòng)態(tài)等導(dǎo)航定位數(shù)據(jù)處理中獲得了廣泛應(yīng)用，比傳統(tǒng)最小二乘和卡爾曼濾波具有更高的定位精度[1-4]。在城市峽谷環(huán)境，由于建筑物的遮擋，過(guò)街方向的衛(wèi)星信號(hào)是阻礙的，這使得在過(guò)街方向的定位精度很差。然而在沿街方向衛(wèi)星信號(hào)是無(wú)阻礙的，利用非線性最優(yōu)估計(jì)算法優(yōu)化GPS的定位精度在沿街方向比過(guò)街方向更接近真實(shí)值[5]。此外，Groves提出了在城市峽谷場(chǎng)景一種新的提高定位精度的陰影匹配算法SM(Shadow matching)，該算法了利用衛(wèi)星可見(jiàn)性判斷來(lái)實(shí)現(xiàn)定位，在真實(shí)場(chǎng)景靜態(tài)實(shí)驗(yàn)驗(yàn)證過(guò)街方向可達(dá)到米級(jí)定位精度[6]；隨后L.Wang針對(duì)陰影匹配打分模板進(jìn)行改進(jìn)，新的模板與傳統(tǒng)模板相比，過(guò)街方向誤差減少了9.4%[7-8]；L.Wang在2014年應(yīng)用非線性算法陰影匹配/卡爾曼濾波、陰影匹配/粒子濾波組合算法實(shí)現(xiàn)定位，新的算法在過(guò)街方向達(dá)到2 m精度的概率是72.4%[9]。但是，陰影匹配只能提高過(guò)街方向的定位精度，卻不能提高沿街方向的定位精度。本文針對(duì)此問(wèn)題，提出了陰影匹配/粒子濾波自適應(yīng)加權(quán)組合定位算法。

1　粒子濾波及陰影匹配算法

1.1粒子濾波算法

粒子濾波算法基本步驟包括：初始化、重要性權(quán)值計(jì)算、權(quán)值歸一化、重采樣以及輸出。

(1)

(2)

步驟三：權(quán)值歸一化

(3)

步驟四：重采樣。采用系統(tǒng)重采樣算法，權(quán)值較大的樣本被多次復(fù)制，權(quán)值較小的樣本則可能會(huì)被剔除。

步驟五：輸出。系統(tǒng)狀態(tài)的最小均方誤差估計(jì)

(4)

令k=k+1，返回步驟二。

1.2陰影匹配算法

陰影匹配算法原理是根據(jù)衛(wèi)星信號(hào)被建筑物遮擋情況判斷接收機(jī)位置。通過(guò)建立的建筑物輪廓模型數(shù)據(jù)和衛(wèi)星星歷，預(yù)測(cè)城市峽谷中候選位置處的衛(wèi)星可見(jiàn)性，與實(shí)際該候選位置接收到衛(wèi)星信號(hào)的可見(jiàn)性進(jìn)行比較。依照打分模板，當(dāng)預(yù)測(cè)和實(shí)際觀測(cè)衛(wèi)星都不可見(jiàn)或預(yù)測(cè)和實(shí)際觀測(cè)衛(wèi)星都可見(jiàn)，則給定分值1。當(dāng)預(yù)測(cè)衛(wèi)星不可見(jiàn)，而實(shí)際觀測(cè)可見(jiàn)或預(yù)測(cè)衛(wèi)星可見(jiàn)，而實(shí)際觀測(cè)不可見(jiàn)，則給定分值0。對(duì)于每個(gè)候選位置的打分函數(shù)如下：

(5)

式(5)中，fpos,k(j)是j候選位置在k時(shí)刻最終的分值；fsat,k(i,j)是k時(shí)刻第i顆衛(wèi)星在候選位置點(diǎn)j處使用打分模板得到的分值，綜合考慮到系統(tǒng)對(duì)算法精度和速度的要求，選定候選位置網(wǎng)格點(diǎn)Nsm=600。仿真數(shù)據(jù)顯示k時(shí)刻對(duì)應(yīng)的候選位置打分分值分布如圖1。

圖1　陰影匹配候選位置分值分布圖Fig.1　The score distribution of candidate location of the Shadow matching

圖1中深色對(duì)應(yīng)的高分值候選位置，可看出有很多高分值候選位置，這些高分值候選點(diǎn)認(rèn)為是近鄰關(guān)系，因此使用類似K-NN(k-nearestneighbours)算法來(lái)估計(jì)最終位置。對(duì)于L個(gè)高分值區(qū)域，陰影匹配定位結(jié)果如圖用“*”表示，公式為：

(6)

(7)

其中，xsme,k，xsmn,k對(duì)應(yīng)的是k時(shí)刻使用陰影匹配定位算法在沿街和過(guò)街的定位位置。ei,k和ni,k分別對(duì)應(yīng)的是k時(shí)刻第i個(gè)高分值候選位置的沿街、過(guò)街位置。

2　陰影匹配/粒子濾波自適應(yīng)加權(quán)組合定位算法

將粒子濾波應(yīng)用于GPS定位解算中，首要是建立狀態(tài)模型和觀測(cè)模型。定義xpf,k為系統(tǒng)狀態(tài)向量，zk為系統(tǒng)觀測(cè)向量。

xpf,k=[xpfe,k,vpfe,k,xpfn,k,vpfn,k]T

(8)

zk=[ze,k,zn,k,vk,θk]T

(9)

式中系統(tǒng)狀態(tài)向量由粒子濾波優(yōu)化GPS數(shù)據(jù)在k時(shí)刻的沿街、過(guò)街位置xpfe,k,xpfn,k和沿街、過(guò)街速度vpfe,k,vpfn,k構(gòu)成，觀測(cè)向量由GPS輸出經(jīng)轉(zhuǎn)換矩陣轉(zhuǎn)換在k時(shí)刻的沿街、過(guò)街位置ze,k,zn,k，速度vk和方位角θk構(gòu)成。

2.1系統(tǒng)狀態(tài)方程

本文以PV(Position，Velocity)模型來(lái)模擬接收機(jī)在城市峽谷場(chǎng)景的運(yùn)動(dòng)狀態(tài)。狀態(tài)量包括沿街、過(guò)街位置、速度、方位角。其中每個(gè)方向上的位置和速度所含噪聲是相互獨(dú)立的，相關(guān)系數(shù)為0。系統(tǒng)的狀態(tài)轉(zhuǎn)移方程為：

xpf,k=Φkxpf,k-1

(10)

其中

(11)

2.2系統(tǒng)觀測(cè)方程

系統(tǒng)的觀測(cè)方程，沿街位置和過(guò)街位置和系統(tǒng)狀態(tài)參數(shù)的關(guān)系為：

ze,k=xpfe,k+εe,k

(12)

zn,k=xpfn,k+εn,k

(13)

(14)

(15)

(16)

2.3自適應(yīng)加權(quán)因子αk,βk的計(jì)算

該算法根據(jù)陰影匹配運(yùn)動(dòng)狀態(tài)下定位駐留時(shí)間特性和模板分值統(tǒng)計(jì)特性，計(jì)算自適應(yīng)加權(quán)因子。本文中設(shè)定陰影匹配過(guò)街和粒子濾波算法沿街具有相同的加權(quán)因子，這充分利用陰影匹配過(guò)街定位精度和粒子濾波優(yōu)化GPS沿街定位精度的優(yōu)勢(shì)，加權(quán)組合模型如下式：

xe,k=αkxsme,k+βkxpfe,k

(17)

xn,k=βkxsmn,k+αkxpfn,k

(18)

式(17)(18)中xe,k、xn,k為k時(shí)刻輸出最終沿街、過(guò)街定位結(jié)果，xpfe,k、xpfn,k為k時(shí)刻經(jīng)粒子濾波優(yōu)化GPS定位算法輸出結(jié)果，αk,βk分別為對(duì)陰影匹配和粒子濾波沿街和過(guò)街的加權(quán)因子。在城市峽谷環(huán)境中，由于建筑物輪廓信息變化不明顯，使得衛(wèi)星在候選位置點(diǎn)的定位可見(jiàn)性變化不明顯，陰影匹配運(yùn)動(dòng)狀態(tài)下在同一位置定位駐留一定時(shí)間，定位駐留時(shí)間tsm,k越大，造成定位誤差越大。同時(shí)，從圖1中也可以看出，符號(hào)“?”對(duì)應(yīng)真實(shí)位置，符號(hào)“*”對(duì)應(yīng)陰影匹配定位位置，可見(jiàn)通過(guò)模板分值統(tǒng)計(jì)高分值候選位置數(shù)目hsm,k越大，陰影匹配定位誤差也就越大。因此本文建立基于tsm,k，hsm,k的自適應(yīng)因子αk,βk計(jì)算，用來(lái)充分利用粒子濾波沿街精度和陰影匹配過(guò)街精度，如下：

(19)

βk=1-ak

(20)

3　試驗(yàn)結(jié)果與分析

本文實(shí)驗(yàn)在華東交通大學(xué)15與16棟之間峽谷場(chǎng)景，實(shí)驗(yàn)路徑如圖2(左)中A-B所示，初始位置A坐標(biāo)為(28°44′52.178 75″N,115°51′41.745 64″E,45.25m)，終止位置B坐標(biāo)(28°44′52.722 85″N,115°51′43.545 70″E,45.12m)，AB距離為83.27m。其中峽谷寬度為15m，峽谷長(zhǎng)度為50m，實(shí)景圖如圖2(右)；通過(guò)4臺(tái)南方S82RTK進(jìn)行靜態(tài)聯(lián)測(cè)，獲取精確基準(zhǔn)點(diǎn)A和B坐標(biāo)，架設(shè)徠卡TS06全站儀在基準(zhǔn)點(diǎn)A上測(cè)量A點(diǎn)到B點(diǎn)參考路線基本信息；在基準(zhǔn)點(diǎn)A和B上，利用全站儀測(cè)量建筑物各個(gè)特征頂點(diǎn)的坐標(biāo)，建立建筑物輪廓模型；使用更新率為1Hz的SuperstarIIGPS接收機(jī)在2015年5月11日17:35:07～17:36:41采集實(shí)驗(yàn)數(shù)據(jù)。

圖2　城市峽谷路線A-B和實(shí)景圖Fig.2　The route A-B and scenario of the urban canyon

圖3是SuperstarII接收機(jī)在沿街、過(guò)街定位誤差結(jié)果。GPS接收機(jī)在38～80s之間沿街、過(guò)街都存在較大誤差，沿街方向最大誤差為6.5m，而過(guò)街方向最大誤差為13m。在城市峽谷場(chǎng)景中，GPS接收機(jī)在沿街方向的定位精度優(yōu)于過(guò)街方向。

圖3　SuperstarII在沿街、過(guò)街方向定位誤差Fig.3　The positioning error of the along-street and cross-street measured by superstarII

3.1陰影匹配定位結(jié)果和陰影匹配定位特性

圖4陰影匹配沿街方向定位結(jié)果呈齒狀，是由于衛(wèi)星短時(shí)間內(nèi)星空分布不變，且建筑物邊緣輪廓信息變化不明顯，使得陰影匹配運(yùn)動(dòng)狀態(tài)下定位同一位置駐留一定時(shí)間。圖中看出沿街最大定位誤差16m，尤其在36～69s之間，輸出同一位置持續(xù)時(shí)間達(dá)33s，造成大的定位誤差。從圖4看出陰影匹配定位算法在過(guò)街方向有較高定位精度，最大誤差2.4m。

圖4　陰影匹配沿街、過(guò)街定位結(jié)果Fig.4　The along-street and cross-street positioning results of the shadow matching

從圖5看出，陰影匹配運(yùn)動(dòng)狀態(tài)下只在11個(gè)位置處存在定位結(jié)果，根據(jù)式(19)中tsm,k在一定時(shí)間內(nèi)持續(xù)增大，在37m處，tsm,k最大達(dá)33s，產(chǎn)生大的定位誤差。模板分值統(tǒng)計(jì)高分值候選位置數(shù)目，在47～69s間，hsm,k數(shù)目達(dá)到210個(gè)，對(duì)應(yīng)數(shù)目越大，定位誤差也越大。根據(jù)式(19)、式(20)駐留時(shí)間tsm,k和高分值候選位置數(shù)目hsm,k可以計(jì)算αk，βk值，當(dāng)tsm,k和hsm,k增大，對(duì)應(yīng)αk逐漸變小，此時(shí)陰影匹配沿街定位誤差大，而βk逐漸增大，充分利用粒子濾波優(yōu)化GPS的沿街定位精度和陰影匹配的過(guò)街定位精度。

圖5　陰影匹配動(dòng)態(tài)下定位駐留時(shí)間、高分值候選位置數(shù)目、自適應(yīng)加權(quán)因子Fig.5　The lingering time of shadow matching under dynamic positioning， number of candidate locations of high scores， adaptive weighted factors

3.2算法性能比較

圖6—圖9為陰影匹配粒子濾波自適應(yīng)加權(quán)組合定位算法與幾種算法的性能比較結(jié)果。

從圖6可以看出，該方法相比陰影匹配算法，從即將進(jìn)入峽谷、峽谷中、出峽谷沿街方向都具有良好的定位精度，在過(guò)街方向，該方法比陰影匹配定位精度高，峽谷中僅在70s附近存在一定偏差，當(dāng)快速收斂到真實(shí)位置附近；從圖7可以看出，該方法相比粒子濾波算法，在峽谷外和峽谷內(nèi)，沿街方向都比粒子濾波定位精度高或保持同粒子濾波相同定位精度，在過(guò)街方向，可以明顯看出該方法比粒子濾波定位精度高很多；從圖8可以看出，該方法與陰影匹配/卡爾曼組合算法相比，在沿街方向存在較高的定位精度，在過(guò)街方向基本上精度更好，僅存在部分點(diǎn)有3m左右偏差；從圖9可以看出，該方法與陰影匹配/粒子濾波組合算法相比，在沿街方向存在較高定位精度，在過(guò)街方向也基本上存在較高的定位的精度，僅部分點(diǎn)位置有3m左右偏差。從這幾個(gè)圖能夠看出該算法具有很高的定位精度，詳細(xì)性能比較見(jiàn)表1。

圖6　該算法與陰影匹配算法的比較Fig.6　The algorithm was compared with the Shadow matching algorithm

圖7　該算法與粒子濾波算法的比較Fig.7　The algorithm was compared with the Particle filter algorithm

圖8　該算法與陰影匹配/卡爾曼組合算法的比較Fig.8　The algorithm combined with Shadow match/Kalman algorithm

圖9　該算法與陰影匹配/粒子濾波組合算法的比較Fig.9　The algorithm is compared with Shadow matching/Particle filter algorithm

定位算法沿街定位誤差均值/m過(guò)街定位誤差均值/m定位均方根誤差/m陰影匹配定位算法SM5.041.466.64粒子濾波定位算法PF3.204.876.82陰影匹配/卡爾曼濾波定位算法SM/KF4.071.405.60陰影匹配/粒子濾波定位算法SM/PF3.701.375.50陰影匹配/粒子濾波自適應(yīng)加權(quán)組合定位算法(α,β)SM/PF2.801.303.47

4　結(jié)論

本文提出了陰影匹配/粒子濾波自適應(yīng)加權(quán)組合定位算法，該算法根據(jù)陰影匹配運(yùn)動(dòng)狀態(tài)下定位駐留時(shí)間特性和模板分值統(tǒng)計(jì)特性，計(jì)算自適應(yīng)加權(quán)因子，進(jìn)而對(duì)陰影匹配和粒子濾波優(yōu)化GPS進(jìn)行加權(quán)。實(shí)驗(yàn)結(jié)果表明該算法與單獨(dú)使用陰影匹配算法、單獨(dú)使用粒子濾波算法、陰影匹配/卡爾曼組合定位算法那、陰影匹配/粒子濾波組合定位算法相比，可以解決城市峽谷場(chǎng)景沿街定位精度低、不連續(xù)的問(wèn)題， 同時(shí)獲得過(guò)街方向定位高精度。為實(shí)現(xiàn)在

城市環(huán)境中基于位置服務(wù)LBS的應(yīng)用提供重要參考。

[1]WangE,ZhaoW,CaiM.ResearchonimprovingaccuracyofGPSpositioningbasedonparticlefilter[C]//IndustrialElectronicsandApplications(ICIEA), 2013 8thIEEEConferenceonIEEE, 2013:1167-1171.

[2]王爾申,蔡明,龐濤.MCMC粒子濾波的GPS定位數(shù)據(jù)處理算法[J]. 數(shù)據(jù)采集與處理,2013(2):213-218.

[3]王爾申,龐濤,李興凱,等. 基于遺傳算法和神經(jīng)網(wǎng)絡(luò)的改進(jìn)粒子濾波的GPS定位數(shù)據(jù)處理[J]. 電子器件,2015(2):410-413.

[4]王爾申,龐濤,李興凱,等. 基于遺傳粒子濾波的GPS定位數(shù)據(jù)處理算法[J]. 太赫茲科學(xué)與電子信息學(xué)報(bào),2015(3):419-422.

[5]WangL,GrovesPD,ZiebartMK.GNSSShadowMatching:ImprovingUrbanPositioningAccuracyUsinga3DCityModelwithOptimizedVisibilityScoringScheme[J].Navigation, 2013, 60(3):195-207.

[6]GrovesPD,WangL,WalterD,etal.TheFourKeyChallengesofAdvancedMultisensorNavigationandPositioning[C]//Position,LocationandNavigationSymposium-PLANS2014, 2014IEEE/ION.US:IEEE, 2014:773-792.

[7]WangL,GrovesP,ZiebartM,etal.UrbanPositioningonaSmartphone:Real-timeShadowMatchingUsingGNSSand3DCityModels[J].ProceedingsofInternationalTechnicalMeetingoftheSatelliteDivisionoftheInstituteofNavigation, 2013:1606-1619.

[8]WangL,GrovesPD,ZiebartMK.SmartphoneShadowMatchingforBetterCross-streetGNSSPositioninginUrbanEnvironments[J].JournalofNavigation, 2015, 68:411-433.

[9]WangLei.KinematicGNSSShadowMatchingUsingaParticleFilter[C]//Proceedingsofthe27thInternationalTechnicalMeetingofTheSatelliteDivisionoftheInstituteofNavigation.Tampa,Florida, 2014:1907-1919.

[10]SunZ,MaoX,TianW,etal.Activityclassificationanddeadreckoningforpedestriannavigationwithwearablesensors[J].MeasurementScience&Technology, 2009, 20(1):187-188.

Shadow Matching/Particle Filter Adaptive Weighted Integrated Localization Algorithm

HU Hui，OU Minhui，YAN Yujun

(College of Information Engineering，East China Jiaotong University，Nanchang 330013，China)

Traditional shadow matching localization algorithm could only improve the cross-street positioning accuracy in urban canyon, and could not improve the along-street accuracy. A shadow matching/particle filter adaptive weighted integrated localization algorithm based on particle filter distribution characteristics of the positioning precision was proposed in this article. According to the shadow matching positioning lingering time under motion characteristics and the statistical properties of the template score, this algorithm calculated adaptive weighted factor. Adaptive weighted shadow matching and particle filter positioning algorithm could get the final localization results. Experimental results showed that the proposed algorithm, compared with shadow matching, particle filter, shadow match/Kalman algorithm , shadow matching/particle filter, could decreased average along-street, error from 5.04 m to 2.80 m decreased average cross-street error from 4.87 m to 1.30 m, decreased the root-mean-square error from 6.82 m to 3.47 m.

shadow matching; particle fliter; adaptive weighted; positioning accuracy

2016-02-03

江西省自然科學(xué)基金項(xiàng)目資助(20142BAB207001)；江西省教育廳科學(xué)技術(shù)研究項(xiàng)目資助(GJJ14369)

胡輝(1970—)，男，江西南昌人，博士，教授，研究方向：衛(wèi)星導(dǎo)航定位，并行算法與分析處理，機(jī)器視覺(jué)。E-mail:gnss523@163.com。

TN961

A

1008-1194(2016)04-0082-06