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