(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211047352.3 (22)申请日 2022.08.29 (71)申请人 北京化工大 学 地址 100029 北京市朝阳区北三环东路15 号 (72)发明人 曹政才　胡小博　张东　谢红玉　 王伟　 (74)专利代理 机构 北京思海天达知识产权代理 有限公司 1 1203 专利代理师 张慧 (51)Int.Cl. G01C 21/00(2006.01) G01C 21/16(2006.01) G01S 17/86(2020.01) G01S 17/89(2020.01)G01S 19/45(2010.01) G01S 19/47(2010.01) G06T 3/40(2006.01) G06T 7/73(2017.01) G06T 17/05(2011.01) G06T 19/00(2011.01) G06T 19/20(2011.01) (54)发明名称 一种面向室外场景的多机器人协同定位与 建图方法 (57)摘要 本发明公开了一种面向室外场景的多机器 人协同定位与建图方法， 属于同步定位与建图领 域。 针对室外场景复杂多样的特点， 为兼顾系统 鲁棒性与高效性， 本发明首先采用多个机器人所 搭载的激光雷达、 IMU、 GPS获取环境及自身运动 信息以建立多传感器紧耦合框架保证系统鲁棒 性； 其次构建因子图优化框架并加入 雷达里程计 因子、 IMU预积分因子、 回环因子实现单一机器人 同步定位与地图构建， 所述回环因子通过联合 GPS实时观测数据与三维点 云描述符构建； 再次， 机器人端以数据包的形式上传所提取点云描述 子与自身位姿等信息至服务器端。 最后， 采用一 种高效的描述子匹配算法在服务器端实现机器 人间关键帧匹配， 据此 实现多机器人位姿融合与 全局地图构建。 权利要求书2页 说明书5页 附图3页 CN 115420276 A 2022.12.02 CN 115420276 A 1.一种面向室外场景的多机器人协同定位与建图方法， 其中定位与建图任务基于多传 感器融合框架， 该框架指每个机器人搭载有激光雷达、 IMU以及GPS， 其特征在于， 所述方法 包括如下步骤： 步骤1： 机器人端局部地图构建： 通过各机器人搭载的激光雷达获取机器人周围环境几 何纹理信息， IMU获取机器人运动过程中姿态及加速度信息， GPS获取机器人经纬度海拔信 息， 采用因子图优化理论构建优化框架以处理不同种类传感器所获取数据， 根据激光雷达 数据构建激光雷达里程计因子、 IMU数据构建IMU 预积分因子添加至因子图中以得到 当前时 刻机器人的位姿估计及局部地图； 步骤2： 机器人端回环检测： 为有效解决局部地图构建过程中机器人在长时间运动时存 在的定位累计误差问题， 对每个机器人在执行局部 建图过程中所选取关键帧处激光特征点 云采用一种三维点云描述子加以计算， 提出使用每个关键帧处点云描述子信息与GPS数据 联合搜索回环匹配关系， 以此构建回环因子并添加至因子图优化框架中以消除定位累积误 差； 步骤3： 机器人端数据存储与上传： 保存关键帧处三维激光点云数据、 点云描述子及位 姿估计数据， 以待发送至服 务器端； 步骤4： 服务器端多机器人位姿融合： 服务器端根据所接收的各机器人所存储的三维点 云描述子， 建立 起不同机器人间的位姿关联以得到各机器人在全局坐标系下的位姿估计； 步骤5： 服务器端多机器人地图拼接： 服务器端由所接收到的各机器人关键帧处三维点 云信息及所得到的各机器人在 全局坐标系下的位姿估计， 在设定全局坐标系下合并各机器 人局部定位与建图结果进 而生成全局一 致地图。 2.根据权利要求1所述的多机器人协同定位与建图方法， 其特征在于， 步骤1中使用包 括激光雷达里程计因子与IMU预积分因子的因子图优化框架得到机器人每一时刻位姿估 计， GPS仅用于后续搜索回环匹配关系， 并不加入至因子图优化框架之中。 3.根据权利要求1所述的多机器人协同定位与建图方法， 其特征在于， 步骤2中所述通 过联合搜索以得到回环匹配关系， 具体为： 步骤2.1： 在每个机器人端保存其关键帧处三维点云信 息并计算该三维点云描述子， 同 时构建k‑维树存储描述子， 每当有新的关键帧插入时首先在k ‑维树中进行搜索， 判断是否 存在满足设定阈值的相近描述子， 若存在则加入候选回环关键帧队列； 步骤2.2： 根据当前关键帧时刻及历史关键帧时刻的GPS数据， 判断在历史关键帧中是 否存在满足与当前关键帧直线距离小于 设定阈值的关键帧， 存在则视为候选帧加入候选回 环关键帧队列； 步骤2.3： 采用点云匹配算法匹配当前关键帧点云数据与候选回环关键帧队列中每个 候选帧点云数据， 若存在匹配度满足设定阈值的帧对便依此构建回环因子加入因子图优化 框架中。 4.根据权利要求1所述的多机器人协同定位与建图方法， 其特征在于， 步骤3中对三维 激光点云数据与点云描述子按照时间先后顺序一一对应并统一编号后以数据包的形式发 送至服务器端以供匹配。 5.根据权利要求1所述的多机器人协同定位与建图方法， 其特征在于， 步骤4中所述建 立机器人间位姿关联， 具体为：权　利　要　求　书 1/2 页 2 CN 115420276 A 2步骤4.1： 服务器构建一个数据库用以保存从各个机器人端发送至服务器端的三维点 云描述子， 以实现机器人间的关键帧匹配。 步骤4.2： 服务器端根据 所得到的帧间匹配关系， 以及接收到的各关键帧处机器人位姿 将各机器人的位姿转换到同一 坐标系下生成全局一 致的位姿估计。 6.根据权利要求1所述的多机器人协同定位与建图方法， 其特征在于， 步骤5中服务器 端解析由各个机器人端发送至服务器端的数据包， 并从 中提取出关键帧处三 维激光点云数 据， 根据步骤4所获取的各机器人全局一致的位姿估计， 在全局坐标系下得到合并各机器人 建图结果后的全局一 致地图。权　利　要　求　书 2/2 页 3 CN 115420276 A 3