开放科学（资源服务）标志码（OSID）：

0 概述

当火灾事故发生时，常需消防员深入火场内部进行灭火救援，为了保障消防员的生命安全以及提高救援效率，需要一款精确且稳健的定位系统，使消防指挥人员能够实时查看消防员所在位置。基于信号强度的ZigBee、RFID、WIFI和计算机视觉技术都可用于消防员室内定位，但这些技术的定位精度较低，抗环境干扰能力较差^[1]，难以实现对消防员的精确定位。

超宽带（Ultra-Wideband，UWB）定位技术具有带宽宽、定位精度高、穿透力强等特点^[2]，在火场环境下对消防员进行定位具有一定的优势^[3-4]。UWB精确定位的前提是定位标签和基站均处于视距（Line-of-Sight，LOS）环境中，而消防员在实际火场救援过程中需要不断地移动位置，标签和基站之间的通信常处于非视距（Non-Line-of-Sight，NLOS）环境，会导致较大的定位误差^[5]。若在定位过程中能检测到处于NLOS环境中的基站，则计算坐标时对其进行针对性的处理，便可以实现准确定位。常用的NLOS信号检测算法有范围估计法^[6]和信道估计法^[7]。然而，上述算法的难点在于不易选取适当的NLOS误差模型和恰当的阈值。

惯性导航系统（Inertial Navigation System，INS）无需依靠任何外界设备，即可实现独立自主定位^[8]。行人轨迹推测（Pedestrian Dead Reckoning，PDR）算法依靠惯性测量单元（Inertial Measurement Unit，IMU）计算行人行走过程中的步频、步长和航向数据，根据初始位置推算出行人的位置坐标。但是基于PDR算法的惯性定位的误差随时间延长而不断增加，因此无法进行长时间的精确定位^[9]。

单一定位方式具有局限性，而多技术组合则可以提高定位的可靠性和精确性。文献[10]提出一种基于优化贝叶斯的WiFi与蓝牙融合定位算法，弥补了单独定位时信标覆盖率及定位精度低的缺点。文献[11]提出WiFi和PDR融合定位算法，虽然在一定程度上解决了室内WiFi定位精度低及PDR定位误差累积问题，但定位精度仍在米级以上。文献[12]将UWB与PDR相融合推算行人行走轨迹，利用相邻时刻UWB的测距差值构造卡尔曼滤波器的测量噪声矩阵，自适应地改变UWB与PDR的定位比重，减小状态预测引入的误差。文献[13]设计了一种融合INS/UWB的低成本行人追踪系统，只依靠1个UWB基站参与定位，在跟踪的初始阶段，利用惯导跟踪算法估计基站位置，将超宽带距离测量值与惯性导航数据融合后再定位。以上2种方法虽然成本低廉，但定位的可靠性不高。文献[14]采用UWB/INS紧组合导航方法，将UWB基站的位置信息引入到系统状态变量中，减少了观测量对基站信息的依赖，提高了组合导航系统的灵活性和估计精度，但对UWB的NLOS误差缓解效果并不明显。基于此，文献[15]构建了一种自适应UWB测距不确定模型，该模型根据实时测量质量自动调整测量距离的数学误差模型，并将其应用于粒子滤波融合算法中，降低了超宽带测量误差，提高了定位精度。但由于使用粒子滤波^[16]进行融合定位时可能会发生轨迹丢失问题，因此文献[17]提出了粒子重置方法，根据测量距离来评估行人靠近UWB基站时粒子集的均方根误差，对粒子状态进行修正。

为了更可靠地评估UWB测量距离的效果，文献[18]提出了一种决策算法，该算法根据信道冲激响应数据和随机森林算法评估UWB测量距离的可用性，并在视距环境下使用卡尔曼滤波融合2个系统的定位信息。为了补偿INS的位置误差，文献[19]利用最小二乘支持向量机辅助无偏有限脉冲响应滤波器，以融合UWB和INS系统的定位信息，实现对室内行人的无缝跟踪。以上2项研究虽然都提高了系统的融合精度，但在UWB距离评估以及数据训练阶段花费时间过长，影响定位系统的实时性。为了提高运算效率，文献[20]提出一种基于移动平均滤波器的IMU/UWB融合算法，使用UWB和IMU数据生成当前位置的虚拟样本，并通过平均虚拟样本值来找到待定位目标的最佳位置估计。文献[21]对惯性传感器的随机误差进行分析和预滤波，并利用互补滤波器提供导航系统的姿态信息，在非视距环境下，通过反磁环消除UWB系统的异常值，采用双态自适应卡尔曼滤波器提高UWB和INS系统的融合定位精度。文献[22]使用迭代卡尔曼滤波器融合UWB和IMU系统，并提出一种三角形不等式定理来检测NLOS测量值，若$ t-1 $时刻标签到基站的距离、$ t-1 $到$ t $时刻标签移动的距离、$ t $时刻UWB测量的标签到基站的距离满足三角不等式定理，则判定$ t $时刻UWB的距离测量值处于LOS环境中，否则判定处于NLOS环境中。该算法在NLOS误差较大时检测性能较好，但当NLOS误差较小时容易出现误检和漏检的现象，导致定位结果不稳定。

本文使用双级EKF以松耦合^[23]的方式融合UWB和INS的定位结果，并基于INS提出一种针对UWB的NLOS检测算法，通过动态调整融合滤波器的测量误差矩阵来缓解NLOS环境下异常UWB测量值对定位结果的影响。

1 消防员室内定位系统

消防员室内定位系统主要由UWB定位模块和INS定位模块构成，如图 1所示。当火灾发生时，使用挂载UWB基站的无人机悬停在建筑物窗户外围，通过特制的定向天线向建筑物内部发射UWB脉冲。消防员胸前佩戴的定位终端集成了UWB定位标签和MPU9250惯性传感器，分别用于获取行走过程中标签到各个基站的距离和惯性数据。通过数据融合算法实现消防员的精确定位。

定位系统采用的是东北天（East North Up，ENU）导航坐标系，其中的X、Y、Z轴分别为正东、正北和法线方向。为简单起见，本文只考虑二维定位。

1.1 INS模块

由惯性测量单元集成的INS模块包含3轴加速度计、3轴陀螺仪和3轴磁力计。消防员将INS模块佩戴在胸前，根据行走时惯性传感器输出的加速度值、角速度值和磁场强度值，使用四元数求出航向角，根据加速度幅值的变化趋势使用峰值检测法求出步数，根据每一步的加速度最大值和最小值使用非线性步长模型求出步长。最后根据求出的步长、步数和航向角，使用PDR算法结合EKF求出消防员的初始位置坐标^[8]。

PDR算法是根据当前时刻的位置坐标$ ({E}_{k}, {N}_{k}) $，结合步长$ {S}_{k} $和航向角$ {\theta }_{k} $推算出下一步行人的位置坐标$ ({E}_{k+1}, {N}_{k+1}) $，具体过程如图 2所示。

1.2 UWB模块

UWB定位系统由佩戴在用户身上的标签和若干个固定在已知位置上的基站组成，标签和基站之间使用射频脉冲进行通信。根据TDOA算法，通过标签到基站之间的信号传播时间差，便可计算出标签到各个基站的距离差。

在实际应用中，UWB设备的测量结果会受到测量误差和NLOS误差的影响。如果基站和标签之间处于NLOS环境中，则相应的信号传输时间会增加，所得到的距离差值存在偏差。如图 3所示，2条双曲线的相交部分不一定是标签的准确位置，NLOS误差越严重，最终的定位误差越大。

测量误差来源于硬件系统自身的精度，且较为稳定。而NLOS误差取决于具体定位环境的变化，可能产生巨大的波动。因此，本文提出一种双级EKF框架实现UWB和INS的数据融合，在融合的过程中借助INS的定位结果对含有NLOS误差的UWB距离测量值进行检测和处理，以提高定位系统的抗NLOS干扰能力。

融合算法包含2级EKF：第1级EKF用于INS，根据前一时刻的联合位置估计值，使用第1级EKF计算出当前时刻消防员的初始坐标估计值并转化为到各个基站的距离值，将此距离值和当前时刻UWB的距离测量值相减并取绝对值，然后与预设阈值相比较。若大于阈值，则判定当前时刻的UWB距离测量值存在异常，极有可能是NLOS干扰；若小于则判定正常，对正常基站和异常基站的UWB距离测量值分别计算残差值并构成残差矩阵。第2级EKF用于数据融合，第1级EKF所得的残差矩阵用于调整第2级EKF的测量噪声矩阵，以此提高定位系统抗NLOS干扰的能力，使用第1级EKF的预测过程并结合第2级EKF的测量过程得到UWB和INS的联合位置估计值。

2 NLOS检测算法与UWB/INS的数据融合

本文所提的松耦合数据融合方式可以利用任意数量的UWB测量距离值，以增加定位系统的健壮性。最终的定位结果皆为融合定位所得，根据测量噪声残差矩阵自适应定位环境的变化，避免了定位模式的硬切换，增加了系统的实用性。基于迭代的卡尔曼滤波器使整个系统具备较高的实时性。

UWB的NLOS检测与UWB/INS的数据融合在双级EKF框架下完成。利用INS短时间内可精确定位的优势，将其应用于UWB的NLOS检测中。根据检测结果计算残差矩阵调整融合滤波器的测量噪声矩阵。第1级EKF所得的初始估计坐标仅用于NLOS检测，并未用于反馈EKF的预测过程，所以INS产生的误差并不会累积。在第2级EKF中，INS测量值和较为准确的UWB测量值都会被输入到融合滤波器中，INS的误差在此过程中得到不断修正。总体而言，整个双级EKF框架在利用INS定位优势的基础上很好的抑制了INS的误差累积。

双级EKF框架如图 4所示，第1级EKF用于INS，可得到当前消防员的初始位置估计坐标$ {\boldsymbol{X}}_{\mathrm{i}\mathrm{n}\mathrm{s}, k}^{+} $，使用此坐标进行NLOS检测，并计算残差值构成残差矩阵$ {\boldsymbol{R}}_{\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{i}\mathrm{d}\mathrm{u}\mathrm{e}, k} $。第2级EKF用于融合UWB和IMU的数据，将NLOS检测过程中获得的残差矩阵乘以融合滤波器的测量噪声矩阵，以此来减小UWB的NLOS误差，第2级EKF的定位结果$ {\boldsymbol{X}}_{\mathrm{f}\mathrm{u}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}, k}^{+} $作为整个系统的反馈值，用于下一次定位。

2.1 惯性导航的EKF

惯性导航的EKF即第1级EKF。由于航向、步长存在一定程度的测量误差，单独使用PDR算法不能获得准确的INS位置估计值，因此本文将EKF融入PDR算法中，以增加INS定位结果的准确性，具体过程如下：

状态变量为：

$ \boldsymbol{X}={\left[\begin{array}{cccc}E& N& S& \theta \end{array}\right]}^{\mathrm{T}} $

(1)

其中：$ E $和$ N $为消防员在ENU坐标系中的东向和北向坐标；$ S $为步长；$ \theta $为航向角（$ 0°\le \theta \le 360° $）。

观测量为：

$ {\boldsymbol{Z}}_{\mathrm{i}\mathrm{n}\mathrm{s}}={\left[\begin{array}{cc}{S}_{\mathrm{i}\mathrm{n}\mathrm{s}}& {\theta }_{\mathrm{i}\mathrm{n}\mathrm{s}}\end{array}\right]}^{\mathrm{T}} $

(2)

通过EKF的预测过程和测量过程可以得到消防员的初始状态估计：

$ {\boldsymbol{X}}_{\mathrm{i}\mathrm{n}\mathrm{s}, k}^{+}=\left[{\hat{E}}_{\mathrm{i}\mathrm{n}\mathrm{s}, k}{\hat{N}}_{\mathrm{i}\mathrm{n}\mathrm{s}, k}{\hat{S}}_{\mathrm{i}\mathrm{n}\mathrm{s}, k}{\hat{\theta }}_{\mathrm{i}\mathrm{n}\mathrm{s}, k}\right] $

(3)

$ k $时刻消防员在ENU坐标系中的初始位置估计坐标为$ ({\hat{E}}_{\mathrm{i}\mathrm{n}\mathrm{s}, k}, {\hat{N}}_{\mathrm{i}\mathrm{n}\mathrm{s}, k}) $。

虽然通过上述过程，可以得到消防员的初始位置，但是INS短时间内定位较为精确，长时间使用会使定位误差逐渐增加，因此需要融合UWB才能实现持续、稳定的精确定位。

2.2 NLOS检测算法

为了消除UWB的NLOS误差，借助第1级EKF所得INS定位结果来检测UWB的距离测量是否含有NLOS误差。NLOS检测算法利用INS系统短期定位精确的特点增加了算法的可靠性；通过分析UWB的距离测量来检测NLOS误差，算法的复杂度较低，增加了算法的实用性；根据检测结果求取残差矩阵，方便在数据融合中缓解NLOS误差。NLOS检测算法如图 5所示。

U点为$ k-1 $时刻UWB和INS的联合定位坐标最优估计值，根据U点的坐标使用第2.1节的惯性导航算法进行下一步估计，可以得到$ k $时刻的初始位置估计坐标P$ ({\hat{E}}_{\mathrm{i}\mathrm{n}\mathrm{s}, k}, {\hat{N}}_{\mathrm{i}\mathrm{n}\mathrm{s}, k}) $。根据P点的坐标求出P点到各个基站的距离：

$ {D}_{\mathrm{i}\mathrm{n}\mathrm{s}\mathrm{m}, k}=\sqrt{({\hat{E}}_{\mathrm{i}\mathrm{n}\mathrm{s}, k}-{E}_{\mathrm{u}\mathrm{m}}{)}^{2}+({\hat{N}}_{\mathrm{i}\mathrm{n}\mathrm{s}, k}-{N}_{\mathrm{u}\mathrm{m}}{)}^{2}} $

(4)

其中：$ ({E}_{\mathrm{u}\mathrm{m}}, {N}_{\mathrm{u}\mathrm{m}}) $是第$ m(m=\mathrm{1, 2}, \cdots , M) $个UWB基站的位置坐标。

用$ k $时刻定位终端到第$ m $个UWB基站的距离测量值$ {D}_{\mathrm{u}\mathrm{m}, k} $减去$ {D}_{\mathrm{i}\mathrm{n}\mathrm{s}\mathrm{m}, k} $并取绝对值，若此绝对值大于预设的阈值，则判定当前时刻的UWB距离测量值包含NLOS误差；若小于阈值，则判定此刻的UWB测量值在正常的误差范围内：

$ ‖{D}_{\mathrm{u}\mathrm{m}, k}-{D}_{\mathrm{i}\mathrm{n}\mathrm{s}\mathrm{m}, k}‖ > {T}_{\mathrm{t}\mathrm{h}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{h}\mathrm{o}\mathrm{l}\mathrm{d}} $

(5)

其中：阈值$ {T}_{\mathrm{t}\mathrm{h}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{h}\mathrm{o}\mathrm{l}\mathrm{d}} $是由UWB设备的测量精度和惯性导航的精度决定的。

对包含NLOS误差的UWB距离测量值求取残差：

$ {R}_{m, k}^{\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{i}\mathrm{d}\mathrm{u}\mathrm{e}}=\beta ·(‖{D}_{\mathrm{u}\mathrm{m}, k}-{D}_{\mathrm{i}\mathrm{n}\mathrm{s}\mathrm{m}, k}‖-{T}_{\mathrm{t}\mathrm{h}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{h}\mathrm{o}\mathrm{l}\mathrm{d}}) $

(6)

其中：$ \beta $为残差系数，通常设置为一个较大的值。NLOS误差越大，所求取的残差值越大。

不包含NLOS误差的UWB距离测量值残差为1，用所有残差值构成一个残差矩阵：

$ {\boldsymbol{R}}_{\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{i}\mathrm{d}\mathrm{u}\mathrm{e}, k}=\left[\begin{array}{ccc}{R}_{\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{i}\mathrm{d}\mathrm{u}\mathrm{e}}^{1, k}& \cdots & 0\\ ⋮& & ⋮\\ 0& \cdots & {R}_{\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{i}\mathrm{d}\mathrm{u}\mathrm{e}}^{M, k}\end{array}\right] $

(7)

在数据融合中将使用融合滤波器的测量噪声矩阵乘以该残差矩阵来达到缓解NLOS误差的目的。

2.3 数据融合的EKF

数据融合的EKF即第2级EKF，以松耦合的方式实现UWB/INS数据融合，可以提高数据的利用率。在进行二维定位时，一般需要3个或3个以上标签到基站的距离信息，当少于3个距离信息时，传统的3边测量算法将导致定位失败，当大于3个测距信息时，3边测量算法和松散耦合的方式将会浪费多余的UWB测量数据。以松耦合方式实现数据融合，可以利用任意数量的UWB测量数据，增加系统的健壮性。

第2级EKF的状态变量、状态方程、状态转移矩阵、预测噪声矩阵与2.1节的第1级EKF相同，此处不再赘述。

第2级EKF的观测量为：

$ {\boldsymbol{Z}}_{\mathrm{f}\mathrm{u}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}}={\left[\begin{array}{cc}{Z}_{\mathrm{u}\mathrm{w}\mathrm{b}}& {Z}_{\mathrm{i}\mathrm{n}\mathrm{s}}\end{array}\right]}^{\mathrm{T}} $

(8)

其中：$ {\boldsymbol{Z}}_{\mathrm{u}\mathrm{w}\mathrm{b}}=[{D}_{\mathrm{u}1}, {D}_{\mathrm{u}2}, \cdots , {D}_{\mathrm{u}m}, \cdots , {D}_{\mathrm{u}M}] $；$ {D}_{\mathrm{u}\mathrm{m}} $是定位终端到第$ m(m=\mathrm{1, 2}, \cdots , M) $个UWB基站的距离测量值，$ {\boldsymbol{Z}}_{\mathrm{i}\mathrm{n}\mathrm{s}} $如式（2）所示。

观测噪声矩阵为：

$ {\boldsymbol{R}}_{\mathrm{f}\mathrm{u}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}}=\left[\begin{array}{cc}{\boldsymbol{R}}_{\mathrm{u}\mathrm{w}\mathrm{b}}& \boldsymbol{0}\\ \boldsymbol{0}& {\boldsymbol{R}}_{\mathrm{i}\mathrm{n}\mathrm{s}}\end{array}\right] $

(9)

其中：$ {\boldsymbol{R}}_{\mathrm{u}\mathrm{w}\mathrm{b}} $为UWB定位模块的观测噪声矩阵；$ {\boldsymbol{R}}_{\mathrm{i}\mathrm{n}\mathrm{s}} $为INS定位模块的观测噪声矩阵。

将NLOS误差检测所得到的残差矩阵乘以融合滤波器的观测矩阵得到调整后的观测噪声矩阵：

$ {\boldsymbol{R}}_{\mathrm{f}\mathrm{u}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}, k}={\boldsymbol{R}}_{\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{i}\mathrm{d}\mathrm{u}\mathrm{e}, k}{\boldsymbol{R}}_{\mathrm{f}\mathrm{u}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}} $

(10)

残差矩阵中的元素大小和对应UWB测量值的NLOS误差成正比。将融合滤波器的测量噪声矩阵乘以残差矩阵，来调整对应UWB测量值的噪声，从而达到缓解NLOS误差的目的。

第2级EKF将输出最终的联合位置估计向量：

$ {\boldsymbol{X}}_{\mathrm{f}\mathrm{u}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}, k}^{+}=\left[\begin{array}{cccc}{\hat{E}}_{\mathrm{f}\mathrm{u}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}, k}& {\hat{N}}_{\mathrm{f}\mathrm{u}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}, k}& {\hat{S}}_{\mathrm{f}\mathrm{u}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}, k}& {\hat{\theta }}_{\mathrm{f}\mathrm{u}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}, k}\end{array}\right] $

(11)

由此可以得到UWB和INS融合定位的位置坐标估计值为$ ({\hat{E}}_{\mathrm{f}\mathrm{u}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}, k}, {\hat{N}}_{\mathrm{f}\mathrm{u}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}, k}) $。

算法的时间复杂度分析：给定初始位置后，根据图 4所示的算法流程循环迭代执行本文定位算法。假设定位目标完成1段测试路线要行走n步，那么定位过程需要进行n次，每执行一次定位的时间复杂度为O（1），执行完整个定位过程的时间复杂度为O（n），与单一定位算法的运算复杂度在1个量级上。

3 实验结果与分析 3.1 MATLAB仿真验证与分析 3.1.1 仿真方案

为了验证本文NLOS检测算法和数据融合算法的性能，设计了2组模拟实验。在相同的实验条件下比较基于UWB的EKF算法、融合了UWB/INS的双级EKF算法（包括有NLOS和无NLOS的检测算法）的定位精度，通过计算估计坐标和真实坐标的均方根误差（Root Mean Square Error，RMSE）来评估不同算法的性能。此外，文献[22]所述的基于三角形原理检测NLOS算法同时被比较。

在仿真实验中，步长、航向角、UWB的距离测量值被加上了高斯白噪声代表测量误差。为了模拟UWB标签和基站在通信的过程中被介质遮挡而产生的NLOS误差，随机选取一定数量的UWB距离测量值为其加入正向偏差。文献[24]描述了UWB信号被人、大理石柱遮挡时NLOS误差为0.50~3.45 m之间。本文在2组实验中分别模拟了NLOS误差为1.2 m和0.6 m左右的情况。仿真实验中共设有6个UWB基站，在ENU坐标系中的坐标分别为A1=（1 m，1 m），A2=（1 m，17 m），A3=（9 m，17 m），A4=（17 m，17 m），A5=（17 m，1 m），A6=（9 m，1 m）。模拟的行走轨迹是一个正方形，4个顶点坐标分别为（3 m，3 m），（3 m，15 m），（15 m，15 m），（15 m，3 m）。

3.1.2 仿真结果与分析

仿真实验分为以下2种：

1）仿真实验1

此模拟实验为了验证在NLOS误差为1.2 m左右的情况下所提算法的定位性能。实验中模拟了2种情况：第1种情况是基站3、4处于NLOS环境中，第2种情况是基站1、3、4、6处于NLOS环境中。分别给处于NLOS环境中的每个UWB基站的距离测量随机加入50个1.2 m的正向偏差代表NLOS误差。

图 6和图 7分别为2种模拟情况下由不同算法解算出的定位轨迹。从图中可以看出，基于UWB的EKF算法解算出的定位轨迹较差。这是由于定位参考信息中仅包括定位标签到各个基站的距离数据，缺少其他辅助参考信息及检测和缓解NLOS误差的能力。而融合UWB和INS进行定位，加入了惯性测量数据，使得定位参考信息增多，有助于更加精确的定位。但在双级EKF无NLOS检测的情况下，由于UWB的距离测量值包含NLOS误差，UWB和INS简单融合解算出的定位轨迹远远偏离真实轨迹；而使用了NLOS检测的UWB和INS融合算法轨迹较为接近真实轨迹。由此可见NLOS误差对定位结果会产生较大影响。通过对比本文NLOS检测算法和三角形原理检测算法，可以看到两者所形成的轨迹图差别不大。这说明在此情况下，2个算法都可以很好地检测到包含NLOS误差的UWB距离测量值。

图 8为两种模拟情况下各算法的均方根定位误差，通过对比可知，处于NLOS情况下的基站越多，定位误差越大。从定位方法上分析，基于UWB的EKF算法的均方根定位误差高于融合UWB/INS的其他3种定位算法，这说明融合定位要优于单一定位方式。通过比较3种融合算法的均方根定位误差可以看出，有NLOS检测的三角形算法和双级EKF算法要明显优于无NLOS检测的双级EKF算法。

该模拟实验验证了融合定位的优势，说明在NLOS误差较大的情况下，使用NLOS检测算法可以很好地提高融合定位精度。此外，本文算法和三角形原理检测算法在1.2 m NLOS误差情况下的性能接近，且前者略好于后者。

2）仿真实验2

此模拟实验为了验证在NLOS误差为0.6 m左右的情况下所提算法的定位性能。NLOS误差越小，越难检测，误检和漏检的概率也越大。类似实验1，本次实验也模拟了2种情况，分别给处于NLOS环境中的每个UWB基站的距离测量随机加入50个0.6 m的正向偏差代表NLOS误差。

图 9和图 10是由各种算法解算出的定位轨迹，由图可知，基于UWB的EKF算法、无NLOS检测的UWB和INS简单融合算法所形成的轨迹明显偏离真实轨迹，和实验1结论相同。和实验1不同的是，基于三角形原理的NLOS检测算法所形成的轨迹也和真实轨迹有较大出入，大量的误检和漏检导致许多包含NLOS误差的UWB距离测量值被代入坐标运算中，最终导致所得定位轨迹偏离真实轨迹。这说明此算法在NLOS误差较小时检测性能较差。然而，本文所提的NLOS检测算法解算出的定位轨迹依然接近真实轨迹，在NLOS误差较小时仍旧具备良好的检测性能。

如图 11所示，各个算法的均方根定位误差也验证了上述结论，三角形原理检测算法的均方根定位误差甚至大于未包含NLOS检测的一般性融合算法。

该模拟实验验证了当NLOS误差较小且不易检测时，本文所提的NLOS检测算法依然具备良好的检测能力和较强的稳定性。

3.2 实测验证与分析 3.2.1 实验环境布置和设备

本文选取西安邮电大学3号实验楼某实验室进行实地测试，以验证所提算法的实际定位性能。该实验室是一个10 m ×10 m的方形区域，如图 12所示，室内摆放有实验桌、电脑、实验箱、柜子等物品。图中共有4个UWB基站，其中基站A0和A3被放置在室内，基站A1和A2被放置在室外，中间隔着一堵墙，墙的附近还堆积着一些实验箱。墙、实验箱和人体将导致不同程度的NLOS误差。经过多次测试，这些遮挡物将产生0.5~1 m的UWB测距误差。基站A0和基站A3一直处于LOS环境中，而A1和A2则一直处于NLOS环境中。各个UWB基站的坐标为：A0=（9 m，10.8 m），A1=（9 m，0.75 m），A2=（3 m，0.75 m），A3=（3 m，10.8 m）。测试人员沿着室内提前规划好的虚线路线行走，其身上携带的数据采集终端包含2个模块，1个是UWB标签，和UWB基站的结构一样，另外1个是集成MPU9250传感器的JY901惯性导航模块。实验设备如图 13所示，其中UWB基站和标签搭载Decawave公司的DWM1000模块，通信距离可达200 m，在LOS条件下测距误差在10 cm左右。MPU9250是一款9轴运动跟踪传感器，用来获取测试人员行走时的惯性数据。

实验中，设定UWB的数据采集频率是50 Hz，MPU9250的数据采集频率是100 Hz，波特率是115 200 bit/s。测试人员将数据采集终端佩戴在胸前，双手抱着笔记本电脑。传感器获取的数据经过串口传输到PC端，再使用MATLAB软件进行数据处理与算法分析。

3.2.2 结果分析

图 14所示为不同算法解算出的行走轨迹。从图中可以看出，基于UWB的EKF算法、无NLOS检测的UWB和INS简单融合定位的算法性能较差，所形成的轨迹与真实轨迹偏差较大。由于本次实验环境中基站A1和A2的NLOS误差在0.5~1 m范围内波动，基于三角形原理的NLOS检测算法会产生大量的漏检，这将对定位产生极大的影响。图中三角形检测算法的定位轨迹大幅度偏离真实轨迹，这说明使用三角形算法检测NLOS误差并不稳定，和3.1节的仿真实验结论一致。本文所提的双级EKF和NLOS检测算法具备良好的性能，且定位轨迹相比于其他算法更接近真实轨迹。

图 15所示为各算法在1个定位周期内每解算出1个位置坐标所耗费的时间，表 1是对检测实验中各算法的定位精度和实时性的对比。结合图 15和表 1可知，与基于UWB的EKF算法、融合UWB/INS的双级EKF无NLOS检测算法和三角形原理检测算法相比，融合UWB/INS的双级EKF和NLOS检测算法的均方根定位误差分别降低了85.40 %、83.68 %、87.21 %，验证了所提算法具有较高的定位精度。计算时间反映了算法的开销问题，不同计算机可能得到不同的计算时间。4种算法的运行时间大致如图 15和表 1所示，在1个迭代周期中，三角形原理检测算法运行时间最长，其次是双级EKF和NLOS检测算法，接着是双级EKF无NLOS检测算法，而基于UWB的EKF算法的运行时间最短，但总体而言4种算法的运行时间相差不大。综上所述，本文所提算法定位精度相对较高，虽然总运行时间与单一定位方式相比虽略有上升，但是基本可以满足消防员室内实时精确定位的需求。

4 结束语

本文以UWB技术和INS技术为基础，针对消防员在火灾救援时难以精确定位的问题，设计一种双级EKF框架，该框架以松耦合的方式实现UWB和IMU的融合定位。在此基础上，借助INS提出一种用于UWB的NLOS检测算法。当UWB基站和标签之间的通信存在遮挡时，该算法可以很好地检测出非视距情况，并通过计算残差矩阵缓解NLOS误差所带来的干扰。实验结果表明，本文算法具有良好的定位性能和较好的稳定性，为火灾救援时精确定位消防员所在位置提供了一种有效的解决方案。由于在大多数情况下多名消防员会同时进入火场，因此下一步将考虑协同定位机制，将可以准确定位的消防员作为参考点，定位处于定位盲区的消防员，以提升定位系统的覆盖范围。此外，将定位问题从二维平面扩展到三维空间以直观地查看消防员所在的空间位置也是下一步的研究重点。