基于Cartographer建图、Fast_LIO2-ICP定位及A*-TEB路径规划的导航方案

REF：基于 ROS2 平台的室内移动机器人导航系统设计与研究

1. 基础模型

• SLAM 问题模型

a9c48057668aa20c9782abc11d4e182e.png

• 机器人Link 与 Joint关系模型

0692875c3aab2fe253e2cdde1e40461a.png

• ROS2 框架结构

561022fdae46c25604f62ba9c678ce85.png

• ROS2 通信机制架构

ff80708f1f60d0ab37463075f9139071.png

• 导航系统整体框架

e6fbb816265fee5ee384bf291c49c418.png

• 机器人运动框架

a356c74be700f073f9fd02f55f7a6496.png

• 常用距离模型

f8cd6c9fcf72954dcb57f73877eb87cc.png

• Navigation2 导航框架

149e3f0dc9cde23a46909a1fce047d95.png

2. Cartographer SLAM实现建图

• 传统Cartographer算法： 在建图时引入了子图(Submap)，将雷达扫描获得的位姿数据(Scan)与现有子图使用非线性优化进行对齐（扫描匹配(Scan Matching)），最佳估计位姿加入到现有子图成为下一帧的子图，所有子图组合在一起成为了一个完整的地图。

4ce15204c6ba9d17efd20f72f377a18f.png

• 传统Cartographer遇到的问题： 快速原地旋转使 Cartographer 算法建图难以相互匹配，因为激光雷达发射和接收激光束存在时间间隔，激光束是以扇形发射的，而机器人在这个过程中发生运动，激光雷达发射位置和接收位位置不同，解算出的激光束回波时间存在一定误差，导致物体位置解算存在误差，若机器人移动速度越快，则地图漂移和畸变越严重。

26fbeab881f937da887636201603b827.png

• EKF融合IMU优化Cartographer： 通过虚拟串口获取型号为 GY95T的 IMU的加速度和速度信息，利用扩展卡尔曼滤波将 IMU 数据与车轮里程计数据融合，对里程计数据进行修正，生成新的机器人实际观测里程计数据，并以/odom 发布

eceb1cd5c38c194e96fa176ba85e4631.png

3. 融合Fast_LIO2和ICP实现3D定位

• Fast_LIO2算法构建三维点云地图： 建立在高效紧耦合迭代卡尔曼滤波器的基础上，一种快速、稳健且通用的激光惯性里程计框架，可以容许原始点直接注册到点云地图上，即使机器人在混乱的环境中剧烈运动，也能够更加准确可靠的匹配地图
• 地面分割提取算法（Linefit ground segmentation， LGS）： 基于线性拟合的方法实现地面点与非地面点的分离，由水平面或者近似水平面组成的，通过对每个点拟合直线进行判断，将与地面足够接近的点标记为地面点，而非地面点则被视为噪声或障碍物。
• 迭代最近点(Iterative Closest Point，ICP)算法定位： 通过迭代计算找到两个点云数据之间的最小欧式距离，目的是配准两帧激光点云信息，解算出上一时刻和当前时刻机器人的未知关系，其中误差函数为最小二乘法

f186f06a09777ec0f9f0c2d0c34b9f9e.png

4. 融合A*和TEB实现路径规划

• 使用切比雪夫距离优化A*启发函数：

aa9d15eb4e6a9c6de6aebba159907de9.png

• TEB 算法： 规划路径时，生成一系列带有时间信息的离散位姿，通对跟随全局路径、最小化轨迹运行时间、路径最优化、远离障碍物、机器人速度与加速度等多种动态约束进行加权多目标优化，以获取最佳路径。

6aa6b225fe4c5d1d626f8ca0cde2ddb8.png

• 跟随路径：跟随全局路径使TEB 规划路径更倾向于全局路径
• 避障约束：使规划路径与障碍物之间保持一定的安全距离

5ad186a47f5f9121e61d579e1335bc35.png

• 速度和加速度约束：

f844534d816bcb62af69441f9c5a197a.png

• 最快路径约束：机器人在路径上的每个位姿之间按照一定的时间间隔均匀分布，路径最短的情况通常是为了实现最短时间路径

f9b2adfc8f42e461f02cad5167f44bfc.png

• 运动学约束：局部路径由多个类似弧线一样的小段组成的流畅轨迹

68618d42be2ec00d9e9c57284010c58d.png