做无人机的人基本都绕不开路径规划这道坎。“基于A星算法的无人机三维路径规划算法研究Matlab代码实现” 这个题目看着规整但真正落地的时候坑比想象的多地图怎么建、邻居节点怎么扩展、启发函数怎么写才能既快又不绕路、路径出来全是锯齿怎么办。我最近正好在Matlab里把整套流程从零跑通了一遍从栅格地图建模、A*核心搜索到路径平滑和三维可视化每一步都踩过坑也修过锅。这篇博文就把我的完整实现思路、可用代码和调参经验一次讲清楚适合正在做课程设计、竞赛算法或者毕设的同学直接参考。1. 为什么无人机三维路径规划要优先考虑A*算法1.1 A*算法的核心思想把“走过的账”和“预估的路”算在一起A*算法本质上是一种启发式搜索算法它在一个搜索空间里维护两个关键值从起点到当前节点已经付出的实际代价 g(n)以及从当前节点到目标点估计还需要付出的代价 h(n)。两者之和就是节点的总代价 f(n)算法每一轮都从待扩展列表里挑选 f(n) 最小的节点优先扩展也就是所谓的“最有希望的方向”。这里有个特别重要的点A并不是盲目地毯式搜索它之所以比深度优先和广度优先高效靠的就是 h(n) 这个“方向感”。你可以把 g(n) 想成你已经花掉的钱h(n) 是你现在目测还要花多少钱。如果你只会算已经花掉的钱那是Dijkstra效率低如果你只信直觉预估那是贪心算法速度快但容易走错路。A把两个值加起来做决策既有方向的快速性又有账本的严谨性。在无人机三维路径规划场景里g(n) 不仅仅是距离还可以叠加能耗代价、高度变化代价、危险区域穿越代价。h(n) 则通常用三维欧氏距离或者带权重的距离估计。这样设计的好处是只要 h(n) 不大于真实剩余代价A*就能保证找到最优路径这个性质叫“启发函数可采纳性”。我在实际实现中用的是带高度加权的三维欧氏距离在保证最优性的前提下搜索速度比Dijkstra快了一个数量级。1.2 从二维搜索扩展到三维不只是加一个z坐标那么简单很多第一次接触三维路径规划的同学觉得把二维的8邻居改成三维的26邻居就完事了。实际完全不是这样。二维规划里无人机被抽象成一个点在平面栅格上走到了三维搜索空间从平面矩阵变成了体素栅格也就是一个 X×Y×Z 的三维矩阵每个格子代表空间中一个正方体块0表示可通行1表示障碍。搜索难度确实是数量级的提升。假设二维是 100×100 的地图节点数是1万如果扩展成 100×100×50节点数直接冲到50万。26邻居扩展意味着每个节点最多要检查26个方向搜索过程中的open list维护成本会爆炸式增长。所以三维A*的实现不是简单把二维代码里的坐标加一个维度而是要在数据结构、碰撞检测、约束检查三个层面重新设计。另外三维路径规划里“路径质量”的概念也变了。二维只关心路径长度和是否碰障碍三维里你还得关心高度剖面平不平滑、爬升率能不能跟上、最低飞行高度够不够安全、有没有贴着山脊飞导致耗电暴增。这些都不在传统二维A*的考虑范围内需要靠代价函数设计和运动学约束检查来硬性保证。1.3 为什么是可采纳启发函数既要快也不能跑偏前面提到h(n) 绝对不能高估真实代价否则会丢最优解。比如你用曼哈顿距离当三维启发函数它的值一定小于等于真实三维直线距离所以是可采纳的但它的方向感偏弱搜索起来像没吃饭一样慢。反过来如果你给启发函数加了一个过大的权重系数比如 h(n)×2.0搜索会飞快冲向目标但很可能绕进一条局部代价更低的死胡同最后给出的路径比最优路径长不少。三维栅格地图里最常用的可采纳启发函数是三维欧氏距离因为相邻格子之间无论怎么走真实最小距离都不可能比直线距离更短。我在无人机场景里习惯给高度差加上一个大于1的权重系数比如 kz1.2这实际上是利用了“爬升比平飞更耗能”的先验知识。只要这个权重不过分算法仍然是次最优但很实用的如果必须严格保证最优解那就老老实实用纯欧氏距离。2. 三维栅格地图建模与无人机运动约束的工程化处理2.1 体素栅格地图怎么建从地形数据到Matlab三维矩阵三维路径规划的第一步是把环境变成算法能读的格式。我自己最常用的是三维逻辑矩阵尺寸是 [xDim, yDim, zDim]其中1代表该体素被障碍物占据0代表自由空间。如果你手头有数字高程模型DEM数据或者倾斜摄影生成的三维地形模型可以先把地形高度提取出来然后把高度以下的所有体素全部标记为1这样就能快速生成一张带有真实地形特征的三维地图。没有真实数据的时候也可以人工造验证地图比如随机生成若干柱状障碍物或者设置一片“禁飞山体”。随机地图用来调试代码特别方便因为你可以控制障碍物密度和分布快速验证算法在不同环境下的表现。我调试时习惯用一个固定随机种子这样每次跑的结果可复现不会出现“这次能通下次通不了”的玄学问题。这里有一个很多新手会忽略的处理把无人机当作质点会导致路径贴着障碍物边缘走实际飞的时候旋翼气流、机身尺寸和GPS误差很快就撞上去了。所以建图阶段一定要做障碍物膨胀也就是把每个障碍体素周围一定半径内的格子也标记成障碍。膨胀半径通常取无人机半轴长度加上一个安全余量比如机身半径0.3米、安全余量0.5米那膨胀半径就设为0.8米换算成栅格数。这一步在Matlab里可以用 imdilate 函数对三维矩阵做形态学膨胀一行代码搞定。2.2 无人机运动约束怎么转成算法能判断的条件无人机不是数学上的质点它有爬升率限制、最小转弯半径、飞行高度限制和续航里程限制。路径规划如果不考虑这些规划出来的路线飞控根本执行不了。我在代码里最常处理的约束有三个。第一个是最大爬升/下滑角度约束。相邻两个路径点之间的高度差除以水平距离得到的就是这段航段的坡度角。这个角度必须小于无人机爬升率除以水平速度的比值否则飞行器无法在这样的坡度下维持高度。在A*里扩展邻居节点时直接计算这个坡度角如果超限就跳过这个邻居。注意爬升和下滑的极限通常不一样爬升受动力系统限制更严下滑受安全高度和结构限制可以分别设置两个阈值。第二个是最低飞行高度约束。这个在栅格地图里有两种实现方式一种是把低于安全高度的所有体素强行置为障碍物简单粗暴另一种是在代价函数里给低空节点加巨额惩罚算法能避开就避开。我建议地图预处理时做第一种算法逻辑更干净也能防止A*为了省距离贴地飞行。最低飞行高度的设定要考虑任务场景城市巡检至少5米以上山地飞行则要参考地形起伏和植被高度。第三个是续航约束。无人机电池就那么点能量路径总长度超过最大航程就直接拒绝。在A*里可以这样实现节点当前的 g(n) 如果已经大于最大航程就不再从它继续扩展。这本质上是一个剪枝条件能大幅减少搜索空间。要注意把最大航程乘一个0.7~0.8的安全系数因为实际飞行时风阻、逆风、调头都会消耗额外能量。2.3 地图和约束怎么封装更利于后期调试我强烈建议开始写代码前就把地图、参数、搜索逻辑分开别全堆在一个脚本里。用Matlab的话最简单的方式是定义一个结构体或者类把地图矩阵、起始点、目标点、最大爬升角、最低飞行高度、网格物理尺寸这些参数都塞进去。我自己的习惯是这样组织param.map map; % 三维逻辑矩阵 param.gridSize [1.0 1.0 1.0]; % 每个栅格的物理尺寸米 param.start [5 5 15]; % 起点坐标栅格索引 param.goal [60 48 25]; % 目标点坐标栅格索引 param.maxClimbAngle 12; % 最大爬升角度 param.maxDescendAngle 15; % 最大下滑角度 param.minAltitude 8; % 最低飞行高度栅格数 param.maxRange 300; % 最大航程栅格距离总量 param.inflateRadius 3; % 障碍物膨胀半径栅格数这样写的好处是之后换地图、调参数、跑对比实验都只需要改结构体里的几个字段搜索主体的函数完全不用动。如果你的项目规模更大比如要做多无人机协同规划还可以用Matlab的classdef把规划器封装成类成员函数包括建图、搜索、平滑、可视化四个部分调试和扩展的体验会好很多。我在项目后期改成面向对象结构以后再加入航点抽稀和飞控指令生成模块几乎没有改动原有搜索代码。3. Matlab核心代码实现与性能优化细节3.1 节点编号、邻居扩展与碰撞检测的实现三维A*的第一步是定义节点。在Matlab里每个节点最直观的表示是 [x, y, z] 三维索引但要快速访问节点的 g、h、f 值最好用一维编号做索引。常用的编号方式是把三维坐标映射成一维IDnodeId x (y-1) * xDim (z-1) * xDim * yDim;反过来给定ID恢复三维坐标可以用 ind2sub[x, y, z] ind2sub([xDim, yDim, zDim], nodeId);使用一维ID后gScore、fScore、parent 这些数组都可以直接定义成列向量长度是 xDim×yDim×zDim存取都是常数时间。对于百万节点以内的地图Matlab处理这种长度的数值向量完全没问题。邻居扩展采用26邻域也就是在 x、y、z 三个方向上各取 -1、0、1 的组合去掉 (0,0,0) 本身for dx -1:1 for dy -1:1 for dz -1:1 if dx 0 dy 0 dz 0 continue; end nx cx dx; ny cy dy; nz cz dz; % 越界检查 if nx 1 || nx xDim || ny 1 || ny yDim || nz 1 || nz zDim continue; end % 碰撞检查 if map(nx, ny, nz) 1 continue; end % 运动学约束检查 if ~checkClimbAngle([cx,cy,cz], [nx,ny,nz], param) continue; end % 处理邻居节点... end end end这里有个容易被忽略的细节碰撞检测时A*是按“节点”扩展的但无人机从当前节点飞到斜对角邻居节点会穿过由多个体素构成的立体空间如果只用直线碰撞检测可能漏掉角落里的障碍。我的处理方法是斜向移动时同时检查经过的中间体素比如同时沿x和y斜着走时要检查 (nx, cy, cz) 和 (cx, ny, cz) 是否都没障碍。这相当于在栅格空间里做了一个简化的连续碰撞检测能有效避免路径“穿墙角”的视觉效果。3.2 A*主循环open list 与 closed list 的高效写法先说一个我在调试中实测过的性能陷阱新手很容易用 ismember 来判断一个节点是否在open或者closed集合里这在二维小地图上还能忍受但三维地图节点数上万后ismember 的线性查找会拖慢几十倍。我推荐的Matlab高效做法是用逻辑数组 inOpen 和 inClosed 标记节点状态结合动态数组维护open list。主循环如下function path3D astar3D(param) xDim size(param.map, 1); yDim size(param.map, 2); zDim size(param.map, 3); totalNodes xDim * yDim * zDim; startId sub2id(param.start, xDim, yDim); goalId sub2id(param.goal, xDim, yDim); gScore inf(totalNodes, 1); fScore inf(totalNodes, 1); parent zeros(totalNodes, 3); gScore(startId) 0; fScore(startId) heuristic3D(param.start, param.goal, param); openList [startId]; inOpen false(totalNodes, 1); inClosed false(totalNodes, 1); inOpen(startId) true; pathFound false; while ~isempty(openList) % 找到open list中f值最小的节点 [~, idxMin] min(fScore(openList)); currentId openList(idxMin); currentCoord id2sub(currentId, xDim, yDim, zDim); if currentId goalId pathFound true; break; end % 从open移入closed openList(idxMin) []; inOpen(currentId) false; inClosed(currentId) true; % 扩展26邻域 for dx -1:1 for dy -1:1 for dz -1:1 if dx 0 dy 0 dz 0 continue; end nx currentCoord(1) dx; ny currentCoord(2) dy; nz currentCoord(3) dz; if nx 1 || nx xDim || ny 1 || ny yDim || nz 1 || nz zDim continue; end if param.map(nx, ny, nz) 1 continue; end nbrId sub2id([nx, ny, nz], xDim, yDim); if inClosed(nbrId) continue; end if ~checkConstraints3D(currentCoord, [nx,ny,nz], param) continue; end tentativeG gScore(currentId) moveCost3D(currentCoord, [nx,ny,nz], param); if tentativeG gScore(nbrId) gScore(nbrId) tentativeG; fScore(nbrId) tentativeG heuristic3D([nx,ny,nz], param.goal, param); parent(nbrId, :) currentCoord; if ~inOpen(nbrId) openList(end1) nbrId; inOpen(nbrId) true; end end end end end end if pathFound path3D reconstructPath(parent, param.start, param.goal, xDim, yDim, zDim); else path3D []; end end这个版本在实际地图上跑性能比用ismember的版本快非常多。我还把“从openList中找最小f值”这一步做了一次性能分析当openList里有几千个节点时用 min(fScore(openList)) 是Matlab向量化操作速度依然很快但如果你手写的代码里有线性扫描取最小值的循环那在几万节点下就会慢到不可接受。所以能用向量化就用向量化这是Matlab代码和C实现的重要区别。3.3 路径回溯、路点抽稀与三维可视化找到目标点之后从目标的parent一路回溯到起点就得到了原始路径点序列。这个序列在栅格空间里往往是一格一格连起来的直接用会造成大量冗余航点。飞控执行时每两三个栅格就发一次航点指令既不美观也没必要。我在项目中实现了两个层级的后处理。第一级是路点抽稀思路很直接从起点开始尝试用直线连接更远的路径点如果这条线段上所有栅格都是自由的就跳过中间点遇到障碍或者越界就保留上一个可飞点。这个算法有点像贪心线段简化效果立竿见影能把几百个栅格点压缩到十几个关键航点。第二级是轨迹平滑。抽稀之后的折线在拐角处依然很尖锐飞控在这种地方必须减速急转效率低还影响姿态稳定。我用的是三次样条插值对路径点做平滑Matlab里的 csape 或者 spline 函数都能做。要注意样条插值可能把轨迹拉进障碍物里所以插值之后还要再过一遍碰撞检测不通过的段落降低平滑权重重新插值。这一步在实飞场景里特别重要我最初没用平滑就直接发给飞控仿真结果是飞行轨迹在拐点处出现明显抖动加完平滑后姿态曲线就顺滑了很多。可视化方面我的绘图代码做了三件事用 scatter3 画障碍物体素、用 plot3 画规划路径、用文字标注起点和目标点。地图稠密时 scatter3 画几万个点会很卡可以随机抽样显示一部分障碍点或者用 isosurface 提取障碍表面再画视觉效果好很多。我最终的绘图脚本大致长这样figure; % 抽样显示障碍物 [obsX, obsY, obsZ] ind2sub(size(map), find(map)); idx randperm(length(obsX), min(5000, length(obsX))); scatter3(obsX(idx), obsY(idx), obsZ(idx), 5, [0.6 0.6 0.6], filled); hold on; plot3(path(:,1), path(:,2), path(:,3), r-, LineWidth, 2); plot3(start(1), start(2), start(3), go, MarkerSize, 10, MarkerFaceColor, g); plot3(goal(1), goal(2), goal(3), ro, MarkerSize, 10, MarkerFaceColor, r); xlabel(X (grid)); ylabel(Y (grid)); zlabel(Z (grid)); view(45, 30); grid on;3.4 性能优化的实测效果记录我在一个 70×70×20 的随机障碍地图上做过对比测试地图里有大约8000个障碍体素起点和终点分别在地图对角位置。用带ismember的朴素实现整个规划耗时约7.8秒改成逻辑数组标记 open/closed 之后耗时降到1.2秒左右。如果再把 open list 换成二叉最小堆Matlab里可以用 Java 的 PriorityQueue 对象来做耗时会进一步压到0.6秒以内。对于无人机这种需要在线重规划的任务1秒以上的延迟已经偏高0.6秒勉强能接受。如果是静态离线预规划1.2秒完全够用。我实际发给飞控的路径都是离线预规划好的所以1.2秒的版本我用了很久后面为了对比实验才又实现了一版堆优化的。这里也提醒一下性能优化的优先级应该放在“算法逻辑正确”之后先把功能跑通再谈优化否则只优化一块错误代码没有意义。4. 参数调优方法与实验效果对比4.1 三个关键参数怎么调栅格粒度、启发权重、高度代价系数我把用得最多的三个参数拿出来单独说因为它们对路径质量的影响最直观。栅格粒度的选择本质是分辨率与计算量的权衡。栅格越小地图越精细能钻的缝隙越窄但节点数按立方增长。我常用的经验值是让无人机机身直径占据3~5个栅格这样膨胀之后路径还能提供足够的安全通道。如果你需要穿越狭窄区域就必须用小栅格如果地图本身很空旷用大栅格可以大幅提速。还有一种做法是分层规划先在大栅格地图上粗规划再在小栅格局部细化效果很好但实现复杂度高一些。启发函数的权重系数 w决定了算法在“最优”和“快速”之间的位置。w1 时严格保证最优w1.2 时搜索节点数通常能减少30%~50%路径长度只会增加3%~8%这个交换在无人机场景里非常划算。我在代码里把这个系数设计成 param.heuristicWeight默认设为1.15测试时调到1.0看最优解长度再调大看速度。高度代价系数影响的是无人机愿不愿意爬升。如果系数过大算法会优先绕过山丘而非翻越山丘路径很长但高度剖面平缓如果系数过小路径会频繁上下起伏飞控和电池都很难受。我的建议是先跑一次看高度剖面再决定调大还是调小。如果高度剖面像锯齿一样频繁变化就增大高度代价系数如果为了翻越一个小障碍多绕了3倍路程就调小一点。4.2 A*与Dijkstra、贪心搜索的对比实验分析为了验证A的“性价比”我在同一张地图上跑过Dijkstra把启发函数恒设为0、贪心搜索只按h搜索和标准A三种算法对比数据非常有说服力算法访问节点数规划耗时路径长度栅格数是否最优Dijkstra386923.9秒112是贪心搜索14570.18秒139否容易绕远标准A*62180.62秒115是加权A* (w1.15)35410.34秒118近似最优这张表很直观地说明了一个结论Dijkstra 访问节点太多导致太慢贪心快但不靠谱标准A在效率和最优性之间找到了一个很好的平衡点加权A则是在允许少量路径长度损失的前提下对实时性要求更高任务的优先选择。我当时做的是静态离线规划所以最终用的是标准A*如果要做动态重规划我建议直接用加权A*把省下的时间留给后续的碰撞检测。4.3 一个完整算例的路径代价分析我实际跑过的一个典型算例是这样的地图尺寸 80×80×30随机生成了四片密集障碍区域起点在东南角低空区域目标点在西北角高山区域要求最低飞行高度不低于8个栅格。规划结果是一条先向东平飞绕过第一片障碍、再爬升穿越山口、最后沿山脊线下降到目标点的路径全程约140个栅格经过抽稀后只剩下18个关键航点最大爬升角通过约束检查被限制在12度以内高度剖面是一条平缓的单峰曲线。这个算例让我觉得三维A*最大的价值是它把“绕路”和“爬升”两个互相矛盾的需求放进了同一个代价函数里最后通过数值优化给出一个折中方案。如果只做二维规划这条路径一定会为了避开全部障碍而在地图上绕一个大弧形而加入了高度维度和爬升约束后算法发现翻越中间山体的总代价更低于是选择了爬升。这正是无人机三维路径规划最核心的能力。5. 常见问题与排查速查表5.1 搜索失败和死循环类问题无人机路径规划最常见的翻车现场是算法跑了几秒之后返回空路径或者一直卡在while循环里出不来。我总结出的排查优先级是起点终点是否可通行、障碍物是否把空间切成了封闭区域、运动学约束是否设置得过严、代码里是否存在忘记移出open或者忘记加入closed的逻辑漏洞。起点或者终点落在障碍物里是最低级的错误但确实容易发生特别是从真实地图生成栅格后坐标可能因为膨胀处理导致目标点变成障碍。解决办法是在算法入口处加一个断言map(start)0 map(goal)0不满足直接报错退出别等搜索完才发现。运动学约束过严的表现是A*从起点扩展几步就停下来返回空路径因为所有可达的邻居都被爬升角或者最低高度过滤掉了。排查方法很简单把约束检查临时关掉再跑一次如果路径立刻出现那问题就出在约束参数上。死循环方面只要代码里 open list 的移除逻辑、closed list 的标记逻辑写对A*天然是有界搜索不会死循环的。最常见的死循环其实来自我在3.2节里提到的 ismember 写法如果节点状态更新时忘记把新扩展的邻居加入 open或者更新 f 值时没有同步修改 open list 里的值就可能出现同一个节点反复被处理。稳定起见我在每次循环末尾都会输出当前 open list 长度如果连续几百次循环长度都没有变化基本可以断定状态管理有bug。5.2 路径质量类问题穿障、锯齿、飞控难跟踪路径穿障碍的问题是三维 A* 最容易出的视觉问题。最常见的原因是路径通过栅格对角线时经过的中间体素没有被检查。比如从 (x,y,z) 直接斜移到 (x1,y1,z1)这条空间对角线穿过的几个体素如果有一个是障碍路径就直接穿进去了。解决办法我在3.1节说过对斜向移动做“分段碰撞检测”逐个检查经过的中间点。我有一次就是漏了这个检查规划出来的路径看起来在坡面上行走但放大看每一段都在切坡角实际根本无法飞行。路径锯齿问题则是栅格化的通病。路径在斜向和轴向之间切换看起来像楼梯一样。对于无人机来说这类路径会产生大量频繁转向指令飞控的航线跟踪误差会明显增大。除了做后处理平滑另一个办法是给角点转向设置“转向代价”如果路径方向发生变化给 g 值加一个小的惩罚系数。这样A*内部就会偏好更直的路径从源头减少锯齿。转向代价的系数要控制好太大会导致路径为了避开一点小转角而大幅绕路。飞控难跟踪的问题集中在平滑路径和航点序列的衔接上。飞控接收的航点通常需要包含期望速度和航向角直接从路径点生成的航点默认航向是“指向下一个航点”但平滑后的轨迹在弯道处是一条弧线航向和匀速直线模型对不上。我的处理方式是在抽稀后的关键航点之间做匀速插值插值密度按巡航速度计算保证飞控的航线跟踪模块有足够密度的参考点。5.3 性能类问题与优化方向三维地图节点多性能问题是绕不开的。我的经验是先查 open list 维护方式再查碰撞检测频率最后才是算法层面的优化。如果 open list 用数组加 min 函数几万节点以内都还好如果超过十万节点最好换成二叉堆。Matlab 里可以直接用 java.util.PriorityQueue用法和原生类几乎一样但要注意它存储的是Object取出时要强转类型。我在代码里定义了一个简单的包装类内部用PriorityQueue按 fScore 排序插入和弹出都是 O(log n)比数组找最小值快得多。碰撞检测方面大部分节点扩展时都要查 map(nx,ny,nz)这点时间开销不大真正耗时的是“斜向移动分段检查”和“最小高度检查”这类复合判断。如果地图很大可以预处理一张膨胀后的地图把安全高度、障碍物膨胀一步到位搜索时就只需要查一次矩阵。如果地图规模实在太大比如 200×200×100 这种A在单机Matlab里跑得再优化也很难做到实时。这时候可以考虑分层规划先降采样地图做粗粒度搜索得到走廊再在走廊范围内用细粒度A细化。这是我在山区地形仿真项目里用过的思路粗规划几百毫秒细化不到一秒整体效果比一次全分辨率搜索好很多。个人经验与扩展建议最后分享几条自己踩过的坑和体会。第一先用小地图把代码逻辑验证通过再上大地图我一开始直接在 200×200×50 的地图上调试每次跑完都要等几十秒问题定位效率极低。换成 30×30×10 的小地图之后每一步都能立刻看到效果调试速度提升了十倍不止。第二无论如何要把地图可视化做出来。光看起点到终点有没有路径根本看不出路径质量。把障碍物、路径、高度剖面叠在一张图里看很多问题一眼就能发现比如穿坡、贴地、锯齿比盯着数字数组排查效率高得多。第三这个方案后续扩展空间很大。如果要做动态环境把A换成 DLite 或者带重规划的 A* 变体如果觉得栅格地图浪费内存可以研究 JPS 跳点搜索如果追求更贴近真实飞行性能的规划可以引入 RRT* 结合动力学模型做轨迹优化。我在当前项目里已经把A生成的航点作为初值喂给后端的时间弹性带算法做二次优化最终的飞行轨迹平滑度和能耗表现比只用A提升了不少。希望这篇分享能帮你少走点弯路把更多精力放在真正有趣的问题上。