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

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

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

0 引言

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

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

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

1 改進的Chan算法

1.1 TDOA定位原理

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

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

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

=-

(1)

=-

(2)

1.2 Chan算法

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

(3)

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

(4)

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

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

=-

(5)

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

(6)

1.3 改進的Chan算法

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

(7)

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

(8)

算法的具體步驟如下：

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

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

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

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

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

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

2 Chan-PF聯合定位算法

2.1 PF

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

PF的步驟為：

1)建立定位數學模型。

=[]

(9)

=[]

(10)

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

2)預測。

+1=()+()+

(11)

其中，為狀態矩陣；為控制矩陣；為過程噪聲；為狀態轉移矩陣；為控制方程。

3)更新。

(12)

其中，為粒子的權重，更新后通過后驗概率判斷粒子的權重，從而判斷當前系統的狀態。

4)重采樣。

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

5)提高樣本多樣性。

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

2.2 Chan-PF聯合定位算法

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

Chan-PF聯合室內定位算法的步驟如下：

1)設狀態方程為

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

(13)

式中，為狀態轉移矩陣；為過程噪聲驅動矩陣；()為過程噪聲。行人矢量為

()=[]

(14)

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

行人觀測方程為

()=(())+()

(15)

式中，()為觀測噪聲。

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

=()

(16)

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

(17)

(18)

6)歸一化權值。

3 仿真與實驗

3.1 仿真結果與分析

假設行人在室內行走，軌跡為矩形，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 視距環境下的仿真結果

在視距環境下，圖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。由此可以看出，在視距環境下，Chan-PF算法和Chan-EKF算法均能小幅度改善定位精度。通過算法計算用時可以看出，Chan-PF算法用時相對更多，實時性上可能不如Chan算法，但是整體而言差距不大，Chan-PF算法在NLOS情況下的表現更好。

表1 視距環境下三種算法的誤差對比

3.1.2 NLOS環境下的仿真結果

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

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

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

(19)

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

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

(a)

(b)

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

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

表2 NLOS環境下三種算法的誤差對比

3.2 實驗驗證

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

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

實驗地點為室內，實驗場景選擇在實驗室，其中，4個基站設在室內，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)。測試人員手持標簽沿預設軌跡緩慢移動，總步數為40，分別采用Chan-PF算法和Chan-EKF算法進行解算。定位誤差如圖6所示，從上到下分別為Chan-PF算法、Chan-EKF算法和Chan算法。表3所示為坐標平均誤差。

(a)

(b)

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

表3 三種算法誤差情況

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

4 結論

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

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

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