RRT：一棵搜索树怎样一步步找到路径

无人机要从起点 S 到达目标 G，中间有一堵墙。规划器从 S 出发，逐步向周围添加可通行的连接，让候选路线像树枝一样向外生长；某条分支接到 G 时，就得到了一条完整路径。

RRT（Rapidly-exploring Random Tree，快速探索随机树） 用随机采样决定探索方向，从已有节点延伸出新的连接，逐渐构建一棵搜索树。本文固定一张二维地图，逐轮计算这棵树怎样长出来。

一、地图与生长规则

1. 地图

考虑固定高度下的平面运动，用 $(x,y)$ 表示无人机中心的位置，单位为米。地图已经计入机体尺寸和预留间距，所以只需检查中心点及其连接是否进入禁行区域。

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

接触墙也计为碰撞，地图范围之外不可通行。墙顶在 $y=3$，上方留有通道。

2. 一轮扩展做什么

开始时树上只有 S。每一轮执行：

本例的规则：

• 最大延伸步长为 2 米：样本距离超过 2 米时朝它前进 2 米，否则直接延伸到样本；
• 每个新节点记录自己是从哪个已有节点接过来的，这个节点称为它的父节点，沿父节点逐级回溯就能回到起点；
• 新节点距离 G 不超过 2 米时，尝试直线连接 G，整条连接通过碰撞检查就结束搜索。

这些“延伸”都发生在计算机里的地图上。树上的边是候选路线，最终选出的路径再交给后续的轨迹规划。

二、六轮采样

为了便于复算，下面使用一组指定的样本；实际运行时，样本由采样程序随机产生。六轮结束后的搜索树如下图，下面逐轮说明它是怎样长出来的：

第一轮：朝目标方向长出第一条边

样本 $Q_1=(8,2)$。树上只有 S，S 与样本相距 $8-2=6$ 米，超过步长，所以向右前进 2 米，得到 $A=(4,2)$。S → A 整段在墙的左侧，通过检查，加入 A，父节点为 S。

采样点给出探索方向，步长决定这一轮实际生成的位置：样本在 $(8,2)$，加入树的是 $(4,2)$。

第二轮：两个端点都空闲，连接却穿过了墙

样本仍为 $Q_2=(8,2)$。树上有 S 和 A：

已有节点	到 $Q_2$ 的距离
S = (2,2)	6 米
A = (4,2)	4 米

选择较近的 A，向右前进 2 米，得到待检查的 $X=(6,2)$。A 在墙左侧，X 在墙右侧，连接它们的线段位于 $y=2$，横坐标从 4 到 6，途中穿过了墙所在的 $4.5\le x\le5.5$。

这次扩展被拒绝，X 和 A → X 都不加入树。碰撞检查的对象是整条新连接：两个端点都可用，并不代表中间经过的区域也可用。

第三轮：换一个方向，向墙顶生长

样本 $Q_3=(4,6)$：

已有节点	距离计算	距离
S = (2,2)	$\sqrt{(4-2)^2+(6-2)^2}$	约 4.47 米
A = (4,2)	$6-2$	4 米

选择 A。它与样本横坐标相同，向上延伸 2 米得到 $B=(4,4)$。A → B 位于 $x=4$，在墙的左侧，通过检查，B 的父节点为 A。

B 的纵坐标为 4，已经高于墙顶，树由此获得了从上方绕过墙的机会。

第四轮：从起点长出另一条分支

样本 $Q_4=(1,2)$。每一轮都要在整棵树中找最近的节点：

已有节点	到 $Q_4$ 的距离
S = (2,2)	1 米
A = (4,2)	3 米
B = (4,4)	$\sqrt{3^2+2^2}\approx3.61$ 米

最近的是 S，距离 1 米小于步长，直接连到样本，得到 $C=(1,2)$，父节点为 S。

上一轮刚加入 B，这一轮样本却让起点 S 获得了扩展机会，树因此长出多条分支。C 这条分支会保留下来，后续样本仍可能引导它继续生长。

第五轮：从 B 越过墙顶

样本 $Q_5=(8,4)$：

已有节点	到 $Q_5$ 的距离
S = (2,2)	$\sqrt{6^2+2^2}\approx6.32$ 米
A = (4,2)	$\sqrt{4^2+2^2}\approx4.47$ 米
B = (4,4)	4 米
C = (1,2)	$\sqrt{7^2+2^2}\approx7.28$ 米

选择 B，向右延伸 2 米得到 $D=(6,4)$。B → D 虽然经过墙的横向范围，但整段位于 $y=4$，高于墙顶，可以通过。D 的父节点为 B。

D 到 G 的距离为 $\sqrt{(8-6)^2+(2-4)^2}=\sqrt{8}\approx2.83$ 米，还没进入 2 米的目标连接范围，继续采样。

第六轮：接近目标，检查最后一段

样本 $Q_6=(8,4)$。离它最近的是 D（2 米），从 D 向右延伸 2 米，正好到达样本，得到 $E=(8,4)$，父节点为 D。

E 到 G 的距离是 $4-2=2$ 米，满足目标连接条件。E → G 位于 $x=8$，在墙的右侧，全程可通行，于是把 G 接到 E 上，搜索结束。

三、沿父节点取出路径

节点	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 往回查：G → E → D → B → A → S，反过来就是最终路径 S → A → B → D → E → G。共五段，每段 2 米，总长 10 米。

C 留在搜索树中，最终路径走的是另一条分支。树保存了探索过程中所有被接受的连接，父节点回溯从中取出一条通往目标的路线。

每个新节点都通过一条检查合格的边接到已有的树上，所以它也有一条从 S 出发的无碰撞路线，这个性质在树生长的每一轮都成立。

四、朝样本前进一步的计算

上面的样本都让树水平或竖直延伸。一般情况下样本在斜向位置，需要算出延伸后的坐标。

1. 斜向例子

已有节点在 $(2,2)$，样本在 $(5,6)$。从节点指向样本，横向增加 3、纵向增加 4，直线长度 $\sqrt{3^2+4^2}=5$ 米。步长是 2 米，所以沿这条线走全长的 $2/5$：横向 $3\times\frac25=1.2$，纵向 $4\times\frac25=1.6$，新位置为：

$$q_{\mathrm{new}}=(2+1.2,\ 2+1.6)=(3.2,3.6).$$

验算：$\sqrt{1.2^2+1.6^2}=2$ 米。沿样本方向走指定距离，就是把各坐标方向的变化量按同一比例缩放。

2. 一般形式

把样本记为 $q_{\mathrm{rand}}$，树上离它最近的节点记为 $q_{\mathrm{near}}$，最大步长记为 $\eta$。两点距离 $L=\|q_{\mathrm{rand}}-q_{\mathrm{near}}\|$（$\|\cdot\|$ 表示向量长度），当 $L>0$ 时：

$$q_{\mathrm{new}} =q_{\mathrm{near}} +\frac{\min(\eta,L)}{L} \left(q_{\mathrm{rand}}-q_{\mathrm{near}}\right).$$

样本较远时前进 $\eta$，较近时前进 $L$，正好到达样本；$L=0$ 时样本与已有节点重合，本轮跳过。这个操作通常称为 Steer，算出位置后，还要用碰撞检测决定是否接受整条连接。

五、随机采样为什么能推动树向外探索

1. 一片区域给最近的节点带来扩展机会

样本落在哪里，就由离它最近的节点尝试延伸。看一条从 0 到 10 的数轴，已有节点在 2 和 3，中点是 2.5：

样本所在范围	最近的节点	均匀采样时的概率
0 到 2.5	位于 2 的节点	$2.5/10=25\%$
2.5 到 10	位于 3 的节点	$7.5/10=75\%$

位于 3 的节点“负责”更大的一片范围，更容易被选中，从而向右侧还没覆盖到的区域延伸。

二维空间也一样：把每个位置归给离它最近的树节点，得到各节点的 Voronoi 区域。均匀采样时，区域面积越大，节点被选来扩展的机会越多。树边缘的节点通常对应较大的区域，所以树天然有向外探索的倾向；障碍物则通过碰撞检查决定这些尝试能否真正增加新节点。

2. 目标偏置

上面的例子里，前两轮把 G 作为样本，树先朝右生长；第三轮上方的样本让树获得绕行的机会。

实际实现中常把两种采样混合：以一定概率直接选择目标，其余时候在地图范围内随机采样，称为目标偏置。例如 10% 的概率选 G、90% 的概率随机采样，这个比例是可调参数。

目标采样增加朝终点延伸的机会，空间采样提供侧向和绕行的机会。在这张地图中如果一直选 G，树会反复尝试第二轮那条穿墙的连接；正是上方的样本帮它越过了墙顶。

六、整理成算法

下面的伪代码与手算规则一致：每轮最多加入一条边，碰撞时放弃本轮，接近目标后再验证末段。

输入：地图、起点 S、目标 G、最大步长 η、最大迭代次数 N

检查 S 和 G 是否位于可用区域
如果无效，返回输入无效
如果 S 与 G 重合，返回只含 S 的路径

树 T 中加入 S
parent[S] = 空

重复最多 N 轮：
    在地图范围内产生采样点 q_rand
    在 T 的全部节点中，找到离 q_rand 最近的 q_near

    L = q_near 到 q_rand 的直线距离
    如果 L = 0：
        继续下一轮

    朝 q_rand 延伸 min(η, L)，得到 q_new

    如果 q_near → q_new 的整条连接不可通行：
        继续下一轮

    将 q_new 加入 T
    parent[q_new] = q_near

    如果 q_new 就是 G：
        沿 parent 回溯并反转，返回路径

    如果 q_new 到 G 的距离不超过 η：
        如果 q_new → G 的整条连接可通行：
            将 G 加入 T
            parent[G] = q_new
            沿 parent 回溯并反转，返回路径

返回：本次搜索在迭代上限内未找到路径

“整条连接可通行”包括地图边界、禁行区域和整段线段的检查：占据栅格地图可以遍历线段经过的格子，几何障碍可以计算线段与障碍是否相交。最近节点最简单的实现是遍历全部已有节点，逐一计算距离取最小值。

七、理解运行结果

1. 各项设置的影响

设置	直接影响
最大延伸步长	每条候选连接的最大长度
样本序列与目标偏置	扩展方向，以及由哪些节点延伸
迭代次数上限	本次搜索允许尝试多少轮
碰撞检查方法与精度	候选连接怎样被验证

较大的步长用较少节点就能跨过开阔区域；较小的步长能尝试更细的转向，但走完相同距离需要更多次扩展。步长和碰撞检查精度是两回事：一条 2 米长的候选边，仍然要检查整段。

不同的样本序列会长出不同的树，也可能得到不同的路径。固定随机种子，可以在相同程序和配置下重现一次搜索，便于逐步检查。

2. 迭代用完意味着什么

有限次数的搜索可能把大量尝试花在受阻的方向上。即使地图中存在绕行通道，也可能在本次预算内还没长出通向它的分支。所以达到迭代上限时，结论只是“本次搜索尚未找到路径”，而不是“不存在路径”。

在存在具有一定障碍间距的可行路径、采样持续覆盖相关空间的条件下，RRT 找到解的概率会随迭代次数增加趋近于 1，这称为概率完备性。

3. 10 米的路径还能改进

本例返回的 S → A → B → D → E → G 每条边都通过了检查，是一条几何可行路径，它的形状由采样顺序、最近节点选择和步长共同决定。

对结果做一次后处理，可以尝试直接连接 B 与 G。B 在 $(4,4)$，G 在 $(8,2)$，这条斜线经过墙的横向范围时，纵坐标从 3.75 降到 3.25，始终高于墙顶 3，可以通过。用 B → G 替换 B → D → E → G 后：

$$2+2+\sqrt{(8-4)^2+(2-4)^2} =4+\sqrt{20} \approx8.47\text{ 米}.$$

基本 RRT 只按“谁离样本最近”选择连接起点，不比较路线长短。要在树的生长过程中持续改善从 S 到各节点的累计代价，就需要选择更好的父节点、并重连附近的节点，这正是 RRT* 的做法。

小结

RRT 的一次扩展包含五个动作：采样一个位置，找到最近节点，朝样本延伸，检查整段连接，记录新节点的父节点。这些动作反复执行，树就逐渐伸向不同区域；某条分支与目标连接成功后，沿父节点回溯即可取出路径。