劉 琦,高成發(fā),尚 睿

(東南大學(xué) 交通學(xué)院測(cè)繪工程系，江蘇 南京 211189)

室內(nèi)定位技術(shù)多種多樣，主要有紅外線、超聲波、藍(lán)牙、ZigBee、WIFI等。與這些技術(shù)相比，UWB技術(shù)具有穿透力強(qiáng)、功耗低、抗多徑效果好、安全性高、系統(tǒng)復(fù)雜度低的特點(diǎn)[1]，理論上超寬帶定位技術(shù)的定位精度可以達(dá)到厘米級(jí)。

目前，常用的UWB室內(nèi)無(wú)線定位測(cè)量方法有很多，包括：基于到達(dá)角度(Angle of Arrival，AOA)的定位方法，利用兩個(gè)或以上接入點(diǎn)提供的AOA測(cè)量值，解算待定點(diǎn)位置； 基于接收信號(hào)強(qiáng)度(Received Signal Strength，RSS)的定位方法，測(cè)量來(lái)自多個(gè)發(fā)射器接收的信號(hào)強(qiáng)度，以便使用信號(hào)強(qiáng)度作為基站和標(biāo)簽之間距離的估算參數(shù)；基于到達(dá)時(shí)間(Time of Arrival，TOA)的定位方法, 基站通過(guò)記錄發(fā)出測(cè)距信號(hào)到收到標(biāo)簽確認(rèn)信號(hào)所花費(fèi)的時(shí)間，再乘以信號(hào)傳播速度，便得到基站與標(biāo)簽之間的距離；基于到達(dá)時(shí)間差(Time Difference of Arrival ，TDOA)的定位方法等[2]。隨著室內(nèi)環(huán)境的擴(kuò)大化、復(fù)雜化以及室內(nèi)活動(dòng)時(shí)間的增加，大眾對(duì)于位置服務(wù)精度的需求越來(lái)越高，用戶不僅關(guān)心二維位置，更希望掌握準(zhǔn)確的三維坐標(biāo)。

UWB定位技術(shù)與衛(wèi)星定位技術(shù)原理相似，都是通過(guò)時(shí)間同步、單程測(cè)距解析實(shí)現(xiàn)定位。在UWB和GNSS進(jìn)行解算時(shí)，方程是非線性的，傳統(tǒng)方法通過(guò)EKF算法，對(duì)非線性函數(shù)的Taylor展開(kāi)式進(jìn)行一階偏導(dǎo)，忽略其余高階項(xiàng)，從而將非線性問(wèn)題轉(zhuǎn)化為線性。文獻(xiàn)[3]在緊耦合組合導(dǎo)航系統(tǒng)中比較EKF和UKF算法精度，發(fā)現(xiàn)兩種算法精度均優(yōu)于單獨(dú)使用GPS得到的導(dǎo)航解，UKF略優(yōu)于EKF算法。文獻(xiàn)[4]實(shí)驗(yàn)結(jié)果表明，UKF比EKF更適合于在INS/GPS系統(tǒng)中使用。但因?yàn)閁WB定位過(guò)程中，基站到達(dá)標(biāo)簽的距離較近，所以使用EKF方法產(chǎn)生的線性化誤差較大。近年來(lái)有人提出利用UKF算法，解決UWB定位方程強(qiáng)非線性的問(wèn)題[5]。這種方法直接利用非線性模型，避免了EKF中的非線性函數(shù)線性化近似過(guò)程中復(fù)雜的Jacobian矩陣求解。文獻(xiàn)[6]闡述一種自適應(yīng)UKF算法，實(shí)驗(yàn)證明，該算法能有效提高定位精度，是一種實(shí)時(shí)高精度的室內(nèi)定位算法。文獻(xiàn)[7]提出了基于TOA技術(shù)的EKF算法，用來(lái)降低測(cè)量噪聲對(duì)定位精度的影響。張桀等人在文獻(xiàn)[8]改進(jìn)了TDOA算法，并聯(lián)合卡爾曼濾波，進(jìn)一步消除傳輸過(guò)程中誤差干擾，達(dá)到了20 cm的定位精度。目前，EKF和UKF是兩種主要的UWB定位算法，但關(guān)于線性化誤差對(duì)于UWB定位系統(tǒng)的精度影響的研究還很少。

本文闡述了UWB定位的幾種常用方法,并分析線性化誤差對(duì)于UWB定位系統(tǒng)的影響。在此基礎(chǔ)上，提出UKF和TDOA算法結(jié)合的實(shí)驗(yàn)?zāi)Ｐ汀Ｊ紫仍?0 m初始偏差下，利用實(shí)驗(yàn)數(shù)據(jù)，比較UKF和EKF兩種算法的定位效果；其次考慮到標(biāo)簽初始化坐標(biāo)偏差的改變會(huì)對(duì)定位結(jié)果產(chǎn)生影響，為探究其影響，在不同的初始偏差設(shè)定下，采用EKF和UKF算法進(jìn)行解算，并對(duì)結(jié)果進(jìn)行分析，進(jìn)一步比較兩種算法的定位精度和穩(wěn)定性。

1 定位方法

1.1 UWB定位方法

對(duì)于UWB定位而言，AOA方法需要傳感器間的合作，因而會(huì)受到誤差累積的影響，不如其他算法實(shí)用；障礙物導(dǎo)致RSS方法存在多徑效應(yīng)，影響定位精度。因此目前最常用的是TOA和TDOA定位方法。

TOA定位方法屬于雙向測(cè)距技術(shù)，需要兩次信號(hào)傳播，在這個(gè)過(guò)程中會(huì)有多徑效應(yīng)、信號(hào)時(shí)延等誤差累積，導(dǎo)致定位結(jié)果誤差較大；此外，這種方法要求基站和接收機(jī)之間實(shí)現(xiàn)精確時(shí)間同步，這一點(diǎn)難以實(shí)現(xiàn)，且成本較高。

TDOA方法避免UWB標(biāo)簽和基站間的往復(fù)通訊，需要UWB定位標(biāo)簽發(fā)射一次信號(hào)，工作時(shí)間大大縮短，降低功耗，并且TDOA僅需要保持基站間的時(shí)鐘同步的要求易于實(shí)現(xiàn)，因此本文選用TDOA方法，其具體過(guò)程如下：

已知測(cè)得第n個(gè)基站接收到標(biāo)簽所發(fā)出的UWB信號(hào)的時(shí)刻分別為ti(i=1,2,3,4,…,n)，標(biāo)簽到第n個(gè)基站的距離為di(i=1,2,3,4,…,n)，在所有基站保持時(shí)間同步的前提下，即Δti-Δtj≠0(i≠j&i,j=0,1,2,3,…,n)，同時(shí)利用多個(gè)測(cè)量值即可求得標(biāo)簽坐標(biāo)。以4組基站為例，在基站間完全同步的情況下，利用式(1)求解。

(1)

1.2 UWB定位系統(tǒng)線性化誤差分析

EKF對(duì)非線性函數(shù)的Taylor展開(kāi)式進(jìn)行一階線性化截?cái)啵雎云溆喔唠A項(xiàng)，從而將非線性問(wèn)題轉(zhuǎn)化為線性，可以將卡爾曼線性濾波算法應(yīng)用于非線性系統(tǒng)中。

EKF算法是對(duì)非線性函數(shù)的Taylor展開(kāi)式進(jìn)行一階線性化截?cái)嗖⒑雎云溆喔唠A項(xiàng)，從而實(shí)現(xiàn)非線性問(wèn)題的線性化，以便于將卡爾曼線性濾波算法用于非線性系統(tǒng)中。UWB定位距離觀測(cè)方程為：

ρn=dn+δtu-δtn+ε.

(2)

式中：ρn為標(biāo)簽到基站n(n=1,2,3,4)的觀測(cè)距離，dn為標(biāo)簽到基站n(n=1,2,3,4)的真實(shí)距離，δtu為標(biāo)簽鐘差，δtn(n=1,2,3,4)為基站鐘差，ε為系統(tǒng)噪聲誤差。

靜態(tài)狀態(tài)下進(jìn)行的本文實(shí)驗(yàn)，EKF算法只需要對(duì)觀測(cè)方程進(jìn)行線性化。用g(xu,yu,zu)表示UWB系統(tǒng)距離觀測(cè)方程，其Taylor展開(kāi)式為：

(3)

用X表示狀態(tài)向量，X0表示其近似估計(jì)值，δX=X-X0。A表示g(x)在x0處的一階偏導(dǎo)，εg表示二階殘余量，

g(X)=g(X0)+AδX+εg,

(4)

(5)

式中：G(X)為海森矩陣(Hessian Matrix)，由二階偏導(dǎo)數(shù)構(gòu)成。假設(shè)u代表UWB標(biāo)簽，i代表UWB基站，則基站與標(biāo)簽間的距離表示為：

(6)

Hessian Matrix為：

(7)

在EKF算法中，二階殘余量εg是直接舍去的，就前面提到的線性化誤差，曾有學(xué)者對(duì)其大小給出了邊界估計(jì)：

(8)

(9)

式中：δ2xu，δ2yu，δ2zu分別為標(biāo)簽坐標(biāo)的方差，對(duì)于衛(wèi)星導(dǎo)航定位，衛(wèi)星距離用戶約2萬(wàn)km，假設(shè)用戶坐標(biāo)誤差為100 m，其觀測(cè)方程線性化誤差也不到1 mm，對(duì)于絕大多數(shù)定位是可以接受的。但是對(duì)于UWB定位系統(tǒng)，標(biāo)簽和基站的距離較近，假設(shè)兩者間距離為20 m，標(biāo)簽坐標(biāo)誤差為5 m，其線性化最大誤差可以達(dá)到米級(jí)，這對(duì)于UWB定位結(jié)果會(huì)產(chǎn)生不可忽略的影響，因此對(duì)于UWB定位系統(tǒng)，應(yīng)采用不產(chǎn)生線性化誤差的UKF算法。

1.3 UKF算法原理

無(wú)損卡爾曼濾波(Unscented Kalman Filter,UKF)，是無(wú)損變換(UT)和標(biāo)準(zhǔn)卡爾曼濾波體系的結(jié)合，通過(guò)無(wú)損變換使非線性系統(tǒng)方程適用于線性假設(shè)下的標(biāo)準(zhǔn)卡爾曼濾波體系。

對(duì)于不同時(shí)刻k，由隨機(jī)變量x和觀測(cè)變量z組成非線性系統(tǒng)，即：

(10)

式中：xk和xk-1分別是k和k-1時(shí)刻的狀態(tài)向量，zk為觀測(cè)向量,ωk為狀態(tài)噪聲向量，vk為觀測(cè)噪聲向量，并且二者是互不相關(guān)的零均值白噪聲序列。

1)確定濾波初值:

(11)

2)計(jì)算Sigma點(diǎn)：

(12)

其中

(13)

式中：n為狀態(tài)向量維數(shù)，a為很小的正數(shù)，用于確定x周圍的Sigma點(diǎn)分布，取值范圍10-4～1；k為第二個(gè)尺度參數(shù)，k=3-n。

3)確定權(quán)值：

(14)

4)時(shí)間更新：

為了簡(jiǎn)便起見(jiàn)，設(shè)M∈n×n是要恢復(fù)的矩陣.rank(M)=r，r?n.{Mij,(i,j)∈Ω}是M中已知的矩陣元素的集合.盡管M中有n2個(gè)元素，但是M的自由度僅為2nr-2r2.當(dāng)r比較小時(shí)，2nr-2r2?n2.那么，是否有可能在已知矩陣部分元素的情形下，恢復(fù)全部的n2個(gè)元素呢？一般來(lái)講，不是所有的矩陣都可以從部分元素準(zhǔn)確恢復(fù)其全部元素,比如下面的矩陣：

(15)

5)測(cè)量更新：

(16)

6)濾波更新：

(17)

1.4 基于無(wú)損卡爾曼濾波的UWB定位

2 實(shí)驗(yàn)與分析

為測(cè)試在室內(nèi)環(huán)境下UKF算法的準(zhǔn)確性，實(shí)驗(yàn)地點(diǎn)選取東南大學(xué)四牌樓校區(qū)交通樓某實(shí)驗(yàn)室內(nèi)，實(shí)驗(yàn)設(shè)備為所購(gòu)買的UWB Mini3模塊，設(shè)置4個(gè)基站和一個(gè)標(biāo)簽。在實(shí)驗(yàn)場(chǎng)地內(nèi)，4個(gè)基站位置見(jiàn)表1。

表1 4個(gè)基站具體位置 m

2.1 10 m初始偏差下UKF和EKF定位準(zhǔn)確性

為了比較在10 m的初始偏差下，EKF和UKF算法定位的準(zhǔn)確性，本文在試驗(yàn)場(chǎng)地內(nèi)選取4個(gè)點(diǎn)位布設(shè)標(biāo)簽，利用其觀測(cè)數(shù)據(jù)進(jìn)行解算，對(duì)定位結(jié)果進(jìn)行比較分析，定位結(jié)果見(jiàn)圖1—圖4。

圖1 第一組數(shù)據(jù)定位結(jié)果

圖2 第二組數(shù)據(jù)定位結(jié)果

圖3 第三組數(shù)據(jù)定位結(jié)果

圖4 第四組數(shù)據(jù)定位結(jié)果

由圖1—圖4可以看出，在初始坐標(biāo)偏差為10 m的情況下，使用EKF方法進(jìn)行數(shù)據(jù)解算，在N,E方向上的結(jié)果與UKF算法的結(jié)果較為接近，但在U方向上，UKF算法的結(jié)果要明顯優(yōu)于EKF算法，這是由于EKF算法在線性化過(guò)程中產(chǎn)生的誤差所導(dǎo)致。圖中明顯看出，在初始坐標(biāo)偏差為10 m時(shí)，EKF算法很難達(dá)到收斂狀態(tài)，而UKF算法在較短時(shí)間內(nèi)便可收斂，這表明在坐標(biāo)偏差較大時(shí)，UKF算法在定位精度和穩(wěn)定性上是優(yōu)于EKF算法的。

2.2 不同初始偏差定位精準(zhǔn)

考慮到線性化模型對(duì)參數(shù)初值比較敏感，因此初始坐標(biāo)偏差的不同，對(duì)于定位結(jié)果也會(huì)產(chǎn)生不同影響。本次實(shí)驗(yàn)，選取第三組數(shù)據(jù)，給定X0不同的偏差，ΔX分別為(0.1 m,0.1 m,0.1 m)T，(0.5 m,0.5 m,0.5 m)T，(1 m,1 m,1 m)T，(5 m,5 m,5 m)T，來(lái)比較在不同偏差情況下，EKF算法和UKF算法的定位效果和穩(wěn)定性。定位結(jié)果如圖5—圖8所示。

圖5 初始偏差0.1 m時(shí)定位結(jié)果

圖6 初始偏差0.5 m時(shí)定位結(jié)果

圖7 初始偏差1 m時(shí)定位結(jié)果

圖8 初始偏差5 m時(shí)定位結(jié)果

從圖5、圖6中可以看出，當(dāng)初始偏差較小時(shí)，EKF算法和UKF算法在N,E,U方向的定位精度較為接近，初始偏差較小，線性化誤差并不明顯，兩種方法的定位效果比較接近。分析比較圖3，圖5—圖8可以發(fā)現(xiàn)，隨著初始偏差的增大，EKF算法在N，E方向上還能保持較好的定位精度，但在U方向上則誤差逐漸增大，這是EKF線性化誤差累積的結(jié)果。而UKF算法，隨著初始誤差的增大，相比于EKF算法，定位結(jié)果并沒(méi)有出現(xiàn)大幅增大，N，E，U方向都可以保持較好的精度，因?yàn)閁KF算法避免了線性化過(guò)程，其定位結(jié)果不受線性化誤差的影響。從圖3，圖5—圖8中可以發(fā)現(xiàn)，UKF算法可以很快達(dá)到收斂，并保持穩(wěn)定，而EKF算法則隨著初始偏差的增大，穩(wěn)定性逐漸下降。在初始偏差較大時(shí)，UKF算法的穩(wěn)定性和定位精度都明顯優(yōu)于EKF算法。

表2 不同初始偏差下定位結(jié)果 m

從表2看出，在初始偏差為0.1 m和0.5 m時(shí)，EKF和UKF算法定位結(jié)果較為接近。當(dāng)初始偏差5 m和10 m時(shí)，EKF算法在U方向上的定位誤差越來(lái)越大。而UKF算法，在初始偏差較大時(shí)，N，E，U方向上定位精度都能保持在10 cm左右。

當(dāng)初始偏差較小時(shí)，EKF算法與UKF算法的定位效果相接近，但隨著偏差的增大，EKF算法在U方向上的定位精度因線性化誤差的影響開(kāi)始增大，并且很難達(dá)到穩(wěn)定。而UKF算法則可以保持較好的定位精度和穩(wěn)定性，UKF算法優(yōu)于EKF算法。

3 結(jié)論和展望

本文從理論上分析了UWB定位系統(tǒng)距離觀測(cè)方程的線性化誤差，發(fā)現(xiàn)對(duì)于UWB定位系統(tǒng)傳統(tǒng)EKF算法產(chǎn)生的誤差不可忽略。在此基礎(chǔ)上，采用UKF和TDOA算法結(jié)合的實(shí)驗(yàn)?zāi)Ｐ停M(jìn)行定位精度實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)論如下:

1)對(duì)于UWB定位系統(tǒng)，傳統(tǒng)EKF算法會(huì)產(chǎn)生米級(jí)的線性化誤差，對(duì)定位結(jié)果有影響，而采用UKF算法可以避免線性化過(guò)程，達(dá)到厘米級(jí)定位精度。

2)在初始坐標(biāo)偏差為10 m情況下，UKF和EKF算法平面定位精度相當(dāng)，但高程方向上，EKF算法誤差達(dá)到0.35 m，UKF算法誤差僅有2 cm。

3)初始坐標(biāo)偏差在0.5 m以內(nèi)時(shí)，兩種算法定位結(jié)果相當(dāng)，隨著初始偏差的增大，EKF算法平面精度仍可以保持較高水平，高程方向誤差越來(lái)越大；而UKF算法，在初始偏差增大情況下，仍然可以保持較好的穩(wěn)定性和定位精度，在初始偏差為5 m和10 m時(shí)，U方向定位精度相比于EKF算法提高達(dá)到90%以上。

4)本次實(shí)驗(yàn)僅在靜態(tài)條件下進(jìn)行測(cè)試，如何在動(dòng)態(tài)條件以及復(fù)雜環(huán)境下保持定位的精度和穩(wěn)定性，是后續(xù)研究?jī)?nèi)容的重點(diǎn)。