EKF融合IMU的Cartographer SLAM建图

REF：基于多传感器融合的室内移动机器人导航方法研究

1. 车辆模型

• 轮式机器人模型： 根据轮子的特性进行分类有差速机器人，阿克曼机器人，全向轮式机器人
  • 差速机器人：通过两个或多个驱动轮的速度差来实现转向和移动
  • 阿克曼转向：现代汽车底盘的标准结构，后轮是驱动轮，采用电子差速。前轮是转向轮，采用舵机进行转向
  • 全向轮式机器人：在平面内实现任意方向移动和旋转，安装全向轮或者麦克纳姆轮

  

  606d618e8c92cc2feb7f68b2ebc2c12d.png

• 机器人硬件框架： 采用阿克曼的转向结构

af7db40eccd7a606fefde8647f0b39fe.png

• 机器人 URDF 模型： 根据连杆link和关节joint的定义，计算机器人的关节角度、末端执行器的位置和姿态等运动学参数，为机器人的控制和轨迹规划提供理论基础

2d3ac6f5274a2db45aad4bc3f3af613f.png

• 车辆运动模型： 通过航迹可以推算出移动机器人前进速度𝑣、转向角速度𝜔，机器人在世界坐标系中某相邻两个时刻的位姿示意图
  • linear：表示轴向上的线速度大小
  • angular：表示轴向上的角速度
  • covariance：速度协方差矩阵，描述速度不确定性

  

  8cb5e8fb38863262a232909c24cbfadc.png

  • position：用 x、y、z 表示机器人的位置信息
  • orientation：用四元数 x、y、z、w 表示机器人当前姿态
  • covariance：位姿协方差矩阵，描述位姿不确定性
  • pose：机器人位姿信息
  • twist：是机器人的速度信息

2. Cartographer实现SLAM建图

• 数据融合： 传感器数据融合主要分为以下三种，数据级融合，特征级融合和决策级融合

6d5456fc22aa28e2dae501158071e86b.png

• EKF融合IMU 和轮式里程计数据： 里程计作为机器人位置估计的依据，因轮子的打滑、空转等因素，数据可能存在累积误差；IMU 主要提供轴向加速度、角速度和航向角信息，可以帮助确定机器人的姿态和运动状态，但其测量也会受到噪声和漂移的影响。

fd45828b8a60582c2332cca4768e6484.png

e49ec7f4b37176d2a45591fddc7ab697.png

• 传统Cartographer 算法： 包含局部 SLAM（前端）和全局 SLAM（后端）两部分，融合后的数据作用于局部 SLAM，通过优化位姿匹配过程提升地图构建的整体质量
• EKF融合IMU优化Cartographer： 通过融合轮式里程计和 IMU 数据来预测机器人的位姿，最新雷达点云数据与子图进行扫描匹配，确定机器人在地图中的最佳位姿，随后将扫描帧插入到子图中以更新地图，后端负责全局地图的优化。

3b3bd7cdbdb6a5284b0a9a16315e513c.png

3. A*和TED实现路径规划

• 传统A*算法： 结合了广度优先搜索的系统性和深度优先搜索的快速性优点，能够高效地在复杂环境地图中找到从起始节点到目标节点的最优或近似最优路径。
• 移动平均滤波平滑路径： 移动平均滤波使用固定大小的窗口在路径点序列上滑动，包含了一定数量的相邻路径点，是一种在时间序列或空间序列数据处理方法。
• 优化启发函数：搜索空间中快速地朝着最优解的方向进行搜索，避免了扩展大量不必要的节点

841673d1998eb4ea4fba1dbbc5652881.png

• 传统TED算法： 将机器人的路径表示为一个弹性带，一系列相连的路径点像弹性带上的珠子，当路径受到环境障碍物或机器人在时间及空间维度的约束影响时，弹性带会自适应地调整形状，最终以找到一条最优路径。

7ea5f47bd32b6e70f32d8a3084b9330f.png

9d5c56f4b5ff086f60f8a9e082d4b34d.png