前阵子帮一个做农用巡田无人机的朋友整航线预规划原型需求很朴素已知起降点和目标测区中间有山头、信号塔和一排高压线走廊希望自动生成一条不撞障碍物的三维航线而不是靠人坐在电脑前手动点几个航点。我第一反应就是上A星算法直接拿Matlab把三维路径规划先跑通验证——这可能是所有路径规划方案里最容易被想到、也最适合快速验证逻辑的思路。这篇文章就把这套基于A星算法的无人机三维路径规划完整实现过程写出来从栅格地图建模、核心搜索代码、路径平滑到参数调优和上真机前要补的坑一次讲清楚。适合正在做无人机相关毕设、竞赛的同学也想给想快速在Matlab里验证路径规划算法的工程师做个参考。先说结论A星算法在“静态已知三维环境里求一条较优参考航点”这件事上非常能打但工程落地时真正决定成败的往往是地图建模和路径后处理而不是那一百多行搜索主循环。下面按我当时推进的顺序一步步拆解。1. 为什么把路径规划敲定给A星算法候选算法排查与选型逻辑1.1 无人机三维路径规划到底在规划什么无人机从A点飞到B点听起来就是一条线段的事但实际上要满足不少约束地形不能穿越、障碍物要留安全间隙、高度不能低于最低安全高度也不能超过飞行升限。如果用数学语言描述就是在一个三维连续空间里找出满足约束的曲线同时尽量让路径最短或者耗能最小。问题在于连续空间是无限维的没法直接拿计算机去穷举。所以通常的做法是先把空间离散化成一个个小格子也就是体素网格然后在这个离散图结构上搜索。A星的搜索对象就是这种栅格化之后的三维地图输出是一条由离散点连成的折线路径后续再交给平滑模块转成可飞行的航线。1.2 和RRT、遗传算法放一起比A星的位置在哪我当时也把其他几个常见方案放在桌面上比了一圈毕竟“路径规划算法”这顶帽子底下选择性太多不逐个排查容易选错。方案优点缺点适合场景A*静态已知环境下能保证找到最短路结果确定实现直观地图大时搜索空间增长快输出是折线点列离线全局航点规划、仿真验证Dijkstra不依赖启发式一定得到最短路无方向引导搜索范围大慢小地图、启发式难设计的场景RRT不用离散化连续空间直接采样高维扩展好路径抖动、不是最优、需要大量后处理复杂约束、动态环境快速找可行解RRT*渐进最优收敛慢、参数敏感算力足够且对路径质量要求高的场合遗传/粒子群能结合路径长度、能耗、威胁等多目标一起优化随机性强、收敛不稳定、不能保证最短路离线多目标寻优、教学演示对照这个表我当时的任务很明确环境是静态已知的离线规划可以接受目标就是给出一个确定、可靠的航点序列。这种情况下A星是最划算的选择。RRT虽然灵活但每次跑出来的路径都不一样还得花时间平滑和去抖遗传算法这类群体优化方法在这些年经常出现在论文里但作为工程验证的起手式有点杀鸡用牛刀而且参数不好调。1.3 A星的适用边界A星也有明显的天花板搜索空间随栅格数量指数膨胀30×30×20的体素网格跑起来很轻松一旦到几百乘几百的高程范围就会卡顿。另外A星本质是几何路径搜索不包含飞机动力学或者转弯半径约束所以它输出的路径只能算“航点参考线”不能直接说这就是飞行轨迹。我对它的定位一直是“全局规划的第一层”后面必须跟着平滑、速度规划这些模块。2. 三维栅格地图建模把真实空域变成Matlab能搜的矩阵2.1 从真实环境数据到体素栅格的映射流程A星搜索的输入是一张三维布尔矩阵0表示可通行1表示障碍。真实环境数据怎么变成这个矩阵是整个流程里最容易出错的一环因为后面搜索算法再漂亮地图错了也是白费。我习惯的流程是先获取测试区域的数字高程模型DEM或者点云数据再按照设定分辨率把水平面切成x、y方向的格子每个格子根据地面高度和地面以上的障碍物高度填充对应层的体素。简单说就是先在地面上建一个“高度柱”柱子里有物体就全部标成1。如果地形上有一座山山体范围内的(x, y)格子里从地面到山顶高度这一段的z层都要标记成障碍。如果只用公开的高程数据做仿真通常没有建筑物和树木的细节这时候我会手动叠加一些模拟的障碍物比如信号塔、高楼、高压线走廊。仿真阶段不需要百分之百还原真实地形但障碍物的相对空间关系一定要正确否则调出来的路径完全没有参考性。2.2 障碍物膨胀和安全高度层不能忽略的两件事真实无人机有翼展和旋翼半径不可能贴着障碍物表面飞行所以在地图建模时必须对障碍做膨胀处理。通俗说就是把每个障碍物周围一定半径内的格子也标记为不可通行。比如飞机半径假设0.5米栅格分辨率1米那就把障碍物外扩至少1格。膨胀不足后面生成的路径会穿过危险区域膨胀过头又会把可飞走廊堵死。另一个容易被忽略的是安全高度层。很多区域不是硬障碍但无人机不适合飞太低比如高压线走廊上方、居民区屋顶上方、树木冠层上方。这类约束我用两种方式处理要么直接在地图里把z轴低层全部标记为1强制飞机爬升到设定高度以上要么在代价函数里加一个高度惩罚项让A星尽量选择高空航线但不把低空彻底封死。实际使用中如果任务没有强制低空飞行的需求我更推荐直接抬高最低飞行高度层简单可靠。2.3 生成模拟测试地图的Matlab代码在Matlab里生成一张测试地图我通常这么写function map createTestMap(nx, ny, nz) % nx, ny, nz 分别是x轴、y轴、z轴方向的栅格数量 map zeros(nx, ny, nz); % 中央山体以(xc, yc)为中心半径8格高度随距离递减 xc round(nx * 0.5); yc round(ny * 0.5); for x 1:nx for y 1:ny d sqrt((x - xc)^2 (y - yc)^2); if d 8 h max(2, round(16 - d * 1.2)); % 山顶约16层边缘逐渐变矮 if h nz map(x, y, 1:h) 1; end end end end % 两座信号塔/高楼模拟竖直柱状障碍 towers [8, 22, 10; 24, 6, 12]; for k 1:size(towers, 1) tx towers(k, 1); ty towers(k, 2); th towers(k, 3); map(tx:tx1, ty:ty1, 2:th) 1; end % 最底层全部设为障碍防止搜索跑到“地下” map(:, :, 1) 1; end这段代码里有个细节最底层直接全部填成障碍。原因很简单如果z1这层能通行A星经常会把路径压到地图最下面走遇到地形数据缺失或者边界处理不当就容易穿地。保守一点把地面层封死让搜索只能在安全高度以上活动。我第一次跑通三维A星时犯过一个典型的坐标系混淆错误地图里map(x, y, z)我把x和y对应反了画出来的路径直接斜穿信号塔Visualization看起来非常滑稽。建议在代码文件顶部用注释写清楚“map(x, y, z)x对应横坐标y对应纵坐标z对应高度”后面画图、取坐标时都统一按这个约定。2.4 为什么不能简单从二维栅格“升维”成三维很多人觉得二维A星写熟了三维不就是多一层z循环吗其实没那么简单。二维地图如果边长是n栅格总数是n^2三维就是n^3搜索空间体量完全不同。我之前帮人改过一个二维A星demo强行把高度方向复制成20层结果原本几毫秒跑完的搜索直接卡了几十秒。原因是原来二维地图里很多自由空间到了三维会变成巨大空洞A星把大量时间花在探索这些不必要的高度组合上。实际工程里我一般不会一上来就用全分辨率的完整三维体素而是先做分层规划第一层用低分辨率粗网格快速得到一个粗路径第二层只在粗路径所在的带状区域内用细网格精修。这样既保留了三维搜索的避障能力又不会让计算量爆炸。3. A星搜索核心拆解启发式、邻域扩展与回溯的实现细节3.1 数据组织用三维矩阵替代数组遍历的Open表网上大量A星教程习惯用open表和closed表存节点对象每个节点包含坐标、g值、h值、父节点指针。这个方法在二维小地图上没问题但三维栅格地图动辄几十万格每次都要在open表里线性扫一遍找最小f值Matlab会慢到无法接受。我在实现里换成了一种更贴合Matlab思维方式的数据组织直接用三维矩阵存g值和f值每个格子对应一个固定位置查找和更新都是常数时间不需要反复对象比较。open表只需要存候选坐标每次迭代时从open表里取f值最小的节点。closed逻辑则用一个inOpen布尔矩阵加inf判断来处理。这样做的优势很明显不管地图多大判断一个节点是否已经搜索过只需要查一次矩阵代码也更简洁。3.2 启发式函数的三维形态与可采纳性A星的效率核心是启发式函数h(n)。在二维里常见的是曼哈顿距离和欧氏距离三维里我几乎只用欧氏距离h(n) w * sqrt((x_n - x_g)^2 (y_n - y_g)^2 (z_n - z_g)^2)其中w是启发式权重通常取1.0。这里必须强调可采纳性概念如果启发式函数永远不高估真实代价A星保证能找到最优路径。欧氏距离在允许对角移动的三维栅格中是可采纳的因为任意两点间最短真实距离不会小于直线距离。但曼哈顿距离在三维对角移动场景下会高估导致路径虽然搜得快但可能丢掉最优解。所以三维A星里我默认用欧氏距离只有在强制6邻域移动时才考虑曼哈顿距离。3.3 邻域扩展与移动代价修正三维A星的邻域扩展和二维最大的区别在于移动代价的修正。我的做法是遍历dx、dy、dz从-1到1的所有组合排除全零组合这样一共是26个候选方向。移动代价不是统一按1算而是按欧氏距离轴向移动代价1面对角移动代价sqrt(2)体对角移动代价sqrt(3)。这一步非常关键否则算法会“偏爱”斜着走因为斜走消耗的g值被低估了直接后果就是路径会扭曲成奇怪的45度斜线并且总路径长度算出来明显偏短。对于固定翼无人机来说体对角移动还要额外谨慎因为真实飞机无法“瞬间横移一个格子”这种大角度机动在平滑阶段会被修正掉。但如果只是做多旋翼的航点预规划允许体对角没太大问题。3.4 主循环代码核心搜索骨架这是整个实现里最核心的一段代码保存为astar3d.mfunction [path, cost] astar3d(map, start, goal, hWeight) % 三维A星搜索 % 输入: % map - 三维逻辑/数值矩阵, 0可通行, 1障碍 % start - 起点坐标 [x, y, z] % goal - 终点坐标 [x, y, z] % hWeight - 启发式权重, 默认1.0 % 输出: % path - Nx3 路径点序列 % cost - 总路径代价 [sx, sy, sz] size(map); start round(start(:)); goal round(goal(:)); if map(start(1), start(2), start(3)) 1 error(起点位于障碍物内部); end if map(goal(1), goal(2), goal(3)) 1 error(终点位于障碍物内部); end gScore inf(sx, sy, sz); gScore(start(1), start(2), start(3)) 0; fScore inf(sx, sy, sz); fScore(start(1), start(2), start(3)) hWeight * norm(start - goal); cameFrom zeros(sx, sy, sz, 3); % 每个位置记录父节点坐标 openList struct(x, start(1), y, start(2), z, start(3)); isInOpen false(sx, sy, sz); isInOpen(start(1), start(2), start(3)) true; expandCount 0; while ~isempty(openList) % 取出f值最小的节点 fVals arrayfun((n) fScore(n.x, n.y, n.z), openList); [~, idx] min(fVals); cur openList(idx); openList(idx) []; isInOpen(cur.x, cur.y, cur.z) false; % 到达终点则回溯 if cur.x goal(1) cur.y goal(2) cur.z goal(3) path reconstructPath(cameFrom, cur); cost gScore(goal(1), goal(2), goal(3)); fprintf(扩展节点数: %d\n, expandCount); return; end expandCount expandCount 1; % 遍历26邻域 for dx -1:1 for dy -1:1 for dz -1:1 if dx 0 dy 0 dz 0 continue; end nx cur.x dx; ny cur.y dy; nz cur.z dz; if nx 1 || nx sx || ny 1 || ny sy || nz 1 || nz sz continue; end if map(nx, ny, nz) 1 continue; end stepCost norm([dx, dy, dz]); % 1 / sqrt(2) / sqrt(3) tentativeG gScore(cur.x, cur.y, cur.z) stepCost; if tentativeG gScore(nx, ny, nz) gScore(nx, ny, nz) tentativeG; fScore(nx, ny, nz) tentativeG hWeight * norm([nx, ny, nz] - goal); cameFrom(nx, ny, nz, :) [cur.x, cur.y, cur.z]; if ~isInOpen(nx, ny, nz) openList(end1) struct(x, nx, y, ny, z, nz); isInOpen(nx, ny, nz) true; end end end end end end % 搜索失败 path []; cost inf; disp([搜索失败扩展了 , num2str(expandCount), 个节点]); end回溯函数单独写一个reconstructPath.mfunction path reconstructPath(cameFrom, cur) path [cur.x, cur.y, cur.z]; while true px cameFrom(cur.x, cur.y, cur.z, 1); py cameFrom(cur.x, cur.y, cur.z, 2); pz cameFrom(cur.x, cur.y, cur.z, 3); if px 0 py 0 pz 0 break; end cur struct(x, px, y, py, z, pz); path [cur.x, cur.y, cur.z; path]; end end这段代码在30×30×20的地图上跑起来是毫秒到百毫秒级别算得上“开箱即用”。3.5 画图验证时最容易发现的问题搜索代码写完别急着往下走一定要先把结果画出来看。我用的是map createTestMap(30, 30, 20); start [2, 2, 3]; goal [28, 28, 15]; [path, cost] astar3d(map, start, goal, 1.4); plot3(path(:, 1), path(:, 2), path(:, 3), r-, LineWidth, 2);画出来如果路径出现“贴墙走”“穿山体”“在地图最底层乱窜”百分之九十是地图标记或坐标轴方向的问题。特别是z轴方向我最初把高度方向压到了矩阵的第一维后面所有索引全乱了。调试这种问题最快的方法是在地图里放一个只在特定高度存在的障碍比如塔楼然后看路径是否真的绕过了塔顶。4. 从栅格骨架到平滑航线后处理环节不能省4.1 为什么不能把A星的折线直接丢给飞控A星返回的路径本质上是沿着体素中心走出来的折线转角都是硬生生90度或者45度真实无人机根本不可能这样飞。多旋翼在这种航线上会大幅减速固定翼更不可能完成这种瞬间转向。所以一拿到路径就开始读飞控文档是不现实的中间必须补平滑这一步。平滑的目标只有一个在保持路径大致形状和避障结果的前提下把锐利的拐角磨圆让航迹变得连续可飞。4.2 B样条平滑的具体操作如果装了Robotics System Toolbox可以直接用bsplinepolytraj对路径做B样条拟合方便但不保证避障。如果没装也可以手写B样条基函数核心是先确定控制点再沿曲线采样得到平滑后的航点序列。我的经验是不要对整条路径做全局B样条拟合因为容易把障碍物附近的折线拉直导致撞障碍。更稳妥的做法是“安全走廊平滑”先把A星路径上每个点向外扩展一个安全半径形成一条管道平滑后的路径点只允许在管道内移动这样既光滑又不会偏离原避险轮廓太多。具体实现不复杂但能大幅降低后面碰撞检测失败的概率。4.3 平滑后的碰撞检测必须做不能信感觉平滑算法不会感知障碍物所以平滑后的每个采样点都要重新查一遍地图确认没有落到障碍格里。我给这段写了个小函数function ok validateCollision(pathSmooth, map, sampleStep) % 沿平滑后路径逐段采样检查是否碰撞障碍物 ok true; for i 1:size(pathSmooth, 1) - 1 segLen norm(pathSmooth(i1, :) - pathSmooth(i, :)); nSamples max(2, ceil(segLen / sampleStep)); for k 0:nSamples pt pathSmooth(i, :) (pathSmooth(i1, :) - pathSmooth(i, :)) * k / nSamples; idx round(pt); if idx(1) 1 || idx(1) size(map, 1) || ... idx(2) 1 || idx(2) size(map, 2) || ... idx(3) 1 || idx(3) size(map, 3) continue; end if map(idx(1), idx(2), idx(3)) 1 ok false; return; end end end end碰撞检测失败后不要无脑调低平滑强度大多数情况是地图膨胀半径不够或者安全高度下限太低。把膨胀半径调大一点重新规划往往比在平滑阶段反复试参数更有效。4.4 航点抽稀与安全高度修正A星在空旷区域也会一格一格地走生成的航点可能有几百个直接传给飞控既浪费带宽部分飞控还有航点数量上限。抽稀我常用Ramer-Douglas-Peucker算法把在一条直线上冗余的中间点删掉只保留必须的转折点。如果主控对航点数量有严格限制比如只有50个航点槽位这一步必不可少。还有一个细节如果地图里最低安全飞行高度设置过低A星可能会给出贴着屋顶高度飞行的路径。真实飞行里这种路径没法用我会在规划前就把z轴下限整体抬高到安全高度以上而不是等路径出来后再去修改。5. 启发式权重、网格密度和邻域模式我实测过的三组调参数据这一节分享一下我在这套代码里实际做过的三组对比测试。测试环境是Matlab R2022b处理器i7-12700H地图用的是30×30×20的体素网格中央一座山体加两座塔楼起点(2,2,3)终点(28,28,15)。5.1 启发式权重w一个旋钮两种性格w扩展节点数路径代价运行时间现象1.0约310047.2约0.8秒探索区域大路径最优但搜索慢1.3约150048.3约0.3秒探索区域明显收缩路径略次优1.6约90050.8约0.18秒路径偏向直线开始出现折角w越大算法越“贪心”越倾向于朝目标方向猛冲代价就是可能错过真正的最优绕行通道。如果地图通道很窄比如两个障碍物之间只有一格缝隙w过大可能直接找不到路。所以我在环境复杂的场景里宁可用1.0到1.2只有在空旷大平原这种场景才会调到1.5以上去压时间。5.2 网格密度精度和效率的权衡同样一块区域把分辨率从1米改成0.5米栅格总量变成8倍。比如从30×30×20到60×60×40体素数量从18000涨到144000gScore和fScore各占一个double数组cameFrom是四维数组内存直接飙升。实测下来运行时间从不足1秒变成十几秒路径的改善却微乎其微。碰到这种情况我的做法是两层规划第一层用粗网格快速抓全局轮廓第二层在粗路径周围取一个窄带再在这个窄带里用细网格重新跑A星。效果接近全局高分辨率搜索时间却少一个量级。5.3 邻域模式对比6/18/26邻域的实测差异邻域模式路径代价折角数量扩展节点数适用建议6邻域约60大量直角较少只能说能走不推荐18邻域约52中等中等多数多旋翼任务的平衡点26邻域约48较少最多空旷环境下路径最自然但搜索慢三维路径里体对角移动可以让路径更“斜”地穿过空旷空间减少折角对固定翼尤其有意义。但如果地图障碍密集26邻域会让搜索在障碍缝隙里浪费大量时间18邻域反而更高效。5.4 Matlab实现的性能瓶颈在哪这套教学版代码在Matlab里的主要性能瓶颈有三个openList删除元素、arrayfun遍历struct、三重循环的领域扩展。20万体素以下还能忍再大就得换数据机构。我建议如果以后要跑大面积真地形数据要么把openList改成最小堆要么直接用Java的PriorityQueueMatlab里可以直接new Java对象要么干脆写成C mex。研究验证阶段Matlab完全够用工程化之后再迁移不迟。6. 从Matlab仿真到真机试飞前还有四类问题要兜住6.1 坐标系统一与航点坐标转换Matlab里的栅格索引不是经纬度也不是UTM坐标而是“第几格”。真要导给飞控必须先建立栅格坐标和真实地理坐标之间的映射关系。我的做法是给地图定义一个左下角原点经纬度再定一个网格分辨率然后把路径点换算成经纬高或者本地东北天坐标。这个环节特别容易栽跟头。有一次我把WGS84高程和本地大地水准面高程混在一起算出来的航线整体偏低了几十米还好是在仿真阶段看出来的。建议从项目一开始就明确统一坐标系并写进代码注释别指望靠记忆。6.2 动态障碍物与定期重规划A星只能处理静态地图飞着飞着如果突然有一座塔吊进入航线之前规划的路径就失效了。工程上最简单的兜底方案是启动定期重规划无人机每飞行固定距离或者间隔几秒以当前位置为起点重新跑一次A星目标点不变。这样虽然不能保证最优但至少能应对突发障碍。如果场景里需要更快的重规划响应可以研究一下D* Lite这类增量搜索算法。注意重规划时无人机当前位置几乎不可能正好落在栅格中心需要先取整到最近的可行体素再把它作为新起点。否则规划起点落在障碍格里程序会直接报错。6.3 速度剖面只有路径没有速度还是不能飞A星输出的是位置点序列飞控还需要知道每一段飞多快、在哪里减速。我通常在平滑后的航线上做梯形速度规划每个航点设定一个目标速度拐弯前的减速段按最大加速度约束倒推出来。更简单的做法是统一巡航速度在拐点附近自动减速。不管用哪种方式速度剖面都要在Matlab里同步可视化和路径画在一起看有没有加速度突跳。6.4 改进方向JPS、混合A星和分层规划文章最后列一下几条后续可以深挖的路线。JPS在大量空旷自由区域里能跳过中间冗余格子搜索效率会比A星高很多前提是地图可以预处理。混合A星把无人机运动学模型并进搜索过程输出的是直接可执行的轨迹但实现复杂度高不少。分层规划前面已经提过是放大规模时最务实的办法。如果对动态环境有强需求D* Lite和RRT*也各有适用场景。我自己在这套A星代码往后推了几个项目之后最深的感受是算法本身真的不难真正让我花时间的是地图数据的质量、坐标系的一致性还有平滑后那一次碰撞检测。地图一旦不可信后面所有环节都是空中楼阁。所以如果你也想用Matlab快速验证无人机三维路径规划的想法我的建议是把精力重点放在地图建模和路径校验上A星主体代码一百多行写好之后几乎不用动耐下心来把环境和约束设计清楚结果自然就稳了。