一款基于鲁班猫和STM32的自主导航实践

REF：基于多传感器融合的救援机器人SLAM地图重建与路径规划研究

1. 系统设计

6c48525bd8447ecb74b805d88c853d60.png

• 硬件

8b31cc19e2d92d5364c3e70b113ac932.png

• 软件

d15dee4b4d5a5315d5303ccfd09b605a.png

• ORB-SLAM2 建图算法

db8c2668bf8ab96ab1a9cc0a824bf7a5.png

• 导航算法

61e0c55c63f69395a6c3b7d4d5dc6d65.png

2. 改进SLAM建图

• 改进 ORB-SLAM2 建图算法：在原有运动估计模型中引入两个关键优化因子，通过平移变换因数和转角综合变换因数作为双因素评价机制，平移变换因数与转角综合变换因数之间满足互不影响

f1fe791d46a8f34512b0acccabaa64b6.png

其中：

• α\alphaα：是平移变换因数，随着摄像头平移量增大而变大，两者呈线性正相关
• β+γ\beta + \gammaβ+γ：是转角综合变换因数，由两个独立分量组成
• β\betaβ：左旋转变换因数，表示相机向左旋转的程度，随左转角度增加而增大，当无左转时为 0，在(−π2,0)( -\frac{\pi}2 , 0)(−2π,0)范围内有效
• γ\gammaγ：右旋转变换因数，表征相机向右旋转的程度，随右转 角度增加而增大，当无右转时为 0，在(0，π2)(0，\frac{\pi}2)(0，2π)范围内有效

d019401d2be5abfaec05ad56ff43cea7.png

• 激光雷达与深度相机联合标定：深度相机是三维数据，激光雷达是二维数据，联合标定实现激光雷达和深度相机的关联。通过动态调整标定板的空间位置，采集多组不同视角的点云数据与图像信息，在数据采集完成后，采用 Autoware 标定工具包，对记录的数据包进行回放，并从中选取具有代表性的图像帧进行参数优化。

37c35310e7c2e0849f13b24e04cb7fdc.png

6aa3a70174601424e0a9239b9023a5eb.png

• ORB-SLAM融合激光雷达建图：采用贝叶斯估计方法实现激光雷达与深度相机数据的融合，首先将激光雷达的二维地图与深度相机的三维点云进行空间对齐，对三维点云进行二维投影，建立统一的栅格地图表示；然后基于贝叶斯概率理论，通过后验概率统计实现传感器数据融合。

030cc56155cebb2c3fbcfaa417d98739.png

3. 改进路径规划

• Astar启发函数距离优化：传统的 Astar算法选择欧氏距离作为启发函数表达式，路径容易发生转向，选择曼哈顿距离作为算法的启发函数

cf3f032511b2b65d5d95fa6767ea9612.png

• 启发函数权重优化：基于实时传感器数据动态调整启发式函数，以提高路径规划的准确性和效率，角度表示起终点连线跟当前位置跟终点连线的夹角

80f8d0d59496e0b97c11265315fd81d6.png

• 删除冗余节点：
  • 如果相邻三个节点连线在同一条直线上，则删除中间位置节点；
  • 如果跳过一个节点后，下一个节点的连线夹角较小，则删除被跳过的节点

  

  cefab63779e094c6fa1457f45a4fceb7.png

• 路径平滑度优化：使用三次 B 样条曲线法对路径进行平滑处理

8d4c78be3c73e6550b3eb95dd5f694bc.png

68e34d441b51ed5e11d3b79847a37e84.png

• 融合局部路径规划DWA：机器人路径规划算法Astar用来规划全局路径，不能实时避开突发性障碍物，当出现未知的静止或移动障碍物时，动态窗口法（DWA）可以避开局部路径规划中的动态障碍物。

8a06366215fefb9b2e45915d81b86228.png