(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211191165.2 (22)申请日 2022.09.28 (71)申请人 中国人民解 放军陆军 装甲兵学院 地址 100072 北京市丰台区杜家坎21号 (72)发明人 张宇　邱绵浩　胡雪松　刘西侠　 赵越　靳国超　卢方杰　宋海军　 张旭东　郑壮壮　 (74)专利代理 机构 北京元本知识产权代理事务 所(普通合伙) 11308 专利代理师 李斌 (51)Int.Cl. G01C 21/20(2006.01) G01S 17/86(2020.01) G06T 7/13(2017.01) G06T 7/521(2017.01)G06T 7/73(2017.01) G06T 7/80(2017.01) (54)发明名称 一种面向越野环境的激光视觉融合SLAM方 法 (57)摘要 本发明提出一种面向越野环境的激光视觉 融合SLAM系统。 所述系统包括单目摄像机、 激光 雷达、 视觉 里程计、 激光里程计、 帧间位姿估计模 块、 建图模块、 后端优化模块； 所述视觉里程计包 括特征提取单元、 深度匹配单元、 非线性优化求 解器； 所述激光里程计包括点云预处理单元、 帧 图匹配单元； 所述后端优化模块包括回环检测单 元和后端优化单元。 本发明还提供一种用于所述 面向越野环境的激光视 觉融合SLAM系统的方法。 权利要求书2页 说明书9页 附图1页 CN 115468567 A 2022.12.13 CN 115468567 A 1.一种面向越野环境的激光视觉融合SLAM系统， 包括： 单目摄像机、 激光雷达、 视觉里 程计、 激光里程计、 帧间位姿估计模块、 建图模块、 后端优化模块； 其特 征在于： 所述激光雷达 输出激光 点云数据， 所述单目摄 像机输出视觉图像； 所述视觉里程计包括特 征提取单元、 深度匹配单 元、 非线性优化 求解器； 所述激光里程计包括 点云预处 理单元、 帧图匹配单 元； 所述特征提取单元采用Harris角点检测方法对所述视觉图像进行特征点提取， 其将每 帧图像中的Har ris特征点送入所述深度匹配单 元； 所述深度匹配单元通过雷达—摄像机之间的坐标变换矩阵将所述激光点云数据转换 到视觉坐标系下以获得 具有深度信息的像素 特征点， 从而获得识别出的角点的深度值； 所述帧图匹配单元与低频建图模块相连， 所述非线性优化求解器与帧间位姿估计模块 相连； 所述低频建图模块 通过所述后端优化模块 生成高精点云地图。 2.根据权利要求1所述的面向越野环境的激光视觉融合SLAM系统， 其特征在于， 所述后 端优化模块包括回环检测单元和 后端优化单元， 当低频建图模块在建立地图时轨迹出现交 叉重叠， 则在回环检测单元内进 行回环检测， 以判断当前的路点与候选点云是否存在回环， 如果存在回环， 则在后端优化单 元内进行优化， 对回环内记录的车辆精准 位姿进行调整。 3.根据权利要求2所述的面向越野环境的激光视觉融合SLAM系统， 其特征在于， 所述后 端优化单元把优化问题以图的形式来体现； 图由端点与边组成， 每一个回环上记录下来的 车辆精准位姿 是一个端点， 相 邻两个车辆精准位姿间的坐标变换则是连接这两个端点间的 边； 当未检测到回环 时， 每当车辆行驶超过一段距离就将最新的车辆精准位姿存入图中作 为端点， 将其与上一端点的变换矩阵作为边存入图中， 依据匹配迭代 次数为边添加 误差范 围； 当回环检测模块检测到回环后， 最新的端点同上一端点间存在边， 也同历史轨迹中的某 个端点构成了一条边， 从而在图中形成了一个圈进行优化。 4.根据权利要求2所述的面向越野环境的激光视觉融合SLAM系统， 其特征在于， 所述回 环检测是基于距离进行的ICP匹配， 所述 ICP匹配在于： 假设用Qi,i＝1,2,3, …Qi,i＝1,2,3, …表示第一个点集， Pi,i＝1,2,3, …Pi,i＝1,2, 3,…表示第二个点 集； 两个点 集的对齐 配准转换为使下列目标函数最小： 其中， 旋转矩阵R和平移矩阵T， 就是找到的待配准点云数据与参考点云数据之间的旋 转参数和平 移参数， 使得两点 集数据之间满足距离度量 准则下的最优匹配。 5.根据权利要求1或2所述的面向越野环境的激光视觉融合SLAM系统， 其特征在于， 所 述非线性优化求解器用来求得相 邻两帧所述视觉图像之 间的位姿变换矩阵， 根据位姿变换 矩阵可以将当前帧中的每个点云进 行变换， 依据得到的坐标变换公 式， 可将t+1时刻的角点 转移到t时刻坐标系 下从而可以代入距离计算公式进行求解， 其优化的目标是各个特征点 之间所对应的距离总和最小， 最后更新 坐标变换矩阵。 6.根据权利要求1所述的面向越野环境的激光视觉融合SLAM系统， 其特征在于， 所述点 云预处理单元对所述激光点云数据进行多种 数据处理， 包括： 降采样处理、 运动畸变矫正、 地面点分离、 特 征点分离 。权　利　要　求　书 1/2 页 2 CN 115468567 A 27.根据权利要求1所述的面向越野环境的激光视觉融合SLAM系统， 其特征在于， 所述激 光里程计中的帧图匹配单元运行帧与帧匹配， 引入所述 非线性优化求解器中的位姿变换矩 阵作为两帧间的初始变换矩阵， 通过点云特 征匹配快速求 解出车辆位姿改变量。 8.根据权利要求1所述的面向越野环境的激光视觉融合SLAM系统， 其特征在于， 所述低 频建图模块进一步包括周边点云地图提取单元、 低频匹配单元和 地图维护单元； 周边点云 地图提取单元可通过坐标变换先行将待匹配路径点相邻的几个路径点所对应的点云进行 叠加从而获得周边环境地图； 低频匹配单元通过引入激光里程计的匹配结果作为初始位姿 变换矩阵， 可以显著减少迭代匹配所需的次数， 防止陷入局部最优解； 地图维护单元保存每 个路径点所对应的点云， 在所有的路径点都不再变化时， 再分别对每个路径点所存的点云 进行叠加。 9.一种面向越 野环境的激光视 觉融合SLAM方法， 包括： 通过单目摄像机获得越野环境的视觉 图像， 将视觉 图像送入特征提取单元， 所述特征 提取单元提取每帧视觉图像中的Harris特征点， 所述Harris特征点送入深度匹配单元； 深 度匹配单元通过雷达 ‑摄像机标定坐标变换矩阵与激光雷达获取的激光点云数据进行匹 配， 将激光 点云数据的深度信息赋予Har ris特征点， 从而输出 具有深度信息的像素 特征点； 将所述具有深度信 息的像素特征点送入视觉里程计中的非线性优化求解器， 所述非线 性优化求解器通过非线性优化的方法求解相 邻两帧图像之 间的位姿变换矩阵， 并将所述位 姿变换矩阵送入激光里程计中的帧图匹配单 元以及帧间位姿估计模块； 通过激光雷达获取越野环境的三维激光点云数据， 所述三维激光点云数据送入点云预 处理单元进行预处理， 经过预处理的激光点云数据送入激光里程计模块中的帧图匹配单 元； 所述帧图匹配单元接收视觉里程计返回的位姿变换矩阵作为初始矩阵， 以将相邻 两激 光雷达扫描周期的点云数据进行匹配， 从而获得匹配后的点云数据； 所述匹配后的点云数据存入低频建图模块中的地图维护单元中， 所述地图维护单元以 1Hz的低频刷新， 以生成高精点云地图。 10.根据权利要求9所述的一种面向越野环境的激光视觉融合SLAM方法， 其特征在于， 还包括： 通过回环检测单 元实时检测当前帧是否与地图中的点云构成回环； 帧间位姿估计模块输出的车辆精准位姿作为当前帧被送入到回环检测模块， 进行回环 检测； 当路径存在回环趋势 时将当前帧与地图点云进行匹配， 当相似度超过设定的阈值时 即认为触发回环； 后端优化单元在检测到回环后进行， 通过对回环内的位姿进行优化处理从而解决长时 间匹配带来的累积误差问题， 最终得到消除误差后的高精度点云地图。权　利　要　求　书 2/2 页 3 CN 115468567 A 3