基于视觉ORB-SLAM和激光Cartographer融合的SLAM建图

REF：基于视觉与激光的AGV混合导航技术研究

1. 基础模型

• AGV 底盘运动模型

3500ef0cae14f7ab46b71f7da3c9994b.png

• AGV平台硬件关系模型

0634fb1e680287a4d76c61934e8ed3a4.png

• AGV系统结构模型

9b723d583c8cac4852b1288be5c17473.png

• 地图模型

8acb9b8eea4e3a273502656973449a2f.png

• 双目相机的几何模型

3c5ec36a7baa59c0807a6d6fcf464694.png

• 针孔相机模型

899cd3790452aff84c299b7beb5d8894.png

• 激光TOF模型

e1aa590d2119f55c10f749f48a77eaee.png

• 激光三角测距法模型

1f7dfe82432abdb93bebb82cd8da3de9.png

2. SLAM建图

• 传统Cartographer算法： 前端部分的主要任务是扫描匹配明确位置姿态参数，后端回环检测用来抵消随时间推移产生的累积定位误差，闭环检测后进行全局位姿优化

ef3c1ccb2a665d2dadf620c511ccc1d9.png

• 传统ORB-SLAM2算法： 前端视觉里程计利用特征点检测和描述符匹配技术建立不同帧图像之间的联系，通过对特征点的追踪和匹配实现相邻帧之间的相对运动初步估计，即相机姿态的变化，而后端全局优化则是对前端得到的轨迹进行整体一致性检验和优化
  • ORB特征提取：ORB特征由两个主要部分组成：oFAST（Oriented FAST）关键点和 rBRIEF（Rotated BRIEF）描述子，oFAST 关键点用于检测图像中的特征点，并通过方向信息对其进行定向（如下图），而rBRIEF 描述子则用于描述这些特征点的局部纹理信息，并通过旋转操作来增强其鲁棒性

  

  2297dbcf54c88b7ff26ee7e075b38c0a.png

  • 暴力匹配法匹配特征：实现传感器采集的新测量数据与系统积累的历史观测记录之间精确而鲁棒的对应关系构建

  

  f9b0a863be7b28f435a90f553d3fe410.png

  • PnP算法估计相机位姿：基于 2D-2D 特征匹配的对极几何法和基于 3D-2D 匹配的 PnP 方法，求解当前帧下的变换矩阵，理解不同视角下图像间的几何对应关系

  

  30e38dff93316076956cca439276e41c.png

  • 基于词袋模型的回环检测：分析图像内部的视觉特征来量化和比较图像间的相似性，模型先从图像中抽取出一系列局部特征视为单词，对这些特征的统计汇总构建一个特征词词典；随后，每幅图像被表示为该词典中单词出现频率的统计向量

  

  60d348e03cccd54cdc2fc812da7567c9.png

  • k 叉树结构存储：深度为 d、每个节点拥有 k 个子节点的树形结构。每层基于上一层的聚类结果，应用 K-means 聚类算法细分每个簇为 k 个子簇作为当前层的聚类结构，根据预设的树深度 d 重复进行，直至构建出完整的 k 叉树结构

  

  fb8d83ebb35bc6541f27335f4e416ff8.png

• 基于网格的运动统计算法(GMS)优化ORB： 通过精细化网格内的运动特征统计，以及对匹配过程的优化，提升在复杂视觉环境下特征匹配的稳定性，加速匹配进程，提高匹配效率
  • 融合运动连续性与统计理论：运用运动的平滑性特征，即正确匹配点附近往往聚集着较多的匹配点，通过量化邻域内匹配点的数量来评估匹配的可靠性（统计单元格周围九宫格领域内的总特征点数）。

  

  9efa634c9e2c8e91c27aeba7046f7fba.png

  • 改进快速网格匹配：GMS 算法对原始特征点集合执行了八次旋转，将 8 次计算邻域匹配量的最大值作为最终判定依据，实现帧间图像匹配时的旋转鲁棒性，将九宫格的8领域优化为X形的4领域，提高计算效率

  

  37be7c37efd1d0df7884e64ee607e65e.png

• ORB-SLAM2稀疏点云地图构建： ORB-SLAM2 算法默认建立三维稀疏地图（地图中只包含少量的三维地图点），这些地图点在多个图像帧中都能够被观测到而被认为是可靠的地图点

3dc4bbe874e49479a705e1ceaea9c241.png

• ORB-SLAM2 稠密点云地图构建： 对每个关键帧所携带的 RGB 图像与深度图进行精确配准，二维特征点被转换为三维空间中的点云信息，随着所有关键帧对应的三维点云的拼接整合，最后实施全局 BA 优化校正点云间的几何偏差提升地图质量，确保所得稠密点云地图的全局一致性与高精度。

6a1efe85cc359036cc44e89c8eefc88f.png

• 八叉树地图构建： 采用了一种层次化的空间划分策略，以一个大立方体代表根节点，通过递归地将每一个立方体均匀细分为八个子立方体（子节点），直至达到预定的划分深度或满足特定条件

3ac3537dcc945d295d39448ed2cde72c.png

f26cb2bca21c7066a6b8190c393843a2.png

• 基于 Bayes 的融合视觉ORB和激光Cartographer： 采用松散耦合的数据融合两个并行过程：
  • 首先，采用 Cartographer 算法对激光雷达数据的环境信息建模，构建出二维栅格地图
  • 其次，通过 ORB-SLAM2 系统对 RGB-D 相机数据生成三维点云地图与八叉树地图，将此三维信息投影至二维平面上，形成与激光雷达构建的二维地图相兼容的表示形式
  • 最后，将两者生成的地图数据融合，其中P1、P2分别表示同一单元格激光雷达和视觉ORB的占据概率

  

  ecf98861a0ff7554f0d155364442369a.png

  

  009bd302ac6eaad07196e736659bef06.png

3. 路径规划

• 优化A*邻域搜索节点： 根据目标点与当前节点的相对位置的夹角，只保留朝向目标位置的 5 个搜索方向

a8a73acd53c03aa084436d1148d5deaa.png

• 引入跳点搜索算法（JPS）： 通过预处理地图，找到可以直接跳跃的节点（即跳点），从而加速搜索。算法在找到跳点后，利用启发式搜索策略，沿着预计的最优路径向目标方向扩展搜索，直到找到最短路径或者搜索完整个图形。

2722d083eb8ac8b69e1efd7942abcd7b.png

• 优化A*代价函数： 在加入路径偏转代价函数C，避免算法得出的轨迹出现不必要的拐点

18ca66ca5274ee92282fbb8e45aad6ae.png

ad5c05e2db073bc3947cda3895e0ec9b.png

• 优化DWA评价函数： 为了提高 AGV 快速向目标前行，并安全绕开障碍物，对加权系数做出调整，使加权系数具有动态性，当 AGV 离障碍物较远时，提高 AGV 速度占比，当前进过程中，障碍物距离 AGV 较近时，提高 AGV 方向性占比

a3b488c2b7f4efb1e561ebf562d74c44.png

• 路径融合： 基于构建的全局栅格地图设置起点和终点，通过改进后的 A-star 算法执行全局路径规划，以保证路径的整体效率和最优性。选取算法规划出的关键路径点作为局部导航的引导点，引导 DWA 算 法在此框架下进行局部路径规划与动态避障操作

beb956b533818c892564f92e2e498bed.png