劉明佳 駱 曦 劉志新

（1.秦皇島港股份有限公司第六港務(wù)分公司，秦皇島 066000）（2.燕山大學(xué)電氣工程學(xué)院 秦皇島 066004）

基于ToA技術(shù)與擴展卡爾曼濾波的室內(nèi)無線定位算法設(shè)計?

劉明佳1駱 曦2劉志新2

（1.秦皇島港股份有限公司第六港務(wù)分公司，秦皇島 066000）（2.燕山大學(xué)電氣工程學(xué)院 秦皇島 066004）

無線室內(nèi)定位是物聯(lián)網(wǎng)技術(shù)的重要應(yīng)用之一，為提高對于運動目標(biāo)的定位和跟蹤的精度，論文設(shè)計了基于TOA技術(shù)的擴展卡爾曼濾波算法。提出利用雙邊雙向測距算法進行節(jié)點間的距離測定，將時鐘誤差及測距誤差視為狀態(tài)噪聲，設(shè)計擴展卡爾曼濾波器，獲得遞推估計算法。在不同運動軌跡和不同噪聲參數(shù)下的仿真測試結(jié)果表明，所設(shè)計算法可有效提高定位精度，改善目標(biāo)跟蹤效果。

室內(nèi)定位；擴展卡爾曼濾波；跟蹤誤差；去噪

1 引言

室內(nèi)無線定位技術(shù)是物聯(lián)網(wǎng)應(yīng)用的關(guān)鍵技術(shù)之一，近年來受到國內(nèi)外研究人員的廣泛關(guān)注［1～2］。相比于室外定位技術(shù)而言，由于室內(nèi)通信環(huán)境更為復(fù)雜，無線信號在傳輸過程中存在不同程度的信號衰減、反射、多徑效應(yīng)的影響，使得定位精度、定位范圍均受到較大程度的影響。利用無線信號進行室內(nèi)定位的手段主要包括超聲波、紅外線、超寬帶（UWB）、射頻識別技術(shù)（RFID）、WiFi、ZigBee等［3～5］。不同的定位技術(shù)具有不同的定位精度和適用場合，如超聲波定位、UWB定位、RFID定位均可達到較高的定位精度，但其受視距測量、設(shè)備成本高等因素的限制。WiFi技術(shù)和ZigBee技術(shù)利用免費的ISM頻段雖具有成本低易于實現(xiàn)的優(yōu)點，但無線信號易受干擾，使得測距定位信息存在誤差而影響定位精度［6～8］。

對室內(nèi)目標(biāo)的定位問題一般可利用三邊定位方法實現(xiàn)，而前提是獲得錨節(jié)點與目標(biāo)間的距離信息。因此測量節(jié)點間的距離是定位實現(xiàn)過程中直接影響性能的關(guān)鍵所在，目前應(yīng)用較為廣泛的室內(nèi)測距技術(shù)主要有基于到達時間（TOA：Time of Arriv?al）、基于到達時間差（TDOA：Time Difference of Ar?rival）、基于信號到達角度（AOA：Angle of Arrival）、基于接收信號強度（RSSI：Received Signal Strength Indicator）等方法［9～10］。其中，TOA 方法簡單易于實現(xiàn)，他通過測量發(fā)射信號在空間上的飛行時間來測得距離，可分為單程測距和雙程測距兩種方式［3］。單程測距為信號從錨節(jié)點到達目標(biāo)節(jié)點后不返回，雙程測距為信號從源節(jié)點到達目標(biāo)節(jié)點后，又從目標(biāo)節(jié)點返回源節(jié)點。兩個節(jié)點之間使用單向測距要時鐘同步時間，而雙向測距不必考慮這個問題，但本地時間引起的偏差及測量噪聲也會影響定位精度［11～12］。為了解決室內(nèi)定位中測距干擾和數(shù)據(jù)噪聲導(dǎo)致的定位與跟蹤誤差較大的問題，本文提出了一種基于擴展卡爾曼濾波的室內(nèi)定位算法。首先使用TOA這一方法進行測距，然后再使用卡爾曼濾波算法去除測量及計算噪聲，最終達到提高室內(nèi)定位系統(tǒng)的性能的目的。

2 定位原理

如前所述，為避免復(fù)雜的時鐘同步問題，本文采用對稱雙邊雙向測距算法（Symmetrical Dou?ble-Sided Two-Way Ranging，SDS-TWR）測量端點間的距離。其原理如圖1所示。

該過程分為兩次測量，首先，節(jié)點A向節(jié)點B發(fā)送數(shù)據(jù)。此時節(jié)點A開始計時，當(dāng)節(jié)點B接收到數(shù)據(jù)后，向節(jié)點A返回數(shù)據(jù)，記錄下節(jié)點B的處理時間T2。節(jié)點B開始計時，當(dāng)節(jié)點A接收到從節(jié)點B返回的數(shù)據(jù)后，停止計時，記錄下節(jié)點A的讀數(shù)T1。然后再次向節(jié)點B發(fā)送數(shù)據(jù)，記錄下節(jié)點A的處理時間T4，當(dāng)節(jié)點B接收到數(shù)據(jù)后，停止計時，記錄下節(jié)點B的讀數(shù)T3。然后節(jié)點B返回初始狀態(tài)，并向節(jié)點A返回數(shù)據(jù)，節(jié)點A接收到返回的數(shù)據(jù)后也返回初始狀態(tài)。節(jié)點A到節(jié)點B之間的單向時間由以下公式得到：

由于無線信號在空氣介質(zhì)中的傳播速度一定，易得兩點間的距離。若目標(biāo)與至少三個錨節(jié)點間距離已知，則可利用三邊定位法確定目標(biāo)節(jié)點的位置，如圖2所示。若已知錨節(jié)點A，B，C的位置以及錨節(jié)點到待測物體的距離，則每一個目標(biāo)節(jié)點的位置，都可以通過以三個錨節(jié)點為圓心的圓唯一地確定，從而確定該目標(biāo)節(jié)點D的位置。

節(jié)點間的距離可表示為

其中：Zk=[d1kd2kd3k]T，表示目標(biāo)節(jié)點到三個錨節(jié)點間的距離，vk是測量噪聲，方差為R。對于運動目標(biāo)進行連續(xù)測距和定位即可實現(xiàn)目標(biāo)跟蹤。

使用SDS-TWR算法雖不需要兩個節(jié)點進行時鐘同步，而且由于進行了兩次測量，對測得時間的平均處理又使得因為本地時鐘造成的距離誤差減小。但由于無線信道的復(fù)雜性，使得接收信號易受到各種噪聲的影響，這里歸結(jié)為噪聲項vk。去除測數(shù)據(jù)噪聲干擾的方法很多，基本上分為硬件濾波和軟件濾波兩種。本文采用的擴展卡爾曼濾波（Extended Kalman Filter，EKF）是軟件濾波的一種方法。EKF的基本流程為：首先使用狀態(tài)函數(shù)對上一時刻狀態(tài)進行迭代，求出先驗狀態(tài)和誤差矩陣。然后求出卡爾曼增益，根據(jù)TOA測得的距離估算目標(biāo)節(jié)點狀態(tài)，然后使用目標(biāo)節(jié)點的狀態(tài)對先驗狀態(tài)進行校正得到目標(biāo)節(jié)點的后驗狀態(tài)，然后代到下一個時刻進行迭代。從而實現(xiàn)移動目標(biāo)的連續(xù)跟蹤。

3 算法設(shè)計

假設(shè)運動目標(biāo)的非線性離散系統(tǒng)的方程如下：

其中：Xk是狀態(tài)變量，f和h均為非線性函數(shù)，分別表示前一時刻狀態(tài)與當(dāng)前時刻狀態(tài)、狀態(tài)變量與觀測值之間的關(guān)系。wk和vk分別為狀態(tài)和觀測噪聲，假設(shè)為高斯噪聲，其方差分別為Qk和Rk。

EKF分為兩個部分：估計部分、校正部分。預(yù)估階段是根據(jù)系統(tǒng)狀態(tài)模型及前一時刻的估計值預(yù)估當(dāng)前系統(tǒng)的狀態(tài)，同時求出預(yù)估狀態(tài)誤差的協(xié)方差矩陣；校正階段是通過預(yù)估狀態(tài)、當(dāng)前時刻的觀測值以及測量模型對當(dāng)前的系統(tǒng)狀態(tài)進行校正，并得到當(dāng)前狀態(tài)誤差的協(xié)方差矩陣，并將當(dāng)前時刻的狀態(tài)變量代入到下一時刻。

在已知初始狀態(tài)的后驗估計X0及其方差P0的條件下，k-1時刻狀態(tài)變量的值及其方差分別為X?k-1和Pk-1，則按如下步驟計算第k時刻的先驗估計。

時間更新方程：

狀態(tài)更新方程：

本書建立在作者多年頂級物流企業(yè)的從業(yè)基礎(chǔ)上，根據(jù)中國電商企業(yè)存在的物流相關(guān)共性問題，結(jié)合中國的現(xiàn)實環(huán)境和商業(yè)趨勢，最終希望解決的是電商物流的現(xiàn)實問題，并提供一套行之有效的方法，同時也為想要了解電商物流的普通讀者奉上了最好的讀物。

下面建立模型并代入到擴展卡爾曼濾波方程中，系統(tǒng)狀態(tài)模型可表示為

然后根據(jù)三邊定位原理建立的觀測模型，已知第n個錨節(jié)點的坐標(biāo)是(xn，yn)，k時刻目標(biāo)節(jié)點到三個錨節(jié)點距離的測量值為(d1k，d2k，d3k)，等式右邊應(yīng)為狀態(tài)方程中得到的坐標(biāo)(xk，yk)到三個錨節(jié)點的距離，測量距離時受到的噪聲用vk表示。目標(biāo)節(jié)點和錨節(jié)點間距離公式如下：

vk是高斯噪聲，方差為R。

由于觀測模型非線性的，因此需要求hk(Xk)的雅可比矩陣：

因此通過第k-1時刻的狀態(tài)估計，估算k時刻狀態(tài)的EKF算法如下：

其具體流程如圖3所示。

4 仿真分析

為測試算法在不同環(huán)境下的定位性能，分別設(shè)定待定位目標(biāo)做直線運動，圓周運動和不規(guī)則運動，并且分析在不同的噪聲干擾下的定位效果。我們假設(shè)系統(tǒng)受到零均值高斯噪聲的影響，設(shè)觀測噪聲的方差R=1，狀態(tài)噪聲的方差Q=0.003。在運動直線運動下定位和跟蹤效果如圖4所示，并與傳統(tǒng)的最小二乘法進行了誤差對比，如圖5所示。

從圖4、圖5看見，EKF對于直線運動具有較好的跟蹤效果。雖在初始階段EKF算法產(chǎn)生較大的誤差，但在迭代一定次數(shù)后定位誤差明顯減小。兩種算法下的平均誤差分別為0.3581m和1.5414m。與最小二乘法得到的估計相比，EKF得到的跟蹤軌跡更加平滑，誤差更小，其濾波去噪的作用更加有效。

對于待跟蹤目標(biāo)做圓周運動和不規(guī)則運動的跟蹤效果分別如圖6和圖7所示。與直線運動跟蹤結(jié)果相似，EKF算法均表現(xiàn)出較好的濾波效果，有效提高了定位精度。在三種不同運動軌跡下的平均誤差如表1所示，可見EKF算法對于規(guī)則運動的目標(biāo)跟蹤效果更好，其主要原因是在狀態(tài)估計的過程中，若兩個觀測點狀態(tài)的相關(guān)性較強，則后驗估計的效果會更加準(zhǔn)確。類似的，當(dāng)改變不同的狀態(tài)干擾噪聲來模擬在測距過程中產(chǎn)生的距離誤差，定位結(jié)果如表2所示?？梢奅KF算法對于抑制狀態(tài)噪聲依然具有較好的效果，需要指出的是，隨著噪聲方差的加大，定位誤差也將隨之增大，這也符合一般的認知規(guī)律。

表1 不同算法下定位平均誤差（單位：m）

表2 不規(guī)則運動下不同參數(shù)下定位平均誤差（單位：m）

5 結(jié)語

本文研究了無線室內(nèi)定位過程中如何降低測量噪聲對定位精度影響的問題，提出利用擴展卡爾曼濾波算法，對測量距離產(chǎn)生的噪聲干擾加以抑制。設(shè)計了基于TOA技術(shù)的濾波算法，分析了目標(biāo)運動軌跡以及噪聲方差對定位和跟蹤性能的影響。仿真結(jié)果表明卡爾曼濾波算法可有效降低定位誤差。

［1］H.Liu，H.Darabi，P.Banerjee，J.Liu.Survey of wireless indoor positioning techniques and systems［J］.IEEE Transactions on Systems，Man，and Cybernetics，Part C：Applications and Reviews，2007，37（6）：1067-1080.

［2］ J.Jiang，X.Zheng，Y.Chen，et al.A distributed RSS-based localization using a dynamic circle expanding mechanism［J］.IEEE Sensors Journal，2013，13（10）：3754-3766.

［3］張勇，黃杰，徐科宇.基于PCA-LSSVR算法的WLAN室內(nèi)定位方法［J］.儀器儀 表學(xué)報，2015，36（2）：408-414.

［4］石柯，陳洪生，張仁同.一種基于支持向量回歸的802.11無線室內(nèi)定位方法［J］.軟件學(xué)報，2014，25（11）：2636-2651

［5］J.Prieto，S.Mazuelas，A.Bahillo，et al.Adaptive data fu?sion for wireless localization in harsh environments［J］，IEEE Transactions on Signal Processing，2012，60（4）：1585-1596.

［6］謝代軍，胡捍英，孔范增.基于分布重疊和特征加權(quán)的無線局域網(wǎng)室內(nèi)定位算法［J］.計算機科學(xué)，2013，40（11）：38-42.

［7］張勇，李飛騰，王昱潔.基于KDDA和SFLA-LSSVR算法的WLAN室內(nèi)定位算法［J］.計算機研究與發(fā)展，2017，54（5）：979-985

［8］呂亞娟，劉珊，胡榮貽，張文浩.一種基于多天線簇的新型LTE室內(nèi)定位系統(tǒng)［J］.電信科學(xué)，2017（3）：67-75

［9］王沁，何杰，張前雄，等.測距誤差分級的室內(nèi)TOA定位算法［J］.儀器儀表學(xué)報，2011，32（12）：2851-2856.

［10］H.Tian，S.Wang，H.Xie.Localization using coopera?tive AOA approach［C］//International Conference on Wireless Communications，Networking and Mobile Com?puting，2007：2416-2419.

［11］朱明強，侯建軍，劉穎，蘇軍峰.一種基于卡爾曼數(shù)據(jù)平滑的分段曲線擬合室內(nèi)定位算法［J］.北京交通大學(xué)學(xué)報，2012，36（5）：95-99

［12］張桀，沈重，聯(lián)合TDOA改進算法和卡爾曼濾波的UWB室內(nèi)定位研究［J］.現(xiàn)代電子技術(shù)，2016，29（13）：1-5.

Design of Indoor Wireless Location Algorithm Based on ToA Technique and Extended Kalman Filtering

LIU Mingjia1LUO Xi2LIU Zhixin3
（1.Qinhuangdao Port Group Co.，Ltd，Qinhuangdao 066000）（2.Institute of Electrical Engineering，Yanshan University，Qinhuangdao 066004）

Indoor wireless location is one of the most important applications of internet of things.To improve the precision of lo?cation and tracking of mobile object，the extended Kalman filter algorithm is proposed based on Time of Arrival（TOA）method in this paper.The symmetrical double-sided two-way ranging algorithm is used to measure the distance between the nodes.The errors of timer and distance are considered as state noise.The extended Kalman filter is designed and the recursive estimation algorithm is achieved.The simulations under different mobile tracking and noise parameters are done，and the results show that the proposed al?gorithm can improve the location precision and tracking performance effectively.

indoor location，extended kalman filter，tracking error，denoising

TP301

10.3969/j.issn.1672-9730.2017.11.022

Class Number TP301

2017年5月17日，

2017年6月23日

國家自然科學(xué)基金項目（編號：61473247）資助。

劉明佳，男，工程師，研究方向：工業(yè)企業(yè)自動化技術(shù)與應(yīng)用。駱曦，男，碩士，實驗師，研究方向：無線通信網(wǎng)絡(luò)及應(yīng)用。劉志新，男，博士，教授，研究方向：無線通信技術(shù)及應(yīng)用。