1. 从RRT到最优路径这个课题到底在解决什么问题先聊一个我经常被问到的场景一个移动机器人要从房间角落走到另一头中间隔着茶几、椅子、花盆地面还有高高低低的落差。你用传统的栅格法做路径规划地图一精细搜索空间就爆炸用普通RRT快速随机树规划倒是够快但出来的路径歪歪扭扭像喝醉了一样机器人走起来一顿一顿的电机和底盘都在抗议。这个标题里我最想强调的其实是最优两个字。单纯的RRT是概率完备的搜索算法它保证只要时间和采样点数足够总能找到一条可行路径但完全不保证路径质量。实践中你经常会看到RRT跑出来的路径绕了远路或者出现大量锯齿状的折线。放在仿真里无所谓的放在真实机器人上一跑路径长度、转向次数、平滑度全是问题。所以这个项目标题组合的思路就非常经典用双向RRT快速找到一条可行的初始路径再用Dijkstra算法在某种图上做最短路径搜索把可行升级为较优。这个组合不是随便拼的我在后面的章节里会拆开解释每一步的动机和坑。这个课题适用于做移动机器人导航、自动驾驶局部规划、机械臂避障路径规划的同学也适合刚入门但是想认真搞懂路径规划原理的初学者。先说结论这个方案非常接地气既有学术上的算法对比价值又有工程上的可落地性而且完全可以在Matlab里一步步手写实现不依赖任何商业化工具箱。跟着这篇文章的思路走你不仅能跑通代码还能把里面每一行逻辑吃透。2. 双向RRT做搜索Dijkstra做优化为什么偏偏是这两兄弟2.1 单向RRT和双向RRT的本质差异RRT全称是Rapidly-exploring Random Tree快速随机探索树。它的核心思想是在自由空间里随机撒点每次从已有的树上找一个最近的节点然后向随机点方向生长一段固定步长。如果生长出来的新节点没有碰到障碍物就把它加入树中并连接父节点。这样反复迭代不断扩展树的分支直到这棵树到达目标区域。单向RRT的局限性在复杂环境里很明显。想象一棵树从起点一路往外长如果目标在起点对面、中间隔着一条又长又窄的通道树要花很长时间才能摸到目标区域。很多采样点都浪费在了死胡同或者远离目标的区域里。双向RRT的思路就聪明在同时从起点和目标点各自生长一棵树交替或者并行地扩展两棵树每次扩展后检查两棵树是否能握手——也就是某棵树的新节点能不能找到距离足够近的另一棵树的节点。一旦两地树连通路径就找到了。这种双向机制最大的优势是探索效率大幅提升。用一个生活化的类比单向RRT像一个人蒙着眼在黑暗房间里找另一个人只能靠墙上摸双向RRT则是两个人都开着手电相向而行相遇的概率和处理复杂狭窄通道的能力都强得多。2.2 Dijkstra算法的最优从何而来Dijkstra算法是图论里最经典的单源最短路径算法核心逻辑是维护一个节点集合不断从尚未访问的节点中选出距离起点最近的节点对它的邻居做松弛操作更新起点到每个节点的最短距离估计值直到目标节点被访问。这个算法是确定性的、完备的只要图上边的权重非负它一定能找到从起点到目标点的最短路径。但直接用它做机器人路径规划有一个致命问题机器人所在的空间不是一张现成的图。连续空间里节点和边的构造方式需要你自己设计。那标题里为什么要把Dijkstra和RRT组合到一起本质上是因为RRT或者双向RRT在搜索过程中已经隐式地构建了一棵树——树上的节点就是采样点和树的分支树上的边就是节点之间的连线段。当双向RRT找到一条初始路径之后我们可以把这棵树或者两棵树合并后的结构当成一张图然后在这张图上跑Dijkstra找一条在已有采样点之间最优的路径。注意这里的最优是在当前采样点集合约束下的最优不代表全局最优——真正的最优是连续空间里可微路径意义上的那是另一套优化算法解决的事。但即使只是采样点约束下的最优效果也已经非常明显了。因为双向RRT生长的时候完全是随机探索路径往往不是最优的甚至包含明显多余的绕路。在图结构上跑Dijkstra就是把这些冗余的枝枝叶叶剪掉保留一条最短的连接顺序。这是整个项目最核心的思路。2.3 为什么不用A*为什么不用PRM很多人在规划路径时第一反应是用A*。A在有栅格地图的离散场景下确实效果好有启发式函数引导搜索效率高。但如果你手头的环境是连续坐标空间障碍物是任意多边形或者不规则形状用A就要先把空间离散成栅格分辨率低怕漏障碍分辨率高内存又爆炸。而且栅格化之后的路径天然存在锯齿还需要额外的后处理。PRM概率路线图也可以做它在自由空间里撒大量采样点构建一张路线图再在这张路上搜路径。PRM擅长的是多查询场景也就是同一张地图要反复规划多次路径时效率高。但如果只规划一次PRM要预先撒非常多点构建图开销反而大。双向RRT的优势是单次查询场景下目标明确、扩展方向可控搜索速度和初始解质量平衡得不错加上Dijkstra二次优化确实是比较干的组合。3. 环境建模与核心数据结构先把这些基础打好3.1 地图怎么建从简单矩形障碍到复杂多边形做路径规划第一步就是把真实环境转成算法能读的数据格式。Matlab里最常用的方式是用二维坐标空间定义地图边界再定义障碍物的位置和形状。最简单的是矩形障碍物用[x, y, width, height]表示比如[2, 2, 1.5, 1]表示障碍物左下角坐标在(2,2)宽1.5米高1米。更复杂的场景可以用多边形障碍物每行存储一个多边形的顶点坐标序列。比如一个不规则石头可以用[x1,y1; x2,y2; x3,y3; x4,y4]表示四个顶点。我建议你从矩形障碍物开始调试因为碰撞检测逻辑简单路径规划算法的核心逻辑不受影响等算法跑通了再换成多边形或者圆形障碍物一步步加难度。3.2 节点和边的数据结构怎么设计在Matlab里做这个项目最典型的数据结构是结构体数组或者类。我习惯用结构体因为写起来直观、调试方便% 双向RRT树节点的设计示例 node struct(x, 0, y, 0, parent, [], cost, 0);x和y是节点坐标parent存父节点的索引cost是从根节点到当前节点的累计路径长度。双向RRT需要维护两棵树一棵从起点出发一棵从目标点出发。两棵树各有一个节点列表每次扩展成功后把新节点追加到对应列表里。两棵树合并之后我们就需要构造一张无向图来跑Dijkstra。最简单的做法是双向RRT构建的树结构本身可以当成图——把每个采样点当成图节点每个父-子连接当成图的一条边。由于一棵树是从起点开始的另一棵从目标点开始的当两棵树一连接整个结构就是一张包含起点和目标点的连通无向图了。不过更稳健的做法是把你所有采样到的点包括两棵树上的节点全部拿出来基于邻接关系重新构建带权邻接矩阵或边列表。3.3 碰撞检测路径规划里最容易被低估的环节碰撞检测的准确性直接决定规划出来的路径能不能在实际环境里走。最常见的做法是检查一条线段是否与障碍物相交。在Matlab里如果障碍物是矩形可以将线段与矩形的四条边分别做线段相交判断。% 线段相交判断示例二叉分割法简化版 function flag isIntersect(p1, p2, obs) % p1, p2为线段的端点obs为矩形的顶点坐标矩阵4x2 flag false; for i 1:size(obs, 1) j mod(i, size(obs, 1)) 1; if segmentsIntersect(p1, p2, obs(i,:), obs(j,:)) flag true; return; end end end这里的segmentsIntersect可以用向量叉积判断两条线段是否相交。很多初学者在这块偷懒只用线段中点是否在障碍物内或者新节点是否在障碍物内来判断结果就是路径像穿墙术一样穿过障碍物的边界因为线段中点不在障碍物内但线段本身斜穿过了障碍物的角。这一步是必须严格做线段级别的碰撞检测的。注意真实机器人不是质点有自身尺寸和形状。简单做法是把机器人半径加到障碍物上也就是做膨胀处理。虽然项目标题没提但你在仿真里可以先不考虑等做实际部署前一定要补上膨胀层。否则仿真里一切正常真机一跑就撞墙。4. 流程图式的分解从初始化到路径输出到底有几步4.1 整体执行框架整个项目的算法框架可以拆成以下几个阶段地图参数、起点、目标点、步长、最大迭代次数的初始化。初始化两棵RRT树分别把起点和目标点作为根节点。双向扩展循环一棵树向随机目标点方向扩展一个步长。检查新节点是否碰撞不碰撞则加入树中。检查新节点是否能与另一棵树上某个节点直接连接线段不碰撞。若可以连接两树连通退出循环否则交换两棵树的身份继续下一轮。如果退出循环时两树连通提取从起点到目标点的原始路径节点序列。构建图模型节点集 边权重运行Dijkstra算法得到最短路径序列。输出可视化结果和前后的路径长度对比。4.2 双向扩展的详细过程双向RRT具体怎么扩以起点树为例随机在自由空间里采样一个点rand_point在起点树里找离rand_point最近的节点nearest_node然后从nearest_node沿nearest_node到rand_point方向生长固定步长step_size得到新节点new_node。如果new_node与nearest_node之间的线段不碰撞则将new_node加入起点树。接着判断new_node是否可以直接连接到目标树上的某个节点如果可以路径就找到了。找到路径之后我需要提取原始路径。具体做法是从连接点分别向起点和终点回溯父节点链然后拼接成一条完整的节点序列。4.3 从树到图的转换这一步很多人会犯错。直接用树的父-子关系跑Dijkstra其实效果已经很好了因为树本身就是无向图的一个子集树上的节点和边天然构成一个连通图。但更彻底的优化是把所有两两距离小于某个阈值的节点之间都尝试建立边。这个做法的好处是让Dijkstra有更多可选边从而可能跳过某些树上绕路的中间节点直接连接两个距离较近但不在同一父-子链上的节点。不过这种做法要控制阈值设得太大会导致边数量爆炸Dijkstra会变慢设得太小等于没加。我的经验是基于步长来设比如阈值取2 * step_size可以让Dijkstra在搜索效率和优化空间之间找到一个平衡点。5. 手把手Matlab实现双向RRT Dijkstra代码拆解5.1 主函数结构%% 双向RRT结合Dijkstra算法的主函数 clear; clc; close all; %% 地图参数设置 map_width 10; % 地图宽度 map_height 10; % 地图高度 obs_list [ 2, 2, 2, 1.5; % 第一个矩形障碍物 x, y, w, h 5, 5, 1.8, 2; % 第二个 7, 1, 2.2, 1.2; % 第三个 ]; % 起点和终点 start [0.5, 0.5]; goal [9.0, 9.0]; %% 算法参数 step_size 0.5; % 生长步长 max_iter 5000; % 最大迭代次数 neighbor_radius 1.0; % 两树连接判定半径 %% 初始化两棵树 tree_start.x start(1); tree_start.y start(2); tree_start.parent 0; tree_goal.x goal(1); tree_goal.y goal(2); tree_goal.parent 0; tree1 tree_start; tree2 tree_goal; path_found false;主函数最开始是参数设置和树初始化。这里obs_list用行向量表示矩形障碍物。tree_start和tree_goal分别存储两棵树的根节点parent0表示根节点没有父节点。5.2 双向扩展主循环%% 双向RRT主循环 for iter 1:max_iter % 随机采样 rand_point [rand * map_width, rand * map_height]; % 在树1中找最近节点并扩展 [nearest_node1, idx1] findNearestNode(tree1, rand_point); new_node1 steer(nearest_node1, rand_point, step_size); % 碰撞检测 if ~checkCollision(new_node1, nearest_node1, obs_list) new_node1.parent idx1; tree1 [tree1; new_node1]; % 尝试连接树2 [nearest_node2, idx2] findNearestNode(tree2, new_node1); if distance(new_node1, nearest_node2) neighbor_radius if ~checkCollision(nearest_node2, new_node1, obs_list) % 两树连通 idx_conn1 length(tree1); idx_conn2 idx2; path_found true; break; end end end % 交换两棵树实现双向交替扩展 temp tree1; tree1 tree2; tree2 temp; end注意这里有个非常关键的swap操作。为什么两棵树要交换因为如果一直只扩展起点树目标树永远不生长那仍然是单向RRT。通过每轮迭代交换树的角色两棵树得到了公平的扩展机会搜索效率才能真正提升。findNearestNode是最近邻搜索函数最简单的方式是遍历所有节点算欧氏距离。这样做在节点数量少几千个以内时完全没问题Matlab跑起来也快。如果节点到了上万个可以考虑KD树或者其他加速结构但那就牵扯到更复杂的工程优化了这个项目不需要。steer函数就是从最近节点向随机点方向走步长function new_node steer(nearest, rand_point, step_size) dx rand_point(1) - nearest.x; dy rand_point(2) - nearest.y; dist sqrt(dx^2 dy^2); if dist step_size new_node.x rand_point(1); new_node.y rand_point(2); else new_node.x nearest.x dx / dist * step_size; new_node.y nearest.y dy / dist * step_size; end new_node.parent []; new_node.cost 0; end5.3 路径提取与转换两棵树连通后要从树结构提取节点序列。比如我用idx_conn1作为树1上的连接点索引idx_conn2作为树2上的连接点索引。从idx_conn1沿着parent一直回溯到根节点得到从起点到连接点的路径从idx_conn2回溯到树2的根节点目标点再把这条链反过来得到从连接点到目标点的路径。两段拼接就是初始路径。%% 提取初始路径 path1 []; idx idx_conn1; while idx ~ 0 path1 [path1; tree1(idx).x, tree1(idx).y]; idx tree1(idx).parent; end % path1此时是从起点到连接点需要reverse才符合方向 path1 flipud(path1); path2 []; idx idx_conn2; while idx ~ 0 path2 [path2; tree2(idx).x, tree2(idx).y]; idx tree2(idx).parent; end % path2此时是从目标点回溯到连接点需要reverse path2 flipud(path2); % 拼接时避免重复连接点 initial_path [path1; path2(2:end, :)];拼接完成后initial_path就是双向RRT的原始路径。这时候画出来你会看到一条有点绕、有些折角的路径。接下来才是Dijkstra登场的时候。5.4 Dijkstra优化实现把初始路径上的节点作为图节点加上两棵树采样过程中产生的一些可行节点可选构建邻接矩阵。最直接的邻接矩阵定义方式%% 构建图模型 nodes [initial_path; extra_nodes]; % extra_nodes可以是从两树中收集的可行节点 n size(nodes, 1); adj inf(n, n); for i 1:n for j i1:n % 检查节点i与节点j之间连线是否碰撞 if ~checkCollisionLine(nodes(i,:), nodes(j,:), obs_list) dist norm(nodes(i,:) - nodes(j,:)); adj(i, j) dist; adj(j, i) dist; end end end %% Dijkstra算法 start_idx 1; goal_idx 2; % 初始路径的起点和终点对应的索引 dist inf(n, 1); prev zeros(n, 1); visited false(n, 1); dist(start_idx) 0; while ~visited(goal_idx) % 选择当前未访问节点中dist最小的节点 min_dist inf; u -1; for i 1:n if ~visited(i) dist(i) min_dist min_dist dist(i); u i; end end if u -1 break; end visited(u) true; for v 1:n if ~visited(v) adj(u, v) inf new_dist dist(u) adj(u, v); if new_dist dist(v) dist(v) new_dist; prev(v) u; end end end end %% 回溯最优路径 opt_path []; idx goal_idx; while idx ~ 0 opt_path [opt_path; nodes(idx, :)]; idx prev(idx); end opt_path flipud(opt_path);这段代码里的核心逻辑是经典的Dijkstra。你要注意如果图构建时只用了初始路径上的节点那Dijkstra优化的空间就局限在初始路径的绕行优化上——但是路径节点是顺序连通的每条边如果都合法Dijkstra基本上找不出更短的替代因为图只有一条链。这也是我在5.3里留了extra_nodes的原因。想要Dijkstra真正发挥作用必须给它额外的可选边。我建议的做法是在两棵树扩展到最终阶段时收集两棵树上的所有节点只要这些节点在初始路径附近一定范围内比如距离初始路径小于1.5倍步长都加入图节点集合。这样Dijkstra就有可能在两个看似相隔较远的路径节点之间找到一条直连的可行边从而剪掉中间那些绕路的锯齿。5.5 可视化与结果比较路径规划最直观的确认方式就是画图。用plot把障碍物画成矩形用line把树画成淡色连线用plot把初始路径画成红色虚线把Dijkstra优化后的路径画成蓝色实线。%% 可视化 figure; hold on; axis equal; grid on; xlim([0 map_width]); ylim([0 map_height]); % 绘制障碍物 for i 1:size(obs_list, 1) rectangle(Position, obs_list(i,:), FaceColor, [0.8 0.8 0.8]); end % 绘制树 for i 2:length(tree1) if tree1(i).parent 0 plot([tree1(tree1(i).parent).x, tree1(i).x], ... [tree1(tree1(i).parent).y, tree1(i).y], Color, [0.5 0.9 0.5]); end end % 绘制初始路径 plot(initial_path(:,1), initial_path(:,2), r--, LineWidth, 1.5); % 绘制优化路径 plot(opt_path(:,1), opt_path(:,2), b-, LineWidth, 2); % 标记起点终点 plot(start(1), start(2), go, MarkerSize, 10, MarkerFaceColor, g); plot(goal(1), goal(2), ro, MarkerSize, 10, MarkerFaceColor, r); legend(障碍物, RRT树, 初始路径, Dijkstra优化路径);跑完这段代码你会非常直观地看到红线和蓝线的区别。6. 参数调优与实验结果分析6.1 步长step_size怎么定步长是双向RRT最敏感的参数。步长太小树扩展得慢需要迭代很多次才能连通路径上的节点非常密集后续Dijkstra图构建和搜索都会变慢。步长太大路径会非常粗糙甚至可能因为迈大步而错过狭窄通道导致路径找不到。我的经验值是这样把地图最大边长除以50到100作为参考步长。比如10米的地图步长取0.1米到0.2米比较保险。如果环境里有狭窄通道步长应该小于通道最窄处的宽度的一半这样才能保证扩展的节点不会跳过通道。你可以通过几次调试快速找到一个适合当前地图的步长。6.2 邻居半径neighbor_radius的影响邻居半径是两树连接判定的阈值。它设得太小两棵树明明已经靠得很近了但因为距离没有达到阈值而错过连接机会白白继续扩展很多节点设得太大可能跨过障碍物直接误判连通——因为两棵树上的节点距离近不代表它们之间的连线是安全的。所以这个值需要两步设置先定一个较小的连接判定半径比如1.5 * step_size再在判定时检查线段碰撞。后者比前者重要每次连接都要做完整碰撞检测。6.3 迭代上限与随机种子max_iter设得不够大可能最后没有找到路径程序报错或者循环结束后path_foundfalse。你可以加一个对path_found的判断如果没找到就增加迭代上限或者适当增大步长。另外由于RRT是随机算法同一次运行的结果每次都不一样是正常的。为了做对比实验和调试设置固定随机种子是个好习惯rng(42); % 固定随机数种子保证可重复性6.4 路径长度对比怎么量化优化效果优化效果不能光靠眼睛看。我建议你在主函数里计算两条路径的总长度initial_length sum(sqrt(sum(diff(initial_path).^2, 2))); opt_length sum(sqrt(sum(diff(opt_path).^2, 2))); fprintf(初始路径长度: %.4f\n, initial_length); fprintf(优化后路径长度: %.4f\n, opt_length); fprintf(优化比例: %.2f%%\n, (initial_length - opt_length) / initial_length * 100);实测下来在复杂障碍物环境下双向RRT的初始路径往往比理论最短路径长20%到50%经过Dijkstra优化后至少能缩短10%到30%。这个数字会随地图复杂度浮动但趋势很稳定。你还可以做一组对比实验分别跑单纯的双向RRT不接Dijkstra和双RRTDijkstra各跑20次取平均路径长度你会发现后者不仅平均长度更短方差也更小说明优化不仅提升了质量还提升了稳定性。7. 实际踩坑记录与排查技巧7.1 路径穿墙多半是碰撞检测偷懒了我最常遇到的bug就是路径直接穿过障碍物。排查顺序很简单先在可视化代码里把路径节点画成空心圆点再看是哪个线段穿墙。如果节点本身都不在障碍物里但线段穿过了障碍物边界那一定是碰撞检测没有做线段相交检测只做了点检测。还有一个情况是碰撞检测函数没有处理线段恰好经过障碍物顶点这种临界情况只靠叉积符号判断就有边界误判风险。建议碰撞检测写成容差形式当叉积的绝对值非常小比如小于1e-9时也认为相交。7.2 Dijkstra结果反而更差图构建有问题如果你发现优化后的路径比初始路径还长那基本可以断定问题出在图模型上。可能性有几种一是邻接矩阵只保留了初始路径上相邻节点的连接Dijkstra没有可替代的边可选最终结果和原路径一致甚至因为路径重构引入额外绕行二是额外节点的加入没有做碰撞检测导致Dijkstra穿墙作弊路径长度虽然短了但完全不可行。这两种情况我都遇到过。修复办法保证图构建时所有边都经过严格的碰撞检测并且确实把两棵RRT树上的有效采样点初始路径附近的那些加入节点集合让Dijkstra有真正的选择空间。7.3 双向RRT死循环或者极慢如果程序跑很久都没找到路径先看障碍物是否把起点和目标点隔开了。如果在环境连通性上就没解任何随机算法都找不到路径。其次看步长和邻居半径是否合理特别是在狭窄通道场景里步长太大会让树永远跳不过去。还有一个容易忽略的问题随机采样点如果完全没有限制在自由空间里很多采样点落在障碍物内部白白浪费计算量。建议做采样时先检查采样点是否在障碍物内如果是就重新采样提高有效采样率。7.4 Matlab小技巧用profile找性能瓶颈当节点数量多起来之后比如超过1000个findNearestNode和碰撞检测会成为性能瓶颈。Matlab里直接在命令行输入profile on跑完算法输入profile viewer就能看到每行代码的耗时。优化重点通常是最近邻搜索和碰撞检测函数。最近邻搜索可以在findNearestNode里改用kd tree或者只搜索部分范围内的点碰撞检测则可以把矩形相交判断用向量化方式写避免一轮一轮循环。7.5 路径平滑和后续工程化Dijkstra优化后的路径依然是折线只是折点数变少了。在实际机器人上运行之前建议再加一步平滑处理。最常用的是Bezier曲线插值或者B样条曲线拟合。Matlab里有cscvn或者interp1可以做简单平滑但注意平滑后的路径要再做一次碰撞检测否则可能因为插值点偏离原线段而撞上障碍物。工程上还会做梯形速度规划或者时间最优轨迹规划那是另外一个课题了。8. 最后的经验总结与扩展方向我个人在实际操作中的体会是这个项目最大的价值不是让你跑通一个特定算法实现而是让你真正理解搜索和优化两个概念的配合关系。双向RRT负责在连续空间里探索、生成可行解Dijkstra负责在可行解的图结构里提炼最优解。许多类似的规划问题都可以套用这个快速规划器 优化器的组合思路例如先用RRT生成粗略轨迹再用二次规划做轨迹优化或者用Lattice Planner生成备选轨迹再用代价函数评估选择最优。想通了这个框架以后接触复杂的规划算法会轻松很多。还有一个小技巧实现完这个项目后你可以把它扩展成三维空间的路径规划只要把x, y改成x, y, z碰撞检测改成三维几何体相交检测步长仍然是一段空间线段整个双向RRT和Dijkstra的框架完全不需要动。这是检验你理解程度的很好的练手方式。最后再提醒一句话Matlab跑这个项目很顺手但如果你想部署到真实的嵌入式机器人上C版本的实现思路几乎是一样的只是数据结构、内存管理方式不同。思路一旦打通换语言只是翻译工作。这个标题看起来像是一个课程设计题但把里面的每一环都吃透你的机器人路径规划功底就不只是会用工具箱那么简单了。
