韩 天，郝 敏，李传苗，赵雄伟，缪存孝

(北京科技大学机械工程学院，北京 100083)

0 引言

随着通信技术的高速发展，室内定位技术在监控管理、无人驾驶等领域占据越来越重要的地位，可以为运动目标提供准确位置信息的室内定位技术越来越成为人们关注的焦点[1-5]。惯性导航技术是最早被应用到室内定位领域的导航技术，国内外学者针对基于惯性导航的定位算法做了大量的研究，但是由于惯性导航存在累积误差，所以并不适合长时间定位[6]。超宽带(Ultra-wideband，UWB)是一种新型的无线通信技术，相比于传统的无线通信技术，UWB具有功率谱低、保密性强、功耗低等优点[7-8]。自2010年以来，已有几种成熟的UWB无线电传感器定位设备能够提供厘米级的定位，例如DecaWave[9]的DWM1000和KickStarter的Pozyx。与其他室内定位技术相比，UWB定位通过发送纳秒(ns)或微秒(μs)非正弦窄脉冲可实现高精度解决方案，而不会产生误差累积[10]。UWB信号容易被外界环境所干扰，而IMU的INS对这些干扰不敏感，在复杂环境中难以使用单一类型的传感器满足需求，所以为了克服多径效应，UWB通常需要与惯导融合。

Mäkelä等[11]讨论了传感器融合技术，用于利用室内导航安装的IMU、气压计和UWB，它们可以在地板上获得准确的位置，在现实的战术测试场景中显示。Sczyslo等使用了松散组合法，基于扩展Kalman滤波器来跟踪行人的运动[12-16]。有学者提出了一种紧密耦合的方法，该方法结合了UWB范围和INS的测量[17-19]。文献[20]设计了一种用于无人机室内导航的定位方法，该方法集成了3D激光扫描仪、UWB和INS的信息，该策略可改善定位效果，与仅INS和仅UWB的方法相比，准确性显着提高。文献[21]设计了一种使用松散耦合EKF算法的UWB和IMU集成系统，在NLOS和UWB信号不足或不可用的情况下，有效地提高了定位的精度和稳健性。北京航空航天大学的仪玉杰等[22]针对在复杂室内环境下单纯依靠UWB技术定位结果会严重失真甚至缺失的问题，提出了一种将UWB与行人航迹推算方法(PDR)相结合的方法，解决了在复杂室内环境下定位解算点可能缺失的问题，并且较显著地提高了在复杂环境下定位系统的鲁棒性和定位精度。文献[23]利用安装在移动节点上的UWB模块产生位置的估计值，以校正惯导的航位推算法所得到的移动节点的位置坐标校正惯导中由于陀螺漂移导致的累积定位误差。文献[24]基于UWB结合IMU的室内定位设计方法提出了一种定位技术，可以有效抑制IMU的误差积累，进一步提高了定位精度。文献[25]针对UWB和IMU融合车辆的定位问题，提出了一种约束鲁棒迭代扩展Kalman滤波算法，克服了针对非Gauss噪声先天缺陷的扩展Kalman滤波器算法的缺点，定位精度达到0.21m。

尽管这些方法确实可以提高定位精度，但都没有考虑基站信号的丢失对系统产生的影响。单一的定位系统或精度不够、或应用场景受限等，无法达到实际的应用要求。为提高室内定位精度、克服多径效应，本文首先开展了UWB和惯性导航融合定位方法研究。同时，为了解决基站信号丢失对系统产生的影响，先基于改进马氏距离对测距异常值进行剔除，后引入了协方差矩阵迹作为基站选择的度量信息，利用小车进行矩形运动实验，实测数据结果表明，本文提出的最优基站选择算法的定位精度较轮询算法有较大提升。

1 基于改进马氏距离判定的UWB测距

1.1 双边双向测距原理

UWB模块进行测距的原理为：利用飞行时间(Time of Fly，TOF)来获得两个模块之间的距离。测距采用的通信方式如图1所示，在传统的双边双向测距(Double-sided Two-way Ranging，DS)协议的基础上增加了一个REPORT消息帧，以便将所有的距离信息汇总到标签中进行下一步处理。

图1 UWB测距的通信过程Fig.1 Diagram of UWB ranging communication process

UWB标签首先广播一个UWB信号POLL，并记录下发送时间Ta1，经过时间tf后到达UWB基站；基站记录下接收到POLL信号的时间Tb1，并对信号进行处理，经过时间Treply1后，在Tb2(Tb2＝Tb1+Treply1)时刻基站向标签回应一个ANSWER信号；标签在成功接收到基站的ANSWER后记录下接收时间Ta2，并经过时间Treply2后发送FINAL信号；基站接收到标签的FINAL信号后，根据各个接收时间节点的时间戳，估计出传播时间

在已知UWB信号在空气中传播速度的前提下，可以很容易地计算出标签和基站之间的距离d

1.2 基于改进马氏距离判定的测距异常值检测

本文是在传统马氏距离异常值检测方法的基础上进行的改进，在检测过程中使用的均值和协方差矩阵都是在最小协方差行列式(Minimum Covariance Determinant，MCD)估计中计算出的稳定值，这样计算的样本中的异常值和正常值的马氏距离有明显的差值，从而可以实现对异常值的剔除。

传统的马氏距离可由如下计算公式得到

改进马氏距离异常值检测方法的核心思想是利用Rousseeuw提出来的快速MCD算法(FASTMCD)，再结合传统马氏距离的计算过程，得到一个稳定的协方差矩阵和一个稳定的均值向量，然后再根据传统马氏距离的计算公式计算出每个样本数据的稳定马氏距离，再进行异常值的剔除。

首先从M个距离样本中随机选取h个距离数据，h＝Mα(0.5＜α＜1)，并计算随机选取的这h个距离数据的均值T1和协方差矩阵S1，然后计算M个距离样本与T1的马氏距离d。接着从M个距离样本中选出马氏距离最小的h个数据样本，再根据新选取的这h个样本数据计算出新的均值T2和协方差矩阵S2，仅当T1＝T2且detS1＝detS2时，S1＝S2才成立。 这个过程不断迭代， 直到Ti＝Ti－1、Si＝Si－1时迭代停止，这时即认为得到的Ti和Si为稳定的均值和协方差，最后根据Ti和Si计算距离样本中每个数据的马氏距离，完成对异常值的检测。

基于改进马氏距离的异常值检测方法的流程图如图2所示。

图2 基于改进马氏距离的异常值检测方法流程图Fig.2 Flowchart of outlier detection method based on improved Mahalanobis distance

2 UWB和惯导融合定位方法

由于UWB信号具有抗干扰能力强、分辨率高等一系列的优点，使得基于UWB信号的无线定位技术在无线定位领域中脱颖而出。但是，由于室内条件下环境复杂度高和障碍物多等问题，导致信号在传播过程中会遇到非视距和多路径等问题，定位精度会随环境的复杂度提升而大幅下降，故可以采用UWB和惯导融合的方法来提高室内定位精度。惯性导航系统利用IMU加速度计和陀螺对定位目标的位姿进行解算，UWB标签与基站通过图1所示过程获得距离，然后利用EKF将IMU与UWB信息进行融合，最终完成对定位目标的位置信息解算。

2.1 IMU定位原理

惯性导航系统是一种完全自主且不受外部环境干扰的独立式导航系统，具有隐蔽性好和独立性高等优势。如图3所示，惯性导航系统的基本工作原理是以IMU作为系统硬件部分的核心传感器，以牛顿定律作为姿态解算的理论基础。IMU主要包括敏感线运动的加速度计传感器和敏感角运动的陀螺传感器。其中，利用加速度计可以获得载体的加速度，利用陀螺可以获得载体的角速度。加速度对时间进行一次积分计算后即为载体的速度，对时间进行再次积分可以得到载体的位移，角速度对时间进行一次积分可以得到载体的姿态信息。

图3 IMU定位原理Fig.3 Positioning principle of IMU

2.2 UWB／IMU融合定位原理

由于Kalman滤波只能用于线性系统，故使用以Kalman滤波算法为基础的EKF算法进行融合。

(1)状态方程

在定位的过程中，用转换矩阵R表示载体坐标系到导航坐标系的变换，利用IMU和UWB对系统的12维状态进行估计，系统状态取为位置、速度、方向。故在EKF计算的过程中，将状态向量ξ(k)取为

式(4)中，X为定位目标在导航坐标系下的位置坐标，ρ＝R－1˙X为定位目标在载体系下的速度，三维矢量δ为定位目标的姿态误差。

定位目标的方向这里采用了参考姿态Rref和旋转矢量δ来表示，δ的大小表示旋转角，δ方向上的单位矢量表示旋转轴，则其对应的旋转矩阵可表示为Dδ。

将Dδ一阶展开，有

式(5)中，I为大小为3×3的单位矩阵。

假设在离散时刻k到k+1的Δt内，定位目标相对载体坐标系的位置 变化 ΔX＝[ΔxbΔybΔzb]， 则

将R表示为如下形式

则k+1时刻定位目标在导航坐标系下的位置为

k+1时刻定位目标在载体坐标系下的速度可由如下方程式得到

d为由Rodriguez参数表示的姿态误差，有

式(11)中，ωx、ωy、ωz分别为陀螺在离散时间间隔内相对载体坐标系三个轴上的角速度，在采样时间内视为常数。

δ的更新可由下式得到

式(12)中，Dd为d对应的旋转矩阵。

系统的状态模型可表示为

式(13)中，f(·)为对系统进行一阶Taylor展开后的线性化函数模型。

(2)观测方程

在惯导和UWB融合定位中，由于UWB测距不存在累积误差，且测距误差仅在0.1m左右，属于一种较高精度的无线测距技术，所以UWB的测距值可作为EKF中的观测值。

假设位于已知位置的UWB基站i的坐标为(ρi，x，ρi，y，ρi，z)， 则在k时刻利用UWB标签解算得到的测距值为di，k。 假设此时定位目标的实际位置坐标为Xk＝(xk，yk，zk)， 则观测方程可以建模为

则观测矩阵为

图4 UWB和IMU融合定位的整体流程图Fig.4 Flowchart of UWB and IMU fusion positioning

式(16)中，

(3)EKF融合

根据状态方程和观测方程，通过EKF的时间更新和状态更新可以得到定位目标的状态信息。时间更新方程为

状态更新方程为

综上，基于UWB和惯性导航融合的定位系统整体流程如图4所示。

3 最优基站选择方法

经过UWB和惯导的融合定位后，对于定位区域内的基站，传统的选择方法采用的是轮询通信的方式，即按照固定规则的通信顺序与区域内的基站分别进行通信。这种传统的选择方法没有考虑到在每个时刻每个基站与定位目标之间产生的测距值会对EKF解算过程中的协方差矩阵的更新产生的影响是不同的，这就导致传统的基站轮询选择方法不能充分利用各个基站的位置信息。所以，为了进一步提高融合定位的精度，本文提出了一种基于协方差矩阵迹的最优基站选择方法。

不同于轮询方法，本文提出的UWB基站选择方法使定位标签可以自由选择区域内参与下一时刻EKF解算的最优基站。

根据定位目标的运动轨迹具有连续性这一特点，通过已知的当前时刻定位目标的位置坐标预测下一时刻在当前区域内的所有基站所能产生的测距值，根据测距值进一步计算出所能引起的协方差矩阵迹的变化，并以此作为在下一时刻进行基站选择的度量信息，目的是在每一时刻都以协方差矩阵迹的变化作为基准，选择当前区域内能使系统状态量的估计误差最小的基站作为下一时刻的通信基站，即最优基站。

在EKF融合解算过程中的协方差矩阵可以表示为

根据来自基站i的测距值di， 利用本文上节中的EKF观测方程的建立过程，带入式(16)可以得到对应的观测矩阵Hi， 并且可以进一步计算出对应的Kalman滤波增益Ki

式(22)中，R为测距噪声的方差阵。利用Kalman滤波增益Ki对协方差矩阵进行更新，有

由于观测矩阵具有稀疏性的特点，所以可以将式(24)简化为

根据式(25)，可在任一时刻计算定位目标所在区域内所有基站能引起的协方差的变化。

矩阵的迹定义为矩阵主对角元素之和，由协方差矩阵的特殊性质可以知道，协方差矩阵的迹即为系统所有状态量方差之和，即

ξ为系统的状态量，所以本文提出了一种利用协方差矩阵迹作为对下一时刻基站进行选择的度量指标，在每个时刻都选择定位目标所在区域内能使协方差矩阵迹最小，即能使系统的状态估计误差最小的基站作为下一时刻的最优通信基站。

再次利用观测矩阵的稀疏性质，对式(27)进一步简化

在每个时刻计算定位目标当前所在区域内每个基站的迹，然后选择值最小的基站作为下一时刻参与EKF解算的最优通信基站。

4 实验与结果分析

4.1 实验环境与硬件介绍

本文的实验测试环境为北京科技大学机电信息楼814实验室，实验室环境布局的平面简图如图5所示。房间大小为5.0m×8.0m，在房间内4.0m×4.2m大小的场地上方四周设有运动捕捉系统Optitrack定位摄像头，本文的实验场地位于运动捕捉系统捕捉范围内一个2.4m×2.4m的正方形区域。实际的实验环境和上方的运动捕捉系统的部分定位摄像头如图6所示。运动捕捉系统属于高精度的室内定位系统，采用红外摄像头覆盖室内的定位空间。在被定位的物体上放置反光标记点，通过获取这些反光标记点反射在定位摄像头上的图像，计算被定位物体的三维位置信息，且Optitrack定位摄像头的定位精度能达到0.1mm。在本文的实验中均使用了运动捕捉系统解算的运动轨迹作为定位目标的实际运动轨迹，与本文中所提出的定位方法形成定位精度的对比。

图5 实验室平面简图Fig.5 Diagram of laboratory plane

图6 实验环境Fig.6 Diagram of experiment environment

在实验过程中实时采集的UWB和IMU数据通过SPI和I2C的串行通信方式与STM32F405RG处理器进行通信，并对数据进行解算得到定位坐标，最后将最终的定位结果、UWB测距值和IMU数据通过蓝牙发送至上位机端保存成csv格式，并导入Matlab进行解算和绘图。定位标签和上位机端的蓝牙接收装置如图7所示。

图7 定位标签和蓝牙接收器Fig.7 Diagram of location tag and bluetooth receiver

UWB基站在实验环境中的布置和本文中使用的导航坐标系示意图如图8所示。

图8 UWB的基站布置和导航坐标系的建立Fig.8 UWB base station layout and establishment of navigation coordinate system

在对基站坐标进行测量时，先将可由运动捕捉系统识别的反光标记点放置在基站上，通过运动捕捉系统捕捉到的位置坐标作为各个基站的具体位置坐标，如表1所示。

表1 UWB基站坐标Table 1 Coordinates of UWB base station

4.2 测距异常值剔除实验

将基于马氏距离的异常值检测算法程序加入到测距程序中，仍让标签分别位于基站的1m和2.5m处，测距实验共进行1000次，测距频率为100Hz，实验结果如图9所示。

图10为采用基于改进马氏距离的异常值检测方法对异常值进行剔除后的1.0m和2.5m处的测距结果。

图9 测距异常值检测结果Fig.9 Diagram of ranging results using outlier detection

图10 测距异常值剔除后的检测结果Fig.10 Diagram of ranging results after removing the outliers

由图10可知，在整个测距的过程中没有出现测距异常值，且测距的精度均在0.1m左右，上述的测距实验表明了本文采用的基于改进马氏距离的异常值检测方法的有效性，能够在保证测距实时性的同时，有效避免测距过程中异常值的出现。

4.3 最优基站选择融合定位实验

为了验证本文提出的UWB和IMU融合定位方法的精度，在实际场景下进行实验并计算其估计轨迹误差。如图11所示，实验中将定位标签固定在可由运动捕捉系统反馈控制运动的定位小车上，定位标签由充电宝通过USB口进行供电，并在定位标签中心位置上布设一个可由运动捕捉系统识别的反光标记点。

图11 定位标签与小车Fig.11 Diagram of locating tag and trolley

控制小车在实验场地中按照一个1.2m×1.2m的矩形轨迹进行运动，实验结果中以由运动捕捉系统获得的定位标签中心的反光标记点的轨迹作为定位标签的实际运动轨迹。

移动小车按照矩形轨迹运行两次，分别采用本文的最优基站选择方法和传统的轮询方法估计小车的位置轨迹。实验中的真实轨迹均采用的为运动捕捉系统的输出结果，最后的真实轨迹取为两次运动捕捉系统数据的平均值，实验对比结果如图12示。

图12 定位结果对比Fig.12 Comparison of positioning results

由图12可知，在x方向和y方向上虽然某些位置处会出现采用传统选择方法的精度高于本文的最优基站选择方法的情况，但是整体轨迹中明显能看出最优基站选择方法对精度的改进。在z方向上，当使用传统的基站轮询选择方法进行定位时，定位结果具有较大的波动性，而使用了最优基站选择的定位结果中，z方向数据的平稳性有了很大提升。图13给出了利用两种不同基站选择方法进行定位时整体误差的比较。

图13 两种不同基站选择方法的误差对比Fig.13 Error comparison of two different base station selection methods

由图13明显可知，最优基站选择方法对定位误差波动幅度有一定的改善，定位精度得到提升。在整个运动过程中，采用最优基站选择方法的最大定位误差值为0.258m，绝对误差的平均值为0.226m，均方根误差为0.227m。采用传统轮询方法时，在整个过程中的最大定位误差值为0.342m，绝对误差的平均值为0.267m，均方根误差为0.269m。相比采用轮询方法的定位结果，使用本文提出的最优基站选择方法时，平均精度提高了15.3%，均方根误差减小了15.6%。

5 结论

本文对UWB和惯性导航的融合定位算法进行了研究，通过采用基于改进马氏距离的异常值检测算法，有效地对测距过程中的异常值进行了剔除。然后提出了一种基于协方差矩阵迹的最优基站选择算法，不同于按照给定顺序进行通信的轮询算法，所提出的算法能够实时计算所在区域内每个基站在下一时刻能对当前时刻协方差的迹产生的变化量，并选择能使当前协方差的迹变化最大的基站作为下一时刻的通信基站。实验结果表明，本文提出的基站选择算法具有计算量小、实时性高等优点，与传统的轮询算法相比，平均定位精度提高了15.3%，是一种高精度的实时室内定位方法。