一種UWB與PDR融合的行人室內(nèi)定位方法

儀玉杰，黃智剛，蘇 雨

一種UWB與PDR融合的行人室內(nèi)定位方法

儀玉杰，黃智剛，蘇 雨

（北京航空航天大學(xué) 電子信息工程學(xué)院，北京 100191）

針對(duì)在復(fù)雜室內(nèi)環(huán)境下，單純依靠超寬帶技術(shù)（UWB）定位結(jié)果會(huì)嚴(yán)重失真，甚至缺失的問(wèn)題，提出一種將UWB與行人航跡推算方法（PDR）相結(jié)合的方法：基于在人體胸部或者腰部等位置佩戴慣性器件的通常情形，利用慣性器件數(shù)據(jù)估計(jì)行人步長(zhǎng)與每步航向，建立UWB與PDR的非線性擴(kuò)展卡爾曼濾波器；然后通過(guò)將UWB相鄰時(shí)刻間的差值信息更新到卡爾曼濾波器中的觀測(cè)方差陣中，來(lái)自適應(yīng)地實(shí)現(xiàn)定位結(jié)果估計(jì)。實(shí)驗(yàn)結(jié)果表明，該算法與單純依靠UWB技術(shù)進(jìn)行定位的算法相比，能夠解決在復(fù)雜室內(nèi)環(huán)境下定位解算點(diǎn)可能缺失的問(wèn)題，并且可較顯著地提高在復(fù)雜環(huán)境下定位系統(tǒng)的魯棒性和定位精度。

室內(nèi)定位；超寬帶；行人航跡推算；擴(kuò)展卡爾曼濾波器；自適應(yīng)觀測(cè)方差陣

0 引言

隨著手機(jī)等智能終端的普及，個(gè)體獲取定位導(dǎo)航服務(wù)的便利性大大增加，同時(shí)也對(duì)基于位置服務(wù)各方面的要求不斷提高。戶外的定位導(dǎo)航服務(wù)已經(jīng)非常普及，全球定位系統(tǒng)（global position system, GPS）的定位精度已經(jīng)能夠達(dá)到米級(jí)別。但是由于GPS信號(hào)在“城市峽谷”之間會(huì)發(fā)生嚴(yán)重失真，導(dǎo)致定位精度嚴(yán)重降低。因此在建筑樓體林立的定位環(huán)境內(nèi)，需要尋求其他技術(shù)手段來(lái)實(shí)現(xiàn)高樓之間或樓體內(nèi)部的定位服務(wù)。目前，存在的室內(nèi)定位技術(shù)有紅外線、藍(lán)牙、慣性導(dǎo)航、無(wú)線保真（wireless fidelity, WiFi）、超寬帶技術(shù)（ultra wide band, UWB）技術(shù)等。UWB由于其具有定位精度高、功耗低、有一定信號(hào)穿透性等優(yōu)點(diǎn)而脫穎而出，已成為一個(gè)越來(lái)越被關(guān)注的技術(shù)領(lǐng)域。

任何無(wú)線電技術(shù)不可避免地都有缺陷，UWB技術(shù)也不例外。當(dāng)應(yīng)用環(huán)境比較復(fù)雜，有較大障礙物時(shí)，UWB測(cè)距信號(hào)的到達(dá)時(shí)間信息會(huì)發(fā)生嚴(yán)重失真，甚至當(dāng)接收到的獨(dú)立信息量少于定位解算所需要的數(shù)量時(shí)，不會(huì)有定位解算點(diǎn)，因而會(huì)造成定位點(diǎn)缺失。解決這個(gè)問(wèn)題的一般想法是將慣性導(dǎo)航技術(shù)與UWB技術(shù)相結(jié)合。慣性導(dǎo)航具有數(shù)據(jù)輸出穩(wěn)定連續(xù)、不依賴外部環(huán)境的優(yōu)點(diǎn)，這些優(yōu)點(diǎn)剛好可以與UWB的缺陷形成互補(bǔ)。目前參與融合的慣性導(dǎo)航方法中分為捷聯(lián)慣性導(dǎo)航和行人航跡推算算法（pedestrian dead reckoning, PDR）。2種方法各有特點(diǎn)，應(yīng)用于不同的場(chǎng)景。

在慣性導(dǎo)航技術(shù)與超寬帶技術(shù)相結(jié)合的方法中，一般的方法是將捷聯(lián)慣導(dǎo)算法技術(shù)與超寬帶技術(shù)相結(jié)合。文獻(xiàn)[1-5]就是利用捷聯(lián)慣導(dǎo)方程作為卡爾曼方程的狀態(tài)方程進(jìn)行數(shù)據(jù)處理；但是這種方法需要處理的數(shù)據(jù)量比較大，并且需要將測(cè)試標(biāo)簽放置在人體腳部，利用人走路時(shí)的短暫靜止來(lái)進(jìn)行零速修正，對(duì)于定位解算在平臺(tái)端完成的情況，需要傳輸?shù)男畔⒘看?，并且不易?shí)現(xiàn)到某些需要將標(biāo)簽佩戴到非足部位置的情況中。文獻(xiàn)[6-7]中從分別佩戴在頭部的UWB器件和佩戴在腳部的慣性測(cè)量單元(inertial measurement unit, IMU)器件之間的幾何角度提出一個(gè)距離上界來(lái)實(shí)現(xiàn)限制濾波器。文獻(xiàn)[8]中也是將UWB標(biāo)簽與慣導(dǎo)器件分離，慣導(dǎo)使用零速更新方法提供速度誤差觀測(cè)量，UWB提供位置誤差觀測(cè)量實(shí)現(xiàn)擴(kuò)展卡爾曼數(shù)據(jù)融合。文獻(xiàn)[9]則是基于UWB與PDR二者求解出的解算結(jié)果進(jìn)行松組合。文獻(xiàn)[10]中用于組合定位的方法是GPS和PDR，通過(guò)提出基于加速度和航向的PDR算法、基于機(jī)電信號(hào)和航向的PDR算法進(jìn)行融合。

上述文獻(xiàn)中所述方法對(duì)于慣導(dǎo)器件在人體的佩戴位置有極強(qiáng)的限制性，必須配置在測(cè)試人員足部。對(duì)于一些應(yīng)用場(chǎng)景，將標(biāo)簽佩戴在腳部位置時(shí)不科學(xué)且不方便；所以本文基于測(cè)試人員可以將標(biāo)簽配置在腰部、胸部等主體軀干位置的使用場(chǎng)景下，提出一種新型的利用PDR與UWB技術(shù)進(jìn)行融合的方法：利用PDR推算方程作為狀態(tài)方程，從慣性數(shù)據(jù)中估計(jì)行人步長(zhǎng)和航向參與運(yùn)算；利用UWB的測(cè)距定位方程作為觀測(cè)方程，建立擴(kuò)展卡爾曼濾波方程。

1 組合定位系統(tǒng)的整體框架

圖1所示為基于UWB和PDR技術(shù)融合定位系統(tǒng)的整體框架圖。PDR部分，通過(guò)MEMS器件獲得相應(yīng)的加速度和陀螺儀原始數(shù)據(jù)，基于這2個(gè)部分慣導(dǎo)器件數(shù)據(jù)估計(jì)出行人的步長(zhǎng)和航向，然后將步長(zhǎng)和航向值輸入到PDR推算模型中，也就是卡爾曼濾波器中的狀態(tài)方程；超寬帶部分，通過(guò)雙向測(cè)距獲得所有基站與標(biāo)簽的測(cè)距時(shí)間信息，這些信息作為觀測(cè)量輸入到卡爾曼濾波器中；同時(shí)將相鄰時(shí)刻測(cè)距時(shí)間的差值信息更新到觀測(cè)方差陣中，來(lái)自適應(yīng)地表征UWB測(cè)距信息的好壞；最后通過(guò)非線性卡爾曼濾波器完成定位解算輸出。

圖1 UWB與PDR融合定位系統(tǒng)框圖

2 基于UWB和PDR的卡爾曼模型

2.1 UWB定位系統(tǒng)結(jié)構(gòu)和方程

UWB定位方程組成非線性卡爾曼模型中的觀測(cè)方程，由于原始的UWB定位方程為非線性的，對(duì)其作線性化處理，轉(zhuǎn)化為

2.2 PDR定位方法

行人航跡推算利用慣性傳感器的原始數(shù)據(jù)來(lái)估計(jì)行人的步長(zhǎng)和每一步的方向，從而達(dá)到對(duì)人員進(jìn)行定位跟蹤的目的。PDR算法一般包括3個(gè)部分：步態(tài)檢測(cè)、步長(zhǎng)估計(jì)和航向計(jì)算。行人在行走過(guò)程中，由于左右腳一前一后，處于加速和減速的循環(huán)狀態(tài)，因而行人加速度呈現(xiàn)正負(fù)值周期性交替變化。步態(tài)檢測(cè)就是根據(jù)這種規(guī)律來(lái)判別行人的步行狀態(tài)。而步長(zhǎng)估計(jì)和航向的計(jì)算都是基于步態(tài)檢測(cè)，在檢測(cè)好的每一步中對(duì)其時(shí)間范圍內(nèi)的加速度和陀螺儀的數(shù)據(jù)進(jìn)行處理。其中，步態(tài)檢測(cè)和步長(zhǎng)估計(jì)都是基于三軸加速度計(jì)的原始數(shù)據(jù)來(lái)處理，而方向計(jì)算則依賴于陀螺儀的輸出。步長(zhǎng)參數(shù)和方向參數(shù)被估計(jì)出來(lái)后，就可以利用PDR進(jìn)行坐標(biāo)計(jì)算。PDR的坐標(biāo)推算模型為

2.2.1 步態(tài)檢測(cè)

文中步態(tài)檢測(cè)方法直接從慣性器件中的加速度數(shù)據(jù)來(lái)判別，從時(shí)間和加速度值2個(gè)維度來(lái)考慮步態(tài)是否變化。圖2所示為步態(tài)檢測(cè)方法的流程圖。

圖2 步態(tài)檢測(cè)處理流程

按照數(shù)據(jù)處理先后順序，檢測(cè)流程分為以下3個(gè)部分：

圖3 行人行走加速度數(shù)據(jù)曲線

2.2.2 步長(zhǎng)估計(jì)

對(duì)于步長(zhǎng)估計(jì)，選取式（6）所示的非線性步長(zhǎng)估計(jì)模型，即

該模型被許多文獻(xiàn)所采用，主要是因?yàn)槠渲恍枰粋€(gè)參數(shù)，比較容易獲得統(tǒng)計(jì)特征值，使得算法簡(jiǎn)單易實(shí)現(xiàn)。

2.2.3 航向計(jì)算

PDR中的航向計(jì)算則依賴于航向推算模型，即

2.3 基于UWB和PDR的卡爾曼模型

經(jīng)過(guò)線性化處理的觀測(cè)方程為

3 實(shí)驗(yàn)與結(jié)果分析

獲得UWB測(cè)距數(shù)據(jù)和MEMS芯片輸出的加速度數(shù)據(jù)和角速度數(shù)據(jù)后，利用文中提出的融合算法對(duì)數(shù)據(jù)進(jìn)行處理實(shí)現(xiàn)定位解算，定位結(jié)果如圖5所示。

圖4 測(cè)試環(huán)境示意

圖5 不同軌跡測(cè)試下的真實(shí)軌跡與估計(jì)軌跡

從圖5中可以看到，跟預(yù)期的情況一樣，單純使用UWB信息的解算軌跡雖然有時(shí)與真實(shí)軌跡貼合比較好，但是某些解算點(diǎn)跳躍跨度大，還有一部分定位點(diǎn)缺失的情況出現(xiàn)。由圖6的誤差曲線可以看出基于UWB和PDR 2種信息源經(jīng)過(guò)卡爾曼處理的估計(jì)曲線能夠在所有情況下保持較高的精度，定位結(jié)算點(diǎn)跳躍幅度明顯減小，完全消除了定位點(diǎn)缺失的情況發(fā)生，增強(qiáng)了定位系統(tǒng)的魯棒性。

圖6 不同軌跡測(cè)試下的真實(shí)軌跡與估計(jì)軌跡誤差曲線

從表1和表2中可以看到，在2個(gè)軌跡測(cè)試中，EKF算法完全消除了定位缺失點(diǎn)的問(wèn)題，平均定位誤差比單純使用UWB的誤差要小，衡量定位誤差穩(wěn)定性的標(biāo)準(zhǔn)差指標(biāo)也比單純使用UWB的指標(biāo)值小。

表1 矩形軌跡下UWB定位算法與EKF定位算法測(cè)試數(shù)據(jù)比較

表2 “8”字形軌跡UWB定位算法與EKF定位算法測(cè)試數(shù)據(jù)比較

4 結(jié)束語(yǔ)

文中提出了一種在新的應(yīng)用場(chǎng)景和測(cè)試場(chǎng)景下，即慣性器件佩戴在胸部或者腰部，而不是腳部的條件下，將UWB技術(shù)與PDR技術(shù)進(jìn)行組合定位的擴(kuò)展卡爾曼濾波器模型。模型中將UWB相鄰時(shí)刻的原始數(shù)據(jù)差值信息更新到卡爾曼方程中的觀測(cè)方差陣中來(lái)自適應(yīng)地改變UWB信息與PDR信息量在定位結(jié)果輸出中的比重。經(jīng)過(guò)實(shí)際場(chǎng)景的實(shí)驗(yàn)測(cè)試，可以看出組合定位方法相比于只使用UWB技術(shù)的定位方法，能夠在定位精度和魯棒性方面得到明顯改善，對(duì)于室內(nèi)定位方向的研究有一定參考價(jià)值。

[1] FOXLIN E. Pedestrian tracking with shoe-mounted inertial sensors[J]. IEEE Computer Graphics and Applications, 2005, 25(6): 3-6.

[2] YAO L, WU Y W A, YAO Lei, et al.An integrated IMU and UWB sensor based indoor positioning system[C]//The Institute of Electrical and Electronic Engineers（IEEE）.Proceedings of the 2017 International Conference on Indoor Positioning and Indoor Navigation.Sapporo, Japan：IEEE，2017:1-8.

[3] CORRALES J A, CANDELAS F A, TORRES F.Hybrid tracking of human operators using IMU/UWB data fusion by a Kalman filter[C]//The Institute of Electrical and Electronic Engineers(IEEE).Proceedings of the 20083rd ACM/IEEE International Conference on Human-Robot Interaction (HRI). Amsterdam: 2008:3-6.DOI: 10.1145/ 1349822.1349848.

[4] MARQUES A,TANK B, MEGHANI S K.Accurate UWB and IMU based indoor localization for autonomous robots[C]//The Institute of Electrical and Electronic Engineers（IEEE）.Proceedings of the2017 IEEE 30th Canadian Conference on Electrical and Computer Engineering (CCECE).Windsor,Canada：IEEE,2017:2-4.DOI: 10.1109/ CCECE.2017.7946751.

[5] 汪少初. 基于PDR的定位與跟蹤技術(shù)研究及系統(tǒng)設(shè)計(jì)[D]. 天津: 天津大學(xué). 2014: 91-109.

[6] ZAMPELLA F, ANGELIS A D, SKOG I, et al.A constraint approach for UWB and PDR fusion[C]//The Institute of Electrical and Electronic Engineers(IEEE).Proceedings of the 2012 International Conference on Indoor Positioning and Indoor Navigation(IPIN).Sydney, Australia:IEEE, 2012: 2-7.

[7] CHEN Z, MU R, CHEN C H.Implement of cooperative navigation in pedestrian navigation based on distance restraint[C]//The Institute of Electrical and Electronic Engineers(IEEE). Proceedings of the 2016 IEEE Chinese Guidance, Navigation and Control Conference(CGNCC). Nanjing,China:IEEE,2016: 1-4.

[8] 孫璧文,樊啟高,武亞恒,等.基于PDR/UWB緊耦合的足綁式行人導(dǎo)航技術(shù)[J].傳感器與微系統(tǒng). 2017,36(3): 2-4.

[9] SCZYSLO S, SCHROEDER J, GALLER S, et al.Hybrid localization using UWB and inertial sensors[C]//The Institute of Electrical and Electronic Engineers(IEEE).Proceedings of the2008 IEEE International Conference on Ultra-Wideband.Hannover, Germany: IEEE,2008:7-15. DOI: 10.1109/ICUWB.2008.4653423.

[10] 陳偉.基于GPS和自包含傳感器的行人室內(nèi)外無(wú)縫定位算法研究[D].北京：中國(guó)科學(xué)技術(shù)大學(xué), 2010: 36-54.

A method of pedestrian indoor positioning combined UWB with PDR

YI Yujie, HUANG Zhigang, SU Yu

（School of Electronic and Information Engineering, Beijing University of Aeronautics and Astronautics, Beijing 100191, China）

Aiming at the problem that it is susceptible to serious distortion and even loss for the positioning result by relying solely on UWB technology under complex indoor environments, the paper proposed a method combing UWB with PDR: based on the general case that the inertial device is usually placed on the chest or waist of human’s body, the step length and orientation of every step of the pedestrian were estimated by using data of the inertial equipment to establish the nonlinear extended Kalman filter (EKF) of UWB and PDR; and the positioning estimates were realized adaptively by updating the difference of adjacent time of UWB into the covariance matrix of the measurement equation of EKF. Experimental result showed that, compared with the positioning method solely relying on UWB, the propsoed method could avoid the loss of calculated positioning points under complex indoor environments, and efficiently improve the accuracy and robustness of the positioning system in a complex environment.

indoor positioning; ultra wide band；pedestrian dead reckoning; extended Kalman filter; adaptive measuring covariance matrix

P228

A

2095-4999(2019)03-0038-06

2018-11-02

儀玉杰（1992—），男，山西運(yùn)城人，碩士生，研究方向?yàn)槭覂?nèi)定位與導(dǎo)航。

儀玉杰，黃智剛，蘇雨.一種UWB與PDR融合的行人室內(nèi)定位方法[J].導(dǎo)航定位學(xué)報(bào),2019,7(3):38-43.（YI Yujie,HUANG Zhigang,SU Yu.A method of pedestrian indoor positioning combined UWB with PDR[J].Journal of Navigation and Positioning,2019,7(3):38-43.）

10.16547/j.cnki.10-1096.20190307.