RRT-Connect两棵树同时生长为什么能明显加快找到路径关键词双向 RRTConnect快速可行解前言普通 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* 那样花大量时间优化树的局部父子关系因此在机械臂、狭窄构型空间等场景中经常用来快速得到初始解。如果后续还需要更平滑、更短可以再进行shortcutspline 平滑trajectory optimization。这也是工程中很常见的“先可行再优化”路线。代码详解1. 两棵树初始化sample_list_f [start, 0, start]; sample_list_b [goal, 0, goal];f可以理解为 forwardb是 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-98. 最直观的观察方式如果可视化两棵树通常能看到它们从 start、goal 两侧向中间快速靠拢。在比较开阔的地图里RRT-Connect 经常只需要较少采样就能连接复杂窄通道地图中优势会有所减弱但双向探索仍然很有价值。完整 MATLAB 实现rrt_connect.mfunction [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 i1: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 i1: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 i1: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 i1: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 i1: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 很适合体现工程算法的一种朴素美感没有复杂的新目标函数只是把“单向慢慢长”改成“双向主动连接”效果就可能明显改善。有时候真正有效的改进并不一定来自更复杂的数学。