改進的Chan-粒子濾波算法超寬帶室內(nèi)三維定位

李 鵬，向宇翔，榮冬成，凌智琛,李 朋

(湘潭大學自動化與電子信息學院，湖南湘潭 411100)

0 引言

隨著人們生活水平的不斷提高，對室內(nèi)位置服務的要求也越來越高，室內(nèi)定位的精度問題也成為了室內(nèi)位置服務中的重中之重。超寬帶(Ultra-Wide Band，UWB)技術(shù)作為近年來室內(nèi)定位中非常熱門的一種定位方式，憑借時間分析率高、穿透性強、覆蓋范圍廣、系統(tǒng)復雜度低、抗多徑效應良好等優(yōu)勢，越來越受到廣大公司和研究者的重視。但是傳統(tǒng)的UWB定位算法大多都有相應的缺點，如Chan算法、Fang算法、泰勒級數(shù)展開法等，這些算法要想得到較高的定位精度，就必須要求系統(tǒng)滿足測量誤差服從理想高斯分布。而在現(xiàn)實環(huán)境中，處處都存在非視距(Non Line of Sight，NLOS)誤差，而NLOS誤差的存在又會導致定位時產(chǎn)生多徑效應，從而無法進行準確定位，最終影響室內(nèi)定位精度。

在到達時間差(Time Difference of Arrival，TDOA)算法中，由測量值建立的標簽與基站之間的關系方程是非線性的，而粒子濾波(Particle Filter，PF)算法又是基于序貫蒙特卡羅算法結(jié)構(gòu)，常用于非線性非高斯系統(tǒng)模型，因此在NLOS較強的環(huán)境中，PF算法相比擴展卡爾曼濾波(Extended Kalman Filter，EKF)、無跡卡爾曼濾波(Unscented Kalman Filter，UKF)算法有更好的估計性能。文獻[10]提出了一種自適應卡爾曼濾波算法，能進一步提高室內(nèi)視距環(huán)境下的定位精度，但是這種算法有著傳統(tǒng)卡爾曼濾波算法的通病，即不能有效解決在NLOS環(huán)境中定位精度低的問題。文獻[11]提出了將UWB定位中的包括到達時間(Time of Arrival，TOA)、接收信號強度指示(Received Signal Strength Indicator，RSSI)、到達角(Angle of Arrival，AOA)在內(nèi)的多種算法得到的測距信息進行融合定位，從而達到抑制NLOS誤差的效果，但是該方法成本較大。文獻[12]提出了對UWB采集的原始數(shù)據(jù)進行兩次濾波，從而實現(xiàn)高精度定位，但是該作者使用了TOA方法進行初步定位，該方法在實際環(huán)境中往往面臨硬件無法實現(xiàn)時鐘同步的問題，因而無法避免地會產(chǎn)生定位誤差。文獻[13]將TDOA 算法與UKF算法進行融合，提高了室內(nèi)定位精度，但是UKF在非線性嚴重的環(huán)境中仍然無法進行準確定位。

針對上述問題，為了能夠有效抑制在室內(nèi)定位中存在的NLOS誤差，實現(xiàn)室內(nèi)高精度定位，滿足智能時代人們對于高精度室內(nèi)定位的需求，本文提出了一種Chan-PF UWB聯(lián)合室內(nèi)定位算法。首先，利用模擬退火算法能在解空間內(nèi)隨機尋找全局最優(yōu)解的能力輔助Chan算法進行定位解算，由于Chan算法受NLOS影響較大，因此定位精度會隨著室內(nèi)障礙物的增多而大幅度降低；所以，第二步將Chan算法解算得到的坐標通過PF算法進一步提高精度，得到精確的標簽坐標，從而最大限度地削弱NLOS誤差的影響，實現(xiàn)高精度室內(nèi)定位。

1 改進的Chan算法

1.1 TDOA定位原理

TDOA算法是對TOA算法的改進，與TOA定位算法相比，TDOA是用多個基站接收到信號的時間差來確定標簽的位置，不需要實現(xiàn)硬件中的時鐘同步，定位精度也更高。

TDOA定位原理如下：假設有個基站，基站1作為參考基站，已知基站(=1,2,…，)的坐標為(,),并假設標簽的坐標為(,)、基站與標簽的距離為。

已知信號到達參考基站1和第個基站的時間差及信號傳播速度，可以得到不同基站與標簽的距離差為-(=1,2,…，)。因此，可以建立以2個基站為焦點、距離差為長軸的雙曲線方程來求解標簽位置。

=-

(1)

=-

(2)

1.2 Chan算法

Chan算法是基于TDOA定位方法的一種定位算法。比起TDOA定位方法中的另外兩種算法：Fang算法和泰勒算法，Chan算法計算量小，且在視距環(huán)境下能夠取得良好的定位精度。但由于在NLOS環(huán)境下Chan算法的定位精度會大幅度下降，所以一般與其他算法協(xié)同定位。由TDOA方法中的幾何關系可知

(3)

將式(1)、式(2)代入式(3)得

(4)

其中，,1=-，,1=-，,1=-。

以=[,,]作為未知變量，可建立誤差矢量的方程組

=-

(5)

通過加權(quán)最小二乘法(Weighted Least Squar-es, WLS)，計算出第一次估計結(jié)果

(6)

1.3 改進的Chan算法

為了進一步提高定位精度，除了加入能解決NLOS誤差的PF之外，本文還在Chan算法中加入了模擬退火算法，模擬退火算法具有局部搜索能力強、運行時間較短等優(yōu)勢。假設場所內(nèi)共有個基站，個標簽，對于每個待測目標，模擬退火算法的目標函數(shù)設置為

(7)

其中，為初始坐標(,,)；為目標與基站的距離估計值；′為測量值。目標函數(shù)的含義為：使用估計的待測目標坐標求出的與測量值′差距的絕對值越小，估計的坐標越準確。

(8)

算法的具體步驟如下：

1)設初始坐標為，同時計算目標函數(shù)()，設當前迭代次數(shù)=0，當前溫度=，∈(0,1)用于控制降溫退火。

2)在初始坐標周圍生成新的坐標′，并再次計算目標函數(shù)(′)。

4)判斷是否達到設置的迭代次數(shù)，如果沒有則繼續(xù)步驟2)。

5)若連續(xù)若干個新解都沒有滿足條件，則終止算法，滿足的話輸出最終結(jié)果，否則重置迭代次數(shù)，并降低初始溫度=。

6)得到坐標初始估計值(′′′)。

7)將得到的初始值代入Chan算法中，求出式(6)中的，從而得到第一次最小二乘法的解。

2 Chan-PF聯(lián)合定位算法

2.1 PF

PF的基本思想是利用一組粒子近似地表示系統(tǒng)的后驗概率，從而估計系統(tǒng)的狀態(tài)。與卡爾曼濾波和EKF相比，PF常用于非線性環(huán)境中，因此在NLOS環(huán)境中，本文使用PF對TDOA的測量值進行濾波。

PF的步驟為：

1)建立定位數(shù)學模型。

=[]

(9)

=[]

(10)

其中，為狀態(tài)矩陣；為觀測矩陣；、、分別為、、軸坐標；、、分別為、、方向的速度。

2)預測。

+1=()+()+

(11)

其中，為狀態(tài)矩陣；為控制矩陣；為過程噪聲；為狀態(tài)轉(zhuǎn)移矩陣；為控制方程。

3)更新。

(12)

其中，為粒子的權(quán)重，更新后通過后驗概率判斷粒子的權(quán)重，從而判斷當前系統(tǒng)的狀態(tài)。

4)重采樣。

為降低粒子退化，對粒子進行重采樣處理。重采樣根據(jù)粒子的權(quán)重，在原有基礎上生成新的粒子集，由于粒子集中的粒子數(shù)不多且多項式重采樣易于實現(xiàn)，因此本文選擇多項式重采樣。多項式重采樣算法直接從均勻分布(0,1]中，隨機抽取個獨立分布的隨機樣本，并將抽取到的樣本與歸一化權(quán)值的累計和進行比較來選取粒子。

5)提高樣本多樣性。

重采樣過程雖然可以讓粒子更加集中在有效區(qū)域，改善粒子的退化現(xiàn)象，但由于在重采樣過程中復制了許多大權(quán)重粒子，導致降低了粒子的多樣性。本文對樣本集合中的每個粒子都加上一個零均值高斯噪聲擾動，使粒子更加分散，提高粒子的多樣性，即粒子粗化過程。

2.2 Chan-PF聯(lián)合定位算法

在實際應用環(huán)境中，基站與標簽之間常常會存在一些障礙物。這些障礙物導致UWB信號發(fā)生折射現(xiàn)象，從而產(chǎn)生NLOS誤差。為了解決在復雜環(huán)境中較大的NLOS誤差導致定位精度低的問題，本文提出了一種Chan-PF聯(lián)合定位算法。

Chan-PF聯(lián)合室內(nèi)定位算法的步驟如下：

1)設狀態(tài)方程為

()=(-1)+(-1)

(13)

式中，為狀態(tài)轉(zhuǎn)移矩陣；為過程噪聲驅(qū)動矩陣；()為過程噪聲。行人矢量為

()=[]

(14)

式中，,,為3個坐標軸的位置；,,為3個方向的速度。

行人觀測方程為

()=(())+()

(15)

式中，()為觀測噪聲。

2)根據(jù)式(12)和式(14)，使用Chan算法進行初步定位，得到相應的測量值

=()

(16)

4)粒子集合按照1)中的運動方程進行運動，得到新的粒子集，并以Chan算法中所得測量值作為觀測值，根據(jù)粒子集合與觀測值之間的距離計算各粒子對應權(quán)值。

(17)

(18)

6)歸一化權(quán)值。

3 仿真與實驗

3.1 仿真結(jié)果與分析

假設行人在室內(nèi)行走，軌跡為矩形，6個基站坐標分別為：(0,0,20cm)；(0,438cm,403cm)；(0,873cm,20cm)；(644cm,873cm,403cm)；(644cm,438cm,20cm)；(644cm,0,403cm)。標簽初始位置(100cm,100cm,100cm)，初始速度為(25cm/s,0,0)，對真實軌跡、用Chan算法得到的標簽軌跡和分別加了Chan-PF、Chan-EKF算法之后的標簽軌跡進行比較。

3.1.1 視距環(huán)境下的仿真結(jié)果

在視距環(huán)境下，圖1所示為行人的真實軌跡和預測軌跡，圖2所示為三種算法在不同坐標軸下的誤差值。

圖1 三維跟蹤軌跡Fig.1 3D tracking trajectory

(a)

(b)

(c)圖2 三種算法的誤差對比圖Fig.2 Error comparison of three algorithms

圖1中，紅點表示基站坐標，橙色線條表示在Chan-PF算法下的行人軌跡，藍色線條表示單一Chan算法下的行人軌跡，紅色線條表示在Chan-EKF算法下的行人軌跡，三角形表示行人行走的真實軌跡。

由表1各個軸的誤差均值可以看出，Chan、Chan-PF、Chan-EKF在方向的定位精度分別為5.72cm、4.11cm、4.67cm；在方向分別為7.87cm、4.15cm、6.23cm；在方向分別為9.73cm、4.52cm、7.32cm。由此可以看出，在視距環(huán)境下，Chan-PF算法和Chan-EKF算法均能小幅度改善定位精度。通過算法計算用時可以看出，Chan-PF算法用時相對更多，實時性上可能不如Chan算法，但是整體而言差距不大，Chan-PF算法在NLOS情況下的表現(xiàn)更好。

表1 視距環(huán)境下三種算法的誤差對比

3.1.2 NLOS環(huán)境下的仿真結(jié)果

在NLOS干擾較大時，圖3所示為行人的真實軌跡和預測軌跡。

圖3 三維跟蹤軌跡Fig.3 3D tracking trajectory

本文在標簽與基站的測量值中加入一個服從指數(shù)分布的隨機變量()表示NLOS誤差。其條件概率密度函數(shù)為

(19)

式中，為均方根時延擴展，IEEE802.15.4a信道模型中該參數(shù)的均值和標準差通常為，nlos=27,,nlos=044。

通過圖4可以直觀地看出，在NLOS干擾較大時，Chan-PF算法能更加有效地降低定位誤差，實現(xiàn)室內(nèi)NLOS情況下的精確定位。

(a)

(b)

(c)圖4 三種算法的誤差比較圖Fig.4 Error comparison of three algorithms

由表2可以看出，在受到NLOS誤差干擾時，Chan-PF算法的優(yōu)化作用更加明顯，尤其在方向和方向，定位精度有了較大的改善。在軸方向，Chan算法平均定位誤差為14.23cm,Chan-PF算法定位誤差為7.725cm,Chan-EKF算法定位誤差為9.88cm；在軸方向，Chan算法平均定位誤差為17.33cm,Chan-PF算法定位誤差為7.12cm,Chan-EKF算法定位誤差為9.28cm。

表2 NLOS環(huán)境下三種算法的誤差對比

3.2 實驗驗證

實驗采用UWB模塊為DecaWave公司的DW1000芯片，采用串口通信，實驗場景如圖5所示。

圖5 UWB測試平臺及測試場景Fig.5 UWB test platform and test scenario

實驗地點為室內(nèi)，實驗場景選擇在實驗室，其中，4個基站設在室內(nèi)，2個基站設在室外。主要的NLOS因素為信號在多面墻壁之間的折射反射，基站坐標為：(0,885cm,180cm)，(0,1435cm,180cm)，(325cm,1435cm,180cm)，(325cm,885cm,180cm)，(560cm,1245cm,180cm)，(560cm,0,180cm)。標簽起點為(560cm,1000cm,180cm)，終點為(325cm,1400cm,180cm)。測試人員手持標簽沿預設軌跡緩慢移動，總步數(shù)為40，分別采用Chan-PF算法和Chan-EKF算法進行解算。定位誤差如圖6所示，從上到下分別為Chan-PF算法、Chan-EKF算法和Chan算法。表3所示為坐標平均誤差。

(a)

(b)

(c)圖6 實際場景軌跡誤差Fig.6 Actual scene trajectory error

表3 三種算法誤差情況

由以上圖表可以得出：在NLOS環(huán)境下， Chan-PF算法的定位精度有了較大的提升。雖然三種算法在實驗中的定位精度相較于在仿真中都有了不同程度的降低，但考慮到實際環(huán)境中的NLOS誤差不僅僅有信號的折射，還存在著墻壁的阻隔，因此該算法還是能夠很好地提高定位精度。

4 結(jié)論

針對室內(nèi)NLOS條件下Chan算法定位精度低的問題，本文提出了一種UWB室內(nèi)混合三維定位算法。首先用模擬退火算法輔助Chan算法處理目標距離基站較近時的初始值估計，尋找最優(yōu)解；其次用PF對改進Chan算法進行濾波，降低在NLOS環(huán)境下的定位誤差，得到標簽的精確位置。通過仿真實驗可以得出結(jié)論：

1)在視距環(huán)境中，由于NLOS誤差影響小，所以應該優(yōu)先選擇效率高、實時性最好的算法。

2)當室內(nèi)存在NLOS誤差或NLOS誤差較大時，單一算法得到的定位效果并不理想。本文提出的改進Chan-PF算法能很好地處理NLOS誤差的情況，從而使定位效果得到有效改善。經(jīng)過實驗驗證，也證明了該方法具有良好的工程應用價值。在后續(xù)的工作中可以通過加入慣導，進一步提高室內(nèi)復雜環(huán)境下的定位精度。