基于自適應(yīng)卡爾曼濾波的超寬帶室內(nèi)定位系統(tǒng)*

童基均， 金利劍， 趙英杰， 高法欽， 柏雁捷

(浙江理工大學(xué) 信息學(xué)院， 浙江 杭州 310018)

0　引　言

隨著物聯(lián)網(wǎng)技術(shù)的發(fā)展以及室內(nèi)導(dǎo)航應(yīng)用的需求日益提高， 精確的室內(nèi)定位技術(shù)成為了各大研究機構(gòu)以及企業(yè)的研究熱點. 但由于室內(nèi)無線環(huán)境復(fù)雜， 無線信道混亂， 原有的室內(nèi)定位方案無法滿足精度要求[1]， 而基于超寬帶(Ultra Wideband， UWB)的室內(nèi)定位方案以其抗干擾、 抗多徑傳播能力強以及低功耗， 低成本， 高精度等特點， 得到了越來越多人的關(guān)注[2]. 比如微軟室內(nèi)定位大賽是國際盛名的室內(nèi)定位大賽(MS Indoor LOC Competition)， 從2014年開始舉辦， 每年都能吸引大量的公司和高校團隊參加， 其中基于UWB的室內(nèi)定位技術(shù)占據(jù)了半壁江山.

基于UWB室內(nèi)定位技術(shù)常用的方法有信號到達(dá)角度AOA， 信號到達(dá)時刻TOA， 信號到達(dá)時差TDOA和信號強度RSSI[3]. 其中AOA和RSSI方法單獨使用時在復(fù)雜的室內(nèi)環(huán)境下無法有效的保證定位精度， 一般作為TOA與TDOA的輔助方法使用[4-6]. 而TDOA是在TOA基礎(chǔ)上進(jìn)行改進(jìn)的方法， 具有定位精度高、 抗多徑能力強等特點， 無需基站和移動目標(biāo)時間上的嚴(yán)格同步， 實際應(yīng)用中更易實現(xiàn)[7]. 目前大部分基于UWB的室內(nèi)定位系統(tǒng)都是采用TDOA方法定位的.

目前， TDOA最常用的算法是Chan算法[8]. Chan使用了兩次最小二乘計算， 在定位基站個數(shù)大于4個， 且在LOS環(huán)境下定位時， 可以達(dá)到克拉美羅下限[9]. 但是， Chan算法對環(huán)境的依賴較高， 在定位范圍內(nèi)出現(xiàn)物體遮擋， 或在NLOS環(huán)境下時， 定位誤差較大. 因此， 論文[10]提出了Chan算法與Taylor展開法相結(jié)合的定位方法[10]， 保證了Taylor算法的快速收斂. 此外， 論文[11]提出了利用卡爾曼濾波與Chan算法相結(jié)合的方法[11]. 文中比對了各種卡爾曼濾波算法與Chan算法結(jié)合運算后的定位結(jié)果， 發(fā)現(xiàn)結(jié)合了卡爾曼濾波的定位算法精度更高， 可靠性更強. 對于卡爾曼濾波的應(yīng)用， 論文[12]和[13]發(fā)現(xiàn)將時延信息通過卡爾曼濾波處理后再進(jìn)行定位， 可以獲得更高精度的定位結(jié)果[12,13].

當(dāng)傳統(tǒng)的卡爾曼濾波用于定位時， 對于濾波所需要的協(xié)方差系數(shù)等先驗知識難以準(zhǔn)確取得， 且在系統(tǒng)運行過程中， 由于環(huán)境因數(shù)的改變或是基站長時間運行導(dǎo)致的測量結(jié)果發(fā)生溫漂等現(xiàn)象時， 傳統(tǒng)的卡爾曼濾波無法有效應(yīng)對環(huán)境產(chǎn)生的干擾和測量值的變化. 因此本文提出了一種自適應(yīng)卡爾曼濾波算法， 可以根據(jù)歷史值估算測量誤差Rk和過程誤差Qk并自動更新， 有效地減少了TDOA值誤差， 提高了定位精度.

1　自適應(yīng)卡爾曼濾波

傳統(tǒng)的卡爾曼濾波方法分為預(yù)測和更新兩步[14,15]， 其中預(yù)測階段的步驟為

(1)

式中：xk為預(yù)測值；xk-1為上一時刻的值；Pk為誤差協(xié)方差矩陣；Fx為狀態(tài)變換矩陣；bk為控制輸入；uk為控制矩陣.

更新階段的步驟為

(2)

在卡爾曼濾波中，Qk和Rk分別為過程誤差和測量誤差的協(xié)方差矩陣， 但他們在現(xiàn)實中很難精確求得， 且在系統(tǒng)運行過程中也常常會發(fā)生變化， 從而影響定位精度. 對此我們使用了可以動態(tài)調(diào)整Qk和Rk值的自適應(yīng)卡爾曼濾波算法. 通過極大似然準(zhǔn)則，Rk由式(3)計算得到

(3)

另有

(4)

式中：N表示了所需要考慮的歷史值的個數(shù). 因此測量誤差協(xié)方差矩陣Rk可以由式(5)計算得到

(5)

根據(jù)式(3)的方程組， 即可得出過程誤差協(xié)方差矩陣

(6)

為了方便計算， 將式(6)變形為

(7)

由式(2)可知

(8)

并且

(9)

由于誤差協(xié)方差矩陣Pk為對稱矩陣， 因此過程誤差協(xié)方差矩陣Qk的計算過程可以表示為

(10)

2　UWB系統(tǒng)實現(xiàn)

2.1　UWB系統(tǒng)架構(gòu)

本文設(shè)計的定位系統(tǒng)如圖 1 所示， 整個系統(tǒng)由時間同步基站， 定位基站， 移動標(biāo)簽以及定位服務(wù)器組成. 同步基站負(fù)責(zé)基站之間的時間同步， 定位基站收到定位標(biāo)簽信息后， 通過以太網(wǎng)將各個標(biāo)簽的時間戳信息傳給定位服務(wù)器. 定位服務(wù)器根據(jù)各種定位算法計算出標(biāo)簽位置信息.

圖 1　基于TDOA的UWB定位系統(tǒng)架構(gòu)圖Fig.1　The schematic of UWB position system based on TDOA

圖 2　標(biāo)簽與基站實物圖Fig.2　The pictures of tags and base station

2.2　標(biāo)簽與基站的硬件實現(xiàn)

采用DecaWave公司生產(chǎn)的DW1000芯片， 通過STM32單片機， 實現(xiàn)了移動芯片和基站的設(shè)計. 移動標(biāo)簽使用了方便攜帶的陶瓷芯片， 如圖 2(a) 所示， 定位基站設(shè)計中增加了全向天線和增益電路， 如圖 2(b) 所示. 標(biāo)簽與基站都使用W5500以太網(wǎng)網(wǎng)絡(luò)模塊通過千兆交換機與定位服務(wù)器相連.

2.3　時間同步方法

基站之間的時間同步方法有多種， 常用的有TWR測距方法、 SDS-TWR測距方法[16]和基于時差提取等方法[17]. 其中TWR測距方法與SDS-TWR測距方法需要計算出每個基站距離標(biāo)簽的距離后再計算距離差， 基站與標(biāo)簽之間的通信時間長且容易造成碰撞， 本文中采用了基于時差提取的方法， 這種時鐘同步方案采用標(biāo)簽和基站之間的雙向握手通信， 因此基站之間不需要嚴(yán)格的參考時鐘， 并且增加定位基站數(shù)量并不會增加系統(tǒng)的同步時間以及復(fù)雜度. 其原理如圖 3 所示.

圖 3　時鐘同步時序圖Fig.3　Timing diagram of clock synchronization

圖 3 中， 標(biāo)簽Tag等間隔地發(fā)送定位信號，T1,T2,T3分別為各基站接收到來自標(biāo)簽的定位信號到達(dá)時刻. 同步基站(Syn)接收到標(biāo)簽信號后延遲T秒發(fā)送信號， 假定間隔T足夠大， 可以保證所有基站能夠接收完同步基站(Syn)發(fā)出的基準(zhǔn)信號.T為設(shè)定的固定值.t1,t2,t3分別是BS1, BS2及BS3的開始工作時間(未知量)；S1,S2,S3是分別是BS1, BS2, BS3收到的來自同步基站(Syn)的基準(zhǔn)信號到達(dá)時刻；D1,D2,D3是同步基站(Syn)與定位基站BSi(i=1, 2, 3)之間的傳輸時延(同步基站(Syn)與定位基站位置固定， 該時延可計算得到)； 則TDOA的值可由式(11)得到， 其中Tij表示信號從標(biāo)簽到定位基站BSi與定位基站BSj(i≠j)之間的TDOA值(時間差).

(11)

2.4　定位算法

目前國內(nèi)定位算法大多數(shù)采用的是Taylor級數(shù)展開法和Chan算法[18]， 在其它方面如解混疊解交織等細(xì)節(jié)方面目前相關(guān)的跟蹤研究都很少見到. 該算法采用二次估計方法實現(xiàn)對MS位置的估計， 計算簡單、 定位精度高具有很大的實用價值[19]. 在二維定位中， 若要利用Chan算法進(jìn)行定位， 則基站數(shù)目必須大于等于4個， 也即列出的方程粗個數(shù)必須大于未知數(shù)個數(shù). 如此才能充分利用Chan算法的定位優(yōu)勢[20]. 但Chan算法在非視距傳輸及信道性能較差的情況下, 定位精度會下降[21]， 因此傳統(tǒng)的定位方法會在Chan算法之后使用Taylor級數(shù)展開法[22]進(jìn)行一次次遞歸來逐步比較真實值.

在本文系統(tǒng)中， 將時延信息經(jīng)自適應(yīng)卡爾曼濾波處理后， 使用Chan算法進(jìn)行定位， 并將其與傳統(tǒng)的定位方法進(jìn)行對比.

2.5　定位服務(wù)器

定位服務(wù)器使用了4核8線程的英特爾i7-4790K處理器， 16 GB內(nèi)存. 所使用的定位程序在Visual Studio2015的平臺上開發(fā)， 使用了windows窗體控件及C#語言編寫， 運行界面如圖 4 所示. 定位開始前系統(tǒng)需要先輸入基站坐標(biāo)， 其中左下方顯示標(biāo)簽坐標(biāo)信息， 右下方可以顯示運動軌跡， 可以直觀地展示標(biāo)簽的位置信息.

圖 4　上位機界面Fig.4　The GUI of application system

3　系統(tǒng)測試與分析

3.1　實驗設(shè)計

本實驗在8 m×7 m大小的空曠室內(nèi)場地中進(jìn)行， 使用了4個定位基站， 放置在4個角落. 實驗中將采用自適應(yīng)卡爾曼濾波的改進(jìn)定位方法與使用了Chan算法配合Taylor級數(shù)展開法的傳統(tǒng)定位方法相比較， 分別測試了標(biāo)簽在固定位置時和在運動狀態(tài)下不同定位方法的定位精度. 在自適應(yīng)卡爾曼濾波算法的初始值設(shè)置中， 為了測試參數(shù)的校準(zhǔn)能力， 將過程誤差矩陣Qk、 測量誤差矩陣Rk和誤差協(xié)方差矩陣Pk的矩陣系數(shù)都設(shè)置為10. 狀態(tài)變換矩陣Fx為[1,1;0,1]， 狀態(tài)轉(zhuǎn)移矩陣Hk為[1,0;0,0].

3.2　測試結(jié)果

3.2.1固定標(biāo)簽定位

實驗測定了定位標(biāo)簽在場地中4個位置的定位信息， 定位結(jié)果為： 其中的平均位置偏差為測出的目標(biāo)節(jié)點的位置與其實際位置的歐氏距離偏差, 每個位置分別測量了15次. 在標(biāo)簽靜止時， 使用自適應(yīng)卡爾曼濾波后的定位結(jié)果與傳統(tǒng)定位方法的定位結(jié)果相比， 定位精度無太大變化， 但均方根誤差有了很大的提高. 說明與傳統(tǒng)定位方法相比， 使用自適應(yīng)卡爾曼濾波算法優(yōu)化后， 定位結(jié)果更為穩(wěn)定.

表 1　兩種定位算法下的固定標(biāo)簽定位精度比較

3.2.2移動標(biāo)簽定位

本文同時測試了標(biāo)簽在移動狀態(tài)下的定位結(jié)果. 其結(jié)果如圖 5 所示， 其中粗線代表真實路徑， 細(xì)線為傳統(tǒng)的Chan算法與Taylor算法結(jié)合后測出的目標(biāo)節(jié)點的位置后形成的路徑， 點劃線表示定位基站測得的TDOA值經(jīng)自適應(yīng)卡爾曼濾波處理后再由Chan算法測出的目標(biāo)節(jié)點的位置后形成的路徑.

圖 5　定位系統(tǒng)軌跡圖(m)Fig.5　The track of positioning system(m)

將采樣點坐標(biāo)與行走軌跡曲線之間的距離定義為測量偏差， 對比分析每一條行走路徑的定位偏差， 結(jié)果如表 2 所示. 其中的平均位置偏差為測出的目標(biāo)節(jié)點的位置與其實際位置的歐氏距離偏差.

表 2　兩種定位算法下的移動標(biāo)簽定位精度比較

由此可以看出， 標(biāo)簽運動時使用了自適應(yīng)卡爾曼濾波后的定位結(jié)果比傳統(tǒng)的定位方法精度更高， 且提高了定位的穩(wěn)定性.

4　結(jié)　論

本文采用了自適應(yīng)卡爾曼濾波和 Chan算法結(jié)合的定位算法. 利用自適應(yīng)卡爾曼濾波的參數(shù)自我調(diào)整的能力， 減少了環(huán)境變化和系統(tǒng)偏差所帶來的定位誤差， 極大程度地提高了定位性能. 實驗中利用射頻芯片 DW1000 作為脈沖超寬帶技術(shù)的硬件基礎(chǔ)搭建定位系統(tǒng)， 并在空曠環(huán)境下進(jìn)行定位試驗. 試驗結(jié)果表明： 采用自適應(yīng)卡爾曼濾波后的定位方法相比于傳統(tǒng)的定位方法， 具有更高的定位精度和更好的穩(wěn)定性， 在動態(tài)定位中平均偏差可以達(dá)到10 cm的誤差， 而在靜態(tài)定位中則可以達(dá)到5 cm以內(nèi).

參考文獻(xiàn)：

[1]劉德亮. 室內(nèi)環(huán)境下無線定位關(guān)鍵技術(shù)研究[D]. 天津： 天津大學(xué), 2015.

[2]賀晶晶， 姜平， 馮曉榮. 基于UWB的無人運輸車的導(dǎo)航定位算法研究[J]. 電子測量與儀器學(xué)報， 2016(11)： 1743-1749.

He Jingjing, Jiang Ping, Feng Xiaorong. Research on navigation and positioning algorithm for unmanned vehicle based on UWB[J]. Journal of Electronic Measurement and Instrumentation, 2016, 30(11)： 1743-1749. (in Chinese)

[3]Gezici S, Tian Z, Giannakis G B, et al. Localization via ultra-wideband radios： a look at positioning aspects for future sensor networks[J].IEEE signal processing magazine, 2005, 22(4)： 70-84.

[4]Shang F, Champagne B, Psaromiligkos I. Joint TOA/AOA estimation of IR-UWB signals in the presence of multiuser interference[Z]. 2014.

[5]Shang F, Champagne B, Psaromiligkos I N. A ML-Based Framework for Joint TOA/AOA Estimation of UWB Pulses in Dense Multipath Environments[J]. IEEE Transactions on Wireless Communications, 2014, 13(10)： 5305-5318.

[6]Hanssens B, Plets D, Tanghe E, et al. An indoor localization technique based on ultra-wideband AoD/AoA/ToA estimation[Z]. 2016.

[7]馬麗， 張蕊萍， 杜國璋. 基于 TDOA 的列車無線定位方法研究[J]. 鐵道標(biāo)準(zhǔn)設(shè)計， 2016, 60(7)： 154-158.

Ma li, Zhang Ruiping, Du Guozhang. Study on TDOA-based wireless train positioning method[J]. Railway Standard Design, 2016, 60(7)： 154-158. (in Chinese)

[8]仲江濤. 基于UWB室內(nèi)定位算法的研究與實現(xiàn)[D]. 深圳： 深圳大學(xué), 2017.

[9]倪磊磊， 楊露菁， 蔡時超， 等. 基于 TDOA 的 Chan 定位算法仿真研究[J]. 艦船電子工程， 2016, 36(5)： 92-95.

Ni Leilei, Yang Lujing, Cai Shichao, et al. Simulation of Chan positioning algorithm based on TDOA[J]. Ship Electronic Engineering, 36(5)： 92-95. (in Chinese)

[10]Huan L, Bo R. Wireless location for indoor based on UWB[Z]. IEEE, 2015.

[11]Li X, Yang S. The indoor real-time 3D localization algorithm using UWB[Z]. IEEE, 2015.

[12]Xu Y, Shmaliy Y S, Li Y, et al. UWB-based indoor human localization with time-delayed data using EFIR filtering[J]. IEEE Access, 2017(5)： 16676-16683.

[13]王長強， 徐愛功， 隋心. UWB測距的NLOS誤差削弱方法[J]. 導(dǎo)航定位學(xué)報， 2017(3)： 24-27.

Wang Changqiang, Xu Aigong, Sui Xin. A method of NLOS error inhibition for UWB ranging[J]. Journal of Navigation and Positioning, 2017, 5(3)： 24-27. (in Chinese)

[14]Welch G, Bishop G. An introduction to the Kalman filter[EB/OL]. Dept.Comput.Sci.,Univ. North Carolina, Chapel Hill, Tech. Rep. TR95041, 2006、.

[15]Khan M, Kai Y D, Gul H U. Indoor Wi-Fi positioning algorithm based on combination of Location Fingerprint and Unscented Kalman Filter[Z]. 2017.

[16]孫小文， 張小超， 趙博， 等. 農(nóng)資倉儲脈沖超寬帶室內(nèi)定位系統(tǒng)設(shè)計與試驗及誤差分析[J]. 農(nóng)業(yè)工程學(xué)報， 2017(S1)： 23-29.

Sun Xiaowen, Zhang Xiaochao, Zhao Bo, et al. Design, experiment and error analysis of impulse radio-ultra wide band indoor positioning system used in agricultural warehousing[J]. Transactions of the Chinese Society of Agricultural Engineering, 2017(S1)： 23-29. (in Chinese)

[17]Djaja-Josko V, Kolakowski J. A new method for wireless synchronization and TDOA error reduction in UWB positioning system[Z]. 2016.

[18]江鳳， 吳飛， 王昌志. 基于CHAN與粒子群算法的協(xié)同定位研究[J]. 電子科技. 2017(08)： 1-5.

Jiang Feng, Wu Fei, Wang Changzhi. Research on collaborative localization based on CHAN and particle swarm optimization algorithm[J]. Electronic Science and Technology, 2017(08)： 1-5. (in Chinese)

[19]Liu J, Wu Z, Yin Z. An improved location algorithm in Wireless Sensor Network[Z]. 2013.

[20]朱彩杰. 基于UWB室內(nèi)定位跟蹤算法的對比分析[C]. 中國衛(wèi)星導(dǎo)航學(xué)術(shù)年會， 上海： 2017.

[21]趙宏旭， 楊文帥. 基于TDOA的Chan算法和Taylor算法的分析與比較[J]. 電子世界， 2017(9)： 176-177.

Zhao Hongxu, Yang Wenshuai. Analysis and comparison of Chan algorithm and Taylor algorithm based on TDOA[J]. Electronics World, 2017(9)： 176-177. (in Chinese)

[22]王磊， 李鵬濤， 賈宗璞. 基于全質(zhì)心-Taylor的UWB室內(nèi)定位算法[J]. 傳感器與微系統(tǒng)， 2017(6)： 146-149.

Wang Lei, Li Pengtao, Jia Zongpu. UWB indoor localization algorithm based on full centroid and Taylor[J]. Transducer and Microsystem Technologies, 2017(6)： 146-149. (in Chinese)