應(yīng)用消息傳遞和卡爾曼模型的無(wú)線傳感網(wǎng)定位跟蹤算法

趙 恒,袁正道,,孫 鵬,張園園,吳 勝

(1.河南開(kāi)放大學(xué) 信息工程與人工智能學(xué)院,鄭州 450002;2.鄭州大學(xué) 電氣與信息工程學(xué)院,鄭州 450001;3.鄭州輕工業(yè)大學(xué) 電子信息學(xué)院,鄭州 450001;4.北京郵電大學(xué) 信息與通信工程學(xué)院,北京 100867)

0 引 言

在無(wú)線通信和物聯(lián)網(wǎng)等新一代信息技術(shù)的浪潮下,無(wú)線傳感網(wǎng)[1](Wireless Sensor Network,WSN)已經(jīng)融入到生活和工業(yè)的各個(gè)角落。WSN系統(tǒng)將傳感節(jié)點(diǎn)嵌入到目標(biāo)物中,測(cè)量物體參數(shù)信息,發(fā)送給處理終端收集。但通常傳感器節(jié)點(diǎn)采集到的只有目標(biāo)物的狀態(tài)參數(shù),如果能結(jié)合定位,則能挖掘出更有價(jià)值的信息,因此無(wú)線定位也成為了WSN系統(tǒng)的關(guān)鍵技術(shù)之一[2]。WSN系統(tǒng)經(jīng)常部署于室內(nèi)、戰(zhàn)場(chǎng)、密林等復(fù)雜環(huán)境,對(duì)定位系統(tǒng)的可靠性有較高要求。此外,待測(cè)目標(biāo)物(如車輛、行人)會(huì)不斷移動(dòng),如果能利用連續(xù)時(shí)間的位置信息(即跟蹤定位),則能極大提升定位的準(zhǔn)確度和可靠性。

目前較常用的短距離無(wú)線測(cè)距、定位方法有射頻身份識(shí)別、超寬帶[3](Ultra-wide Band,UWB)、WiFi、藍(lán)牙等。定位算法的基礎(chǔ)原理是利用信號(hào)強(qiáng)度[4]、到達(dá)時(shí)間(Time of Arrival,TOA)[5]測(cè)量目標(biāo)物與已知位置節(jié)點(diǎn)(也稱之為錨節(jié)點(diǎn))的距離,通過(guò)三角定位、最小二乘估計(jì)(Least Square,LS)[6]、最小均方誤差(Linear Minimum Mean Square Error,LMMSE)、質(zhì)心算法[7]等方法計(jì)算待測(cè)目標(biāo)物的坐標(biāo)。從算法的角度看,以上算法均屬于非貝葉斯類算法,忽略了測(cè)量誤差和噪聲的統(tǒng)計(jì)特性,未利用節(jié)點(diǎn)的先驗(yàn)信息和方差,在干擾較大的場(chǎng)景下會(huì)引起較大的性能損失[8]。

消息傳遞算法[9]是變分貝葉斯推理類方法的典型代表,近年來(lái)在圖像處理、信道估計(jì)、接收機(jī)設(shè)計(jì)[10]、信號(hào)處理[11]等領(lǐng)域得到了廣泛應(yīng)用。

近年來(lái)國(guó)內(nèi)外研究團(tuán)隊(duì)提出了多種基于因子圖和變分消息傳遞的貝葉斯類定位算法[12]。文獻(xiàn)[13]研究了基于貝葉斯測(cè)距和LS的定位算法,在測(cè)距階段使用變分貝葉斯方法消除噪聲干擾,在定位階段使用LS方法估計(jì)坐標(biāo)。文獻(xiàn)[14]提出了一種基于概率圖模型的單信標(biāo)定位算法,能夠在信標(biāo)數(shù)據(jù)缺失的情況下聯(lián)合其他位置的測(cè)量信息實(shí)現(xiàn)目標(biāo)定位。上述兩種方案利用了部分先驗(yàn)和方差信息,但是未考慮連續(xù)時(shí)間內(nèi)的相關(guān)性。文獻(xiàn)[15]研究了基于因子圖的動(dòng)態(tài)無(wú)線網(wǎng)絡(luò)協(xié)作定位算法,利用無(wú)線傳感網(wǎng)中錨節(jié)點(diǎn)和多個(gè)定位節(jié)點(diǎn)互通實(shí)現(xiàn)了高精度定位。但是該方案需要整個(gè)網(wǎng)絡(luò)中所有節(jié)點(diǎn)參與,復(fù)雜度較高,很難在實(shí)際場(chǎng)景中得到應(yīng)用。

為解決前述無(wú)線傳感網(wǎng)中的節(jié)點(diǎn)定位和跟蹤問(wèn)題,本文提出了一種融合了消息傳遞算法和卡爾曼[16-17]模型的解決方案,并搭建了基于超寬帶的測(cè)距跟蹤系統(tǒng),采集實(shí)際測(cè)距數(shù)據(jù),對(duì)所提算法和現(xiàn)有算法進(jìn)行實(shí)測(cè)對(duì)比驗(yàn)證。

本文的創(chuàng)新點(diǎn)可以歸納為如下兩個(gè)方面：一是將定位與卡爾曼模型融合,建立統(tǒng)一的概率分解和因子圖模型,利用連續(xù)時(shí)間內(nèi)坐標(biāo)的相關(guān)性實(shí)現(xiàn)了定位跟蹤;二是對(duì)卡爾曼模型參數(shù)進(jìn)行實(shí)時(shí)更新,使其能夠適用于更通用的場(chǎng)景。本質(zhì)上卡爾曼模型是將前一時(shí)刻定位結(jié)果作為本次定位的先驗(yàn)信息。本文所提方法是將定位和卡爾曼模型融合為一個(gè)整體,利用消息傳遞算法進(jìn)行更新,每一步計(jì)算都包含了方差項(xiàng),而方差是對(duì)估計(jì)值的不確定度,所形成的迭代算法更穩(wěn)定。該算法的性能增益也在仿真和實(shí)測(cè)環(huán)境中得到了驗(yàn)證。

1 系統(tǒng)建模

假定t時(shí)刻待測(cè)節(jié)點(diǎn)觀測(cè)到周圍N+個(gè)錨節(jié)點(diǎn)的位置信息(xn,yn,zn),n=1：N+,該節(jié)點(diǎn)t-1時(shí)刻估計(jì)得到的位置為(x(t-1),y(t-1),z(t-1)),如圖1所示。

圖1 動(dòng)態(tài)定位示意Fig.1 Schematic diagram of dynamic positioning

t時(shí)刻待測(cè)節(jié)點(diǎn)與第n個(gè)錨節(jié)點(diǎn)之間的歐氏距離計(jì)算為

(1)

根據(jù)待測(cè)節(jié)點(diǎn)的位置不會(huì)發(fā)生突變,可設(shè)定閾值剔除異常距離測(cè)量,假定剔除后的錨節(jié)點(diǎn)數(shù)量為N。設(shè)節(jié)點(diǎn)真實(shí)坐標(biāo)為(x(t),y(t),z(t)),將其向量化為

α(t)?(x(t),y(t),z(t))T

(2)

(3)

移項(xiàng)可得

(4)

將上式整理為觀測(cè)模型(5)(文獻(xiàn)中也稱之為線性混合模型)：

r(t)=Φ(t)α(t)+ω(t)

(5)

式中：

(6)

矩陣Φ(t)∈(N-1)×3定義為

(7)

由于坐標(biāo)x,y,z完全對(duì)等,此處以x為例進(jìn)行計(jì)算,并不失一般性。假定連續(xù)的觀測(cè)時(shí)間t內(nèi)坐標(biāo)具有相關(guān)性,表達(dá)式如下：

(8)

(9)

(10)

式中：U(a,b)表示上下界為[a,b]的均勻分布。

將式(5)中的矩陣運(yùn)算,利用全概率公式和變量間的馬爾可夫特性,將后驗(yàn)概率分布P(r,α,Φ,A,B)分解為似然函數(shù)與先驗(yàn)概率相乘的形式：

P(r,α,Φ,A,B)∝

P(α(0))P(A)P(B)=

(11)

(12)

表1 因式分解與函數(shù)表達(dá)式Tab.1 Factorization and corresponding function

將式(11)所示的因式分解和表1所示函數(shù)表達(dá)式用因子圖形式表示,如圖2所示。

圖2 因式分解(11)對(duì)應(yīng)因子圖Fig.2 Factor graph corresponding to Equation (11)

2 算法推導(dǎo)

圖3 坐標(biāo)x的相關(guān)因子圖Fig.3 Factor graph related to coordinate x

2.1 晶格部分消息更新

(13)

(14)

(15)

2.2 卡爾曼部分消息更新

(16)

(17)

(18)

(19)

(20)

在消息傳遞算法中,置信的物理意義為該變量的聯(lián)合后驗(yàn)概率,即式(20)中得到了在t時(shí)刻待測(cè)節(jié)點(diǎn)坐標(biāo)的估計(jì)。

2.3 卡爾曼參數(shù)估計(jì)部分消息更新

(21)

(22)

(23)

(24)

(25)

(26)

由于坐標(biāo)x,y,z完全獨(dú)立,則可以根據(jù)上述x坐標(biāo)的估計(jì)步驟類比得到坐標(biāo)y,z的更新方法。

2.4 消息迭代算法歸納

本節(jié)將前述因子圖中所有消息迭代計(jì)算、初始化和迭代次序、結(jié)束條件等歸納為消息迭代算法。由于x,y,z坐標(biāo)完全對(duì)等,在算法中僅以x坐標(biāo)為例,坐標(biāo)y,z可以類比得到。

自適應(yīng)卡爾曼濾波定位算法偽代碼如下：

Fort=1：Q

For Iter=1：TGlob

10 ?n,由式(20)更新置信b(x(t));

11 ?n,類比得到對(duì)坐標(biāo)y,z的估計(jì);

end For

end For

2.5 復(fù)雜度分析

本文所提基于消息傳遞和卡爾曼濾波的定位算法復(fù)雜度較低,每次迭代僅需要計(jì)算有限次的乘加運(yùn)算和指數(shù)運(yùn)算。針對(duì)t時(shí)刻單次迭代的復(fù)雜度分析如下：晶格部分消息更新所需乘-加運(yùn)算次數(shù)為O(3N),N表示錨節(jié)點(diǎn)個(gè)數(shù);卡爾曼濾波部分與錨節(jié)點(diǎn)無(wú)關(guān),僅需要O(3)次乘-加運(yùn)算;卡爾曼參數(shù)估計(jì)部分,假設(shè)參數(shù)A,B與最近的Q次觀測(cè)相關(guān),則其復(fù)雜度為O(3Q)。

3 仿真性能對(duì)比

建立一個(gè)長(zhǎng)寬均為5 m、高為1 m的虛擬空間,空間的內(nèi)壁上以1 m為間隔部署錨節(jié)點(diǎn)。為模擬真實(shí)環(huán)境,假定在安裝錨節(jié)點(diǎn)時(shí)存在一些裝配誤差(誤差服從[-0.05 0.05]m的均勻分布)。為測(cè)試跟蹤算法的有效性,仿真中設(shè)待測(cè)目標(biāo)沿x,y平面對(duì)角線勻速移動(dòng),同時(shí)在z軸上以正弦周期移動(dòng),運(yùn)動(dòng)速度為1 m/s。此外,假定目標(biāo)物的移動(dòng)速度還加入了服從[-0.05 0.05]m/s均勻分布的隨機(jī)抖動(dòng)。圖4給出了虛擬空間和目標(biāo)物示意圖,圖中星號(hào)代表在上下墻壁上部署的錨節(jié)點(diǎn),藍(lán)色軌跡代表待測(cè)目標(biāo)物的運(yùn)動(dòng)曲線。

圖4 錨節(jié)點(diǎn)部署位置和待測(cè)設(shè)備移動(dòng)軌跡Fig.4 Anchor node position and the device moving path

(27)

式中：wn為白高斯噪聲;測(cè)距信噪比為4～16 dB。仿真中選擇有效錨節(jié)點(diǎn)個(gè)數(shù)為4～12,測(cè)量間隔為5次/s。為了驗(yàn)證算法的可靠性,仿真中模擬50次獨(dú)立測(cè)量,然后對(duì)估計(jì)結(jié)果進(jìn)行平均。為驗(yàn)證本文所提算法的有效性,仿真中還與文獻(xiàn)中已有的質(zhì)心算法、三點(diǎn)質(zhì)心法、最小二乘法、最小均方誤差(LMMSE)方法進(jìn)行對(duì)比。算法的定位精度由均方誤差(Mean Square Error,MSE)衡量,仿真中定義

(28)

圖5展示了本文所提算法的定位MSE隨測(cè)距信噪比的變化曲線,可以看出,本文所提自適應(yīng)定位算法能夠極大地提升定位精度。當(dāng)有效錨節(jié)點(diǎn)N=4時(shí),相對(duì)于最小二乘估計(jì),本文所提自適應(yīng)算法性能僅提升0.5 dB;而當(dāng)N為6～8時(shí),性能提升可達(dá)4 dB,充分驗(yàn)證了自適應(yīng)卡爾曼預(yù)測(cè)的有效性。

圖5 定位精度隨信噪比變化曲線Fig.5 Positioning accuracy varies with SNR

為了更直觀展示定位性能與錨節(jié)點(diǎn)個(gè)數(shù)的關(guān)系,圖6給出了定位精度隨N的變化曲線。從圖6中可以看出,錨節(jié)點(diǎn)個(gè)數(shù)為4時(shí),本文所提自適應(yīng)算法對(duì)精度提升有限;當(dāng)錨節(jié)點(diǎn)為6時(shí),性能增益約為4 dB;當(dāng)錨節(jié)點(diǎn)個(gè)數(shù)增加至8以上后,本文所提算法相比最小二乘估計(jì)和最小均方誤差估計(jì)的性能增益穩(wěn)定在2 dB。

圖6 定位精度隨有效錨節(jié)點(diǎn)個(gè)數(shù)變化曲線Fig.6 Positioning accuracy vs. anchor nodes

4 實(shí)測(cè)性能對(duì)比

為了驗(yàn)證本文所提算法的有效性,在室內(nèi)搭建了基于超寬帶的室內(nèi)二維定位跟蹤系統(tǒng),利用實(shí)測(cè)數(shù)據(jù)進(jìn)行測(cè)試。選擇的硬件系統(tǒng)為廣州聯(lián)網(wǎng)科技的室內(nèi)超寬帶定位模組DWM-PG+,包含6個(gè)錨節(jié)點(diǎn)和1個(gè)待測(cè)節(jié)點(diǎn)。錨節(jié)點(diǎn)橫向間隔2.5 m,縱向間隔2 m。為保證定位標(biāo)簽勻速運(yùn)動(dòng),將定位標(biāo)簽安裝于尋跡小車上,以約0.23 m/s的速度勻速沿黑色Z字形軌跡運(yùn)動(dòng),如圖7所示。

圖7 基于超寬帶的室內(nèi)定位系統(tǒng)實(shí)測(cè)場(chǎng)地Fig.7 Field measure of positioning system based on UWB

定位標(biāo)簽的測(cè)距間隔為40 ms,即測(cè)距頻率為25 Hz。每個(gè)錨節(jié)點(diǎn)獨(dú)立測(cè)量與定位標(biāo)簽的距離,匯聚到E基站后通過(guò)串口上傳至上位機(jī)。圖8為定位系統(tǒng)的上位機(jī)軟件,通過(guò)串口接收所有錨節(jié)點(diǎn)的測(cè)距結(jié)果,通過(guò)最小均方誤差和卡爾曼濾波得到目標(biāo)節(jié)點(diǎn)坐標(biāo)并顯示。為了計(jì)算各種算法估計(jì)坐標(biāo)與真實(shí)坐標(biāo)比較的均方誤差值,對(duì)如圖8所示的實(shí)測(cè)數(shù)據(jù)進(jìn)行截取,僅保留圖中3段直線位置數(shù)據(jù),使得真實(shí)坐標(biāo)(x(t),y(t)),t=1：T能夠計(jì)算得到。

圖8 定位跟蹤系統(tǒng)上位機(jī)軟件Fig.8 Host software of positioning and tracking system

(29)

經(jīng)過(guò)5次實(shí)測(cè)計(jì)算取平均得到的MSE值如表2所示,可以看出,除質(zhì)心類算法以外,其他算法的定位MSE均達(dá)到-5 dB以上,實(shí)測(cè)定位誤差也均在厘米級(jí),完全能夠滿足室內(nèi)定位的需要。由于基站安裝位置偏差、尋跡行走偏差、疊加噪聲等因素,實(shí)測(cè)定位的MSE并未達(dá)到仿真中的-20 dB,但是從表2中仍能夠看出本文所提定位算法能夠有效提高定位跟蹤精度。

表2 實(shí)測(cè)數(shù)據(jù)MSE對(duì)比Tab.2 MSE comparison of field measured data

從實(shí)測(cè)數(shù)據(jù)可以看出,由于本文所提方法利用卡爾曼濾波的結(jié)構(gòu)發(fā)掘相鄰時(shí)刻的定位信息,即將上一時(shí)刻的位置作為先驗(yàn)信息提供給本時(shí)刻的定位估計(jì),能夠獲得額外的性能增益。以上結(jié)論也在本文的仿真和實(shí)測(cè)中得到了驗(yàn)證。

5 結(jié) 論

為解決WSN系統(tǒng)中的節(jié)點(diǎn)定位問(wèn)題,本文提出了一種融合了卡爾曼模型和因子圖的跟蹤定位算法,充分發(fā)掘相鄰觀測(cè)坐標(biāo)中所隱藏信息,作為定位的先驗(yàn)信息提供給本次位置估計(jì)。由于將定位、卡爾曼濾波和參數(shù)估計(jì)作為整體進(jìn)行優(yōu)化,相較于分開(kāi)處理類方法,本文所提算法能夠做到全局最優(yōu)。在仿真中,本文建立虛擬環(huán)境對(duì)所提算法和文獻(xiàn)中已有方法進(jìn)行了對(duì)比,結(jié)果表明本文所提算法具有2 dB的性能增益。最后搭建室內(nèi)實(shí)測(cè)環(huán)境,采集6個(gè)錨節(jié)點(diǎn)的實(shí)時(shí)測(cè)距結(jié)果并對(duì)比了所有算法的定位結(jié)果,實(shí)測(cè)數(shù)據(jù)也驗(yàn)證了本文所提算法的性能優(yōu)勢(shì)。