基于萤火虫+Gmapping、分层+A*优化的导航方案

REF：基于激光和视觉ＳＬＡＭ 的自主导航机器人系统设计

1. 系统架构

• 机器人硬件框架

684e5c6e729e993531b3ad88859332ae.png

• 机器人系统软硬件框架

bd8a11b731acdbc7ae0d4461b9d59df2.png

2. SLAM建图

• RTAB-MAP位姿估计：（real-time appearance-based mapping），将深度相机和激光雷达的特征描述符放入相同的视觉词袋进行联合，采用优化框架Bundle Adjustment将特征点的匹配关系和位姿变换进行联合优化，通过高斯牛顿法最小化重投影误差Ｅ，得到最优位姿估计。
• 萤火虫算法（FA）：通过模拟萤火虫之间的相互吸引行为来寻找最优解,吸引度函数用于描述萤火虫之间的吸引强度
• 基于FA优化GMapping算法：萤火虫算法引导重采样过程，通过选择有效粒子数 Neff为优化目标函数，对建图精度进行优化
  1. 生成重采样阈值：随机生成一组初始萤火虫
  2. 计算退化度：对每个重采样阈值，计算目标粒子退化度
  3. 计算吸引度：根据目标和当前值的间距，计算吸引度函数，亮度高的萤火虫吸引周围较暗的萤火虫向其移动
  4. 更新重采样阈值：根据吸引行为，更新重采样阈值使粒子退化度向目标值的方向移动
  5. 局部优化：更新粒子退化度时加入随机数，对部分重采样阈值进行局部搜索
  6. 更新退化度：对新的重采样阈值重新计算粒子退化度，即更新亮度
  7. 迭代：重复执行以上步骤，直至建图精度达到要求。

  

  e3bcb95b08adde8eaeec119c0a34af8e.png

• SLAM三维建图：

6b4302bf2ab971dac0cba05959183b2d.png

3. 路径规划

• 传统 Ａ* 算法：启发式搜索算法

d82eac3ddfae11430c9b2e5fca04896a.png

• 分层并行 Ａ* （HPA*）算法：在 Ａ* 算法基础上引入多层次地图和并行计算，在高层次上快速规划，逐渐细化到低层次，减少计算复杂度，通过并行计算拓展多个节点，提高计算效率

6d727bdd442d4e77accb4136ecf0fdbe.png

• 导航方案对比：
  • 方案一：FA + GMapping + AMCL + HPA* + DWA
  • 方案二：FA + GMapping + AMCL + HPA* + DWA + EKF + RATB-MAP
  • 方案三：GMapping + AMCL + A* + DWA

  

  5fb18ca7f5bb133272c6058f09eab761.png