基于信息融合的電網(wǎng)作業(yè)智能終端定位算法①

薛 彤,夏震宇,張 森,葉立兆,單海鷗

(國(guó)家電網(wǎng) 溫州供電公司,溫州 325028)

近年來(lái),在電力系統(tǒng)中,電網(wǎng)移動(dòng)互聯(lián)建設(shè)快速發(fā)展,隨之而來(lái)的基于云平臺(tái)和移動(dòng)公共服務(wù)的全過(guò)程配網(wǎng)作業(yè)管控系統(tǒng)應(yīng)運(yùn)而生.配網(wǎng)作業(yè)管控系統(tǒng)可實(shí)現(xiàn)對(duì)現(xiàn)場(chǎng)作業(yè)的管理和實(shí)時(shí)監(jiān)控.作為配網(wǎng)作業(yè)智能管控系統(tǒng)的關(guān)鍵節(jié)點(diǎn)——智能終端,在整個(gè)系統(tǒng)中起著重要作用.智能終端的定位及移動(dòng)軌跡是實(shí)現(xiàn)現(xiàn)場(chǎng)作業(yè)的管理和實(shí)時(shí)監(jiān)控的必備功能.

現(xiàn)有的電力作業(yè)智能終端采用GPS進(jìn)行定位,此類方法主要通過(guò)由己知的衛(wèi)星坐標(biāo)及到達(dá)移動(dòng)終端的時(shí)間、角度測(cè)量值來(lái)計(jì)算所在位置.在移動(dòng)環(huán)境下,無(wú)線電波易受信號(hào)干擾,受建筑物、樹木遮擋等因素影響,存在散射或反射影響,使電波傳播發(fā)生了很大延遲,從而使發(fā)送端與接收端之間的距離或相應(yīng)的距離差無(wú)法較為準(zhǔn)確的測(cè)量[1],并且測(cè)得的定位位置和軌跡記錄存在漂移現(xiàn)象.在沒(méi)有GPS信號(hào)或者GPS衛(wèi)星數(shù)量不足的情況下,GPS難以實(shí)現(xiàn)有效定位[2],導(dǎo)致在監(jiān)控中心出現(xiàn)智能終端移動(dòng)軌跡紊亂,甚至智能終端消失的情況也時(shí)有發(fā)生,嚴(yán)重影響了系統(tǒng)可靠性和使用效果.

為解決上述問(wèn)題,不同的技術(shù)方法也隨即涌現(xiàn),常用的方法有加權(quán)平均[3]、卡爾曼濾波[4,5]、自適應(yīng)濾波[6]、信息融合方法[7-14]等.加權(quán)平均是比較簡(jiǎn)單粗略的方法,在長(zhǎng)時(shí)間干擾或者無(wú)信號(hào)時(shí),誤差依然嚴(yán)重.卡爾曼濾波在大多條件下具備較好的濾波效果,能有效剔除異常數(shù)據(jù),但在GPS智能終端大范圍機(jī)動(dòng)或信號(hào)被干擾時(shí),會(huì)帶來(lái)較大誤差.Saga-Husa自適應(yīng)濾波算法狀態(tài)變量的維數(shù)較高,并且要計(jì)算系統(tǒng)噪聲統(tǒng)計(jì)特性,計(jì)算量大,實(shí)時(shí)性、收斂性和穩(wěn)定性難以保證,不適用于對(duì)動(dòng)態(tài)目標(biāo)的定位[6].強(qiáng)跟蹤kalman自適應(yīng)濾波算法濾波收斂性有所增強(qiáng),但破壞了濾波器的最優(yōu)條件,易導(dǎo)致濾波結(jié)果的波動(dòng),影響濾波精度[6].近年來(lái)基于濾波方法的信息融合定位[11-13]雖然能改善濾波效果,提高預(yù)測(cè)精度,但在GPS信號(hào)干擾嚴(yán)重或者無(wú)法接收到GPS信號(hào)的情況下仍然難以在本質(zhì)上改善定位效果.文獻(xiàn)[7-10]提出了在GPS+PDR 組合的基礎(chǔ)上,用卡爾曼濾波對(duì)GPS 和PDR數(shù)據(jù)進(jìn)行融合,因采用的卡爾曼濾波所假定的信號(hào)噪聲為高斯白噪聲,且沒(méi)有對(duì)PDR的有效誤差校正,會(huì)產(chǎn)生濾波結(jié)果偏差和積累誤差,導(dǎo)致定位精度下降.

本文提出了AGPS、無(wú)線基站定位和行人航位推測(cè)相結(jié)合的基于粒子濾波的定位系統(tǒng),實(shí)現(xiàn)在GPS信號(hào)較弱或者無(wú)信號(hào)的條件下,智能終端依然可完成高精度定位.

1 基于信息融合的定位算法原理

本文采用基于信息融合的定位方法,即采用”AGPS+無(wú)線基站信號(hào)+行人航位推測(cè)+粒子濾波算法”實(shí)現(xiàn)融合定位,其原理如圖1所示,其步驟如下：

步驟1.智能終端的磁力計(jì)、陀螺儀完成角度測(cè)量、加速度傳感器實(shí)現(xiàn)加速度測(cè)量,通過(guò)兩者測(cè)量的數(shù)據(jù)計(jì)算去除重力并以地面作為參考系的加速度值.

步驟2.根據(jù)加速度檢測(cè)值,利用基于零速更新的步長(zhǎng)檢測(cè)算法(Zero Velocity Update,ZUPT)實(shí)現(xiàn)步長(zhǎng)檢測(cè).

步驟3.根據(jù)陀螺儀完成的角度測(cè)量及步驟2實(shí)現(xiàn)的步長(zhǎng)檢測(cè),實(shí)現(xiàn)行人航位(位置)推測(cè).

步驟4.根據(jù)信號(hào)強(qiáng)度——距離數(shù)學(xué)模型,測(cè)量智能終端與基站間距離,根據(jù)AGPS觀測(cè)值并結(jié)合無(wú)線基站測(cè)算,計(jì)算坐標(biāo)位置.

步驟5.針對(duì)步驟3、步驟4的測(cè)量結(jié)果進(jìn)行信息融合,利用粒子濾波算法預(yù)測(cè)智能終端當(dāng)前位置,并輸出定位結(jié)果.

圖1 融合定位算法原理圖

2 航位推測(cè)(PDR)定位算法

2.1 步長(zhǎng)檢測(cè)

假設(shè)智能終端內(nèi)帶的加速度傳感器以自身坐標(biāo)的加速度為以地面為坐標(biāo)的去除重力后對(duì)應(yīng)的加速度a在三方向上的分量為ax,ay,az,陀螺儀完成的角度測(cè)量對(duì)應(yīng)的偏航角 φ,俯仰角 θ,滾轉(zhuǎn)角 γ,則：

其中,RS=RERNRD,E代表東,N代表北,D代表指向地,RS表示為：

假設(shè)從加速度傳感器讀取加速度采樣值的頻率為則對(duì)應(yīng)的速度為：

只通過(guò)加速度對(duì)采樣頻率的除法運(yùn)算得到的速度值,會(huì)隨著時(shí)間的積累產(chǎn)生漂移,從而生產(chǎn)較大的誤差,因此采用零速更新的步長(zhǎng)修正.

假設(shè)為在第個(gè)姿態(tài)附近的平均速度,則：

式中ik為第k個(gè)姿態(tài)時(shí)的采樣索引,w為平均窗口取值,為一常數(shù).假設(shè)mk為在第k個(gè)姿態(tài)內(nèi)的總采樣數(shù),則通過(guò)線性積分求得在連續(xù)的姿態(tài)間的速度為：

則在第k個(gè)姿態(tài)/腳步的位置增量?PkD)可表示為：

在水平二維空間,步長(zhǎng)可表示為：

2.2 行人航位推測(cè)

方向角和位移推測(cè)航跡原理如圖2所示.航向角為陀螺儀和磁力計(jì)組合測(cè)量得到的磁力線朝北方向和行走方向的夾角.因此有：

圖2 方向角和位移推測(cè)航跡

3 AGPS/基站信號(hào)強(qiáng)度融合定位

基站無(wú)線信號(hào)強(qiáng)度R與通信距離d的關(guān)系可表示為：

式中,α為離dAP1米處的信號(hào)強(qiáng)度,β為衰減系數(shù),dAP為基站節(jié)點(diǎn)所在位置,d為被測(cè)點(diǎn)所在位置,n oise為高斯測(cè)量誤差.dAP,α,β為已知量.

假設(shè)第k個(gè)(k=1,2,3,4,···)基站節(jié)點(diǎn)的位置dAP坐標(biāo)為 (xAPk,yAPk) ,智能終端所在位置d坐標(biāo)為 (xd,yd,zd),dk為k個(gè)節(jié)點(diǎn)到被測(cè)點(diǎn)的距離,有：

針對(duì)基站信號(hào),若不考慮高度,取k=3,則

其中,

通過(guò)式(11)可求得被測(cè)點(diǎn)所在位置d坐標(biāo)為(xd,yd).

AGPS觀測(cè)方程為：

在惡劣條件下,AGPS定位系統(tǒng)和無(wú)線通信網(wǎng)絡(luò)都無(wú)法獨(dú)自有效定位時(shí),可融合GPS測(cè)得的偽距和無(wú)線通信網(wǎng)絡(luò)中的終端測(cè)得位置實(shí)現(xiàn)定位的算法[15].令 δ為智能終端到初始觀測(cè)值距離差,即則有：

其中,G、Q、h由文獻(xiàn)[15,16]給出,根據(jù)式(16)可迭代得到終端的位置估計(jì)d坐標(biāo),算得的δ加上初始值即得下一迭代的初始值.

4 基于粒子濾波的定位預(yù)測(cè)

與卡爾曼濾波、擴(kuò)展卡爾曼濾波相比,粒子濾波用隨機(jī)采樣的粒子表達(dá)概率密度分布,而不假設(shè)狀態(tài)變量為高斯隨機(jī)變量,因此在處理非高斯問(wèn)題、非線性上具有優(yōu)勢(shì)[17].基于粒子濾波的位置預(yù)測(cè)如圖3所示.

圖3 基于粒子濾波的定位預(yù)測(cè)原理

根據(jù)行人航位推測(cè)PDR的預(yù)測(cè)模型可由式(8)得：

式中表示當(dāng)前的預(yù)測(cè)位置為上一次預(yù)測(cè)位置

無(wú)線測(cè)距預(yù)測(cè)模型似然函數(shù)：

式中,σ為正態(tài)分布的方差為根據(jù)第3小節(jié)測(cè)算的位置,可由式(9)得：

式中,R[i]為讀取的無(wú)線信號(hào)強(qiáng)度,α、 β為經(jīng)驗(yàn)值.粒子權(quán)重為：

γn為對(duì)應(yīng)WiFi、藍(lán)牙信號(hào)的權(quán)值,且γ1+γ2+···+γn=1.

估計(jì)狀態(tài)的輸出為：

5 實(shí)驗(yàn)結(jié)果與分析

5.1 基于零速更新(ZUPT)的步長(zhǎng)檢測(cè)算法

圖4為系統(tǒng)讀取加速度并對(duì)其積分后的速度曲線.紅色三角曲線、藍(lán)色圓點(diǎn)曲線分別表示有ZUPT和沒(méi)有ZUPT算法作用下的速度值比較,由圖可看出,在沒(méi)有ZUPT算法作用下的速度值會(huì)隨著時(shí)間的增大,誤差明顯增大,而在有ZUPT算法作用下的速度值并無(wú)明顯累計(jì)誤差.

圖4 基于零速更新的速度檢測(cè)比較

如圖5所示為基于ZUPT的步長(zhǎng)檢測(cè)算法進(jìn)行了實(shí)驗(yàn)測(cè)試.在3種不同的步速情況下對(duì)步長(zhǎng)進(jìn)行了估計(jì).最粗的曲線表示慢速行走,最細(xì)的曲線表示快速行走,粗細(xì)中等的曲線表示正常行走,實(shí)線表示實(shí)際的平均值,虛線表示利用算法進(jìn)行的估計(jì).從圖中可以看出,虛線所表示的檢測(cè)估計(jì)基本上在實(shí)線所表示的實(shí)際平均值上下浮動(dòng).

圖5 基于零速更新的步長(zhǎng)檢測(cè)算法測(cè)試結(jié)果

表1為在行走100米的情況下,3種步速的累積誤差,快速行走比慢走所測(cè)量的誤差小,正常行走的誤差在兩者之間.

表1 不同步速誤差比較(總計(jì)100米)

5.2 不同定位預(yù)測(cè)算法比較

圖6為不同定位方法的均方根誤差(RMSE)比較,步行距離為1000米,步行速度有快、慢、正常,實(shí)驗(yàn)在室外展開.“破折-點(diǎn)”線表示采用基于無(wú)線信號(hào)強(qiáng)度和行人航位推測(cè)的粒子濾波算法均方根誤差,細(xì)實(shí)線表示只采用行人航位推測(cè)的均方根誤差,粗實(shí)線表示只采用無(wú)線信號(hào)強(qiáng)度(AGPS+無(wú)線基站)實(shí)現(xiàn)定位的均方根誤差.從圖中可知,只采用無(wú)線信號(hào)強(qiáng)度(AGPS+無(wú)線基站)實(shí)現(xiàn)定位的均方根誤差在較大的誤差范圍內(nèi)波動(dòng),只采用行人航位推測(cè)實(shí)現(xiàn)定位的均方根誤差隨著步行推進(jìn),存在積累誤差.采用基于AGPS+無(wú)線基站和行人航位推測(cè)的粒子濾波算法實(shí)現(xiàn)的融合定位,均方根誤差隨著粒子推進(jìn),誤差明顯減小,在行走狀態(tài)下,誤差控制在0.8米以內(nèi).

為進(jìn)一步驗(yàn)證在GPS信號(hào)較弱或者無(wú)GPS信號(hào)下的定位精度,實(shí)驗(yàn)在室內(nèi)(室內(nèi)無(wú)GPS信號(hào))特定設(shè)置的5×5米區(qū)域進(jìn)行測(cè)試.基于AGPS+無(wú)線基站和行人航位推測(cè)的粒子濾波算法實(shí)現(xiàn)的融合定位實(shí)測(cè)數(shù)據(jù)如圖7所示.帶星曲線(紅色)為實(shí)測(cè)數(shù)據(jù),圓點(diǎn)直線(黑色)為理論數(shù)據(jù),數(shù)據(jù)表明,實(shí)測(cè)值基本在理論數(shù)據(jù)附近,精度可控制在1米以內(nèi),滿足實(shí)際定位精度要求.

圖6 不同定位方法的均方根誤差(RMSE)比較

圖7 基于信息融合的定位數(shù)據(jù)測(cè)試比較

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

本文提出一種AGPS/無(wú)線基站網(wǎng)絡(luò)、行人航位推測(cè)相結(jié)合的粒子濾波定位算法.在單獨(dú)采用AGPS或者基站定位方法時(shí),在沒(méi)有足夠觀測(cè)量而無(wú)法精確定位時(shí),通過(guò)AGPS/無(wú)線基站網(wǎng)絡(luò)、行人航位推測(cè)相結(jié)合的信息融合定位方法,實(shí)現(xiàn)了電網(wǎng)作業(yè)智能終端的高精度定位.實(shí)驗(yàn)表明,在室外環(huán)境,定位精度可控制在0.8米以內(nèi),在室內(nèi)精度可控制在1米以內(nèi).

1 Chien YR.Design of GPS anti-jamming systems using adaptive notch filters.IEEE Systems Journal,2015,9(2)：451-460.[doi：10.1109/JSYST.2013.2283753]

2 劉子政,李默穎,黃長(zhǎng)青,等.顧及速度約束的基于時(shí)間序列GPS漂移數(shù)據(jù)處理方法.測(cè)繪地理信息,2017,42(1)：14-18.

3 杜曉輝,任章.基于卡爾曼濾波的GPS靜態(tài)定位精度分析.全球定位系統(tǒng),2008,33(5)：47-51.

4 Yang Y,Yue XK,Dempster AG.GPS-based onboard realtime orbit determination for leo satellites using consider Kalman filter.IEEE Transactions on Aerospace and Electronic Systems,2016,52(2)：769-777.[doi：10.1109/TAES.2015.140758]

5 王素珍,劉樹坤,張德華,等.基于GPS定位導(dǎo)航的電力巡檢PDA系統(tǒng)研發(fā).山東農(nóng)業(yè)大學(xué)學(xué)報(bào)(自然科學(xué)版),2016,47(4)：540-547.

6 趙德安,賈偉寬,張?jiān)?等.農(nóng)業(yè)機(jī)器人自主導(dǎo)航改進(jìn)自適應(yīng)濾波控制器研究.農(nóng)業(yè)機(jī)械學(xué)報(bào),2015,46(5)：1-6.[doi：10.6041/j.issn.1000-1298.2015.05.001]

7 Li L,Quddus M,Zhao L.High accuracy tightly-coupled integrity monitoring algorithm for map-matching.Transportation Research Part C：Emerging Technologies,2013,36：13-26.[doi：10.1016/j.trc.2013.07.009]

8 Zhang YL,Chong KT.A GPS/DR data fusion method based on the GPS characteristics for mobile robot navigation.International Journal of Control and Automation,2014,7(10)：119-132.[doi：10.14257/ijca]

9 Sabet MT,Daniali HRM,Fathi AR,et al.Experimental analysis of a low-cost dead reckoning navigation system for a land vehicle using a robust AHRS.Robotics and Autonomous Systems,2017,95：37-51.[doi：10.1016/j.robot.2017.05.010]

10 Jo K,Lee M,Sunwoo M.Fast GPS-DR sensor fusion framework：Removing the geodetic coordinate conversion process.IEEE Transactions on Intelligent Transportation Systems,2016,17(7)：2008-2013.[doi：10.1109/TITS.2015.2475620]

11 張怡,席彥彪,李剛偉,等.基于卡爾曼濾波的TDOA/AOA混合定位算法.計(jì)算機(jī)工程與應(yīng)用,2015,51(20)：62-66.[doi：10.3778/j.issn.1002-8331.1310-0207]

12 Jwo DJ,Yang CF,Chuang CH,et al.Performance enhancement for ultra-tight GPS/INS integration using a fuzzy adaptive strong tracking unscented Kalman filter.Nonlinear Dynamics,2013,73(1-2)：377-395.[doi：10.1007/s11071-013-0793-z]

13 張秋昭,張書畢,鄭南山,等.GPS/INS組合系統(tǒng)的多重漸消魯棒容積卡爾曼濾波.中國(guó)礦業(yè)大學(xué)學(xué)報(bào),2014,43(1)：162-168.

14 Liu CF,Yang J,Wang FS.Joint TDOA and AOA location algorithm.Journal of Systems Engineering and Electronics,2013,24(2)：183-188.[doi：10.1109/JSEE.2013.00023]

15 龔福祥,王慶,張小國(guó).非視距環(huán)境下的GPS/TDOA最速下降混合定位算法.儀器儀表學(xué)報(bào),2011,32(7)：1500-1506.

16 龔福祥,王慶,張小國(guó).通信網(wǎng)絡(luò)中的GPS/TDOA混合定位算法.中國(guó)慣性技術(shù)學(xué)報(bào),2011,19(2)：209-213.

17 Kim H,Liu BB,Coh CY,et al.Robust vehicle localization using entropy-weighted particle filter-based data fusion of vertical and road intensity information for a large scale urban area.IEEE Robotics and Automation Letters,2017,2(3)：1518-1524.[doi：10.1109/LRA.2017.2673868]