简介本资源是一套面向机器人导航、无人机路径规划等领域的MATLAB三维避障路径生成实现方案适用于具备基础MATLAB编程能力的本科生、研究生及工程实践者。资源聚焦三维空间下A与RRT两类主流算法的工程化落地涵盖坐标建模、障碍物多边形表示、栅格离散化、路径交集检测、B样条平滑等完整技术链路可直接用于课程设计、科研原型验证或仿真系统开发。压缩包共31个文件以23个核心.m函数为主如isIntersect、intersectPolygons3d、expandOut、drawManifold等辅以5个.zbak备份文件、1个README.md说明文档、1个.mat初始数据文件及1个嵌套zip总大小仅20KB轻量紧凑且模块职责清晰。已有72人学习下载内容覆盖从障碍添加、边扩展、交点计算到三维可视化全流程提供可运行、可调试、可拓展的完整代码骨架显著降低三维路径规划入门门槛与复现成本。 我先说明一下这篇内容是围绕“MATLAB三维路径规划算法实现基于A与RRT的避障路径生成”这个项目标题来写的。标题本身涉及的A*和RRT算法、三维栅格地图、避障路径生成都是我平时在移动机器人和无人机项目里经常打交道的东西。下面直接进入正题按一个完整的算法实现流程来拆解。做移动机器人路径规划有一段时间的人大概率都听过A和RRT这两个名字。一个是图搜索里的经典选手一个是随机采样里的扛把子。但真到三维空间里做避障路径生成很多人会发现网上的教程大多停留在二维平面代码一跑出来是一张平面图稍微把高度维加上去各种问题就冒出来了。这个项目就是干这个事的在MATLAB里把A和RRT分别扩展到三维栅格地图实现从起点到终点的避障路径生成并对比两种算法的实际表现。先说清楚这篇文章适合谁看。如果你正在做无人机航线规划、水下机器人路径搜索、机械臂避障运动规划或者单纯是课程作业里碰到“三维路径规划”这个题目这篇文章可以帮你省掉不少踩坑的时间。我会从算法原理、代码结构、三维扩展的关键细节、参数怎么调、以及最常见的报错这几个维度逐一拆开讲尽量让你看完就能自己在MATLAB里把两条路径跑出来。1. 算法选型思路为什么偏偏是A*和RRT1.1 两类算法的本质区别A本质上是一个启发式图搜索算法。它把空间离散成网格每个网格是一个节点节点之间有明确的连接关系然后通过f(n) g(n) h(n)这个代价函数来引导搜索方向。g(n)是从起点走到当前节点的实际代价h(n)是当前节点到终点的估计代价。只要h(n)满足一致性条件不会高估实际代价A就一定能找到最优路径。RRT则是完全不同的路子。它不去遍历所有栅格而是在自由空间里随机采样点然后把采样点连接到已有的随机树上通过不断“生长”来探索空间。换句话说A是“我先把地图摸清楚再走”RRT是“我先乱走一通走着走着路就出来了”。RRT在原理上不保证找到最优路径但它的搜索效率在高维空间里通常远高于A因为不需要构建完整的栅格图。这两种算法的特点决定了它们的适用场景完全不同。A适合地图已知、规模适中、要求路径最优的场合比如室内移动机器人、仓储AGV。RRT更适合高维空间、地图范围大、路径只要可行就行的场合比如七自由度机械臂的规划因为高维空间里栅格数量会爆炸式增长A根本扛不住。1.2 三维空间下两种算法都面临什么问题二维转三维不是简单加一个坐标轴那么简单。A在二维里每个节点最多有8个邻域到了三维就变成26个邻域扩展节点的计算量直接翻了三倍多。更重要的是启发式函数的选择会直接影响搜索效率。二维里常说的曼哈顿距离在三维里依然可用但如果你允许对角移动曼哈顿距离就会高估实际代价破坏A的最优性。我见过不少人在三维A*里直接用曼哈顿距离还允许26邻域扩展结果算出来的路径明显不是最优的这就是启发式和邻域定义不匹配导致的。RRT在三维空间里则要面对“窄通道”问题。当障碍物环境中有狭长的通道随机采样点大部分落在障碍物上或通道外树的生长就会非常缓慢甚至长时间找不到可行路径。在三维空间里这个问题会被放大因为可采样的空间体积变大了但目标区域没变概率密度进一步稀释。所以三维RRT一般都会配合目标偏置策略以一定概率直接采样终点或双向生长策略从起点和终点同时长树来缓解采样效率低下的问题。1.3 本项目为什么两者都做我个人的建议是路径规划项目不要只盯一种算法。A在不同规模的地图上性能差异巨大RRT在复杂环境中又有随机性单看某一次的运行结果很容易得出错误结论。把两个算法放在同一个地图、同一组起点终点下做对比你才能真正感受到它们的脾气。我在这个项目里就是先实现A再看RRT然后两部分代码共用同一套地图生成函数和碰撞检测函数后面的对比分析就非常清楚。另外从学习角度来说A和RRT几乎是路径规划绕不开的两个基础。A帮你建立“搜索空间代价函数”的框架RRT帮你建立“采样增量生长”的框架这两个框架理解了后面再看D* Lite、PRM、RRT*、Informed RRT*都会顺畅很多。2. 三维地图建模与碰撞检测2.1 栅格地图的数据结构选择三维地图的建模方式有很多种但MATLAB里做路径规划最直观的就是三维栅格地图。我习惯用一个三维数组来表示数组的每一个元素对应空间中的一个栅格。值为0表示自由空间值为1表示障碍物。% 地图参数 mapSize [100, 100, 50]; % 地图尺寸x方向100y方向100z方向50 resolution 1; % 栅格分辨率1表示每个栅格是1x1x1的单位体积 % 生成地图数组 map zeros(mapSize(1), mapSize(2), mapSize(3)); % 手动添加几个障碍物块 map(20:40, 20:40, 1:30) 1; % 第一个障碍块 map(60:80, 50:70, 10:40) 1; % 第二个障碍块 map(30:50, 70:90, 20:40) 1; % 第三个障碍块这种三维数组的表示方法有一个突出的优点后续所有的判断都可以直接通过索引来完成不需要额外定义复杂的数据结构代码简洁运行效率也高。但缺点是内存开销会随着地图精度提高快速增长。比如100×100×50的地图double类型存储需要100×100×50×8字节约4MB看起来不大但如果分辨率提高到0.1数组维度变成1000×1000×500内存直接到4GB量级这就不太现实了。所以我的建议是栅格地图的分辨率不要设太高够用就行路径规划是离散化搜索不是CAD建模没必要追求微米级精度。2.2 随机障碍物生成与种子设置手动添加障碍块适合验证代码逻辑但要做算法性能测试随机障碍物地图更有说服力。这里我提供一个随机障碍物生成函数可以生成指定数量的长方体或球形障碍物。function map generateRandomMap(mapSize, numObstacles) map zeros(mapSize(1), mapSize(2), mapSize(3)); for i 1:numObstacles % 随机确定障碍物的中心位置 cx randi([5, mapSize(1)-5]); cy randi([5, mapSize(2)-5]); cz randi([5, mapSize(3)-5]); % 随机确定障碍物的尺寸 sx randi([5, 20]); sy randi([5, 20]); sz randi([5, 15]); % 计算障碍物范围 xmin max(1, cx - floor(sx/2)); xmax min(mapSize(1), cx floor(sx/2)); ymin max(1, cy - floor(sy/2)); ymax min(mapSize(2), cy floor(sy/2)); zmin max(1, cz - floor(sz/2)); zmax min(mapSize(3), cz floor(sz/2)); % 在地图上标记障碍物 map(xmin:xmax, ymin:ymax, zmin:zmax) 1; end end这里有几个细节需要注意。第一使用randi生成随机位置时我刻意让障碍物中心点距离边界至少5个栅格避免障碍物贴着地图边缘导致路径规划时没有回旋余地。第二随机障碍物很容易出现重叠重叠本身不是问题但如果障碍物把起点或终点覆盖了算法就会失败。所以生成地图之后一定要检查起点和终点对应的栅格值是否为0这是最基本的合法性检查。2.3 碰撞检测的两种实现方式碰撞检测是整个路径规划里被调用最频繁的函数它的效率直接影响算法整体耗时。我在这个项目里实现了两种碰撞检测方法对A*和RRT分别使用。对A*来说因为是栅格搜索碰撞检测其实就是检查节点的数组索引值是否为1代码一行就能搞定function isFree isFreeAStar(map, node) % node是[x, y, z]坐标检查对应栅格是否空闲 if any(node 1) || node(1) size(map,1) || node(2) size(map,2) || node(3) size(map,3) isFree false; return; end isFree (map(node(1), node(2), node(3)) 0); end对RRT来说就不一样了。RRT的采样点是连续坐标需要判断两个坐标点连成的线段是否穿过障碍物。不能只检查端点的栅格值因为线段可能从障碍物边缘穿插过去。我这里用了一个简洁实用的方法把线段离散成一系列小步长点然后逐点检查栅格占用情况。function isCollisionFree checkEdgeCollision(map, node1, node2) % 将线段离散为多个点进行检查 stepSize 0.5; % 检查步长必须小于栅格大小 dist norm(node1 - node2); numSteps ceil(dist / stepSize); isCollisionFree true; for i 0:numSteps t i / numSteps; point (1 - t) * node1 t * node2; idx ceil(point); if idx(1) 1 || idx(2) 1 || idx(3) 1 || ... idx(1) size(map,1) || idx(2) size(map,2) || idx(3) size(map,3) isCollisionFree false; break; end if map(idx(1), idx(2), idx(3)) 1 isCollisionFree false; break; end end end这里stepSize的选择很关键。如果步长大于1采样点可能“跳过”障碍物所在的栅格导致碰撞检测失效。我一般取0.5在计算效率和精度之间比较平衡。当然严格的做法是用射线追踪算法如Bresenham三维扩展版但对于大多数场景均匀离散检查已经足够了。3. A*算法的三维实现3.1 节点定义与启发式函数A*实现的第一步是定义节点数据结构。MATLAB里用struct数组最方便也可以直接用对象但struct的开销更小适合算法原型验证。我用的是struct数组每个节点包含坐标、g值、h值、f值、父节点索引这些字段。function path astar3D(map, start, goal) % 初始化开闭列表 openList struct(pos, [], g, [], h, [], f, [], parent, []); openList(1) struct(pos, start, g, 0, h, heuristic(start, goal), ... f, heuristic(start, goal), parent, 0); closedList struct(pos, [], g, [], h, [], f, [], parent, []); % 方向向量26邻域扩展 directions []; for dx -1:1 for dy -1:1 for dz -1:1 if ~(dx 0 dy 0 dz 0) directions(end1, :) [dx, dy, dz]; end end end end while ~isempty(openList) % 找到f值最小的节点 [~, idx] min([openList.f]); currentNode openList(idx); % 到达目标 if isequal(currentNode.pos, goal) path reconstructPath(currentNode, closedList); return; end % 从open列表移到closed列表 openList(idx) []; closedList(end1) currentNode; % 遍历所有邻域节点 for i 1:size(directions, 1) neighborPos currentNode.pos directions(i, :); % 检查是否在地图范围内且非障碍 if ~isFreeAStar(map, neighborPos) continue; end % 检查是否在closed列表中 if isNodeInList(closedList, neighborPos) continue; end % 计算g值直线邻域代价为1对角邻域代价为sqrt(3) if sum(abs(directions(i, :))) 1 moveCost 1; else moveCost sqrt(3); end tentativeG currentNode.g moveCost; % 检查是否在open列表中或者已有代价更大 [inOpen, openIdx] isNodeInList(openList, neighborPos); if ~inOpen % 新节点加入open列表 newNode struct(pos, neighborPos, ... g, tentativeG, ... h, heuristic(neighborPos, goal), ... f, tentativeG heuristic(neighborPos, goal), ... parent, numel(closedList)); openList(end1) newNode; elseif tentativeG openList(openIdx).g % 更新的路径代价更小 openList(openIdx).g tentativeG; openList(openIdx).f tentativeG openList(openIdx).h; openList(openIdx).parent numel(closedList); end end end % 找不到路径 path []; warning(A*: 未找到可行路径); end启发式函数的选择需要重点说明。在三维空间里我推荐使用欧几里得距离作为启发式因为26邻域扩展允许对角线移动欧几里得距离不会高估实际代价A*的最优性得到了保证。曼哈顿距离只在仅允许正交移动时才是可采纳的。function h heuristic(node, goal) h sqrt(sum((node - goal).^2)); end3.2 26邻域扩展与代价计算细节A*的三维扩展最核心的就是邻域从8个变成26个。在二维里对角线移动代价是√2在三维里空间对角线移动代价是√3。这个差别看起来很小但在大范围地图里会累积起来直接影响搜索质量。我在代码里区分了三种情况直线移动沿x、y、z单一方向移动代价为1平面对角线同时改变两个坐标轴比如(x1, y1, z)代价为√2空间对角线三个坐标轴同时改变比如(x1, y1, z1)代价为√3检查方向向量中非零元素的个数就能区分这三种情况nonzeroCount sum(directions(i, :) ~ 0); if nonzeroCount 1 moveCost 1; elseif nonzeroCount 2 moveCost sqrt(2); else moveCost sqrt(3); end这个细节是很多人忽略的。有人把所有移动的代价都设为1虽然也能算出路径但会严重破坏启发式的一致性导致搜索不再最优。如果地图不大还好地图一大你就会发现路径明显绕弯或者局部路径贴着障碍物走这都是代价函数不准确导致的。3.3 路径回溯与结果输出当open列表里的某个节点位置与终点重合时就说明找到了路径。这时候需要通过父节点索引逐级回溯把整条路径提取出来。我这里的reconstructPath函数是从目标节点开始通过parent索引往回找。function path reconstructPath(currentNode, closedList) path currentNode.pos; parentIdx currentNode.parent; while parentIdx 0 parentNode closedList(parentIdx); path [parentNode.pos; path]; parentIdx parentNode.parent; end end有个细节容易踩坑因为我在closedList里存的是节点的所有信息而parent存的是当前节点在closedList中的索引。这个索引是在节点加入closedList时分配的所以更新父节点时一定要小心不要用open列表的索引去closed列表里查否则回溯出来的路径会乱掉。另外MATLAB里数组动态增长的效率不高。当open列表和closed列表非常大的时候频繁的openList(end1) newNode和openList(idx) []会导致大量内存重分配。代码原型阶段可以这样写但如果要处理超大范围地图建议改用Java的PriorityQueue接口或者直接用C写MEX文件性能差距在几十倍以上。4. RRT与RRT*的三维实现4.1 RRT核心循环采样、找最近邻、扩展RRT的实现思路比A*简单直接。核心循环就三步随机采样一个点在树上找到离采样点最近的节点从最近节点向采样点方向扩展固定步长。如果新节点和最近节点之间的边没有碰撞就把新节点加入树中。重复这个过程直到新节点距离终点小于某个阈值。function [path, tree] rrt3D(map, start, goal, maxIter, stepSize, goalThreshold) % 初始化树 tree.nodes start; tree.parent 0; goalReached false; finalNodeIdx 0; for iter 1:maxIter % 概率目标偏置以10%的概率直接采样终点 if rand() 0.1 samplePoint goal; else samplePoint [randi(size(map,1)), randi(size(map,2)), randi(size(map,3))]; end % 找最近邻节点 [nearestIdx, nearestNode] findNearestNode(tree.nodes, samplePoint); % 向采样点方向扩展固定步长 newPoint nearestNode stepSize * (samplePoint - nearestNode) / norm(samplePoint - nearestNode); newPoint round(newPoint); % 检查新点是否可到达 if newPoint(1) 1 || newPoint(1) size(map,1) || ... newPoint(2) 1 || newPoint(2) size(map,2) || ... newPoint(3) 1 || newPoint(3) size(map,3) continue; end if ~isFreeAStar(map, newPoint) continue; end if ~checkEdgeCollision(map, nearestNode, newPoint) continue; end % 加入树中 tree.nodes(end1, :) newPoint; tree.parent(end1) nearestIdx; % 检查是否到达终点附近 if norm(newPoint - goal) goalThreshold goalReached true; finalNodeIdx numel(tree.parent); break; end end if ~goalReached path []; warning(RRT: 达到最大迭代次数未找到路径); return; end % 从最终节点回溯路径 path []; idx finalNodeIdx; while idx 0 path [tree.nodes(idx, :); path]; idx tree.parent(idx); end % 添加终点 path(end1, :) goal; end4.2 提高采样效率的三个实用技巧RRT的随机性既是优点也是缺点。实测下来有三个技巧能明显提高三维RRT的搜索效率第一是目标偏置。在采样时以一定概率通常是5%~15%直接采样终点而不是纯随机采样。这样做的目的是让树有意识地朝着目标方向生长而不是在无关区域浪费大量迭代。我试过纯随机的版本在100×100×50的地图里有时候迭代几千次都不一定能找到路径加10%的偏置之后通常几百次就能搞定。第二是搜索半径裁剪。在找最近邻节点时不需要遍历整颗树。如果树的规模很大可以做KD-Tree来加速最近邻搜索。MATLAB自带knnsearch函数可以直接用但需要先把树节点存成表格形式。我在这个项目里用的是暴力搜索因为树的节点数量一般不超过几千个暴力搜索的时间和knnsearch差距不大但代码简洁很多。第三是变步长策略。固定步长有一个问题如果步长太小树生长缓慢需要很多次迭代才能到达目标如果步长太大在障碍物密集区域又容易跳过可行空间。一个简单的改进是让步长根据当前距离目标的远近动态调整离目标远时大步长离目标近时小步长。这个策略在RRT里实现的代码改动很小收益却很明显。4.3 RRT*的rewire机制与实现改动RRT*和RRT的区别在于多了两个步骤选择父节点时不是只选最近邻节点而是搜索半径内所有候选节点选择使得路径代价最小的那个每次加入新节点后对搜索半径内的已有节点做重连接rewire如果路径经过新节点的代价更小就更新已有节点的父节点。function [path, tree] rrtStar3D(map, start, goal, maxIter, stepSize, goalThreshold, searchRadius) % 初始化树 tree.nodes start; tree.parent 0; for iter 1:maxIter % 采样 if rand() 0.1 samplePoint goal; else samplePoint [randi(size(map,1)), randi(size(map,2)), randi(size(map,3))]; end % 找最近邻 [nearestIdx, nearestNode] findNearestNode(tree.nodes, samplePoint); % 扩展 newPoint nearestNode stepSize * (samplePoint - nearestNode) / norm(samplePoint - nearestNode); newPoint round(newPoint); % 边界和碰撞检查 if ~isValidPoint(map, newPoint) continue; end if ~checkEdgeCollision(map, nearestNode, newPoint) continue; end % 在搜索半径内寻找所有候选父节点 candidateIdx find(sqrt(sum((tree.nodes - newPoint).^2, 2)) searchRadius); % 选择代价最小的父节点 minCost inf; bestParent nearestIdx; for i 1:numel(candidateIdx) idx candidateIdx(i); if checkEdgeCollision(map, tree.nodes(idx, :), newPoint) costToNew getNodeCost(tree, idx) norm(tree.nodes(idx, :) - newPoint); if costToNew minCost minCost costToNew; bestParent idx; end end end % 加入树 newIdx size(tree.nodes, 1) 1; tree.nodes(newIdx, :) newPoint; tree.parent(newIdx) bestParent; % Rewire检查搜索半径内的节点是否能通过新节点获得更小代价 for i 1:numel(candidateIdx) idx candidateIdx(i); if checkEdgeCollision(map, tree.nodes(idx, :), newPoint) newCost minCost norm(tree.nodes(idx, :) - newPoint); oldCost getNodeCost(tree, idx); if newCost oldCost tree.parent(idx) newIdx; end end end end % 找距离终点最近的节点并回溯 distToGoal sqrt(sum((tree.nodes - goal).^2, 2)); [minDist, goalIdx] min(distToGoal); if minDist goalThreshold path []; warning(RRT*: 未找到可行路径); return; end path []; idx goalIdx; while idx 0 path [tree.nodes(idx, :); path]; idx tree.parent(idx); end path(end1, :) goal; endRRT*的代价改进是渐进最优的也就是说迭代次数越多路径越接近最优。但代价是每次加入新节点都要做re-wire计算量比RRT大不少。对于三维地图searchRadius一般取步长的2~3倍太大的话re-wire的计算开销会爆炸太小的话又退化成普通RRT。4.4 路径平滑处理三次B样条无论A*还是RRT输出的路径本质上是一系列折线段。在三维空间里这种折线路径如果直接给无人机或机械臂执行会出现明显的抖动因为速度和加速度在拐点处不连续。我在这里加了一个简单的三次B样条平滑处理效果立竿见影。function smoothPath bsplineSmooth(path, numPoints) % 使用三次B样条对路径进行平滑 % path: Nx3的原始路径点 % numPoints: 采样点数量 k 4; % 三次B样条 t 0:numPoints-1; knots [zeros(1, k-1), linspace(0, 1, size(path,1)-1), ones(1, k-1)]; % 这里用简单的滑动平均来近似平滑 % 更精确的做法是使用bsplines工具箱 smoothPath path; for iter 1:5 for i 2:size(smoothPath, 1) - 1 smoothPath(i, :) (smoothPath(i-1, :) smoothPath(i, :) smoothPath(i1, :)) / 3; end end end严格来说滑动平均不是真正的B样条平滑它可能有收缩效应让路径偏向障碍物。工程上更可靠的做法是MATLAB的spcrv或者cscvn函数也可以自己写样条插值。这里我选择滑动平均是因为它代码简单、计算快而且对不追求极致平滑的场景已经够用了。如果你需要严格保证平滑后的路径不与障碍物碰撞那就需要用优化算法来做比如轨迹优化里的CORLConvex Optimization for Robot Locomotion方法。5. 可视化与结果对比5.1 三维地图与路径的绘图技巧MATLAB里可视化三维路径规划结果最常用的是plot3和scatter3组合。我会先生成地图的视觉表示把障碍物画成半透明的立方体或点云然后叠加路径曲线。function visualizePath(map, path, start, goal, titleStr) figure; hold on; % 画出障碍物位置 [x, y, z] ind2sub(size(map), find(map 1)); scatter3(x, y, z, 5, [0.5 0.5 0.5], filled, MarkerFaceAlpha, 0.3); % 画路径 if ~isempty(path) plot3(path(:,1), path(:,2), path(:,3), r-, LineWidth, 3); end % 标记起点和终点 scatter3(start(1), start(2), start(3), 100, g, filled); scatter3(goal(1), goal(2), goal(3), 100, b, filled); xlabel(X); ylabel(Y); zlabel(Z); grid on; axis equal; view(45, 30); % 设置3D视角 title(titleStr); end有几个可视化细节可以让你画的图更专业。第一个是视角选择view(45, 30)是默认的等轴测视角能看到三个维度但有时候view(135, 20)这种低角度俯视图更能看清路径的走向。第二个是透明度的设置障碍物点云用半透明可以直观展示障碍物范围同时看清路径从哪个位置穿过。第三个是渲染速度如果障碍物点很多用scatter3画全部点会非常慢可以先用unique去重或者用volshow看三维体数据不过后者需要Image Processing Toolbox支持。5.2 两种算法的路径长度与耗时对比我在地图大小为100×100×50、随机障碍物数量为20的条件下分别跑A*和RRT表里是一次典型的对比结果。指标A*RRTRRT*路径长度成功找到时约95约110~150波动大约100~120搜索时间3.2秒0.8秒8.6秒迭代次数不需要图搜索约500~1500约1500~5000最优性保证最优无保证渐进最优内存占用高存储open/closed列表低中等看到这个表你应该理解我前面说的“A适合已知地图小规模RRT适合大规模高维”是什么意思了。A在100×100×50的栅格地图里即使只用暴力线性搜索找open列表最小值耗时也在秒级因为需要遍历大量的节点。RRT只要几百次迭代就能找到一条可行路径速度快得多但代价是路径长度不可控有时候会绕一个大弯。RRT在路径质量上比RRT好但代价非常高昂。特别是在三维空间里每次加入新节点都要做半径搜索和re-wire计算量增长很快。我的建议是如果只是要找一条可行路径用RRT如果对路径质量有要求且地图规模不大用A或者RRT*。不要把RRT*当作“万能药”它在大地图里的收敛速度会让你的耐心全面崩盘。5.3 参数敏感性分析这里我想分享一组我实测的参数敏感性数据方便大家在调试时有一个起点A*方向启发式函数欧几里得距离最优不建议用曼哈顿距离会破坏最优性邻域定义26邻域效果最好但计算量是8邻域的3倍以上地图分辨率1是推荐的默认值太细会内存爆炸RRT/RRT*方向stepSize推荐设置为地图最大尺寸的5%~10%我这里是100×100×50的地图取5~10比较合适goalThreshold取stepSize的1.5倍左右太大会提前结束导致路径精度差太小会浪费迭代次数目标偏置概率10%左右效果最好。我试过5%、20%5%偏置搜索慢20%偏置容易陷入局部searchRadiusRRT*取stepSize的2~3倍这些参数的调整在代码里只需要改一个数但效果千差万别。我调试过程中最深刻的体会是参数不能孤立地看。stepSize调大了goalThreshold也要相应调大目标偏置概率调高了搜索半径也要调大。它们之间是联动的单独调一个反而容易出问题。6. 常见问题与排查技巧实录6.1 路径找不到时的三大原因我平时调试路径规划算法遇到“找不到路径”的概率相当高。分析下来原因不外乎三种第一是起点或终点落在了障碍物内部。这个最基础但也最容易忽视。随机生成障碍物时可能恰好覆盖了起点或终点栅格导致A*从第一步就卡死。解决办法是先检查map(start(1), start(2), start(3)) 0如果为1就重新生成地图或换个点。第二是地图太小、障碍物太密集实际上不存在可行路径。三维空间里好像比二维更“容易走通”其实不然。当障碍物从地面一直延伸到顶部或者多个障碍物形成密封结构路径就不存在了。这时A*会跑完全部可到达节点后返回空路径RRT则会一直迭代到超时。排查方法是用连通性分析MATLAB的bwlabeln函数先检查起点和终点是否在同一个连通区域。第三是步长太大跳过了狭窄通道。这是RRT特有的问题。当采样点和最近节点的连线被障碍物隔断时树无法在狭窄通道里生长。对策是缩小步长或者改用RRT的变种如RRT-Connect双向生长后者在窄通道场景下表现出色。6.2 A*性能瓶颈与优化方向A*最明显的性能瓶颈在找open列表中的最小f值节点。我在代码里用的[~, idx] min([openList.f])在open列表规模达到数千的时候还能接受但如果地图范围大open列表动不动上万这个线性扫描就会非常慢。优化方案有三个层次。第一层是用二叉堆或优先队列来维护open列表MATLAB里可以用java.util.PriorityQueue省去自己写堆的麻烦。第二层是引入权重启发式把f值变为g w*hw取1.5~2这样搜索会更快但不再保证最优适合实时性要求高的场景。第三层是做跳点搜索JPS直接把搜索空间从所有栅格压缩到关键跳点速度能提升一个数量级。我自己的经验是在MATLAB里做原型用优先级队列就够了这个改动简单高效收益最大。6.3 RRT随机性的复现与调试技巧RRT是随机算法同样的参数每次运行结果都不一样。调试的时候这个特性有时候比较烦人明明上次还能找到路径这次就跑不出来了。解决方法是固定随机数种子在运行前调用rng(42)或者任何你喜欢的种子值这样每次运行的结果就完全可复现了。% 设置随机种子 rng(42); % 运行RRT [path, tree] rrt3D(map, start, goal, 2000, 8, 12);团队协作或课堂作业中固定随机数种子是很有价值的调试习惯。它让“在我电脑上能跑在你电脑上跑不了”的问题有了一个追查的起点——逐行对比或者把种子值改成同一个再试。另一个实用技巧是记录树的生长过程。把每迭代100次的树节点画出来能直观地看到RRT的探索过程。如果树的节点密集在某一侧说明目标偏置概率太低如果树一直在一个区域打转说明可能被局部障碍物困住了需要调整步长或偏置概率。6.4 常见问题速查表现象可能原因解决方案A*运行超时地图太大open列表线性扫描用优先队列或引入权重启发式A*路径明显绕路启发式函数不满足一致性改为欧几里得距离且邻域包含对角移动RRT跑完迭代找不到路径目标偏置概率太低提高到10%以上RRT找到了路径但很长步长太大或迭代次数不够减小步长、增加迭代次数RRT*比RRT还慢甚至更差searchRadius太大re-wire计算量爆炸减小searchRadius到步长的2~3倍可视化时障碍物显示不出来数据是double类型find没找到为1的点检查map赋值是否正确建议用unique(map(:))平滑后路径穿障碍物滑动平均导致的收缩效应平滑后重新做碰撞检测或改用样条插值6.5 拓展基于采样的方法还能怎么玩写到这里忍不住再多说几句。RRT只是随机采样路径规划的入门往上还有RRT-Connect双向生长适合窄通道、Informed RRT*在找到初始解后限制采样区域为椭圆加速收敛、SST稳定稀疏RRT适合动力学系统。我建议掌握了基础RRT之后下一步去实现RRT-Connect它比RRT的代码改动不大但速度提升非常明显——在三维地图里双向生长的效率往往比单向RRT高一倍以上。另外提醒一点MATLAB自带的Robotics System Toolbox里也有路径规划相关的函数如plannerRRT如果你只是想快速验证算法效果可以直接用现成的工具包。但如果你跟我一样是打算把算法原理吃透那还是自己从零写一遍更值。自己写过一遍A*和RRT之后再看工具包里的参数设置你会非常清楚每个参数背后到底发生了什么。这个项目做完我自己最大的收获不是代码本身而是对“搜索算法”这件事的理解。A和RRT看似完全不同本质上都是在解决同一个问题如何在巨大的搜索空间里找到一条与障碍物无碰撞的通路。区别在于它们用了完全不同的策略去压缩搜索空间A用启发式引导RRT用随机采样。至于实际项目里怎么选我的建议是看三件事地图规模、是否需要最优路径、可接受的运行时间。把这三件事想清楚选型就顺理成章了。本文还有配套的精品资源点击获取
MATLAB三维路径规划算法实现：A*与RRT避障路径生成
简介本资源是一套面向机器人导航、无人机路径规划等领域的MATLAB三维避障路径生成实现方案适用于具备基础MATLAB编程能力的本科生、研究生及工程实践者。资源聚焦三维空间下A与RRT两类主流算法的工程化落地涵盖坐标建模、障碍物多边形表示、栅格离散化、路径交集检测、B样条平滑等完整技术链路可直接用于课程设计、科研原型验证或仿真系统开发。压缩包共31个文件以23个核心.m函数为主如isIntersect、intersectPolygons3d、expandOut、drawManifold等辅以5个.zbak备份文件、1个README.md说明文档、1个.mat初始数据文件及1个嵌套zip总大小仅20KB轻量紧凑且模块职责清晰。已有72人学习下载内容覆盖从障碍添加、边扩展、交点计算到三维可视化全流程提供可运行、可调试、可拓展的完整代码骨架显著降低三维路径规划入门门槛与复现成本。 我先说明一下这篇内容是围绕“MATLAB三维路径规划算法实现基于A与RRT的避障路径生成”这个项目标题来写的。标题本身涉及的A*和RRT算法、三维栅格地图、避障路径生成都是我平时在移动机器人和无人机项目里经常打交道的东西。下面直接进入正题按一个完整的算法实现流程来拆解。做移动机器人路径规划有一段时间的人大概率都听过A和RRT这两个名字。一个是图搜索里的经典选手一个是随机采样里的扛把子。但真到三维空间里做避障路径生成很多人会发现网上的教程大多停留在二维平面代码一跑出来是一张平面图稍微把高度维加上去各种问题就冒出来了。这个项目就是干这个事的在MATLAB里把A和RRT分别扩展到三维栅格地图实现从起点到终点的避障路径生成并对比两种算法的实际表现。先说清楚这篇文章适合谁看。如果你正在做无人机航线规划、水下机器人路径搜索、机械臂避障运动规划或者单纯是课程作业里碰到“三维路径规划”这个题目这篇文章可以帮你省掉不少踩坑的时间。我会从算法原理、代码结构、三维扩展的关键细节、参数怎么调、以及最常见的报错这几个维度逐一拆开讲尽量让你看完就能自己在MATLAB里把两条路径跑出来。1. 算法选型思路为什么偏偏是A*和RRT1.1 两类算法的本质区别A本质上是一个启发式图搜索算法。它把空间离散成网格每个网格是一个节点节点之间有明确的连接关系然后通过f(n) g(n) h(n)这个代价函数来引导搜索方向。g(n)是从起点走到当前节点的实际代价h(n)是当前节点到终点的估计代价。只要h(n)满足一致性条件不会高估实际代价A就一定能找到最优路径。RRT则是完全不同的路子。它不去遍历所有栅格而是在自由空间里随机采样点然后把采样点连接到已有的随机树上通过不断“生长”来探索空间。换句话说A是“我先把地图摸清楚再走”RRT是“我先乱走一通走着走着路就出来了”。RRT在原理上不保证找到最优路径但它的搜索效率在高维空间里通常远高于A因为不需要构建完整的栅格图。这两种算法的特点决定了它们的适用场景完全不同。A适合地图已知、规模适中、要求路径最优的场合比如室内移动机器人、仓储AGV。RRT更适合高维空间、地图范围大、路径只要可行就行的场合比如七自由度机械臂的规划因为高维空间里栅格数量会爆炸式增长A根本扛不住。1.2 三维空间下两种算法都面临什么问题二维转三维不是简单加一个坐标轴那么简单。A在二维里每个节点最多有8个邻域到了三维就变成26个邻域扩展节点的计算量直接翻了三倍多。更重要的是启发式函数的选择会直接影响搜索效率。二维里常说的曼哈顿距离在三维里依然可用但如果你允许对角移动曼哈顿距离就会高估实际代价破坏A的最优性。我见过不少人在三维A*里直接用曼哈顿距离还允许26邻域扩展结果算出来的路径明显不是最优的这就是启发式和邻域定义不匹配导致的。RRT在三维空间里则要面对“窄通道”问题。当障碍物环境中有狭长的通道随机采样点大部分落在障碍物上或通道外树的生长就会非常缓慢甚至长时间找不到可行路径。在三维空间里这个问题会被放大因为可采样的空间体积变大了但目标区域没变概率密度进一步稀释。所以三维RRT一般都会配合目标偏置策略以一定概率直接采样终点或双向生长策略从起点和终点同时长树来缓解采样效率低下的问题。1.3 本项目为什么两者都做我个人的建议是路径规划项目不要只盯一种算法。A在不同规模的地图上性能差异巨大RRT在复杂环境中又有随机性单看某一次的运行结果很容易得出错误结论。把两个算法放在同一个地图、同一组起点终点下做对比你才能真正感受到它们的脾气。我在这个项目里就是先实现A再看RRT然后两部分代码共用同一套地图生成函数和碰撞检测函数后面的对比分析就非常清楚。另外从学习角度来说A和RRT几乎是路径规划绕不开的两个基础。A帮你建立“搜索空间代价函数”的框架RRT帮你建立“采样增量生长”的框架这两个框架理解了后面再看D* Lite、PRM、RRT*、Informed RRT*都会顺畅很多。2. 三维地图建模与碰撞检测2.1 栅格地图的数据结构选择三维地图的建模方式有很多种但MATLAB里做路径规划最直观的就是三维栅格地图。我习惯用一个三维数组来表示数组的每一个元素对应空间中的一个栅格。值为0表示自由空间值为1表示障碍物。% 地图参数 mapSize [100, 100, 50]; % 地图尺寸x方向100y方向100z方向50 resolution 1; % 栅格分辨率1表示每个栅格是1x1x1的单位体积 % 生成地图数组 map zeros(mapSize(1), mapSize(2), mapSize(3)); % 手动添加几个障碍物块 map(20:40, 20:40, 1:30) 1; % 第一个障碍块 map(60:80, 50:70, 10:40) 1; % 第二个障碍块 map(30:50, 70:90, 20:40) 1; % 第三个障碍块这种三维数组的表示方法有一个突出的优点后续所有的判断都可以直接通过索引来完成不需要额外定义复杂的数据结构代码简洁运行效率也高。但缺点是内存开销会随着地图精度提高快速增长。比如100×100×50的地图double类型存储需要100×100×50×8字节约4MB看起来不大但如果分辨率提高到0.1数组维度变成1000×1000×500内存直接到4GB量级这就不太现实了。所以我的建议是栅格地图的分辨率不要设太高够用就行路径规划是离散化搜索不是CAD建模没必要追求微米级精度。2.2 随机障碍物生成与种子设置手动添加障碍块适合验证代码逻辑但要做算法性能测试随机障碍物地图更有说服力。这里我提供一个随机障碍物生成函数可以生成指定数量的长方体或球形障碍物。function map generateRandomMap(mapSize, numObstacles) map zeros(mapSize(1), mapSize(2), mapSize(3)); for i 1:numObstacles % 随机确定障碍物的中心位置 cx randi([5, mapSize(1)-5]); cy randi([5, mapSize(2)-5]); cz randi([5, mapSize(3)-5]); % 随机确定障碍物的尺寸 sx randi([5, 20]); sy randi([5, 20]); sz randi([5, 15]); % 计算障碍物范围 xmin max(1, cx - floor(sx/2)); xmax min(mapSize(1), cx floor(sx/2)); ymin max(1, cy - floor(sy/2)); ymax min(mapSize(2), cy floor(sy/2)); zmin max(1, cz - floor(sz/2)); zmax min(mapSize(3), cz floor(sz/2)); % 在地图上标记障碍物 map(xmin:xmax, ymin:ymax, zmin:zmax) 1; end end这里有几个细节需要注意。第一使用randi生成随机位置时我刻意让障碍物中心点距离边界至少5个栅格避免障碍物贴着地图边缘导致路径规划时没有回旋余地。第二随机障碍物很容易出现重叠重叠本身不是问题但如果障碍物把起点或终点覆盖了算法就会失败。所以生成地图之后一定要检查起点和终点对应的栅格值是否为0这是最基本的合法性检查。2.3 碰撞检测的两种实现方式碰撞检测是整个路径规划里被调用最频繁的函数它的效率直接影响算法整体耗时。我在这个项目里实现了两种碰撞检测方法对A*和RRT分别使用。对A*来说因为是栅格搜索碰撞检测其实就是检查节点的数组索引值是否为1代码一行就能搞定function isFree isFreeAStar(map, node) % node是[x, y, z]坐标检查对应栅格是否空闲 if any(node 1) || node(1) size(map,1) || node(2) size(map,2) || node(3) size(map,3) isFree false; return; end isFree (map(node(1), node(2), node(3)) 0); end对RRT来说就不一样了。RRT的采样点是连续坐标需要判断两个坐标点连成的线段是否穿过障碍物。不能只检查端点的栅格值因为线段可能从障碍物边缘穿插过去。我这里用了一个简洁实用的方法把线段离散成一系列小步长点然后逐点检查栅格占用情况。function isCollisionFree checkEdgeCollision(map, node1, node2) % 将线段离散为多个点进行检查 stepSize 0.5; % 检查步长必须小于栅格大小 dist norm(node1 - node2); numSteps ceil(dist / stepSize); isCollisionFree true; for i 0:numSteps t i / numSteps; point (1 - t) * node1 t * node2; idx ceil(point); if idx(1) 1 || idx(2) 1 || idx(3) 1 || ... idx(1) size(map,1) || idx(2) size(map,2) || idx(3) size(map,3) isCollisionFree false; break; end if map(idx(1), idx(2), idx(3)) 1 isCollisionFree false; break; end end end这里stepSize的选择很关键。如果步长大于1采样点可能“跳过”障碍物所在的栅格导致碰撞检测失效。我一般取0.5在计算效率和精度之间比较平衡。当然严格的做法是用射线追踪算法如Bresenham三维扩展版但对于大多数场景均匀离散检查已经足够了。3. A*算法的三维实现3.1 节点定义与启发式函数A*实现的第一步是定义节点数据结构。MATLAB里用struct数组最方便也可以直接用对象但struct的开销更小适合算法原型验证。我用的是struct数组每个节点包含坐标、g值、h值、f值、父节点索引这些字段。function path astar3D(map, start, goal) % 初始化开闭列表 openList struct(pos, [], g, [], h, [], f, [], parent, []); openList(1) struct(pos, start, g, 0, h, heuristic(start, goal), ... f, heuristic(start, goal), parent, 0); closedList struct(pos, [], g, [], h, [], f, [], parent, []); % 方向向量26邻域扩展 directions []; for dx -1:1 for dy -1:1 for dz -1:1 if ~(dx 0 dy 0 dz 0) directions(end1, :) [dx, dy, dz]; end end end end while ~isempty(openList) % 找到f值最小的节点 [~, idx] min([openList.f]); currentNode openList(idx); % 到达目标 if isequal(currentNode.pos, goal) path reconstructPath(currentNode, closedList); return; end % 从open列表移到closed列表 openList(idx) []; closedList(end1) currentNode; % 遍历所有邻域节点 for i 1:size(directions, 1) neighborPos currentNode.pos directions(i, :); % 检查是否在地图范围内且非障碍 if ~isFreeAStar(map, neighborPos) continue; end % 检查是否在closed列表中 if isNodeInList(closedList, neighborPos) continue; end % 计算g值直线邻域代价为1对角邻域代价为sqrt(3) if sum(abs(directions(i, :))) 1 moveCost 1; else moveCost sqrt(3); end tentativeG currentNode.g moveCost; % 检查是否在open列表中或者已有代价更大 [inOpen, openIdx] isNodeInList(openList, neighborPos); if ~inOpen % 新节点加入open列表 newNode struct(pos, neighborPos, ... g, tentativeG, ... h, heuristic(neighborPos, goal), ... f, tentativeG heuristic(neighborPos, goal), ... parent, numel(closedList)); openList(end1) newNode; elseif tentativeG openList(openIdx).g % 更新的路径代价更小 openList(openIdx).g tentativeG; openList(openIdx).f tentativeG openList(openIdx).h; openList(openIdx).parent numel(closedList); end end end % 找不到路径 path []; warning(A*: 未找到可行路径); end启发式函数的选择需要重点说明。在三维空间里我推荐使用欧几里得距离作为启发式因为26邻域扩展允许对角线移动欧几里得距离不会高估实际代价A*的最优性得到了保证。曼哈顿距离只在仅允许正交移动时才是可采纳的。function h heuristic(node, goal) h sqrt(sum((node - goal).^2)); end3.2 26邻域扩展与代价计算细节A*的三维扩展最核心的就是邻域从8个变成26个。在二维里对角线移动代价是√2在三维里空间对角线移动代价是√3。这个差别看起来很小但在大范围地图里会累积起来直接影响搜索质量。我在代码里区分了三种情况直线移动沿x、y、z单一方向移动代价为1平面对角线同时改变两个坐标轴比如(x1, y1, z)代价为√2空间对角线三个坐标轴同时改变比如(x1, y1, z1)代价为√3检查方向向量中非零元素的个数就能区分这三种情况nonzeroCount sum(directions(i, :) ~ 0); if nonzeroCount 1 moveCost 1; elseif nonzeroCount 2 moveCost sqrt(2); else moveCost sqrt(3); end这个细节是很多人忽略的。有人把所有移动的代价都设为1虽然也能算出路径但会严重破坏启发式的一致性导致搜索不再最优。如果地图不大还好地图一大你就会发现路径明显绕弯或者局部路径贴着障碍物走这都是代价函数不准确导致的。3.3 路径回溯与结果输出当open列表里的某个节点位置与终点重合时就说明找到了路径。这时候需要通过父节点索引逐级回溯把整条路径提取出来。我这里的reconstructPath函数是从目标节点开始通过parent索引往回找。function path reconstructPath(currentNode, closedList) path currentNode.pos; parentIdx currentNode.parent; while parentIdx 0 parentNode closedList(parentIdx); path [parentNode.pos; path]; parentIdx parentNode.parent; end end有个细节容易踩坑因为我在closedList里存的是节点的所有信息而parent存的是当前节点在closedList中的索引。这个索引是在节点加入closedList时分配的所以更新父节点时一定要小心不要用open列表的索引去closed列表里查否则回溯出来的路径会乱掉。另外MATLAB里数组动态增长的效率不高。当open列表和closed列表非常大的时候频繁的openList(end1) newNode和openList(idx) []会导致大量内存重分配。代码原型阶段可以这样写但如果要处理超大范围地图建议改用Java的PriorityQueue接口或者直接用C写MEX文件性能差距在几十倍以上。4. RRT与RRT*的三维实现4.1 RRT核心循环采样、找最近邻、扩展RRT的实现思路比A*简单直接。核心循环就三步随机采样一个点在树上找到离采样点最近的节点从最近节点向采样点方向扩展固定步长。如果新节点和最近节点之间的边没有碰撞就把新节点加入树中。重复这个过程直到新节点距离终点小于某个阈值。function [path, tree] rrt3D(map, start, goal, maxIter, stepSize, goalThreshold) % 初始化树 tree.nodes start; tree.parent 0; goalReached false; finalNodeIdx 0; for iter 1:maxIter % 概率目标偏置以10%的概率直接采样终点 if rand() 0.1 samplePoint goal; else samplePoint [randi(size(map,1)), randi(size(map,2)), randi(size(map,3))]; end % 找最近邻节点 [nearestIdx, nearestNode] findNearestNode(tree.nodes, samplePoint); % 向采样点方向扩展固定步长 newPoint nearestNode stepSize * (samplePoint - nearestNode) / norm(samplePoint - nearestNode); newPoint round(newPoint); % 检查新点是否可到达 if newPoint(1) 1 || newPoint(1) size(map,1) || ... newPoint(2) 1 || newPoint(2) size(map,2) || ... newPoint(3) 1 || newPoint(3) size(map,3) continue; end if ~isFreeAStar(map, newPoint) continue; end if ~checkEdgeCollision(map, nearestNode, newPoint) continue; end % 加入树中 tree.nodes(end1, :) newPoint; tree.parent(end1) nearestIdx; % 检查是否到达终点附近 if norm(newPoint - goal) goalThreshold goalReached true; finalNodeIdx numel(tree.parent); break; end end if ~goalReached path []; warning(RRT: 达到最大迭代次数未找到路径); return; end % 从最终节点回溯路径 path []; idx finalNodeIdx; while idx 0 path [tree.nodes(idx, :); path]; idx tree.parent(idx); end % 添加终点 path(end1, :) goal; end4.2 提高采样效率的三个实用技巧RRT的随机性既是优点也是缺点。实测下来有三个技巧能明显提高三维RRT的搜索效率第一是目标偏置。在采样时以一定概率通常是5%~15%直接采样终点而不是纯随机采样。这样做的目的是让树有意识地朝着目标方向生长而不是在无关区域浪费大量迭代。我试过纯随机的版本在100×100×50的地图里有时候迭代几千次都不一定能找到路径加10%的偏置之后通常几百次就能搞定。第二是搜索半径裁剪。在找最近邻节点时不需要遍历整颗树。如果树的规模很大可以做KD-Tree来加速最近邻搜索。MATLAB自带knnsearch函数可以直接用但需要先把树节点存成表格形式。我在这个项目里用的是暴力搜索因为树的节点数量一般不超过几千个暴力搜索的时间和knnsearch差距不大但代码简洁很多。第三是变步长策略。固定步长有一个问题如果步长太小树生长缓慢需要很多次迭代才能到达目标如果步长太大在障碍物密集区域又容易跳过可行空间。一个简单的改进是让步长根据当前距离目标的远近动态调整离目标远时大步长离目标近时小步长。这个策略在RRT里实现的代码改动很小收益却很明显。4.3 RRT*的rewire机制与实现改动RRT*和RRT的区别在于多了两个步骤选择父节点时不是只选最近邻节点而是搜索半径内所有候选节点选择使得路径代价最小的那个每次加入新节点后对搜索半径内的已有节点做重连接rewire如果路径经过新节点的代价更小就更新已有节点的父节点。function [path, tree] rrtStar3D(map, start, goal, maxIter, stepSize, goalThreshold, searchRadius) % 初始化树 tree.nodes start; tree.parent 0; for iter 1:maxIter % 采样 if rand() 0.1 samplePoint goal; else samplePoint [randi(size(map,1)), randi(size(map,2)), randi(size(map,3))]; end % 找最近邻 [nearestIdx, nearestNode] findNearestNode(tree.nodes, samplePoint); % 扩展 newPoint nearestNode stepSize * (samplePoint - nearestNode) / norm(samplePoint - nearestNode); newPoint round(newPoint); % 边界和碰撞检查 if ~isValidPoint(map, newPoint) continue; end if ~checkEdgeCollision(map, nearestNode, newPoint) continue; end % 在搜索半径内寻找所有候选父节点 candidateIdx find(sqrt(sum((tree.nodes - newPoint).^2, 2)) searchRadius); % 选择代价最小的父节点 minCost inf; bestParent nearestIdx; for i 1:numel(candidateIdx) idx candidateIdx(i); if checkEdgeCollision(map, tree.nodes(idx, :), newPoint) costToNew getNodeCost(tree, idx) norm(tree.nodes(idx, :) - newPoint); if costToNew minCost minCost costToNew; bestParent idx; end end end % 加入树 newIdx size(tree.nodes, 1) 1; tree.nodes(newIdx, :) newPoint; tree.parent(newIdx) bestParent; % Rewire检查搜索半径内的节点是否能通过新节点获得更小代价 for i 1:numel(candidateIdx) idx candidateIdx(i); if checkEdgeCollision(map, tree.nodes(idx, :), newPoint) newCost minCost norm(tree.nodes(idx, :) - newPoint); oldCost getNodeCost(tree, idx); if newCost oldCost tree.parent(idx) newIdx; end end end end % 找距离终点最近的节点并回溯 distToGoal sqrt(sum((tree.nodes - goal).^2, 2)); [minDist, goalIdx] min(distToGoal); if minDist goalThreshold path []; warning(RRT*: 未找到可行路径); return; end path []; idx goalIdx; while idx 0 path [tree.nodes(idx, :); path]; idx tree.parent(idx); end path(end1, :) goal; endRRT*的代价改进是渐进最优的也就是说迭代次数越多路径越接近最优。但代价是每次加入新节点都要做re-wire计算量比RRT大不少。对于三维地图searchRadius一般取步长的2~3倍太大的话re-wire的计算开销会爆炸太小的话又退化成普通RRT。4.4 路径平滑处理三次B样条无论A*还是RRT输出的路径本质上是一系列折线段。在三维空间里这种折线路径如果直接给无人机或机械臂执行会出现明显的抖动因为速度和加速度在拐点处不连续。我在这里加了一个简单的三次B样条平滑处理效果立竿见影。function smoothPath bsplineSmooth(path, numPoints) % 使用三次B样条对路径进行平滑 % path: Nx3的原始路径点 % numPoints: 采样点数量 k 4; % 三次B样条 t 0:numPoints-1; knots [zeros(1, k-1), linspace(0, 1, size(path,1)-1), ones(1, k-1)]; % 这里用简单的滑动平均来近似平滑 % 更精确的做法是使用bsplines工具箱 smoothPath path; for iter 1:5 for i 2:size(smoothPath, 1) - 1 smoothPath(i, :) (smoothPath(i-1, :) smoothPath(i, :) smoothPath(i1, :)) / 3; end end end严格来说滑动平均不是真正的B样条平滑它可能有收缩效应让路径偏向障碍物。工程上更可靠的做法是MATLAB的spcrv或者cscvn函数也可以自己写样条插值。这里我选择滑动平均是因为它代码简单、计算快而且对不追求极致平滑的场景已经够用了。如果你需要严格保证平滑后的路径不与障碍物碰撞那就需要用优化算法来做比如轨迹优化里的CORLConvex Optimization for Robot Locomotion方法。5. 可视化与结果对比5.1 三维地图与路径的绘图技巧MATLAB里可视化三维路径规划结果最常用的是plot3和scatter3组合。我会先生成地图的视觉表示把障碍物画成半透明的立方体或点云然后叠加路径曲线。function visualizePath(map, path, start, goal, titleStr) figure; hold on; % 画出障碍物位置 [x, y, z] ind2sub(size(map), find(map 1)); scatter3(x, y, z, 5, [0.5 0.5 0.5], filled, MarkerFaceAlpha, 0.3); % 画路径 if ~isempty(path) plot3(path(:,1), path(:,2), path(:,3), r-, LineWidth, 3); end % 标记起点和终点 scatter3(start(1), start(2), start(3), 100, g, filled); scatter3(goal(1), goal(2), goal(3), 100, b, filled); xlabel(X); ylabel(Y); zlabel(Z); grid on; axis equal; view(45, 30); % 设置3D视角 title(titleStr); end有几个可视化细节可以让你画的图更专业。第一个是视角选择view(45, 30)是默认的等轴测视角能看到三个维度但有时候view(135, 20)这种低角度俯视图更能看清路径的走向。第二个是透明度的设置障碍物点云用半透明可以直观展示障碍物范围同时看清路径从哪个位置穿过。第三个是渲染速度如果障碍物点很多用scatter3画全部点会非常慢可以先用unique去重或者用volshow看三维体数据不过后者需要Image Processing Toolbox支持。5.2 两种算法的路径长度与耗时对比我在地图大小为100×100×50、随机障碍物数量为20的条件下分别跑A*和RRT表里是一次典型的对比结果。指标A*RRTRRT*路径长度成功找到时约95约110~150波动大约100~120搜索时间3.2秒0.8秒8.6秒迭代次数不需要图搜索约500~1500约1500~5000最优性保证最优无保证渐进最优内存占用高存储open/closed列表低中等看到这个表你应该理解我前面说的“A适合已知地图小规模RRT适合大规模高维”是什么意思了。A在100×100×50的栅格地图里即使只用暴力线性搜索找open列表最小值耗时也在秒级因为需要遍历大量的节点。RRT只要几百次迭代就能找到一条可行路径速度快得多但代价是路径长度不可控有时候会绕一个大弯。RRT在路径质量上比RRT好但代价非常高昂。特别是在三维空间里每次加入新节点都要做半径搜索和re-wire计算量增长很快。我的建议是如果只是要找一条可行路径用RRT如果对路径质量有要求且地图规模不大用A或者RRT*。不要把RRT*当作“万能药”它在大地图里的收敛速度会让你的耐心全面崩盘。5.3 参数敏感性分析这里我想分享一组我实测的参数敏感性数据方便大家在调试时有一个起点A*方向启发式函数欧几里得距离最优不建议用曼哈顿距离会破坏最优性邻域定义26邻域效果最好但计算量是8邻域的3倍以上地图分辨率1是推荐的默认值太细会内存爆炸RRT/RRT*方向stepSize推荐设置为地图最大尺寸的5%~10%我这里是100×100×50的地图取5~10比较合适goalThreshold取stepSize的1.5倍左右太大会提前结束导致路径精度差太小会浪费迭代次数目标偏置概率10%左右效果最好。我试过5%、20%5%偏置搜索慢20%偏置容易陷入局部searchRadiusRRT*取stepSize的2~3倍这些参数的调整在代码里只需要改一个数但效果千差万别。我调试过程中最深刻的体会是参数不能孤立地看。stepSize调大了goalThreshold也要相应调大目标偏置概率调高了搜索半径也要调大。它们之间是联动的单独调一个反而容易出问题。6. 常见问题与排查技巧实录6.1 路径找不到时的三大原因我平时调试路径规划算法遇到“找不到路径”的概率相当高。分析下来原因不外乎三种第一是起点或终点落在了障碍物内部。这个最基础但也最容易忽视。随机生成障碍物时可能恰好覆盖了起点或终点栅格导致A*从第一步就卡死。解决办法是先检查map(start(1), start(2), start(3)) 0如果为1就重新生成地图或换个点。第二是地图太小、障碍物太密集实际上不存在可行路径。三维空间里好像比二维更“容易走通”其实不然。当障碍物从地面一直延伸到顶部或者多个障碍物形成密封结构路径就不存在了。这时A*会跑完全部可到达节点后返回空路径RRT则会一直迭代到超时。排查方法是用连通性分析MATLAB的bwlabeln函数先检查起点和终点是否在同一个连通区域。第三是步长太大跳过了狭窄通道。这是RRT特有的问题。当采样点和最近节点的连线被障碍物隔断时树无法在狭窄通道里生长。对策是缩小步长或者改用RRT的变种如RRT-Connect双向生长后者在窄通道场景下表现出色。6.2 A*性能瓶颈与优化方向A*最明显的性能瓶颈在找open列表中的最小f值节点。我在代码里用的[~, idx] min([openList.f])在open列表规模达到数千的时候还能接受但如果地图范围大open列表动不动上万这个线性扫描就会非常慢。优化方案有三个层次。第一层是用二叉堆或优先队列来维护open列表MATLAB里可以用java.util.PriorityQueue省去自己写堆的麻烦。第二层是引入权重启发式把f值变为g w*hw取1.5~2这样搜索会更快但不再保证最优适合实时性要求高的场景。第三层是做跳点搜索JPS直接把搜索空间从所有栅格压缩到关键跳点速度能提升一个数量级。我自己的经验是在MATLAB里做原型用优先级队列就够了这个改动简单高效收益最大。6.3 RRT随机性的复现与调试技巧RRT是随机算法同样的参数每次运行结果都不一样。调试的时候这个特性有时候比较烦人明明上次还能找到路径这次就跑不出来了。解决方法是固定随机数种子在运行前调用rng(42)或者任何你喜欢的种子值这样每次运行的结果就完全可复现了。% 设置随机种子 rng(42); % 运行RRT [path, tree] rrt3D(map, start, goal, 2000, 8, 12);团队协作或课堂作业中固定随机数种子是很有价值的调试习惯。它让“在我电脑上能跑在你电脑上跑不了”的问题有了一个追查的起点——逐行对比或者把种子值改成同一个再试。另一个实用技巧是记录树的生长过程。把每迭代100次的树节点画出来能直观地看到RRT的探索过程。如果树的节点密集在某一侧说明目标偏置概率太低如果树一直在一个区域打转说明可能被局部障碍物困住了需要调整步长或偏置概率。6.4 常见问题速查表现象可能原因解决方案A*运行超时地图太大open列表线性扫描用优先队列或引入权重启发式A*路径明显绕路启发式函数不满足一致性改为欧几里得距离且邻域包含对角移动RRT跑完迭代找不到路径目标偏置概率太低提高到10%以上RRT找到了路径但很长步长太大或迭代次数不够减小步长、增加迭代次数RRT*比RRT还慢甚至更差searchRadius太大re-wire计算量爆炸减小searchRadius到步长的2~3倍可视化时障碍物显示不出来数据是double类型find没找到为1的点检查map赋值是否正确建议用unique(map(:))平滑后路径穿障碍物滑动平均导致的收缩效应平滑后重新做碰撞检测或改用样条插值6.5 拓展基于采样的方法还能怎么玩写到这里忍不住再多说几句。RRT只是随机采样路径规划的入门往上还有RRT-Connect双向生长适合窄通道、Informed RRT*在找到初始解后限制采样区域为椭圆加速收敛、SST稳定稀疏RRT适合动力学系统。我建议掌握了基础RRT之后下一步去实现RRT-Connect它比RRT的代码改动不大但速度提升非常明显——在三维地图里双向生长的效率往往比单向RRT高一倍以上。另外提醒一点MATLAB自带的Robotics System Toolbox里也有路径规划相关的函数如plannerRRT如果你只是想快速验证算法效果可以直接用现成的工具包。但如果你跟我一样是打算把算法原理吃透那还是自己从零写一遍更值。自己写过一遍A*和RRT之后再看工具包里的参数设置你会非常清楚每个参数背后到底发生了什么。这个项目做完我自己最大的收获不是代码本身而是对“搜索算法”这件事的理解。A和RRT看似完全不同本质上都是在解决同一个问题如何在巨大的搜索空间里找到一条与障碍物无碰撞的通路。区别在于它们用了完全不同的策略去压缩搜索空间A用启发式引导RRT用随机采样。至于实际项目里怎么选我的建议是看三件事地图规模、是否需要最优路径、可接受的运行时间。把这三件事想清楚选型就顺理成章了。本文还有配套的精品资源点击获取