尧图网站建设 
简介这份MATLAB路径规划资源面向计算机及相关专业学生尤其适合正在做课程设计、期末大作业或希望以实战案例提升编程能力的学习者。内容围绕移动机器人路径规划展开整合了A*搜索、概率路线图PRM与快速探索随机树RRT三类经典算法及其改进版本可用于理解算法原理、掌握改进思路并积累MATLAB工程经验。压缩包共31个文件约6.28MB以20个m脚本为核心实现辅以zbak备份、gif运行演示、zip子包及license、md说明文档目录按A-Star、PRM-Astar、RRT等模块划分结构清晰便于对照学习。已有44人学习下载。代码经测试可读性与可扩展性较好读者能获得完整算法实现、可视化演示与模块化组织方式为课程项目或后续研究提供可靠参考。1. 从一份 MATLAB 路径规划作业说起A*、PRM、RRT 到底怎么选如果你正在做移动机器人方向的课程设计或者被“matlab 期末大作业”卡在仿真出图这一步大概率会遇到同一个问题A*、PRM、RRT 这三个算法名字都听过但真到写代码时不知道该用哪个、参数怎么调、为什么别人的图那么干净而自己的路径像喝多了。这份资源就是围绕这个痛点来的——它用 MATLAB 把改进 A*、PRM、RRT 三套路径规划算法完整实现了一遍包含栅格地图构建、算法主循环、路径平滑和可视化对比。适合两类人一类是刚接触路径规划、需要一份能跑通的参考实现来交作业或入门的新手另一类是已经写过基础版本、想看看改进点在哪、参数边界怎么设的熟手。下面我按“先讲清原理和选型再落到代码和参数最后说坑”的顺序拆一遍。2. 三种算法的 MATLAB 落地从栅格地图到路径输出2.1 为什么路径规划要先统一地图表示不管后面用哪种算法第一步都是把连续环境变成算法能吃的离散结构。移动机器人路径规划里最常见的做法是栅格法把工作空间切成 M×N 的方格每个格子标记为自由或障碍。这份资源里地图用一个二维矩阵表示0 是自由区1 是障碍物起点和终点用坐标索引给出。这样做的直接好处是三种算法可以共用同一张地图切换算法时只换规划函数不用重做环境。常见做法是用binaryOccupancyMap或者直接手写矩阵。手写矩阵更轻适合课程作业用 Robotics Toolbox 的binaryOccupancyMap则能直接对接后续的可视化和碰撞检测函数。这份实现走的是手写矩阵路线依赖少MATLAB 基础版本就能跑。% 构建 50x50 栅格地图1 表示障碍0 表示自由 mapSize 50; map zeros(mapSize, mapSize); % 添加矩形障碍物区域 map(10:20, 15:35) 1; map(30:40, 5:25) 1; map(25:35, 40:48) 1; % 起点和终点坐标行, 列 startPos [2, 2]; goalPos [48, 48]; % 检查起点终点是否落在障碍上 assert(map(startPos(1), startPos(2)) 0, 起点在障碍物内); assert(map(goalPos(1), goalPos(2)) 0, 终点在障碍物内);这段代码的逻辑很直白先开一块全零矩阵再把几块矩形区域置 1 当障碍。startPos和goalPos用的是行列索引不是笛卡尔坐标这一点后面写启发函数时要注意别搞反。assert两行是后悔药——如果起点或终点不小心落在障碍里算法会直接报错而不是给你一条穿墙路径。参数方面mapSize决定分辨率50×50 在课程作业里够用再大可视化会挤。障碍物坐标是硬编码的换成实际场景时可以从图片二值化或激光数据生成。注意 MATLAB 里矩阵索引是 (行, 列)而行对应 y 方向、列对应 x 方向写距离公式时如果混了路径会朝奇怪的方向偏。2.2 改进 A*启发函数和邻域扩展改在哪A* 的核心是f g hg 是起点到当前点的实际代价h 是当前点到终点的估计代价。基础版用 4 邻域或 8 邻域扩展启发函数用曼哈顿或欧氏距离。问题在于 8 邻域下直线和对角线代价没区分路径容易出现不必要的斜切另外障碍物密集时扩展节点多速度掉得厉害。这份实现里的改进点主要有两个一是启发函数加权重二是邻域扩展时对对角移动做障碍检测。加权启发是h w * euclideanDistancew 略大于 1 时搜索更偏向终点扩展节点数下降但 w 太大会丢最优性。对角检测是防止机器人从两个对角障碍之间“穿过去”。function [path, visited] improvedAStar(map, startPos, goalPos, w) [rows, cols] size(map); % 8 邻域方向 dirs [-1 0; 1 0; 0 -1; 0 1; -1 -1; -1 1; 1 -1; 1 1]; costs [1 1 1 1 sqrt(2) sqrt(2) sqrt(2) sqrt(2)]; gScore inf(rows, cols); gScore(startPos(1), startPos(2)) 0; openList [startPos, heuristic(startPos, goalPos, w)]; visited false(rows, cols); parent zeros(rows, cols, 2); while ~isempty(openList) [~, idx] min(openList(:, 3)); current openList(idx, 1:2); openList(idx, :) []; if isequal(current, goalPos) path reconstructPath(parent, current); return; end visited(current(1), current(2)) true; for k 1:8 neighbor current dirs(k, :); if ~inBounds(neighbor, rows, cols) || map(neighbor(1), neighbor(2)) 1 continue; end % 对角移动时检查两个相邻格是否都是障碍 if k 4 if map(current(1), neighbor(2)) 1 || map(neighbor(1), current(2)) 1 continue; end end tentativeG gScore(current(1), current(2)) costs(k); if tentativeG gScore(neighbor(1), neighbor(2)) gScore(neighbor(1), neighbor(2)) tentativeG; parent(neighbor(1), neighbor(2), :) current; f tentativeG heuristic(neighbor, goalPos, w); openList [openList; neighbor, f]; end end end path []; end function h heuristic(pos, goal, w) h w * sqrt((pos(1)-goal(1))^2 (pos(2)-goal(2))^2); end主循环每次从 openList 里取 f 最小的节点然后对 8 个邻居逐个判断。k 4那段是对角移动的额外检查如果当前点和邻居点之间的两个直角相邻格有任意一个是障碍就跳过这个对角移动避免路径贴着障碍角穿过去。w是权重参数常见取值 1.0 到 1.5设 1.0 就是标准 A*设 1.2 左右能在速度和最优性之间取平衡。costs数组里对角代价是sqrt(2)直线是 1这样路径不会无故斜着走。parent是三维数组存每个节点的父节点坐标最后reconstructPath从终点往回倒推。visited用来记录扩展过的节点画图时能看出搜索范围。如果跑完 openList 空了还没到终点返回空路径说明地图上起点到终点不连通。2.3 PRM采样、连线与查询三阶段PRM 是概率路线图思路和 A* 完全不同。它不在地图上逐格搜而是先随机撒点把能互相看见的点连成边构成一张路网然后在路网上搜路径。优点是适合高维空间和多次查询缺点是采样点不够时可能找不到路径而且每次跑结果不一样。这份实现把 PRM 拆成三步采样、建图、查询。采样阶段在地图自由区随机生成 N 个点建图阶段对每个点找 K 个最近邻如果两点连线不穿障碍就加边查询阶段用 A* 或 Dijkstra 在路网上搜。function roadmap buildPRM(map, numNodes, K) [rows, cols] size(map); nodes []; while size(nodes, 1) numNodes p [randi(rows), randi(cols)]; if map(p(1), p(2)) 0 nodes [nodes; p]; end end edges {}; for i 1:size(nodes, 1) dists sqrt(sum((nodes - nodes(i, :)).^2, 2)); [~, order] sort(dists); neighbors order(2:min(K1, end)); for j neighbors if ~collisionCheck(nodes(i, :), nodes(j, :), map) edges{end1} [i, j]; end end end roadmap.nodes nodes; roadmap.edges edges; end function free collisionCheck(p1, p2, map) steps max(abs(p2 - p1)) * 2; if steps 0, free true; return; end xs round(linspace(p1(1), p2(1), steps)); ys round(linspace(p1(2), p2(2), steps)); free all(map(sub2ind(size(map), xs, ys)) 0); end采样循环用randi生成随机行列落在障碍上的点直接丢弃直到凑够numNodes。建图时对每个节点算到其他所有节点的距离取最近的 K 个作为候选邻居再用collisionCheck判断连线是否穿障碍。collisionCheck的做法是在两点之间等距取采样点检查这些点是否都在自由区steps取坐标差最大值的两倍是为了保证采样密度够不会漏掉细障碍。参数上numNodes常见 100 到 500地图越大、障碍越复杂取值越高。K一般 5 到 15太小路网不连通太大边数爆炸、查询变慢。PRM 的随机性意味着同一张地图跑两次可能一个成功一个失败实际用的时候要么固定随机种子要么多跑几次取成功的那次。2.4 RRT从起点长树到双向 RRTRRT 是快速扩展随机树从起点开始每次随机采一个点找树上离它最近的节点朝那个方向走一步如果没撞障碍就把新节点加到树上。重复直到树长到终点附近。RRT 的优点是快、适合复杂环境缺点是路径通常很绕、不是最优。这份实现里除了基础 RRT还带了双向 RRT 的版本。双向 RRT 同时从起点和终点长两棵树交替扩展直到两棵树连上。这样收敛更快路径也更短。function path bidirectionalRRT(map, startPos, goalPos, stepSize, maxIter) treeA struct(nodes, startPos, parent, 0); treeB struct(nodes, goalPos, parent, 0); for iter 1:maxIter % 交替扩展两棵树 if mod(iter, 2) 1 [treeA, newIdx] extendTree(treeA, treeB.nodes(end, :), map, stepSize); if newIdx 0 norm(treeA.nodes(newIdx,:) - treeB.nodes(end,:)) stepSize path connectTrees(treeA, treeB, newIdx, size(treeB.nodes,1)); return; end else [treeB, newIdx] extendTree(treeB, treeA.nodes(end, :), map, stepSize); if newIdx 0 norm(treeB.nodes(newIdx,:) - treeA.nodes(end,:)) stepSize path connectTrees(treeB, treeA, newIdx, size(treeA.nodes,1)); return; end end end path []; end function [tree, newIdx] extendTree(tree, target, map, stepSize) dists sqrt(sum((tree.nodes - target).^2, 2)); [~, nearIdx] min(dists); nearNode tree.nodes(nearIdx, :); direction (target - nearNode) / norm(target - nearNode); newNode round(nearNode direction * stepSize); if inBounds(newNode, size(map,1), size(map,2)) ... map(newNode(1), newNode(2)) 0 ... ~collisionCheck(nearNode, newNode, map) tree.nodes [tree.nodes; newNode]; tree.parent [tree.parent; nearIdx]; newIdx size(tree.nodes, 1); else newIdx 0; end endbidirectionalRRT用两个 struct 分别存两棵树的节点和父节点索引。每次迭代交替扩展一棵树扩展时以另一棵树的最新节点为目标方向。extendTree先找树上离目标最近的节点朝目标方向走stepSize步新节点如果在地图内、不在障碍上、连线不穿障碍就加到树上。两棵树的最新节点距离小于stepSize时认为连通connectTrees把两棵树的路径拼起来。stepSize是关键参数太小树长得慢、迭代次数多太大容易在窄通道处撞障碍。常见取值 1 到 5 个栅格。maxIter是保险丝防止死循环一般设 5000 到 10000。双向 RRT 比单向快但实现复杂度高一点调试时先跑通单向再换双向。3. 避坑与排查路径规划仿真里最容易翻车的五件事3.1 路径穿墙或贴角现象是画出来的路径明明经过障碍物区域或者紧贴着障碍角斜穿过去。原因通常是碰撞检测只查了节点本身没查节点之间的连线对角移动时没检查两个直角相邻格。解决是把collisionCheck做成沿线段采样对角移动加相邻格判断采样步长至少是坐标差的两倍。3.2 A* 跑得动但路径不是最短现象是路径能到终点但绕路明显或者和 Dijkstra 结果差很多。原因多半是启发函数权重w设太大或者邻域代价没区分直线和对角。解决是把w调回 1.0 到 1.2检查costs数组里对角是不是sqrt(2)另外确认启发函数用的是欧氏距离而不是曼哈顿距离——8 邻域下用曼哈顿会高估代价。3.3 PRM 时好时坏现象是同一张地图有时候能找到路径有时候返回空。原因是采样点随机某次采样恰好没覆盖关键通道。解决是固定随机种子rng(42)保证可复现或者把numNodes加大、K调高再不行就在窄通道附近手动补几个采样点。3.4 RRT 路径抖动严重现象是路径由很多小折线段组成看起来像锯齿。原因是stepSize太小树节点太密。解决是适当加大stepSize或者在得到路径后做一次平滑常见做法是沿路径取隔点连线如果连线不穿障碍就替换中间段。3.5 MATLAB 版本和函数兼容问题现象是换台电脑跑报错提示函数不存在或行为不一致。原因是 Robotics Toolbox 版本差异或者用了新版本才有的函数。解决是尽量用基础矩阵运算少依赖工具箱如果必须用binaryOccupancyMap之类在代码开头检查ver输出把版本要求写在注释里。另外中文注释在部分版本里会乱码保存时选 UTF-8 编码。4. 进阶技巧把三种算法放进同一个对比框架跑通单个算法之后下一步通常是要做对比实验——同一张地图、同一个起点终点看三种算法的路径长度、扩展节点数、运行时间差多少。我一般会写一个统一的入口脚本把地图和起终点固定依次调用三个规划函数把结果存到结构体里再统一画图。% 统一对比入口 rng(42); % 固定随机种子保证 PRM/RRT 可复现 map buildMap(); startPos [2, 2]; goalPos [48, 48]; results struct(); % A* tic; [pathA, visitedA] improvedAStar(map, startPos, goalPos, 1.2); results.astar.time toc; results.astar.path pathA; results.astar.visited visitedA; % PRM tic; roadmap buildPRM(map, 200, 8); pathP queryPRM(roadmap, startPos, goalPos, map); results.prm.time toc; results.prm.path pathP; % 双向 RRT tic; pathR bidirectionalRRT(map, startPos, goalPos, 3, 8000); results.rrt.time toc; results.rrt.path pathR; % 统一可视化 figure; subplot(1,3,1); drawMap(map); hold on; plot(pathA(:,2), pathA(:,1), r-, LineWidth, 2); title(sprintf(A*: %.3fs, results.astar.time)); subplot(1,3,2); drawMap(map); hold on; plot(pathP(:,2), pathP(:,1), b-, LineWidth, 2); title(sprintf(PRM: %.3fs, results.prm.time)); subplot(1,3,3); drawMap(map); hold on; plot(pathR(:,2), pathR(:,1), g-, LineWidth, 2); title(sprintf(RRT: %.3fs, results.rrt.time));这个脚本的关键点是rng(42)放在最前面保证 PRM 和 RRT 每次跑出来的随机序列一致对比才有意义。tic/toc分别记录三种算法的耗时存进results结构体。画图时注意plot的第一个参数是列坐标、第二个是行坐标因为矩阵索引和笛卡尔坐标的 x/y 是反的这里容易翻车。对比时重点看三个指标路径长度、扩展节点数、运行时间。A* 通常路径最短但扩展节点多PRM 查询快但建图慢且路径偏折RRT 最快但路径最绕。如果要做改进算法的展示可以在同一框架里把改进前后的版本都跑一遍用表格把指标列出来。指标A*PRM双向 RRT路径长度最短中等偏长扩展节点数多少查询阶段中等运行时间中等建图慢快可复现性确定随机随机适合场景静态已知地图多次查询复杂高维环境最后说一个我自己的习惯每次改完参数或者换地图先只跑 A*确认地图和起终点没问题再跑 PRM 和 RRT。因为 A* 是确定性的它如果出问题大概率是地图或坐标搞错了而不是算法本身。从那以后我每次做路径规划对比实验都强制先跑一遍 A* 当基准确认无误再上随机算法。希望帮到你。本文还有配套的精品资源点击获取 MATLAB路径规划实战：改进A*、PRM与RRT算法实现与对比
简介这份MATLAB路径规划资源面向计算机及相关专业学生尤其适合正在做课程设计、期末大作业或希望以实战案例提升编程能力的学习者。内容围绕移动机器人路径规划展开整合了A*搜索、概率路线图PRM与快速探索随机树RRT三类经典算法及其改进版本可用于理解算法原理、掌握改进思路并积累MATLAB工程经验。压缩包共31个文件约6.28MB以20个m脚本为核心实现辅以zbak备份、gif运行演示、zip子包及license、md说明文档目录按A-Star、PRM-Astar、RRT等模块划分结构清晰便于对照学习。已有44人学习下载。代码经测试可读性与可扩展性较好读者能获得完整算法实现、可视化演示与模块化组织方式为课程项目或后续研究提供可靠参考。1. 从一份 MATLAB 路径规划作业说起A*、PRM、RRT 到底怎么选如果你正在做移动机器人方向的课程设计或者被“matlab 期末大作业”卡在仿真出图这一步大概率会遇到同一个问题A*、PRM、RRT 这三个算法名字都听过但真到写代码时不知道该用哪个、参数怎么调、为什么别人的图那么干净而自己的路径像喝多了。这份资源就是围绕这个痛点来的——它用 MATLAB 把改进 A*、PRM、RRT 三套路径规划算法完整实现了一遍包含栅格地图构建、算法主循环、路径平滑和可视化对比。适合两类人一类是刚接触路径规划、需要一份能跑通的参考实现来交作业或入门的新手另一类是已经写过基础版本、想看看改进点在哪、参数边界怎么设的熟手。下面我按“先讲清原理和选型再落到代码和参数最后说坑”的顺序拆一遍。2. 三种算法的 MATLAB 落地从栅格地图到路径输出2.1 为什么路径规划要先统一地图表示不管后面用哪种算法第一步都是把连续环境变成算法能吃的离散结构。移动机器人路径规划里最常见的做法是栅格法把工作空间切成 M×N 的方格每个格子标记为自由或障碍。这份资源里地图用一个二维矩阵表示0 是自由区1 是障碍物起点和终点用坐标索引给出。这样做的直接好处是三种算法可以共用同一张地图切换算法时只换规划函数不用重做环境。常见做法是用binaryOccupancyMap或者直接手写矩阵。手写矩阵更轻适合课程作业用 Robotics Toolbox 的binaryOccupancyMap则能直接对接后续的可视化和碰撞检测函数。这份实现走的是手写矩阵路线依赖少MATLAB 基础版本就能跑。% 构建 50x50 栅格地图1 表示障碍0 表示自由 mapSize 50; map zeros(mapSize, mapSize); % 添加矩形障碍物区域 map(10:20, 15:35) 1; map(30:40, 5:25) 1; map(25:35, 40:48) 1; % 起点和终点坐标行, 列 startPos [2, 2]; goalPos [48, 48]; % 检查起点终点是否落在障碍上 assert(map(startPos(1), startPos(2)) 0, 起点在障碍物内); assert(map(goalPos(1), goalPos(2)) 0, 终点在障碍物内);这段代码的逻辑很直白先开一块全零矩阵再把几块矩形区域置 1 当障碍。startPos和goalPos用的是行列索引不是笛卡尔坐标这一点后面写启发函数时要注意别搞反。assert两行是后悔药——如果起点或终点不小心落在障碍里算法会直接报错而不是给你一条穿墙路径。参数方面mapSize决定分辨率50×50 在课程作业里够用再大可视化会挤。障碍物坐标是硬编码的换成实际场景时可以从图片二值化或激光数据生成。注意 MATLAB 里矩阵索引是 (行, 列)而行对应 y 方向、列对应 x 方向写距离公式时如果混了路径会朝奇怪的方向偏。2.2 改进 A*启发函数和邻域扩展改在哪A* 的核心是f g hg 是起点到当前点的实际代价h 是当前点到终点的估计代价。基础版用 4 邻域或 8 邻域扩展启发函数用曼哈顿或欧氏距离。问题在于 8 邻域下直线和对角线代价没区分路径容易出现不必要的斜切另外障碍物密集时扩展节点多速度掉得厉害。这份实现里的改进点主要有两个一是启发函数加权重二是邻域扩展时对对角移动做障碍检测。加权启发是h w * euclideanDistancew 略大于 1 时搜索更偏向终点扩展节点数下降但 w 太大会丢最优性。对角检测是防止机器人从两个对角障碍之间“穿过去”。function [path, visited] improvedAStar(map, startPos, goalPos, w) [rows, cols] size(map); % 8 邻域方向 dirs [-1 0; 1 0; 0 -1; 0 1; -1 -1; -1 1; 1 -1; 1 1]; costs [1 1 1 1 sqrt(2) sqrt(2) sqrt(2) sqrt(2)]; gScore inf(rows, cols); gScore(startPos(1), startPos(2)) 0; openList [startPos, heuristic(startPos, goalPos, w)]; visited false(rows, cols); parent zeros(rows, cols, 2); while ~isempty(openList) [~, idx] min(openList(:, 3)); current openList(idx, 1:2); openList(idx, :) []; if isequal(current, goalPos) path reconstructPath(parent, current); return; end visited(current(1), current(2)) true; for k 1:8 neighbor current dirs(k, :); if ~inBounds(neighbor, rows, cols) || map(neighbor(1), neighbor(2)) 1 continue; end % 对角移动时检查两个相邻格是否都是障碍 if k 4 if map(current(1), neighbor(2)) 1 || map(neighbor(1), current(2)) 1 continue; end end tentativeG gScore(current(1), current(2)) costs(k); if tentativeG gScore(neighbor(1), neighbor(2)) gScore(neighbor(1), neighbor(2)) tentativeG; parent(neighbor(1), neighbor(2), :) current; f tentativeG heuristic(neighbor, goalPos, w); openList [openList; neighbor, f]; end end end path []; end function h heuristic(pos, goal, w) h w * sqrt((pos(1)-goal(1))^2 (pos(2)-goal(2))^2); end主循环每次从 openList 里取 f 最小的节点然后对 8 个邻居逐个判断。k 4那段是对角移动的额外检查如果当前点和邻居点之间的两个直角相邻格有任意一个是障碍就跳过这个对角移动避免路径贴着障碍角穿过去。w是权重参数常见取值 1.0 到 1.5设 1.0 就是标准 A*设 1.2 左右能在速度和最优性之间取平衡。costs数组里对角代价是sqrt(2)直线是 1这样路径不会无故斜着走。parent是三维数组存每个节点的父节点坐标最后reconstructPath从终点往回倒推。visited用来记录扩展过的节点画图时能看出搜索范围。如果跑完 openList 空了还没到终点返回空路径说明地图上起点到终点不连通。2.3 PRM采样、连线与查询三阶段PRM 是概率路线图思路和 A* 完全不同。它不在地图上逐格搜而是先随机撒点把能互相看见的点连成边构成一张路网然后在路网上搜路径。优点是适合高维空间和多次查询缺点是采样点不够时可能找不到路径而且每次跑结果不一样。这份实现把 PRM 拆成三步采样、建图、查询。采样阶段在地图自由区随机生成 N 个点建图阶段对每个点找 K 个最近邻如果两点连线不穿障碍就加边查询阶段用 A* 或 Dijkstra 在路网上搜。function roadmap buildPRM(map, numNodes, K) [rows, cols] size(map); nodes []; while size(nodes, 1) numNodes p [randi(rows), randi(cols)]; if map(p(1), p(2)) 0 nodes [nodes; p]; end end edges {}; for i 1:size(nodes, 1) dists sqrt(sum((nodes - nodes(i, :)).^2, 2)); [~, order] sort(dists); neighbors order(2:min(K1, end)); for j neighbors if ~collisionCheck(nodes(i, :), nodes(j, :), map) edges{end1} [i, j]; end end end roadmap.nodes nodes; roadmap.edges edges; end function free collisionCheck(p1, p2, map) steps max(abs(p2 - p1)) * 2; if steps 0, free true; return; end xs round(linspace(p1(1), p2(1), steps)); ys round(linspace(p1(2), p2(2), steps)); free all(map(sub2ind(size(map), xs, ys)) 0); end采样循环用randi生成随机行列落在障碍上的点直接丢弃直到凑够numNodes。建图时对每个节点算到其他所有节点的距离取最近的 K 个作为候选邻居再用collisionCheck判断连线是否穿障碍。collisionCheck的做法是在两点之间等距取采样点检查这些点是否都在自由区steps取坐标差最大值的两倍是为了保证采样密度够不会漏掉细障碍。参数上numNodes常见 100 到 500地图越大、障碍越复杂取值越高。K一般 5 到 15太小路网不连通太大边数爆炸、查询变慢。PRM 的随机性意味着同一张地图跑两次可能一个成功一个失败实际用的时候要么固定随机种子要么多跑几次取成功的那次。2.4 RRT从起点长树到双向 RRTRRT 是快速扩展随机树从起点开始每次随机采一个点找树上离它最近的节点朝那个方向走一步如果没撞障碍就把新节点加到树上。重复直到树长到终点附近。RRT 的优点是快、适合复杂环境缺点是路径通常很绕、不是最优。这份实现里除了基础 RRT还带了双向 RRT 的版本。双向 RRT 同时从起点和终点长两棵树交替扩展直到两棵树连上。这样收敛更快路径也更短。function path bidirectionalRRT(map, startPos, goalPos, stepSize, maxIter) treeA struct(nodes, startPos, parent, 0); treeB struct(nodes, goalPos, parent, 0); for iter 1:maxIter % 交替扩展两棵树 if mod(iter, 2) 1 [treeA, newIdx] extendTree(treeA, treeB.nodes(end, :), map, stepSize); if newIdx 0 norm(treeA.nodes(newIdx,:) - treeB.nodes(end,:)) stepSize path connectTrees(treeA, treeB, newIdx, size(treeB.nodes,1)); return; end else [treeB, newIdx] extendTree(treeB, treeA.nodes(end, :), map, stepSize); if newIdx 0 norm(treeB.nodes(newIdx,:) - treeA.nodes(end,:)) stepSize path connectTrees(treeB, treeA, newIdx, size(treeA.nodes,1)); return; end end end path []; end function [tree, newIdx] extendTree(tree, target, map, stepSize) dists sqrt(sum((tree.nodes - target).^2, 2)); [~, nearIdx] min(dists); nearNode tree.nodes(nearIdx, :); direction (target - nearNode) / norm(target - nearNode); newNode round(nearNode direction * stepSize); if inBounds(newNode, size(map,1), size(map,2)) ... map(newNode(1), newNode(2)) 0 ... ~collisionCheck(nearNode, newNode, map) tree.nodes [tree.nodes; newNode]; tree.parent [tree.parent; nearIdx]; newIdx size(tree.nodes, 1); else newIdx 0; end endbidirectionalRRT用两个 struct 分别存两棵树的节点和父节点索引。每次迭代交替扩展一棵树扩展时以另一棵树的最新节点为目标方向。extendTree先找树上离目标最近的节点朝目标方向走stepSize步新节点如果在地图内、不在障碍上、连线不穿障碍就加到树上。两棵树的最新节点距离小于stepSize时认为连通connectTrees把两棵树的路径拼起来。stepSize是关键参数太小树长得慢、迭代次数多太大容易在窄通道处撞障碍。常见取值 1 到 5 个栅格。maxIter是保险丝防止死循环一般设 5000 到 10000。双向 RRT 比单向快但实现复杂度高一点调试时先跑通单向再换双向。3. 避坑与排查路径规划仿真里最容易翻车的五件事3.1 路径穿墙或贴角现象是画出来的路径明明经过障碍物区域或者紧贴着障碍角斜穿过去。原因通常是碰撞检测只查了节点本身没查节点之间的连线对角移动时没检查两个直角相邻格。解决是把collisionCheck做成沿线段采样对角移动加相邻格判断采样步长至少是坐标差的两倍。3.2 A* 跑得动但路径不是最短现象是路径能到终点但绕路明显或者和 Dijkstra 结果差很多。原因多半是启发函数权重w设太大或者邻域代价没区分直线和对角。解决是把w调回 1.0 到 1.2检查costs数组里对角是不是sqrt(2)另外确认启发函数用的是欧氏距离而不是曼哈顿距离——8 邻域下用曼哈顿会高估代价。3.3 PRM 时好时坏现象是同一张地图有时候能找到路径有时候返回空。原因是采样点随机某次采样恰好没覆盖关键通道。解决是固定随机种子rng(42)保证可复现或者把numNodes加大、K调高再不行就在窄通道附近手动补几个采样点。3.4 RRT 路径抖动严重现象是路径由很多小折线段组成看起来像锯齿。原因是stepSize太小树节点太密。解决是适当加大stepSize或者在得到路径后做一次平滑常见做法是沿路径取隔点连线如果连线不穿障碍就替换中间段。3.5 MATLAB 版本和函数兼容问题现象是换台电脑跑报错提示函数不存在或行为不一致。原因是 Robotics Toolbox 版本差异或者用了新版本才有的函数。解决是尽量用基础矩阵运算少依赖工具箱如果必须用binaryOccupancyMap之类在代码开头检查ver输出把版本要求写在注释里。另外中文注释在部分版本里会乱码保存时选 UTF-8 编码。4. 进阶技巧把三种算法放进同一个对比框架跑通单个算法之后下一步通常是要做对比实验——同一张地图、同一个起点终点看三种算法的路径长度、扩展节点数、运行时间差多少。我一般会写一个统一的入口脚本把地图和起终点固定依次调用三个规划函数把结果存到结构体里再统一画图。% 统一对比入口 rng(42); % 固定随机种子保证 PRM/RRT 可复现 map buildMap(); startPos [2, 2]; goalPos [48, 48]; results struct(); % A* tic; [pathA, visitedA] improvedAStar(map, startPos, goalPos, 1.2); results.astar.time toc; results.astar.path pathA; results.astar.visited visitedA; % PRM tic; roadmap buildPRM(map, 200, 8); pathP queryPRM(roadmap, startPos, goalPos, map); results.prm.time toc; results.prm.path pathP; % 双向 RRT tic; pathR bidirectionalRRT(map, startPos, goalPos, 3, 8000); results.rrt.time toc; results.rrt.path pathR; % 统一可视化 figure; subplot(1,3,1); drawMap(map); hold on; plot(pathA(:,2), pathA(:,1), r-, LineWidth, 2); title(sprintf(A*: %.3fs, results.astar.time)); subplot(1,3,2); drawMap(map); hold on; plot(pathP(:,2), pathP(:,1), b-, LineWidth, 2); title(sprintf(PRM: %.3fs, results.prm.time)); subplot(1,3,3); drawMap(map); hold on; plot(pathR(:,2), pathR(:,1), g-, LineWidth, 2); title(sprintf(RRT: %.3fs, results.rrt.time));这个脚本的关键点是rng(42)放在最前面保证 PRM 和 RRT 每次跑出来的随机序列一致对比才有意义。tic/toc分别记录三种算法的耗时存进results结构体。画图时注意plot的第一个参数是列坐标、第二个是行坐标因为矩阵索引和笛卡尔坐标的 x/y 是反的这里容易翻车。对比时重点看三个指标路径长度、扩展节点数、运行时间。A* 通常路径最短但扩展节点多PRM 查询快但建图慢且路径偏折RRT 最快但路径最绕。如果要做改进算法的展示可以在同一框架里把改进前后的版本都跑一遍用表格把指标列出来。指标A*PRM双向 RRT路径长度最短中等偏长扩展节点数多少查询阶段中等运行时间中等建图慢快可复现性确定随机随机适合场景静态已知地图多次查询复杂高维环境最后说一个我自己的习惯每次改完参数或者换地图先只跑 A*确认地图和起终点没问题再跑 PRM 和 RRT。因为 A* 是确定性的它如果出问题大概率是地图或坐标搞错了而不是算法本身。从那以后我每次做路径规划对比实验都强制先跑一遍 A* 当基准确认无误再上随机算法。希望帮到你。本文还有配套的精品资源点击获取