一种火场消防员协同接力定位方法与流程

1.本发明涉及室内定位技术领域，特别是涉及一种火场消防员协同接力定位方法及系统。


背景技术：

2.建筑物内火灾事故频发，常需消防员深入火场内部展开灭火救援，如果没有一种良好的消防员定位方法相辅助，那么负责协调调度任务的消防指挥人员因对消防员所在位置及运动状态不了解，在消防员自身遭遇险情时，就无法通知邻近消防员对被困同伴进行施救，也无法为其规划合理的逃生路线，从而错失自救以及营救时机，导致消防员的伤亡率不断攀升。因此，消防员定位问题已然成为现阶段亟待解决的热点问题。
3.现有的消防员定位方法均是对定位基础设施覆盖范围内的消防员进行定位，未考虑某些消防员因处于定位盲区而造成的定位失效问题，导致消防员的定位率不高；由于定位方式比较单一，导致消防员的定位精度有限；定位基础设施常需提前在建筑物内布设，灵活性和实用性不够。
4.因此，本领域亟需一种利用多种参考节点准确定位的协同接力定位方案。


技术实现要素：

5.本发明的目的是提供一种火场消防员协同接力定位方法及系统，利用多重参考节点进行层层定位，有效解决了定位基础设施不足的问题。
6.为实现上述目的，本发明提供了如下方案：
7.一种火场消防员协同接力定位方法，所述方法包括：
8.将处于三个及以上数量的uwb基站范围内的所有标签设置为第一优先级标签，将处于三个以下数量的uwb基站范围内的标签设置为其余优先级标签；所述标签由消防员随身携带；
9.利用融合uwb/ins的定位算法定位第一优先级标签；
10.将定位后的所述第一优先级标签与能够覆盖所述其余优先级标签的uwb基站组合成参考节点，利用基于uwb的协同定位算法定位所述其余优先级标签。
11.在一些实施例中，所述将定位后的所述第一优先级标签与能够覆盖所述其余优先级标签的uwb基站组合成参考节点，利用基于uwb的协同定位算法定位所述其余优先级标签，具体包括：
12.确定与待定位标签距离最近的第一已定位标签；所述待定位标签为所述其余优先级标签中的一个；
13.确定与所述第一已定位标签的距离在设定范围内的第二已定位标签；所述第一已定位标签和所述第二已定位标签均为定位后的第一优先级标签；
14.以所述第一已定位标签、所述第二已定位标签以及能够覆盖所述其余优先级标签的基站为参考节点，采用基于uwb的协同定位算法确定所述其余优先级标签的位置。
15.在一些实施例中，所述利用融合uwb/ins的定位算法定位第一优先级标签，具体包括：
16.获取佩戴标签的消防员的定位数据；所述定位数据包括：坐标数据、步数、步长和航向角；
17.根据所述定位数据利用ekf和pdr算法计算标签的初始位置估计坐标；
18.根据所述标签的初始位置估计坐标计算所述标签到每个uwb基站的距离，得到距离计算值；
19.计算所述距离计算值与所述标签到每个uwb基站的距离测量值的残差，得到残差矩阵；所述距离测量值使用uwb定位设备直接测量得到；
20.根据所述定位数据、所述距离测量值以及所述残差矩阵，通过ekf计算标签的联合位置估计坐标。
21.在一些实施例中，所述根据所述定位数据利用ekf和pdr算法计算标签的初始位置估计坐标，具体包括：
22.根据所述定位数据得到ekf的状态变量为：x＝[e
k
‑1,n
k
‑1,s
k
‑1,θ
k
‑1]
t
；其中e
k
‑1和n
k
‑1分别为第k
‑
1时刻消防员的东向和北向位置坐标值，s
k
‑1为第k
‑
1时刻的步长，θ
k
‑1为第k
‑
1时刻的航向角；
[0023]
根据所述定位数据利用pdr算法得到第一级状态方程：其中，e
k
和n
k
分别为第k时刻消防员的东向和北向位置坐标值，s
k
为第k时刻的步长，θ
k
为第k时刻的航向角，ω
e
和ω
n
分别为消防员的东向和北向位置坐标e和n的过程噪声，服从数学期望为0、方差为和的正态分布，记为ω
s
为步长值s的过程噪声，服从数学期望为0、方差为的正态分布，记为ω
θ
为航向角θ的过程噪声，服从数学期望为0、方差为的正态分布，记为
[0024]
根据所述定位数据得到第一级观测量为：z
ins
＝[s
ins
,θ
ins
]
t
；其中，s
ins
和θ
ins
分别为消防员每一步的步长和航向角；
[0025]
根据所述定位数据得到第一级观测方程为：其中，和分别为步长s和航向角θ的观测噪声，服从数学期望为0、方差为和的正态分布，记为
[0026]
通过ekf的预测过程和更新过程得到标签的初始位置估计坐标。
[0027]
在一些实施例中，所述计算所述距离计算值与所述标签到每个uwb基站的距离测量值的残差，得到残差矩阵，具体包括：
[0028]
将所述距离计算值与所述标签到每个uwb基站的距离测量值的差值与设定的差值阈值比较；若所述差值小于或等于所述差值阈值，则判定所述距离测量值在正常的误差范围内，若所述差值大于所述差值阈值，则判定所述距离测量值异常；
[0029]
计算异常距离测量值的残差：residue
m,k
＝λ
·
(||d
um,k
‑
d
insm,k
||
‑
threshold)，其中，λ为残差系数，d
um,k
为距离测量值，d
insm,k
为距离计算值，threshold为设定的差值阈值；
[0030]
将正常距离测量值的残差设置为1，利用所有残差值构成残差矩阵。
[0031]
在一些实施例中，所述根据所述定位数据、所述距离测量值以及所述残差矩阵，通过ekf计算标签的联合位置估计坐标，具体包括：
[0032]
根据所述定位数据得到ekf的状态变量为：x＝[e
k
‑1,n
k
‑1,s
k
‑1,θ
k
‑1]
t
；其中e
k
‑1和n
k
‑1分别为第k
‑
1时刻消防员的东向和北向位置坐标值，s
k
‑1为第k
‑
1时刻的步长，θ
k
‑1为第k
‑
1时刻的航向角；
[0033]
根据所述定位数据利用pdr算法得到第二级状态方程：其中，e
k
和n
k
分别为第k时刻消防员的东向和北向位置坐标值，s
k
为第k时刻的步长，θ
k
为第k时刻的航向角，ω
e
和ω
n
分别为消防员的东向和北向位置坐标e和n的过程噪声，服从数学期望为0、方差为和的正态分布，记为ω
s
为步长值s的过程噪声，服从数学期望为0、方差为的正态分布，记为ω
θ
为航向角θ的过程噪声，服从数学期望为0、方差为的正态分布，记为
[0034]
根据所述距离测量值得到第二级观测量为：z
fusion
＝[z
uwb
,z
ins
]
t
，其中z
uwb
＝[d
u1
,...,d
um
,...,d
um
]，d
um
是定位标签到第m(m＝1,2,...,m)个uwb基站的距离测量值，z
ins
为第一级观测量；
[0035]
观测噪声的协方差矩阵为：其中r
uwb
为uwb的观测噪声协方差矩阵，r
ins
为ins的观测噪声协方差矩阵；
[0036]
将残差矩阵乘以所述观测噪声的协方差矩阵，得到调整观测噪声协方差矩阵；
[0037]
通过ekf的预测过程和更新过程得到标签的联合位置估计坐标。
[0038]
在一些实施例中，所述以所述第一已定位标签、所述第二已定位标签以及能够覆盖所述其余优先级标签的基站为参考节点，采用基于uwb的协同定位算法确定所述其余优先级标签的位置，具体包括：
[0039]
根据其余优先级标签的运动状态，建立多目标状态方程，并根据所述其余优先级标签与所述参考节点之间、所述其余优先级标签彼此之间的测量距离，建立多目标测量方程；
[0040]
根据所述多目标状态方程和所述多目标测量方程，利用基于偏移扩展卡尔曼滤波
的协同定位算法估算所述其余优先级标签的初始位置；
[0041]
利用基于阈值筛选的均值滤波算法对所述其余优先级标签的初始位置进行优化。
[0042]
在一些实施例中，所述根据其余优先级标签的运动状态，建立多目标状态方程，并根据所述其余优先级标签与所述参考节点之间、所述其余优先级标签彼此之间的测量距离，建立多目标测量方程，具体包括：
[0043]
利用m个参考节点和n个标签组成二维协同定位小组，参考节点的坐标向量记为x
a
∈r2,a＝1,2,...,m，标签的坐标向量记为x
i
∈r2,i＝1,2,...,n，则第i个标签在k时刻的状态向量表示为其中，分别代表标签i在k时刻的横坐标、纵坐标、x轴方向的速度分量、y轴方向的速度分量；
[0044]
n个相互协同的标签在时刻k的状态向量表示为：s(k)＝[s1(k),s2(k),...,s
n
(k)]
t
；其中，s
n
(k)表示第n个相互协同的标签在时刻k的状态向量；
[0045]
以距离的平方作为观测量，则在k时刻，m个参考节点和n个标签之间形成维数为的测量向量z(k)：z(k)＝[r
112
(k),...,r
ia2
(k),...,r
nm2
(k),d
122
(k),...,d
ij2
(k),...,d
(n
‑
1)n2
(k)]
t
；其中，；其中，||
·
||表示欧氏距离，||表示欧氏距离，分别表示参考节点a(a＝1,2,...,m)、标签i，j(i,j＝1,2,...,n且i≠j)在直角坐标系下的真实坐标，v
ia
，v
ij
分别表示标签i和参考节点a、标签j之间的距离测量噪声，且δ
r
的取值由uwb模块的测量精度决定；
[0046]
利用泰勒级数展开所述测量向量，得到多目标测量方程为：z(k)＝h(k)s(k)+v(k)；其中，v(k)为测量噪声，v～n(0,r)，r为测量噪声协方差矩阵。
[0047]
在一些实施例中，所述根据所述多目标状态方程和所述多目标测量方程，利用基于偏移扩展卡尔曼滤波的协同定位算法估算所述其余优先级标签的初始位置，具体包括：
[0048]
根据所述n个相互协同的标签在k
‑
1时刻的状态和误差协方差p(k
‑
1)预测对应标签在k时刻的状态和误差协方差p(k|k
‑
1)：p(k|k
‑
1)＝φ(k)p(k
‑
1)φ
t
(k)+q；其中，φ表示系统状态转移矩阵，q表示系统过程噪声的协方差矩阵；
[0049]
根据预测得到的状态和误差协方差计算k时刻的观测矩阵和测量残差向量；
[0050]
根据所述观测矩阵计算k时刻的卡尔曼增益；
[0051]
将所述残差向量表示为：y
res
(k)＝[y
res1
(k),...,y
resi
(k),...,y
resn
(k)]
t
，i＝1,2,...,n；其中，y
resi
(k)表示所述残差向量中的第i个元素，n表示元素的总个数；
[0052]
根据所述残差向量中元素的值的正负，构造调整系数：其中，0
＜α＜1，β≥1；
[0053]
利用所述调整系数构造调整矩阵：
[0054]
将所述调整矩阵与所述卡尔曼增益相乘，得到调整后的卡尔曼增益；
[0055]
利用调整后的卡尔曼增益计算所述n个相互协同的标签在k时刻的状态和误差协方差矩阵。
[0056]
在本发明的另一方面，还提供了一种火场消防员协同接力定位系统，所述系统包括：
[0057]
优先级确定模块，用于将处于三个及以上数量的uwb基站范围内的标签设置为第一优先级标签，将处于三个以下数量的uwb基站范围内的标签设置为其余优先级标签；所述标签由消防员随身携带；
[0058]
第一优先级标签定位模块，用于利用融合uwb/ins的定位算法定位第一优先级标签；
[0059]
其余优先级标签定位模块，用于将定位后的所述第一优先级标签与能够覆盖所述其余优先级标签的uwb基站组合成参考节点，利用基于uwb的协同定位算法定位所述其余优先级标签。
[0060]
根据本发明提供的具体实施例，本发明公开了以下技术效果：
[0061]
本发明涉及一种火场消防员协同接力定位方法及系统。按照待定位标签能通信的uwb基站数目，先将其分为多种优先级，再进行针对性定位。针对第一优先级标签，将uwb和ins相结合，在uwb基站充足但却多半处于nlos环境的情况下，削弱了uwb测距值中的nlos误差，提高了第一优先级标签的定位精度。然后将已经定位的标签作为移动锚点，与对应的uwb基站组合成参考节点，针对其余优先级标签，根据其周围分布的uwb基站和移动锚点情况，采用基于uwb的协同定位算法，以分组定位的方式对待定位标签进行接力定位，提高了其余优先级标签的定位率。
附图说明
[0062]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0063]
图1为本发明实施例提供的火场消防员定位方法示意图。
[0064]
图2为本发明实施例提供的火场消防员协同接力定位方法流程图。
[0065]
图3为本发明实施例提供的火场消防员协同接力定位方法的详细步骤流程图。
[0066]
图4为本发明实施例提供的火场消防员协同接力定位方法的定位优先级设置示意图。
[0067]
图5为本发明实施例提供的火场消防员协同接力定位方法的第一优先级标签的定位算法流程图。
[0068]
图6为本发明实施例提供的火场消防员协同接力定位方法的定位分组图。
[0069]
图7为本发明实施例提供的火场消防员协同接力定位方法的移动锚点选举算法示意图。
[0070]
图8为本发明实施例提供的火场消防员协同接力定位方法的移动锚点选举算法流程图。
[0071]
图9为本发明实施例提供的火场消防员协同接力定位方法的基于uwb的协同定位算法流程图。
[0072]
图10为本发明实施例提供的火场消防员协同接力定位方法的第二、三优先级标签的定位算法流程图。
[0073]
图11为本发明实施例提供的火场消防员协同接力定位方法的定位场景示意图。
[0074]
图12为本发明实施例提供的火场消防员协同接力定位方法的参与定位的移动锚点数示意图。
[0075]
图13为本发明实施例提供的火场消防员协同接力定位方法的定位算法运行时间比较图。
[0076]
图14为本发明实施例提供的火场消防员协同接力定位方法的定位率与定位标签关系图。
[0077]
图15为本发明实施例提供的火场消防员协同接力定位系统的框图。
具体实施方式
[0078]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0079]
本发明的目的是提供一种火场消防员协同接力定位方法及系统，利用多重参考节点进行层层定位，有效解决了定位基础设施不足的问题。
[0080]
为了便于理解，本发明中用到的部分专业术语和简称解释如下：
[0081]
uwb(ultra
‑
wideband)：超宽带。
[0082]
ins(inertial navigation system)：惯性导航系统。
[0083]
待定位标签：消防员身上佩戴的未知位置的uwb标签。
[0084]
已定位标签：消防员身上佩戴的已知位置的uwb标签。
[0085]
移动锚点：可协助定位待定位标签的已定位标签。
[0086]
los(line
‑
of
‑
sight)：视距，基站之间或者标签与基站之间没有遮挡。
[0087]
nlos(non
‑
line
‑
of
‑
sight)：非视距，基站之间或者标签与基站之间存在遮挡。
[0088]
ekf(extended kalman filter)：扩展卡尔曼滤波。
[0089]
pdr(pedestrian dead reckoning)：行人轨迹推测。
[0090]
为了提高火场消防员的定位精度和定位率，本发明提出一种基于uwb的火场消防员协同接力定位方法。当火灾事故发生时，将挂载有uwb定位基站和北斗接收模块的无人机快速悬停在利于uwb信号穿透的窗口、门口等处，消防员携带集成有uwb定位标签和ins模块的定位设备进入火场展开救援。利用北斗卫星导航系统快速获取室外uwb基站的精确位置
坐标，作为整个室内定位系统的参考基准。对于能够和足量uwb基站保持良好通信的待定位标签，设计融合uwb/ins的定位算法，借助ins消除uwb测距值中的nlos误差，实现对每个标签的精确定位；对于不能和足量uwb基站通信的待定位标签，设计移动锚点选举算法选举出一定数量的已定位标签作为移动锚点，与uwb基站组合成新的参考节点，然后采用分组定位的方式，利用待定位标签到其周围参考节点以及若干个相邻待定位标签的uwb测距信息，设计基于uwb的协同定位算法，接力定位待定位标签，达到对处于定位系统覆盖区域和处于定位盲区的消防员都能实现可靠定位的目的，为消防指挥人员提供重要的作战指挥信息。火场消防员定位方法示意图如图1所示，当消防员携带定位设备深入火场内部展开灭火救援时，因部分消防员处于定位系统的弱覆盖区域，从而导致定位不准或者难以定位，为此，本发明提出一种火场消防员协同接力定位方法，在保证消防员高精度定位的同时还能提高消防员的定位率。首先，根据待定位目标可通信的uwb基站数目设置定位优先级。然后针对不同的优先级设计相应的定位算法，对于能够和三个及以上uwb基站通信的第一优先级定位目标，设计融合uwb/ins的定位算法，在定位过程中借助ins消除uwb测距值中的nlos误差，保证第一优先级定位目标的精确定位。对于不能和足量uwb基站通信的第二和第三优先级定位目标，首先按照待定位目标有可共同通信的uwb基站和已定位目标的原则对其进行分组，然后设计移动锚点选举算法，在定位小组中选举出可与待定位目标可靠通信的已定位目标作为移动锚点，并与uwb基站组合成新的参考节点，最后通过基于uwb的协同定位算法，估计出定位系统弱覆盖区域的目标位置坐标。
[0091]
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
[0092]
实施例一：
[0093]
如图2所示，本实施例提供了一种火场消防员协同接力定位方法，该方法的详细步骤流程图如图3所示，其具体步骤包括：
[0094]
s1、将处于三个及以上数量的uwb基站范围内的所有标签设置为第一优先级标签，将处于三个以下数量的uwb基站范围内的标签设置为其余优先级标签；所述标签由消防员随身携带。
[0095]
详见图4，根据标签可通信的uwb基站数目对标签进行优先级划分，图4是定位优先级设置图，正方形代表uwb基站a0
‑
a3、虚线圆代表基站的通信范围，五角星、三角形、圆点代表待定位标签。将在3个及以上uwb基站通信范围内的标签设置为第一优先级，在两个uwb基站通信范围内的标签设置为第二优先级，在一个uwb基站通信范围内的标签设置为第三优先级。
[0096]
在待定位标签的优先级确定之后，将在共同uwb基站通信范围内的标签划分到一个定位小组中。如图4中，第一优先级待定位标签t0、t1和uwb基站a0、a1、a2、a3被分为一组；第一优先级待定位标签t4、t5和uwb基站a0、a2、a3被分为一组；第二优先级待定位标签t6、t7、t8和uwb基站a0、a3被分为一组；第三优先级t9、t10和uwb基站a3被分为一组。
[0097]
s2、利用融合uwb/ins的定位算法定位第一优先级标签。
[0098]
由于第一优先级标签在定位之后要参与第二、第三优先级标签的定位，所以第一优先级标签的精确定位是保证其他优先级标签位置精确的前提。虽然能与第一优先级标签通信的uwb基站数量充足，但uwb精确定位的前提是定位标签和基站均处于los环境中，而消
防员在实际火场救援过程中需要不断的移动位置，标签和基站之间的通信常处于nlos环境，会导致较大的定位误差。本发明借助ins的短时定位精确特性，采用融合uwb/ins的定位算法，在两级ekf的框架下完成对uwb测距值的nlos检测和uwb/ins定位数据的融合，实现对第一优先级标签的精确定位。
[0099]
具体的：
[0100]
s21、获取佩戴标签的消防员的定位数据；所述定位数据包括：坐标数据、步数、步长和航向角；在消防员行走过程中，先利用ins模块获取消防员的加速度值、角速度值和磁场强度值，转换为步数、步长和航向角，然后利用第一级ekf和pdr算法计算消防员在惯性导航系统下的初始位置估计坐标。
[0101]
根据所述定位数据得到ekf的状态变量为：
[0102]
x＝[e
k
‑1,n
k
‑1,s
k
‑1,θ
k
‑1]
t
；
ꢀꢀ
(1)
[0103]
其中，e
k
‑1和n
k
‑1分别为第k
‑
1时刻消防员的东向和北向位置坐标值，s
k
‑1为第k
‑
1时刻的步长，θ
k
‑1为第k
‑
1时刻的航向角；
[0104]
利用pdr算法得到第一级状态方程：
[0105][0106]
其中，e
k
和n
k
分别为第k时刻消防员的东向和北向位置坐标值，s
k
为第k时刻的步长，θ
k
为第k时刻的航向角，ω
e
和ω
n
分别为消防员的东向和北向位置坐标e和n的过程噪声，服从数学期望为0、方差为和的正态分布，记为ω
s
为步长值s的过程噪声，服从数学期望为0、方差为的正态分布，记为ω
θ
为航向角θ的过程噪声，服从数学期望为0、方差为的正态分布，记为
[0107]
根据公式(2)，可将系统的状态方程统一表示为f
ins,k
＝f
ins
(x)+w
ins
。其中x为状态变量，f
ins
(x)为未加入任何噪声的真实状态方程，w
ins
为过程噪声。
[0108]
第一级观测量为：
[0109]
z
ins
＝[s
ins
,θ
ins
]
t
；
ꢀꢀꢀ
(3)
[0110]
其中，s
ins
和θ
ins
分别为消防员每一步的步长和航向角。
[0111]
第一级观测方程为：
[0112][0113]
其中，和分别为步长s和航向角θ的观测噪声，服从数学期望为0、方差为和的正态分布，记为
[0114]
通过ekf的预测过程和更新过程得到标签在惯性导航系统下的初始位置估计坐标，具体实现过程如下：
[0115]
(1)预测过程：
[0116]
首先根据k
‑
1时刻消防员在超宽带和惯性导航融合系统下的状态估计值利用公式(2)预测k时刻消防员在惯性导航系统下的状态值：
[0117][0118]
其中，其中，和分别为第k时刻消防员在惯性导航系统下的东向和北向位置坐标预测值，为第k时刻的步长预测值，为第k时刻的航向角预测值。
[0119]
然后根据k
‑
1时刻误差协方差矩阵预测k时刻的误差协方差矩阵：
[0120][0121]
其中，φ
k
为惯性导航系统k时刻的状态转移矩阵，q
ins
为过程噪声协方差矩阵。由于公式(2)表示的状态方程是非线性方程，根据ekf原理，可对状态方程求取雅各比矩阵近似表示φ
k
，即：
[0122][0123]
过程噪声w
ins
的协方差矩阵q
ins
可表示为:
[0124][0125]
其中，协方差矩阵q
ins
中对角线位置的字母分别是公式(2)中4个过程噪声ω
e
、ω
n
、ω
s
、ω
θ
对应的方差值。
[0126]
(2)更新过程：
[0127]
由于观测方程是线性方程，则观测矩阵可表示为：
[0128][0129]
观测噪声的协方差矩阵为：
[0130][0131]
于是根据k时刻的误差协方差预测矩阵观测矩阵h
ins
、观测噪声的协方差矩阵r
ins,k
，可计算出卡尔曼增益：
[0132][0133]
k时刻预测出的观测值可表示为：
[0134][0135]
最后根据k时刻的状态预测值卡尔曼增益k
ins,k
、观测值z
ins,k
以及观测预测值对状态变量和误差协方差进行更新：
[0136][0137][0138]
更新后的状态变量可表示为：
[0139][0140]
那么k时刻消防员在惯性导航系统下的初始位置估计坐标为
[0141]
s22、得到初始位置估计坐标之后，检测uwb测距值的nlos误差：
[0142]
根据所述标签的初始位置估计坐标计算所述标签到每个uwb基站的距离，得到距离计算值；根据第一级ekf得到的第一优先级标签的初始位置估计坐标，计算出它到各个uwb基站的距离：
[0143][0144]
其中(e
um
,n
um
)是第m(m＝1,2,...,m)个uwb基站的位置坐标。
[0145]
计算所述距离计算值与所述标签到每个uwb基站的距离测量值的残差，得到残差矩阵。所述距离测量值使用uwb定位设备直接测量得到。
[0146]
计算残差矩阵的具体步骤包括：
[0147]
将所述距离计算值与所述标签到每个uwb基站的距离测量值的差值与设定的差值阈值比较；若所述差值小于或等于所述差值阈值，则判定所述距离测量值在正常的误差范围内，若所述差值大于所述差值阈值，则判定所述距离测量值异常。
[0148]
在具体操作过程中，用k时刻定位的第一优先级标签到第m个uwb基站的距离测量值d
um,k
减去d
insm,k
并取绝对值，若此绝对值大于预设的阈值threshold，则判定当前时刻的uwb距离测量值异常，包含nlos误差，若小于等于阈值threshold，则判定此刻的uwb距离测量值在正常的误差范围内：
[0149][0150]
对包含nlos误差的uwb距离测量值求取残差：
[0151]
residue
m,k
＝λ
·
(||d
um,k
‑
d
insm,k
||
‑
threshold)
ꢀꢀ
(18)
[0152]
式中λ为残差系数，通常设置为一个比较大的值。nlos误差越大，所求取的残差值也越大，d
um,k
为距离测量值，d
insm,k
为距离计算值，threshold为设定的差值阈值。
[0153]
将正常距离测量值的残差设置为1，利用所有残差值构成残差矩阵：
[0154][0155]
s23、根据所述定位数据、所述距离测量值以及所述残差矩阵，通过ekf计算标签在
超宽带和惯性导航融合系统下的联合位置估计坐标。
[0156]
此次ekf的状态变量和状态方程与第一级ekf相同。
[0157]
第二级ekf观测量为：
[0158]
z
fusion
＝[z
uwb
,z
ins
]
t
，
ꢀꢀ
(20)
[0159]
其中z
uwb
＝[d
u1
,...,d
um
,...,d
um
]，d
um
是定位标签到第m(m＝1,2,...,m)个uwb基站的距离测量值，z
ins
为第一级观测量；
[0160]
第二级观测方程为：
[0161][0162]
式中：uwb观测噪声满足m为uwb基站的数量，步长s
ins
、航向角θ
ins
的观测噪声与第一级观测量的观测噪声一致。
[0163]
根据公式21，可将系统的观测方程统一表示为z
fusion,k
＝h
fusion,k
(x)+w
fusion
。其中x为状态变量，h
fusion,k
(x)为未加入任何噪声的真实观测方程，w
fusion
为观测噪声。
[0164]
由于观测方程是非线性方程，在k时刻对观测方程求取雅各比矩阵，得到观测矩阵：
[0165][0166]
式中：
[0167][0168][0169]
观测噪声w
fusion
的协方差矩阵为：
[0170][0171]
其中r
uwb
为uwb的观测噪声协方差矩阵，r
ins
为ins的观测噪声协方差矩阵，与公式10相同。即：
[0172][0173]
δ
um
由超宽带设备的测量精度决定，和由惯性导航设备的测量精度决定。
[0174]
将残差矩阵乘以融合滤波器的观测噪声的协方差矩阵，得到调整观测噪声协方差矩阵：
[0175]
r
fusion,k
＝r
residue,k
r
fusion
ꢀꢀ
(27)
[0176]
残差矩阵中的元素大小和对应uwb距离测量值中的nlos误差成正比，通过该方式调整对应uwb距离测量值中的噪声协方差矩阵，可达到缓解nlos误差的目的。
[0177]
通过第二级ekf的预测过程和更新过程得到标签在超宽带和惯性导航系统下的联合位置估计坐标，具体步骤如下：
[0178]
(1)融合预测过程：
[0179]
根据k
‑
1时刻误差协方差矩阵预测k时刻的误差协方差矩阵：
[0180][0181]
其中，φ
k
，q
ins
的定义与第一级ekf预测过程中φ
k
，q
ins
的定义相同。
[0182]
(2)融合更新过程：
[0183]
根据k时刻的误差协方差预测矩阵观测矩阵h
fusion,k
、调整后的观测噪声协方差矩阵r
fusion,k
，可计算出卡尔曼增益：
[0184][0185]
将第一级ekf预测过程得到的状态预测值代入h
fusion,k
(x)，则k时刻预测出的观测值可表示为：
[0186][0187]
最后根据k时刻的状态预测值卡尔曼增益k
fusion,k
、观测值z
fusion,k
以及观测预测值对状态变量和误差协方差进行更新：
[0188][0189][0190]
第二级ekf将输出最终的联合位置估计向量：
[0191][0192]
由此可以得到k时刻uwb和ins融合后的位置估计坐标
[0193]
第一优先级标签的定位算法流程图如图5所示。
[0194]
s3、将定位后的所述第一优先级标签与能够覆盖所述其余优先级标签的uwb基站组合成参考节点，利用基于uwb的协同定位算法定位所述其余优先级标签。
[0195]
首先，确定与待定位标签距离最近的第一已定位标签；所述待定位标签为所述其余优先级标签中的一个；
[0196]
然后，确定与所述第一已定位标签的距离在设定范围内的第二已定位标签；所述第一已定位标签和所述第二已定位标签均为定位后的第一优先级标签；
[0197]
再以所述第一已定位标签、所述第二已定位标签以及能够覆盖所述其余优先级标签的基站为参考节点，采用基于uwb的协同定位算法确定所述其余优先级标签的位置。
[0198]
作为一种可选的实施方式，本实施例中在利用融合uwb/ins的定位算法得到第一优先级标签的精确位置后，将它们分配到第二、第三优先级标签所在的定位小组中，分组原则为待定位标签有共同通信的uwb基站和已定位标签。图6是定位分组图，虚线圆表示待定位标签的通信范围，对于第二优先级标签t6、t7、t8，它们所在的定位小组包含uwb基站a0、a3，第一优先级已定位标签t2、t3、t4；对于第三优先级标签t9、t10，它们所在的定位小组包含uwb基站a0，第一优先级已定位标签t4。
[0199]
在此次分组完成之后，便可利用定位小组中已定位标签的位置接力定位第二优先级标签的位置坐标。但当定位小组中的已定位标签数量较多时，为了减小因参考节点冗余带来的计算量，本实施例提出了移动锚点选举算法，按照与待定位标签距离适中且均匀分布在待定位标签周围的原则选举一定数量的已定位标签作为移动锚点参与待定位标签的定位。图7是移动锚点选举算法示意图，假如一个定位小组中有m个待定位标签和l个已定位标签。
[0200]
在第一轮选举中，先对m个待定位标签进行随机排序，将排在第1位的标签单独列出，然后寻找距离该标签最近的已定位标签作为第1个移动锚点。
[0201]
在第二轮选举中，以第1个移动锚点为参照，按照距离公式34计算该移动锚点与小组内其他已定位标签之间的距离值，然后判断距离值是否符合公式35，如果距离值在预设的距离阈值下限l
th
和阈值上限u
th
之间，那么就将已定位标签保留下来，否则就将已定位标签剔除。
[0202][0203]
其中(x
ta
,y
ta
)表示移动锚点a的坐标，(x
ti
,y
ti
)表示已定位标签i的坐标。
[0204]
l
th
≤d
tatj
≤u
th
ꢀꢀ
(35)
[0205]
其中l
th
表示预设的距离阈值下限，u
th
表示预设的距离阈值上限。
[0206]
根据保留下来的定位标签个数，判断是否进行下一轮选举，如果保留下来的个数为0，直接结束选举；如果个数为1，那么就将这个被保留的已定位标签升级为第2个移动锚点，锚点选举算法至此结束；否则就进行第三轮选举，以此类推。
[0207]
在经历了若干轮选举之后，从l个已定位标签中选举出合适数量的移动锚点，与定位小组中的uwb基站组合成新的参考节点，接力定位待定位标签。移动锚点选举算法流程图如图8所示。
[0208]
在完成移动锚点选举之后，以定位小组为单元，将移动锚点与uwb基站组合成新的参考节点，采用基于uwb的协同定位算法估算定位小组中第二、第三优先级标签的位置坐标。
[0209]
基于uwb的协同定位算法的设计思路如下：首先是系统模型的构建，根据定位小组中目标的运动状态，建立多目标状态方程，再根据目标节点与参考节点之间、目标节点彼此之间的测量距离，建立多目标测量方程。然后根据系统模型，设计偏移扩展卡尔曼滤波算法，通过适当调整卡尔曼增益，减小uwb测距值中的正向nlos偏差对定位结果的影响，将目
标的状态估计修正至更加准确的方向。最后根据目标前后时刻行走步伐的稳定性特征，设计基于阈值筛选的均值滤波算法，根据设定的阈值先剔除状态估计中的粗大点，再采用均值滤波算法实现定位结果的二次优化。
[0210]
首先，根据其余优先级标签的运动状态，建立多目标状态方程，并根据所述其余优先级标签与所述参考节点之间、所述其余优先级标签彼此之间的测量距离，建立多目标测量方程。
[0211]
具体步骤如下：
[0212]
利用m个参考节点和n个标签组成二维协同定位小组，参考节点的坐标向量记为x
a
∈r2,a＝1,2,...,m，标签的坐标向量记为x
i
∈r2,i＝1,2,...,n，则第i个标签在k时刻的状态向量表示为：
[0213][0214]
其中，分别代表标签i在k时刻的横坐标、纵坐标、x轴方向的速度分量、y轴方向的速度分量。
[0215]
假设k时刻标签i在运动过程中受到的随机干扰为且可以将第i个标签的状态方程表示为：
[0216][0217]
其中，δt为采样时间间隔，根据公式37，将状态方程统一为：
[0218][0219]
式中，为状态转移矩阵，τ
i
为噪声驱动矩阵，u
i
的协方差为q
i
，定义如下：
[0220][0221][0222][0223]
其中，方差δ
x2
和δ
y2
的取值与环境有关，标签i的过程噪声协方差q
i
＝τ
i
q
i
τ
it
。
[0224]
n个相互协同的标签在时刻k的状态向量表示为：
[0225]
s(k)＝[s1(k),s2(k),...,s
n
(k)]
t
ꢀꢀ
(42)
[0226]
其中，s
n
(k)表示第n个相互协同的标签在时刻k的状态向量。
[0227]
那么系统状态方程可表示为：
[0228]
s(k)＝φs(k
‑
1)+w(k
‑
1)
ꢀꢀ
(43)
[0229]
其中，系统状态转移矩阵φ、系统过程噪声w的协方差矩阵q分别为：
[0230][0231][0232]
式中0表示的是4阶零矩阵。
[0233]
协同定位所需的测量信息来自于标签i和参考节点a之间的测距值r
ia
，以及标签i和标签j之间的测距值d
ij
，以距离的平方作为观测量，则在k时刻，m个参考节点和n个标签之间形成维数为的测量向量z(k)：
[0234]
z(k)＝[r
112
(k),...,r
ia2
(k),...,r
nm2
(k),d
122
(k),...,d
ij2
(k),...,d
(n
‑
1)n2
(k)]
t
ꢀꢀ
(46)
[0235]
其中，
[0236][0237][0238]
式中，||
·
||表示欧氏距离，分别表示参考节点a(a＝1,2,...,m)、标签i，j(i,j＝1,2,...,n且i≠j)在直角坐标系下的真实坐标，v
ia
，v
ij
分别表示标签i和参考节点a、标签j之间的距离测量噪声，且v
ia
,δ
r
的取值由uwb模块的测量精度决定；根据公式47、48，可将系统的测量方程统一表示为：
[0239]
z(k)＝h(s(k))+v(k)
ꢀꢀ
(49)
[0240]
h(s(k))为由状态向量表示的没有添加任何噪声的测量方程，v(k)表示测量噪声，v～n(0,r)，r为测量噪声协方差矩阵，定义为：
[0241][0242]
利用泰勒级数展开可将非线性测量方程转化为线性测量方程，得到多目标测量方程为：
[0243]
z(k)＝h(k)s(k)+v(k)
ꢀꢀ
(51)
[0244]
其中，
[0245]
h(k)表示测量矩阵，定义为：
[0246][0247]
其中，h
(n*m)
×
(n*m)
、的定义分别如下：
[0248]
[0249][0250]
根据所述多目标状态方程和所述多目标测量方程，利用基于偏移扩展卡尔曼滤波的协同定位算法估算所述其余优先级标签的初始位置。
[0251]
具体步骤包括：
[0252]
(1)预测阶段
[0253]
根据所述n个相互协同的标签在k
‑
1时刻的状态和误差协方差p(k
‑
1)预测对应标签在k时刻的状态和误差协方差p(k|k
‑
1)：
[0254][0255]
p(k|k
‑
1)＝φ(k)p(k
‑
1)φ
t
(k)+q
ꢀꢀ
(56)
[0256]
其中，φ表示系统状态转移矩阵，q表示系统过程噪声的协方差矩阵；
[0257]
根据预测得到的状态和误差协方差计算k时刻的观测矩阵和测量残差向量；
[0258][0259][0260]
其中是指将k时刻的状态预测值代入没有添加任何噪声的测量方程h(s(k))后所得到的观测预测值。
[0261]
根据所述观测矩阵计算k时刻的卡尔曼增益：
[0262]
k(k)＝p(k|k
‑
1)h
t
[h(k)p(k|k
‑
1)h
t
(k)+r]
‑1。
ꢀꢀ
(59)
[0263]
(2)卡尔曼增益调整
[0264]
一旦uwb信号受到障碍物遮挡，uwb模块间的距离测量值就会产生较大的正向偏移，此时将距离值直接代入运算必然会降低定位精度，为此本实施例通过调整卡尔曼增益
把偏离的状态估计值拉回到正确的方向上。
[0265]
在上一步的预测阶段，测量残差向量中的元素可表示为：y
res
(k)＝[y
res1
(k),...,y
resi
(k),...,y
resn
(k)]
t
，i＝1,2,...,n；其中，y
resi
(k)表示所述残差向量中的第i个元素，n表示元素的总个数；通过判断测量残差元素的正负，可得到如下调整系数ξ
i
和调整矩阵ξ：
[0266][0267]
其中，0＜α＜1，β≥1；
[0268][0269]
将所述调整矩阵与所述卡尔曼增益相乘，得到调整后的卡尔曼增益：
[0270]
k'(k)＝k(k)*ξ
ꢀꢀ
(62)
[0271]
即先对元素y
resi
进行符号判断，如果y
resi
>0，代表状态估计值会大于状态预测值，可通过系数α下调卡尔曼增益以削弱预测值往正向修正的程度，如果y
resi
<0，代表状态估计值会小于状态预测值，可通过系数β上调卡尔曼增益以增强预测值往负向修正的程度；然后将得到的n个增益系数构造成系数矩阵ξ；再通过新的卡尔曼增益代替原来的卡尔曼增益，把估计值往较小的方向修正，从而达到减小正向非视距误差的目的。
[0272]
(3)更新阶段
[0273]
利用调整后的卡尔曼增益计算所述n个相互协同的标签在k时刻的状态和误差协方差矩阵：
[0274][0275]
p(k)＝[i
‑
k(k)h(k)]p(k|k
‑
1)
ꢀꢀ
(64)
[0276]
由此便可得到一个定位小组中n个消防员的初始位置坐标
[0277]
最后，利用基于阈值筛选的均值滤波算法对所述其余优先级标签的初始位置进行优化。
[0278]
具体优化步骤如下：
[0279]
考虑到正常情况下，人在前后时刻行走步伐的稳定性特征，即：
[0280][0281]
其中，为目标节点在k时刻、k
‑
1时刻估算出的初始位置坐标，uth为位移阈值上限。
[0282]
如果出现位移较大的情况，很有可能该时刻的定位结果受到了随机噪声的影响。为此本实施例采用基于阈值筛选的均值滤波算法进一步缓解定位坐标的跳变问题。具体做法是：当k
‑
1时刻到k时刻的位移满足公式65时，对初始位置坐标直接进行均值滤波处理；当不满足公式65时，将k时刻的初始位置坐标舍弃，将滤波窗口内k
‑
l,...,k
‑
1,k+1...,k+l(l＝(l
‑
1)/2)时刻的初始位置坐标相加取平均值作为k时刻目标节点i的滤波输出值。
[0283][0284]
其中，为定位目标i的位置滤波中心点，k＝1,2,...,t，l＝2l+1为均值滤波窗口的大小。
[0285]
图9是基于uwb的协同定位算法流程图。第一部分是根据偏移扩展卡尔曼滤波算法初步估计移动目标的位置坐标。第二部分是二次优化移动目标的位置坐标，在初步估计出一个时刻的移动目标状态后，累计一个滤波窗口长度，当累计窗口长度l未达到预设长度时，返回第一部分进行下一次状态估计，直至达到预设长度，就可以进入基于阈值筛选的改进均值滤波算法环节，假如滤波窗口长度达到要求的时刻是k，那么需要判断移动目标节点从k'
‑
1到k'时刻(k'＝k
‑
(l
‑
1)/2)的位移是否在预设的阈值范围内，然后根据公式66进行相应的滤波处理，处理完成后，将滤波窗口中数据队列的队首去掉，此时窗口长度已不满足要求，如果定位时间未到，那么再次通过窗口长度判断，返回第一部分估计出最新时刻的移动目标节点位置，将其加入到队尾，周而复始，达到滑动滤波的效果。
[0286]
图10是第二、三优先级标签的定位算法流程图。在第一优先级标签的位置确定后，如果第二和第三优先级标签所在定位小组分配到的已定位标签数量较多，与uwb基站组合之后，数量大于3个，那么经过移动锚点选举算法和基于uwb的协同接力定位算法，就可以同时解算出第二、第三优先级标签的位置。如果第三优先级标签所在定位小组的已定位标签和uwb基站数目小于3个，则需等待第二优先级标签的位置确定后，将其分配到第三优先级标签所在的定位小组中，增加定位所需的参考信息，然后再经过移动锚点选举算法和基于uwb的协同接力定位算法，方可实现对第三优先级标签的定位。
[0287]
本发明实施例提出了一种火场消防员协同接力定位方法。该方法按照待定位标签能通信的uwb基站数目，先将其分为三种优先级，再进行针对性定位。
[0288]
(1)针对第一优先级标签，将uwb和ins相结合，在uwb基站充足但却多半处于nlos环境的情况下，削弱了uwb测距值中的nlos误差，提高了第一优先级标签的定位精度。
[0289]
(2)在进行协同接力定位时，会出现待定位标签周围的参考节点数量过多的情况，本发明以定位小组中待定位标签与已定位标签的距离、已定位标签彼此之间的距离为突破口，提出了一种基于距离的移动锚点选择方法，快速选举出与待定位标签距离适中且均匀分布在待定位标签周围的移动锚点辅助定位，与现有技术相比，降低了算法的时间复杂度。
[0290]
(3)针对第二、第三优先级标签，根据其周围分布的uwb基站和移动锚点情况，采用基于uwb的协同定位算法，以分组定位的方式对待定位标签进行接力定位，提高了第二、三优先级标签的定位率。
[0291]
总体来讲，本发明可以降低定位对uwb基站分布密度高或者通信半径长的要求，最大程度的使用整个协同定位网络资源，从而获取更多的定位参考信息，对深入火场内部的消防员因障碍物遮挡导致的定位不准和定位困难问题予以解决。
[0292]
为了验证本发明的定位性能，在一个包含4个uwb基站a0
‑
a3和34个待定位标签t0
‑
t33的大面积定位场景下进行了实验，实验布局图如图11所示，虚线弧分别代表4个uwb基站的通信范围，34个待定位标签在不同的uwb基站的覆盖范围内，其中t0
‑
t13沿不同方向做直
线运动，而t14
‑
t33处于静止状态。与本发明相对比的方法有基于最小二乘的三边协同接力定位算法(cp
‑
ls)、基于最小二乘和扩展卡尔曼滤波的协同接力定位算法(cp
‑
ls
‑
ekf)、基于偏移扩展卡尔曼滤波和改进均值滤波的协同接力定位算法(cp
‑
bekf
‑
imf)、基于uwb/ins无移动锚点选举的协同接力定位算法(cp
‑
uwb/ins
‑
bi)。
[0293]
从时间复杂度和定位精度的角度分析，本发明方法可使参与定位的锚点个数下降33％～50％，定位运算时间比前3种方法稍长，但比第4种方法要短一些，且平均定位误差可以控制在0.0505～0.3425m之间，在时间复杂度和定位精度的平衡上优于其他定位方法，实验结果如图12、图13和表1所示。
[0294]
从定位率的角度分析，随着定位标签数量的不断增加，本发明方法可使目标的定位率得到迅速提升，在本实验场景下，定位率可达到90％以上，明显优于传统非协同接力定位方法，实验结果如图14所示。
[0295]
总之，本发明在定位精度、实时性、定位率方面都表现良好，为提高消防员的定位精度以及扩大定位系统的定位覆盖范围问题提供了一种有效的解决方案。
[0296]
表1时间复杂度和定位精度分析对照表
[0297][0298]
此外，本发明实施例还提供了一种火场消防员协同接力定位系统，详见图15，该系统包括：
[0299]
优先级确定模块m1，用于将处于三个及以上数量的uwb基站范围内的标签设置为第一优先级标签，将处于三个以下数量的uwb基站范围内的标签设置为其余优先级标签；所述标签由消防员随身携带。
[0300]
第一优先级标签定位模块m2，用于利用融合uwb/ins的定位算法定位第一优先级标签。
[0301]
其余优先级标签定位模块m3，用于将定位后的所述第一优先级标签与能够覆盖所述其余优先级标签的uwb基站组合成参考节点，利用基于uwb的协同定位算法定位所述其余优先级标签。
[0302]
对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
[0303]
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。