一、项目开源地址

https://github.com/jiale67/autonomous_navigation

视频：【开源】基于RRT*全局搜索的移动机器人自主导航系统_哔哩哔哩_bilibili

二、局部规划器实现

CMU团队设计的局部规划器摒弃了传统DWA、MPC等优化算法实时求解最优路径的做法，而是提前使用Matlab通过采样的离散点做前向模拟三次样条生成路径点，在考虑车辆动力学模型和转弯半径下生成7×7×7=343条路径。

对于碰撞检查，使用覆盖了传感器范围的体素网格。根据样条距离，传感器范围为3.2*4.5,在该区域生成体素网格，如下图所示，在这里考虑了车辆半径的遮挡。         

                                

机器人在运行过程中通过将当前帧点云数据映射到体素网格中，对每条路径进行评分，剔除掉存在障碍遮挡的路径，选择出代价最小的路径执行。

三、局部规划局限性

local_planner本身已经足够强大，完全可以满足大多数简单环境下的导航，但是对于迷宫这类复杂环境由于缺少全局规划器的指引，只使用local_planner容易使机器人陷入死区，如下图：

机器人在一面墙的前方（rviz中表示坐标系），发送目标点是墙的另一侧（rviz中粉色圆点），由于避障约束远远强于其他约束，导致机器人一直在来回摆动，无法到达指定导航点。

针对这种情况，可以给local_planner加上全局规划器来解决这个问题，一些比较常见的A*、RRT等算法都可以实现，但是都需要先验地图的支持，需要先通过雷达数据建出地图。

四、RRT*全局规划器实现

             RRT算法是比较经典的搜索算法，选择牺牲一部分路径质量换取了搜索效率，具体实现流程 如下：

复杂全局路径规划采用 RRT 地图采样策略，区别在于采用偏向采样策略而不
是随机采样。每次采样时，有概率 p 直接采用目标点作为采样点，采样随机空间自
由点的概率 1 − p。若 p 设为 0.2，每 5 次迭代至少 1 次强制向目标扩张，树以 20%
的概率直线延伸至目标方向

 复杂全局路径规划采用 RRT 地图采样策略，采用偏向采样策略而不是随机采样。每次采样时，有概率 p 直接采用目标点作为采样点，采样随机空间自由点的概率 1 − p。若 p 设为 0.2，每 5 次迭代至少 1 次强制向目标扩张，树以 20%的概率直线延伸至目标方向。

本项目采用的是RRT*算法来实现全局路径规划。相比于RRT，RRT* 的核心改进在于引入了“重布线”机制。它不仅像 RRT 一样快速扩展随机树以找到可行路径，还会在树生长过程中不断检查并优化已有路径：当发现某个新节点能为周围节点提供更短的路径时，就重新连接它们。这一机制使得 RRT* 找到的路径能够随着时间推移不断被“拉直”和优化，从而渐进地逼近理论上的最短路径，而 RRT 找到的路径一旦生成就不再改变。

考虑到local_planner的特殊性，本项目摒弃了传统覆盖全局路径执行规划的策略，而是赋予local_planner更高的自主权，而全局规划器生成的RRT*路径只提供方向指引。全局规划器规划出全局路径后不进行平滑处理，而是将路径拐点提取出来发送给local_planner，local_planner依次将全局路径拐点作为阶段性航点进行追踪，最终指引机器人完成导航任务。

该导航框架在圆柱障碍和迷宫环境进行测试，均取得较好的效果。