测距辅助的多地面机器人协同建图方法、装置和设备制造方法及图纸

本公开提供了一种测距辅助的多地面机器人协同建图方法、装置和设备，可以应用于计算机视觉技术领域、工程技术领域和数据处理技术领域。该方法包括：获取主节点机器人在第k时刻的第一点云数据和第一位姿变换信息；获取主节点机器人与从节点机器人之间的第一距离；获取第一预设数量个历史时刻的历史位姿变换信息集合；接收从节点机器人在第k时刻的第二点云数据和第二位姿变换信息；根据第一距离、第一位姿变换信息、历史位姿变换信息集合和第二位姿变换信息，得到第k时刻的第三位姿变换信息；根据第一点云数据、第二点云数据和第三位姿变换信息协同建图。换信息协同建图。换信息协同建图。

【技术实现步骤摘要】
测距辅助的多地面机器人协同建图方法、装置和设备


[0001]本公开涉及计算机视觉
、工程
和数据处理
，更具体地，涉及一种测距辅助的多地面机器人协同建图方法、装置和设备。

技术介绍

[0002]在没有全球导航卫星系统(Global Navigation Satellite System，GNSS)和运动捕捉系统等外部定位系统的情况下，机器人之间的相对定位一般依赖机器人之间的视觉/激光雷达闭环。这种方法只有机器人探索同一区域时才能完成地图融合，这大大限制了机器人的活动范围。协同技术在协同VR游戏、协同定位、建图等领域有着广泛的应用。现有基于视觉的协同建图技术必须要有区域重叠才能建立机器人相对位姿约束实现协同定位，当机器人探索不同区域时，这种方法完全失效。
[0003]相关技术中，视觉同时定位与建图技术(Simultaneous Localization and Mapping，SLAM)以其成本低、结构简单等优点受到了广大研究者的青睐。视觉SLAM的地图多为稀疏点云地图，无法反映环境的结构和纹理信息，更不能进一步为机器人本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种测距辅助的多地面机器人协同建图方法，包括：获取主节点机器人在第k时刻的第一点云数据和第一位姿变换信息，其中，所述第一位姿变换信息表征所述主节点机器人在第k时刻与第k
‑
1时刻之间的第一视觉里程计坐标系下的位姿变换信息；获取所述主节点机器人与从节点机器人之间的第一距离；获取第一预设数量个历史时刻的历史位姿变换信息集合，其中，所述历史位姿变换信息集合表征所述主节点机器人在每个所述历史时刻的在世界坐标系下的位姿变换信息和所述从节点机器人在每个所述历史时刻的在世界坐标系下的位姿变换信息的集合；接收从节点机器人在第k时刻的第二点云数据和第二位姿变换信息，其中，所述第二位姿变换信息表征所述从节点机器人在第k时刻与第k
‑
1时刻之间的第二视觉里程计坐标系下的位姿变换信息；根据所述第一距离、所述第一位姿变换信息、所述历史位姿变换信息集合和所述第二位姿变换信息，得到第k时刻的第三位姿变换信息，其中，所述第三位姿变换信息表征所述主节点机器人在第k时刻的在世界坐标系下的位姿变换信息和所述从节点机器人在第k时刻的在世界坐标系下的位姿变换信息；根据所述第一点云数据、所述第二点云数据和所述第三位姿变换信息协同建图。2.根据权利要求1所述的方法，其中，所述根据所述第一距离、所述第一位姿变换信息、所述历史位姿变换信息集合和所述第二位姿变换信息，得到第k时刻的第三位姿变换信息包括：根据所述历史位姿变换信息集合，得到第一状态向量和第二状态向量，其中所述第一状态向量和所述第二状态向量维度一致，所述第一状态向量包括所述历史位姿变换信息集合中包括的所有位姿变换信息，所述第二状态向量包括待求解的第三位姿变换信息和历史位姿变换信息集合中距离第k时刻较近的位姿变换信息；根据所述第一状态向量和所述第二状态向量，得到先验残差；根据所述第一距离和所述待求解的第三位姿变换信息，得到距离残差；根据所述第一位姿变换信息、所述第二位姿变换信息和所述待求解的第三位姿变换信息，得到位姿残差；对所述先验残差、距离残差和位姿残差进行加和，根据加和结果得到所述第三位姿变换信息。3.根据权利要求2所述的方法，其中，根据所述第一状态向量和所述第二状态向量，得到先验残差包括按照如下公式(一)计算：其中，表征所述先验残差，表征所述第一状态向量，表征所述第二状态向量，k表征第k时刻，表征马哈拉诺比斯二次范数，表征k时刻的先验残差的协方差等于k1时刻的位姿的协方差，操作符表征对与进行广义减法运算，并对减法运算结果中的四元数变量对应的切向量进行对数映射，k1表征第二状态向量中第一个时刻。4.根据权利要求2所述的方法，其中，根据所述第一距离和所述待求解的第三位姿变换
信息，得到距离残差包括按照如下公式(二)计算：其中，表征所述距离残差，表征所述第一状态向量，k表征第k时刻，ρ{}表征胡伯尔范数，表征马哈拉诺比斯范数，表征距离噪声方差，表征所述第一距离，i表征距离测量集合中的第i节点机器人，i大于等于1，g表征所述第i节点机器人上的第g个测距装置，h表征所述主节点机器人上的第h个测距装置，G表征所述第i节点机器人上的测距装置的总数，G大于等于2，H表征所述主节点机器人上的测距装置的总数，H大于等于4，表征与所述第i节点机器人对应的第k时刻的所述待求解的第三位姿变换信息，W表征世界坐标系，表征所述第i节点机器人的所述第二视觉里程计坐标系，表征所述第i节点机器人的第g个测距装置相对于所述第i节点机器人的所述第二视觉里程计坐标系的位置，表征与所述主节点机器人对应的第k时刻的所述待求解的第三位姿变换信息，表征所述主节点机器人的所述第一视觉里程计坐标系，表征所述主节点机器人的第h个测距装置相对于所述主节点机器人的所述第一视觉里程计坐标系的位置，0表征所述主节点机器人。5.根据权利要求2所述的方法，其中，所述根据所述第一位姿变换信息、所述第二位姿变换信息和所述待求解的第三位姿变换信息，得到位姿残差包括按照如下公式(三)计算：其中，表征所述位姿残差，表征所述第一位姿...

【专利技术属性】
技术研发人员：张海峰，李志天，邹旭东，
申请(专利权)人：中国科学院空天信息创新研究院，
类型：发明
国别省市：