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

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

1. 系统架构

• 机器人硬件框架

  

• 机器人系统软硬件框架

  

2. SLAM建图

• RTAB-MAP位姿估计：（real-time appearance-based mapping），将深度相机和激光雷达的特征描述符放入相同的视觉词袋进行联合，采用优化框架Bundle Adjustment将特征点的匹配关系和位姿变换进行联合优化，通过高斯牛顿法最小化重投影误差Ｅ，得到最优位姿估计。

• 萤火虫算法（FA）：通过模拟萤火虫之间的相互吸引行为来寻找最优解,吸引度函数用于描述萤火虫之间的吸引强度

• 基于FA优化GMapping算法：萤火虫算法引导重采样过程，通过选择有效粒子数 Neff为优化目标函数，对建图精度进行优化

  1. 生成重采样阈值：随机生成一组初始萤火虫

  2. 计算退化度：对每个重采样阈值，计算目标粒子退化度

  3. 计算吸引度：根据目标和当前值的间距，计算吸引度函数，亮度高的萤火虫吸引周围较暗的萤火虫向其移动

  4. 更新重采样阈值：根据吸引行为，更新重采样阈值使粒子退化度向目标值的方向移动

  5. 局部优化：更新粒子退化度时加入随机数，对部分重采样阈值进行局部搜索

  6. 更新退化度：对新的重采样阈值重新计算粒子退化度，即更新亮度

  7. 迭代：重复执行以上步骤，直至建图精度达到要求。

     

• SLAM三维建图：

  

3. 路径规划

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

  

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

  

• 导航方案对比：

  • 方案一：FA + GMapping + AMCL + HPA* + DWA

  • 方案二：FA + GMapping + AMCL + HPA* + DWA + EKF + RATB-MAP

  • 方案三：GMapping + AMCL + A* + DWA