前言

• 上一期我们实现了 Hybrid-A* 全局规划器，它把机器人建模为自行车，在 $(x,y,\theta )$ 三维空间中搜索——路径天然满足运动学约束，还支持倒车。
• 往期内容：
  • 第一期：【10天速通Navigation2】(一) 框架总览和概念解释
  • 第二期：【10天速通Navigation2】(二) ：ROS2gazebo阿克曼小车模型搭建-gazebo_ackermann_drive等插件的配置和说明
  • 第三期：【10天速通Navigation2】(三) ：Cartographer建图算法配置：从仿真到实车，从原理到实现
  • 第四期：【10天速通Navigation2】(四) ：ORB-SLAM3的ROS2 humble编译和配置
  • 第五期：【10天速通Navigation2】(五) ：基于gazebo仿真的复杂地形的ORB-SLAM3配置
  • 第六期：【10天速通Navigation2】(六) ：Navigation2基础配置与参数解析
  • 第七期：【10天速通Navigation2】(七) ：Hybrid-A*全局规划器的原理推导与Nav2插件实现
• 本教材将贯穿nav2的全部内容，使用ROS2和C++实现一些仿真乃至实车中常见的建图和路径规划算法，例如cartographer,ORB-SLAM,RRT,hybrid-astar。我们将注重与原理讲解和代码实现，去详细讲解每一步的配置过程和代码复现细节。
• 同理本教材默认大家有一些基础的ROS2和C++的编程基础，故不对一些基础部分进行详细说明。
• 本教程使用的环境：
  • ROS2 humble
  • ubuntu 22.04 LTS
• 本期我们将换一种思路——不再用网格覆盖整个空间，而是用随机采样来"摸清"环境。这就是采样规划（Sampling-based Planning）的核心思想。我们将实现两个算法：
  • RRT（Rapidly-exploring Random Tree）：快速随机探索树，用一棵不断生长的树来连接起点和终点
  • RRT*（RRT Star）：在 RRT 的基础上加入"择优父节点"和"重连接"两步，使路径渐进收敛到最优
• 两者都以插件形式注册到 Nav2 中，可以随时切换。本文环境同样沿用上一期的 linorobot2 4WD 仿真平台。最终效果如下：

---

1 为什么需要采样规划

• Hybrid-A* 是高维搜索中兼顾精度和运动学的典范。但它有一条底线：必须对状态空间做网格离散化。一旦维度超过 3（比如加入速度、加速度、时间），网格数量爆炸，A* 类方法直接不可用。

• 采样规划的思想完全不同：

不在网格上一步步走，而是随机撒点，把撒到的自由点用线段连起来——只要起点和终点在同一个连通分量里，就找到了一条路径。

• 说人话就是：

把机器人扔进一个黑屋子，不要求它一步步摸清每块地砖——它只需要随机往黑暗里摸，摸到哪算哪，直到摸到门为止。

• RRT 和 RRT* 的共同特点：

特点	说明
不需要显式离散化	直接在连续空间采样，天然支持高维
概率完备	只要存在路径，采样次数足够多就一定能找到
碰撞检测简单	只检测采样点和线段，不需要遍历网格
RRT* 渐进最优	采样越多，路径越接近理论最优

---

2 RRT 快速随机探索树

2-1 核心思想

• RRT（Rapidly-exploring Random Tree）由 Steven LaValle 在 1998 年提出。算法极其简单——四个字：撒点、连线。

• 每一步迭代做三件事：

  1. 随机采样（Sample）：在地图空闲区域随机撒一个点
  2. 找最近节点（Nearest）：在已有的树里找到离采样点最近的节点
  3. 扩展一步（Steer）：从最近节点朝采样点方向走一步（步长 ≤ max_step），如果路径无碰撞，就把新节点加入树

• 重复上述过程直到树的某个节点距离目标足够近，然后从该节点沿着父节点链回溯到起点，就得到了一条路径。

• 说人话就是：

想象一棵树从起点开始生长。每轮迭代，随机往地上扔一个飞镖，树就朝飞镖的方向长一小节。成千上万轮之后，树的枝干就会"填满"整个可达空间——其中一定有一根枝条碰到了终点。

2-2 Node 结构体与树的构成

• 要理解 RRT，先搞清楚它的数据结构。RRT 中的"树"不是一个真正的树形数据结构——没有指针、没有递归嵌套。它只是一个 扁平的 std::vector<Node>，完全靠 parent_idx（父节点在数组中的下标）来组织父子关系。

struct Node
{
  double x, y;        // 节点的 2D 坐标
  int parent_idx;     // 父节点在 tree_ 数组中的下标（起点 = -1）
};

• 树的结构（以 6 个节点为例）：

tree_[0]  (x=start, y=start, parent=-1)    ← 根节点（起点）
     ↑ parent_idx=0
tree_[1]  (x=..., y=..., parent=0)
     ↑ parent_idx=1
tree_[2]  (x=..., y=..., parent=1)
     ↑ parent_idx=0          
tree_[3]  (x=..., y=..., parent=0)    
     ↑ parent_idx=3
tree_[4]  (x=..., y=..., parent=3)
     ↑ parent_idx=3
tree_[5] (x=..., y=..., parent=3)

• 每个节点只知道"我是谁（x, y）“和"我爹在哪（parent_idx）”。整棵树存在 std::vector<Node> tree_ 中——数组的每个元素是一个节点，数组下标就是节点 ID。不需要显式存边、不需要指针、不需要 children 列表。

• 回溯路径就是沿着 parent_idx 链从终点一路爬到起点，再把结果翻转：

nav_msgs::msg::Path extractPath(const std::vector<Node> & tree, int goal_idx)
{
  std::vector<Node> reversed;
  int idx = goal_idx;
  while (idx >= 0) {
    reversed.push_back(tree[idx]);
    idx = tree[idx].parent_idx;       // 顺着父链往上爬
  }
  std::reverse(reversed.begin(), reversed.end());   // 翻转成起点→终点
  // ... 转换为 nav_msgs::Path ...
}

• 说人话就是：

RRT 的树就像一串用"爹的下标"串起来的糖葫芦。每个节点只知道自己的坐标和谁是它爹。要找完整路径，就从终点开始反复问"你爹是谁"，一路问到起点。

• 数据结构极简的好处有两个：

  1. 内存布局紧凑：所有节点在内存中连续排列（std::vector），缓存友好，遍历快
  2. 最近邻搜索简单：只需遍历数组，计算平方距离，找最小值——代码不到 10 行

• 后续的四个关键操作（采样、最近邻、扩展、碰撞检测）本质上就是围绕这个 tree_ 数组增删节点。

2-3 算法流程伪代码

树 ← {起点}

for iter = 1 to max_iterations:
    if rand() < goal_bias:    采样点 = 目标点（目标偏置）
    else:                     采样点 = random_free_point()

    最近节点 ← nearest(树, 采样点)
    新节点 ← steer(最近节点, 采样点, max_step)

    if is_line_free(最近节点, 新节点):
        新节点.parent ← 最近节点
        树.添加(新节点)

        if distance(新节点, 目标) < tolerance:
            return 回溯路径(新节点)

2-4 四个关键操作

2-4-1 随机采样 sampleFree

• 在地图的空闲区域随机生成一个点。为了避免死循环，最多尝试 1000 次：

bool sampleFree(double & x, double & y)
{
  std::uniform_real_distribution<double> dist_x(origin_x_, origin_x_ + size_x_ * resolution_);
  std::uniform_real_distribution<double> dist_y(origin_y_, origin_y_ + size_y_ * resolution_);

  for (int attempt = 0; attempt < 1000; ++attempt) {
    x = dist_x(rng_);
    y = dist_y(rng_);
    if (isPointFree(x, y)) return true;   // 命中了空闲区域
  }
  return false;  // 1000 次都撞到障碍物，地图基本堵死了
}

• 目标偏置 goal_bias：纯随机采样有一个致命问题——目标区域只是一个半径 0.3m 的圆，面积约 0.28m²，在 10m×10m=100m² 的地图上占比不到 0.3%。随机撒一个点恰好落在目标区域里的概率极低，即使树已经扩展到目标附近，纯随机采样也可能绕着目标打转几百轮都收束不了。
• goal_bias_ 的解决方案很直接：以概率 goal_bias_（默认 10%）直接用目标点作为本轮采样点，让树每 ~10 轮就有一次机会朝终点方向扩展一步。90% 的随机性保留绕障碍的能力，10% 的偏置确保树不会在终点跟前"迷路"。*

if (bias_dist(rng_) < goal_bias_) {
  rx = goal_x; ry = goal_y;          // 10% 的概率直接瞄准目标
} else if (!sampleFree(rx, ry)) {
  continue;                          // 采样失败则跳过本轮
}

• goal_bias 越大收敛越快，但太大就退化成"直直往终点冲"——失去了随机探索绕障碍的能力。0.05~0.15 是比较平衡的范围。

2-4-2 找最近节点 nearestNeighbor

• 在全部树节点中找离采样点最近的那个——暴力线性搜索：

int nearestNeighbor(double x, double y)
{
  int best = -1;
  double best_dist = std::numeric_limits<double>::max();
  for (size_t i = 0; i < tree_.size(); ++i) {
    double d = (tree_[i].x - x) * (tree_[i].x - x) + (tree_[i].y - y) * (tree_[i].y - y);
    if (d < best_dist) { best_dist = d; best = static_cast<int>(i); }
  }
  return best;
}

• 这里没有用距离开方（std::sqrt），因为只做比较不需要精确值——平方距离就能判断远近，省了开方运算。RRT 的树通常几千个节点，线性搜索 $O(N)$ 完全够；如果上万节点可以换成 KDTree。

2-4-3 扩展一步 steer

• 从最近节点朝采样点走，但步长不超过 max_step_：

Node steer(const Node & from, const Node & to)
{
  double dx = to.x - from.x;
  double dy = to.y - from.y;
  double dist = std::sqrt(dx * dx + dy * dy);

  Node result;
  if (dist <= max_step_) {
    result = to;                                   // 采样点很近，直接取它
  } else {
    result.x = from.x + dx / dist * max_step_;     // 朝采样点走 max_step 米
    result.y = from.y + dy / dist * max_step_;
  }
  return result;
}

• max_step_ 控制树的生长粒度。太大 → 一次跨太远，容易撞墙被拒绝，树长得稀疏；太小 → 节点太密集，收敛慢。默认 0.5m 是个平衡值。

2-4-4 碰撞检测 isLineFree

• 检查从 A 到 B 的线段是否全程无碰撞。原理是等步长插值采样：

bool isLineFree(double x1, double y1, double x2, double y2)
{
  double dist = std::sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
  int steps = std::max(1, static_cast<int>(dist / (resolution_ * 0.5)));
  for (int i = 0; i <= steps; ++i) {
    double t = static_cast<double>(i) / steps;
    double x = x1 + t * (x2 - x1);
    double y = y1 + t * (y2 - y1);
    if (!isPointFree(x, y)) return false;   // 线段上的某个插值点位于障碍物内
  }
  return true;
}

• 步长取 resolution * 0.5（约 0.025m）保证碰撞检测的精度不低于代价地图分辨率。

2-5 路径剪枝 prunePath

• RRT 生成的原始路径弯弯绕绕，因为树是随机涨出来的。用视线剪枝（Line-of-Sight Pruning）做后处理：从起点开始，依次尝试跳到最远的可见节点——如果两点之间直线无碰撞，中间所有节点都可以跳过去。

void prunePath(nav_msgs::msg::Path & path)
{
  nav_msgs::msg::Path pruned;
  pruned.poses.push_back(path.poses.front());        // 保留起点

  for (size_t i = 1; i < path.poses.size() - 1; ++i) {
    const auto & last = pruned.poses.back().pose.position;
    const auto & next = path.poses[i + 1].pose.position;
    if (!isLineFree(last.x, last.y, next.x, next.y)) {
      pruned.poses.push_back(path.poses[i]);          // 看不到下一个点，保留当前点
    }
  }
  pruned.poses.push_back(path.poses.back());          // 保留终点
  path = pruned;
}

• 说人话就是：

原始路径像一根弯来弯去的铁丝。剪枝 = 拉一根绳子从起点到终点，绳子碰不到障碍物的地方就可以"跳过"中间那些弯。最终路径保留的只是绳子碰到障碍物前必须转弯的那些拐角。

2-6 完整主循环

nav_msgs::msg::Path RRTPlanner::createPlan(
  const geometry_msgs::msg::PoseStamped & start,
  const geometry_msgs::msg::PoseStamped & goal)
{
  // ---- 初始化 ----
  tree_.clear();
  Node root; root.x = start.pose.position.x; root.y = start.pose.position.y;
  root.parent_idx = -1;
  tree_.push_back(root);

  int best_goal_idx = 0;
  double best_goal_dist = std::numeric_limits<double>::max();

  // ---- 主循环 ----
  for (int iter = 0; iter < max_iterations_; ++iter) {
    if (timeout) break;

    // ① 采样（目标偏置）
    double rx, ry;
    if (rand_01() < goal_bias_) { rx = goal_x; ry = goal_y; }
    else if (!sampleFree(rx, ry)) continue;

    // ② 最近邻
    int nearest = nearestNeighbor(rx, ry);

    // ③ 扩展
    Node target; target.x = rx; target.y = ry;
    Node new_node = steer(tree_[nearest], target);

    // ④ 碰撞检测
    if (!isLineFree(tree_[nearest].x, tree_[nearest].y, new_node.x, new_node.y))
      continue;

    // ⑤ 加入树
    new_node.parent_idx = nearest;
    tree_.push_back(new_node);

    // ⑥ 检查目标
    double d = distance(new_node, goal);
    if (d < best_goal_dist) { best_goal_dist = d; best_goal_idx = tree_.size() - 1; }
    if (d < goal_xy_tolerance_) break;     // 到达目标！
  }

  // ---- 回溯 + 剪枝 + 插值 ----
  auto path = extractPath(tree_, best_goal_idx);
  prunePath(path);
  // ... 路径插值省略 ...
  return path;
}

• RRT 的复杂度分析：
  • 每次迭代：$O(N)$ 最近邻搜索（$N$ = 当前树大小）
  • 总复杂度：$O(K^2)$ 其中 $K$ 为迭代次数
  • 空间：$O(K)$

2-7 RRT 的缺点

• RRT 找到的是"某一条"可行路径，不保证最优。树一旦通过某个节点连上了终点，那一枝就被固定了——即使后来发现了更短的路径，也没有机制去修正。这就是 RRT* 要解决的问题。

---

3 RRT* 渐进最优的 RRT

3-1 核心思想

• RRT*（RRT Star）由 Karaman & Frazzoli（2011）提出，在 RRT 的基础上多了两步：

  1. ChooseParent（择优父节点）：新节点加入时，不只认最近的那个节点做父节点——而是在邻域内找一个"总代价最小"的节点做父节点
  2. Rewire（重连接）：新节点加入后，检查邻域内的其他节点——如果经由新节点到达它们的代价比当前代价更小，就把它们的父节点改为新节点

• RRT* 的理论保证：当采样次数趋于无穷时，路径代价收敛到全局最优。这就是"渐进最优"（asymptotic optimality）的含义。

• 说人话就是：

RRT 是"有路就走，不管好坏"。RRT* 是"有路之后，不停地优化这条路"——新来的节点会重新评估周围邻居的最短路径，如果发现有更短的走法，就把别人的父亲改掉，让大家一起"抄近路"。

3-2 Node 结构多了 cost 字段

• RRT 的 Node 只存 $(x,y,parent\_idx)$。RRT* 的 Node 额外存了 cost——从起点到该节点的累计路径长度：

struct Node
{
  double x, y;
  double cost;         // 新增：累计代价（路径长度）
  int parent_idx;
};

• cost 是 ChooseParent 和 Rewire 的基础——我们用路径长度作为代价，RRT* 优化的是总路径长度。如果考虑其他代价（如远离障碍物、曲率），可以在 cost 中额外加权。

3-3 自适应邻域半径

• ChooseParent 和 Rewire 都需要一个"邻域"——在其内部搜索。半径不能固定：树小时邻居太少，树大时邻居太多。

• RRT* 使用理论保证的衰减半径：

$$radius=\min \left(\gamma \cdot \sqrt{\frac{\log n}{n}},3\cdot max\_step\right)$$

其中 $n$ 是当前树的大小。$\sqrt{\log n/n}$ 随着树增大而缓慢减小——树越大，每个节点附近自然就有更多邻居，半径可以收窄。取 max_step * 3 作为上限防止早期树小时邻居太少。

double gamma = rewire_radius_;
double n = static_cast<double>(tree_.size());
double radius = std::min(gamma * std::sqrt(std::log(n + 1.0) / (n + 1.0)), max_step_ * 3.0);

• rewire_radius_（默认 3.0）控制基础搜索半径。实际使用中被 log(n)/n 压制，所以初期半径较大（_{2-3m），后期自动收窄（}0.5-1m）。

3-4 ChooseParent——择优父节点

• RRT 中，新节点直接认 nearest 做父亲。RRT* 遍历邻域内所有节点，找一条总代价最小的路径：

int chooseParent(double x, double y, const std::vector<int> & neighbors)
{
  int best = -1;
  double best_cost = std::numeric_limits<double>::max();

  for (int idx : neighbors) {
    if (!isLineFree(tree_[idx].x, tree_[idx].y, x, y)) continue;       // 必须无碰撞
    double cost = tree_[idx].cost + distance(tree_[idx].x, tree_[idx].y, x, y);
    if (cost < best_cost) {
      best_cost = cost;
      best = idx;
    }
  }
  return best;  // 返回代价最小的父节点候选，可能返回 -1（邻居都不可达）
}

• 说人话就是：新来的节点环顾四周，看哪个邻居能给自己最低的从起点到达这里的代价，就认它做爸爸。

3-5 Rewire——重连接

• 新节点加入后，反过来检查邻居：如果经由新节点到达某个邻居的代价比邻居当前路径更短，就把那个邻居的父节点改成新节点：

void rewire(int new_idx, const std::vector<int> & neighbors)
{
  for (int idx : neighbors) {
    double new_dist = distance(tree_[new_idx].x, tree_[new_idx].y, tree_[idx].x, tree_[idx].y);
    double cost_via_new = tree_[new_idx].cost + new_dist;

    if (cost_via_new < tree_[idx].cost &&
        isLineFree(tree_[new_idx].x, tree_[new_idx].y, tree_[idx].x, tree_[idx].y)) {
      tree_[idx].parent_idx = new_idx;   // 改爹
      tree_[idx].cost = cost_via_new;     // 更新代价
    }
  }
}

• 说人话就是：新来的节点加入后，对周围邻居说"你们试试从我这里走到起点，是不是更短？"——如果是，就改认我做父亲。

3-6 RRT* 主循环

for (int iter = 0; iter < max_iterations_; ++iter) {
  // ① 采样 + 最近邻 + 扩展（和 RRT 一样）
  sampleFree(rx, ry);
  int nearest = nearestNeighbor(rx, ry);
  Node new_node = steer(tree_[nearest], target);

  // ② 计算自适应邻域半径
  double radius = std::min(gamma * std::sqrt(std::log(n + 1.0) / (n + 1.0)), 3 * max_step_);

  // ③ 找邻居
  std::vector<int> neighbors;
  nearbyNodes(new_node.x, new_node.y, radius, neighbors);

  // ④ ChooseParent：择优父节点
  int best_parent = nearest;
  double best_cost = tree_[nearest].cost + distance(nearest, new_node);
  int opt_parent = chooseParent(new_node.x, new_node.y, neighbors);
  if (opt_parent >= 0) {
    double opt_cost = tree_[opt_parent].cost + distance(opt_parent, new_node);
    if (opt_cost < best_cost) { best_parent = opt_parent; best_cost = opt_cost; }
  }

  // ⑤ 加入树
  new_node.cost = best_cost;
  new_node.parent_idx = best_parent;
  tree_.push_back(new_node);

  // ⑥ Rewire：重连周围的树
  rewire(tree_.size() - 1, neighbors);
}

• 对照 RRT，RRT* 多了三步：算自适应半径（第②步）、ChooseParent（第④步）、Rewire（第⑥步）。代价是每轮多了一次邻域搜索——但 nearbyNodes 也是 $O(N)$ 线性扫描，和 nearestNeighbor 同级，并没有增加渐近复杂度。

3-7 RRT vs RRT* 对比

	RRT	RRT*
路径质量	可行但不优	渐进最优
计算量	每轮 $O(N)$	每轮 $O(N)$ + 常数
收敛速度	快（找到路径就停）	较慢（持续优化）
内存	$O(K)$	$O(K)$
额外参数	无	rewire_radius
适用场景	实时避障、快速规划	离线规划、短路径优先

---

4 在 Nav2 中替换

• 两个规划器已注册在 plugins.xml 中：

<class name="planner/RRTPlanner" type="planner::RRTPlanner"
       base_class_type="nav2_core::GlobalPlanner">
  <description>RRT global planner</description>
</class>
<class name="planner/RRTStarPlanner" type="planner::RRTStarPlanner"
       base_class_type="nav2_core::GlobalPlanner">
  <description>RRT* global planner with parent selection and rewiring</description>
</class>

• 在 navigation_sim.yaml 中切换：

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "planner/RRTPlanner"           # 换成 RRT
      # plugin: "planner/RRTStarPlanner"     # 或者 RRT*
      max_step: 0.5
      goal_bias: 0.1
      max_iterations: 10000
      max_planning_time: 5.0
      goal_xy_tolerance: 0.3
      # rewire_radius: 3.0                 # 仅 RRT* 需要

• 确认生效：

ros2 param get /planner_server GridBased.plugin
# planner/RRTPlanner  或  planner/RRTStarPlanner

• 启动与上一期完全一致：

./1_bringup.sh    # 仿真
./3_nav.sh        # 导航

• 在 RViz2 中添加 MarkerArray 话题 /plan/rrt_tree 或 /plan/rrt_star_tree，可以实时看到整棵树在空间中生长的过程。两种规划器发布的可视化内容相同：

元素	话题	显示方式	含义
树的边	~/rrt_tree	蓝色细线	每对父子节点之间的连线——展示树的分枝结构
采样节点	~/rrt_tree	绿色小球	树中的节点（每隔一定数量采样，避免太密）
最终路径	~/rrt_path	红色粗线	回溯到起点后，再加剪枝和插值的最终路径

• RRT 和 RRT* 的树看起来有明显的区别：
  • RRT：树枝随机向四面八方伸展，看起来很"毛茸茸"，粗细不均匀，路径经常弯弯绕绕。因为每个节点只认最近的那个爹，树的结构完全随机。
    
    
  • RRT*：树枝整体更"规矩"，尤其在起点到目标之间会形成一条相对明显的主干，主干两旁的侧枝更少。原因是 Rewire 不断把树枝"抄近路"归拢——视觉上就是树更紧凑、路径更短更直。

---

总结

• 本期实现了两种采样规划器——RRT 和 RRT*，都以 Nav2 插件形式集成。

1. RRT 的核心：四个操作走天下——sampleFree（采样）、nearestNeighbor（最近邻）、steer（扩展）、isLineFree（碰撞检测）。目标偏置加速收敛，路径剪枝消除冗余弯折。不保证最优，但够快。
2. RRT* 的进化：RRT 基础上多了 ChooseParent 和 Rewire 两步——新节点择优认父、反过来替周围邻居"抄近路"。理论上渐进最优，代价是每轮多一次邻域搜索。
3. 采样规划 vs 网格规划：采样规划不需要离散化状态空间，天然支持高维，适合复杂场景。和 Hybrid-A* 不是替代关系——低维精确场景用 Hybrid-A*，高维/大场景用 RRT/RRT*，各有用武之地。
4. 插件体系优势：三个规划器（Hybrid-A*、RRT、RRT*）都在同一个包 planner 中，改一行 YAML 即可切换。编译方法同上一期。

• 本期实现了 RRT 和 RRT* 两种采样规划器，解决了"如何找到一条路径"的问题。但找到了路径之后，怎么沿着这条路走——加速、减速、转向的时机和力度——属于控制器的范畴。下一期我们将进入最优控制领域，手写 LQR（线性二次型调节器）控制器，同样以 Nav2 插件的形式接入~
• 感谢支持！！！！
• 如有错误，欢迎指出！！！！！！