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

李向博，熊 鳴，王麗婕

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

0 引 言

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

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

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

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

1 TDOA定位技術及算法

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

(1)

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

1.1 Chan算法

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

(2)

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

h-Gaza=0

(3)

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

ψ=h-Gaza

(4)

(5)

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

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

(6)

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

(7)

1.2 標準UKF算法

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

基于基站與標簽距離的關系建立線性狀態方程和非線性觀測方程：

(8)

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

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

(9)

(10)

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

UKF算法的具體實現過程如下：

(1)濾波初值：

(11)

(12)

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

(13)

(3)一步預測k時刻的狀態與協方差：

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

(14)

(15)

(16)

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

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

(25)

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

(26)

2 Chan-AUKF融合算法

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

具體步驟如下：

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

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

(2)建立系統狀態誤差補償函數。

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

圖1 Chan-AUKF算法流程

(27)

根據坐標偏差大小，分別設置坐標的補償尺度因子kx、ky、kz，尺度補償因子大小與UWB的誤差特性和實際環境有關，將坐標狀態補償量標記量xoffset標記為：

(28)

xoffset=[offsetx,offsety,offsetz]T

(29)

根據標簽坐標狀態的補償量，將簡化UKF模型的狀態方程更新為：

(30)

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

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

3 實驗與結果分析

為了充分驗證所提方法的合理性，搭建實驗環境對算法進行驗證。將5個基站放置在10m×10m×30m的實驗區域內：基站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)。實驗測試環境如圖2所示，測試前對各個基站在視距環境下校準。

圖2 實驗測試環境

將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算法解算出的標簽坐標與標簽實際坐標的誤差隨著標簽與基站之間距離的增大而增大，解算結果存在發散的現象。

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

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

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

表1 不同算法的RMSE值對比

為了從數值的角度分析四種算法的誤差，對四種算法估計結果的RMSE進行統計。從表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 結束語

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