RRT 与 RRT* 不只是动画：碰撞检测、重连和可重复验证

为什么“画出一条线”还不够

RRT 很适合可视化：随机采样、寻找最近节点、向目标方向扩展，很快就能画出一棵树。但动画成功不代表规划器可靠。节点可能在障碍物外，连接节点的线段却穿过障碍物；找到目标附近节点，也不代表最后一段路径安全。

可靠实现需要明确空间边界、机器人尺寸、碰撞模型、终止条件和失败返回值，并为随机算法建立可重复测试。

基本扩展步骤

一次 RRT 迭代通常包含：

在地图范围内随机采样一个点。
找到树中距离采样点最近的节点。
从最近节点向采样点方向移动固定步长。
检查新节点和整条连接边是否安全。
安全时把新节点加入树，并记录父节点。
尝试连接目标或继续采样。

目标偏置可以让一部分采样直接选择目标点，从而加快搜索，但偏置过大可能降低绕开复杂障碍的能力。

步长影响的不只是速度

步长过大时，树扩展很快，但容易越过窄通道或产生大量碰撞边；步长过小时，路径更细致，却需要更多节点和距离计算。步长应结合地图尺度、障碍物间距和机器人半径设置。

扩展函数应避免除零，并保证实际移动距离不超过设定步长：

def steer(source, target, step_size):
    dx = target[0] - source[0]
    dy = target[1] - source[1]
    length = math.hypot(dx, dy)
    if length == 0:
        return source
    scale = min(1.0, step_size / length)
    return source[0] + dx * scale, source[1] + dy * scale

必须检查整条边

只检查端点是否位于障碍物内是不够的。对于圆形障碍物，可以计算圆心到线段的最短距离，并把机器人半径作为额外安全间隙。

def distance_point_to_segment(point, a, b):
    ax, ay = a
    bx, by = b
    px, py = point
    dx, dy = bx - ax, by - ay

    if dx == 0 and dy == 0:
        return math.hypot(px - ax, py - ay)

    t = ((px - ax) * dx + (py - ay) * dy) / (dx * dx + dy * dy)
    t = min(1.0, max(0.0, t))
    closest_x = ax + t * dx
    closest_y = ay + t * dy
    return math.hypot(px - closest_x, py - closest_y)

当距离小于“障碍物半径 + 机器人半径 + 安全余量”时，这条边应判定为碰撞。矩形障碍物也要考虑机器人尺寸，可以先对障碍物做膨胀，再把机器人当作质点规划。

目标连接要单独验证

新节点进入目标半径后，仍需检查新节点到精确目标点的最后一条边。如果最后一段穿过障碍物，不能因为“已经很近”就直接宣布成功。

成功结果应返回节点链或路径点列；达到最大迭代次数仍未找到路径时，应返回明确的无解状态，而不是空列表与程序错误混在一起。

RRT* 多了两次邻域计算

RRT* 在加入新节点前，会在一定半径内寻找候选父节点，选择累计代价最低且连接安全的节点作为父节点。加入后还会尝试把邻域节点重新连接到新节点，以降低路径代价。

候选父节点的比较不能只看局部边长，而要看从起点到候选节点的累计代价：

best_parent = nearest
best_cost = nearest.cost + distance(nearest.point, new_point)

for node in near_nodes:
    edge_cost = distance(node.point, new_point)
    candidate_cost = node.cost + edge_cost
    if candidate_cost < best_cost and collision_free(node.point, new_point):
        best_parent = node
        best_cost = candidate_cost

重连后要更新整棵子树

一个常见错误是只修改被重连节点的父节点和代价，却没有更新它的后代。这样树中的累计代价会失真，后续父节点选择也会基于错误数据。

实现时可以让节点保存子节点集合，重连后从该节点开始递归或迭代更新所有后代代价。更新前还要从旧父节点的子节点集合中移除它，避免树结构出现重复关系。

路径提取与平滑是两个阶段

从目标节点沿父指针回溯即可得到一条可行路径。路径平滑可以尝试跳过中间节点：若较远两个路径点之间的直线仍无碰撞，就删除中间段。

平滑只能在可行路径基础上进行，并且每一次捷径都必须重新调用同一套碰撞检测。平滑后的路径更短，但不应改变安全半径定义。

随机算法也能稳定测试

测试时固定随机种子，可以稳定复现树的扩展过程。建议至少准备以下地图：

无障碍地图，验证基础连接和目标终止。
单个障碍物地图，验证绕行与边碰撞。
窄通道地图，观察步长和安全半径影响。
封闭目标地图，验证最大迭代和无解返回。
起点或终点非法地图，验证输入检查。

GUI 与命令行模式必须调用同一个规划器核心。可视化层只显示节点、边和最终路径，不能复制一套不同的规划逻辑。

评价指标要说明随机性

单次运行的路径长度或耗时不能代表算法水平。更合适的记录包括多随机种子下的成功率、节点数、规划耗时和路径长度分布。若比较 RRT 与 RRT*，还要使用相同地图、随机种子集合、步长和碰撞规则。

从演示到工程还差什么

二维圆形或矩形障碍物适合教学，但真实机器人还需要处理运动学约束、转弯半径、动态障碍、定位误差和轨迹跟踪。现阶段最重要的是把边界写清：规划器能够证明几何路径在当前静态模型下无碰撞，但不能自动证明真实车辆一定能执行这条路径。