(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210931448.X (22)申请日 2022.08.04 (71)申请人 中国矿业大 学 （北京） 地址 100083 北京市海淀区学院路丁1 1号 (72)发明人 李庆玲　王凤栋　芦迪　胡一鸣　 (74)专利代理 机构 北京科迪生专利代理有限责 任公司 1 1251 专利代理师 金怡 (51)Int.Cl. G06V 10/80(2022.01) G06V 10/762(2022.01) G01S 17/89(2020.01) G01S 17/08(2006.01) G01S 17/06(2006.01) (54)发明名称 一种融合视觉回环检测的激光SLAM方法及 系统 (57)摘要 本发明涉及一种融合视觉回环检测的激光 SLAM方法及系统， 其方法包 括： S1： 通过3D激光雷 达采集激光点云数据， 并构建激光点云关键帧， 利用激光点云关键帧获取当前定位信息； S2： 将 每一个激光点云关键帧中包含的点云注册到世 界坐标系下后获取全局点云地图； S3： 使用视觉 相机采集视觉图像， 并融合激光点云关键帧构建 点云‑图像融合关键帧以检测回环信息， 当检测 到回环信息时， 获取对应的回环约束及回环轨 迹； S4： 基于回环轨迹 上的所有的点云 ‑图像融合 关键帧建立位姿图模型， 利用回环约束作为约束 边对位姿图模型进行优化， 得到优化后的点云 ‑ 图像融合关键帧以更新当前定位信息与全局点 云地图。 本发明提供的方法及系统可提高激光 SLAM系统定位与建图的精度。 权利要求书2页 说明书5页 附图3页 CN 115240047 A 2022.10.25 CN 115240047 A 1.一种融合视 觉回环检测的激光SLAM方法， 其特 征在于， 包括： 步骤S1： 激光里程计： 通过3D激光雷达采集激光点云数据， 提取所述激光点云数据的点 云线特征与面特征， 并以所述3D激光雷达的位移量为判断条件获取激光点云关键帧， 同时 将所述激光点云关键帧与局部点云地图、 将所述局部点云地图与全局点云地图按照所述点 云线特征与面特 征进行匹配， 以获取当前定位信息； 步骤S2： 地图构建： 将每一个所述激光点云关键帧中包含的点云注册到世界坐标系下， 经点云降采样后得到更新后的全局点云地图； 步骤S3： 视觉回环检测： 使用视觉相机采集视觉图像， 并融合所述激光点云关键帧构建 点云‑图像融合关键帧以检测回环信息， 当检测到回环信息时， 获取对应的回环约束及回环 轨迹； 步骤S4： 全局图优化： 基于所述回环轨迹上的所有的所述点云 ‑图像融合关键帧建立位 姿图模型， 将所述回环约束作为约束边添加到所述位姿图模型中， 使用GTSAM库对所述位姿 图模型进 行优化得到优化后的点云 ‑图像融合关键帧， 并根据所述优化后的点云 ‑图像融合 关键帧更新当前 所述定位信息与所述全局点云地图。 2.根据权利 要求1所述的融合视觉回环检测的激光SLAM方法， 其特征在于， 所述步骤S1 中以所述3D激光雷达的位移量 为判断条件获取激光 点云关键帧， 具体包括： 计算所述3D激光雷达在运行时的位移量， 当所述位移量超过阈值时， 则建立一帧新的 激光点云关键帧， 并在当前工作周期 [tk,tk+1]的tk+1时刻发布所述激光 点云关键帧。 3.根据权利 要求1所述的融合视觉回环检测的激光SLAM方法， 其特征在于， 所述步骤S3 中使用视觉相机采集视觉图像， 并融合所述激光点云关键帧构建点云 ‑图像融合关键帧， 具 体包括： 步骤S301： 获取距离时刻tk+1最近的tp时刻的视觉图像作为视觉图像关键帧与tk+1时刻 发布的所述激光点云关键帧进行关联， 构成初始点云 ‑图像融合关键帧， 其中， tp在工作周 期[tk,tk+1]之间； 步骤S302： 对[tk,tk+1]时间内所述初 始点云‑图像融合关键帧中所包含激光点 云数据通 过线性插值方法进行运动补偿， 首先如公式(1)所示， 计算所述初始点云 ‑图像融合关键帧 在ti时刻的点 云Xi在[tk,ti]之间的位姿变换Tk,i,同理可计算[tk,tp]之间的位姿变换Tk,p如 公式(2)： 其中， Tk,k+1是所述激光点云数据在激光点云时间戳[tk,tk+1]之间的位姿变换， 可由激 光里程计直接获得； 将所述初始点云 ‑图像融合关键帧中的所有点云Xi利用位姿变换Tk,i转换到tk时刻坐标 下， 得到点云Xk， 再将点云Xk利用位姿变换Tk,p转换到视觉图像时间戳tp时刻下， 得到与视觉 图像的时间戳同步的Xp， 实现了点云与 视觉图像的时间同步， 得到运动补偿后的点云 ‑图像 融合关键帧。 4.根据权利 要求1所述的融合视觉回环检测的激光SLAM方法， 其特征在于， 所述步骤S3权　利　要　求　书 1/2 页 2 CN 115240047 A 2中检测回环信息、 获取对应的回环约束及回环轨 迹， 具体包括： 步骤S311： 选用ORB特征对已有的视觉图像样本集提取特征点集合， 使用Kmeans算法对 所述特征点集合进行聚类， 将聚类结果作为单词并以树形 数据结构存 储， 构建视 觉词典； 步骤S312： 对当前所述视觉图像关键帧进行ORB特征提取得到特征点， 在 所述视觉词典 中查询所述特征点对应单词出现 的词频， 采用TF ‑IDF方法计算每个所述单词的分数， 得到 当前所述视觉图像关键帧的词袋向量Vk(v1,v2,···vn)； 步骤S313： 遍历所有历史点云 ‑图像融合关键帧， 将其中所述历史点云 ‑图像融合关键 帧中所对应的视觉图像作为历史视觉图像关键帧， 并获取所有所述历史视觉图像关键帧的 词袋向量Vh(v1,v2,···vn)， 并根据公式(3)～(4)计算Vk(v1,v2,···vn)与Vh(v1, v2,···vn)的曼哈顿距离S得到二者图像相似度 η， 选取相似度 η最高且大于阈值T的历史 帧作为回环关键帧： 其中， vi∈Vk， vj∈Vh， S(k,h)为Vk和Vh之间的曼哈顿距离用来表示词袋向量距离； S(k,k‑1)) 表示当前 所述视觉图像关键帧与前一视 觉图像关键帧之间的词袋向量距离； 步骤S314： 将检测到所述回环关键帧的点云 ‑图像融合关键帧的点云特征点投影到所 述视觉图像关键帧， 得到所述视觉图像关键 帧特征点二维像素坐标， 采用三点估计的方法 对所述二 维像素坐标进行深度赋值， 可得到所述当前视觉图像关键帧特征点的三 维空间坐 标； 步骤S315： 利用所述特征点的三维空间坐标， 将所述特征点与所述回环关键帧进行特 征点匹配， 应用PnP算法构建重投影误差模型， 通过L ‑M算法优化求解式(x)最小二乘问题， 可得到回环约束。 5.一种融合视 觉回环检测的激光SLAM系统， 其特 征在于， 包括下述模块： 激光里程计模块， 用于通过3D激光雷达采集激光点云数据， 提取所述激光点云数据的 点云线特征与面特征， 并以所述3D激光雷达的位移量为判断条件获取激光点云关键帧， 同 时将所述激光点云关键帧与局部点云地图、 将所述局部点云地图与全局点云地图按照所述 点云线特 征与面特 征进行匹配， 以获取当前定位信息； 地图构建模块， 用于将每一个所述激光点云关键帧中包含的点云注册到世界坐标系 下， 经点云降采样后得到更新后的全局点云地图； 视觉回环检测模块， 用于使用视觉相机采集视觉 图像， 并融合所述激光点云关键帧构 建点云‑图像融合关键帧以检测回环信息， 当检测到回环信息时， 获取对应的回环约束及回 环轨迹； 全局图优化模块， 用于基于所述回环轨迹上的所有的所述点云 ‑图像融合关键帧建立 位姿图模 型， 将所述回环约束作为约束边添加到所述位姿图模型中， 使用GTSAM库对 所述位 姿图模型进 行优化得到优化后的点云 ‑图像融合关键帧， 并根据所述优化后的点云 ‑图像融 合关键帧更新当前 所述定位信息与所述全局点云地图。权　利　要　求　书 2/2 页 3 CN 115240047 A 3