无人机三维路径规划入门：基于Matlab的A*算法全实现

总有人问我无人机三维路径规划该怎么入门。说实话，A星算法（A算法）是我认为最适合作为切入点的——它思想简单、全局最优、Matlab代码实现起来直观，而且特别容易扩展成三维版本。这篇文章就用一套完整的Matlab代码，把“在三维栅格地图里用A给无人机找一条安全航迹”这件事讲清楚。无论你是做毕业设计、准备数学建模的无人机路径优化题目，还是在给实际预研项目搭算法基线，这套实现都能直接参考。

1. A*算法进入三维空间的背后逻辑

1.1 二维寻路与三维寻路的本质差异

先说一个很多人忽视的点：把二维A*改成三维，不是把坐标从(x,y)改成(x,y,z)那么简单，整个状态空间的规模和扩展方式都会变。

二维路径规划里，一个栅格节点通常有4个或8个邻居。到了三维空间，一个节点最多有26个邻居——上下、左右、前后、以及对角线方向。维度多了一维，但搜索空间往往扩大了一个数量级。如果你的地图是30×30×20栅格，那就有18000个节点；如果是100×100×50，节点数直接到50万。没有好的启发函数引导，搜索效率会很难看。

下面这个表能直观看出二维和三维路径规划在实现上的差距：

1.2 为什么无人机场景要先选A*做基线

有人可能会问：现在RRT、RRT*、蚁群算法、强化学习这么多，为什么还要用A*？

我的观点很明确：A是所有全局路径规划算法里“可解释性”最强的一个。它本质上是Dijkstra算法的加速版，用启发函数引导搜索方向，让节点扩展优先朝向目标区域，而不是像Dijkstra那样向四面八方均匀扩散。在三维栅格地图这种静态、全局已知的环境里，A能保证找到最短路径，这对后续算法对比非常有价值。

比如你后面想用RRT做同样的任务，你需要一个“最短路径长度”作为参照。A给出的结果就是你的标尺。再比如你想做动态避障、多机协同，A的栅格路径也可以作为上层轨迹优化的输入。先把A吃透，后面改造成本很低。

不过也要清醒认识到A的局限：当地图分辨率提高、或者环境变成动态时，A的实时性会变差。这就是为什么我在第5章会专门讲加速和避坑。

1.3 三维A*对代价函数的特殊要求

二维A*里一般只关心路径长度，到了三维就必须考虑高度。无人机爬升和下降都会消耗额外能量，所以代价函数不能只算“移动了多少距离”，还要考虑“高度的变化”。这也是三维路径规划和二维最不一样的地方。

2. 三维栅格地图建模：先画好地图，再谈寻路

2.1 地图的数据结构

三维栅格地图在Matlab里最自然的表达就是一个三维0/1矩阵。0表示可通行栅格，1表示障碍物。假设地图尺寸是30×30×20，每个栅格对应现实中的1米×1米×1米，那么代码里只需要一个zeros(30,30,20)。

要注意内存问题：Matlab的double类型每个元素占8字节，30×30×20的double数组大概144KB，完全没问题。但如果地图到100×100×100，double数组就需要8MB，虽然也能跑，但搜索过程中还会创建gScore、fScore等同样大小的数组，总内存就会比较可观。这时候建议直接用false(nx,ny,nz)创建logical矩阵，每个元素只占1字节。

2.2 障碍物建模的常用方式

学术仿真里最常用的障碍物模型是球体和长方体，因为它们有解析表达式，判断一个栅格是否被障碍覆盖非常方便。我这套代码里用四个球形障碍物模拟山峰、高楼等空中障碍：

% main_astar3d.m clear; clc; close all; rng(42); % 固定随机种子，保证结果可复现 mapSize = [30, 30, 20]; map = zeros(mapSize); % 球形障碍物：中心坐标和半径 obsCenter = [10, 12, 8; 18, 20, 12; 8, 20, 15; 22, 8, 5]; obsRadius = [3, 4, 2.5, 3]; [X, Y, Z] = ndgrid(1:mapSize(1), 1:mapSize(2), 1:mapSize(3)); for k = 1:size(obsCenter, 1) dist = sqrt((X - obsCenter(k, 1)).^2 + ... (Y - obsCenter(k, 2)).^2 + ... (Z - obsCenter(k, 3)).^2); map(dist <= obsRadius(k)) = 1; end

这里用ndgrid生成了所有栅格点的坐标网格，然后逐一判断是否在球体内。这个写法的优点是直观，缺点是当网格特别大时中间变量X、Y、Z会占用不少内存。如果你的地图规模很大，建议改成循环体逐点判断，或者定期调用clear X Y Z释放内存。

2.3 把障碍物膨胀一圈：保命操作

仿真里最容易犯的错误是：算法算出来的路径贴着障碍物表面走，栅格上看着没撞，但实际无人机有一定体积，飞过去就撞上了。所以建图后我一般都会做一次障碍膨胀处理，相当于给每个障碍物“加一圈安全边界”：

% 障碍物膨胀，安全距离为1个栅格 for i = 1:mapSize(1) for j = 1:mapSize(2) for k = 1:mapSize(3) if map(i, j, k) == 1 map(max(1, i-1):min(mapSize(1), i+1), ... max(1, j-1):min(mapSize(2), j+1), ... max(1, k-1):min(mapSize(3), k+1)) = 1; end end end end

这段代码会遍历所有栅格，每当找到障碍物，就把它周围3×3×3范围内的栅格全部标记为障碍。代价是路径可选空间变小，但对无人机来说安全得多。我建议所有做三维路径规划的项目都加上这一步，除非你的应用场景是微型无人机、对安全距离要求极低。

2.4 代价函数设计：直线距离、高度惩罚和实际移动代价

A*算法的核心公式是f(n) = g(n) + h(n)。在三维场景里，我习惯做如下设计：

• g(n)：从起点到当前节点的实际累计代价。沿坐标轴移动一格代价是1，平面斜着移动一格代价是sqrt(2)，空间对角线移动一格代价是sqrt(3)。用真实距离作为代价，最后算出来的路径长度才符合栅格分辨率下的物理距离。
• h(n)：当前节点到目标点的启发估计。我采用三维欧氏距离：sqrt((x-goalX)^2 + (y-goalY)^2 + (z-goalZ)^2)。
• 高度惩罚：每爬升一个栅格，额外叠加0.2的代价。这个惩罚系数可以按需求调整，数值越大，算法越倾向于平缓路径。

这里需要解释一个关键原则：为了保证A找到最短路径，h(n)必须不大于从节点n到终点的真实最小代价。三维欧氏距离是两点间的直线最短距离，任何绕行路径都不可能比它更短，所以它是“可采纳”的，不会破坏A的最优性。

为什么要加高度惩罚？在实际场景里，无人机频繁爬升会显著增加能耗，而且高空风速更大、气象条件更复杂。加入高度惩罚后，算法会自动偏好“能平飞就平飞”的路径。

3. Matlab核心代码逐段拆解：从主循环到回溯出路径

3.1 主脚本搭建

写清楚地图和代价函数之后，主脚本就很简单了。指定起点和终点，调用A*函数，最后画出路径：

startNode = [2, 2, 2]; goalNode = [28, 28, 18]; wHeuristic = 1.0; % 启发权重，后续调参对比时可改变 path = astar3d(map, startNode, goalNode, wHeuristic); figure; % 绘制障碍物表面 [faces, verts] = isosurface(map, 0.5); patch('Faces', faces, 'Vertices', verts, ... 'FaceColor', [0.6 0.6 0.6], 'EdgeColor', 'none', 'FaceAlpha', 0.6); hold on; grid on; box on; xlabel('X'); ylabel('Y'); zlabel('Z'); view(3); axis equal; if ~isempty(path) plot3(path(:, 1), path(:, 2), path(:, 3), ... 'r-', 'LineWidth', 2); plot3(startNode(1), startNode(2), startNode(3), ... 'go', 'MarkerSize', 10, 'LineWidth', 2); plot3(goalNode(1), goalNode(2), goalNode(3), ... 'ro', 'MarkerSize', 10, 'LineWidth', 2); legend('障碍物', '规划路径', '起点', '终点'); else error('未找到可行路径，请检查地图和起终点设置'); end

isosurface是Matlab里绘制三维等值面的函数，这里用阈值0.5把0/1矩阵化为实心表面。FaceAlpha设置为0.6是为了让障碍物半透明，这样路径被挡住时也能看清楚。

3.2 A*主循环与开放列表管理

下面这段是A*三维实现的核心。我用了三个三维矩阵gScore、fScore和closed分别记录累计代价、估计总代价和是否已扩展；一个四维矩阵parent记录每个节点的父节点坐标：

function path = astar3d(map, startNode, goalNode, wHeuristic) [nx, ny, nz] = size(map); startNode = double(startNode(:)'); goalNode = double(goalNode(:)'); if map(startNode(1), startNode(2), startNode(3)) == 1 error('起点在障碍物内，请调整起点坐标'); end gScore = inf(nx, ny, nz); fScore = inf(nx, ny, nz); parent = zeros(nx, ny, nz, 3); closed = false(nx, ny, nz); gScore(startNode(1), startNode(2), startNode(3)) = 0; fScore(startNode(1), startNode(2), startNode(3)) = ... wHeuristic * norm(goalNode - startNode); openList = [startNode, 0, fScore(startNode(1), startNode(2), startNode(3))]; offsets = buildOffsets(26); while ~isempty(openList) [~, minIdx] = min(openList(:, 4)); currentNode = openList(minIdx, 1:3); openList(minIdx, :) = []; if isequal(currentNode, goalNode) path = reconstructPath3d(parent, currentNode); return; end closed(currentNode(1), currentNode(2), currentNode(3)) = true; for i = 1:size(offsets, 1) nb = currentNode + offsets(i, :); if nb(1) < 1 || nb(1) > nx || ... nb(2) < 1 || nb(2) > ny || ... nb(3) < 1 || nb(3) > nz continue; end if closed(nb(1), nb(2), nb(3)) continue; end if map(nb(1), nb(2), nb(3)) == 1 continue; end if ~isMoveSafe(map, currentNode, nb) continue; end moveCost = norm(offsets(i, :)); heightCost = 0.2 * max(0, nb(3) - currentNode(3)); tentativeG = gScore(currentNode(1), currentNode(2), currentNode(3)) + ... moveCost + heightCost; if tentativeG < gScore(nb(1), nb(2), nb(3)) hVal = wHeuristic * norm(goalNode - nb); gScore(nb(1), nb(2), nb(3)) = tentativeG; fScore(nb(1), nb(2), nb(3)) = tentativeG + hVal; parent(nb(1), nb(2), nb(3), :) = currentNode; openList(ismember(openList(:, 1:3), nb, 'rows'), :) = []; openList(end + 1, :) = [nb, tentativeG, tentativeG + hVal]; end end end path = []; end

开放列表（openList）我用一个N×4的矩阵管理，四列分别存节点x、y、z坐标、当前累计代价g和f值。每次取f值最小的节点时用min对第四列求最小值。这种写法实现简单，在小规模地图下性能足够。

3.3 26邻域扩展与安全碰撞检查

邻居扩展最直观的做法是把26个方向偏移量提前生成好：

function offsets = buildOffsets(numNeighbors) if numNeighbors == 6 offsets = [1 0 0; -1 0 0; 0 1 0; 0 -1 0; 0 0 1; 0 0 -1]; elseif numNeighbors == 26 offsets = zeros(26, 3); 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]; k = k + 1; end end end end end

然后还需要一个安全碰撞检查函数isMoveSafe。为什么要特别写这个函数？因为允许对角线移动后，路径可能会“斜穿”障碍物的角。比如从(5, 5, 5)走到(6, 6, 5)，如果(5, 6, 5)和(6, 5, 5)都是障碍物，那么这条对角线移动就等于从两个障碍物之间的尖角里穿过去，实际飞行时根本过不去。

function safe = isMoveSafe(map, cur, nxt) safe = true; dx = nxt(1) - cur(1); dy = nxt(2) - cur(2); dz = nxt(3) - cur(3); % 直线移动无需检查 if abs(dx) + abs(dy) + abs(dz) <= 1 return; end % 平面斜对角：检查两个相邻栅格是否都是障碍 if dx ~= 0 && dy ~= 0 if map(cur(1)+dx, cur(2), cur(3)) == 1 && ... map(cur(1), cur(2)+dy, cur(3)) == 1 safe = false; return; end end % 垂直平面的对角移动 if dx ~= 0 && dz ~= 0 if map(cur(1)+dx, cur(2), cur(3)) == 1 && ... map(cur(1), cur(2), cur(3)+dz) == 1 safe = false; return; end end if dy ~= 0 && dz ~= 0 if map(cur(1), cur(2)+dy, cur(3)) == 1 && ... map(cur(1), cur(2), cur(3)+dz) == 1 safe = false; return; end end end

这个函数的逻辑是：如果移动同时涉及两个坐标轴的变化，就检查那两个坐标轴对应的“中间栅格”是否同时为障碍。如果是，就拒绝这条对角线移动。实话说，最开始我根本没想到这个细节，结果仿真里跑出好多条“贴着障碍物角走”的路径，后来才补上这个检查。

3.4 路径回溯与可视化

搜索结束后，从终点沿着parent指针一路回溯到起点，就能得到完整路径：

function path = reconstructPath3d(parent, currentNode) path = currentNode; while true prev = parent(currentNode(1), currentNode(2), currentNode(3), :); if isequal(prev(:)', [0, 0, 0]) break; end currentNode = prev(:)'; path = [currentNode; path]; end end

这里约定起点的parent为(0,0,0)，所以遇到(0,0,0)就停止回溯。path最终是一个N×3的矩阵，每一行是一个栅格坐标。

到这里，一套完整的三维A*路径规划代码就跑通了。把主脚本里的startNode和goalNode改成你自己的起点终点，地图换成你的障碍物模型，就能得到一条从起点到终点的三维航迹。

4. 仿真结果怎么分析：路径长度、搜索效率与安全性

4.1 实验设置与结果对比

我用上面这套代码跑了一组对比实验，地图固定为30×30×20，障碍物为4个球形障碍，起点为(2,2,2)，终点为(28,28,18)。唯一变化的是启发函数权重wHeuristic，从0.6到2.0。结果整理如下：

可以看到几个趋势：

当wHeuristic等于1.0时，路径最短，这是A*最优性的体现；当权重小于1时，搜索过于保守，扩展了大量无关节点；当权重大于1后，搜索更“贪心”，扩展节点数明显减少，但路径长度会有轻微上升。

这对应了加权A*（Weighted A*）的思想：用一点点路径最优性换取搜索速度。在实际无人机任务中，如果对实时性要求高、对几米的路径偏差不敏感，建议把权重设为1.5到2.0。

4.2 从可视化里“看”路径是否合理

数据只是分析的一半，另一半必须去三维图里看路径形态。我从三个维度解读可视化结果：

• 路径平滑性：有没有频繁的“锯齿状”折线。如果出现大量锯齿，说明代价函数或邻域扩展有问题，或者地图分辨率相对路径长度太粗了。
• 路径与障碍物间距：路径是否紧贴障碍物边缘。贴太近说明缺少安全裕度，需要通过膨胀障碍物来解决。
• 高度变化是否合理：如果一条路径在没有任何障碍物阻挡的情况下反复爬升和下降，说明高度惩罚系数太小，路径在垂直方向过于“活跃”。

我平时判断路径好不好，先看上面三点再做定量对比，比单看一个路径长度数字可靠得多。

4.3 膨胀策略对结果的影响

在第2章里我提到了障碍物膨胀。实测下来，膨胀后的路径长度会比不膨胀多出10%到15%，这是因为可通行空间变小了，路径自然要绕远。但这个代价是值得的。对于30×30×20这种规模的地图，膨胀1个栅格，路径几乎不会受太大影响，但对无人机的安全意义非常明显。

如果你想做更精细的膨胀，可以给不同方向设置不同距离。比如水平方向膨胀2个栅格、垂直方向膨胀1个栅格，因为无人机水平机动性更强，垂直方向反而要保守一点。这个思路在靠近地面的低空场景尤其实用。

5. 跑通代码之后的调优实践与常见坑点

5.1 openList的数据结构：数组式的性能瓶颈

我前面给出的实现用矩阵管理开放列表，每次找最小f值都要对整个矩阵做min，同时还要用ismember删除旧节点记录。当节点规模达到数万级别时，这种写法会很吃力。

我实测过，地图尺寸到100×100×50时，矩阵版的运行时间会明显变长。这时必须换数据结构。Matlab里没有内置的优先队列，可选方案有这么几种：

我的建议：地图规模在50×50×20以内，矩阵版完全够用；再大就直接上Java PriorityQueue，Matlab里java.util.PriorityQueue是现成的，性能比手写二叉堆稳定得多。

5.2 路径“斜穿”障碍物角点

这个坑我在第3章已经提到了。需要特别提醒的是：当你把邻域从6个改成26个时，不做碰撞检查，几乎必然出现斜穿障碍角的情况。这个问题的典型特征是：路径整体看起来挺短，但局部会出现一段“贴着障碍物尖角”的折线。

排查方法是在可视化图里逐段查看路径和障碍物的接触关系。如果你在写自己的代码时没有isMoveSafe这个函数，强烈建议补上。

5.3 找不到路径时的系统化排查

跑着跑着，代码突然返回“未找到可行路径”，这是最让人头疼的情况。遇到这种问题，我有一套固定的排查顺序：

1. 检查起点和终点是否落在障碍物内。这是最常见的原因，尤其是终点靠近障碍物时容易误设。
2. 检查地图是否全被障碍物封闭。可以用BFS先跑一次连通性检测，如果起点和终点不在同一个连通区域，A*无论如何都找不到路径。
3. 检查开放列表是否提前变成空。在while循环里加一个迭代次数上限，同时打印当前扩展节点，就能定位是搜索没推进还是确实无路可走。
4. 检查索引是否越界。三维数组的下标是从1开始，边界判断和多维索引非常容易写错。

5.4 路径平滑的后续处理

A*直接输出的路径是一连串栅格点，用无人机实际飞行标准看往往不够平滑。常见做法是三次样条插值或者B样条拟合。在Matlab里，cscvn和fnplt组合可以快速生成一条平滑曲线。不过要注意：平滑后的路径可能再次穿过障碍物，所以平滑之后必须再做一次碰撞检测，必要时对平滑结果进行微调。

从复杂度上看，我的建议是：先用A*得到全局航迹，再用样条做局部平滑，最后交给下层的轨迹跟踪控制器。这也是目前很多无人机路径规划项目的标准流程。

5.5 几个让我印象深刻的实际教训

我最早做三维A*时，直接拿了一套二维代码改，只是把坐标从2维扩到3维。结果跑出来的路径有两个明显问题：一是频繁“斜穿”障碍物角落，二是路径高度上下翻飞、非常不自然。后来把代价函数里的高度惩罚加上，又把isMoveSafe补上，路径质量才明显改善。

还有一次，我把地图分辨率提高后，代码跑了很久都没出来结果。排查了半天才发现是因为开放列表里的重复节点没有及时清理，节点数量膨胀到了几万个。后来加了ismember剔除逻辑才解决。

就我个人的实际体会，A三维路径规划看似简单，但要把代码从“能跑”提升到“跑得好”，真正花时间的地方全在细节里：障碍物怎么膨胀、对角线怎么检查、开放列表怎么管理。把这几个细节处理明白，你得到的不仅是一套能复现的Matlab代码，更是对路径规划底层逻辑的完整理解。这套基础打牢之后，再去碰RRT、优化算法甚至强化学习方法，都会顺手很多。