CN116907484A - 一种基于自主导航与ZigBee约束的应急救援人员协同定位方法 - Google Patents

一种基于自主导航与ZigBee约束的应急救援人员协同定位方法 Download PDF

Info

Publication number
CN116907484A
CN116907484A CN202310862667.1A CN202310862667A CN116907484A CN 116907484 A CN116907484 A CN 116907484A CN 202310862667 A CN202310862667 A CN 202310862667A CN 116907484 A CN116907484 A CN 116907484A
Authority
CN
China
Prior art keywords
emergency rescue
rescue personnel
personnel
condition
estimated position
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202310862667.1A
Other languages
English (en)
Inventor
姬淼鑫
任桂辉
张会娟
刘建娟
吴才章
张弛
袁航
陈红梅
许强伟
刘振江
董鸿儒
徐晟皓
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Henan University of Technology
Original Assignee
Henan University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Henan University of Technology filed Critical Henan University of Technology
Priority to CN202310862667.1A priority Critical patent/CN116907484A/zh
Publication of CN116907484A publication Critical patent/CN116907484A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明提供一种基于自主导航与ZigBee约束的应急救援人员协同定位方法，包括以下步骤：使用INS、GPS分别获取应急救援人员的初始估计位置坐标和指挥人员的位置坐标；通过ZigBee定位系统获取TDOA测量值，建立方程式组，将方程式组用Taylor级数法展开，得到目标应急救援人员的估计位置坐标；使用气压计和磁力计分别对目标应急救援人员的高度和航向角进行修正；将目标应急救援人员的估计位置坐标与目标应急救援人员的初始估计位置坐标之差、气压计输出的高度与INS捷联解算后的高度之差、磁力计解算出来的航向角与INS捷联解算后的航向角之差送入卡尔曼滤波，通过卡尔曼滤波估计出状态误差值，通过闭环反馈对INS进行校正，得到目标应急救援人员准确的位置、速度和姿态信息。

Description

一种基于自主导航与ZigBee约束的应急救援人员协同定位 方法
技术领域
本发明涉及一种救援定位方法，具体的说，涉及了一种基于自主导航与ZigBee约束的应急救援人员协同定位方法。
背景技术
应急救援协同定位是面对地震、火灾、坍塌、人员失踪等突发事件时实施应急管理与救援的必要环节，突发事件对人民的生命和财产安全造成了极大的威胁。近年来，随着经济建设的快速发展，越来越多的人涌入大自然体验户外运动，但由此引发的遇险、受困情况也屡屡发生。户外运动的地点大都比较偏远，应急救援人员在山区、海边还是其他野外环境中等一些复杂场景下工作时，环境干扰大且难以获取外界信号、定位困难。所以，通过研究全方位的应急救援人员生命保障系统，实现应急救援人员精确定位，辅助指挥人员进行决策，选取最优的搜救、撤离路径，进行人员搜救和安全撤离引导，对提高救援效率，确保人员生命安全，以及应急救援工作的顺利实施具有重要意义。
应急救援任务具有不确定性和突发性的特点，需要提前安装信标设备或者基站的定位方法是不可行的。全球定位系统(GPS)只适合用于视野开阔、障碍物较少的环境，在地下以及一些被遮挡的环境中都会导致GPS信号有不同程度的衰减，影响其定位精度。因此惯性导航系统在应急救援定位中发挥着不可替代的作用。微机电系统(MEMS)传感器体积小、重量轻、成本低，可以很好的嵌入到应急救援人员的衣服或者鞋子之中，但是低成本的惯性测量单元(IMU)受限于器件精度及技术工艺水平等诸多因素，存在累计误差，在长航状态下难以满足定位精度需求。而在应急救援场景下，需要对应急救援人员的位置进行精确的定位和追踪，以便及时采取有效的救援措施。
因此有必要利用有效的外部信息辅助惯导系统来校正和抑制其累积误差，从而在没有固定基站和基础设施的复杂环境下，实现对应急救援人员的精准定位，满足应急救援人员的工作需求。
为了解决以上存在的问题，人们一直在寻求一种理想的技术解决方案。
发明内容
本发明的目的是针对现有技术的不足，从而提供了一种基于自主导航与ZigBee约束的应急救援人员协同定位方法，实现应急救援人员在室外复杂工作场景下的定位功能。
为了实现上述目的，本发明所采用的技术方案是：
一种基于自主导航与ZigBee约束的应急救援人员协同定位方法，包括以下步骤：
使用INS、GPS分别获取应急救援人员的初始估计位置坐标和指挥人员的位置坐标；
通过ZigBee定位系统获取TDOA测量值，建立方程式组，并将方程式组用Taylor级数法展开，得到目标应急救援人员的估计位置坐标；
使用气压计和磁力计分别对目标应急救援人员的高度和航向角进行修正；
将目标应急救援人员的估计位置坐标与目标应急救援人员的初始估计位置坐标之差、气压计输出的高度与INS捷联解算后的高度之差、磁力计解算出来的航向角与INS捷联解算后的航向角之差送入卡尔曼滤波，通过卡尔曼滤波估计出状态误差值，并通过闭环反馈对INS进行校正，得到目标应急救援人员准确的位置、速度和姿态信息。
在一种实施例中，建立的方程式组为：
w1、w2、w3分别为为直角坐标系下X、Y、Z维度下应急救援人员坐标方差的倒数，Si为第i个应急救援人员到目标应急救援人员之间的距离，S1为目标应急救援人员到指挥人员之间的距离，M为救援小队中应急救援人员的个数,c为光速，为测得的第i个应急救援人员与指挥人员之间的信号到达时间差，μ为测距噪声，服从均值为零的高斯分布。
在一种实施例中，根据第一路径的接收功率和信号功率识别视距条件和非视距条件；视距条件下，选择第一测距噪声；非视距条件下，选择第二测距噪声，其中，第二测距噪声大于第一测距噪声。
在一种实施例中，根据第一路径的接收功率和信号功率识别视距条件和非视距条件的具体步骤为：
设定接收功率为：
其中，P为信道脉冲响应功率，N为前导累积计数值,A为与寄存器配置相关的常数；
设定第一条路径的信号功率为：
其中，F1、F2、F3是寄存器中的第一路径振幅值；
获取第一路径接收功率与信号功率的差值：εP＝RXL-FPL；
εP大于视距阈值条件时，判定为视距条件；否则为非视距条件。
在一种实施例中，将方程式组用Taylor级数法展开，得到目标应急救援人员的估计位置坐标包括：
根据目标人员的初始估计位置坐标(x,y,z)，对方程式(1)进行泰勒级数展开并忽略二阶以上分量，得到式(4)：
式中，γ为误差矢量；
采用加权最小二乘算法对式(4)进行处理，得到θ的最小二乘估计式：
其中B为TDOA测量值的协方差矩阵；
设定x'＝x+Δx，y'＝y+Δy，z′＝z+Δz，重复上述步骤直至递归至|Δx|+|Δy|+|Δz|<η；
其中，η为预先设定的门限值；
此时的(x',y',z')即为目标应急救援人员的估计位置坐标。
本发明对应所述基于自主导航与ZigBee约束的应急救援人员协同定位方法还提供一种基于自主导航与ZigBee约束的应急救援人员协同定位系统，包括：
INS模块，用于测量角速度和比力，基于角速度和比力估计应急救援人员的初始估计位置坐标；
GPS结算模块，用于获得指挥人员的位置坐标；
ZigBee定位系统，用于获取TDOA测量值；
估计位置坐标模块，用于基于TDOA测量值，建立方程式组，并将方程式组用Taylor级数法展开，得到目标应急救援人员的估计位置坐标；
气压计，用于对目标应急救援人员的高度进行修正；
磁力计，用于对目标应急救援人员的航向角进行修正；
卡尔曼滤波模块，用于根据目标应急救援人员的估计位置坐标与目标应急救援人员的初始估计位置坐标之差、气压计输出的高度与INS捷联解算后的高度之差、磁力计解算出来的航向角与INS捷联解算后的航向角之差估计出状态误差值；
闭环反馈模块，用于采用卡尔曼滤波模块估计出的状态误差值通过闭环反馈对INS进行校正，得到目标应急救援人员准确的位置、速度和姿态信息。
本发明相对现有技术具有突出的实质性特点和显著的进步，具体的说，本发明采用的是INS、GPS和ZigBee无线传感器网络定位系统组合的方式，并且用到达时间差法解算出应急救援人员的估计位置坐标，同时用气压计、磁力计分别对目标应急救援人员的高度和航向角进行修正，最后进行卡尔曼滤波融合，从而获得目标应急救援人员准确的位置、速度和姿态信息。
进一步的，本发明根据应急救援人员所处环境的不同，选择不同的测距噪声参数，可以进一步提高ZigBee定位系统在非视距环境下的定位精度。
附图说明
图1为本发明的流程图。
图2为本发明的IMU/GPS/ZigBee的系统组合原理图。
具体实施方式
下面通过具体实施方式，对本发明的技术方案做进一步的详细描述。
如图1所示，一种基于自主导航与ZigBee约束的应急救援人员协同定位方法，包括以下步骤：
使用INS、GPS分别获取应急救援人员的初始估计位置坐标和指挥人员的位置坐标；
其中，采用全球定位系统GPS得到指挥人员所在位置坐标(X1,Y1,Z1),惯性导航系统INS得到应急救援人员的初始估计位置坐标(x,y,z)，以及第i个应急救援人员的位置坐标(Xi,Yi,Zi)。
通过ZigBee定位系统获取TDOA测量值，建立方程式组：
其中，
w1、w2、w3分别为为直角坐标系下X、Y、Z维度下其他队员坐标方差的倒数，Si为第i个应急救援人员到目标应急救援人员之间的距离，S1为目标应急救援人员到指挥人员之间的距离，M为救援小队中应急救援人员的个数,c为光速，为测得的第i个应急救援人员与指挥人员之间的信号到达时间差，μ为测距噪声，服从均值为零的高斯分布，(x,y,z)为目标应急救援人员的初始估计位置坐标。
将方程式组用Taylor级数法展开，得到目标应急救援人员的估计位置坐标，具体包括：
根据目标人员的初始估计位置坐标(x,y,z)，对方程式(1)进行泰勒级数展开并忽略二阶以上分量，得到式(4)：
式中，γ为误差矢量；
采用加权最小二乘算法对式(4)进行处理，得到θ的最小二乘估计式：
其中B为TDOA测量值的协方差矩阵；
设定x'＝x+Δx，y'＝y+Δy，z'＝z+Δz，重复上述步骤直至递归至|Δx|+|Δy|+|Dz|<η；
其中，η为预先设定的门限值；
此时的(x',y',z')即为目标应急救援人员的估计位置坐标。
使用气压计和磁力计分别对目标应急救援人员的高度和航向角进行修正。
将目标应急救援人员的估计位置坐标与目标应急救援人员的初始估计位置坐标之差、气压计输出的高度与INS捷联解算后的高度之差、磁力计解算出来的航向角与INS捷联解算后的航向角之差送入卡尔曼滤波，通过卡尔曼滤波估计出状态误差值，并通过闭环反馈对INS进行校正，得到目标应急救援人员准确的位置、速度和姿态信息。
本实施例中，利用INS和ZigBee定位系统联合解算，INS建立在东北天(ENU)坐标系下，在保证INS和ZigBee定位系统时间同步的前提下，用INS的速度误差、位置误差、姿态误差、陀螺漂移以及加速度零偏作为状态量，用三组数据作为观测量：目标应急救援人员的估计位置坐标与目标应急救援人员的初始估计位置坐标之差、气压计输出的高度与INS捷联解算后的高度之差、磁力计解算出来的航向角与INS捷联解算后的航向角之差，通过卡尔曼滤波即可估计出状态误差值。
实施例2
与实施例1相比，本实施例给出了一种测距噪声的设定方法。
根据应急救援人员所处环境的不同，测距噪声可能具有不同的误差特征。在视距(LOS)条件下，测距值相对准确，测距噪声的标准差较小。因此，测距噪声可以很容易地预先校准。但是在非视距条件下，测距测量误差分布模型复杂，没有统一的模型,会对测距精度产生影响。
为此，本实施例中，根据第一路径的接收功率和信号功率识别视距条件和非视距条件；视距条件下，选择第一测距噪声；非视距条件下，选择第二测距噪声，其中，第二测距噪声大于第一测距噪声。
在具体实施时，根据第一路径的接收功率和信号功率识别视距条件和非视距条件的具体步骤为：
设定接收功率为：
其中，P为信道脉冲响应功率，N为前导累积计数值,A为与寄存器配置相关的常数；
设定第一条路径的信号功率为：
其中，F1、F2、F3是寄存器中的第一路径振幅值；
获取第一路径接收功率与信号功率的差值：εP＝RXL-FPL；
εP大于视距阈值条件时，判定为视距条件；否则为非视距条件。
实施例3
本实施例为对应所述基于自主导航与ZigBee约束的应急救援人员协同定位方法的系统实施例。
具体的，一种基于自主导航与ZigBee约束的应急救援人员协同定位系统，包括：
INS模块，用于测量角速度和比力，基于角速度和比力估计应急救援人员的初始估计位置坐标；
GPS结算模块，用于获得指挥人员的位置坐标；
ZigBee定位系统，用于获取TDOA测量值；
估计位置坐标模块，用于基于TDOA测量值，建立方程式组，并将方程式组用Taylor级数法展开，得到目标应急救援人员的估计位置坐标；
气压计，用于对目标应急救援人员的高度进行修正；
磁力计，用于对目标应急救援人员的航向角进行修正；
卡尔曼滤波模块，用于根据目标应急救援人员的估计位置坐标与目标应急救援人员的初始估计位置坐标之差、气压计输出的高度与INS捷联解算后的高度之差、磁力计解算出来的航向角与INS捷联解算后的航向角之差估计出状态误差值；
闭环反馈模块，用于采用卡尔曼滤波模块估计出的状态误差值通过闭环反馈对INS进行校正，得到目标应急救援人员准确的位置、速度和姿态信息。
其中，估计位置坐标模块建立的方程式组为：
其中，
w1、w2、w3分别为直角坐标系下X、Y、Z维度下应急救援人员坐标方差的倒数，Si为第i个应急救援人员到目标应急救援人员之间的距离，S1为目标应急救援人员到指挥人员之间的距离，M为救援小队中应急救援人员的个数,c为光速，为测得的第i个应急救援人员与指挥人员之间的信号到达时间差，μ为测距噪声，服从均值为零的高斯分布。
其中，估计位置坐标模块将方程式组用Taylor级数法展开，得到目标应急救援人员的估计位置坐标包括：
根据目标人员的初始估计位置坐标(x,y,z)，对方程式(1)进行泰勒级数展开并忽略二阶以上分量，得到式(4)：
式中，γ为误差矢量；
采用加权最小二乘算法对式(4)进行处理，得到θ的最小二乘估计式：
其中B为TDOA测量值的协方差矩阵；
设定x'＝x+Δx，y'＝y+Δy，z'＝z+Δz，重复上述步骤直至递归至|Δx|+|Δy|+|Δz|<η；
其中，η为预先设定的门限值；
此时的(x',y',z')即为目标应急救援人员的估计位置坐标。
实施例4
根据应急救援人员所处环境的不同，测距噪声可能具有不同的误差特征。在视距(LOS)条件下，测距值相对准确，测距噪声的标准差较小。因此，测距噪声可以很容易地预先校准。但是在非视距条件下，测距测量误差分布模型复杂，没有统一的模型,会对测距精度产生影响。
为此，本实施例与实施例3的区别在于：还包括视距条件判断模块和测距噪声选择模块。
所述视距条件判断模块，根据第一路径的接收功率和信号功率识别视距条件和非视距条件；
所述测距噪声选择模块，用于在视距条件下，将μ取值为第一测距噪声；非视距条件下，将μ取值为第二测距噪声，其中，第二测距噪声大于第一测距噪声。
在具体实施时，所述视距条件判断模块，根据第一路径的接收功率和信号功率识别视距条件和非视距条件的具体步骤为：
设定接收功率为：
其中，P为信道脉冲响应功率，N为前导累积计数值,A为与寄存器配置相关的常数；
设定第一条路径的信号功率为：
其中，F1、F2、F3是寄存器中的第一路径振幅值；
获取第一路径接收功率与信号功率的差值：εP＝RXL-FPL；
εP大于视距阈值条件时，判定为视距条件；否则为非视距条件。
最后应当说明的是:以上实施例仅用以说明本发明的技术方案而非对其限制；尽管参照较佳实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者对部分技术特征进行等同替换；而不脱离本发明技术方案的精神，其均应涵盖在本发明请求保护的技术方案范围当中。

Claims (10)

1.一种基于自主导航与ZigBee约束的应急救援人员协同定位方法，其特征在于，包括以下步骤：
使用INS、GPS分别获取应急救援人员的初始估计位置坐标和指挥人员的位置坐标；
通过ZigBee定位系统获取TDOA测量值，建立方程式组，并将方程式组用Taylor级数法展开，得到目标应急救援人员的估计位置坐标；
使用气压计和磁力计分别对目标应急救援人员的高度和航向角进行修正；
将目标应急救援人员的估计位置坐标与目标应急救援人员的初始估计位置坐标之差、气压计输出的高度与INS捷联解算后的高度之差、磁力计解算出来的航向角与INS捷联解算后的航向角之差送入卡尔曼滤波，通过卡尔曼滤波估计出状态误差值，并通过闭环反馈对INS进行校正，得到目标应急救援人员准确的位置、速度和姿态信息。
2.根据权利要求1所述的基于自主导航与ZigBee约束的应急救援人员协同定位方法，其特征在于，建立的方程式组为：
其中，
w1、w2、w3分别为为直角坐标系下X、Y、Z维度下应急救援人员坐标方差的倒数，Si为第i个应急救援人员到目标应急救援人员之间的距离，S1为目标应急救援人员到指挥人员之间的距离，M为救援小队中应急救援人员的个数,c为光速，为测得的第i个应急救援人员与指挥人员之间的信号到达时间差，μ为测距噪声，服从均值为零的高斯分布；(Xi,Yi,Zi)为第i个应急救援人员的位置坐标，(X1,Y1,Z1)为指挥人员的位置坐标，(x,y,z)为目标应急救援人员的初始估计位置坐标。
3.根据权利要求2所述的基于自主导航与ZigBee约束的应急救援人员协同定位方法，其特征在于：根据第一路径的接收功率和信号功率识别视距条件和非视距条件；视距条件下，选择第一测距噪声；非视距条件下，选择第二测距噪声，其中，第二测距噪声大于第一测距噪声。
4.根据权利要求3所述的基于自主导航与ZigBee约束的应急救援人员协同定位方法，其特征在于，根据第一路径的接收功率和信号功率识别视距条件和非视距条件的具体步骤为：
设定接收功率为：
其中，P为信道脉冲响应功率，N为前导累积计数值,A为与寄存器配置相关的常数；
设定第一条路径的信号功率为：
其中，F1、F2、F3是寄存器中的第一路径振幅值；
获取第一路径接收功率与信号功率的差值：εP＝RXL-FPL；
εP大于视距阈值条件时，判定为视距条件；否则为非视距条件。
5.根据权利要求1至4任一项所述的基于自主导航与ZigBee约束的应急救援人员协同定位方法，其特征在于，将方程式组用Taylor级数法展开，得到目标应急救援人员的估计位置坐标包括：
根据应急救援人员的初始估计位置坐标(x,y,z)，对方程式(1)进行泰勒级数展开并忽略二阶以上分量，得到式(4)：
式中，为误差矢量；
采用加权最小二乘算法对式(4)进行处理，得到的最小二乘估计式：
其中B为TDOA测量值的协方差矩阵；
设定x'＝x+Δx，y'＝y+Δy，z′＝z+Δz，重复上述步骤直至递归至
Δx+Δy+Δz<；
其中，为预先设定的门限值；
此时的(x',y',z')即为目标应急救援人员的估计位置坐标。
6.一种基于自主导航与ZigBee约束的应急救援人员协同定位系统，其特征在于，包括：
INS模块，用于测量角速度和比力，基于角速度和比力估计应急救援人员的初始估计位置坐标；
GPS结算模块，用于获得指挥人员的位置坐标；
ZigBee定位系统，用于获取TDOA测量值；
估计位置坐标模块，用于基于TDOA测量值，建立方程式组，并将方程式组用Taylor级数法展开，得到目标应急救援人员的估计位置坐标；
气压计，用于对目标应急救援人员的高度进行修正；
磁力计，用于对目标应急救援人员的航向角进行修正；
卡尔曼滤波模块，用于根据目标应急救援人员的估计位置坐标与目标应急救援人员的初始估计位置坐标之差、气压计输出的高度与INS捷联解算后的高度之差、磁力计解算出来的航向角与INS捷联解算后的航向角之差估计出状态误差值；
闭环反馈模块，用于采用卡尔曼滤波模块估计出的状态误差值通过闭环反馈对INS进行校正，得到目标应急救援人员准确的位置、速度和姿态信息。
7.根据权利要求6所述的一种基于自主导航与ZigBee约束的应急救援人员协同定位系统，其特征在于，估计位置坐标模块建立的方程式组为：
其中，
w1、w2、w3分别为为直角坐标系下X、Y、Z维度下应急救援人员坐标方差的倒数，Si为第i个应急救援人员到目标应急救援人员之间的距离，S1为目标应急救援人员到指挥人员之间的距离，M为救援小队中应急救援人员的个数,c为光速，为测得的第i个应急救援人员与指挥人员之间的信号到达时间差，μ为测距噪声，服从均值为零的高斯分布；(Xi,Yi,Zi)为第i个应急救援人员的位置坐标，(X1,Y1,Z1)为指挥人员的位置坐标，(x,y,z)为目标应急救援人员的初始估计位置坐标。
8.根据权利要求7所述的一种基于自主导航与ZigBee约束的应急救援人员协同定位系统，其特征在于：还包括视距条件判断模块和测距噪声选择模块；
所述视距条件判断模块，根据第一路径的接收功率和信号功率识别视距条件和非视距条件；
所述测距噪声选择模块，用于在视距条件下，将μ取值为第一测距噪声；非视距条件下，将μ取值为第二测距噪声，其中，第二测距噪声大于第一测距噪声。
9.根据权利要求8所述的基于自主导航与ZigBee约束的应急救援人员协同定位方法，其特征在于，所述视距条件判断模块，根据第一路径的接收功率和信号功率识别视距条件和非视距条件的具体步骤为：
设定接收功率为：
其中，P为信道脉冲响应功率，N为前导累积计数值,A为与寄存器配置相关的常数；
设定第一条路径的信号功率为：
其中，F1、F2、F3是寄存器中的第一路径振幅值；
获取第一路径接收功率与信号功率的差值：εP＝RXL-FPL；
εP大于视距阈值条件时，判定为视距条件；否则为非视距条件。
10.根据权利要求6至9任一项所述的基于自主导航与ZigBee约束的应急救援人员协同定位系统，其特征在于，将方程式组用Taylor级数法展开，得到目标应急救援人员的估计位置坐标包括：
根据目标人员的初始估计位置坐标(x,y,z)，对方程式(1)进行泰勒级数展开并忽略二阶以上分量，得到式(4)：
式中，γ为误差矢量；
采用加权最小二乘算法对式(4)进行处理，得到θ的最小二乘估计式：
其中B为TDOA测量值的协方差矩阵；
设定x'＝x+Δx，y'＝y+Δy，z'＝z+Δz，重复上述步骤直至递归至|Δx|+|Δy|+|Δz|<η；
其中，η为预先设定的门限值；
此时的(x',y',z')即为目标应急救援人员的估计位置坐标。
CN202310862667.1A 2023-07-13 2023-07-13 一种基于自主导航与ZigBee约束的应急救援人员协同定位方法 Pending CN116907484A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310862667.1A CN116907484A (zh) 2023-07-13 2023-07-13 一种基于自主导航与ZigBee约束的应急救援人员协同定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310862667.1A CN116907484A (zh) 2023-07-13 2023-07-13 一种基于自主导航与ZigBee约束的应急救援人员协同定位方法

Publications (1)

Publication Number Publication Date
CN116907484A true CN116907484A (zh) 2023-10-20

Family

ID=88357605

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310862667.1A Pending CN116907484A (zh) 2023-07-13 2023-07-13 一种基于自主导航与ZigBee约束的应急救援人员协同定位方法

Country Status (1)

Country Link
CN (1) CN116907484A (zh)

Similar Documents

Publication Publication Date Title
US8688375B2 (en) Method and system for locating and monitoring first responders
Rantakokko et al. Accurate and reliable soldier and first responder indoor positioning: Multisensor systems and cooperative localization
US8473241B2 (en) Navigation trajectory matching
US11035915B2 (en) Method and system for magnetic fingerprinting
EP3047304B1 (en) Method and apparatus for determination of misalignment between device and vessel using acceleration/deceleration
CN105628024A (zh) 基于多传感器融合的单人定位导航器和定位导航方法
JP2005535886A (ja) ネットワーク・モバイル通信デバイスの相対位置を決定するための方法およびシステム
US10782135B2 (en) Magnetic beacon and inertial sensor localization technology
US10598494B2 (en) Method and apparatus for position estimation
US10054442B2 (en) Method and apparatus for handling vertical orientations of devices for constraint free portable navigation
CN112923919B (zh) 基于图优化的行人定位方法及系统
KR101885845B1 (ko) 다중 센서를 이용한 실내 위치 추적을 위한 통합 항법 시스템
CN102998689A (zh) 基于虚拟传感器的区域判断方法
KR20190094684A (ko) 실내외 연속 위치측정 시스템
CN104897155A (zh) 一种个人携行式多源定位信息辅助修正方法
AU2015201877B2 (en) Method and system for locating and monitoring first responders
Xia et al. A novel PDR aided UWB indoor positioning method
CN117516515A (zh) 一种足履惯导/头戴视觉协同的人员定位装置和方法
CN116907484A (zh) 一种基于自主导航与ZigBee约束的应急救援人员协同定位方法
US10274317B2 (en) Method and apparatus for determination of misalignment between device and vessel using radius of rotation
Tolt et al. Real-time multi-sensor positioning for first responders
Croci et al. A GNSS/INS-based architecture for rescue team monitoring
AU2012203438A1 (en) Method and system for locating and monitoring first responders
KR101753756B1 (ko) 건설현장용 통합형 단말기
Rydell et al. (Positioning evaluation) 2

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination