简介本资源是一套面向机器人导航、无人机路径规划等领域的MATLAB三维避障路径生成实现方案适用于具备基础编程与几何建模能力的本科生、研究生及算法工程师。资源聚焦三维空间下A与RRT两类主流路径规划算法的工程化落地涵盖障碍物建模含多边形与点云接口、空间栅格化、碰撞检测isIntersect等核心函数、路径扩展与后处理B样条平滑等关键环节。压缩包共31个文件以23个.m主程序文件为核心如addObstacles、intersectPolygons3d、expandOut等辅以5个.zbak备份脚本、1个README.md说明文档、1个.mat初始数据及1个.zip嵌套包总容量仅20KB轻量紧凑且模块划分清晰。已有72人学习下载提供完整可运行的三维路径规划闭环代码包含可视化绘图drawManifold等、障碍随机生成、路径可行性验证及典型场景测试用例便于快速理解算法逻辑、调试参数并拓展至实际系统应用。 很多人第一次把A算法从二维推到三维时第一反应都是把坐标从(x,y)改成(x,y,z)把四个方向改成六个方向不就完事了吗。真跑起来你就会发现问题远没有这么简单。三维栅格化的内存开销、26邻域的扩展逻辑、启发式函数的权重选择、随机采样算法的步长调参每一个环节都藏着坑。这篇文章就是把我用MATLAB实现三维A*和RRT避障路径生成的过程完整复盘一遍从环境建模、算法核心循环、碰撞检测到路径平滑和实测对比适合正在做无人机航线规划、机械臂路径生成、AUV水下航行或相关课题的学生和工程师参考。1. 为什么三维路径规划不能直接把二维算法“加一个Z轴”1.1 三维和二维的本质差异搜索空间从平面变成体积二维路径规划里的栅格地图是平面网格比如100x100就有一万个格子。到了三维100x100x100就是一百万个栅格。如果每一个栅格都存一个结构体光是内存就够呛。这是最直观的差异搜索空间从面积变成了体积算法的时空开销几乎是立方级增长的。邻域扩展方式也不一样。二维平面里一个格子最多有8个邻居上下左右加四个对角到了三维就变成26个邻居三个轴的组合包括6个面邻接、12个边邻接、8个顶点邻接。如果再考虑运动的连续性还有代价差异的问题面邻接移动一步代价为1边邻接移动一步代价为sqrt(2)顶点邻接移动一步代价是sqrt(3)。这部分直接决定了A*的g值计算方式。启发式函数的选择也变了。二维里常用的曼哈顿距离在三维中偏保守会导致搜索节点过多欧氏距离虽然直观但在栅格地图里与真实移动代价不匹配会引入额外的搜索开销。也就是说同样的A*算法换到三维不仅要改数据结构整个代价模型都要重新设计。1.2 A*与RRT的核心思路以及为什么这两者常被放在一起对比A和RRT是两种思想完全相反的算法。A是确定性搜索依赖栅格地图通过启发式函数引导搜索方向只要存在可行路径且权重设为1就一定能找到最优解。RRT是随机采样的概率完备算法它不需要栅格化直接在连续空间中生长搜索树通过在空间中随机撒点来探索环境速度快、内存占用小但不保证路径最优甚至不一定能在有限迭代内找到路径。两者常被放在一起对比因为它们正好代表了路径规划的两大流派一个是“精度优先”一个是“速度优先”。在实际工程中这两者不是二选一的关系而是可以结合使用先用RRT快速找到一条可行路径再用A思路做局部优化或者先用A在低分辨率栅格上找大方向再在局部用RRT做精细避障。我在后面的章节会分别说清楚各自的三维实现细节最后放一组实测对比。2. 环境建模三维栅格地图与碰撞检测函数2.1 用MATLAB构建三维栅格地图三维路径规划的第一步是把环境表示成算法能算的形式。对A*来说环境是离散栅格地图对RRT来说环境是连续空间加上碰撞检测函数。这两者都要先建立后面才能共用。我习惯用逻辑数组存三维栅格地图% 构建三维栅格地图mapSize对应x,y,z三个方向的栅格数量 mapSize [100, 100, 100]; map false(mapSize); % false表示空闲true表示障碍物 % 添加一个球体障碍物 center [50, 50, 50]; radius 10; [X, Y, Z] ndgrid(1:mapSize(1), 1:mapSize(2), 1:mapSize(3)); dist sqrt((X - center(1)).^2 (Y - center(2)).^2 (Z - center(3)).^2); map(dist radius) true; % 添加一个长方体障碍物 map(20:40, 70:90, 30:60) true;这里有几个关键点用ndgrid生成坐标网格然后向量化计算距离最后用逻辑索引赋值。新手容易用三重循环逐个格子判断在100x100x100规模下三重循环就是一百万次迭代MATLAB循环效率低跑起来会很痛苦。向量化之后同样的操作几乎是瞬间完成。2.2 连续空间碰撞检测让RRT跑在真正的三维空间RRT算法不需要把整个空间栅格化但它需要判断两点之间的线段是否会穿过障碍物。这个功能要单独写一个isCollision函数它的输入是起点、终点、障碍物列表输出是布尔值。function hit isCollision(p1, p2, obstacles) % 沿线段采样采样分辨率决定了碰撞检测精度 dist norm(p2 - p1); numSamples max(1, ceil(dist / 1.0)); hit false; for i 0:numSamples t i / numSamples; pt p1 t * (p2 - p1); for k 1:numel(obstacles) if obstacles(k).f(pt) hit true; return; end end end end采样分辨率是碰撞检测精度的关键。我都是按1个单位距离采一个点如果步长是5个单位那每段路径至少会采样5次这样球体障碍物即使很小也能被检测到。如果你把采样间隔设得太大可能一条线段“跳过”了障碍物的尖端导致路径看起来没碰障碍物实机执行时却撞上去了。2.3 障碍物建模方盒、球体与混合场景碰撞检测函数依赖障碍物的距离判断。我用结构体数组保存障碍物每个障碍物有一个匿名函数句柄f输入一个点坐标输出是否在障碍物内部。这样的好处是add obstacle时根本不需要改碰撞检测主函数扩展性很好。obstacles struct(f, {}); % 球体障碍物 obstacles(end1).f (pt) norm(pt - [50, 50, 50].) 10; % 立方体障碍物AABB包围盒 obstacles(end1).f (pt) all(pt [20, 70, 30].) all(pt [40, 90, 60].);注意我传进来的pt是列向量而RRT中树的节点是行向量两者差一个转置这个细节很容易忽略一旦处理不对碰撞检测永远判定无障碍物或者永远判定有障碍物。实际调试时如果不确定建议先把一个已知会碰撞的线段丢进去测试。3. 三维A*从栅格搜索到路径回溯3.1 状态表示与26邻域扩展三维A*的节点状态是三个整数坐标(x_idx, y_idx, z_idx)。我把每个节点编码成线性索引这样在MATLAB里可以用一维数组存g值、f值和父节点比用三维数组更好做节点遍历。邻域扩展是三维A和二维A最大的区别。二维用8邻域三维推荐直接用26邻域否则路径会错过很多可行的斜向移动。生成邻域偏移的代码如下dirs []; for dx -1:1 for dy -1:1 for dz -1:1 if dx 0 dy 0 dz 0 continue; end dirs(end1, :) [dx, dy, dz]; %#okSAGROW end end end % dirs的行数就是26每一行是一个偏移量在计算移动代价时判断三个坐标里有几个非零就是几维移动。一个非零代价是1两个非零是sqrt(2)三个非零是sqrt(3)。这个代价模型与栅格空间中的真实欧氏距离一致邻居之间不互相穿模。3.2 启发式函数欧氏距离、曼哈顿距离与octile距离的选择启发式函数直接决定A*的搜索效率。二维中常用曼哈顿距离因为它与四邻域移动的代价模型匹配。三维中如果也用曼哈顿距离估计出的代价往往远低于真实代价导致搜索范围变得很大节点数爆炸。欧氏距离是最贴近真实的三维距离度量但它在栅格地图中有一个问题它偏向于高估实际移动代价让A*更快收敛到目标却可能放弃真正的最优路径。这不是说欧氏距离不好而是当你做工程时需要在最优性和实时性之间权衡。实际使用中我推荐octile距离也就是三维切比雪夫距离的变体function h octileDistance(p1, p2) dx abs(p1(1) - p2(1)); dy abs(p1(2) - p2(2)); dz abs(p1(3) - p2(3)); m max([dx, dy, dz]); s dx dy dz; h (sqrt(3) - sqrt(2)) * m (sqrt(2) - 1) * s; end这个公式的原理是先走代价最小的方向把三维移动分解成多个对角和直线运动尽可能接近真实代价。实测下来它在三维栅格中比曼哈顿和纯欧氏距离都快而且在权重设为1时仍然保持最优性。3.3 核心数据结构与MATLAB实现细节三维A*的核心数据结构有五个open列表、closed标志、g值数组、f值数组、父节点数组。% 地图尺寸 gridX 100; gridY 100; gridZ 100; % 父节点数组初始化为0表示无父节点 parent zeros(gridX, gridY, gridZ); % g值和f值初始化为inf gScore inf(gridX, gridY, gridZ); fScore inf(gridX, gridY, gridZ); % 起点和目标的线性索引 startIdx sub2ind([gridX, gridY, gridZ], sx, sy, sz); goalIdx sub2ind([gridX, gridY, gridZ], gx, gy, gz);open列表我用的是数组存节点线性索引。最简单的pop方法是线性扫描找f值最小的节点。这种实现在50x50x50的地图上够用在100x100x100的地图上就明显吃力了。如果你想跑大图我建议自己写一个最小堆类。MATLAB的classdef虽然写起来比Python啰嗦但实现一个基本的二叉堆只需几十行。核心接口就三个insert、popMin、isEmpty。这里给出一个简化的最小堆类框架classdef MinHeap handle properties data []; fval []; end methods function obj MinHeap() end function insert(obj, val, f) obj.fval(end1) f; obj.data(end1) val; idx numel(obj.fval); while idx 1 obj.fval(idx) obj.fval(floor(idx/2)) tmp obj.fval(idx); obj.fval(idx) obj.fval(floor(idx/2)); obj.fval(floor(idx/2)) tmp; tmp obj.data(idx); obj.data(idx) obj.data(floor(idx/2)); obj.data(floor(idx/2)) tmp; idx floor(idx/2); end end function [val, f] popMin(obj) val obj.data(1); f obj.fval(1); obj.data(1) obj.data(end); obj.fval(1) obj.fval(end); obj.data(end) []; obj.fval(end) []; idx 1; while true left idx*2; right idx*21; smallest idx; if left numel(obj.fval) obj.fval(left) obj.fval(smallest), smallest left; end if right numel(obj.fval) obj.fval(right) obj.fval(smallest), smallest right; end if smallest idx, break; end tmp obj.fval(idx); obj.fval(idx) obj.fval(smallest); obj.fval(smallest) tmp; tmp obj.data(idx); obj.data(idx) obj.data(smallest); obj.data(smallest) tmp; idx smallest; end end function flag isEmpty(obj) flag isempty(obj.fval); end end end这段代码不算长但能撑起100x100x100以内的三维A*搜索。如果你只是做作业或小规模测试用线性扫描就够了不建议一开始就把堆的复杂度引入进来。3.4 A*的一个性能瓶颈内存与节点管理三维A*最大的坑是内存爆炸。100x100x100的地图父节点用zeros数组每个元素是double类型一个维度就是100万个double占8MB三个数组加起来24MB左右看起来不大但如果你用cell数组或结构体数组存节点信息比如每个节点存一个struct包含坐标、g值、f值、父节点索引那100万个struct的内存开销会迅速膨胀到几百MBMATLAB直接卡死。所以我的经验是能拼数组就不要用cell能整型就不要用double。父节点数组如果用uint32类型内存直接减半。如果地图是50x50x50这种规模用uint32就够了。如果你要跑200x200x200的图建议先重新审视需求因为三维A*这个算法本身就不适合超大尺度地图。关于邻域扩展还有一个细节检查邻居是否越界时用线性索引和sub2ind来回转换会很慢。我建议在扩展26个邻居时把三个坐标拆出来单独算越界判断用if nx 1 nx gridX ...这样的形式虽然看着啰嗦但比sub2ind快一个量级。实测在100x100x100地图上这个细节能节省总时间的30%以上。4. 三维RRT随机采样、最近邻扩展与双向加速4.1 RRT的核心循环采样、最近邻、步进、碰撞检测RRT的算法流程比A*简单得多核心循环就四步随机采样一个目标点从当前树中找到最近的节点从最近节点向采样点步进一个固定距离检查新路径是否碰撞没碰撞就加入树。stepSize 5; maxIter 8000; tree startPoint; % Nx3矩阵保存节点坐标 parent 0; % 父节点索引数组 for i 1:maxIter % 1. 采样有时直接采样目标点引导树朝目标生长 if rand 0.1 sample goalPoint; else sample [rand*100, rand*100, rand*100]; end % 2. 最近邻 diff tree - sample; distSq sum(diff.^2, 2); [~, idx] min(distSq); nearest tree(idx, :); % 3. 步进 delta sample - nearest; len norm(delta); if len stepSize newPoint sample; else newPoint nearest delta / len * stepSize; end % 4. 碰撞检测 if ~isCollision(nearest, newPoint, obstacles) tree [tree; newPoint]; %#okAGROW parent [parent; idx]; %#okAGROW if norm(newPoint - goalPoint) stepSize break; end end end这段代码最需要注意的是动态数组扩展问题。tree [tree; newPoint]在迭代里不断改变数组长度如果迭代几千次MATLAB需要反复重新分配内存速度会很慢。更稳妥的做法是预分配一个大矩阵tree zeros(maxIter 1, 3); tree(1, :) startPoint; nodeCount 1; % 循环中 nodeCount nodeCount 1; tree(nodeCount, :) newPoint;最后截断tree(1:nodeCount, :)即可。这个优化看起来不起眼但RRT迭代到一万次时速度差距可能接近五倍。4.2 碰撞检测和步长设置碰撞检测的精度和步长息息相关。步长太长路径会穿过障碍物步长太短树生长缓慢迭代次数暴增。我常用的经验公式是步长取地图尺寸的1/50到1/100。比如100x100x100的空间stepSize取2到5都合理。障碍物尺寸越密集步长越要小一些。碰撞检测函数里有一个隐藏的坑它检测的是“线段是否穿过障碍物”而不是“点是否在障碍物内”。如果步长是10而球体障碍物半径是3从障碍物一侧跳到了另一侧两个端点和中间采样点都落在障碍物外面但实际上穿过去了。所以采样分辨率必须足够细至少要小于障碍物最小特征尺寸的1/2。这也是我把采样间隔固定为1个单位的原因。4.3 双向RRT的原理与实现双向RRT的思想很好理解同时从起点和目标点生长两棵树每次各自向外扩展当两棵树距离小于stepSize时认为找到了路径。这个策略在开阔场景下能让搜索速度翻好几倍。treeA startPoint; parentA 0; treeB goalPoint; parentB 0; nodeCountA 1; nodeCountB 1; success false; for i 1:maxIter % 扩展A树 [newA, idxA] extendRRT(treeA, goalPoint, stepSize, obstacles); if ~isempty(newA) nodeCountA nodeCountA 1; treeA(nodeCountA, :) newA; parentA(nodeCountA) idxA; % 扩展B树目标点设为newA [newB, idxB] extendRRT(treeB, newA, stepSize, obstacles); if ~isempty(newB) nodeCountB nodeCountB 1; treeB(nodeCountB, :) newB; parentB(nodeCountB) idxB; if norm(newB - newA) stepSize success true; break; end end end % 交换两棵树保持扩展平衡 % 注意交换时treeA和treeB、parentA和parentB要一起换 end实现双向RRT时要特别小心一个细节两棵树的节点数量可能不均衡如果一直让A树扩展而B树被动接受速度优势就没了。我一般每扩展A树一次后如果nodeCountA nodeCountB 5就交换指针让两棵树规模保持接近。这个平衡策略对收敛速度影响很大。4.4 让RRT在窄通道中更快目标偏置与采样策略RRT最大的弱点是窄通道问题。想象一个狭长的走廊随机采样点落进走廊的概率很低树的生长就会非常慢。最常见的改进是目标偏置每次采样有5%到10%的概率直接使用目标点而不是随机点这样树会倾向于朝目标方向生长。但这还不够。在窄通道场景里我建议把目标偏置提高到15%到20%并且配合双向RRT使用。双向RRT天然缓解窄通道问题因为两棵树同时从两端长在通道中间相遇的概率远大于一棵树从一端生长到另一端。另一个技巧是在使用RRT时不要一开始就追求最优路径而是先快速找到一条可行路径再用第五章的路径平滑策略处理。RRT本身是概率完备的在复杂环境中可能需要多次运行才能找到一条可行路径我习惯跑5次、取路径最短的那条这样比单次跑更长的迭代时间更划算。5. 路径后处理从折线路径到可用的平滑轨迹5.1 为什么要做路径平滑无论是A还是RRT直接输出的路径都是一系列折线段。A的路径贴着栅格的边和对角线走RRT的路径更夸张充满了锯齿状转向。这种路径如果直接交给无人机或机械臂执行飞机会在折点处急剧减速、转向严重时无法跟踪。路径后处理通常分两步先去冗余点再平滑插值。去冗余点的思路是贪心算法从起点开始尝试连接尽可能远的节点如果两点之间没有碰撞就跳过中间所有节点如有碰撞就回退到上一个安全节点。5.2 去冗余节点与B样条平滑我写过这样一个简化函数输入RRT/A*的路径点序列输出去除冗余点后的关键点序列。function simplifiedPath simplifyPath(path, obstacles) simplifiedPath path(1, :); i 1; while norm(simplifiedPath(end, :) - path(end, :)) 1e-6 % 从最远的节点开始尝试找到一个能直线到达的节点 found false; for j size(path, 1):-1:i if ~isCollision(simplifiedPath(end, :), path(j, :), obstacles) simplifiedPath(end1, :) path(j, :); %#okAGROW i j; found true; break; end end if ~found break; end end end注意这里有个细节循环里要判断path(j,:)是否已经在简化路径里否则可能死循环。我在代码里用norm(simplifiedPath(end,:) - path(end,:)) 1e-6做终止条件思路是简化路径的最后一个点只要还没到达原路径终点就继续找。去冗余之后用三次样条对x、y、z分别插值t 1:size(simplifiedPath, 1); tt linspace(1, size(simplifiedPath, 1), 200); sx spline(t, simplifiedPath(:, 1), tt); sy spline(t, simplifiedPath(:, 2), tt); sz spline(t, simplifiedPath(:, 3), tt); smoothPath [sx(:), sy(:), sz(:)];三次样条的优点是经过所有控制点而且一阶导数连续轨迹光滑。但直接对RRT原始路径做样条插值会overshoot生成远离原路径的曲线极有可能穿过障碍物所以必须先做去冗余再做样条。5.3 平滑后的碰撞校验平滑后必须重新做碰撞检测。我用的是一个笨办法但很可靠把平滑后的轨迹每隔0.5个单位采一个点逐个检查是否在障碍物内。如果发现碰撞说明控制点还是太少或者路径太曲折需要增加控制点或改用B样条。如果出现平滑后穿障碍物一个非常实用的处理是把平滑轨迹穿过障碍物的那段区间找出来把附近的控制点重新拉回原来路径的位置然后重新平滑。这本质上是一个“局部修复”的思路比全盘重新规划快得多。6. A*与RRT实测对比时间、内存、路径质量与选型建议6.1 同一场景下的对比测试我拿一个100x100x100的已知障碍物场景做了对比测试。场景中有三个球体障碍物和一个长方体障碍物起点在(5,5,5)目标在(95,95,95)。A*使用octile距离、26邻域、最小堆open列表RRT使用步长5、目标偏置0.1、最大迭代8000次。结果如下指标A*RRT双向RRT规划时间约2.8秒约0.15秒约0.08秒路径长度约152约198约172内存占用高三个百万级数组低只存节点坐标低两棵树最优性最优不一定不保证路径形态栅格折线较规整锯齿状需后处理锯齿状需后处理路径长度是简化之后的值。RRT的路径经过贪婪简化后缩短了大约15%但仍然比A长20%左右。如果你把RRT多跑几次取最短的那条差距会缩小但大概率还是比A长。6.2 什么场景选A*、什么场景选RRT这是我做项目时比较主观但实用的选型标准供你参考如果地图尺寸不大50x50x50以下且需要保证路径最短直接选A*。比如室内移动机器人在小范围迷宫中的路径规划A*的确定性是最大优势。如果地图很大100x100x100以上且对实时性要求高选RRT或双向RRT。比如无人机在城市环境中快速重新规划航线RRT的速度优势是压倒性的。如果场景中有大量窄通道比如机械臂穿过管道或货架间隙双向RRT比A更合适因为RRT在连续空间中的节点分布更均匀A在离散栅格中反而容易把窄通道漏掉。还有一种组合思路先用RRT快速生成一条可行路径找到路径上的关键转折点然后在这些关键点附近用A做局部优化。我在一个项目里用过这个策略规划时间基本等于RRT的时间而路径长度能接近A的水平。6.3 参数调优与避坑经验最后分享几个我在实际调试中踩过的坑和优化经验。第一个是A的open列表线性扫描问题。很多人直接在open列表里用[~, idx] min(fScore)找最小节点这在节点数几千时没问题但三维A很容易出现几万个open节点线性扫描的时间会拖垮整个搜索。如果你写了一次完整的三维A*建议直接上最小堆省得后面返工。第二个是RRT的步长和碰撞检测的一致性。我曾经把步长设成8但碰撞检测的采样间隔是1.5结果树的一根树枝“跳过”了一个半径3的球体障碍物。后来我把采样间隔改成不大于步长的1/3就再没出过这类问题。第三个是关于运行环境的提示。如果你在虚拟机上跑MATLAB三维栅格地图的A*会有明显卡顿因为内存分配和数据搬运在虚拟化环境里开销更大。我建议在原生系统或性能足够的Linux环境下跑大尺寸地图规划时间和内存稳定性都会好不少。还有一个细节是关于RRT树的预分配。我刚才提到了预分配矩阵但别忘了parent数组也要同步预分配。如果你只给tree预分配了parent还是动态增长照样会拖慢速度。路径平滑时也容易踩坑。有一次我直接对RRT原始路径做三次样条结果生成的轨迹在球体障碍物边缘绕了一个大圈看起来还在障碍物外面但无人机的翼展比较大实际执行时蹭到障碍物了。从那以后我每次平滑完都会做一遍全轨迹的碰撞检测并且把安全距离要求直接加在碰撞检测函数里把障碍物半径膨胀20%再检测。做MATLAB三维路径规划这个需求说难不难说简单也不简单。A*和RRT各自的坑我都替你踩过一遍了照着上面的代码和参数调应该能少走很多弯路。如果你在实机测试中碰到规划路径没问题但跟踪效果差的情况多半要回头检查路径平滑那段逻辑而不是怀疑规划算法本身。本文还有配套的精品资源点击获取