[发明专利]一种火场消防员协同接力定位方法

权利要求书

1.一种火场消防员协同接力定位方法，其特征在于，所述方法包括：

将处于三个及以上数量的UWB基站范围内的所有标签设置为第一优先级标签，将处于三个以下数量的UWB基站范围内的标签设置为其余优先级标签；所述标签由消防员随身携带；

利用融合UWB/INS的定位算法定位第一优先级标签；

将定位后的所述第一优先级标签与能够覆盖所述其余优先级标签的UWB基站组合成参考节点，利用基于UWB的协同定位算法定位所述其余优先级标签。

2.根据权利要求1所述的火场消防员协同接力定位方法，其特征在于，所述将定位后的所述第一优先级标签与能够覆盖所述其余优先级标签的UWB基站组合成参考节点，利用基于UWB的协同定位算法定位所述其余优先级标签，具体包括：

确定与待定位标签距离最近的第一已定位标签；所述待定位标签为所述其余优先级标签中的一个；

确定与所述第一已定位标签的距离在设定范围内的第二已定位标签；所述第一已定位标签和所述第二已定位标签均为定位后的第一优先级标签；

以所述第一已定位标签、所述第二已定位标签以及能够覆盖所述其余优先级标签的基站为参考节点，采用基于UWB的协同定位算法确定所述其余优先级标签的位置。

3.根据权利要求1所述的火场消防员协同接力定位方法，其特征在于，所述利用融合UWB/INS的定位算法定位第一优先级标签，具体包括：

获取佩戴标签的消防员的定位数据；所述定位数据包括：坐标数据、步数、步长和航向角；

根据所述定位数据利用EKF和PDR算法计算标签的初始位置估计坐标；

根据所述标签的初始位置估计坐标计算所述标签到每个UWB基站的距离，得到距离计算值；

计算所述距离计算值与所述标签到每个UWB基站的距离测量值的残差，得到残差矩阵；所述距离测量值使用UWB定位设备直接测量得到；

根据所述定位数据、所述距离测量值以及所述残差矩阵，通过EKF计算标签的联合位置估计坐标。

4.根据权利要求3所述的火场消防员协同接力定位方法，其特征在于，所述根据所述定位数据利用EKF和PDR算法计算标签的初始位置估计坐标，具体包括：

根据所述定位数据得到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时刻的航向角；

根据所述定位数据利用PDR算法得到第一级状态方程：其中，E_k和N_k分别为第k时刻消防员的东向和北向位置坐标值，S_k为第k时刻的步长，θ_k为第k时刻的航向角，ω_E和ω_N分别为消防员的东向和北向位置坐标E和N的过程噪声，服从数学期望为0、方差为和的正态分布，记为ω_S为步长值S的过程噪声，服从数学期望为0、方差为的正态分布，记为ω_θ为航向角θ的过程噪声，服从数学期望为0、方差为的正态分布，记为

根据所述定位数据得到第一级观测量为：Z_ins＝[S_ins,θ_ins]^T；其中，S_ins和θ_ins分别为消防员每一步的步长和航向角；

根据所述定位数据得到第一级观测方程为：其中，和分别为步长S和航向角θ的观测噪声，服从数学期望为0、方差为和的正态分布，记为

通过EKF的预测过程和更新过程得到标签的初始位置估计坐标。