(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211160212.7 (22)申请日 2022.09.22 (71)申请人 北京航空航天大 学杭州创新研究院 地址 310052 浙江省杭州市滨江区创慧街 18号 (72)发明人 欧阳真超 　胡庆雷　李东禹　 郭鹏宇　王薪宇　张昌杰　 (74)专利代理 机构 北京八月瓜知识产权代理有 限公司 1 1543 专利代理师 秦莹 (51)Int.Cl. G06T 7/579(2017.01) G06V 20/64(2022.01) G06V 10/26(2022.01) G06V 10/774(2022.01)G01C 21/20(2006.01) (54)发明名称 基于人在环优化的点云语义SLAM的方法、 系 统及装置 (57)摘要 本发明公开了一种基于人在环优化的机器 人点云语义SLA M方法、 系统及装置， 其中， 该方法 包括： S1、 移动机器人上的激光雷达对目标地点 进行数据采集， 得到每帧点云的相对位姿信息， 完成建图； S2、 建立点 云连续轨迹， 人工观测建图 误差， 构建稠密因子图， 然后基于因子图进行全 局优化， 调整总体点云错误； S3、 基于预定义的点 云语义类别并采用标注工具进行叠加地图的点 云语义标注， 累计点云语义标记结果得到点云语 义分割数据集； S4、 基于点云语义分割数据集进 行分割模型的训练和评估得到训练和评估后的 分割模型； S5、 基于训练和评估后的分割模型， 进 行点云语义定位与优化。 采用本发 明可以实现点 云语义定位与优化。 权利要求书3页 说明书10页 附图7页 CN 115496792 A 2022.12.20 CN 115496792 A 1.一种基于人在环优化的机器人点云语义SLAM方法， 其特 征在于， 包括， S1、 通过移动机器人上的采集模块对目标地点进行数据采集， 在采集过程中将SLAM算 法作为初始建图算法计算得到每帧点云的相对位姿信息， 完成建图； S2、 建立点云连续轨迹， 获取人工观测的建图误差信息， 基于所述建图误差信息选择建 图误差周围正确的点云帧重新进 行局部ICP的配准优化， 获取关键位姿点之间的闭环， 构建 稠密因子图， 然后基于所述稠 密因子图进 行全局优化， 调整总体 建图错误， 迭代配准优化和 全局优化； S3、 将进行局部ICP配准优化后的点云位姿和优化后的点云位姿对应点云导入标注工 具， 基于预定义的点云语义类别， 采用标注工具进 行叠加地图的点云语义标注， 累计点云语 义标记结果得到点云语义分割数据集； S4、 基于点云语义分割数据集进行点云语义分割模型的训练和评估得到训练和评估后 的点云语义分割模型； S5、 基于训练和评估后的分割模型， 进行点云语义定位与优化。 2.根据权利要求1所述的方法， 其特征在于， 所述S2具体包括： 通过将连续点云和连续 点云对应的推算位姿进行地图叠加， 基于点云数据的采样顺序对每帧点云的中心点进 行关 联， 形成连续轨 迹， 基于轨迹计算闭环边， 并添加所述闭环边构建新闭环； 获取人工观测的建图误差信 息， 基于所述建图误差信 息选择建图误差周围正确的点云 帧进行局部配准算法的配准优化； 获取关键位姿点之间的闭环， 构建稠密因子图， 然后基于 因子图进行全局优化， 调整总体建图错 误， 迭代所述配准优化和全局优化。 3.根据权利要求2所述的方法， 其特征在于， 所述S4具体包括： 将点云语义分割数据集 分为训练集和测试集， 基于训练集和测试集对基于深度学习的点云语义分割模型进行模型 训练和评估， 并保留总体分割精度高于某个阈值的模型参数作为实时点云语义分割的模 型。 4.根据权利要求3所述的方法， 其特 征在于， 所述S5具体包括： 将点云发送到实时点云语义分割的模型得到点分割的语义标签， 然后将一个单帧点云 n×[x， y， z]使用基于激光雷达笛卡尔到极坐标投影公式投影到一个x ‑o‑y平面， 对xy轴进 行极坐标转化得到2D特征图， 基于预定义的点云语义配色进行2D特征图着色， 得到2D点云 语义特征图； 获得第一帧点云后， 基于点云语义信息删除每帧点云的动态语义点， 然后快速估计2D 位姿， 以2D位姿作为初始解估计3D点云位姿， 进行SLAM的3D点云地图构建和2D语义特征地 图构建， 周期性基于蒙特卡罗粒子滤波对当前扫描点云的2D点云特征图进 行全局2D 语义特 征地图的粗粒度定位； 获取移动机器人在全局点云地图上的初始位置， 对里程计前端估计进行修正， 对于前 端里程计估计时的帧间点云配准， 使用语义标签进行类间分隔和类内匹配， 实现点云搜索 的加速； 同时， 检测闭环， 当发现新闭环则进行后端的因子图优化， 更新全局位姿信息 。 5.一种基于人在环优化的机器人点云语义SLAM系统， 其特 征在于， 包括， 采集模块， 安装于移动 机器人， 用于对目标地点进行数据采集， 在采集过程中将SLAM算 法作为初始建图算法计算得到每帧点云的相对位姿信息， 完成建图；权　利　要　求　书 1/3 页 2 CN 115496792 A 2优化模块， 用于建立连续轨迹， 获取人工观测的建图误差信 息， 基于所述建图误差信 息 选择建图误差周围正确的点云帧重新进行局部ICP的配准优化， 获取关键位姿点之间的闭 环， 构建稠密因子图， 然后基于所述稠密因子图进行全局优化， 调整总体建图错误， 迭代配 准优化和全局优化； 数据集模块， 用于将进行局部ICP配准优化后的点云位姿和优化后的点云位姿对应点 云导入标注工具， 基于预定义的点云语义类别， 采用标注工具进行叠加 地图的点云语义标 注， 累计点云语义标记结果得到点云语义分割数据集； 分割模型模块， 用于基于点云语义分割数据集进行分割 模型的训练和评估得到训练和 评估后的分割模型； 定位优化模块， 用于基于训练和评估后的分割模型， 进行点云语义定位与优化。 6.根据权利要求5所述的系统， 其特征在于， 所述优化模块具体用于： 通过将连续点云 和连续点云对应的推算位姿进 行地图叠加， 基于点云数据的采样顺序对每帧点云的中心 点 进行关联， 形成连续轨 迹， 基于轨迹计算闭环边， 并添加所述闭环边构建新闭环； 获取人工观测的建图误差信 息， 基于所述建图误差信 息选择建图误差周围正确的点云 帧进行局部配准算法的配准优化； 获取关键位姿点之间的闭环， 构建稠密因子图， 然后基于 因子图进行全局优化， 调整总体建图错 误， 迭代所述配准优化和全局优化。 7.根据权利要求6所述的系统， 其特征在于， 所述分割模型模块具体用于： 将点云语义 分割数据集分为训练集和测试集， 基于训练集和测试集对基于深度学习的点云语义分割模 型进行模型训练和评估， 并保留总体分割精度高于某个阈值的模型参数作为 实时点云语义 分割的模型。 8.根据权利要求7 所述的系统， 其特 征在于， 所述定位优化模块具体用于： 将点云发送到实时点云语义分割的模型得到点分割的语义标签， 然后将一个单帧点云 n×[x， y， z]使用基于激光雷达笛卡尔到极坐标投影公式投影到一个x ‑o‑y平面， 对xy轴进 行极坐标转化得到2D特征图， 基于预定义的点云语义配色进行2D特征图着色， 得到2D点云 语义特征图； 获得第一帧点云后， 基于点云语义信息删除每帧点云的动态语义点， 然后快速估计2D 位姿， 以2D位姿作为初始解估计3D点云位姿， 进行SLAM的3D点云地图构建和2D语义特征地 图构建， 周期性基于蒙特卡罗粒子滤波对当前扫描点云的2D点云特征图进 行全局2D 语义特 征地图的粗粒度定位； 获取移动机器人在全局点云地图上的初始位置， 对里程计前端估计进行修正， 对于前 端里程计估计时的帧间点云配准， 使用语义标签进行类间分隔和类内匹配， 实现点云搜索 的加速； 同时， 检测闭环， 当发现新闭环则进行后端的因子图优化， 更新全局位姿信息 。 9.一种基于人在环优化的机器人点云语义SLAM装置， 其特征在于， 包括： 存储器、 处理 器及存储在所述存储器上并可在所述处理器上运行的计算机程序， 所述计算机程序被所述 处理器执行时实现如权利要求1至4中任一项所述的基于人在环优化的机器人点云语义 SLAM方法的步骤。 10.一种计算机可读存储介质， 其特征在于， 所述计算机可读存储介质上存储有信息传 递的实现程序， 所述程序被处理器执行时实现如权利要求 1至4中任一项 所述的基于人在环权　利　要　求　书 2/3 页 3 CN 115496792 A 3