基于UWB的消防员室内协同定位算法

杨 刚，朱士玲，李 强，赵克松，赵 杰，郭 建

西安邮电大学 通信与信息工程学院，西安710121

建筑物内火灾事故频发，常需消防员深入火场内部展开灭火救援，如果负责协调调度任务的消防指挥人员能够获取消防员的精确位置，那么在消防员自身遭遇险情时，便可通过对讲设备通知邻近消防员施以救助，并为其合理规划逃生路线，降低消防员的伤亡率。基于此，如何实现对消防员的高精度定位是本文的研究重点。

虽然GPS等全球卫星导航系统发展成熟，但由于卫星信号难以穿透建筑物，导致室内定位实现困难。超宽带（ultra-wideband，UWB）作为室内定位领域的新兴技术，与蓝牙、WiFi、RFID、Zigbee 等技术相比，具有穿透能力强、测距精度高、抗多径能力强等优点[1-3]，成为实现消防员室内精确定位的关键技术。文献[4]基于IR-UWB搭建无线传感器网络，利用标签和固定锚点之间的多个测距信息，采用三边定位算法获取消防员所在位置。文献[5]提出将小功率UWB 基站模块安装于智能应急标志灯具中，利用TDOA技术实现对携带定位标签的消防员实时定位。文献[6]提出一种人体运动辅助的UWB室内定位方法，当人体转动时根据UWB信号的TOA和RSS 测量值，判断被困人员所在方位，有效引导救援人员进行施救。以上方法虽然实现简单，但因未考虑UWB 信号穿墙等因素引起的非视距（non-line-of-sight，NLOS）误差，导致定位精度过低。为此部分学者提出了多技术融合的定位方法。文献[7]将UWB 与蓝牙相结合建立目标函数及其几何约束条件，并利用粒子群优化算法估计目标位置。文献[8]提出将UWB 与惯性测量单元相融合，利用最大似然估计法消除NLOS 影响，并通过扩展卡尔曼滤波信息融合策略推算目标轨迹。文献[9]提出将UWB 与WiFi、惯性测量单元相结合，并通过粒子滤波进行数据融合，估算移动目标位置。以上方法虽然在一定程度上提升了定位精度，但融合的定位技术越多，成本越高，计算越复杂。为了避免上述问题，减少对定位基础设施的依赖，将定位群体之间的内在联系考虑在内，在室外环境下，文献[10]提出了一种动态协同定位算法，解决了复杂环境下可见卫星不足导致的动态用户无法定位问题。文献[11]提出了一种利用距离信息进行协同车辆位置估计的方法，提高了车辆的GPS定位精度。在室内环境下，文献[12]提出了一种基于UWB测距的协同定位方法，通过最大限度地利用待定位节点之间的视距（line-of-sight，LOS）链路来提高NLOS 环境中的定位精度。然而，LOS 链路可能被错误地识别为NLOS 链路，反之亦然，这会严重影响定位性能。基于此，文献[13]提出了一种基于半定规划的协作定位算法，缓解了NLOS误差的影响，但在强NLOS环境下，定位性能的改善并不明显。文献[14]提出了一种分布式协同定位方法，为在无特征环境中定位一组机器人提供了解决方案，但每次定位至少需要一个机器人作为临时地标保持静止。文献[15]提出了一种基于扩展卡尔曼滤波的移动群体定位算法，利用移动群体之间的距离信息提升定位性能。虽然这种方法可以相对提高定位精度，但在NLOS 环境下得到的测量值不准确时，目标的估计位置往往偏离实际位置。

本文结合UWB技术的优良特性和协同技术在群体定位中的性能增强特性，提出一种基于UWB 的消防员室内协同定位算法，在待定位消防员群体之间建立通信连接，以降低定位对UWB 基站分布密度高或者通信半径长的要求，最大程度地使用整个协同网络的资源，从而获取更多的定位参考信息，对深入火场内部的消防员因障碍物遮挡导致的定位困难或定位不准问题予以解决。首先，通过线性拟合方式对UWB 测量距离进行预处理，以减小距离中的标准偏差。其次，设计了一种基于偏移扩展卡尔曼滤波的协同定位算法，该算法将待定位目标看作一个整体，每个目标都可作为彼此的移动参考节点辅助定位，以增加定位参考信息，提高定位稳定性。此外，通过判定距离测量残差的正负，对卡尔曼增益做出调整，如果为正，则下调卡尔曼增益以削弱状态预测值往正向修正的程度，如果为负，则上调卡尔曼增益以削弱状态预测值往负向修正的程度，将偏离的位置估计值拉回到正确方向，缓解NLOS 误差，实现目标的初步定位。最后，设计了一种基于阈值筛选的均值滤波算法，如果定位目标在某一时刻的位移超出设定阈值，则将该时刻的定位坐标舍弃，将滤波窗口内其他定位坐标的均值作为该时刻的滤波输出值，以消除定位坐标跳变问题。

1 UWB测距及协同定位原理

1.1 UWB测距原理

通过UWB模块获得可靠的测距信息是保证消防员室内精确定位的前提。图1 为三消息双边双向测距（double-sided two-way ranging，DS-TWR）法的测距原理图，其中RMARKER 是发送/接收时间戳，用于标记UWB 模块每一次发送/接收数据的时刻。模块A 作为测距的发起者在Ta1时刻首次启动消息发送帧，发送一个Poll消息，之后模块B在Tb1时刻接收到该消息，并经过Treply1后在Tb2发送一个Response 消息，以响应该消息发送帧，模块A 在Ta2时刻接收到该响应消息，并经过Treply2后在Ta3发送一个Final消息，请求结束本次测距，模块B在Tb3时刻接收到消息后，整个测距过程结束。最终得到4个时间差数据Tround1、Treply1、Tround2、Treply2。

图1 三消息双边双向测距Fig.1 DS-TWR with 3 messages

从而可计算出UWB信号的飞行时间[16]：

由此可得到模块A 和模块B 之间的距离dAB=c×Tfab，其中c为光速。

在实际环境中，由于UWB脉冲信号是以光速传播，显然信号在传播过程中会受到其他因素影响，导致测距结果不理想，其中NLOS误差是造成定位精度下降的主要原因之一。在k时刻，两个UWB 模块之间的测量距离d(k)可表示为：

其中，r为真实距离值；e为标准测量误差；εNLOS为非视距误差，一般认为它是一个正的随机变量，在不同的信道环境下有可能服从高斯、指数、均匀或δ分布。

为了减小标准测量误差e，可对UWB 测距数据进行一次线性拟合，并利用拟合优度判定系数R2来判断拟合优劣[17]：

其中，y为真实测量距离；Y为拟合距离；R2的取值范围为（0，1），R2越小，拟合得越差，R2越大，拟合得越好。

当UWB 模块之间有障碍物遮挡时，由于NLOS 误差εNLOS的存在，使得测量距离变大，如果不进行消除，会使定位精度明显下降。文中将通过第2 章提出的定位算法间接消除非视距误差，提高定位精度。

1.2 协同定位原理

复杂的火场环境通常会存在定位盲区，仅依靠数量有限的基站进行定位既不能保证定位准确性，又不能保证定位覆盖率。与传统定位方式相比，协同定位充分考虑到消防员团体作战的特性，在定位过程中，基站充当固定参考节点，标签充当彼此的移动参考节点，使用标签到基站、标签到标签的距离信息来估计消防员的位置。

图2 是以三基站为例的定位方式示意图。图2（a）是传统定位方式，图2（b）是协同定位方式，图中A1、A2、A3 代表定位基站，虚线圆代表基站的通信范围，T1、T2、T3 代表待定位标签。图2（a）中待定位标签T1在基站A1、A2、A3的通信范围内，根据传统的三边定位法，实现定位的前提是要知道标签和至少3个已知基站间的距离信息，故T1的位置能唯一确定。而T2和T3分别在两个基站的通信范围内，即只知道标签和两个基站的距离信息，不满足传统三边定位方法的条件，因此T2、T3不能保证被定位。与图2（a）不同的是，图2（b）的T1、T2、T3 能够相互通信测距，一旦T1 的位置确定后，便可作为移动参考节点辅助T2、T3定位。

图2 传统定位方式与协同定位方式示意图Fig.2 Schematic diagram of traditional positioning mode and collaborative positioning mode

2 基于偏移扩展卡尔曼滤波和改进均值滤波的协同定位算法

下面将通过基于偏移扩展卡尔曼滤波和改进均值滤波的协同定位算法对消防员进行定位，并对定位过程中出现的NLOS 误差及定位坐标跳变问题予以缓解。首先建立适合多目标协同场景的状态模型和量测模型；然后根据此模型推导出可缓解NLOS 误差的偏移扩展卡尔曼滤波算法；最后通过分析目标运动的特征，推导出基于阈值筛选的均值滤波算法，改善定位坐标跳变问题。

2.1 系统状态模型

给定一个由M个UWB 基站和N个UWB 标签组成的二维协同定位网络，其中基站是位置已知的固定锚节点，其坐标向量记为xa∈R2,a=1,2,…,M，UWB 标签是待定位的移动目标节点，其坐标向量记为xi∈R2,i=1,2,…,N。假设消防员将标签携带在头盔上，并保持匀速行走，则第i个移动标签在k时刻的状态向量可表示为：

假设标签i在运动过程中受到的随机扰动为ui(k)=[uxi(k),uyi(k)]T,且，可以将系统表示为[18]：

其中，pxi(k)、pyi(k)、vxi(k)、vyi(k)分别代表标签i在k时刻的横坐标、纵坐标、x轴方向的速度分量、y轴方向的速度分量；ΔT为采样时间间隔。根据式（5），可将系统的状态方程表示为：

式中，φ为状态转移矩阵，τ为噪声驱动矩阵，u的协方差为q，定义如下：

其中，和的取值与环境有关，标签i的过程噪声协方差为Qi=τiqiτiT。

基于以上对单个标签的分析，N个相互协同的移动标签在时刻k的状态向量可以表示为：

系统状态方程可表示为：

其中，系统状态转移矩阵Φ可表示为：

系统过程噪声W的协方差矩阵Q可表示为：

式（12）、（13）中的0 表示4阶零矩阵。

2.2 系统量测模型

协同定位所需的距离测量值来自于标签i和基站a之间的测量距离ria，以及标签i和标签j之间的测量距离dij，文中取距离的平方作为观测量，则在k时刻，M个UWB 基站和N个UWB 标签之间可形成维数为的测量向量Z(k)：

式（14）可通过下式求得：

其中，‖ ⋅ ‖表示欧氏距离，xa=(lxa,lya)，xi=(pxi,pyi)，xj=(pxj,pyj) 分别表示基站a(a=1,2,…,M)、标签i,j(i,j=1,2,…,N且i≠j) 在直角坐标系下的真实坐标。假定via、vij分别表示标签i和基站a、标签j之间的距离测量噪声，且，δR的取值由UWB 模块的测量精度决定，则系统的量测方程可以表示为：

其中，V～N(0,R)，R为测量噪声协方差矩阵：

通过泰勒级数展开可将非线性量测方程（17）转化为线性量测方程，即：

2.3 偏移扩展卡尔曼滤波算法

扩展卡尔曼滤波算法的基本思想就在于利用2.1和2.2 节构造的状态模型和量测模型，通过预测和更新两个阶段把定位目标的状态值与当前时刻的测量值和状态预测值联系起来，从而估计出目标的位置，具体步骤如下：

在预测阶段，根据移动目标群组k-1 时刻的状态和误差协方差预测其在k时刻的状态和误差协方差：

然后根据式（20）得到的预测状态计算出系统在k时刻的观测矩阵和测量残差：

再由式（21）、（22）计算出k时刻的卡尔曼增益：

在更新阶段，由式（20）～（24）可计算出目标在k时刻的状态估计和误差协方差矩阵：

扩展卡尔曼滤波算法对测量值的准确度要求较高，在视距环境下还能表现出优良的跟踪性能[19]，一旦UWB信号受到障碍物遮挡，UWB 模块间的距离测量值就会产生较大的正向偏移，此时将距离值直接代入该算法必然会降低定位精度。为此文中将常规扩展卡尔曼滤波算法改进为偏移扩展卡尔曼滤波算法，通过调整卡尔曼增益把偏离的状态估计值拉回到正确的方向上[20]。具体做法是在式（24）和（25）之间加入如下公式：

ξ的推导过程如下：

在式（23）中，测量残差向量中的元素可表示为yres(k)=[yres1(k),yres2(k),…,yresi(k),…,yresn(k)]T,i=1,2,…,n,n为总的测量值个数，则：

其中0<α<1，β≥1。

在式（28）中，先对元素yresi进行符号判断，如果yresi>0，代表状态估计值会大于状态预测值，可通过系数α上调卡尔曼增益以削弱预测值往正向修正的程度，如果yresi<0，代表状态估计值会小于状态预测值，可通过系数β下调卡尔曼增益以削弱预测值往负向修正的程度；然后将得到的n个增益系数构造成式（29）所示的系数矩阵ξ；再通过式（27）用新的卡尔曼增益代替原来的卡尔曼增益，把估计值往正确的方向修正，从而达到缓解非视距误差的目的。另外，式（28）中α和β的取值与实验环境密切相关，且关系到定位算法的定位性能。

2.4 改进均值滤波算法

均值滤波算法是一种依据图像局部统计信息对图像进行滤波的方法，可有效消除图像中的高斯椒盐混合噪声[21]。考虑到火场环境中测量距离受到随机因素影响产生的随机误差与图像的噪声特点有相似之处，故提出用均值滤波算法对基于偏移扩展卡尔曼滤波的协同定位算法得到的定位结果进行二次优化，以达到进一步缓解定位坐标跳变问题，使定位结果更加平滑，更加逼近真实值。

经典均值滤波的基本原理为：

其中，x(k)为滤波中心点，k=1,2,…,T;(k)为经过均值滤波后的结果；L=2l+1 为均值滤波窗口的大小。经典均值滤波本质上是在滤波窗口范围内，将随机噪声产生的误差由滤波窗口内的各个点均分承担，使数据变得相对平滑，这对抑制误差具有一定效果，但是并不能真正消除误差。

考虑到正常情况下人员的行走状态，移动目标节点i从k-1 时刻到k时刻的位移应相对稳定，即：

其中，xi(k-1)、xi(k)为目标节点在k时刻、k-1 时刻的定位坐标；Uth为位移阈值上限。

如果出现位移较大的情况，很有可能该时刻的定位结果受到了随机噪声的影响。为此，本文提出基于阈值筛选的改进均值滤波算法，以提高移动目标节点的定位精度。具体做法是当k-1时刻到k时刻的位移满足式（31）时，对基于偏移扩展卡尔曼滤波的协同定位算法得出的定位坐标直接进行均值滤波处理；当不满足式（31）时，将k时刻的定位坐标舍弃，将窗口内其他定位坐标的均值作为k时刻目标节点i的滤波输出值。改进算法公式表示如下：

2.5 算法流程

基于偏移扩展卡尔曼滤波和改进均值滤波的协同定位算法通过不断的循环迭代，实时估计移动目标节点位置，具体算法分为两部分：

第一部分是根据偏移扩展卡尔曼滤波算法初步估计移动目标的位置坐标。由2.3 节式（20）、（21）首先预测出目标节点在k时刻的状态和误差协方差；然后根据系统观测矩阵计算出卡尔曼增益，并通过系数矩阵调整卡尔曼增益；在更新阶段便可根据当前的状态预测值、卡尔曼增益、测量残差估计出目标在k时刻的最佳状态和误差协方差矩阵，并为下一次状态预测和更新做准备。

第二部分是二次优化移动目标的位置坐标。在初步估计出一个时刻的移动目标状态后，累计一个滤波窗口长度，当累计窗口长度L未达到预设要求时，返回第一部分进行下一次状态估计，直至达到要求，就可以进入基于阈值筛选的改进均值滤波算法环节。假如滤波窗口长度达到要求的时刻是k，那么需要判断移动目标节点从k′-1 到k′时刻(k′=k-(L-1)/2)的位移是否在预设的阈值范围内。然后根据2.4节式（32）进行相应的滤波处理，处理完成后，将滤波窗口中数据队列的队首去掉。此时窗口长度已不满足要求，如果定位时间未到，那么再次通过窗口长度判断，返回第一部分估计出最新时刻的移动目标节点位置，将其加入到队尾，周而复始，达到滑动滤波的效果。详细算法流程如图3所示。

图3 定位算法流程图Fig.3 Flow chart of positioning algorithm

3 实验及分析

3.1 仿真实验及结果分析

3.1.1 仿真实验配置

图4为仿真实验的定位场景示意图，定位区域是一个20 m×26 m 的长方形区域，4 个UWB 基站A0～A3 分别位于长方形区域的4个角，其平面坐标分别是(-1,-1)、(19,-1)、(19,25)、(-1,25)。UWB 基站和标签的通信半径为35 m，5个标签T0～T4分别按照预先设计好的L形、三角形、矩形、蝶形、圆弧与直线相混合的路线从坐标原点（0，0）开始移动。其中标签T0 模拟消防员在行走过程中突然静止不动，其他标签模拟消防员正常行走。假设在该场景下，所有节点都是连通的，那么基站到标签、标签到标签之间总共有30条通信路径，且基站2和基站3处于非视距状态，所有和这两个基站连通的路径都属于非视距路径，总共有10条。为了简便起见，仿真实验中使用的UWB测距值是在真实距离值上添加噪声得到的。其中视距路径下的测距值是在真实距离值上添加了均值为0，方差为0.4的高斯白噪声，非视距路径下的测距值在真实距离值上除添加了高斯白噪声之外，还添加了0.5～1.5 m 的随机噪声，仿真采样时间间隔ΔT为1 s，总的采样点数为120个，5个标签的初始状态都设为[0,0,0,0]T，即，初始误差协方差矩阵P(0)=10-6I20×20,过程噪声协方差矩阵Q中的Q1=，测量噪声协方差矩阵R=0.2I30×30,I为单位矩阵，增益系数α=0.2，β=1，滑动均值滤波窗口长度L设为3。

图4 定位场景示意图Fig.4 Schematic diagram of positioning scene

3.1.2 仿真结果及分析

图5展示的是5个定位标签T0～T4在4种定位算法下解算出的行走轨迹，红线代表真实轨迹，绿线代表不同算法解算出的行走轨迹。其中TRI、EKF分别是基于传统固定基站的三边定位算法、扩展卡尔曼滤波算法，CO-EKF 是基于扩展卡尔曼滤波的协同定位算法，COBEKF-IMF 是本文提出的基于偏移扩展卡尔曼滤波和改进均值滤波的协同定位算法。

图5 各标签的定位轨迹Fig.5 Positioning trajectory of each tag

通过对比发现，TRI算法解算出的定位坐标波动明显，与真实轨迹偏差较大，说明该算法的抗噪声能力较差；EKF 算法在解算当前时刻的位置坐标时，利用了前一时刻的定位信息和当前时刻的测量信息、预测信息，使得前后时刻的定位联系更加紧密，减少了定位突变点的产生，但与真实轨迹仍有差距；CO-EKF 算法在EKF的基础上，将待定位标签T0～T4 看作一个整体，充分利用了UWB 基站和标签之间、标签和标签之间的测距信息，最大程度地使用了整个协同网络的定位资源，使得定位参考信息增多，定位结果得到进一步改善，但从图中可以看出仍未达到理想水平；本文提出的CO-BEKFIMF 算法在CO-EKF 算法的基础上考虑了测量距离中非视距误差的特性，根据测量信息和预测信息的大小，通过系数矩阵调整卡尔曼增益，有效抑制了非视距误差对定位结果的影响，实现了初步定位，为了使定位轨迹更加平滑，又通过基于阈值筛选的改进均值滤波算法，丢弃异常偏差值，对定位坐标做了二次优化，从图中可以看出，所提算法解算出的定位轨迹最贴近真实轨迹。

图6给出了不同算法下各标签的均方根定位误差，CO-BEKF-IMF算法与TRI、EKF、CO-EKF算法相比，分别降低了47.4%～57.7%、38.6%～48.4%、27.6%～38.9%，证实了所提算法在多节点协同、偏移扩展卡尔曼滤波和改进均值滤波的相互作用下，可有效抑制噪声对定位结果带来的不良影响，定位精度得到了提升。

图6 标签与基站正常通信下各算法的均方根定位误差Fig.6 Root mean square positioning error of each algorithm under normal communication between tags and base stations

图7是各算法下5个定位标签的平均定位误差随非视距误差增大的变化情况。从图中可以看出，非视距误差越大，4种算法的平均定位误差也越来越大，这是由各节点间的距离信息准确度不断下降造成的，但相比较而言，CO-BEKF-IMF 表现出了更好的抗非视距误差性能。在0～1.0 m的非视距误差下，与TRI、EKF、CO-EKF相比，CO-BEKF-IMF 算法的平均定位误差分别降低了55.1%、41.5%、31.7%；在0.5～1.5 m 的非视距误差下，分别降低了54.2%、44.7%、36%；在1.0～2.0 m 的非视距误差下，分别降低了51.8%、44.2%、36.2%；在1.5～2.5 m 的非视距误差下，分别降低了50.3%、43.4%、36.4%；在2.0～3.0 m 的非视距误差下，分别降低了47.9%、42.9%、34.5%。

图7 不同非视距误差下的平均定位误差Fig.7 Average positioning error under different NLOS error

图8 是各算法的平均定位误差随标签数量增加的变化情况。通过对比发现，TRI和EKF算法的平均定位误差较大，但波动较小，原因是这两种算法只利用了基站到标签之间的测距信息，标签数量的增加对定位结果基本没有产生影响。CO-EKF和CO-BEKF-IMF算法随着标签数量的增加，平均定位误差逐渐减小，且COBEKF-IMF算法的定位误差更小。从图中还可以看出，当标签数量增加到10 个时，所提算法的平均误差基本趋于稳定，说明参与协同定位的标签增加到一定数量时，由于可参考定位信息足以获得最佳定位性能，再增加标签数量基本不会影响定位结果。

图8 不同标签数量下的平均定位误差Fig.8 Average positioning error under different number of positioning tags

为了更进一步说明所提算法的抗非视距性能，图9展示了各种算法的平均定位误差随非视距路径增加的变化情况。从图中可以看出，随着非视距路径的增加，TRI、EKF算法的平均定位误差呈不断上升趋势，但因为两种算法不涉及标签和标签通信测距，所以在非视距路径数达到20 条后，再增加标签到标签之间的非视距路径对它们没有影响；CO-EKF算法虽与前两种算法相比定位误差有所下降，但并未很好地抑制非视距误差；CO-BEKF-IMF算法的平均定位误差与其他3种算法相比大幅下降，有效抑制了非视距误差，定位稳定性更好。

图9 不同非视距路径下的平均定位误差Fig.9 Average positioning error under different NLOS paths

以上从多个角度证明了如果T0～T4在4个UWB基站的通信范围内，通过CO-BEKF-IMF算法，可使T0～T4的定位精度得到有效提高。但在实际环境中，定位标签不一定在所有基站的通信范围内，假如T3 超出了A1、A2 的通信范围，在基站A0、A3 的左侧从(-16,-1) 到(-16,14)做往返直线运动；T4 超出了A2、A3 的通信范围，在基站A0、A1的下侧从(-1,-10)到(8,-10)做往返直线运动，此时就需要通过两阶段CO-BEKF-IMF 算法才能解算出T3、T4的位置。首先根据T0、T1、T2到4个固定基站的UWB 测距值以及它们相互之间的距离信息，利用CO-BEKF-IMF 算法解算出T0、T1、T2 的精确位置；然后选择能和T3、T4 可靠通信的T0 作为虚拟信标，再根据T3到T0、固定基站A0、A3的距离，T4到T0、固定基站A0、A1的距离，以及T3、T4之间的距离，再次利用CO-BEKF-IMF算法便可解算出T3、T4的位置。

图10是几种算法的均方根定位误差对比图。由于标签T3、T4分别只能和两个固定基站通信测距，传统的基于固定基站的TRI算法已无法适用；EKF算法虽能解算出T3、T4 的坐标，但是产生的定位误差较大；采用CO-EKF算法虽然可以缓解这一问题，但由于已定位的T0存在的定位误差较大，在升级为虚拟信标协助T3、T4定位时，会将误差代入到定位算法中，导致解算出的坐标误差也较大；两阶段CO-BEKF-IMF 算法在第一阶段已将T0 的误差降低到较小的程度，因此在第二阶段由T0协助定位T3、T4时产生的误差也较小，与EKF、CO-EKF算法相比，分别降低了39.1%～44.6%、32.4%～33.8%。

图10 标签与基站非正常通信下各算法的均方根定位误差Fig.10 Root mean square positioning error of each algorithm under abnormal communication between tags and base stations

图11 对比了T3、T4 在带有和不带误差的虚拟信标T0 协助下，采用CO-BEKF-IMF 算法解算出的行走轨迹，以说明虚拟信标的定位精度对被定位目标的影响程度。图中T0、T1、T2的行走轨迹与前文保持一致。因为有误差的T0坐标是通过CO-BEKF-IMF算法得到的，所以与无误差的T0 相比，解算出的T3、T4 的行走轨迹略差一些。

图11 虚拟信标有无误差下的定位轨迹Fig.11 Positioning trajectory with or without error of virtual beacon

图12展示了T3、T4在各采样点处坐标估计值与真实值的绝对偏差。从整体来看，有误差的T0 要比无误差的T0 协助定位时产生的定位误差大，但并不是随着定位时间的增加而逐渐增大的，这说明虚拟信标的加入并未导致误差累计。

图12 绝对定位误差Fig.12 Absolute positioning error

表1 给出了虚拟信标有无误差两种情况下所提算法的均方根定位误差变化情况。与无误差的虚拟信标相比，有误差信标协助定位T3、T4时产生的定位误差分别增长了9.40%、3.03%，表明虚拟信标的位置越精确，协助定位其他目标时产生的定位误差越小。文中所提两阶段CO-BEKF-IMF算法已在第一阶段改善了虚拟信标的定位精度，使其在第二阶段参与运算时，不会代入较大的误差。与文中其他算法相比，所提算法不仅提高了目标的定位精度，而且扩大了定位覆盖范围，同时也提高了协同网络资源的利用率。

表1 虚拟信标对定位精度的影响分析Table 1 Influence of virtual beacon on positioning accuracy

3.2 实测实验及结果分析

3.2.1 实验设备和环境配置

为了验证所提方法在实际环境中的可行性和有效性，使用研创物联网科技有限公司开发的UWB Mini4splus 模块进行数据采集，其中的UWB 收发模组DWM1000是基于Decawave公司DW1000芯片设计的，主控芯片采用STM32F103T8U6单片机。通过keil软件的二次开发，实现了基站与标签、标签与标签之间的通信测距功能，且基站A0 作为汇聚节点通过串口与计算机相连，负责将所有测试数据汇总到一起。在实验中，UWB模块工作电压为5 V，数据采样频率设置为50 Hz，波特率设置为921 600 bit/s。实验场地为一间9 m×7 m×4 m的仓库，仓库内部布局如图13所示。

图13 实验环境布局Fig.13 Layout of experimental environment

实验分为两组：第一组实验中4 个UWB 基站A0～A3通过支架放置在室内4个角落，其平面坐标分别是（0，0）、（0，8）、（6，8）、（6，0）。4 名测试人员将4 个UWB 标签T0～T3 分别佩戴在头上，根据提前规划好的矩形路线分别以矩形的4个角为起始点，按逆时针方向同时行走，其中矩形大小为7.0 m×4.5 m。第二组实验中将第一组实验的UWB基站A0、A3平移1 m放置在室外，此时A0 的坐标调整为(0,-1)，A3 的坐标调整为(6,-1)，其他条件保持不变。从图13 中可以看出，测试人员在行走过程中，UWB 信号会受人体、柱子、仓储物品、墙壁、铁门等的影响。第一组实验属于弱非视距环境，第二组实验属于强非视距环境。为了便于数据分析，在Matlab 软件上根据汇总的测试数据，使用不同的定位算法解算出测试人员的行走轨迹，通过对比分析验证所提算法的定位性能。实验的参数配置与3.1节仿真实验一致。

3.2.2 UWB测距值校准

由于UWB模块自身的精度有限以及环境因素的影响，导致测量数据与真实数据存在偏差，为了准确解算出测试人员的位置坐标，在使用定位算法前需对测量值进行校准，以消除UWB模块的标准测量偏差。

在室内无障碍物遮挡的视距环境下，将UWB 标签T0与计算机相连固定在一个位置，UWB标签T1相对于T0 沿直线每0.5 m 移动一次，移动至10 m，总共20 个测距点，每个测距点采样50 个距离数据，用3σ准则剔除粗大数据后，取剩余数据的平均值作为该点的测量距离，与真实距离进行线性拟合，拟合函数为：

其中，x是测量距离，y是拟合距离。由式（3）可得出拟合优度的判定系数R2=0.999 7。由于本实验中所有的UWB 模块具有相同的结构，故测量距离都可以根据上述拟合函数进行校准。图14 为UWB 标准偏差拟合曲线。从图中可以看出，测量距离与真实距离之间达到了较为理想的拟合效果。

图14 UWB标准偏差拟合曲线Fig.14 UWB standard deviation fitting curve

3.2.3 实测结果及分析

实验1UWB基站A0～A3在室内

因为4名测试人员都是沿着矩形路线行走，且解算出的行走轨迹基本相同，所以只给出图15 所示的标签T1的定位轨迹。图中红色方块代表UWB基站，红线代表真实轨迹，绿线代表不同算法解算出的行走轨迹。测试人员在行走过程中，因UWB信号受到柱子、仓储物品和人体的阻挡，使得UWB 模块的发射和接收信号时间延长，导致模块间的测量距离加大。与仿真实验结果相一致，直接使用传统的TRI算法解算出的行走轨迹明显偏离了真实轨迹，定位效果较差；传统的EKF算法虽减少了定位突变点的产生，但仍然与真实轨迹存在差距；CO-EKF算法使定位结果得到进一步改善，但仍未达到理想水平；CO-BEKF-IMF 算法解算出的行走轨迹与其他3种算法相比，在轨迹的平滑度以及与真实轨迹的贴合度上达到了最佳水平。

图15 T1的定位轨迹（实验1）Fig.15 Positioning trajectory of T1（Experiment 1）

图16 为定位标签T0～T3 在4 种定位算法下的均方根定位误差，CO-BEKF-IMF 算法与TRI、EKF、CO-EKF算法相比，分别降低了22.0%～49.0%、18.4%～20.2%、

图16 弱非视距环境下各算法的均方根定位误差Fig.16 Root mean square positioning error of each algorithm in weak NLOS environment

图18 为定位标签T0～T3 在4 种定位算法下的均方根定位误差，CO-BEKF-IMF 算法与TRI、EKF、CO-EKF算法相比，分别降低了34.0%～48.8%、31.2%～42.3%、19.6%～31.5%，其定位精度在0.28～0.34 m 之间，优于其他3种算法，且与实验1的弱非视距环境相比，在强非视距环境下，所提算法表现出了更加显著的定位性能。

图18 强非视距环境下各算法的均方根定位误差Fig.18 Root mean square positioning error of each algorithm in strong NLOS environment

4 结束语

为了准确获取消防员在复杂火灾环境中的位置，提出了一种基于UWB 的消防员室内协同定位算法，较传统定位方法的改进之处为每个消防员携带的定位标签可以作为其他标签的移动参考节点，并且它们自身的定位信息以及相互之间的距离信息都可以用来辅助定12.4%～16.5%，其定位精度在0.17～0.26 m 之间，在弱非视距环境下能达到比其他3种算法更优的定位效果。

实验2UWB基站A0、A3在室外，A1、A2在室内

图17 展示了在强非视距环境下由4 种定位算法解算出的标签T1的定位轨迹。此环境下，UWB信号不仅受到柱子、仓储物品和人体的遮挡，还受到了墙壁和铁门的遮挡。与实验1的分析类似，TRI、EKF、CO-EKF算法解算出的定位轨迹与真实轨迹相比逐渐改善，但由障碍物遮挡而导致的轨迹偏移问题没有得到有效解决。而由本文提出的CO-BEKF-IMF 算法改善了这一问题，解算出的定位轨迹与真实轨迹更加贴近。位。通过对超宽带测量距离进行线性拟合，减小了距离中存在的标准偏差；通过偏移扩展卡尔曼滤波算法，缓解了障碍物遮挡引起的NLOS误差，实现了目标的初步定位；通过改进的均值滤波算法，进一步优化了定位坐标，使定位轨迹更加平滑，更接近真实轨迹。仿真和实测实验一致验证了所提算法的良好定位性能。在弱NLOS 环境下，所提算法的定位精度可达0.17～0.26 m，在强NLOS 环境下，定位精度可达0.28～0.34 m，明显优于其他定位算法。本文提出的算法为消防员的高精度定位提供了一种有效的解决方案。在未来的研究中，将考虑使用无人机搭载UWB 基站，使其悬停至建筑物四周，以减少基站的部署时间，适应真实的火灾环境。

图17 T1的定位轨迹（实验2）Fig.17 Positioning trajectory of T1（Experiment 2）