企业官网建设流程全解析

RRT-Connect：两棵树同时生长，为什么能明显加快找到路径？

关键词｜双向 RRT｜Connect｜快速可行解

前言

普通 RRT 只有一棵树：

start → 不断向外生长

如果终点很远，中间障碍复杂，树可能需要很久才碰到 goal 附近。RRT-Connect 的改法非常直接：

start 长一棵树 goal 也长一棵树 让它们在中间会合

但真正让 RRT-Connect 快的，不只是双向，而是Connect这个动作。

原理讲解

1. 双向探索

维护两棵树：

初始：

Ta 以 start 为根 Tb 以 goal 为根

每一轮先让一棵树向随机点扩展。

2. Extend：只走一步

和普通 RRT 一样：

随机点 → 最近节点 → 向前一步

得到新节点x_new。

3. Connect：另一棵树连续追

另一棵树不会只走一步。它会不断朝x_new生长：

走一步 没撞 继续走 没撞 继续走 ...

直到：

• 接到x_new

• 或被障碍挡住

这就是 RRT-Connect 的提速关键。

4. 为什么双向更容易成功

如果 start 到 goal 距离很长，单树需要独立完成整段探索。双树相当于：

两边一起缩短距离

尤其在开阔空间中，成功连接通常非常快。

5. 为什么两棵树还要交换

很多实现会让较小的树优先扩展。原因很朴素：

不希望一边已经长成“巨树”，另一边还只有几个节点。

交换可以让两侧生长更均衡。

6. 经典 Extend 可以分成三个状态

RRT-Connect 论文里常把一次 Extend 描述成：

• Trapped：被障碍挡住，一步也走不了；

• Advanced：成功前进，但还没到目标；

• Reached：已经到达目标节点。

Connect 的逻辑就是：

只要还是 Advanced，就继续 Extend

直到变成Trapped或Reached。用这三个状态理解代码，比只看while true更容易把握算法结构。

7. 为什么 RRT-Connect 常用在“先找到一条路”

RRT-Connect 的强项是快速建立连通性。它不会像 RRT* 那样花大量时间优化树的局部父子关系，因此在机械臂、狭窄构型空间等场景中，经常用来快速得到初始解。如果后续还需要更平滑、更短，可以再进行：

• shortcut；

• spline 平滑；

• trajectory optimization。

这也是工程中很常见的“先可行，再优化”路线。

代码详解

1. 两棵树初始化

sample_list_f = [start, 0, start]; sample_list_b = [goal, 0, goal];

f可以理解为 forward，b是 backward。

2. forward 树先扩一步

[node_new, success] = get_nearest(...);

成功后加入树。

3. backward 树开始 Connect

关键通常是：

while true

在循环中不断向node_new方向走。这和普通 RRT 每次扩一步就重新随机采样完全不同。

4. 两树相接

一旦连接点重合，就分别回溯两棵树：

start → connection connection → goal

最后拼接。

5. 浮点相等要留心

如果代码直接写：

x1 == x2

用于判断两个浮点节点是否重合，工程上不够稳。更建议：

norm(p1-p2) < 1e-9

8. 最直观的观察方式

如果可视化两棵树，通常能看到它们从 start、goal 两侧向中间快速靠拢。在比较开阔的地图里，RRT-Connect 经常只需要较少采样就能连接；复杂窄通道地图中，优势会有所减弱，但双向探索仍然很有价值。

完整 MATLAB 实现：rrt_connect.m

function [path, flag, cost, expand] = rrt_connect(map, start, goal) % 单次扩展最大距离 param.max_dist = 0.5; % 最大随机采样次数 param.sample_num = 10000; % 目标采样概率 param.goal_sample_rate = 0.05; % 地图尺寸 [param.x_range, param.y_range] = size(map); % 连线碰撞检测分辨率 param.resolution = 0.1; % 分别以 start 和 goal 为根初始化两棵树 sample_list_f = [start, 0, start]; sample_list_b = [goal, 0, goal]; path = []; flag = false; cost = 0; expand = []; for i=1:param.sample_num % 先为当前前向树生成一个目标样本 node_rand = generate_node(goal, param); % 前向树朝随机点扩展一步 [node_new, success] = get_nearest(sample_list_f, node_rand, map, param); if success sample_list_f = [node_new; sample_list_f]; % 另一棵树先朝前向新节点扩展一步 [node_new_b, success_b] = get_nearest(sample_list_b, node_new(1:2), map, param); if success_b sample_list_b = [node_new_b; sample_list_b]; % Connect 阶段：若没有碰撞，就持续朝 node_new 贪心推进 while true distance = min(param.max_dist, dist(node_new(1:2), node_new_b(1:2)')); theta = angle(node_new_b, node_new); node_new_b2 = [node_new_b(1) + distance * cos(theta), ... node_new_b(2) + distance * sin(theta), ... node_new_b(3) + distance, ... node_new_b(1:2)]; if ~is_collision(node_new_b2(1:2), node_new_b(1:2), map, param) sample_list_b = [node_new_b2; sample_list_b]; node_new_b = node_new_b2; else break end % 两棵树到达完全相同的连接点时，直接拼接路径 if node_new_b(1) == node_new(1) && node_new_b(2) == node_new(2) flag = true; cost = sample_list_f(1, 3) + sample_list_b(1, 3); path = extract_path(sample_list_f, sample_list_b, start, goal); expand = [sample_list_f; sample_list_b]; return end end end end % 让节点数量较少的一棵树优先成为下一轮的前向树 [len_f, ~] = size(sample_list_f); [len_b, ~] = size(sample_list_b); if len_b < len_f temp = sample_list_f; sample_list_f = sample_list_b; sample_list_b = temp; end end end %% function index = loc_list(node, list, range) % 查找节点列表中的指定字段 num = size(list); index = 0; if ~num(1) return else for i=1:num(1) if isequal(node(range), list(i, range)) index = i; return; end end end end function node = generate_node(goal, param) % 按设定概率生成全局随机点或直接采样 goal if rand() > param.goal_sample_rate x = 0.5 + (param.x_range - 1) * rand(); y = 0.5 + (param.y_range - 1) * rand(); node = [x, y]; return end node = goal; return end function [new_node, flag] = get_nearest(node_list, node, map, param) % 找到最近树节点，再沿目标方向生成受 max_dist 限制的新节点 flag = false; dist_vector = dist(node_list(:, 1:2), node'); [~, index] = min(dist_vector); node_near = node_list(index, :); distance = min(dist(node_near(1:2), node'), param.max_dist); theta = angle(node_near, node); new_node = [node_near(1) + distance * cos(theta), ... node_near(2) + distance * sin(theta), ... node_near(3) + distance, ... node_near(1:2)]; if is_collision(new_node(1:2), node_near(1:2), map, param) return end flag = true; end function flag = is_collision(node1, node2, map, param) % 对候选树枝做长度限制和栅格碰撞检查 flag = true; theta = angle(node1, node2); distance = dist(node1, node2'); if (distance > param.max_dist) return end n_step = round(distance / param.resolution); for i=1:n_step x = node1(1) + i * param.resolution * cos(theta); y = node1(2) + i * param.resolution * sin(theta); if map(round(x), round(y)) == 2 return end end flag = false; end function path = extract_path(node_list_f, node_list_b, start, goal) % 分别回溯两棵树，并在连接点处拼接 start-goal 路径 if isequal(node_list_b(end, 1:2), start) temp = node_list_f; node_list_f = node_list_b; node_list_b = temp; end path = []; % 回溯 start 一侧树 [len_f, ~] = size(node_list_f); index = 1; while 1 path = [node_list_f(index, 1:2); path]; if isequal(node_list_f(index, 1:2), start) break; end for i=1:len_f if isequal(node_list_f(i, 1:2), node_list_f(index, 4:5)) index = i; break; end end end % 回溯 goal 一侧树 [len_b, ~] = size(node_list_b); index = 1; while 1 path = [path; node_list_b(index, 1:2)]; if isequal(node_list_b(index, 1:2), goal) break; end for i=1:len_b if isequal(node_list_b(i, 1:2), node_list_b(index, 4:5)) index = i; break; end end end end function theta = angle(node1, node2) % 平面两点之间的方向角 theta = atan2(node2(2) - node1(2), node2(1) - node1(1)); end

总结与思考

RRT-Connect 是一个很典型的“非常工程化”的改进。它不追求复杂理论，而是观察到：

单树从头走到尾太慢，那就两边一起长；另一棵树既然已经看到目标节点，就别每次只走一步，干脆一直追。

这种简单改动在实际效果上往往很明显。但它依旧主要追求的是：

尽快找到第一条可行路径

如果你开始关心“找到以后还能不能继续变短”，就进入 RRT* 的世界了。RRT-Connect 很适合体现工程算法的一种朴素美感：没有复杂的新目标函数，只是把“单向慢慢长”改成“双向主动连接”，效果就可能明显改善。有时候真正有效的改进，并不一定来自更复杂的数学。