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

(湘潭大學(xué)自動(dòng)化與電子信息學(xué)院，湖南湘潭 411100)

0 引言

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

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

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

1 改進(jìn)的Chan算法

1.1 TDOA定位原理

TDOA算法是對(duì)TOA算法的改進(jìn)，與TOA定位算法相比，TDOA是用多個(gè)基站接收到信號(hào)的時(shí)間差來(lái)確定標(biāo)簽的位置，不需要實(shí)現(xiàn)硬件中的時(shí)鐘同步，定位精度也更高。

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

已知信號(hào)到達(dá)參考基站1和第個(gè)基站的時(shí)間差及信號(hào)傳播速度，可以得到不同基站與標(biāo)簽的距離差為-(=1,2,…，)。因此，可以建立以2個(gè)基站為焦點(diǎn)、距離差為長(zhǎng)軸的雙曲線方程來(lái)求解標(biāo)簽位置。

=-

(1)

=-

(2)

1.2 Chan算法

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

(3)

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

(4)

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

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

=-

(5)

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

(6)

1.3 改進(jìn)的Chan算法

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

(7)

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

(8)

算法的具體步驟如下：

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

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

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

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

6)得到坐標(biāo)初始估計(jì)值(′′′)。

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

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

2.1 PF

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

PF的步驟為：

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

=[]

(9)

=[]

(10)

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

2)預(yù)測(cè)。

+1=()+()+

(11)

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

3)更新。

(12)

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

4)重采樣。

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

5)提高樣本多樣性。

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

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

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

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

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

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

(13)

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

()=[]

(14)

式中，,,為3個(gè)坐標(biāo)軸的位置；,,為3個(gè)方向的速度。

行人觀測(cè)方程為

()=(())+()

(15)

式中，()為觀測(cè)噪聲。

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

=()

(16)

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

(17)

(18)

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

3 仿真與實(shí)驗(yàn)

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

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

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

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

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

(a)

(b)

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

圖1中，紅點(diǎn)表示基站坐標(biāo)，橙色線條表示在Chan-PF算法下的行人軌跡，藍(lán)色線條表示單一Chan算法下的行人軌跡，紅色線條表示在Chan-EKF算法下的行人軌跡，三角形表示行人行走的真實(shí)軌跡。

由表1各個(gè)軸的誤差均值可以看出，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算法均能小幅度改善定位精度。通過(guò)算法計(jì)算用時(shí)可以看出，Chan-PF算法用時(shí)相對(duì)更多，實(shí)時(shí)性上可能不如Chan算法，但是整體而言差距不大，Chan-PF算法在NLOS情況下的表現(xiàn)更好。

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

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

在NLOS干擾較大時(shí)，圖3所示為行人的真實(shí)軌跡和預(yù)測(cè)軌跡。

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

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

(19)

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

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

(a)

(b)

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

由表2可以看出，在受到NLOS誤差干擾時(shí)，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)境下三種算法的誤差對(duì)比

3.2 實(shí)驗(yàn)驗(yàn)證

實(shí)驗(yàn)采用UWB模塊為DecaWave公司的DW1000芯片，采用串口通信，實(shí)驗(yàn)場(chǎng)景如圖5所示。

圖5 UWB測(cè)試平臺(tái)及測(cè)試場(chǎng)景Fig.5 UWB test platform and test scenario

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

(a)

(b)

(c)圖6 實(shí)際場(chǎng)景軌跡誤差Fig.6 Actual scene trajectory error

表3 三種算法誤差情況

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

4 結(jié)論

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

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

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