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

劉明佳 駱 曦 劉志新

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

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

劉明佳1駱 曦2劉志新2

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

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

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

1 引言

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

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

2 定位原理

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

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

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

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

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

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

3 算法設(shè)計(jì)

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

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

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

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

時(shí)間更新方程：

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

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

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

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

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

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

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

其具體流程如圖3所示。

4 仿真分析

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

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

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

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

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

5 結(jié)語(yǔ)

本文研究了無(wú)線室內(nèi)定位過(guò)程中如何降低測(cè)量噪聲對(duì)定位精度影響的問(wèn)題，提出利用擴(kuò)展卡爾曼濾波算法，對(duì)測(cè)量距離產(chǎn)生的噪聲干擾加以抑制。設(shè)計(jì)了基于TOA技術(shù)的濾波算法，分析了目標(biāo)運(yùn)動(dòng)軌跡以及噪聲方差對(duì)定位和跟蹤性能的影響。仿真結(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é)報(bào)，2015，36（2）：408-414.

［4］石柯，陳洪生，張仁同.一種基于支持向量回歸的802.11無(wú)線室內(nèi)定位方法［J］.軟件學(xué)報(bào)，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ú)線局域網(wǎng)室內(nèi)定位算法［J］.計(jì)算機(jī)科學(xué)，2013，40（11）：38-42.

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

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

［9］王沁，何杰，張前雄，等.測(cè)距誤差分級(jí)的室內(nèi)TOA定位算法［J］.儀器儀表學(xué)報(bào)，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］朱明強(qiáng)，侯建軍，劉穎，蘇軍峰.一種基于卡爾曼數(shù)據(jù)平滑的分段曲線擬合室內(nèi)定位算法［J］.北京交通大學(xué)學(xué)報(bào)，2012，36（5）：95-99

［12］張桀，沈重，聯(lián)合TDOA改進(jìn)算法和卡爾曼濾波的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日

國(guó)家自然科學(xué)基金項(xiàng)目（編號(hào)：61473247）資助。

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