(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211040010.9 (22)申请日 2022.08.29 (71)申请人 浙江工业大 学 地址 310014 浙江省杭州市拱 墅区潮王路 18号 (72)发明人 禹鑫燚　张震　郑万财　胡佳南　 欧林林　蔡传烩　 (74)专利代理 机构 杭州天正专利事务所有限公 司 33201 专利代理师 舒良 (51)Int.Cl. G06T 7/70(2017.01) G06V 10/74(2022.01) G06V 10/762(2022.01) G06T 3/00(2006.01)G06T 1/20(2006.01) G06T 7/80(2017.01) (54)发明名称 一种动态环境下基于多传感器的同时定位 与建图方法 (57)摘要 一种动态环境下基于多传感器的同时定位 与建图方法， 包括： 首先， 利用YOL OV5进行动态目 标检测并发送检测框； 利用惯性测量单元(IMU) 矫正点云畸变； 提取地面激光点云， 方便后续的 点云聚类； 提取检测框内的激光点云， 聚类出动 态物体的激光点云； 在矫正畸变后的激光点云中 去除聚类出的动态物体点云用于后续算法计算； 利用迭代卡尔曼滤波融合激光点云和IMU信息， 得到当前的位姿信息； 根据位姿信息， 将去除畸 变和动态物体后的点云与点云地图叠加， 获得 实 时更新后的点云地图； 利用关键帧和词袋模型以 及激光点云进行回环检测。 权利要求书2页 说明书4页 附图3页 CN 115359115 A 2022.11.18 CN 115359115 A 1.动态环境下基于多传感器的同时定位与建图方法， 包括以下步骤： 步骤1： 图像处理程序在GPU中单独运行， 利用相机实时获取当前环境中的图像信息， 并 进行基于 YOLO V5的动态目标检测， 输出检测到的动态 物体的检测框信息； 步骤2： 利用惯性测量单元(IMU)和激光点云的时间戳信息矫正点云运动畸变； 通过线 性插值和IMU预积分的方法， 得到激光点对应时间戳时刻相对于当前帧初始时刻的相对运 动信息； 根据激光 点的相对运动数据， 将激光 点矫正到正确的位置上； 步骤3： 提取地面激光点云， 方便后续的点云聚类； 默认环境中的动态物体存在于可行 地面之上， 因此先提取出地面激光点云大大降低聚类难度， 去除动态物体上的点云后， 再将 地面点云与去除动态 物体点的非地 面点云合并， 用于后续的点云匹配； 步骤4： 将激光点云投影到图像数据上， 提取检测框内的激光点云， 聚类出动态物体的 激光点云； 步骤5： 将点云分为动态点云和非动态点云； 保留动态物体的激光点云， 可供后续的动 态避障算法使用； 在矫正畸变后的激光点云中去除聚类出的动态物体点云用于算法的定位 和建图； 步骤6： 利用迭代卡尔曼滤波融合激光点云和IMU信息， 得到机器人当前相对于上一帧 数据的相对位姿和世界坐标系下的绝对位姿； 步骤7： 根据位姿信息， 将去除畸变和动态物体后的点云与点云地图叠加， 获得实时更 新后的点云地图； 步骤8： 保留相机关键帧， 利用关键帧和词袋模型以及激光点云进行回环检测， 进一步 减小累计误差 。 2.如权力要求1所述的动态环境下基于多传感器的同时定位与建图方法， 其特征在于： 步骤3中提取地 面激光点云， 具体步骤 包括： 步骤3.1： 将矫正运动畸变后的激光点云根据XY坐标在XY平面上以一定的边长进行栅 格化； 步骤3.2： 对于每一个栅格内的点云寻找其中的最低 点， 如果该最低点的绝对高度高于 某一阈值， 则认为该栅格内没有地面点， 否则， 将栅格内所有与最低点高度差不超过预设阈 值的激光 点认为是 可能的地 面点； 步骤3.3： 求出所有筛选出的可能的地面点的高度均值， 将超出均值一定范围的点从可 能的地面点中去除， 剩余的点作为 地面点。 3.如权力要求1所述的动态环境下基于多传感器的同时定位与建图方法， 其特征在于： 步骤4中提取检测框内的激光 点云， 聚类出动态 物体的激光 点云， 具体步骤 包括： 步骤4.1： 利用相机的内参数据和相机雷达事先标定得到的外参数据， 将激光雷达点云 数据投影到图像数据上； 步骤4.2： 相机和激光雷达数据分别在GPU和CPU上并行处理， 并且不用进行相机和雷达 话题的同步， 而 是根据相机和雷达数据获取的时间戳信息， 利用IMU的角速度和加速度信息 对检测框位置进 行补偿； 在时间差内对IMU信息进 行积分， 得到机器人在相机和雷达数据获 取时刻之间产生的相对运动， 用计算出来的相对运动再将 图像和相机数据进行进一步对 准， 之后将检测框内的点云进行提取； 步骤4.3： 先将提取出来的点云根据距离和高度进行直通滤波； 然后再利用基于欧式距权　利　要　求　书 1/2 页 2 CN 115359115 A 2离的聚类方法， 当一个点周围的点在一定范围内时， 将周围的点认为是同一类的点， 当不能 再加入新的点时， 则认为聚类出一个完整的类； 将检测框内的所有点云进 行聚类后， 再根据 每个类中的点云数量、 点云形态等信息 筛选出目标动态 物体上的点云。 4.如权力要求1所述的动态环境下基于多传感器的同时定位与建图方法， 其特征在于： 步骤8中关键帧和词袋模型以及激光 点云进行回环检测， 具体步骤 包括： 步骤8.1： 生成图像关键帧， 用词袋模型通过统计特征类型来描述该关键帧， 以表征向 量的形式记录关键帧以及当前位姿； 步骤8.2： 将新生成的关键帧与之前的关键帧进行相似度对比， 取出相似度较 高的关键 帧对应位姿下的点云地图中的点云； 步骤8.3： 将取出的几帧点云与当前点云进行匹配， 将匹配度最高的作为回环结果加入 到后端优化中。权　利　要　求　书 2/2 页 3 CN 115359115 A 3