基于视觉ORB和激光CSM邻近检测的SLAM建图

REF：视觉与激光雷达融合的室内移动机器人自主导航研究

1. 基础模型

• RGB相机模型： 三维空间到二维平面的几何映射，针孔模型作为经典表征方 法，理论框架源于小孔成像物理机制，构建光线经孔径限束后生成倒像投影的数学模型

0dfe7767774d8ffb1f00ebc7e13fb98e.png

• 结构光相机模型： 通过发射器将特定模式的光线投射到被测物体表面，利用物体表面对光线的调制作用，生成反映物体形貌的三维信息

5e6687d08f687fe8b4288f9abb1a2038.png

• 激光雷达模型： 在视场角 FOV 内投射调制光束，基于飞行时间和相位偏移原理，通过解算发射波束与回波信号的时间差及角度偏差，计算反射点相对于雷达原点的径向距离

69ed2afbba5b799a191c756712d0c5e5.png

• IMU 数学模型： 通常由加速度计、陀螺仪和磁力计组成：加速度计用于测量物体在三个正交轴上的线性加速度，陀螺仪用于测量物体绕三个轴的角速度，而磁力计则用于检测地球磁场，提供方向信息。

1389e1f1146c0db58109dfd541c9d7e4.png

• 多传感器数据时间同步模型： 不同传感器对同一目标，在同一时刻采集的数据及其时间戳严格同步

4bcd6a9697027ab85238e4be777b2304.png

• 相机与激光雷达的联合标定模型： 由于传感器坐标系各异，同一目标特征在相机和激光雷达坐标系中的表达存在差异，为准确处理数据，需将其统一至同一坐标系，相机与激光雷达的联合标定本质上是确定两者坐标系间的转换关系。

457343d3477d349821248632a9853334.png

2. SLAM建图

4f48ce81e025e9ea70ea8822f57eb16d.png

• 视觉位姿估计： ORB算法整合 FAST 角点检测和 BRIEF 二进制描述符的优势，通过解析环境纹理信息，利用连续图像序列估计相机的运动轨迹，视觉里程计主要分为直接法和特征点法两类，特征点法的实现过程包含三个关键环节：特征检测、特征匹配及位姿估计

72aee9207fb70156ecef0d6ad58157a2.png

• PnP 算法位姿求解： 利用匹配点对(P₁、P₂)及其对应的三维空间点 P，构建旋转矩阵 R 与平移向量T 的优化模型，通过最小化重投影误差 e(即变换后投影点与实际观测点的偏差)，实现相机运动参数的精确估计

b7c5cddea1638236a0edf89857c9e761.png

• 激光雷达位姿估计： CSM 邻近检测算法，通过当前点云与局部栅格地图的配准，搜索最优位姿参数，核心在于通过几何一致性最大化实现最优匹配，即调整位姿使激光点云与地图高概率区域充分重合，从而在提升定位精度的同时抑制累计误差

3d050b688166cb68a3659bc35206d82b.png

• 基于视觉的回环检测： 回环检测与重定位均通过历史帧与当前帧的特征关联建立约束关系，前者用于修正系统累积误差，后者解决位姿跟踪丢失后的恢复问题。回环检测技术的关键在于通过视觉传感器获取的环境特征匹配度来判断移动机器人是否到达历史位点
  • 基于贝叶斯滤波推导的回环概率，仅保留最高概率候选并设置置信阈值过滤低概率误匹配
  • 在回环验证阶段，通过 RANSAC 算法对特征关键点进行几何一致性筛选，得到一组高置信度的三维匹配点对
  • 最小二乘问题通过非线性优化方法迭代求解，最终得到使两节点点云空间一致性最高的刚性变换参数 R 和 t
• 基于激光雷达的邻近检测： 节点存储机器人运动状态信息，同步解析节点间的空间变换参数与几何距离信息，执行邻近检测时，系统依据预设的空间半径阈值筛选候选节点群。系统对回环概率进行评估，若发现当前节点与群内其他节点存在显著的回环可能性，则直接调用节点所存储的RGB-D 信息，从而快速建立邻近关系并跳过后续阶段。然后计算节点群中各节点与最近邻节点的位姿变换关系，仅对时间戳最新的近邻节点更新激光雷达扫描数据，以提升计算效率。随后，基于 2D 迭代最近点算法对相邻节点的激光扫描数据进行位姿优化，从而校正位姿偏差，并建立当前节点与最近邻节点之间的空间关联。

5da15ccbeaa52e30aae4dec1ad70c696.png

• 占据栅格地图生成： 采用点云库 PCL中的体素滤波器，设定网格尺寸将三维空间划分为若干立方体单元，当网格划分达到一定精细度时，单元内的点云信息可视为高度一致，此时可采用单元内所有点的几何中心来代表该单元的全部信息。在完成点云的稀疏处理后，计算其法向量信息。通过筛选法向量与 Z 轴平行且满足预设阈值的点云，准确识别地面区域。完成点云分类后，将不同高度的点云数据统一投影至 XY 平面。最终，结合激光雷达采集的距离数据，对生成的二维投影地图进行精度校正与补充完善。

3. 路径规划

• 全局路径Astar搜索方向优化： 在原有 8 个子节点的基础上向外围继续扩展一层，这样就有了 16 个搜索方向

f209d1e66f96c9994c52943a03415b67.png

• 均匀B样条插值平滑优化： 使用二次样条曲线，搜索连续节点 P1，P2，P3 的位置，分别计算对于中心 m12，m23；以 m12，m23 为端点绘制样条曲线；使样条曲线在 m12，m23 处与节点连线相切

8dce45ac9a92b678387420b3bf6e21cf.png

• 局部路径DWA轨迹方向优化： 评价当前预测轨迹末端距离全局路径的距离，对太过偏离的轨迹做出惩罚

805f0d18bdb6588bab8a954026fba624.png

• 融合改进 Astar 和改进 DWA 算法： 基于改进 Astar算法生成全局参考路径，设计路径特征点提取机制，将全局路径分解为多个连续性导航子目标；其次，通过改进 DWA 构建局部避障模块，引入动态目标点判定机制解决未知障碍干扰问题

048713378c86334eb5c2f93df2e4f586.png