段世紅 姚 翠 徐 誠(chéng) 何 杰

(北京科技大學(xué)計(jì)算機(jī)與通信工程學(xué)院 北京 100083) (材料領(lǐng)域知識(shí)工程北京市重點(diǎn)實(shí)驗(yàn)室(北京科技大學(xué)) 北京 100083) (duansh@ustb.edu.cn)

無(wú)線傳感器網(wǎng)絡(luò)(wireless sensor network, WSN)由大量傳感器節(jié)點(diǎn)組成，是一種全新的信息獲取和處理技術(shù)，在現(xiàn)實(shí)生活中得到了越來(lái)越廣泛的應(yīng)用，如軍事領(lǐng)域中實(shí)時(shí)監(jiān)視戰(zhàn)場(chǎng)狀況、生態(tài)領(lǐng)域中環(huán)境監(jiān)測(cè)和生物種群研究、工業(yè)領(lǐng)域中建筑物安全狀況的檢測(cè)等.其中的許多監(jiān)測(cè)信息需要附帶相應(yīng)的位置信息，因此傳感器網(wǎng)絡(luò)中節(jié)點(diǎn)位置信息的獲取至關(guān)重要.無(wú)線傳感器網(wǎng)絡(luò)最簡(jiǎn)單的定位方法是為每個(gè)節(jié)點(diǎn)裝載全球定位系統(tǒng)(global positioning system, GPS)接收器[1]，用以確定節(jié)點(diǎn)位置.但是由于經(jīng)濟(jì)因素、節(jié)點(diǎn)能量制約以及GPS對(duì)部署環(huán)境有一定條件的限制，導(dǎo)致這種方案的可行性較差.目前采用最多的定位方法是基于測(cè)距的方法[1]，比較主流的3種測(cè)量方法：1)基于時(shí)間的方法，包括基于信號(hào)到達(dá)時(shí)間(time-of-arrival, TOA)的方法[2-3]和基于信號(hào)到達(dá)時(shí)間差(time-difference-of-arrival, TDOA)的方法[4-5]；2)基于到達(dá)信號(hào)角度(angle-of-arrival, AOA)的方法[6-7]；3)基于接收信號(hào)強(qiáng)度(received signal strength, RSS)的方法[8-9].超寬帶(ultra-wideband, UWB)由于其準(zhǔn)確的延遲分辨率，能夠提供精確的TOA到達(dá)時(shí)間估計(jì)，因此在傳感網(wǎng)定位系統(tǒng)中應(yīng)用廣泛[10-11].然而，UWB測(cè)距方法容易受到多徑效應(yīng)和節(jié)點(diǎn)間相對(duì)幾何位置關(guān)系的影響.近年來(lái)，慣性導(dǎo)航作為一種輔助定位方法能夠補(bǔ)償U(kuò)WB的多徑效應(yīng)和幾何拓?fù)鋯?wèn)題，嵌入傳感器的慣性測(cè)量單元(inertial measurement units, IMU)如加速度計(jì)、陀螺儀、磁力計(jì)等能夠提供一系列連續(xù)的慣性信息，從而提高定位精度[12-13].本文提出了一種UWB和IMU融合定位方法，將UWB測(cè)距信息以及IMU的慣性信息融合起來(lái)，來(lái)定位傳感網(wǎng)中的目標(biāo)節(jié)點(diǎn).

在定位過(guò)程中，由于信號(hào)在傳輸過(guò)程中會(huì)受到噪聲、干擾、多路徑等隨機(jī)現(xiàn)象的影響，節(jié)點(diǎn)的位置估計(jì)會(huì)存在不確定性，從而影響定位精度.定位系統(tǒng)性能評(píng)估標(biāo)準(zhǔn)主要有幾何精度因子(geometric dilution of precision, GDOP)[14-15]、克拉美羅下限(Cramer-Rao lower bound,CRLB)[16-18]、后驗(yàn)克拉美羅下限(posterior Cramer-Rao lower bound,PCRLB)[19-20]，其中CRLB比較常用.文獻(xiàn)[16]提出了基于CRLB的線性定位估計(jì)問(wèn)題，給出了最小CRLB的推導(dǎo)過(guò)程；文獻(xiàn)[3,17]給出了UWB測(cè)距系統(tǒng)中基于TOA方法的位置估計(jì)誤差下限，得出在不存在多徑的理想情況下，CRLB與UWB脈沖寬度及信噪比(signal-to-noise ratio, SNR)有關(guān)；文獻(xiàn)[18]提出了在傳感網(wǎng)中一種基于半定理編程的節(jié)點(diǎn)定位算法，目標(biāo)節(jié)點(diǎn)(target node, TN)的位置信息可以由錨節(jié)點(diǎn)(anchor node, AN)的位置信息及其他慣性信息(如加速度和方位角)來(lái)確定，并進(jìn)一步導(dǎo)出了相應(yīng)的CRLB.

然而CRLB僅僅關(guān)注空間狀態(tài)下相對(duì)位置間的關(guān)系對(duì)定位目標(biāo)精度的影響，忽略了時(shí)間信息，如文獻(xiàn)[18]，不能滿足定位系統(tǒng)中對(duì)時(shí)間評(píng)估的要求.PCRLB考慮了時(shí)域信息[19-20]，可以作為定位系統(tǒng)性能的另一個(gè)評(píng)價(jià)標(biāo)準(zhǔn)；文獻(xiàn)[19]將基于無(wú)線電定位的位置估計(jì)、慣性傳感器測(cè)量和隨機(jī)游走模型結(jié)合起來(lái)，推導(dǎo)了用于慣性傳感器增強(qiáng)的移動(dòng)定位的PCRLB；文獻(xiàn)[20]使用混合的射頻(radio frequency, RF)和圖像處理定位技術(shù)來(lái)定位膠囊內(nèi)窺鏡檢查中的膠囊位置，以PCRLB作為性能評(píng)估的框架，實(shí)現(xiàn)了毫米級(jí)的定位精度.

基于以上分析，本文首先介紹了無(wú)線傳感網(wǎng)融合定位系統(tǒng)中狀態(tài)轉(zhuǎn)換和測(cè)量模型，然后推導(dǎo)了CRLB和PCRLB，最后進(jìn)行了實(shí)驗(yàn)仿真.實(shí)驗(yàn)結(jié)果表明，本文提出的融合定位方法具有更好的時(shí)空定位性能，更能接近實(shí)際應(yīng)用的理論精度下限.

1 無(wú)線傳感網(wǎng)融合定位系統(tǒng)建模

我們將UWB測(cè)距系統(tǒng)與慣性測(cè)量單元IMU結(jié)合，用來(lái)定位隨機(jī)移動(dòng)模型中節(jié)點(diǎn)的位置.

令ck=(P,N)T=(xPk,yPk,xNk,yNk)T表示移動(dòng)節(jié)點(diǎn)在狀態(tài)k時(shí)的狀態(tài)向量，其中k=1,2,…,K,K為總觀察數(shù).(xPk,yPk)表示世界坐標(biāo)空間中節(jié)點(diǎn)的2D坐標(biāo)；(xNk,yNk)表示節(jié)點(diǎn)移動(dòng)方向的標(biāo)準(zhǔn)向量.令zk表示移動(dòng)節(jié)點(diǎn)在狀態(tài)k時(shí)的測(cè)量向量，則該系統(tǒng)的狀態(tài)方程和觀測(cè)方程被定義為

(1)

其中，qk表示過(guò)程誤差，即基于IMU慣性測(cè)量單元的誤差，服從協(xié)方差為Q的高斯分布；rk表示測(cè)量誤差，即UWB測(cè)距的誤差，服從協(xié)方差為R的高斯分布；矩陣Ak和矩陣Hk分別表示狀態(tài)轉(zhuǎn)移矩陣和測(cè)量關(guān)系矩陣，分別為

(2)

其中，dk表示基于IMU慣性測(cè)量單元測(cè)得的步長(zhǎng);

(3)

算法1. 傳感網(wǎng)融合定位系統(tǒng)中的卡爾曼濾波.

① 初始化z1,c1,P1,令c1=z1;

② fork=1,2,…,Kdo

④ 預(yù)測(cè)誤差協(xié)方差先驗(yàn)估計(jì):

⑤ 更新旋轉(zhuǎn)矩陣:R=RRkR-1;

⑥ 計(jì)算卡爾曼增益:

⑧ 更新誤差協(xié)方差后驗(yàn)估計(jì):

⑨ end for

2 性能評(píng)估

最初無(wú)線傳感網(wǎng)的定位性能由CRLB進(jìn)行評(píng)估，其中的觀察結(jié)果彼此獨(dú)立，反映了空間狀態(tài)下相對(duì)位置間的關(guān)系對(duì)定位目標(biāo)精度的影響.然而考慮到時(shí)域信息，觀察結(jié)果之間的獨(dú)立性不再存在，當(dāng)前狀態(tài)移動(dòng)節(jié)點(diǎn)的位置信息需要該節(jié)點(diǎn)在前一狀態(tài)的位置信息來(lái)判斷，即通過(guò)先驗(yàn)信息得到后驗(yàn)信息，因此引入了后驗(yàn)CRLB，即PCRLB.

2.1 誤差分析

基于UWB和IMU的融合定位方法也會(huì)產(chǎn)生一定的誤差.基于UWB測(cè)距定位方法在測(cè)量目標(biāo)節(jié)點(diǎn)與錨節(jié)點(diǎn)之間的距離時(shí)由于環(huán)境干擾等因素會(huì)造成測(cè)距誤差；基于IMU慣性器件(如加速度計(jì)、陀螺儀等)的定位方法在定位過(guò)程中會(huì)獲得一系列慣性信息如加速度、角速度等，將這些慣性信息進(jìn)行姿態(tài)解算[21]可以獲得目標(biāo)節(jié)點(diǎn)的步長(zhǎng)信息以及角度信息，由于慣性信息本身存在誤差，因此獲得的步長(zhǎng)信息以及角度信息也會(huì)存在誤差.由此我們可以得出，基于UWB和IMU的融合定位方法的誤差來(lái)源主要有：UWB的測(cè)距誤差以及IMU的步長(zhǎng)誤差和角度誤差.

假設(shè)狀態(tài)k時(shí)目標(biāo)節(jié)點(diǎn)TN的坐標(biāo)為mk=(m1,k,m2,k)T；第n個(gè)錨節(jié)點(diǎn)AN的坐標(biāo)為an=(a1,n,a2,n)T,n=1,2,…,N，其中N為錨節(jié)點(diǎn)的個(gè)數(shù).目標(biāo)節(jié)點(diǎn)的狀態(tài)轉(zhuǎn)移圖如圖1所示.假設(shè)目標(biāo)節(jié)點(diǎn)的狀態(tài)信息矩陣為m=(m1,m2,…,mK)T，K為總的狀態(tài)數(shù)，對(duì)于狀態(tài)信息矩陣中的每一個(gè)元素可以得到：

mk=mk-1+dkwk+rk,

(4)

其中，dk為狀態(tài)k到狀態(tài)k+1時(shí)節(jié)點(diǎn)的移動(dòng)步長(zhǎng)，wk=(cosφk,sinφk)T表示節(jié)點(diǎn)的移動(dòng)方向，rk表示由步長(zhǎng)誤差和角度誤差造成的整體誤差.

Fig. 1 State transition of the target node圖1 目標(biāo)節(jié)點(diǎn)的狀態(tài)轉(zhuǎn)移

基于IMU慣性測(cè)量單元測(cè)得的步長(zhǎng)估計(jì)可以表示為

(5)

其中dk為實(shí)際步長(zhǎng)：

(6)

基于IMU慣性測(cè)量單元測(cè)得的角度估計(jì)表示為

(7)

其中φk為實(shí)際角度：

(8)

除了基于IMU慣性測(cè)量單元獲得這些位置信息之外，我們還可以基于UWB測(cè)距方法測(cè)得目標(biāo)節(jié)點(diǎn)與錨節(jié)點(diǎn)之間的距離估計(jì):

(9)

其中sn,k為目標(biāo)節(jié)點(diǎn)TN和第n個(gè)錨節(jié)點(diǎn)AN之間的實(shí)際距離：

(10)

2.2 空間性能評(píng)估：CRLB

(11)

其中J(mk)為費(fèi)舍爾信息矩陣，在定義費(fèi)舍爾信息矩陣之前，我們先定義聯(lián)合概率密度函數(shù)：

(12)

其中:

(13)

(14)

(15)

根據(jù)式(12)，我們可以得出費(fèi)舍爾信息矩陣中的元素：

(16)

將式(12)～(15)帶入式(16)，可得費(fèi)舍爾信息矩陣：

(17)

其中，

(18)

(19)

(20)

2.3 時(shí)間性能評(píng)估：PCRLB

如本節(jié)開(kāi)頭所說(shuō)，不考慮時(shí)域信息時(shí)，CRLB最常用于提供系統(tǒng)性能的理論下限.利用后驗(yàn)信息如步長(zhǎng)和角度，再加上時(shí)間信息，我們可以將CRLB擴(kuò)展到PCRLB.在推導(dǎo)PCRLB之前，我們先定義聯(lián)合概率密度函數(shù)：

(21)

(22)

為了計(jì)算狀態(tài)k時(shí)的費(fèi)舍爾信息矩陣，定義：

(23)

其中:

因此:

(24)

子矩陣Jk可以通過(guò)該矩陣的偽逆求得，即:

(25)

k+1狀態(tài)的聯(lián)合概率密度pk+1為

(26)

根據(jù)狀態(tài)k+1的聯(lián)合概率密度我們可以求出狀態(tài)k+1的費(fèi)舍爾信息矩陣J(m0:k+1)：

(27)

(28)

(29)

(30)

βk+1反映的是基于UWB測(cè)距的位置信息，即:

(31)

由J(m0:k+1)和Jk我們可以得到狀態(tài)k+1的費(fèi)舍爾信息矩陣，即：

(32)

(33)

(34)

(35)

因此，后驗(yàn)費(fèi)舍爾信息矩陣：

Jk+1=βk+1+Hk-Hk(Jk+Hk)-1Hk,

(36)

進(jìn)一步簡(jiǎn)化可得：

(37)

其中，βk+1反映了基于UWB測(cè)距的信息，Hk反映了基于IMU慣性測(cè)量單元的信息.

3 實(shí)驗(yàn)仿真與結(jié)果

本節(jié)我們用所提出的方法計(jì)算CRLB和PCRLB，來(lái)綜合評(píng)估UWB和IMU融合方法的時(shí)空定位性能.設(shè)計(jì)一個(gè)40 m×40 m的2D正方形傳感網(wǎng)環(huán)境，如圖2所示.區(qū)域中部署了4個(gè)錨節(jié)點(diǎn)AN，坐標(biāo)分別是(10,10)，(10,30)，(30,10)，(30,30)，區(qū)域中間分散著其他的目標(biāo)節(jié)點(diǎn)TN，其中*代表錨節(jié)點(diǎn)，○代表目標(biāo)節(jié)點(diǎn).我們假設(shè)錨節(jié)點(diǎn)的通信范圍可以覆蓋整個(gè)區(qū)域，目標(biāo)節(jié)點(diǎn)可以從錨節(jié)點(diǎn)接收到所需要的信息.

Fig. 2 Deployment of nodes in sensor network圖2 傳感網(wǎng)中節(jié)點(diǎn)的部署圖

3.1 CRLB仿真

我們主要從3個(gè)角度來(lái)分析不同因素對(duì)CRLB的影響.

1) UWB+IMU融合方法和單獨(dú)采用UWB方法對(duì)CRLB的影響

Fig. 3 CRLB based on the proposed hybrid method圖3 基于UWB+IMU融合方法的CRLB

2) UWB測(cè)距誤差和IMU步長(zhǎng)誤差對(duì)CRLB的影響

Fig. 5 Average CRLB of different IMU step length error圖5 IMU步長(zhǎng)誤差對(duì)平均CRLB的影響

3) 錨節(jié)點(diǎn)的部署結(jié)構(gòu)和數(shù)目對(duì)CRLB的影響

我們還研究了錨節(jié)點(diǎn)在不同位置以及錨節(jié)點(diǎn)數(shù)量不同時(shí)對(duì)系統(tǒng)定位精度的影響.我們采用了4種錨節(jié)點(diǎn)部署方式，如圖7所示，其中*代表基站位置.按照不同的拓?fù)鋱D，計(jì)算相應(yīng)的CRLB，如圖8～11所示.

Fig. 7 Different deployment of the anchor node圖7 錨節(jié)點(diǎn)的不同部署

Fig. 8 CRLB of Topology 1圖8 拓?fù)鋱D1對(duì)應(yīng)的CRLB

Fig. 9 CRLB of Topology 2圖9 拓?fù)鋱D2對(duì)應(yīng)的CRLB

Fig. 10 CRLB of Topology 3圖10 拓?fù)鋱D3對(duì)應(yīng)的CRLB

Fig. 11 CRLB of Topology 4圖11 拓?fù)鋱D4對(duì)應(yīng)的CRLB

從圖8～11中可以看出：①當(dāng)錨節(jié)點(diǎn)數(shù)目相同時(shí)，錨節(jié)點(diǎn)的部署形狀越接近于矩形，定位精度越高.如圖8所示，錨節(jié)點(diǎn)的部署形狀為正方形，最壞的CRLB=0.32 m，與圖9和圖10最壞的CRLB進(jìn)行比較，精度分別提高了3 cm和5 cm.②圖10的CRLB最大，即表示定位精度最差，可能是由于錨節(jié)點(diǎn)部署的太密集而導(dǎo)致.③圖11的錨節(jié)點(diǎn)數(shù)目為6個(gè)，得到的最好CRLB=0.23 m，與圖8的最好CRLB相比，精度提高了4 cm.

3.2 PCRLB仿真

在計(jì)算PCRLB時(shí)，我們可以通過(guò)2.3節(jié)的公式遞歸地計(jì)算出費(fèi)舍爾信息矩陣，由于式(28)～(30)中的方程表達(dá)式通常沒(méi)有近似解，為了解決這個(gè)問(wèn)題，我們采用蒙特卡羅方法將連續(xù)積分轉(zhuǎn)換為離散求和，并最終計(jì)算出PCRLB.

從2個(gè)角度來(lái)分析不同因素對(duì)PCRLB的影響.

1) UWB+IMU融合方法中不同α和ε對(duì)PCRLB的影響以及單獨(dú)采用UWB方法和單獨(dú)采用IMU方法對(duì)PCRLB的影響

Fig. 12 PCRLB of the proposed hybrid method圖12 基于UWB+IMU融合方法的PCRLB

圖12顯示了在UWB+IMU融合方法中，在不同α和ε下可實(shí)現(xiàn)的最小誤差，我們還給出了單獨(dú)使用UWB方法(即沒(méi)有IMU慣性測(cè)量單元)以及單獨(dú)使用IMU方法(即沒(méi)有UWB測(cè)距定位)時(shí)可以實(shí)現(xiàn)的最小誤差.從圖12可以看出：①與基于單一UWB方法相比，基于UWB+IMU融合方法將IMU步長(zhǎng)信息以及角度信息融合后，顯著提高了移動(dòng)節(jié)點(diǎn)的定位精度.單獨(dú)使用UWB方法時(shí)，PCRLB=0.29m；采用融合方法后當(dāng)α=0.05,ε=10°時(shí)，PCRLB=0.22m，定位精度為提高了7 cm.②與基于單一IMU方法相比，基于UWB+IMU融合方法將UWB測(cè)距信息融合后，補(bǔ)償了基于IMU方法所產(chǎn)生的累積誤差問(wèn)題.③隨著α和ε的增加，PCRLB也增加，這意味著UWB+IMU融合方法的定位精度與步長(zhǎng)及角度誤差成反比.④UWB+IMU融合方法的PCRLB在一定階段后會(huì)趨于穩(wěn)定.

2) 不同錨節(jié)點(diǎn)部署結(jié)構(gòu)對(duì)PCRLB的影響

按照?qǐng)D7所示的不同拓?fù)鋱D，我們可以計(jì)算出相應(yīng)的PCRLB，這里α=0.1,ε=10°，如圖13所示:

Fig. 13 PCRLB of different topologies圖13 不同拓?fù)浣Y(jié)構(gòu)產(chǎn)生的PCRLB

從圖13可以看出：①相同數(shù)目的錨節(jié)點(diǎn)但不同結(jié)構(gòu)的拓?fù)?，?duì)定位精度會(huì)產(chǎn)生不同的影響，但影響較小，如拓?fù)鋱D1的PCRLB=0.235 m，拓?fù)鋱D2的PCRLB=0.24 m，拓?fù)?的PCRLB=0.245 m，波動(dòng)范圍不超過(guò)1 cm.②不同數(shù)目的錨節(jié)點(diǎn)對(duì)定位精度產(chǎn)生的影響較大，拓?fù)?的PCRLB=0.205 m，將拓?fù)?和拓?fù)?進(jìn)行比較，可以看出定位精度提高了3 cm.

4 總結(jié)和展望

本文提出了一種基于UWB和IMU的融合定位方法，來(lái)實(shí)現(xiàn)傳感網(wǎng)中目標(biāo)節(jié)點(diǎn)的位置追蹤.通過(guò)計(jì)算克拉美羅下限CRLB來(lái)表征融合定位方法的空間定位性能，驗(yàn)證其在解決多徑和幾何拓?fù)鋯?wèn)題上的有效性.通過(guò)計(jì)算后驗(yàn)克拉美羅下限PCRLB來(lái)表征融合定位方法的時(shí)間定位性能，驗(yàn)證其在累積誤差糾正上的有效性，為基于UWB和IMU融合定位算法的設(shè)計(jì)和仿真提供理論支持.實(shí)驗(yàn)結(jié)果表明，本文提出的融合定位方法具有更好的時(shí)空定位性能，更能接近實(shí)際應(yīng)用的理論精度下限.

然而，雖然基于UWB和IMU的融合定位方法相比于單一UWB定位或單一IMU定位方法能夠獲得更好的性能，但其還是會(huì)帶來(lái)一定的問(wèn)題：1)設(shè)備復(fù)雜性問(wèn)題.相對(duì)于單一的定位方法，融合定位方法必定會(huì)對(duì)硬件設(shè)計(jì)(如設(shè)備體積等)提出更高的要求，如何在滿足實(shí)際要求的情況下，設(shè)計(jì)融合定位裝置是未來(lái)工作中需要考慮和解決的.2)算法復(fù)雜性問(wèn)題.融合定位過(guò)程中需要進(jìn)行多源數(shù)據(jù)的處理，因此需要合適的融合算法對(duì)這些數(shù)據(jù)進(jìn)行融合，融合算法帶來(lái)的算法復(fù)雜性問(wèn)題也是實(shí)際應(yīng)用中需要考慮和解決的.3)資源分配和功耗問(wèn)題.單獨(dú)采用IMU方法時(shí)不需要部署錨節(jié)點(diǎn)，在目標(biāo)間通信上節(jié)省了很大的資源，但UWB和IMU融合定位方法中，必不可少地需要進(jìn)行目標(biāo)節(jié)點(diǎn)與錨節(jié)點(diǎn)之間的資源分配和任務(wù)調(diào)度，同時(shí)相應(yīng)的功耗也需要考量，因此，如何設(shè)計(jì)融合定位中的任務(wù)調(diào)度和通信處理，盡可能地降低系統(tǒng)功耗是我們下一步的研究重點(diǎn).