基于UWB优化配置的室内行人导航方法

曾庆化，王敬贤，孟 骞，熊 智，钱伟行

（1. 南京航空航天大学 导航研究中心，南京 210016；2. 卫星通信与导航协同创新中心，南京 210016；

3. 南京师范大学 电气与自动化工程学院，南京 210094）

基于UWB优化配置的室内行人导航方法

曾庆化1,2，王敬贤1,2，孟 骞1,2，熊 智1,2，钱伟行3

（1. 南京航空航天大学 导航研究中心，南京 210016；2. 卫星通信与导航协同创新中心，南京 210016；

3. 南京师范大学 电气与自动化工程学院，南京 210094）

在卫星信号无法覆盖的室内条件下，行人的精确导航问题是当前研究的热点。为解决基于MEMS的微惯性导航系统误差随时间发散、航向角发散快的问题，提出了一种利用UWB辅助修正惯性导航系统，实现室内较高精度定位的方法。该方法采用基于零速修正的微惯导系统进行导航，以抑制惯导误差随时间发散，并在建筑内拐角、楼梯口等关键节点处优化配置UWB设备，采用UWB信息与微惯导数据进行卡尔曼滤波，实现对微惯导航向及位置的修正。与大规模使用UWB系统进行室内定位相比，该方法降低了系统布设成本，避免了UWB出现问题时对整体导航结果的影响，有效地保证了系统的定位精度。行走实验表明：直线行走时，微惯导最终定位误差为1.8%；转弯行走时，在UWB辅助定位下，微惯导最终定位误差小于1.0%。

室内定位；惯性导航系统；超宽带无线技术；组合导航

基于卫星定位的行人导航系统已广泛应用于定位领域[1]，由于卫星信号容易被建筑物阻隔，产生多路径、非视距、时变性等问题，因此卫星导航接收机很难实现室内的高精度定位应用。美国Draper实验室于20世纪末提出将惯性测量器件安装在人体的足部实现人体定位功能，其中应用零速修正技术较好地解决了低成本惯导系统误差发散快的问题。国内外学者对该算法进行了大量的改进研究[2-4]。

由于室内航向角很难精确获得，因此行人惯性导航系统中的航向角误差已成为惯性导航系统误差的主要来源之一[5-6]。当前解决航向角发散问题的方法主要有磁航向辅助算法[7]、陀螺旋转修正算法[8]等。磁航向辅助算法主要运用于室外，室内磁场环境复杂，很难应用该算法；陀螺旋转修正算法多用于机器，行人无法满足算法所需的陀螺匀速旋转的条件。对室内行人惯性导航系统航向进行修正的重要性日益突出。

UWB（超宽带无线技术）是当前国内外定位领域研究热点之一[9-10]。在室内定位过程中，UWB具有厘米级的定位精度，良好的抗多路径性能，较低的发射功率及一定的穿透能力，与WiFi、ZigBee、RFID等室内定位技术相比优势显著。目前，国内外研究者对惯导/UWB组合的研究主要集中在UWB与IMU组合实现无人机的室内定位[11]，UWB与PDR/SLU （Pedestrian dead reckoning /Step length update）组合实现行人导航[12-13]。PDR/SLU对行人的步长、步数精度要求较高，而基于足部微惯性系统的零速修正算法则不需获知行人的精确步长，因此本文选择UWB与使用零速修正的惯导系统组合，以UWB修正惯导系统的航向与位置，同时在UWB受干扰定位出现偏差时，利用惯导保证定位的准确性实现低成本高精度的室内导航。

1 惯性导航系统定位方法

本文选用低成本的微惯性传感器是消费级别的，因此传统捷联惯性导航算法无法使之满足行人惯导系统的需要。在足部运动时，足部的惯性传感器数据使用捷联惯导算法递推得到导航结果；在足部静止时，采用零速修正算法将速度置零，并重新计算姿态角以修正导航结果、抑制误差发散。具体流程如图1所示。

图1 基于零速修正算法的行人惯性导航算法原理图Fig. 1 Pedestrian navigation algorithm principle based on zero-velocity update algorithm

准确判断足部的零速时刻是零速修正算法最重要的条件。由图2可以看出，足部处于零速时刻时，惯性器件有一段较为明显的平稳输出区间。本文采取了以陀螺仪输出为主，加速度计输出为辅的滑动窗口判断方法，对足部的零速时刻进行判定。基于不同的行走状态将滑动窗口宽度设置为变量N，该值的大小与行走状态有关，相关变量定义如式(1)所示：

图2 行人行走时惯性器件输出数据Fig.2 The output data of inertial device for pedestrian

为了避免零速判定的条件过于严格导致惯导误差的积累，本文适度放宽零速判定的条件，当上述4个变量有一个满足所给定的限定门限值时，就认为处于零速时刻，并对惯性器件进行零速修正。当惯性器件处于零速修正时刻，利用式(2)可以使用加速度计输出信息求出该器件的横滚角与俯仰角。

低成本惯性器件稳定性、重复性都比较差，每次使用前均需要对其进行标定，以扣除陀螺仪和加速度计的零偏误差，提高传感器的精度。陀螺仪取静止时刻的输出作为零偏，但加速度计无法仅由某一位置的输出求出零偏，因此本文使用一种三位置加速度计标定法，通过传感器在三个不同位置的静态数据，求出加速度计的零偏值。该方法简单实用，是一种适用于安装完成后的现场标定方法，其算法如式(3)所示：

惯性导航系统通过惯性器件获得加速度计与陀螺仪的测量信息，去除零偏后解算出自身的位置与姿态，同时在足部静止时刻引入零速修正以抑制误差的发散，从而保证一定时间内的精确导航。

2 UWB定位方法

常用的无线定位算法有以下四种：测量信号的到达时间（TOA，Time of Arrival）；信号的到达时间差（TDOA，Difference of Time Arrival）；信号的到达角度（AOA，Angel of Arrival）；接收信号的强度（RSSI，Received Signal Strength）。由于UWB技术支持使用纳秒级的同步时钟，因此能够确保时间的准确性，故本文中选用 TOA方法：精确测量定位芯片发出的无线脉冲信号到达各基站所经过的时间，乘以光速即获得芯片与各基站之间的精确距离，从而解算出较高精度的芯片位置[14]。

图3 TOA定位方法的几何模型Fig.3 Geometric model of TOA location method

然而在大部分情况下，由于1r、r2、r3的测量值存在误差，三球并不会正好相交于一点，因此需要N个（3个以上）基站才能完成定位，如式(4)所示：

式(4)通过平方和求差可得式(5)：

N个定位基站可得N-1个类似式(5)的方程，写成如式(6)所示的矩阵形式：

其中，

根据基站的数据，获得M和b的值，带入式(6)，利用最小二乘法，即可求出定位芯片MS的坐标I。

3 惯导/UWB组合定位方法

3.1 惯导/UWB组合定位原理

基于零速修正的惯性导航技术通过自身加速度计与陀螺仪测得的信息计算出行人的位置和航向，同时在足部静止时刻引入零速修正算法，以修正导航结果。虽然零速修正算法在一定程度上能抑制误差的发散，保证一定时间的独立工作能力，但是误差随着行人走动仍会不断累积，最后无法满足导航的需求。同时在行人行进时航向角计算会出现偏差，若不对其进行及时修正，会对后续定位造成很大的影响。

基于TOA方法的UWB技术采用测量无线脉冲信号准确的飞行时间，从而解得厘米级的精确位置，可以为行人提供准确的位置与航向。但是UWB信号易被人体所吸收，尤其在对快速行走的人体进行定位时，这种影响所带来的误差将达到分米级[15]。另外 UWB基站成本相对微惯性器件来说较高，很多室内导航系统的预算无法支持UWB的全面覆盖。

惯导/UWB组合定位相辅相成，可以实现定位精度与成本之间的平衡。在直线行走时，采用惯性传感器自主导航，在楼道关键节点处（如拐角）布设UWB基站予以辅助定位，既修正了惯性传感器的航向，同时又消除了惯导之前行走过程所积累的位置误差，还避免了建筑内大规模布设UWB基站的高昂成本，在满足定位需求的同时降低了成本的投入。

本文采用的是惯性/UWB的松组合导航系统，具体原理如图4所示，组合过程分为以下三步：

1）实时由惯性导航系统进行导航解算，同时依靠零速修正算法对其误差进行修正。

2）UWB模块尝试接收UWB信号，当能获取到可用的UWB定位信息时，转到步骤3）。

图4 惯导/UWB组合定位原理图Fig.4 INS/UWB integrated positioning principle

3）使用UWB定位信息与惯导解算结果进行卡尔曼滤波，对惯导解算数据进行修正，返回步骤 1）继续进行惯导解算。

3.2 卡尔曼滤波原理

基于对数据融合的有效性与算法的总体计算量的考虑，本文选择仅在零速修正时刻进行基本线性卡尔曼滤波，其状态方程与观测方程为

式中：

卡尔曼滤波的状态量如下所示：

卡尔曼滤波的量测量为

由于惯导传感器采样周期为200 Hz，UWB采样周期为 20 Hz，因此本文所选量测量并非时刻存在，故仅在可以获得有效量测量的零速时刻进行系统状态更新和量测更新，其余时刻滤波器仅作状态更新。经过滤波器量测更新得到的导航误差估计量被返回至惯导解算环节，惯导系统输出修正后的导航结果。

4 实验验证与数据分析

为了对本文提出的UWB辅助惯导定位方法进行验证，项目组在南京航空航天大学自动化学院楼群进行了室内行走实验。本次实验将低成本微惯性器件MPU-6050置于足部，器件输出频率设为200 Hz，以测量行人行走过程中的足部运动信息，实现零速修正下的惯性导航，同时将UWB标签置于上半身（本次实验选择肩部），避免下半身行走时抖动过大对UWB测量精度的影响。具体设备如图5所示。

图5 惯性传感器与UWB标签、基站示意图Fig.5 Schematic of inertial sensor, UWB tag and UWB base station

本次实验分为两部分进行：第一部分实验验证惯导在长距离直线行走中的准确性；第二部分实验验证UWB辅助修正惯导的效果。

4.1 纯惯导长距离直线行走实验

本部分实验在4号楼内一条贯穿全楼的直线过道中行走，由激光测距仪测得行走距离长52.081 m，行走效果如图6所示。

图6 长距离直线行走实验结果Fig.6 Experiment result of long distance walking straight

图6中直线前后的红点所示理论起始点为（0, 0），理论终点为（52.081, 0）。实际行走结果由蓝色曲线表示，起始点为（0, 0），终点为（52.912, -0.437）。由数据可知，起点-终点位置差为 0.939 m，位置误差占行走距离的1.8%。可见，本文所采用的基于零速修正的惯性导航算法，在直线行走时可以基本满足室内行人定位的需求。

4.2 UWB辅助惯导实验

本UWB实验室构型如图7所示，室内安装4个UWB基站实时向计算机传输标签定位数据。实验区为边长5 m的正方形，各点坐标由激光测距仪测得，将惯导与UWB的坐标原点均设于其左下角O点，X、Y轴如图7所示。实验路线设置为从O点起沿Y轴正向顺时针绕行一圈回到O点，共20 m。实验过程中：前15 m实验者正常行走；后5 m设置一名人员位于实验者与某一基站之间，起到对信号的阻挡作用。

图7 设备位置及实验路线示意图Fig.7 Sketch map of equipment position and experimental route

实验结果如图8所示，其中：黑色点划线为规划行走路线；红色虚线为UWB测量路线，由于遮挡的存在，在（0, 0）附近时候存在较大的误差；浅蓝色点线为惯导测量路线，存在一定的误差累积情况；深蓝色粗实线为惯导/UWB组合导航路线，与规划行走路线贴合最好。

图8 惯导、UWB导航与组合导航实验结果对比Fig.8 Comparison on experimental results of inertial navigation, UWB navigation and integrated navigation

由图8可得：

1）前15 m中，由于实验者实际行走时肩部存在轻微晃动，不能保证时刻与规划路线一致，UWB数据与规划路线之间存在20 cm以内的误差，具有较高的可靠性。后5 m中，由于部分信号被阻挡，UWB信号产生漂移。

2）惯导数据由于误差累积，随着时间推移，后期偏差逐步扩大，最终位置为（0.6918, 0.1279），与原点位置差为0.7035 m，位置误差占行走距离的3.52%，全程平均位置误差为0.6437 m。

3）惯导/UWB组合导航数据最终位置为（-0.0738, -0.1588），与原点位置差为0.1751 m，位置误差占行走距离的0.88%，全程平均位置误差为0.0968 m。

由于本文设置仅在零速修正时刻进行卡尔曼滤波，因此每次刚进入零速修正时刻开始滤波时，输出结果存在小幅度的跳变。

综上，惯导/UWB组合导航相对于惯性导航单独使用在绕圈行走的最终结果以及整个导航过程的平均误差方面都有明显的精度提升，同时与UWB单独导航相比，基本接近UWB的正常导航精度，且在UWB定位出现问题时，系统依然能靠惯导保证定位的精度，能够满足较长时间导航的需要。

5 结 论

在一般室内区域，本文采用基于零速修正的惯导系统进行导航，以抑制惯导误差随时间发散。同时，在关键节点（拐弯）处布设UWB设备，通过松组合的形式，将UWB/惯导组合在一起，则可以实现在零速修正时刻对UWB与惯导进行卡尔曼滤波，实现对惯导信息的修正。

从实验结果可以看出，本文提出的算法能够将文中室内导航的定位精度从 3.52%提升到 0.88%。本方法修正了惯性传感器之前行走过程所积累的误差，解决了低精度消费级惯导无法较长时间准确导航定位的问题；与完全使用UWB进行室内定位相比，系统既实现了定位精度与成本的平衡，又能够避免UWB出现问题时对整体定位性能的影响。

（References）：

[1] Hsu L T, Gu Y, Kamijo S. 3D building model-based pedestrian positioning method using GPS/GLONASS/QZSS and its reliability calculation[J]. GPS Solutions, 2015: 1-16.

[2] Zhang Xin-xi, Zhang Rong, Guo Mei-feng, et al. Yaw error self-observation algorithm for pedestrian navigation via foot-mounted inertial navigation system[J]. Journal of Chinese Inertial Technology, 2015, 23(4): 457-466.

[3] 万骏炜. 多信息融合室内外无缝个人定位导航系统实现研究[D]. 南京航空航天大学, 2015. Wan Jun-wei. Research on multi-information fusion based seamless outdoor/indoor personal navigation system [D]. Nanjing University of Aeronautics and Astronautics, 2015.

[4] Girisha R, Prateek G V, Hari K V S, et al. Fusing the navigation information of dual foot-mounted zero-velocity-update-aided inertial navigation systems[C]//2014 IEEE International Conference on Signal Processing and Communications. 2014: 1-6.

[5] 曾庆化, 万骏炜, 刘建业, 等. 基于蜂窝网格粒子滤波的行人导航航向估计方法[J]. 中国惯性技术学报, 2014, 22(5): 576-579. Zeng Qing-hua, Wan Jun-wei, Liu Jian-ye, et al. Heading compensation algorithm based on cellular structure particle filter for pedestrian navigation[J]. Journal of Chinese Inertial Technology, 2014, 22(5): 576-579.

[6] 王云舒, 刘建业, 曾庆化, 等. 结构光辅助的惯性/视觉室内导航三维环境重构方法[J]. 中国惯性技术学报, 2016, 24(1): 51-58. Wang Yun-shu, Liu Jian-ye, Zeng Qing-hua, et al. 3D environment restructure method with structured light for indoor vision/inertial navigation[J]. Journal of Chinese Inertial Technology, 2016, 24(1): 51-58.

[7] Zhang R, Hoflinger F, Reindl L. Inertial sensor based indoor localization and monitoring system for emergency responders[J]. IEEE Sensors Journal, 2013, 13(2): 838-848.

[8] 吴哲明, 孙振国, 张文增, 等. 基于惯性测量单元旋转的陀螺漂移估计和补偿方法[J]. 清华大学学报(自然科学版), 2014(9): 1143-1147. Wu Zhe-ming, Sun Zhen-guo, Zhang Wen-zeng, et al. Gyroscope bias estimation and compensation by rotation of the inertial measurement unit[J]. Journal of Tsinghua University(Science and Technology), 2014(9): 1143-1147.

[9] Suski W, Banerjee S, Hoover A. Using a map of measurement noise to improve UWB indoor position tracking[J]. IEEE Transactions on Instrumentation & Measurement, 2013, 62(8): 2228-2236.

[10] Tiemann J, Schweikowski F, Wietfeld C. Design of an UWB indoor-positioning system for UAV navigation in GNSS-denied environments[C]//International Conference on Indoor Positioning and Indoor Navigation. IEEE, 2015.

[11] Benini A, Mancini A, Longhi S. An IMU/UWB/Visionbased extended Kalman filter for mini-UAV localiza- tion in indoor environment using 802.15.4a wireless sensor network[J]. Journal of Intelligent & Robotic Systems, 2013, 70(1-4): 461-476.

[12] Ascher C, Zwirello L, Zwick T, et al. Integrity monitoring for UWB/INS tightly coupled pedestrian indoor scenarios [C]//International Conference on Indoor Positioning and Indoor Navigation. IEEE, 2011: 1-6.

[13] Zampella F, De Angelis A, Skog I, et al. A constraint approach for UWB and PDR fusion[C]//International Conference on Indoor Positioning and Indoor Navigation. 2012: 1-9.

[14] Irahhauten Z, Nikookar H, Klepper M. 2D UWB localization in indoor multipath environment using a joint ToA/ DoA technique[C]//IEEE Wireless Communications & Networking Conference. 2012: 2253-2257.

[15] Kilic Y, Ali A J, Meijerink A, et al. The effect of humanbody shadowing on indoor UWB TOA-based ranging systems[C]//The Workshop on Positioning Navigation & Communication. IEEE, 2012: 126-130.

Indoor pedestrian navigation method based on optimal allocation of UWB

ZENG Qing-hua1,2, WANG Jing-xian1,2, MENG Qian1,2, XIONG Zhi1,2, QIAN Wei-xing3
(1. Navigation Research Center, Nanjing University of Aeronautics and Astronautics, Nanjing 210016, China;
2. Satellite Communication and Navigation Collaborative Innovation Center, Nanjing 210016, China;
3. School of Electrical and Automation Engineering, Nanjing Normal University, Nanjing 210094, China)

In view that the heading and positioning errors of the MEMS micro-inertial navigation system is divergent with time, a method for realizing the indoor high-precision pedestrian positioning system is put forward based on an ultra-wideband-aided micro-inertial navigation system. A micro-inertial navigation system based on zero velocity update is adopted to suppress the divergence of inertial navigation error with time. The UWB (Ultra-wideband) equipment is optimally installed in key areas of the building, and a Kalman filter is used to modify the direction and the position of the micro-inertial navigation system with the help of the UWB signal and the inertial navigation data. Compared with the method based on a large number of UWB devices, the proposed method has such advantages as high positioning accuracy, low cost, high autonomy, etc. Meanwhile, this method can avoid the poor UWB information’s influences on the overall navigation results. The walking experiments indicate that the final positioning error of the micro-inertial navigation system is 1.8% in the linear walking, and the final positioning error is less than 1.0% when assisted with the UWB in the turning walking.

indoor positioning; inertial navigation; ultra-wideband radio technology; integrated navigation

U666.1

A

1005-6734(2017)02-0186-06

10.13695/j.cnki.12-1222/o3.2017.02.009

2017-01-04；

2017-03-28

国家自然科学基金（61533008，61374115，61328301，61304227）；江苏省自然科学基金（BK20141453）；中央高校基本科研业务费专项资金（NS2015037）

曾庆化（1979—），男，副教授，从事惯性导航及组合导航技术研究。E-mail: zengqh@nuaa.edu.cn