Matlab实现三维路径规划：栅格地图与A*算法实战

简介：三维路径规划是机器人学、无人机自主导航与自动驾驶中的关键技术，这份资源聚焦基于蚁群算法（ACO）的三维路径规划问题，提供一套可直接运行的MATLAB实现，面向需要学习智能优化算法、完成毕设课设或参与机器人竞赛的初学者和工程技术人员。资源包共8个文件，含7个.m脚本与1个.mat高度图数据文件；代码按功能模块化设计，覆盖环境与起终点初始化、信息素矩阵构建、蚂蚁路径搜索、适应度计算、障碍规避判断及迭代更新等关键环节，压缩包仅6KB，结构紧凑，便于逐模块理解与修改调试。已有187人学习参考。通过运行主程序即可观察三维空间中路径随迭代收敛的动态过程，调整蚂蚁数量、信息素蒸发率、启发因子等参数可直观感受算法效果；利用附带数据文件还能快速替换环境地图，将方法迁移至无人机航迹规划、仓储机器人避障等现实场景，是理解蚁群算法三维路径规划编程实现的实用入门资源。

1. 三维路径规划不是二维加一个 z 轴

无人机在城市楼宇间做巡检航线规划时，问题往往不是“走哪条路最短”，而是“这条路是否存在于三维空间里”。二维路径规划把世界压成一张高度图，机器只能在 x-y 平面绕行；三维里一个平面上被围死的区域，从 z 方向却可能直接翻过去。第三维带来的是搜索空间的数量级膨胀：二维栅格 500×500 有 25 万个节点，加 100 层高度后变成 2500 万个节点，A*、RRT、蚁群这些经典算法从教学案例变成真正考验数据结构和内存管理的工程问题。选择 Matlab 做算法原型，是因为三维地图天然就是 m×n×p 矩阵，障碍判断只是一次数组下标访问，可视化又能用 surf、plot3 配视角旋转观察路径与障碍的贴合关系。下文以栅格地图为主线，把三维路径规划从算法选型、Matlab 实现、参数调优到批量验证的链路拆开讲，所有代码都按能直接复制运行的标准写。

2. 三维路径规划算法选型：地图结构决定算法上限

2.1 栅格法、几何构型法与拓扑法怎么选

在动手写规划函数之前，先回答“这片空间在代码里长什么样”。常见选择有三类。几何构型法用球体、圆柱、长方体包络障碍物，碰撞检测直接算距离，适合障碍数量少、形状规则的厂房环境；栅格法把空间离散化成等尺寸体素，地图就是一个三维逻辑数组，检查障碍就是map(ix,iy,iz)==true，实现最直接；拓扑法把自由空间压缩成图，适合走廊、管道这类结构化场景，但抽象图本身的构建要额外设计。

做算法原型验证一般建议先上栅格法。第一，算法迁移成本低，二维 A* 的邻域概念直接推广到三维 26 邻域；第二，避障判断不涉及浮点几何计算，不存在边界误判；第三，和激光点云、深度图这类传感器输出天然接近，点云经过体素化后能得到占据栅格，深度图用相机内参投影到世界坐标网格也能生成同类地图。代价是内存随分辨率立方增长，地图超过千万格子就必须考虑稀疏表示或八叉树，这一点在 4.1 节会具体算账。

2.2 RRT、A*、群体智能算法的适用边界对比

A* 的强项是可复现、有最优性保证，代价是搜索范围大。RRT/RRT* 适合连续高维空间，机械臂 6 自由度甚至 14 自由度的规划经常用它，但在三维栅格上反而不划算：栅格本来就把空间离散好了，A* 能直接利用这个离散结构，RRT 反而慢在每次采样都要查询碰撞状态，而且路径不稳定。蚁群、粒子群这类群体方法适合让路径长度、能耗、威胁度一起进入目标函数的情况，代价是每次结果不一样，超参数要对具体地图标定，验证结论时说服力较弱。

做算法对比实验时我一般这样做：同一张 60×60×30 地图分别跑 A* 和 RRT*，记录平均规划时间和路径长度。A* 在网格规模不大且环境静态时路径最短且时间方差极小；RRT* 采样数足够时路径接近最优，但 10 次运行可能拿到 10 条不同轨迹。这个差异在需要确定性输出的落地点上很难接受，所以静态栅格场景我最终都回到 A*。

2.3 三维 A* 的代价函数与启发式函数设置

A* 在三维栅格上的公式仍是 f(n)=g(n)+h(n)。g(n) 是起点到当前节点的累积移动代价，h(n) 是当前节点到目标点的启发式估计。三维 26 邻域里移动分三种：轴向移动代价 1，面对角线 sqrt(2)，体对角线 sqrt(3)。启发式直接采用欧几里得距离 h(n)=sqrt((xg-xn)²+(yg-yn)²+(zg-zn)²)，它满足一致性条件，保证第一次从 openList 弹出目标节点时得到的就是最优路径。

两个常见误用值得提醒。一个把 h(n) 设成曼哈顿距离 |dx|+|dy|+|dz|，在三维 26 邻域里会显著高估真实代价，破坏可采纳性，路径不再是最优；另一个贪省事把 h 设成 0，等价于退化成 Dijkstra，搜索铺满大片无意义区域，高分辨率三维地图上内存先撑不住。

代价表必须和扩展方向一一对应。二维 8 邻域推广到三维时，如果只做轴向 6 扩展，路径绕远、拐点变多；用了 26 邻域但把代价统一设为 1，则系统性低估斜穿距离，路径偏向斜向走。这个细节直接影响规划出的轨迹能不能被真实载体执行，也是二维代码改三维时最常见的隐性 bug。

3. 用 Matlab 搭建三维栅格地图并跑通 A* 最小实现

3.1 用三维逻辑数组生成占据栅格地图

% 地图尺寸 60×60×30，false 代表自由空间 map = false(60, 60, 30); % 三个长方体障碍物：x 方向、y 方向、z 方向 map(20:35, 15:20, 5:20) = true; map(10:15, 30:50, 8:25) = true; map(40:55, 40:45, 3:18) = true; % 球体障碍：距球心距离小于 8 的体素全部占据 [x, y, z] = ndgrid(1:60, 1:60, 1:30); dist = sqrt((x-18).^2 + (y-45).^2 + (z-15).^2); map(dist < 8) = true;

这段代码生成一个三维逻辑地图，true 代表该体素被障碍物占据，false 是自由空间。三维数组三个维度按 x、y、z 理解，A* 搜索时用map(ix,iy,iz)一次下标访问就能完成碰撞检测。ndgrid生成的网格坐标顺序和数组维度严格一致，避免meshgrid在三维下 x、y 互换的经典坑。

% 用散点画全部障碍体素 [fx, fy, fz] = ind2sub(size(map), find(map)); plot3(fx, fy, fz, 'k.', 'MarkerSize', 1); axis equal; view(3); grid on;

find返回所有 true 体素的线性索引，ind2sub还原成三维坐标。体素数量超过 20 万时散点图会明显掉帧，可以先对 map 隔点抽样再画，做一个临时图map(1:2:end,1:2:end,1:2:end)来观察，障碍物整体形态已经足够清楚。

3.2 26 邻域偏移与移动代价预生成

% 预生成 26 个邻域偏移及对应移动代价 offsets = zeros(26, 3); costs = zeros(26, 1); k = 1; for dx = -1:1 for dy = -1:1 for dz = -1:1 if dx == 0 && dy == 0 && dz == 0 continue; end offsets(k, :) = [dx, dy, dz]; costs(k) = norm([dx, dy, dz]); k = k + 1; end end end

生成顺序不影响搜索结果，26 行偏移里只有三种代价：轴向 1、面对角线 sqrt(2)、体对角线 sqrt(3)。把代价计算封装在初始化阶段，主循环里直接查表，省掉每扩展一个节点就调一次norm的开销。如果地图各方向分辨率不同，比如 x 方向 0.5m、z 方向 1m，这套均一代价就不成立，需要先把三个方向缩放到同一栅格尺度或者改成加权欧几里得代价。

3.3 三维 A* 主循环：openList 的两种组织方式

function path = astar3d(map, start, goal) % 三维 A* 路径规划（26 邻域） % map : m×n×p 逻辑数组，true 表示障碍 % start : 1×3 整数网格坐标 % goal : 1×3 整数网格坐标 [m, n, p] = size(map); if map(start(1), start(2), start(3)) || map(goal(1), goal(2), goal(3)) path = []; return; end % 26 邻域偏移与代价预生成 offsets = zeros(26, 3); costs = zeros(26, 1); k = 1; for dx = -1:1 for dy = -1:1 for dz = -1:1 if dx == 0 && dy == 0 && dz == 0 continue; end offsets(k, :) = [dx, dy, dz]; costs(k) = norm([dx, dy, dz]); k = k + 1; end end end gScore = inf(m, n, p); fScore = inf(m, n, p); gScore(start(1), start(2), start(3)) = 0; fScore(start(1), start(2), start(3)) = norm(start - goal); parent = cell(m, n, p); % openList 每行: [x, y, z, f] openList = [start, fScore(start(1), start(2), start(3))]; while ~isempty(openList) [~, idx] = min(openList(:, 4)); % 取 f 最小的节点 current = openList(idx, 1:3); if isequal(current, goal) break; end openList(idx, :) = []; % 弹出 for k = 1:26 nb = current + offsets(k, :); % 先边界再障碍，标量比较比数组访问便宜 if any(nb < 1) || nb(1) > m || nb(2) > n || nb(3) > p continue; end if map(nb(1), nb(2), nb(3)) continue; end tentativeG = gScore(current(1), current(2), current(3)) + costs(k); if tentativeG < gScore(nb(1), nb(2), nb(3)) gScore(nb(1), nb(2), nb(3)) = tentativeG; fScore(nb(1), nb(2), nb(3)) = tentativeG + norm(nb - goal); parent{nb(1), nb(2), nb(3)} = current; openList(end + 1, :) = [nb, fScore(nb(1), nb(2), nb(3))]; end end end % 回溯路径 if isequal(current, goal) path = goal; cur = goal; while ~isequal(cur, start) prev = parent{cur(1), cur(2), cur(3)}; if isempty(prev) path = []; return; end path = [prev; path]; cur = prev; end else path = []; % openList 耗尽，目标不可达 end end

几个实现要点。openList用普通矩阵存，每轮min和删行都是 O(n)，地图规模 50×50×50 以下足够用；继续加分辨率就会明显卡顿，这时把数据结构换成二叉堆或java.util.PriorityQueue，插入和取最小降到 O(log n)，是三维 A* 提速收益最大的一处改动。同一坐标可能在 openList 里留有旧记录，不影响正确性，因为gScore只在新代价更小时才写入，旧记录即使被弹出也过不了tentativeG < gScore这层判断。parent用 cell 数组存三维坐标，路径长度上万时回溯阶段[prev; path]的头部插入开销也会累计，改成收集后统一翻转即可。

提示：current一定会在循环里被赋值，因为起点合法时 openList 首行必然存在；若起点终点重合，循环第一次迭代就 break 并返回单点路径，行为正确。

3.4 parent 回溯与可行性检查顺序

回溯逻辑放在主循环外面单独写，和搜索过程解耦，出错时可以在命令行单独调试。parent为空只有一种情况：目标节点从未被任何邻域更新过，也就是实际不可达。路径规划里“不可达”和“没找到”是两回事，前者要回到地图层检查障碍是否把空间完全割裂，后者才需要怀疑算法实现。

可行性检查顺序我固定为：先查起点终点是否在障碍内，再查边界，最后查障碍。边界检查是三次标量比较，障碍检查是数组访问，把便宜的判断放前面能少做很多无效内存访问。跑通后第一个验证动作是画图：把plot3(path(:,1), path(:,2), path(:,3), 'r-', 'LineWidth', 2)叠加到障碍散点图上，旋转视角从上下左右四个方向分别确认路径没有从障碍物中间穿过——三维地图视觉遮挡严重，只从一个视角看很容易漏判。

4. 三维路径规划算法的三个必调参数与平滑后处理

4.1 栅格分辨率与膨胀半径：地图层的两个数字

分辨率先于算法调整。分辨率粗，窄缝直接消失，规划出的路径看着短，物理上根本过不去；分辨率细，内存和规划时间按立方增长。一个可操作的做法是先量载体最小通过尺寸 d_min，分辨率至少取 d_min/4，比如无人机直径 0.5m，栅格边长不要超过 0.125m；再用膨胀操作给障碍物留出安全距离：

se = ones(3, 3, 3); map = imdilate(map, se); % 膨胀一圈 map = imdilate(map, se); % 再膨胀一圈，共两圈安全余量

imdilate配合 3×3×3 结构元相当于把每个障碍体素向 26 邻域各扩一格。膨胀两圈后，规划出的路径距离障碍物至少有两个栅格的间隙，这个间隙要大于载体半径才算合理。容易被忽略的是膨胀之后重新检查起点和终点，两者如果落进膨胀区域，A* 会直接返回空路径，看起来像算法 bug，实际是地图预处理问题。

内存有个快速估算方法：逻辑地图每格占 1 bit，double 类型的 gScore 每格 8 字节。60×60×30 的地图 gScore 约 1 MB，无所谓；到 500×500×100，double 类型的 gScore 是 200 MB，再加 fScore 和 parent 的 cell 开销，内存压力立刻明显。越过这个规模时，把 parent 从 cell 改成 int32 索引数组、gScore 用single，是常规降内存手段。

4.2 启发式权重与搜索步长的参数表

启发式权重体现在f = g + w * h。w=1 保证最优性；w 抬到 1.2，搜索明显更快，路径长度只增加几个百分点，适合无人机巡检这类对绝对最优不敏感、对响应时间敏感的任务。精密装配场景则保持 w=1，不要为了速度牺牲最优性。

搜索步长默认 1 格，即 A* 只走到相邻体素。步长设成 2 或 3 可以减少节点总数，但碰撞检测也必须跟着改：不能只检查目标格子是否落在障碍里，要把当前节点到目标节点之间的所有栅格都采样一遍，否则路径会直接穿墙。步长大于 1 时路径质量开始下降，我只在超大空旷地图上用它。

4.3 用 cscvn 平滑路径并做碰撞回退

A* 输出的路径由栅格中心点组成，拐角接近 45 度或 90 度，飞行控制器直接跟踪这种折线会在每个拐点减速。后处理常用插值样条，Matlab 的 Curve Fitting Toolbox 里cscvn可以直接处理三维点列：

spl = cscvn(path'); % path 尺寸为 N×3，转置成 3×N tt = 0:0.05:spl.breaks(end); % 细分采样参数 smoothPath = fnval(spl, tt)'; % 采样得到平滑轨迹

cscvn 生成的是经过全部原始路径点的插值样条。采样间距 0.05 对应每个栅格内约 20 个点，足够精细。平滑后的点不会严格落在原栅格路径上，所以必须重新做碰撞检查，不能只信可视化：

pts = round(smoothPath); inside = all(pts >= 1, 2) & ... pts(:,1) <= size(map,1) & ... pts(:,2) <= size(map,2) & ... pts(:,3) <= size(map,3); idx = sub2ind(size(map), pts(inside,1), pts(inside,2), pts(inside,3)); collideHits = sum(map(idx)); % 必须为 0

先过滤越界点再统计，因为样条端点可能略超地图边界，round 后直接sub2ind会报错。collideHits大于 0 时，我的回退策略是定位第一个碰撞点的附近区间，把该区间替换回原始折线路径，其余部分保留平滑结果。这样只影响局部，不会让整条路径重新变糙。

提示：没有 Curve Fitting Toolbox 时，可以用spline对 x、y、z 三个分量分别做分段三次插值替代 cscvn，只是端点附近的切线行为略有差异，碰撞回退逻辑完全不变。

5. 三维路径规划结果的验证技巧：批量跑，指标量化

5.1 三维下独有的问题：碰撞点统计

三维路径的碰撞风险集中在平滑段和拐点附近，单独看路径节点是否碰障不够，要对整条轨迹密集采样。把上文的collideHits统计放在一次规划后立即执行，指标必须为 0。除碰撞点之外，再记录三个数：路径总长（平滑后的累积欧几里得距离）、规划耗时、成功标志。这三个数构成一次规划结果的最小描述集，后续所有对比实验都以它为基准。

5.2 蒙特卡洛批量验证脚本

单次规划成功说明不了问题，三维障碍分布多变，必须批量跑。下面脚本用 100 张随机地图验证算法稳定性：

runs = 100; successRate = false(runs, 1); planTime = zeros(runs, 1); parfor i = 1:runs rng(i); % 每次固定随机种子 map = randomMap3d(60, 60, 30); % 随机生成障碍物地图 [sp, gp] = randomEndpoints(map); % 随机选取不在障碍内的起终点 t0 = tic; p = astar3d(map, sp, gp); planTime(i) = toc(t0); successRate(i) = ~isempty(p); % 返回非空路径即算成功 end fprintf('success rate = %.1f%%\n', 100 * mean(successRate)); fprintf('mean plan time = %.3f s\n', mean(planTime)); fprintf('median plan time = %.3f s\n', median(planTime));

parfor需要 Parallel Computing Toolbox，没有时改成for一样能跑。看结果时我一般关注中位数而不是均值，因为少数极端地图会让规划时间拖出长尾，均值容易被带偏。成功率低于 95% 时先查地图生成参数，多数问题不在 A* 本身，而是膨胀半径过小导致路径贴障、或起终点选在了窄通道两侧。

5.3 用参数扫描 + 画图定位最优权重

启发式权重 w 的取值可以用一维扫描确定。w 从 1.0 到 1.5 步进 0.1，每组在固定随机种子下跑 20 次蒙特卡洛，记录平均耗时和平均路径长度，然后在 matlab 画图里用双 y 轴把两条曲线画在一起：左轴是规划时间，右轴是路径长度与直线距离的比值。观察曲线，耗时下降变缓而路径长度开始上升的拐点就是合适的 w。对所有批量实验统一设置rng(42)这类固定种子，保证报告里的数据和别人复跑时完全一致。

本文还有配套的精品资源，点击获取