融合RTK的路径规划方案

ref：基于深度视觉 SLAM 算法的轮式移动机械手时间最优轨迹规划方法 系统架构： 系统包括深度视觉 SLAM 模块、点云地图构建和导航避障3个模块：

8807174879ed8e8a243d841e56598052.png

机器人位姿估计： 基于激光雷达和 IMU 的紧耦合雷达惯性里程计：

1. 对激光雷达点云特征进行提取，以获得关键帧;
2. 对 IMU 测量数据进行 IMU 预积分处理，得到 IMU预积分因子;
3. 基于 IMU 预积分后的预测位姿校正点云畸变，以获得雷达里程计因子;
4. 将校正后的关键帧构成局部子图，并采用帧间匹配法匹配关键帧与子图，获得闭环约束因子;
5. 将 IMU 预积分因子、雷达里程计因子和闭环约束因子添加至因子图中，采用 iSAM2 进行因子图优化，实现机器人位姿估计

全局地图构架： 通过RTK绝对测量数据传感器引入ＲTK 因子进行图优化，解决长时间工作状态下出现缓慢漂移的问题，该传感器基于载波相位观测的实时差分 GPS 技术，实现各控制测量，ＲTK 系统主要分为 INS-D-OEM单元、4G 通信模块和一个 GNSS 主天线和一个从天线 4 个部分，其水平定位误差仅为 1. 83 cm。

全局路径规划： A－Star 算法为静态路网中求解最短路径的直接搜索算法，其也是一种启发式算法，在当前搜索节点往下一节点选择时，通过建立启发函数选取距离目标代价最小的点作为下一搜索节点。

1. 引入基于象限的节点扩展法，减缓搜索范围过大和搜索节点过多的问题，计算目标点和当前点的坐标差值确定其象限，使节点只朝目标节点所在象限扩展。
2. 引入转向惩罚因子，增加路径的平滑性，以减少转弯次数

3e000782d784577286de08ea2fd2729e.png

局部路径规划： DWA 算法作为局部路径规划算法中的经典算法，通过计算速度和角速度的方式对机器人位置进行表示，从而快速得到下一规划轨迹的最优解，具体流程为：首先对初始值进行赋予，然后进行自适应权重调整，计算动态窗口并采样; 之后进行轨迹计算和评价，选出最优轨迹对应的采样速度，根据最优速度运行移动，判断是否达到目标点，若是则可完成局部路径规划，否则重新进行初始赋值。

1. 引入评价子函数（引入角速度、线加速度、角加速度评价子函数），减缓在遇到障碍物时，机器人为进行避障，其角速度方向频繁变换从而出现抖振的现象，避免陷入局部最优解，导致机器人局部路径规划失败。
2. 引入自适应权重调整，对评价函数中的角速度和方位角项使用自适应权重代替传统固定权重，提高机器人遇到障碍物后的移动安全性和适应度能力。