RRT*：一棵搜索树怎样一步步改善路径

一棵搜索树已经绕过墙壁，把起点 S 连到了目标 G，路线长 10 米。继续采样时，新节点可能提供更短的连接，让已有的分支换一条路线接回起点。

RRT* 在树的生长过程中比较累计路径代价：为新节点选择最好的父节点，再用新节点“重连”附近的旧节点，改善它们的路线。本文从一棵已有的可行树出发，用三轮具体计算，看目标路径怎样从 10 米缩短到约 7.76 米。

一、起点：一棵已有的树

1. 地图与当前搜索树

地图与《RRT：一棵搜索树怎样一步步找到路径》相同，坐标单位为米，已经计入机体尺寸和预留间距：

项目	设置
地图范围	$0\le x\le10$ ， $0\le y\le6$
起点	$S=(2,2)$
目标	$G=(8,2)$
墙	$4.5\le x\le5.5$ ， $0\le y\le3$

初始树就是那篇文章六轮扩展后得到的树：S 向左连出 C，向右依次经过 A、B、D、E 到达 G，每段 2 米，当前路线总长 10 米。所有已有连接都通过了碰撞检查。

2. 给每个节点增加一项记录： $g$

用 $g(n)$ 表示沿当前树中的路线，从 S 到节点 $n$ 的累计长度：

节点	S	A	B	C	D	E	G
坐标	(2,2)	(4,2)	(4,4)	(1,2)	(6,4)	(8,4)	(8,2)
父节点	无	S	A	S	B	D	E
$g$	0	2	4	1	6	8	10

例如 $g(B)=4$ 对应路线 S → A → B， $2+2=4$ 米。之后如果找到更短的到达方式，就同时修改父节点和累计长度。

接下来三轮的最大延伸步长和邻域半径都取 2 米。邻域指以新位置为中心、指定半径内的已有节点集合。距离显示到小数点后两位，累计结果按未舍入的数值计算。

二、第一轮：新节点应该接到谁下面

下图左边是第一轮的两步操作，右边是三轮结束后的树，下面逐步计算：

RRT* 的更新过程：左图中新节点 X 在 2 米邻域里选择 S 作为父节点，并让 B 从 A 改接到 X；右图为三轮之后，Y 的两条候选连接穿墙被拒绝，G 改接到新节点 Z，最好路径约 7.76 米

1. 从最近节点生成新位置

样本 $Q_1=(3,3.5)$ 。先按 RRT 的方式，在整棵树中找离样本最近的节点：

已有节点	S	A	B	C	D	E	G
到 $Q_1$ 的距离（米）	1.80	1.80	1.12	2.50	3.04	5.02	5.22

最近的是 B：横向差 1 米、纵向差 0.5 米，距离 $\sqrt{1.25}\approx1.12$ 米，小于步长，直接延伸到样本，得到 $X=(3,3.5)$ 。B → X 位于墙的左侧，连接可行。

到这里已经有一个可行方案：把 X 接在 B 下面。RRT* 还要比较其他接法。

2. 选择父节点

X 的 2 米邻域内有 S、A、B 三个节点，它们都可能成为 X 的父节点。每个方案的总长度等于“从 S 沿当前路线到候选节点”加上“从候选节点直接连到 X”：

候选父节点	当前到达长度	到 X 的连接长度	到达 X 的总长度
B	4	1.12	5.12
A	2	1.80	3.80
S	0	1.80	1.80

三条连接都在墙的左侧，都能通过碰撞检查。S → X 最短，于是 X 的父节点为 S， $g(X)\approx1.80$ 米。

B 离 X 最近，但到达 B 之前已经走了 4 米；S 到 X 的最后一段稍长，整条路线却短得多。最近节点只负责生成新位置，父节点要比较到达新位置的完整路线。

写成公式：设候选父节点为 $p$ ，经过它到达 X 的长度为

$g(p)+\|X-p\|,$

在连接可行的候选中，选这个和最小的作为父节点。这一步称为选择父节点（Choose Parent）。

三、第一轮继续：X 能让哪些旧路线变短

1. 重连 B

X 接到 S 之后， $g(X)\approx1.80$ 米。再检查邻域里的旧节点能否通过 X 获得更短的路线。

到达 B 的旧路线 S → A → B 长 4 米；经过 X 的新路线 S → X → B 长 $\sqrt{3.25}+\sqrt{1.25}\approx2.92$ 米。X → B 在墙的左侧，连接可行，而且更短。于是删除旧连接 A → B，加入新连接 X → B：B 的父节点从 A 改为 X， $g(B)$ 从 4 降到约 2.92。

这种让已有节点改接到新节点下面的操作，称为重连（Rewire）。A 仍然留在树中，只是 B 不再挂在它下面。

2. A 需要重连吗

经过 X 到达 A 的长度为 $g(X)+\|A-X\|=\sqrt{3.25}+\sqrt{3.25}\approx3.61$ 米，比 A 现在的 2 米长，所以 A 保持原来的父节点 S。根节点 S 始终是 $g(S)=0$ 。

3. B 后面的分支一起变短

D、E、G 都挂在 B 后面。到达 B 的路线缩短后，它们的路线也随之缩短，而 B → D、D → E、E → G 各段仍然是 2 米：

节点	原累计长度	更新方式	新累计长度
B	4	S → X → B	2.92
D	6	新的 $g(B)+2$	4.92
E	8	新的 $g(D)+2$	6.92
G	10	新的 $g(E)+2$	8.92

这几个节点都缩短了约 1.08 米，坐标不变。重连改变的是分支接回起点的方式，代价更新沿父节点到子节点的方向往后传播。

注意 X 的邻域只包含 S、A、B，但 D、E、G 的代价也变了：邻域只限定本轮尝试哪些新连接，后代的更新会沿已有分支一直传下去。此时的最好路径是 S → X → B → D → E → G，约 8.92 米。

四、第二轮：碰撞检查参与比较

样本 $Q_2=(5.6,2.8)$ 。最近的已有节点是 $D=(6,4)$ ，相距约 1.26 米，直接延伸得到 $Y=(5.6,2.8)$ 。D → Y 的横坐标始终大于墙的右边界 5.5，连接可行。

Y 的 2 米邻域包含 A、B、D：

候选父节点	当前到达长度	到 Y 的连接长度	候选总长度	连接检查
A	2	1.79	3.79	穿墙
B	2.92	2	4.92	穿墙
D	4.92	1.26	6.19	可通行

A → Y 从 $(4,2)$ 到 $(5.6,2.8)$ ，横向穿过墙体，纵坐标始终低于墙顶 3。B → Y 从 $(4,4)$ 斜向下降到 $(5.6,2.8)$ ，途中经过的 $(5.4,2.95)$ 落在墙内。

所以只能选 D 作为父节点， $g(Y)\approx6.19$ 米。再检查重连：经过 Y 回到 A 或 B 都比它们现在的记录长，保持不变。

这一轮新增了 Y，目标路径仍约 8.92 米。父节点选择和重连都要同时满足两个条件：路线更短，并且新连接可以通行。

五、第三轮：让目标改接到新分支

样本 $Q_3=(6.8,3.4)$ 。它离 D 最近，距离恰好为 $\sqrt{0.8^2+0.6^2}=1$ 米，直接延伸得到 $Z=(6.8,3.4)$ 。Z 的 2 米邻域包含 D、E、G、Y，它们都在墙的右侧，连接都可行。

1. 为 Z 选择父节点

候选父节点	当前到达长度	到 Z 的连接长度	候选总长度
D	4.92	1	5.92
E	6.92	1.34	8.26
G	8.92	1.84	10.76
Y	6.19	1.34	7.53

选择 D， $g(Z)\approx5.92$ 米。

2. 用 Z 重连

已有节点	当前累计长度	经过 Z 的候选长度	本轮结果
E	6.92	7.26	保留原路线
Y	6.19	7.26	保留原路线
G	8.92	7.76	改接到 Z

经过 Z 到达 G 的长度为 $5.92+1.84\approx7.76$ 米，比原来的 8.92 米短，G 的父节点从 E 改为 Z。

从 G 沿父节点回溯：G ← Z ← D ← B ← X ← S，反过来就是当前最好路径 S → X → B → D → Z → G，按实际边长相加：

$\sqrt{3.25}+\sqrt{1.25}+2+1+\sqrt{3.4} \approx7.76\text{ 米}.$

3. 三轮中目标路径的变化

时刻	到达 G 的路线	总长度
初始树	S → A → B → D → E → G	10 米
加入 X 后	S → X → B → D → E → G	约 8.92 米
加入 Y 后	S → X → B → D → E → G	约 8.92 米
加入 Z 后	S → X → B → D → Z → G	约 7.76 米

X 改善了前半段路线，Z 改善了末段；Y 是新的探索分支，没有改变当前的目标路线。在静态地图中，算法保留已有解、只接受更短的可行连接，所以最好路径的代价只会下降或保持不变。

六、整理成算法

1. 一轮 RRT* 的流程

两次比较可以统一写成：

操作	比较的量
选择父节点	对每个候选 $p$ ，比较 $g(p)+\lVert X-p\rVert$
重连旧节点	对每个邻居 $v$ ，比较 $g(X)+\lVert v-X\rVert$ 与原来的 $g(v)$

代价相同时保留已有连接。每次改变父节点，都要删除旧边、加入新边，并同步维护父子关系。

2. 伪代码

下面使用固定目标 G 和直线连接。G 接入树以后，也继续参与后续的代价更新。

检查起点、目标和地图
若 S 与 G 重合，返回只包含 S 的路径

树中加入 S
parent[S] = 空
g[S] = 0

在给定采样次数或时间预算内重复：
    产生样本 q_rand
    在整棵树中找到离样本最近的 q_near
    从 q_near 朝样本延伸，得到 q_new

    若 q_new 与已有节点重合：跳过本轮
    若 q_near → q_new 的连接不可通行：跳过本轮

    按邻域规则，找出 q_new 附近的已有节点集合 N

    # 先保留最近节点提供的可行方案
    best_parent = q_near
    best_cost = g[q_near] + 距离(q_near, q_new)

    # 为新节点选择父节点
    对 N 中的每个候选 p：
        candidate = g[p] + 距离(p, q_new)
        若 candidate 更小，且 p → q_new 可通行：
            best_parent = p
            best_cost = candidate

    将 q_new 接到 best_parent 下
    g[q_new] = best_cost

    # 利用新节点重连附近旧节点
    对 N 中的每个节点 v：
        若 v 是根节点或 q_new 的祖先：跳过
        candidate = g[q_new] + 距离(q_new, v)
        若 candidate < g[v]，且 q_new → v 可通行：
            删除 v 与旧父节点的连接
            将 v 接到 q_new 下
            g[v] = candidate
            从 v 开始，逐层更新全部后代的累计代价

    若 G 尚未加入，且 q_new 位于目标连接范围内：
        若 q_new → G 可通行：
            将 G 接到 q_new 下，并记录它的累计代价

    若树中已有 G：
        沿 parent 回溯，保存当前最好路径及 g[G]

预算结束后：
    若已有目标路径，返回当前最好路径
    否则返回“本次预算内尚未找到路径”

“祖先”指沿父节点回溯时能遇到的节点。重连时跳过祖先，保证每个非根节点只有一个父节点、连接中不出现环，结构始终是一棵树。

实现后代更新时，可以为每个节点记录子节点列表。例如 B 重连后，依次更新 D、E、G，每一步都用 $g(\text{子节点})=g(\text{父节点})+\text{父子连接长度}$ 。

七、邻域、预算与渐近最优性

1. 邻域决定比较哪些连接

第一轮中，X 的 2 米邻域包含 S、A、B，S 才有机会成为父节点。如果保持第一轮开始时的树不变、只把邻域半径改为 1.5 米，X 的邻域就只剩 B，X 只能接在 B 下面，累计长度约 5.12 米，这一轮也就无法借助 S → X 改善 B 后面的分支。

邻域越大，可供比较的替代连接越多，每轮需要检查的候选和连接也越多；邻域越小，每轮处理的范围越局部。最大延伸步长控制新位置怎样产生，邻域规则控制新位置与哪些已有节点比较。完整实现中，常根据节点数量和空间维数调整邻域半径，或取随节点数量增长的一定数量近邻。

2. 找到第一条路径之后

本例从一条 10 米的路径出发，三轮后把最好路径缩短到约 7.76 米。实际搜索也常这样：先保存第一条可行路径，再用剩余时间持续采样和重连。每一轮可能改善目标路线，也可能只增加探索分支。评估时可以同时记录首次找到路径的耗时，以及最好路径长度随时间的变化。

3. 渐近最优性

概率完备性关注能否找到可行解；渐近最优性关注解的代价能否随采样增加趋近最优值。在满足可行性、采样覆盖和邻域连接等条件时，RRT* 具有渐近最优性：采样不断提供新的候选连接，选择父节点和重连持续保留更低代价的路线。

约 7.76 米只是本例此刻保留的最好路线，继续采样仍可能改善。实际运行的停止条件通常是时间预算、迭代预算或设定的路径质量要求。

小结

RRT* 在 RRT 的基础上增加了四个操作：记录累计代价，为新节点选择父节点，重连能够受益的旧节点，更新它们后面的分支。在本例中，X 让 B 改接到更短的路线，D、E、G 随之变短；Y 提供了新的分支；Z 又缩短了目标前的最后一段绕行。

跟踪每次更新时，同时看三样东西：坐标说明连接是否可行，父节点说明路线怎样组成，累计长度说明这次调整改善了多少。