蓝牙阵列/IMU/里程计室内组合定位方法

常康龙，甄 杰，李晨辉，杨文东

蓝牙阵列/IMU/里程计室内组合定位方法

常康龙1,2，甄 杰1，李晨辉1,2，杨文东1

（1. 中国测绘科学研究院，北京 100036；
2. 辽宁工程技术大学测绘与地理科学学院，辽宁阜新 123000）

在室内定位领域，惯性测量单元（IMU）、里程计等常见传感器存在累积误差，且无法利用全球卫星导航系统（GNSS）提供位置校准服务，而采用基于蓝牙信号的阵列天线定位结果易受到非视距（NLOS）误差影响。针对上述问题，提出基于蓝牙阵列/IMU/里程计组合的扩展卡尔曼滤波（EKF）模型。根据惯性导航的误差推算模型，同时利用里程计运动特性与单基站阵列天线测角定位原理，构造了速度观测及2维位置观测信息，从而提高组合导航性能。基于轮式移动测量平台的室内实测结果表明：该组合方法有效减小IMU、里程计存在的误差累计以及蓝牙阵列NLOS误差影响，整体运动轨迹与真实轨迹较为符合，能够为运动载体提供准确且丰富的位姿信息。

组合导航；
扩展卡尔曼滤波；
惯性导航；
里程计；
蓝牙阵列

在车载导航及移动机器人等领域，常常搭载惯性测量单元（inertial measurement unit, IMU）、轮式里程计（简称为里程计）、全球卫星导航系统（global navigation satellite system, GNSS）等多种定位设备。在室内环境下，由于卫星信号受到建筑物的遮挡后衰减严重，GNSS难以在室内环境下提供有效的定位服务，因此室内定位方法往往采用无线保真（wireless fidelity, WiFi）、蓝牙、超宽带（ultra-wide band, UWB）等多种定位技术。

不同传感器系统存在各自的优势与不足。在室内领域基于蓝牙信号的阵列天线系统定位结果受信号遮挡，多径影响明显[1]；
此外阵列天线无法提供载体的姿态角信息，其数据更新速率较慢，当面对复杂环境时，单一的蓝牙阵列系统难以满足实时快速定位需求。惯性导航系统（inertial navigation system, INS）输出频率高，不受外界环境影响，但是其速度与位置误差随着时间积分会加速累积。里程计系统在平整的路面上能够提供较为准确的线速度测量值，然而在复杂不平的地形和光滑地面上的性能较差，且缺乏部分位姿信息[2-3]。因此面对复杂环境时，单一的传感器系统往往不能满足位置服务需求，需要利用不同传感器之间的互补性来面对不同的定位场景。传感器之间的多层次、多空间的信息互补和优化组合处理是目前数据融合的难点，也是当今研究的热点[4]-[5]。

文献[6]采用了基于鲁棒扩展卡尔曼滤波（extended Kalman filter, EKF）的INS/GNSS/视觉里程计组合导航方法，并将EKF与胡贝尔（Huber）方法结合，克服观测量受噪声干扰时对导航性能的影响，以提升系统鲁棒性。文献[7]采取了UWB与INS的融合定位方案，利用单个UWB基于往返时间的测距值与INS进行卡尔曼滤波组合，采用UWB/INS紧组合模型来解决UWB测距的时间异步问题。文献[8]采用卡尔曼滤波算法对轮式里程计与UWB数据进行不同信赖度的融合，实验结果显示；
组合后的数据相较于单独UWB系统而言，稳定性和定位精度均有了较大的提高，很好地解决了里程计的误差累计与UWB系统存在非视距（non line of sight, NLOS）误差的问题。文献[9]提出了双通道开关切换轮采双极化天线阵，外加比对天线的比相测角方法，消除了由天线开关切换引起的频率估计误差，结合角度和极化解耦的超分辨估计算法，实现了单基站高精度定位。

在定位导航过程中，常采用对获得的数据信息中的误差进行滤波的方法，因为误差状态是小量，线性化时更精确，同时能够避免与过度参数化相关的问题以及相关协方差矩阵奇异性的风险[10]。本文提出蓝牙阵列/IMU/里程计3种传感器组合方法，并基于IMU误差模型并融合里程计、蓝牙阵列天线的观测值构建基于误差状态的扩展卡尔曼滤波器，以期有效结合各种传感器的优势特性，增加室内定位结果的可靠性[11]。

本文采用基于误差状态的扩展卡尔曼滤波模型，分为预测和更新2个主要过程。预测过程通过状态方程确定，观测方程给出数据观测部分，通过结合先验值以及当前观测来更新出最优估计值。

1.1 状态方程

IMU的误差方程可以根据惯性导航解算公式[12-13]得到，即为

根据误差方程可以写成状态方程的通用形式为

1.2 观测方程

考虑速度误差时的速度转换方程可以表示为

将通过观测得到的值作为真实值，加入测量误差后称为理想值。二者之间的关系可以表示为

将式（6）代入式（5）可得

将式（4）代入式（7）可得

将载体正常行进中横向和天向的速度设置为零。观测方程写为

对于观测方程，不需要乘以滤波周期，可直接写为

根据文中1.1、1.2节中推导的状态方程与观测方程，并将式（12）和式（13）中各量带入EKF中，可以得到后验状态量与后验协方差值，构建的完整滤波器预测与更新过程可以表示为

1.3 融合整体框架

基于EKF融合的整体框架如图1所示。

图1 基于EKF融合的整体框架

将获取的IMU角速度与加速度数据代入状态方程进行惯性解算，从而得到先验状态值。当存在有里程计速度观测或者蓝牙阵列位置观测值时，代入观测方程，并计算得到卡尔曼增益矩阵；
同时根据滤波器获得位置、速度与姿态误差；
最后得到误差修正后的后验估计值。当不存在观测值时，不需要执行卡尔曼滤波的后3个方程，即后验值等于先验值。本文设计的方案即采用蓝牙阵列/IMU/里程计松耦合方式，结合多种传感器优势，一方面可以依据IMU和里程计的短时间内高频率、高精度的优点来有效解决蓝牙阵列天线中存在NLOS误差的问题，另一方面根据蓝牙阵列天线定位结果消除IMU和里程计存在的累积误差，从而得到更加丰富的载体信息以及鲁棒性更强的导航定位结果。

实验所使用的数据采集设备为杰克卡尔（Jackal）移动测量平台。该平台装备的工控机为8 GB内存，搭载Intel（因特）Core（酷睿）i7-7567U处理器，乌班图（Ubuntu）18.04操作系统。外部传感器上采用克森斯（Xsense）惯性测量单元，内置于移动测量平台，车轮上有着独立的运动控制模块。同时在Jackal移动测量平台上在放置蓝牙标签，作为蓝牙阵列信源以采集阵列天线定位结果。移动测量平台及蓝牙阵列天线系统设备如图2所示。

图2 实验设备

移动测量平台具体技术指标如表1所示。

实验地点选区某栋大楼的3楼，包含走廊及318会议室房间，实验场地实现蓝牙阵列天线全覆盖，实验环境如图3所示。

图3 实验环境示意图

表1 Jackal小型移动平台底盘技术指标

在时间基准统一方面，本文采用服务器时间作为时间基准。蓝牙标签的位置数据由手机客户端获取，里程计及IMU原始数据通过移动测量平台上搭载的米尼（mini）计算机可以获得。2个终端均以服务器时间为基准，并根据网络时间协议（network time protocol, NTP）进行时间同步。

考虑到实际应用层面场景的多样性，本次实验采用复杂程度不同的2条实验路径进行测试，以验证设计方法的可行性。操纵移动测量平台经过不同的路径环境，第1条实验路径为移动测量平台从点出发，依次经由点后沿走廊移动至点结束。第2条实验路径为移动测量平台以点为起始点，经过点后沿走廊返回至点结束。路径1的采样时间为190 s，路径2的采样时间为316 s，IMU的数据输出频率为70 Hz，里程计频率为 50 Hz，获取蓝牙阵列天线定位结果的频率约为 1 Hz左右。图4、图5分别为2条路径下蓝牙阵列定位轨迹。

从图4和图5可以看出：蓝牙阵列动态实验定位结果较符合真实运动轨迹，整体精度较高；
但载体运动过程中实验环境存在墙体等障碍物，信号传播受到NLOS误差影响，部分点位定位结果存在一定程度偏差，若依赖系统本身的原始输出，远距离点的定位精度无法保证。因此考虑加入惯性导航和里程计模型，结合多种传感器信息，并构造基于误差状态的卡尔曼滤波器以改善定位结果。

图4 路径1蓝牙阵列定位轨迹

图5 路径2蓝牙阵列定位轨迹

对蓝牙阵列/IMU/里程计数据进行处理，得到移动测量平台的路径轨迹图以及横滚角、俯仰角、方位角等3个方向的角度值。本文选用高精度激光点云与地图匹配轨迹作为真实轨迹，且通过高精度全站仪对点云匹配输出的定位结果进行测试，定位精度保持在5 cm之内。对路径1和路径2分别进行3次定位实验，3次实验定位结果相近。图6给出路径1的蓝牙阵列/IMU/里程计组合的其中1组的实验轨迹以及真实轨迹。图7给出了对应的3个方向角度。图8给出了一组蓝牙阵列/ IMU/里程计组合实验路径2的实验轨迹以及真实轨迹。图9给出了对应的3个方向角度。

图6 蓝牙阵列/IMU/里程计组合实验路径1

图7 实验路径1三个方向角度

图8 蓝牙阵列/IMU/里程计组合实验路径2

从图6至图9可以看出，蓝牙阵列/IMU/里程计组合定位结果能够很好地改正蓝牙阵列定位结果，且较好地消除IMU、里程计的累计误差，整体轨迹较贴合真实轨迹，并存在3个方向姿态信息。在真实轨迹中找相同时刻对应的位置坐标进行作差处理，得到蓝牙阵列/IMU/里程计组合的平面位置误差曲线，如图10至图11所示。

对路径1和路径2的3次实验结果进行定量分析，统计得到蓝牙阵列/IMU/里程计组合实验的均方根误差，同时对位置误差取绝对值从而计算得到最大值，结果统计如表2所示。

图9 实验路径2三个方向角度值

图10 蓝牙阵列/IMU/里程计组合实验路径1位置误差

图11 蓝牙阵列/IMU/里程计组合实验路径2位置误差

表2 蓝牙阵列/IMU/里程计融合实验平面位置误差 m

从以上实验结果可以看出，蓝牙阵列/IMU/里程计组合结果整体精度较高，保持在0.5 m以内，其中：路径1中方向最大值为0.49 m，均方根误差平均值为0.12 m，方向最大值为0.46 m，均方根误差平均值为0.14 m；
路径2中方向最大值为0.48 m，均方根误差平均值为0.09 m，方向上最大值为0.47 m，均方根误差平均值为0.13 m。

不同复杂程度路径下室内环境的实测数据结果表明，本文组合算法较为可靠，同时结合了多种传感器的优势，弥补单一传感器存在的性能不足。融合结果能够有效地消除IMU、里程计等传感器存在的累积误差，同时依据IMU和里程计准确的误差模型，组合方法能够提高蓝牙阵列定位的稳定可靠性，削弱由于NLOS误差存在的异常值影响，且能提供载体较为精确且高频的姿态及位置信息。蓝牙阵列/IMU/里程计组合定位方法可以在室内环境下提供较为可靠的定位精度。

本文主要研究多传感器定位方法，设计了蓝牙阵列/IMU/里程计的组合方法，构建IMU误差模型，同时提取蓝牙阵列天线和里程计数据作为观测信息。实测数据结果表明，本文提出的组合方法能利用这3个传感器各自优点，且基于误差状态的EKF滤波结果有着较好的实用性，既可以削弱蓝牙信号受NLOS影响的定位偏差，同时也能消除IMU、里程计存在的误差累计问题，有效地将相对定位与绝对定位方式相结合，提升了定位导航系统的精度与鲁棒性，同时也为室内环境下组合导航提供了一种可行性方案。

[1] LI C H, ZHEN J, CHANG K L, et al. An indoor positioning and tracking algorithm based on angle-of-arrival using a dual-channel array antenna[J]. Remote Sensing, 2021, 13(21): 4301.

[2] 陈映秋, 旷俭, 牛小骥, 等. 基于车轮安装惯性测量单元的车载组合导航[J]. 中国惯性技术学报, 2018, 26(6): 799-804.

[3] BORENSTEIN J, FENG L Q. Measurement and cor-rection of systematic odometry errors in mobile robots [J]. IEEE Transactions on Robotics and Automation, 1996, 12(6): 869-880.

[4] 严恭敏, 邓瑀. 传统组合导航中的实用 Kalman 滤波技术评述[J]. 导航定位与授时, 2020, 7(2): 50-64.

[5] 李灿, 沈强, 秦伟伟, 等. 基于平滑变结构-卡尔曼滤波的MIMU/BDS组合导航技术[J]. 航空兵器, 2021, 28(3): 51-58.

[6] 李文华, 汪立新, 沈强, 等. 基于鲁棒EKF的MEMS-INS/GNSS/VO组合导航方法[J/OL]. 系统工程与电子技术. (2022-01-18)[2022-01-20]. https: //kns. cnki. net/kcms/detail/detail. aspx?dbcode=CAPJ&dbname=CAPJLAST& filename=XTYD20220115007&uniplatform=NZKPT&v=zJsPjaDuhsTEorkxMPIyJ83Z1H65HMU63LkwB1jL8CvgZtbTzpDpVXbNdh6sBIV0.

[7] 隋心, 高嵩, 黄俊尧, 等. UWB辅助INS的室内组合方法[J]. 无线电工, 2020, 50(8): 624-630.

[8] 黄俊杰, 李海滨, 贾翠玲. 基于卡尔曼滤波的UWB与里程计融合定位方法[J/OL]. 机床与液压. (2021-06-24) [2022-01-20]. https://kns.cnki.net/kcms/detail/detail.aspx?dbcode=CAPJ&dbname=CAPJLAST&filename=JCYY 20210624000&uniplatform=NZKPT&v=A85CcwTx4DoXWb0MIa7EK6TMK8MRfEEB8SDbgGJBL3mXqeMN-RMDrhdOE9JrldGX.

[9] 甄杰, 吴建新, 刘纪平, 等. 一种单基站高精度室内应急定位方法[J]. 武汉大学学报(信息科学版), 2020, 45(8): 1146-1154.

[10] CHOU J. Quaternion kinematic and dynamic differential equations[J]. IEEE Transactions on Robotics and Automation, 1992, 8(1): 53-64.

[11] 周文鹏, 路林, 王建明. 多传感器信息融合在无人驾驶中的研究综述[J]. 汽车文摘, 2022(1): 45-51.

[12] CHANG G B. On Kalman filter for linear system with colored measurement noise[J]. Journal of Geodesy, 2014, 88(12): 1163-1170.

[13] MARTINELLI A. Vision and IMU data fusion: closed-form solutions for attitude, speed, absolute scale, and bias determination [J]. IEEE Transactions on Robotics, 2012, 28(1): 44-60.

Indoor integrated positioning method based on bluetooth array/IMU/odometer

CHANG Kanglong1,2, ZHEN Jie1, LI Chenhui1,2, YANG Wendong1

（1. Chinese Academy of Surveying and Mapping, Beijing 100036, China；
2. School of Geomatics, Liaoning Technical University, Fuxin, Liaoning 123000, China）

In the field of indoor positioning,common sensors such as inertial measurement units (IMU) and odometers have cumulative errors and cannot use the global navigation satellite system (GNSS) to provide position calibration services, while array antenna positioning results based on bluetooth signals are easily affected by non-line-of-sight (NLOS) errors. In view of the above problems,In this paper, an extended Kalman filter (EKF) model based on the combination of bluetooth array/IMU/odometer is proposed. According to the error estimation model of inertial navigation,the velocity observation and two-dimensional position observation information are constructed by using the motion characteristics of odometer and the principle of angle measurement of single base station array antenna,so as to improve the performance of integrated navigation.The actual measurement results based on the wheeled mobile measurement platform show that the combined method can effectively reduce the error accumulation of IMU and odometer and the influence of the NLOS error of the bluetooth array, The overall motion trajectory is more consistent with the real trajectory, and can provide accurate and rich pose information for the motion carrier.

integrated navigation; extended Kalman filter; inertial navigation; odometer; bluetooth array

P228

A

2095-4999(2022)06-0088-09

常康龙，甄杰，李晨辉，等. 蓝牙阵列/IMU/里程计室内组合定位方法[J]. 导航定位学报, 2022, 10(6): 88-96.（CHANG Kanglong, ZHEN Jie, LI Chenhui, et al. Indoor integrated positioning method based on bluetooth array/IMU/odometer[J]. Journal of Navigation and Positioning, 2022, 10(6): 88-96.）

10.16547/j.cnki.10-1096.20220612.

2022-01-28

国家重点研发计划项目（2016YFB0502201）。

常康龙（1997—），男，河南商丘人，硕士研究生，研究方向为室内定位。

猜你喜欢里程计卡尔曼滤波蓝牙室内退化场景下UWB双基站辅助LiDAR里程计的定位方法导航定位学报(2022年5期)2022-10-13基于深度强化学习与扩展卡尔曼滤波相结合的交通信号灯配时方法北京航空航天大学学报(2022年8期)2022-08-31蓝牙音箱的直线之美工业设计(2022年7期)2022-08-12一种单目相机/三轴陀螺仪/里程计紧组合导航算法中国惯性技术学报(2019年1期)2019-05-21卡尔曼滤波在信号跟踪系统伺服控制中的应用设计电子制作(2019年23期)2019-02-23简单、易用,可玩性强Q AcousticsBT3蓝牙音箱家庭影院技术(2018年10期)2018-11-02适合自己的才是最好的 德生(TECSUN) BT-50蓝牙耳机放大器家庭影院技术(2018年3期)2018-05-09基于递推更新卡尔曼滤波的磁偶极子目标跟踪北京航空航天大学学报(2017年9期)2017-12-18基于有色噪声的改进卡尔曼滤波方法中国惯性技术学报(2017年1期)2017-06-09大角度斜置激光惯组与里程计组合导航方法导航定位与授时(2016年6期)2016-03-16