基于最優(yōu)信標(biāo)組的擴(kuò)展卡爾曼定位算法*

孟文超，俞 立，董齊芬，王 銘

(浙江工業(yè)大學(xué)信息工程學(xué)院，杭州310023)

無線傳感器網(wǎng)絡(luò)(Wireless Sensor Networks)是一門集微電子、通信、計(jì)算機(jī)和傳感器技術(shù)為一體的技術(shù)，具有廣泛的應(yīng)用領(lǐng)域。隨著無線傳感器網(wǎng)絡(luò)的快速發(fā)展，傳感器節(jié)點(diǎn)的位置信息在WSNs的諸多應(yīng)用中扮演著不可或缺的重要角色，如在環(huán)境監(jiān)測、戰(zhàn)場偵察、目標(biāo)跟蹤等應(yīng)用中，監(jiān)測數(shù)據(jù)只有與節(jié)點(diǎn)位置信息綁定才有意義［1］。另外，節(jié)點(diǎn)位置信息也是大多數(shù)路由協(xié)議的基礎(chǔ)［2－3］。

近年來，國內(nèi)外學(xué)者對無線傳感網(wǎng)定位技術(shù)進(jìn)行了深入研究。根據(jù)定位過程中是否測量節(jié)點(diǎn)間的距離，定位算法主要分為基于測距算法和無需測距算法［4］。無需測距的定位方法僅根據(jù)網(wǎng)絡(luò)的連通性來定位，定位精度在一定程序上依賴于網(wǎng)絡(luò)平均每跳距離的估計(jì)精度，而且當(dāng)網(wǎng)絡(luò)的拓?fù)浣Y(jié)構(gòu)不規(guī)則時(shí)，算法的性能急劇下降。如DV-Hop算法、APIT算法、質(zhì)心算法等［5］。基于測距的定位算法通過測量節(jié)點(diǎn)間距離或角度信息完成定位過程，如基于接收信號強(qiáng)度衰減的定位(RSSI)、基于到達(dá)時(shí)間的定位(TOA)、基于到達(dá)時(shí)間差的定位(TDOA)和基于角度的定位(AOA)?；跍y距算法的定位精度一般高于無需測距算法，但一般需要額外硬件的支持［6］。

本文通過分析基于RSSI的定位技術(shù)存在的問題，提出一種新的基于最優(yōu)信標(biāo)組的擴(kuò)展卡爾曼定位算法(BBG-EKF)。該算法在分析未知節(jié)點(diǎn)與信標(biāo)節(jié)點(diǎn)的距離及信標(biāo)節(jié)點(diǎn)間的共線性對定位精度影響的基礎(chǔ)上，提出一種最優(yōu)信標(biāo)組選擇機(jī)制，進(jìn)而通過擴(kuò)展卡爾曼濾波實(shí)現(xiàn)定位，這樣既提供了較強(qiáng)的環(huán)境干擾魯棒性，又沒有增加網(wǎng)絡(luò)通信開銷。實(shí)驗(yàn)結(jié)果表明，本文提出的定位算法在定位精度、通信量和環(huán)境干擾魯棒性等方面具有優(yōu)越性。

論文其余部分安排如下:第1部分，介紹基于RSSI的測距原理;第2部分，分析影響定位精度的因素，并在此基礎(chǔ)上提出一種選擇最優(yōu)信標(biāo)組機(jī)制;第3部分，引入擴(kuò)展卡爾曼濾波消除RSSI測量中的環(huán)境噪聲;最后是算法仿真與總結(jié)。

1 RSSI測距模型

為適應(yīng)實(shí)際環(huán)境中存在的多徑效應(yīng)、繞射、障礙物等因素，本文利用RSSI值并結(jié)合傳播路徑損耗經(jīng)驗(yàn)?zāi)Ｐ陀?jì)算未知節(jié)點(diǎn)到信標(biāo)節(jié)點(diǎn)的距離。通常引入隨機(jī)干擾的對數(shù)－正態(tài)分布模型作為無線傳感器網(wǎng)絡(luò)路徑損耗模型［12］。

式中，d是發(fā)射節(jié)點(diǎn)與接收節(jié)點(diǎn)之間的距離，P(d)是距離為d時(shí)的接收信號強(qiáng)度，d0為參考距離，通常取1 m，P0(d0)是距離為d0時(shí)的接收信號強(qiáng)度，Xσ的均值為0、標(biāo)準(zhǔn)差為σ的高斯白噪聲，n是與環(huán)境相關(guān)的路徑損耗指數(shù)。由于不同的環(huán)境對測距有較大影響，為了得到更高的定位精度，需要針對特定的環(huán)境，通過實(shí)驗(yàn)獲得相應(yīng)的n和Xσ。

2 信標(biāo)節(jié)點(diǎn)選擇

無論是基于測距的定位還是測距無關(guān)的定位，都要利用信標(biāo)節(jié)點(diǎn)。一般來說，可選擇的信標(biāo)節(jié)點(diǎn)信息越多，估計(jì)的位置越準(zhǔn)確。另外，信標(biāo)節(jié)點(diǎn)之間的位置拓?fù)潢P(guān)系及未知節(jié)點(diǎn)與信標(biāo)節(jié)點(diǎn)之間的距離都在很大程度上影響定位結(jié)果。本節(jié)主要從這兩方面分析它們對定位精度的影響，為合理選擇最優(yōu)信標(biāo)節(jié)點(diǎn)提供依據(jù)。

2．1 未知節(jié)點(diǎn)與信標(biāo)節(jié)點(diǎn)之間的距離

在基于RSSI測距的定位技術(shù)中，參與定位的信標(biāo)節(jié)點(diǎn)數(shù)越多，定位精度越高。但是，當(dāng)信標(biāo)組中存在遠(yuǎn)離未知節(jié)點(diǎn)的信標(biāo)節(jié)點(diǎn)時(shí)，定位效果將變差。定義距離估計(jì)誤差即發(fā)射節(jié)點(diǎn)與接收節(jié)點(diǎn)的估計(jì)距離與真實(shí)距離 d的差值為，則=－d。由式(1)易得與 d、Xσ的關(guān)系式為:

從式(3)可以看出:當(dāng)RSSI測距模型中的σ2及n確定時(shí)，距離測量誤差與發(fā)射節(jié)點(diǎn)和接收節(jié)點(diǎn)間的距離成正比，距離越遠(yuǎn)，距離測量誤差越大。為了分析距離對定位精度的影響，考慮未知節(jié)點(diǎn)與信標(biāo)節(jié)點(diǎn)的距離分別從1 m至10 m變化，測距誤差隨距離變化的結(jié)果如圖1所示。

圖1 距離誤差分布

仿真結(jié)果表明:在基于RSSI的測距技術(shù)中，由于誤差的累積性，當(dāng)接收節(jié)點(diǎn)遠(yuǎn)離發(fā)射節(jié)點(diǎn)時(shí)，距離測量誤差也會(huì)隨之增加，從而影響定位精度。因此，為了優(yōu)化定位效果，應(yīng)選擇未知節(jié)點(diǎn)最近的K個(gè)信標(biāo)節(jié)點(diǎn)進(jìn)行定位。理論上，信標(biāo)節(jié)點(diǎn)與未知節(jié)點(diǎn)的距離越近，RSSI值越大，所以在定位過程中，直接選擇RSSI值最大的K個(gè)信標(biāo)節(jié)點(diǎn)。

2．2 信標(biāo)節(jié)點(diǎn)的共線度

在二維空間中，未知節(jié)點(diǎn)只需獲得三個(gè)信標(biāo)節(jié)點(diǎn)的位置信息，就可以確定自身的位置。然而，當(dāng)3個(gè)信標(biāo)節(jié)點(diǎn)共線或接近共線時(shí)，較小的測距誤差都會(huì)導(dǎo)致很大的定位誤差［13］。如圖2所示:當(dāng)信標(biāo)節(jié)點(diǎn)B1、B2、B3幾乎在一條直線上時(shí)，信標(biāo)節(jié)點(diǎn)通信圓周的交點(diǎn)不唯一，也就無法確定未知節(jié)點(diǎn)究竟是在N1還是在N2。

圖2 信標(biāo)節(jié)點(diǎn)共線情況

為了解決信標(biāo)節(jié)點(diǎn)的共線問題，引入共線度NC的定義。共線度NC定義為信標(biāo)節(jié)點(diǎn)組成的三角形中最小角的余弦值。如圖3(a)所示，A、B、C是三個(gè)信標(biāo)節(jié)點(diǎn)，∠A是三角形ABC中最小角，則用∠A的余弦值來衡量信標(biāo)節(jié)點(diǎn)組的共線程度，即:

因?yàn)楣簿€度定義為三角形最小角的余弦值，最小角的取值范圍為0°～60°，對應(yīng)共線度NC的取值范圍為0．5～1。當(dāng)NC值等于0．5時(shí)，代表信標(biāo)節(jié)點(diǎn)組成的三角形是等邊三角形，此時(shí)用這組信標(biāo)節(jié)點(diǎn)進(jìn)行定位，效果最好。當(dāng)NC值等于1時(shí)，代表信標(biāo)節(jié)點(diǎn)組在一條直線上，此時(shí)定位效果最差。

圖3 共線度的定義與實(shí)驗(yàn)示意圖

如圖3(b)所示，為了分析共線度對定位精度的影響，保持信標(biāo)節(jié)點(diǎn)A、B的距離不變，通過改變信標(biāo)C的位置來獲得不同的NC值，信標(biāo)節(jié)點(diǎn)C從AB的中點(diǎn)移動(dòng)到三個(gè)信標(biāo)節(jié)點(diǎn)構(gòu)成等邊三角形的位置，未知節(jié)點(diǎn)N放置在等邊三角形ABC的中心。采用傳統(tǒng)的極大似然估計(jì)對未知節(jié)點(diǎn)N進(jìn)行定位，得到共線度與定位精度的關(guān)系如圖4所示。實(shí)驗(yàn)結(jié)果表明:平均定位誤差隨著共線度NC的增大而增大，當(dāng)NC小于0．75時(shí)，定位性能比較穩(wěn)定，而在NC大于0．75時(shí)，定位性能隨著NC的增大發(fā)生明顯惡化。為了提高定位精度，設(shè)定共線度閾值CM，篩選NC≤CM的信標(biāo)節(jié)點(diǎn)組。

圖4 共線度與定位精度的關(guān)系

2．3 最優(yōu)信標(biāo)組

由以上分析可知，信標(biāo)節(jié)點(diǎn)距離未知節(jié)點(diǎn)越遠(yuǎn)，信標(biāo)節(jié)點(diǎn)間共線度越大，未知節(jié)點(diǎn)的平均定位誤差越大，這為合理選擇最優(yōu)信標(biāo)組提供了依據(jù)，最優(yōu)信標(biāo)組應(yīng)選擇未知節(jié)點(diǎn)附近共線度最小的一組信標(biāo)組。

圖5 最優(yōu)信標(biāo)組

為減少計(jì)算復(fù)雜度，定義一組信標(biāo)組包含三個(gè)信標(biāo)節(jié)點(diǎn)。最優(yōu)信標(biāo)組的選擇如圖5所示，離未知節(jié)點(diǎn)N最近的四個(gè)信標(biāo)節(jié)點(diǎn)分別是A、B、C、D，組成ABC、ABD、BCD、ACD等四個(gè)導(dǎo)標(biāo)三角形，根據(jù)共線度的定義易知三角形ACD的共線度最小，為未知節(jié)點(diǎn)N的最優(yōu)信標(biāo)節(jié)點(diǎn)組。

3 擴(kuò)展卡爾曼定位

理論上，根據(jù)上述信標(biāo)選擇機(jī)制確定了最優(yōu)信標(biāo)組后，利用極大似然法即可得到較高精度的定位效果。然而，實(shí)際環(huán)境中，噪聲、多徑效應(yīng)、非視線關(guān)系等條件的變化使得實(shí)際接收到的信號強(qiáng)度存在比較大的誤差，該誤差表現(xiàn)為高斯白噪聲，影響定位結(jié)果?？柭鼮V波［14－15］在高斯白噪聲的動(dòng)態(tài)系統(tǒng)中是一種最優(yōu)濾波器，可以有效地抑制RSSI測量中噪聲，增強(qiáng)環(huán)境干擾魯棒性，提高定位精度，本節(jié)分析利用基于擴(kuò)展卡爾曼的定位算法。

3．1 擴(kuò)展卡爾曼模型

(1)狀態(tài)方程模型

式中，xk=(xk，yk)T表示未知節(jié)點(diǎn)在第k－1個(gè)信標(biāo)位置進(jìn)行濾波計(jì)算后的坐標(biāo)向量，wk是系統(tǒng)噪聲，服從N(0，Qk)高斯分布，φk是系統(tǒng)矩陣，在未知節(jié)點(diǎn)的定位過程中，認(rèn)為未知節(jié)點(diǎn)的位置是固定的，所以φk為二階單位矩陣，系統(tǒng)噪聲方差Qk=0。

(2)測量方程模型

文中選用未知節(jié)點(diǎn)到信標(biāo)節(jié)點(diǎn)之間的距離作為EKF的測量值，測量方程為

式中，di為未知節(jié)點(diǎn)到第i個(gè)信標(biāo)節(jié)點(diǎn)的距離，它是根據(jù)1節(jié)描述的測距方法測得，(Xi，Yi)是第i個(gè)信標(biāo)節(jié)點(diǎn)的坐標(biāo)，(x，y)是未知節(jié)點(diǎn)的坐標(biāo)，vi是測量噪聲，服從均值為零，方差為R的高斯分布。通過在平衡點(diǎn)展開泰勒級數(shù)對測量方程線性化處理，平衡點(diǎn)取上一次濾波后未知節(jié)點(diǎn)的坐標(biāo)向量，線性化后的測量方程為:

圖6 EKF定位流程圖

擴(kuò)展卡爾曼定位的流程圖如圖6所示，主要包括狀態(tài)變量及誤差方差陣初始化、狀態(tài)方程及測量方程線性化、計(jì)算卡爾曼增益、更新狀態(tài)變量、預(yù)測等五個(gè)步驟。狀態(tài)初值和初始狀態(tài)誤差方差陣的正確選擇，對于EKF的收斂速度和定位精度有很大的影響，為了減少算法的復(fù)雜度，初值任意選取，而不是采用特定定位算法給出比較準(zhǔn)確的值，但同時(shí)為加快EKF的收斂速度，本文選取比較大的初始狀態(tài)及誤差協(xié)方差。

4 算法過程

(1)信標(biāo)節(jié)點(diǎn)周期性發(fā)送包含節(jié)點(diǎn)ID、位置坐標(biāo)信息包。

(2)未知節(jié)點(diǎn)收到信標(biāo)節(jié)點(diǎn)的信息后，對信標(biāo)節(jié)點(diǎn)依其RSSI值從大到小排序，并建立RSSI值與未知節(jié)點(diǎn)到信標(biāo)節(jié)點(diǎn)距離的映射，建立3個(gè)集合。

信標(biāo)節(jié)點(diǎn)集合:

未知節(jié)點(diǎn)到信標(biāo)節(jié)點(diǎn)距離集合:

信標(biāo)節(jié)點(diǎn)位置集合:

(3)未知節(jié)點(diǎn)首先從信標(biāo)集合Beacon_set中選取離未知節(jié)點(diǎn)最近的4個(gè)節(jié)點(diǎn)，然后每3個(gè)信標(biāo)節(jié)點(diǎn)組成一組信標(biāo)組，總共可以分成C34組，分別計(jì)算各個(gè)信標(biāo)組的共線度后，選取共線度最小的一組作為最優(yōu)信標(biāo)組。

(4)未知節(jié)點(diǎn)坐標(biāo)向量的初值任意取定，初始狀態(tài)誤差協(xié)方差P0取值偏大些，使EKF盡快收斂于真實(shí)值。把x0，P0代入圖6的流程圖中，迭代到指定次數(shù)或者滿足定位精度要求為止。

5 仿真實(shí)驗(yàn)

為了檢驗(yàn)BBG-EKF算法的可行性和有效性，本部分主要對算法進(jìn)行仿真。仿真在Matlab平臺上進(jìn)行，默認(rèn)的仿真參數(shù)P0(d0)=5，σ=4，并通過與擴(kuò)展卡爾曼定位算法(EKF)、基于最優(yōu)信標(biāo)組的極大似然估計(jì)算法(BBG-MLE)、極大似然估計(jì)算法(MLE)做比較，分析該算法的性能。不失一般性，假設(shè)200個(gè)未知節(jié)點(diǎn)和一定數(shù)目的信標(biāo)節(jié)點(diǎn)隨機(jī)部署于50 m×50 m的正方形區(qū)域C中，當(dāng)未知節(jié)點(diǎn)通信范圍內(nèi)少于3個(gè)信標(biāo)節(jié)點(diǎn)，采用弗洛伊德算法(Floyd Algorithm)獲得與其它信標(biāo)節(jié)點(diǎn)的最短距離。

圖7是通信半徑R=15、信標(biāo)節(jié)點(diǎn)數(shù)M=20時(shí)BBG-EKF的定位效果，其中圓圈表示未知節(jié)點(diǎn)，方塊表示信標(biāo)節(jié)點(diǎn)，直線連接未知節(jié)點(diǎn)的真實(shí)坐標(biāo)和它的估計(jì)坐標(biāo)，直線越長，定位誤差越大。仿真結(jié)果表明:BBG-EKF的平均定位誤差約為1．5 m，C1區(qū)域中，信標(biāo)節(jié)點(diǎn)的共線度較小，未知節(jié)點(diǎn)的定位誤差偏小，相反，C2區(qū)域中，信標(biāo)節(jié)點(diǎn)幾乎在同一條直線上，未知節(jié)點(diǎn)的定位誤差偏大，同時(shí)受邊緣效應(yīng)的影響，在區(qū)域邊緣的平均定位誤差明顯偏大。

圖7 BBG-EKF定位效果

擴(kuò)展卡爾曼定位算法的定位精度受迭代次數(shù)的影響，迭代次數(shù)越多，定位精度越高。圖8是坐標(biāo)為(5，5)的未知節(jié)點(diǎn)分別采用BBG-EKF及BBG-MLE的定位效果，仿真結(jié)果表明，BBG-EKF定位算法在迭代15次左右收斂于真實(shí)坐標(biāo)，而BBG-MLE定位算法始終發(fā)散。

圖8 迭代次數(shù)影響

如圖9所示是不同通信半徑下平均定位誤差的比較，其中信標(biāo)節(jié)點(diǎn)占25%。在一定范圍內(nèi)，通信半徑與網(wǎng)絡(luò)連通度成正比，即節(jié)點(diǎn)通信半徑越大，平均每個(gè)未知節(jié)點(diǎn)周圍可通信的節(jié)點(diǎn)個(gè)數(shù)越多。仿真實(shí)驗(yàn)表明:BBG-EKF算法、EKF算法、MLE算法的定位準(zhǔn)確度均隨著網(wǎng)絡(luò)連通度的增大逐漸提高，BBGMLE雖然存在最優(yōu)信標(biāo)選擇機(jī)制，但由于沒有擴(kuò)展卡爾曼濾波過程，當(dāng)通信半徑大于25 m時(shí)平均定位誤差反而增大。與其他三種算法相比BBG-EKF算法穩(wěn)定性最好，在網(wǎng)絡(luò)連通度變化的情況下，都具有較高的定位精度。

圖9 通信半徑與定位精度的關(guān)系

圖10是R=25時(shí)信標(biāo)節(jié)點(diǎn)密度對定位誤差的影響。平均定位誤差隨著信標(biāo)節(jié)點(diǎn)密度的增加而降低，在同樣信標(biāo)節(jié)點(diǎn)密度的情況下，BBG-EKF算法具有更好的定位性能，比如在信標(biāo)密度為15%時(shí)，BBG-EKF算法平均定位誤差約為0．37 m，而EKF算法平均定位誤差約為0．52 m，BBG-MLE算法平均定位誤差約為1．42 m，MLE算法平均定位誤差約為0．98 m。另外，BBG-EKF對信標(biāo)節(jié)點(diǎn)密度要求不高，信標(biāo)節(jié)點(diǎn)密度為10%時(shí)算法平均定位誤差為0.6 m左右，所以，BBG-EKF不僅具有很好的定位效果，而且適用于信標(biāo)節(jié)點(diǎn)稀疏的環(huán)境中。

圖10 信標(biāo)節(jié)點(diǎn)密度與定位精度的關(guān)系

6 總結(jié)

本文采用無線傳感網(wǎng)中的RSSI測距技術(shù)，提出一種基于最優(yōu)信標(biāo)組的擴(kuò)展卡爾曼定位算法(BBGEKF)，該算法研究了未知節(jié)點(diǎn)與信標(biāo)節(jié)點(diǎn)的距離、信標(biāo)節(jié)點(diǎn)的共線性對定位精度的影響，為合理選擇最優(yōu)信標(biāo)節(jié)點(diǎn)組進(jìn)行定位提供了一種方法，最后通過擴(kuò)展卡爾曼定位技術(shù)獲得未知節(jié)點(diǎn)的位置信息。仿真實(shí)驗(yàn)表明，本文提出的基于最優(yōu)信標(biāo)組的擴(kuò)展卡爾曼定位算法在沒有增加網(wǎng)絡(luò)開銷的情況下，提高了定位精度，在不同通信半徑及信標(biāo)節(jié)點(diǎn)密度的網(wǎng)絡(luò)中都表現(xiàn)中良好的性能。

［1］ 孫利民．無線傳感器網(wǎng)絡(luò)［M］．北京:清華大學(xué)出版社，2005:137－140．

［2］ Capkun S，Hamdi M，Hubaux J P．GPS-Free Positioning in Mobile Ad-Hoc Networks［J］．Cluster Computing，2002，5(2):161 －176．

［3］ Niculescu D，Nath B．Trajectory Based Forwarding and its Applications［C］//Proceedings of the 9th Annual International Conference on Mobile computing and Networking．ACM New York，NY，USA，2003:260－272．

［4］ 劉新華，李方敏．一種分布式的無線傳感器網(wǎng)絡(luò)Range-free節(jié)點(diǎn)定位算法［J］．傳感技術(shù)學(xué)報(bào)，2008，21(1):154 －157．

［5］ 田增山，羅磊，何維．一種分布式的無線傳感器網(wǎng)絡(luò)節(jié)點(diǎn)定位算法［J］．傳感技術(shù)學(xué)報(bào)，2009，22(3):387 －390．

［6］ Neal P，Joshua N，Spyros K，et al．Locating the Nodes:Cooperative Localization in Wireless Sensor Networks［J］．IEEE Signal Processing Magazine，2005，22(4):54 －69．

［7］ Mao G，F(xiàn)idan B，Anderson B．Wireless Sensor Network Localization Techniques［J］．Computer Networks，2007，51(10):2529 －2553．

［8］ 楊東勇，顧東袁，傅曉婕．一種基于RSSI相似度的室內(nèi)定位算法［J］．傳感技術(shù)學(xué)報(bào)，2009，22(2):264 －268．

［9］ Yi J，Yang S．Multi-Hop Based Monte Carlo Localization for Mobile Sensor Networks［C］//Proceedings of the 9th Annual IEEE Communications Society Conference on Sensor，Mesh and Ad Hoc Communications and Networks，San Diego，CA，2007:162 －171．

［10］ Poqqi C，Mazzini G．Collinearity for Sensor Network Localization［C］//IEEE 58th Vehicular Technology Conference，F(xiàn)errara，IT，2003:3040－3044．

［11］ Wang C L，Chiou Y S，Dai Y S．An Adaptive Location Estimator Based Kalman Filtering for Wireless Sensor Networks［C］//IEEE 65th Vehicular Technology Conference，Dublin，IR，2007:864 －868．

［12］ Wang J，Ghosh R K．A Survey on Sensor Localization［J］．Journal of Control Theory and Applications，2010，8(1):2 －11．

［13］吳凌飛，孟慶虎，梁華為．一種基于共線度的無線傳感器網(wǎng)絡(luò)定位算法［J］．傳感技術(shù)學(xué)報(bào)，2009，22(5):722 －727．

［14］ Sreenath K，Lewis F L，Popa D O．Simultaneous Adaptive Localization of a Wireless Sensor Network［J］．Mobile Computing and Communications Review，2007，11(2):14 －28．

［15］ Yim J，Park C，Joo J．Extended Kalman Filter for Wireless LAN Based Indoor Positioning［J］．Decision Support Systems，2008，45(4):960－971．