Chan-AUKF融合算法在三維定位中的應用

李向博，熊 鳴，王麗婕

(北京信息科技大學 高動態(tài)導航技術(shù)北京市重點實驗室，北京 100192)

0 引 言

隨著信息技術(shù)不斷發(fā)展，基于位置信息的服務在人們的生活和工作中發(fā)揮著越來越重要的作用。UWB定位技術(shù)因其穿透能力強、時間分辨率高、抗多徑干擾能力強等優(yōu)點，廣泛應用于室內(nèi)定位系統(tǒng)中。

UWB定位系統(tǒng)是基于測距方法來實現(xiàn)定位，按照測距原理的不同可以分為四種測距方法[1-2]：基于到達時間(time of arrival，TOA)估計、基于到達時間差(time different of arrival，TDOA)估計、基于到達角度(angle of arrival、AOA)估計和接收信號強度(received signal strength、RSS)估計?；赨WB信號時間分辨率高的優(yōu)點，TOA和TDOA的UWB定位技術(shù)在三維定位中應用最廣泛[3]，能夠滿足精確定位的需求。在視距(line of sight，LOS)環(huán)境中，基于TDOA定位技術(shù)的Chan算法利用兩次加權(quán)最小二乘法(weighted least squares，WLS)對定位標簽的三維坐標進行解算，具有很好的定位效果[4]。但是在非視距環(huán)境(non line of sight、NLOS)中，Chan算法的定位精度會急劇下降[5]。在實際的定位環(huán)境中，為了抑制非視距誤差對定位結(jié)果的影響，提高定位精度，單一算法已經(jīng)很難滿足定位要求[6]。

國內(nèi)外的許多學者對混合定位算法進行研究，文獻[7]提出將RSS、TOA和TDOA三種測量信息構(gòu)成測距信息集，建立多級聯(lián)合判決機制，但該方法復雜度較高，運算時間長;文獻[8]提出使用Chan-UKF算法通過比較設(shè)定閾值與初步定位坐標殘差的大小來鑒別NLOS誤差，但該方法隨著標簽與基站距離的增大，誤差也會隨之增大；文獻[9]提出使用Chan-Taylor-EKF協(xié)同定位算法，利用每個算法分別對非視距誤差進行抑制，但需要設(shè)置多個閾值進行篩選判斷，增大了算法的復雜度。

針對上述問題，為了有效地抑制非視距誤差對定位結(jié)果的影響，對定位結(jié)果的位置偏差進行補償，提出了基于Chan算法和簡化UKF算法的Chan-AUKF算法。根據(jù)定位標簽與不同的定位基站之間的距離差即TDOA測量值，建立定位基站與定位標簽之間的非線性關(guān)系式。Chan-AUKF算法首先利用Chan算法對基于TDOA測量值建立的非線性方程組進行線性化近似，利用兩次WLS得到定位標簽三維坐標估計值；其次將Chan算法得到的標簽三維坐標值作為簡化UKF算法的初值進行濾波處理之后，根據(jù)簡化UKF算法得到定位標簽三維坐標初始估計值與定位基站布置的環(huán)境建立狀態(tài)誤差補償函數(shù)[10]，對簡化UKF算法的狀態(tài)方程進行實時的自適應補償；最后將簡化UKF算法解算得到的標簽三維坐標初始估計值作為自適應補償后的簡化UKF算法的初值代入，對標簽的三維坐標進行精確解算。

1 TDOA定位技術(shù)及算法

ri,1=ri-r1=c(ti-t1)

(1)

其中，ti為基站i接收到標簽信號的時延值，c為電磁波傳播速度，ri,1為定位標簽與基站1與基站i的距離差值。

1.1 Chan算法

Chan算法是基于TDOA定位技術(shù)的非遞歸定位算法[12-13]。根據(jù)公式(1)建立雙曲面方程組并將其線性化處理為公式(2)：

(2)

假設(shè)za=[x,y,z,r1]T為未知量，公式(2)的線性形式為：

h-Gaza=0

(3)

考慮到非視距誤差和多徑效應的影響，系統(tǒng)存在信號延時誤差，建立系統(tǒng)誤差向量方程及其解析公式如下：

ψ=h-Gaza

(4)

(5)

其中，B=diag{r2,r3,…,rn}，誤差協(xié)方差矩陣為ψ=E(ψψT)=c2BQB；Δt為系統(tǒng)信號延時誤差，其均值為0，方差為σ，協(xié)方差矩陣為Q=diag(σ2,1,σ3,1,…,σn,1)。

假設(shè)za中各個元素相互獨立，利用WLS得到za的第一次估計值：

(6)

實際上，za中的元素r1與(x,y,z)之間是有關(guān)聯(lián)的。利用za的第一次估計值對系統(tǒng)誤差向量方程中的值進行更新，通過WLS得到za的第二次估計值：

(7)

1.2 標準UKF算法

卡爾曼濾波器只適用于線性高斯模型，基于TDOA技術(shù)建立基站與標簽距離的關(guān)系為非線性方程組，對于非線性的問題需要使用擴展卡爾曼濾波器或無跡卡爾曼濾波器處理。UKF利用無跡變換在估計點附近選取采樣點，用這些采樣點表示的高斯密度近似狀態(tài)的概率密度函數(shù)，如果狀態(tài)噪聲服從高斯分布，UKF算法近似度可以達到三階近似，即使不服從高斯分布，也能實現(xiàn)二階近似[10]。相比于擴展卡爾曼濾波器對非線性函數(shù)進行近似，UKF是對非線性函數(shù)的概率密度分布進行近似，無需計算雅克比矩陣，提高精度的同時減小了算法計算復雜度。

基于基站與標簽距離的關(guān)系建立線性狀態(tài)方程和非線性觀測方程：

(8)

其中，Xk=[xk,yk,zk]T為標簽的位置坐標，F(xiàn)為狀態(tài)轉(zhuǎn)移矩陣；Zk=[d2,1,d3,1,…,dn,1]T為基站獲得的TDOA距離值；Wk、Vk分別為過程噪聲和觀測噪聲，其協(xié)方差矩陣分別為：Qk、Rk；H(Xk)=[r2,1,r3,1,…,rn,1]T。

UKF使用無跡變換來處理狀態(tài)的均值和協(xié)方差的非線性傳遞問題，對非線性函數(shù)的概率分布進行近似，用一系列確定樣本來逼近狀態(tài)的后驗概率分布。無跡變換獲得2n+1個樣本點X(Sigma點)及其對應的權(quán)值如式(9)與式(10)所示。

(9)

(10)

其中，λ=α2(n+κ)-n，W(m)，W(c)分別為期望和協(xié)方差矩陣的加權(quán)權(quán)重。

UKF算法的具體實現(xiàn)過程如下：

(1)濾波初值：

(11)

(12)

(2)計算k-1時刻Sigma點集：

(13)

(3)一步預測k時刻的狀態(tài)與協(xié)方差：

Xk|k-1=FXk-1|k-1

(14)

(15)

(16)

(4)重復步驟(2)，計算k時刻觀測預測，獲取觀測均值和協(xié)方差：

(17)

Zk+1|k=H(Xk+1|k)

(18)

(19)

PZkZk=

(20)

(21)

(5)量測更新：

Kk=PXkZk(PZkZk)-1

(22)

Pk|k=Pk|k-1-KkPZkZkKkT

(23)

(24)

在標準UKF濾波算法中需要經(jīng)過兩次UT變換，分別對系統(tǒng)的狀態(tài)方程和觀測方程的概率密度分布進行近似。根據(jù)公式(3)可知，基于TDOA定位技術(shù)的Chan算法狀態(tài)方程為線性方程，利用UT變化中Sigma點的預測值加權(quán)求得的系統(tǒng)狀態(tài)預測對提高定位精度的意義不大，反而會加大算法的運算量，降低運算效率。為了降低算法的復雜度， Chan-AUKF算法對標準UKF算法進行了簡化，只進行對量測方程的UT變換，省略標準UKF算法的步驟(2)，簡化步驟(3)，對狀態(tài)預測及預測協(xié)方差進行修正：

(25)

Pk|k-1=FPk-1|k-1FT+Qk-1

(26)

2 Chan-AUKF融合算法

在實際定位環(huán)境中，基站與標簽之間通訊的UWB信號受NLOS與多徑效應的影響，測量得到的UWB信號飛行時間摻雜著很多誤差，將這些含有誤差的測量值直接代入上述算法中解算出來的標簽三維坐標與實際坐標之間存在著很大的誤差[14-15]。針對這一問題，該文提出一種Chan-AUKF混合算法。Chan-AUKF算法使用Chan算法和簡化的UKF算法對標簽的三維坐標進行初步估計，利用標簽的三維坐標初步估計值建立系統(tǒng)狀態(tài)誤差補償函數(shù)，對簡化的UKF算法進行實時自適應補償，并將其作為AUKF算法的初值代入對標簽的精確坐標進行估計。Chan-AUKF算法實現(xiàn)的具體流程如圖1所示。

具體步驟如下：

(1)對標簽三維坐標進行初始估計。

通過UWB信號在基站和標簽之間的傳播時間戳測得標簽與不同基站間的TDOA值，利用Chan算法根據(jù)測得的TDOA值對標簽的三維坐標進行解算；將Chan算法的解算結(jié)果作為UKF算法的初值對標簽的三維坐標進行初始解算。

(2)建立系統(tǒng)狀態(tài)誤差補償函數(shù)。

根據(jù)簡化UKF算法初始解算出標簽的三維坐標值與定位基站分布的中心點坐標值構(gòu)造狀態(tài)誤差補償函數(shù)。假設(shè)定位基站的分布的中心點坐標為(xc,yc,zc)，Chan算法解算得到的定位標簽三維坐標為(xk-1,yk-1,zk-1)，兩個坐標值的偏差如下：

圖1 Chan-AUKF算法流程

(27)

根據(jù)坐標偏差大小，分別設(shè)置坐標的補償尺度因子kx、ky、kz，尺度補償因子大小與UWB的誤差特性和實際環(huán)境有關(guān)，將坐標狀態(tài)補償量標記量xoffset標記為：

(28)

xoffset=[offsetx,offsety,offsetz]T

(29)

根據(jù)標簽坐標狀態(tài)的補償量，將簡化UKF模型的狀態(tài)方程更新為：

(30)

(3)對標簽三維坐標進行精確估計。

根據(jù)公式(30)對簡化UKF算法的狀態(tài)方程和觀測方程進行自適應補償，將標簽的三維坐標的初步解算結(jié)果作為更新后的AUKF算法初值代入，對標簽的三維坐標進行精確估計。

3 實驗與結(jié)果分析

為了充分驗證所提方法的合理性，搭建實驗環(huán)境對算法進行驗證。將5個基站放置在10m×10m×30m的實驗區(qū)域內(nèi)：基站1(0,0,0)，基站2(6.02,3.12,0)，基站3(3.12,6.04,1.56)，基站4(6.16，3.08,3.22)，基站5(3.32,6.21,4.62)，中心點坐標為(3,3,3)。實驗測試環(huán)境如圖2所示，測試前對各個基站在視距環(huán)境下校準。

圖2 實驗測試環(huán)境

將5個基站分別校準后，選取合適的位置放置定位標簽，利用標簽與基站之間的信號傳播，測量TDOA值，并將測得的TDOA值載入MATLAB，分別利用Chan算法、標準UKF算法、Chan-AUKF算法對標簽的三維坐標分別進行解算。

實驗中，對每一個標簽分別進行了10次測量，測得10組TDOA值，將這10組TDOA值載入MATLAB中利用三種算法進行三維解算，如圖3所示，分別為Chan算法、UKF算法、Chan-UKF算法和Chan-AUKF算法得到的估計坐標與實際坐標的對比。

(a)Chan算法估計坐標與實際坐標

(b)UKF算法估計坐標與實際坐標

(c)Chan-UKF算法估計坐標與實際坐標

(d)Chan-AUKF算法估計坐標與實際坐標

從圖3中可以看出，相比于Chan算法、標準UKF算法和Chan-UKF算法，Chan-AUKF算法解算出的標簽坐標估計值與標簽實際坐標最接近， Chan算法解算出的標簽坐標估計值與實際坐標的誤差最大，UKF算法次之，Chan-UKF算法解算出的標簽坐標估計值與實際坐標誤差比UKF算法更小，但與Chan算法相比，UKF算法和Chan-UKF算法解算出的坐標與實際坐標更加接近。對比四種種算法解算出坐標值的分布，隨著定位標簽與定位基站之間距離的增大，Chan-AUKF算法解算出的坐標與實際位置的誤差無明顯的變化，而Chan算法、標準UKF算法和Chan-UKF算法解算出的標簽坐標與標簽實際坐標的誤差隨著標簽與基站之間距離的增大而增大，解算結(jié)果存在發(fā)散的現(xiàn)象。

為了從數(shù)值上對三種算法的定位精度進行比較，分別計算Chan算法、標準UKF算、Chan-UKF算法和Chan-AUKF算法解算出的標簽坐標估計值與實際坐標的均方根誤差(root mean square error，RMSE)值。圖4為三種算法RMSE值變化曲線，表1為四種算法的RMSE值。

圖4 標簽估計坐標RMSE值曲線

從圖4中可以看出，Chan算法的定位精度最低，坐標估計結(jié)果與實際坐標的誤差最大，不能作為最后的估計結(jié)果使用；標準UKF算法和Chan-UKF算法相比較于Chan算法，雖然定位精度有所提高，但隨著標簽與基站之間的距離增大，定位誤差也隨之增大，不能滿足定位要求。Chan-AUKF算法與前兩種算法相比，在提高定位精度的同時，能夠有效抑制基站與標簽之間距離增大給定位結(jié)果帶來的誤差，實時自適應地對狀態(tài)進行補償。

表1 不同算法的RMSE值對比

為了從數(shù)值的角度分析四種算法的誤差，對四種算法估計結(jié)果的RMSE進行統(tǒng)計。從表1中可以看到四種算法對標簽的三維估計坐標的RMSE分析，Chan算法的RMSE值最大為0.814 4 m，最小為0.130 4 m，平均值為0.474 5 m；UKF算法的RMSE值最小為0.093 5 m，最大為0.696 3 m，平均值為0.415 5 m；Chan-UKF算法的RMSE值最大為0.627 9 m,最小值為0.092 2 m，平均值為0.345 8 m；Chan-AUKF算法的RMSE值最大為0.092 8 m,最小值為0.042 2 m，平均值為0.065 7 m。無論是從RMSE的最大值、最小值還是平均值來看，Chan-AUKF算法能夠有效地提高定位精度。

4 結(jié)束語

針對UWB定位系統(tǒng)在三維定位中定位精度低、存在固定偏差的問題，基于Chan算法與簡化UKF算法，根據(jù)定位環(huán)境的誤差特性，通過構(gòu)建狀態(tài)誤差補償函數(shù)，對簡化UKF算法的狀態(tài)方程進行自適應實時狀態(tài)誤差補償。實驗結(jié)果表明，在NLOS環(huán)境中，Chan算法的定位精度最低，UKF算法和Chan-UKF算法的定位精度雖然比Chan算法高，但也受NLOS 誤差的干擾。利用Chan算法和簡化UKF算法得到的初始估計坐標構(gòu)建狀態(tài)誤差補償函數(shù)對簡化的UKF算法進行實時狀態(tài)誤差補償，可以有效降低NLOS誤差對定位結(jié)果的影響，提高定位精度。改進后的Chan-AUKF算法在NLOS環(huán)境中的定位精度可以達到厘米級，在保證穩(wěn)定性的同時，有效地提高了定位精度，滿足了UWB三維定位系統(tǒng)的定位要求。