基于ROS2的语义格栅地图导航

REF：基于多传感器融合的全自主移动机器人系统设计与实现

1. 系统架构

• 机器人系统架构

4c8ec637239f37072da7246ca2b53cd2.png

• 硬件架构

649bb73808c490b3fec25f76f0a11e71.png

• 软件架构

e51b9a0ed9780460822124195fac3764.png

• 导航架构

8785e3adc06cc001311a735f6a7106a1.png

• 四轮差速运动模型：机器人相对于世界坐标系的位移与旋转关系可以通 过两个矩阵来表示，这两个矩阵分别为位移矩阵T和旋转矩阵R

8acf0df6240c4968fec7b979f4544a10.png

2. 构建语义栅格地图

f5ea651116479dd8b9400d9f6c56d20a.png

• 轮式里程计： 也称为编码器里程计，根据编码器产生的脉冲数计算电动机或轮子的旋转圈数，根据轮子周长计算速度，最后根据运动速度积分计算里程变化。对于低速平稳运动，可以实时提供位姿变化，但轮胎打滑、磨损等会导致估算误差，尤其是在转向或急转弯时，误差可能会迅速累积
• 雷达里程计： RF2O方法，根据传感器的速度为每个激光扫描点设定范围约束方程，通过最小化线性独立几何约束的鲁棒函数来获得雷达的运动估计，激光雷达固定在机器人上，计算得到的速度值可以被视为机器人本体的线速度和角速度

f32b16861e10602dc3e0c3bf785696d4.png

• 融合雷达里程计、轮式里程计和 IMU： 通过扩展卡尔曼滤波在某一点对非线性系统进行泰勒展开，从而将其近似为线性模型，利用迭代过程不断优化，实现对系统状态的最佳估计
  • 构建统预测模型，初始化传感器状态矩阵与误差协方差矩阵
  • 根据协方差预测和测量模型的不确定性，计算卡尔曼增益
  • 将预测的状态和里程计测量进行融合，得到更新后的最佳估计和协方差矩阵
  • 重复预测和更新，持续优化状态估计，最后得出融合后的里程计
• Cartographer地图构建算法 ：通常由前端与后端两部分：前端负责初步建立图结构，将机器人在各个时刻的位姿信息作为图中的顶点，将这些位姿之间的相对约束关系构建为图的边，形成一个初始的位姿图；后端则负责图的优化，通过调整顶点的位置，使得整个图中的边约束得到最大程度的满足，从而减少位姿估计的误差，提升地图的全局一致性
  1. 机器人位姿估计：激光雷达点云数据、里程计位姿、IMU 数据和固定坐标系位姿，这些数据经过体素滤波（固定大小）和自适应体素滤波器进行降噪和优化，IMU 用于重力校准，并与里程计数据一起进入位姿推测模块，估算机器人位姿
  2. 局部 SLAM ：利用 Ceres 进行扫描匹配（Scan Matching），结合估算的位姿进行校正得到更准确的位姿观测，经过运动滤波器判断，如果是静止的点云数据，则会被丢弃，否则将用于更新子图
  3. 全局 SLAM ：通过回环检测减少误差，当一个子图构建完成后，系统计算约束关系（包括节点之间的约束和回环约束），进行稀疏位姿调整，并优化所有历史位姿，生成更精准的 SLAM 地图

  

  aa6d234a6ff73473f4aacff17be8775d.png

• Cartographer运用分支定界方法： 顶层A表示整个环境地图，B 为低分辨率子图，C 为高分辨率子图，以 A 层的右下子图为例说明：
  1. 激光数据帧先于 B 层低分辨率子图进行匹配，计算出 B 层的激光匹配得分
  2. B 层匹配得分较高的子图，在 C 层的高分辨率子图中进一步进行匹配计算，得到 C 层的匹配得分
  3. 将 C 层子图中的最高匹配得分与 B 层的匹配得分进行比较，当 C 层子图的最高匹配得分仍然小于 B 层的匹配得分上界时，则剪枝该子图对应的 C 层搜索过程，不再进行更精细的匹配计算

  

  52f70e41b0088bd823a382dfbfce3ccd.png

• 基于 YOLOv5 获取语义 ：先统一输入图像的尺寸，对图像进行大小为 S×S 的单元区域划分，被检测物体的中心点位于网格时，通过网络的特征提取，即可确定物体的位置与其类别信息，特征由单元格的特征提取模块进行预测。

abffd9a0bdca4ce970f4a8c1f7242792.png

• 选取关键帧 ：根据里程计提供的运动数据，计算出算出机器人两帧旋转角度和相对平移量，如果大于特定阈值，表明当前帧与上一帧关键帧之间的空间位姿差异显著，机器人相对于其上一个观察点已发生较明显的位移与视角变化。
• 计算语义物体的位置 ：使用 YOLOv5 算法对 RGB 图像检测，获取到物体的类别和目标物体，在 RGB 图像中的像素坐标，通过计算位置节点对相同时间戳的深度图像处理，在深度图像中使用相应坐标获得的物体深度距离，可得到物体在相机坐标系上的位置，再通过坐标转换，可以获得到物体在地图坐标系上的位置坐标
  1. 计算图像中心位置：在 RGB 图像的像素坐标获取目标物体检测框，取目标物体检测框的中心位置作为一般物体的几何中心，针对特定物体采用规定几何中心坐标
  2. 计算深度相机距离：采用均值滤波避免噪声，在对应的深度图中收集坐标对应周围 9 个点的深度值的均值
  3. 计算深度值：当前坐标位于深度图像坐标系中，需要将其转换到相机坐标系下，先标定深度相机，得到相机焦距，计算出深度值
  4. 计算地图坐标系距离：得到物体在相机坐标系下的坐标后，借助机器人坐标系作为中间桥梁，转换坐标系到地图坐标系
  5. 投影到二维格栅地图：将地图坐标系下的物体三维坐标投影到二维栅格平面，并将其转换为离散的栅格坐标，忽略三维坐标中的高度信息，仅保留 x 和 坐标，就得到目标物体在栅格地图坐标系下的坐标
• 语义标注栅格地图： 采用基于栅格地图的语义标注方法，将语义信息与特定的栅格单元关联（栅格地图中的某些栅格单元被选作关键点，并与对应物体的语义信息相绑定），以构建语义栅格地图

a543c9eebe718dba63383f03d4e18f04.png

3. 路径规划

• 搜索方向优化Astar：默认探索的子节点过多导致计算大量的无关节点，路径规划效率降低

fab33250815f52931cb608d60cd68e81.png

• 动态调整代价函数：基于跟目标点距离变化，以当前点和终点的距离为基准，在起点一侧的节点，权重值较大，加快搜索节点效率；在靠近终点的节点，会逐渐降低的权重，增加探索节点，以能够找到最优路线。

f7d11dbc3ca70172064fd9358d8f23bd.png

• 过滤障碍物附近对角线路径：当节点四周有障碍物时，舍弃与障碍物相邻的子节点

c98f4603aff559219733b2689f4e1e2f.png

2e05c514a94cc24e29755c4cbd77e9d9.png