总有人问我无人机三维路径规划该怎么入门。说实话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万。没有好的启发函数引导搜索效率会很难看。下面这个表能直观看出二维和三维路径规划在实现上的差距对比维度二维寻路三维寻路状态空间(x, y)(x, y, z)邻居数量4或86、18或26地图表达二维数组三维数组启发函数2D欧氏距离/曼哈顿距离3D欧氏距离额外约束避障、最短路径避障、飞行高度、能耗、转弯性能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×100double数组就需要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), i1), ... max(1, j-1):min(mapSize(2), j1), ... max(1, k-1):min(mapSize(3), k1)) 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(未找到可行路径请检查地图和起终点设置); endisosurface是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。结果整理如下启发权重路径总长栅格单位扩展节点数运行耗时秒0.643.5约32000.181.041.0约21000.111.541.7约14000.072.042.6约9000.04可以看到几个趋势当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里没有内置的优先队列可选方案有这么几种数据结构方案优点缺点数组min扫描实现简单容易调试大节点数下性能差二叉堆用结构体数组模拟插入、删除效率高实现复杂容易出bugcontainers.Map按key访问快不支持直接取最小f值Java PriorityQueue性能很好需要熟悉Java接口我的建议地图规模在50×50×20以内矩阵版完全够用再大就直接上Java PriorityQueueMatlab里java.util.PriorityQueue是现成的性能比手写二叉堆稳定得多。5.2 路径“斜穿”障碍物角点这个坑我在第3章已经提到了。需要特别提醒的是当你把邻域从6个改成26个时不做碰撞检查几乎必然出现斜穿障碍角的情况。这个问题的典型特征是路径整体看起来挺短但局部会出现一段“贴着障碍物尖角”的折线。排查方法是在可视化图里逐段查看路径和障碍物的接触关系。如果你在写自己的代码时没有isMoveSafe这个函数强烈建议补上。5.3 找不到路径时的系统化排查跑着跑着代码突然返回“未找到可行路径”这是最让人头疼的情况。遇到这种问题我有一套固定的排查顺序检查起点和终点是否落在障碍物内。这是最常见的原因尤其是终点靠近障碍物时容易误设。检查地图是否全被障碍物封闭。可以用BFS先跑一次连通性检测如果起点和终点不在同一个连通区域A*无论如何都找不到路径。检查开放列表是否提前变成空。在while循环里加一个迭代次数上限同时打印当前扩展节点就能定位是搜索没推进还是确实无路可走。检查索引是否越界。三维数组的下标是从1开始边界判断和多维索引非常容易写错。5.4 路径平滑的后续处理A*直接输出的路径是一连串栅格点用无人机实际飞行标准看往往不够平滑。常见做法是三次样条插值或者B样条拟合。在Matlab里cscvn和fnplt组合可以快速生成一条平滑曲线。不过要注意平滑后的路径可能再次穿过障碍物所以平滑之后必须再做一次碰撞检测必要时对平滑结果进行微调。从复杂度上看我的建议是先用A*得到全局航迹再用样条做局部平滑最后交给下层的轨迹跟踪控制器。这也是目前很多无人机路径规划项目的标准流程。5.5 几个让我印象深刻的实际教训我最早做三维A*时直接拿了一套二维代码改只是把坐标从2维扩到3维。结果跑出来的路径有两个明显问题一是频繁“斜穿”障碍物角落二是路径高度上下翻飞、非常不自然。后来把代价函数里的高度惩罚加上又把isMoveSafe补上路径质量才明显改善。还有一次我把地图分辨率提高后代码跑了很久都没出来结果。排查了半天才发现是因为开放列表里的重复节点没有及时清理节点数量膨胀到了几万个。后来加了ismember剔除逻辑才解决。就我个人的实际体会A三维路径规划看似简单但要把代码从“能跑”提升到“跑得好”真正花时间的地方全在细节里障碍物怎么膨胀、对角线怎么检查、开放列表怎么管理。把这几个细节处理明白你得到的不仅是一套能复现的Matlab代码更是对路径规划底层逻辑的完整理解。这套基础打牢之后再去碰RRT、优化算法甚至强化学习方法都会顺手很多。