基于麦克纳姆轮平台的激光SLAM导航实践

REF：ROS 系统下移动平台 SLAM 与路径规划技术研究

1. 机器人设计

• 总体设计框架

• 麦克纳姆轮移动平台机械结构

51565c25658c1261ed3820c8f2779d31.png

• 移动平台硬件架构

5209e1fff02b7c9aed25d0a560e15859.png

• 移动平台软件架构

d722f83e229fcf97473d125de717cf9d.png

• 下位机FreeRTOS任务状态转换架构

9951664cf0e11ec57161f0356be975e9.png

• 下位机STM32控制架构

2c317e487fdcc71a7dcd3819b21db6e5.png

• 上位机导航架构

bb9a92092fca040122994dfcbd3a9fe9.png

2. 激光SLAM算法测试对比

• 基于粒子滤波器Gmapping 算法： 根据机器人的里程计数据，通过运动模型预测其在地图上的位置和姿态，将实际测量到的激光雷达数据与地图进行匹配，得到机器人当前位置的估计
  • Gmapping 流程图

  

  9c66909ad8e5d84cd8e3f8cfe475e14f.png

  • Gmapping 算法仿真建图结果

  

  6365b252c54fd81aaab1c9b611acaa76.png

• 基于图优化的 Karto 算法： 采用分段优化的策略，将传感器数据分段处理，并通过后端优化及回环检测逐步构建优化地图。
  • Karto 算法前端流程

  

  2e5ae54bc3e93aa36e81440198f50db9.png

  • Karto 算法扫描匹配示意图

  

  7e01e6972b012c34d6d8cafc739e9389.png

  • Karto 后端优化：构建 BA 优化图，并向图结构中添加节点和边约束

  

  3ef01365e05ace7b8395f7ae7bf11cce.png

  • Karto 回环检测：根据相同场景求得前后两个位姿之间的相对坐标变换，将其作为边约束，添加到后端优化中

  

  6c0d59f019427a2f967c66d3178660b0.png

• 基于图优化的 Cartographer 算法： 包括数据预处理、图优化、局部地图构建、全局地图构建等部分
  • Cartographer 算法架构

  

  4e04fb38e5c69819af0dedab7c97735a.png

  • 扩展卡尔曼融合 IMU：采用互相关函数来衡量里程计和 IMU 数据的关联程度，改变噪声协方差矩阵

  

  022149113655b9753e42785a46ce2de8.png

3. 融合A*-TED实现路径规划

• 二维栅格地图

5f5cd24e20e6b76b6a41015f62e359c3.png

• 改进A* 全局规划：

0c1c118d571df22fcb769e240bb11803.png

• n：为当前节点
• gcost：当前节点移动到子节点的移动距离成本
• bid：为当前节点与其子节点，距离目标节点，对角线距离之差

9464fdc20d48872e8fa5156549ae6441.png

• 余弦函数优化代价函数：

04a9aba79322342ac56385fd7262b173.png

• 下一节点预判断优化代价函数：其中
• 三界贝塞尔曲线平滑优化

bcdf53eb0e071358812ca19b98e1c45d.png

• DWA 算法： 在机器人当前位姿附近的速度空间中动态地定义一个速度窗口，通过预测多组未来运动轨迹，并结合环境信息进行评估，从而选择最优轨迹作为下一步的移动路径

ff4e10709cd9547074dad9155e9d8bbe.png

• TEB 算法： 在机器人已规划路径中引入一个时间弹性带，允许路径在时间维度上进行扩展或收缩，以适应运动速度的变化，即全局路径规划指定起始点、目标点状态后，在中间插入多个控制点，将连续两点之间的二维路径看作一条弹力带，约束条件作为弹力带所受的外力，从而使得两点间的路径产生变形