【技术实现步骤摘要】
一种基于UWB和VIO融合的多运动节点定位方法及系统
[0001]本公开涉及复杂环境下组合定位
，具体涉及一种基于UWB和VIO融合的多运动节点定位方法及系统。
技术介绍
[0002]多运动节点(如机器人集群等)通过感知交互、信息传递、协同作业完成各种复杂的任务，这一颠覆性的智能集群技术克服了单个机器人活动区域小、搭载设备有限、任务功能单一等诸多问题。机器人集群协作的重要前提是确定各机器人的运动轨迹，以及机器人之间的相对位置和方向。目前室外开阔环境下主要依赖全球定位系统GlobalPositioning System，GPS)，而在GPS拒止环境下通常采用蓝牙、Wi
‑
Fi、超声波、超宽带(Ultra
‑
Wide Band，UWB)、视觉惯性里程计(Visual Inertial Odometer，VIO)等定位技术。其中，UWB技术通过发射纳秒级脉冲信号进行节点间的通信和距离测量，具有带宽大、安全性高、时间分辨率高、系统复杂度低、测距精度较高等特点，其常规测距精度可达10厘米，因此...
【技术保护点】
【技术特征摘要】
1.一种基于UWB和VIO融合的多运动节点定位方法，其特征在于，包括：S1，获取当前时间点的多运动节点的UWB测距数据和VIO里程计数据组成UWB
‑
VIO数据帧，并将所述UWB
‑
VIO数据帧进行初始化和状态实时预测处理，得到所述UWB
‑
VIO数据帧的预测结果；S2，重复S1步骤多次，得到多帧UWB
‑
VIO数据帧及所述多帧UWB
‑
VIO数据帧的预测结果；S3，根据所述多帧UWB
‑
VIO数据帧构建融合窗口，并根据所述多帧UWB
‑
VIO数据帧的预测结果及所述多帧UWB
‑
VIO数据帧中所述多运动节点的测距约束和里程计约束迭代优化所述融合窗口，得到优化后的所述多运动节点的运动状态，该运动状态表征所述多帧UWB
‑
VIO数据帧的最优定位估计信息。2.根据权利要求1所述的基于UWB和VIO融合的多运动节点定位方法，其特征在于，所述S3包括：S31，根据所述多帧UWB
‑
VIO数据帧构建融合窗口；S32，将所述多帧UWB
‑
VIO数据帧的预测结果作为所述融合窗口的初始值；S33，根据所述多帧UWB
‑
VIO数据帧中所述多运动节点的测距约束和里程计约束来迭代优化所述融合窗口，直至约束残差不再减小为止，得到优化后的所述多运动节点的运动状态。3.根据权利要求2所述的基于UWB和VIO融合的多运动节点定位方法，其特征在于，所述S33包括：将所述多帧UWB
‑
VIO数据帧中所述多运动节点的测距约束和里程计约束输入至所述融合窗口的优化器，利用高斯牛顿法优化所述融合窗口，直至约束残差不再减小为止，得到优化后的所述多运动节点的运动状态。4.根据权利要求1所述的基于UWB和VIO融合的多运动节点定位方法，其特征在于，所述S1包括：S11，获取当前时间点的多运动节点的UWB测距数据和VIO里程计数据组成UWB
‑
VIO数据帧，其中，所述多运动节点包括一主运动节点和多个从运动节点；S12，以所述主运动节点的载体坐标系为全局坐标系原点，根据所述UWB测距数据得到所述多个从运动节点的全局状态，并将所述UWB
‑
VIO数据帧作为当前参考数据帧；S13，根据所述当前参考数据帧中的VIO里程计数据、所述全局状态及当前时刻的VIO里程计数据，实时预测所述多运动节点的节点状态，得到所述UWB
‑
VIO数据帧的预测结果。5.根据权利要求4所述的基于UWB和VIO融合的多运动节点定位方法，其特征在于，该方法还包括：S4，将所述融合窗口内的所述多帧UWB
‑
VIO数据帧进行边缘化处理，并将所述边缘化处理时移除的最后一帧UWB
‑
VIO数据帧作为当前最新参考数据帧，该当前最新参考数据帧用于下一周期中多帧UWB
‑
VIO数据帧的预测参考帧。6.根据权利要求5所述的基于UWB和VIO融合的多运动节点定位方法，其特征在于，该方法还包括：S5，将所述最后一帧UWB
‑
VIO数据帧替换所述S12中的所述当前参考数据帧，重复S11、S13、S2～S4步骤多次，得到多个不同时间段的所述多运动节点的运动状态，该多个不同时间段的所述多运动节点的运动状态表征所述多运动节点的最优估计运动轨迹。
7.根据权利要求4所述的基于UWB和VIO融合的多运动节点定位方法，其特征在于，所述多运动节点的全局状态包括所述多运动节点的全局位置和方向。8.根据权利要求7所述的基于UWB和VIO融合的多运动节点定位方法，其特征在于，所述多运动节点的测距约束e
uwb
表征不同运动节点间每两个UWB模块的测距关系，...
【专利技术属性】
技术研发人员：郑帅康，邹旭东，李志天，熊兴崟，杨伍昊，
申请(专利权)人：中国科学院空天信息创新研究院，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。