1. 为什么二维A星在无人机场景里不够用——三维路径规划的起点我最早接触无人机路径规划这个需求是在做多旋翼巡检项目的时候。地面机器人跑二维栅格地图跑得好好的A星算法一搜一条折线路径就出来了。但换成无人机问题立刻变味飞行器不是贴着地面跑的它要绕开楼宇、爬升越过障碍、在高低起伏的地形里穿梭单纯在x-y平面里规划根本不成立。所以无人机路径规划这件事本质上是一个三维空间搜索问题——这也是为什么很多同学拿着教科书上的二维A星改三维时觉得哪哪都不对劲。先说清楚这件事的适用场景。基于A星算法的无人机三维路径规划解决的是已知三维环境模型给定起点和终点求一条避开障碍、长度尽量短或者满足某种代价最小的可飞航迹这个问题。它在航拍巡检的航线设计、工业园区的低空物流、灾后侦察的预规划里都挺常见。MATLAB做这件事有个天然优势矩阵运算和可视化工具太成熟了栅格地图直接就是三维数组画出来的路径能直观叠在环境模型上调试起来比C和ROS那一套轻量太多。但要提醒一句A星算法本身是全局静态规划算法它假定环境模型事先知道、且规划期间不会变。这在真实无人机场景里通常用作离线航迹预规划或者作为在线规划器的底层搜索组件。真正飞起来之后传感器实时感知到的动态障碍物得靠局部避障算法比如人工势场法、动态窗口法、或者再次触发A星局部重规划去处理。这篇文章我们先把A星三维规划这件事做扎实再谈怎么在工程里扩展它。A星算法的核心思路其实一句话就能讲完在搜索空间中维护一个开放列表候选节点和一个关闭列表已考察节点每次从开放列表里取出估价函数值最小的节点扩展直到终点被取出来。估价函数是f(n) g(n) h(n)g(n)是从起点走到当前节点n已经付出的实际代价h(n)是从当前节点n到终点的启发式估计代价。f(n)就是这整条路径的总代价估计。这里的精髓在于h(n)的设计决定了搜索效率也决定了能不能找到最优解。如果h(n)始终不大于真实代价专业说法叫可采纳性admissibleA星就一定返回最优路径如果h(n)偏大搜索会更快但可能牺牲最优性。三维场景里这个平衡点怎么拿捏我放在后面专门讲。把A星从二维搬到三维最直观的变化是节点定义。二维是(x, y)三维变成(x, y, z)。但对应的概念更重要邻域、代价、启发函数、碰撞检测这四个维度全部要重写。二维的八邻域扩展变成了三维的26邻域甚至更多二维的曼哈顿距离启发函数在三维里会高估代价不满足可采纳性二维的格子是否被占据判断在三维里要小心沿着网格中心走会造成穿墙这类隐性问题。这些我都踩过坑下面逐个拆开讲。2. A星算法原理回顾与三维扩展的核心改动2.1 启发函数的选择为什么曼哈顿距离在三维里会翻车启发函数是整个A星算法里最值得花心思的地方。二维栅格里很多人习惯用曼哈顿距离因为四邻域场景下这样移动的代价非常直白横着走一格代价为1竖着走一格代价为1总代价恰好等于x方向步数加y方向步数。但三维栅格哪怕你只用六邻域上下左右前后曼哈顿距离依然是一种低估型启发函数它可以工作但搜索效率不高——低估意味着开放列表里会有更多节点被反复考察搜索范围膨胀得很厉害。如果允许斜向移动26邻域情况就变了。从(0,0,0)走到(5,5,5)曼哈顿距离是15。实际走对角线路径每步在对角线方向移动一格代价是√3三维空间体对角线走5步代价才8.66。也就是说曼哈顿距离15远远大于真实代价8.66它变成了高估型启发函数破坏了可采纳性A星会放弃最优路径、加速冲向终点——你可能拿到一条好像还行但明显绕了的路径。三维场景正确的启发函数选择是若只允许六邻域移动步长1用三维曼哈顿距离|dx| |dy| |dz|它是可采纳的。若允许26邻域移动含对角线和体对角线用三维欧几里得距离sqrt(dx² dy² dz²)这是最保守的可采纳选择但搜索范围偏大。想兼顾效率可以按移动代价的max值设计max(|dx|, |dy|, |dz|)乘以对角步长代价这在26邻域下是正好等于理想代价的搜索效率最好。我在代码里默认用了三维欧几里得距离代价函数的g(n)也按真实空间距离累计。这样整个算法的逻辑最干净代价单位统一为米启发函数不估高最优性有保证。如果你追求速度可以把h(n)写成max(|dx|, |dy|, |dz|)乘以√3搜索会明显快代价是路径可能长几个百分点工程上经常这么做。2.2 邻域扩展从8邻域到26邻域的取舍二维A星教材里常用的8邻域到了三维对应的是当前格子周围26个格子都算邻居x方向±1共3种取值y方向±1共3种取值z方向±1共3种取值去掉自身后恰好26个。不过这里有个特别容易理解错的地方26邻域并不等于所有邻居的移动代价相同。分三类看沿坐标轴移动的6个邻居面邻居步长为网格分辨率res代价res。沿面对角线移动的12个邻居边邻居步长为res×√2代价按实际空间距离计。沿体对角线移动的8个邻居角邻居步长为res×√3代价按实际空间距离计。有些简化实现为了省事把26个邻居的代价全部设为1或者全部设为res这在二维8邻域里勉强能忍因为只差√2倍放到三维就太粗糙了——√3和1差了1.73倍路径会明显偏好轴向移动导致折线路径绕来绕去。我建议按空间直线距离算代价代码里就一行distance norm(neighborPos - currentNodePos)不费事结果却扎实很多。另一种取舍是如果你的无人机对转弯半径敏感可以进一步限制邻域比如只允许同一高度面的16邻域垂直升降避免出现先斜飞再猛爬升这种对真实飞行器不友好的轨迹。这一步属于应用层面的约束算法层面不必过度设计先把26邻域的搜索实现干净后续加运动学约束再滤波。2.3 三维障碍物建模与碰撞检测的简化处理这一步决定了代码的实用上限。很多人建三维栅格地图很简单zeros(Nx, Ny, Nz)然后把障碍物占据的格子置1。比如要模拟一栋楼就在某个(x, y)范围、z从地面到楼顶的格子全置1。这做起来非常顺手调试可视化也直观——用MATLAB的scatter3或者slice画一下环境一眼就看明白了。但碰撞检测必须比只看当前节点是否在障碍格子里更进一步。A星的搜索过程是沿着网格中心点走的节点本身不穿障碍不代表节点之间的连线不穿障碍。想象一个薄墙只有一格厚起点和终点分别在墙的两侧相邻格A星检查这两个格子都是自由空间直接就把路径连过去了但实际飞行轨迹是穿墙而过的。解决办法是在判断从当前节点扩展到邻居节点这一步时不仅检查邻居格是否自由还要检查两个节点连线经过的所有格子是否自由。三维里做这件事可以用布雷森汉姆直线算法的三维推广或者更简单地在连线上按小步长采样、逐点查占据状态。采样步长取网格分辨率的四分之一到三分之一就够了精度足够且开销可控。我在MATLAB实现里默认用采样检测法两个节点连线长度为L按step res/4采样了大约4个点逐个查询。地图规模不太大时比如100×100×50的体素空间A星主循环里每次扩展做一次采样检测性能完全扛得住。3. MATLAB代码实现数据结构、主流程与关键函数3.1 第一步搭建三维栅格地图我用一个三维数组map3D表示环境0代表自由空间1代表障碍物。数组三个维度的下标分别对应x、y、z方向的网格编号。配套一个结构体env保存元信息网格分辨率res、地图尺寸nx、ny、nz、地图原点坐标origin。为什么单独存原点因为无人机路径规划最终要输出经纬度或局部坐标系下的坐标网格下标只是索引规划完后要换算回实际坐标。这个换算放最后一步做就行。% 初始化地图尺寸和分辨率 nx 100; ny 100; nz 50; res 1; % 每个网格代表1m % 三维地图矩阵 map3D zeros(nx, ny, nz); % 造两个长方体障碍物和一个圆柱障碍简单示意 map3D(20:30, 30:45, 1:25) 1; % 第一个障碍物 map3D(60:75, 50:65, 1:40) 1; % 第二个障碍物 [X, Y] meshgrid(1:nx, 1:ny); cylinderMask ((X - 80).^2 (Y - 25).^2) 100; % 半径10m的圆柱 map3D(:,:,1:20) map3D(:,:,1:20) | repmat(cylinderMask, 1, 1, 20);这里的写法有个小地方要注意MATLAB的meshgrid生成的X、Y矩阵维度与三维数组的维序容易搞混。我建议全程坚持以数组第1维是x、第2维是y、第3维是z这个约定来写画图时再对应到坐标系。如果你用惯imagesc这类二维函数很容易被行列与xy反转坑到三维里这种坑会放大务必统一约定。3.2 第二步节点结构体与开放列表的取舍每一个被搜索的节点需要记录五样东西三维坐标、g值、h值、f值、父节点索引。MATLAB最直观的做法是用结构体数组但结构体数组在频繁增删时性能很差。我建议这样设计用五个单独的数组来存g值、h值、f值、父节点坐标、是否在开放列表/关闭列表的标记。这样虽然代码没那么面向对象但性能至少快一个数量级。% 用三维矩阵直接记录每个格子的搜索状态 gScore inf(nx, ny, nz); % 实际代价 fScore inf(nx, ny, nz); % 估计总代价 parent zeros(nx, ny, nz, 3); % 父节点坐标(x,y,z)数组存3个分量 inOpen false(nx, ny, nz); inClosed false(nx, ny, nz); % 起点初始化 startIdx sub2ind([nx, ny, nz], sx, sy, sz); gScore(sx, sy, sz) 0; fScore(sx, sy, sz) h_func(sx, sy, sz, tx, ty, tz); inOpen(sx, sy, sz) true;关于开放列表的数据结构教科书都会说用优先队列。但如果你的地图是中等规模几十万格以内用每次从inOpen标记的所有节点中找fScore最小者这种朴素线性扫描完全能跑代码也更不容易出错。我第一版就是这么写的100×100×50的地图迷宫类环境跑几秒钟出结果够用了。要做大规模高分辨率地图再用min-heap或者bucket队列优化那是后话先把逻辑跑通。每次找最小f节点我建议存一个openList的线性索引数组避免每次全图扫描bool矩阵。中间的性能对比我放到第4章细说。3.3 第三步A星主循环与邻居扩展核心主循环的伪码逻辑如下直接看MATLAB代码更容易理解% 26个邻域的偏移量 offsets []; for dx -1:1 for dy -1:1 for dz -1:1 if dx 0 dy 0 dz 0 continue; end offsets(end1, :) [dx, dy, dz]; end end end while ~isempty(openList) % 找到f值最小的开放节点 [~, minIdx] min(fScore(openList)); currentIdx openList(minIdx); [cx, cy, cz] ind2sub([nx, ny, nz], currentIdx); % 到达终点则退出 if cx tx cy ty cz tz break; end % 从开放列表移到关闭列表 openList(minIdx) []; inOpen(currentIdx) false; inClosed(currentIdx) true; % 遍历26个邻居 for i 1:26 nx2 cx offsets(i,1); ny2 cy offsets(i,2); nz2 cz offsets(i,3); % 边界检查 if nx2 1 || nx2 nx || ny2 1 || ny2 ny || nz2 1 || nz2 nz continue; end % 障碍物检查 if map3D(nx2, ny2, nz2) 1 continue; end % 连线碰撞检测 if ~isLineCollisionFree(cx, cy, cz, nx2, ny2, nz2, map3D, res) continue; end % 关闭列表检查 if inClosed(nx2, ny2, nz2) continue; end % 计算移动代价 moveCost norm([nx2- cx, ny2 - cy, nz2 - cz]) * res; tentativeG gScore(cx, cy, cz) moveCost; neighborIdx sub2ind([nx, ny, nz], nx2, ny2, nz2); % 如果找到更好的g值更新节点信息 if tentativeG gScore(nx2, ny2, nz2) gScore(nx2, ny2, nz2) tentativeG; hVal h_func(nx2, ny2, nz2, tx, ty, tz); fScore(nx2, ny2, nz2) tentativeG hVal; parent(nx2, ny2, nz2, :) [cx, cy, cz]; if ~inOpen(nx2, ny2, nz2) inOpen(nx2, ny2, nz2) true; openList(end1) neighborIdx; end end end end这里有个细节openList(minIdx) []在MATLAB里删除元素会造成数组重排小地图无所谓大地图会比较慢。替代方案是逻辑删除定期压缩或者记录一个openList数组并且维护一个inOpenLastPos做swap-pop。追求性能时可以考虑但第一版先用直观写法可读性优先。3.4 第四步路径回溯与可视化找到终点后从终点一路查parent回溯到起点得到的节点序列就是规划路径。注意回溯的坐标是网格下标记得乘上res、加上origin换算到实际坐标pathIdx [tx, ty, tz]; while ~(pathIdx(end,1) sx pathIdx(end,2) sy pathIdx(end,3) sz) p parent(pathIdx(end,1), pathIdx(end,2), pathIdx(end,3), :); pathIdx(end1, :) [p(1), p(2), p(3)]; end pathIdx flipud(pathIdx); % 从起点到终点 path (pathIdx - 1) * res origin; % 转换到实际坐标可视化部分我习惯用两个图叠加第一个图用slice或patch画出障碍物表面第二个图用plot3画出路径。再把起点标成绿点、终点标成红点。这样每一帧都能直观看到算法是否绕了路、有没有钻障碍。figure; % 画障碍物表面简化为画占据格子的散点或用isosurface都可以 [fx, fy, fz] ind2sub([nx, ny, nz], find(map3D 1)); scatter3(fx*res, fy*res, fz*res, 1, [0.6 0.6 0.6], .); hold on; plot3(path(:,1), path(:,2), path(:,3), b-, LineWidth, 2); plot3(path(1,1), path(1,2), path(1,3), go, MarkerSize, 10); plot3(path(end,1), path(end,2), path(end,3), r^, MarkerSize, 10); xlabel(x (m)); ylabel(y (m)); zlabel(z (m)); axis equal; grid on;障碍物数量太大时用scatter3会卡这时候改用isosurface提取表面网格性能会好很多但代码相对长一点。工程里我用过一种折中方案只画障碍物外包络的关键切片面比如等间隔取三个高度画二维占据图直观且轻量。4. 算例测试与结果分析同一张地图下的对比4.1 启发函数对比欧几里得距离 vs 对角线距离为了让结论有说服力我在同一张地图上跑了三组实验h0等价于Dijkstra算法、h三维欧几里得距离、hmax(|dx|,|dy|,|dz|)×√3。地图是100×100×50起点在(10,10,5)终点在(90,90,30)中间随机撒了300个大小不一的长方体障碍物。结果很典型启发函数搜索节点数开放列表峰值路径长度m运行时间秒h0Dijkstra49587152.33.8三维欧几里得31824152.32.4对角距离max×√312673156.11.1解读一下欧几里得启发函数没有牺牲路径长度节点数和耗时却下降了四成。对角距离进一步省了一半时间但路径长度多了2.5%。如果你的航线是巡检用的这2.5%无所谓追求响应速度完全可以选对角距离如果你做的是精确物流、每米都得算能耗就老老实实用欧几里得。网格分辨率对运行时间的影响也很大。分辨率从1m变成0.5m体素数量变成8倍A星的搜索节点数往往涨得更猛三维环境路径搜索复杂度近似与体素数量成正比。所以规划超大范围时不要直接加密网格更聪明的做法是两层规划先粗网格规划一条走廊再在走廊内用细网格精修。这个思路跟工程里的分层路径规划完全一致。4.2 邻域碰撞检测的必要性验证我把碰撞检测关掉跑了一次路径长度显著变短短到不真实。打开路径可视化才发现轨迹斜着穿过了好几个薄障碍物的角落。这正好印证了我前面说的那个坑只看节点是否自由、不做线段级碰撞检测A星很容易钻空子。由此得到的路径在仿真里看起来没问题但转成无人机实际航线必撞无疑。碰撞检测的采样步长也有讲究。步长为res时基本没用因为采样点和节点本身重合步长为res/2时已经能挡住多数斜穿我用res/4在测试里表现稳定再加密到res/8并没有明显改善反而让单次扩展耗时翻倍。所以我的建议是采样步长取0.25倍网格分辨率这是性价比很高的经验值。4.3 死胡同环境下的表现A星不是万能钥匙有一次我把地图设置成一个U形障碍物起点在U形内部、终点在外部。A星的行为非常鲜明先把开放列表里的入口方向节点搜个底朝天确认走不通才掉头找U形开口那一侧。这就是A星与贪心算法的本质区别——它宁可慢也要保证能找到路只要确实存在通路。但这个特性也意味着如果地图里有很多凹形障碍物A星的节点数会暴涨。我后来测过把障碍物从规则长方体换成不规则凹形搜索节点数直接翻了2.6倍。这个现象值得记住。真实城区环境里楼群形成的凹坑非常多A星预规划在那种地图上耗时不可控。工程上的对策是栅格地图预处理先把自由空间做形态学膨胀处理缩小窄道减少凹坑或者把地图做分块连通性分析提前排除不可达区域。MATLAB的Image Processing Toolbox里有imdilate函数对三维数组也有效一行代码就能把障碍物膨胀对降低A星搜索压力效果显著。5. 在基础A星上做改进的几条实用路线5.1 双向搜索和JPS跳点搜索的启示三维A星的改进方向其实很清晰。双向A星是首选起点和终点同时向对方搜索每次扩展两个frontier中f值较小的一方相遇时路径就拼接完成。思路上很简单但MATLAB实现有个坑两个方向搜索用的是同一个地图但各自的gScore/parent/inOpen/inClosed要独立维护等于把内存占用翻倍。我试过在中等地图下双向A星能把节点数再省30%左右但代码复杂度上升明显。你要是赶论文或做毕设先把单向的实现搞扎实双向作为加分项。JPSJump Point Search在二维栅格里效果惊艳但三维的JPS实现要比二维复杂得多——需要定义强制邻居的三维判定条件推导的规则集比二维长好几倍。网上有人做过三维JPS但对栅格地图的规则性要求很苛刻不规则障碍物场景下提升有限。我的态度是三维JPS适合当作研究课题去深入不适合急着写进工程代码里。5.2 粗轨迹的后处理平滑三次样条与B样条A星输出的原始路径是沿着网格中心走的折线段即使代价函数用了欧几里得依然有大量锐角拐弯。无人机飞控通常期望轨迹至少是曲率连续的所以路径平滑是三维路径规划里绕不开的一步。我常用的办法是把A星出来的路径节点当作控制点用参数化三次样条做插值再按弧长重新采样。注意直接用spline会过冲可能让平滑后的轨迹重新撞上障碍物。所以平滑后必须再做一次碰撞检查如果有冲突就把那个区域附近的控制点加密重新平滑。这个平滑-检查-加密的迭代过程是我在实际项目里最依赖的套路。% 以路径节点为控制点做B样条拟合 knots 1:length(path); sp spap2(knots, 4, path); % 4阶B样条 smoothedPath fnplt(sp); % 密集采样平滑曲线B样条的好处是局部性——改一个控制点只影响附近一段曲线方便反复迭代修正。三次样条总长更贴近原始折线但全局性强改一处动全身。我偏好B样条。做完平滑再按无人机最大爬升角和最小转弯半径筛选一遍滤掉不可飞的弯道航线基本具备直接给飞控用的条件了。5.3 面对动态环境的局部重规划前文说了A星是全局静态算法但工程上可以用分时重规划的方式让它适应动环境无人机沿预规划航迹飞行时机载传感器探测到前方某个区域新增了障碍物就把当前无人机位置作为新起点、航迹终点不变在局部范围比如前方200米范围内重新执行A星替换掉冲突段航迹。这个方案比重新全图规划快得多也不会破坏全局航迹的合理性。做局部重规划时有个经验新起点的g值不应当从0开始而应该把原全局路径从真正起点到当前位置的累计代价继承下来。这样局部替换后的路径总代价才与全局路径一致不会出现局部最优值看起来比全局还短这种数据矛盾。MATLAB里实现继承只需把新的gScore初始化为该值即可代码改动非常小。6. 我在调试这段代码时踩过的坑和正在做的扩展6.1 三个高频坑死循环、内存暴涨、路径锯齿第一个坑是死循环。新手版代码经常在开放列表为空这个终止条件上忽略处理。当地图有缺陷、起点终点并不连通时开放列表会被搜空程序却还在while里转圈。解决方法是每次循环开头检查isempty(openList)为空就提示没有可行路径直接返回失败状态。这个检查一行代码但能省掉无数排查时间。第二个坑是内存暴涨。三维地图的gScore和fScore用双精度数组存储100×100×50的地图就是50万个元素每个元素8字节两个数组80MB起步。再加parent数组存三维坐标分量内存直奔150MB以上。MATLAB里这已经能感到卡顿了。对策一是尽量用single精度gScore inf(nx,ny,nz,single)内存直接减半对策二是parent不必用三维矩阵存坐标可以压缩成线性索引一个整数就能表达父节点位置回溯时再转坐标。这两个小改动内存量级可以降到一个零头。第三个坑是路径锯齿。前面说了启发函数或者邻域代价设计不当路径会出现先爬升再下降的无意义折返。排查时把fScore和hScore的分布画出来看很直观如果搜索明显集中在一条狭窄走廊里反复试探多半是障碍物膨胀不够或者采样碰撞步长太粗。处理方案是在地图预处理阶段把障碍物膨胀一圈形态学膨胀给路径留出安全余量顺便减少锯齿。6.2 往真实无人机系统延展的接口思路很多时候做完MATLAB仿真下一步就是跟真机对接。市面上常见的做法有两种一是ROS系统里用Python或C复写一遍A星核心逻辑照搬只用改一下地图数据格式二是MATLAB直接生成C代码部署到机载计算机。第二种对算法验证阶段非常友好但要注意A星里的动态数组操作比如删除开放列表元素在生成C代码时可能不支持得提前把数据结构改成堆或固定容量数组。我在做巡检项目时还踩过一个数据格式坑MATLAB仿真环境里的地图坐标系x向右、y向前、z向上和飞控实际使用的NED坐标系x向北、y向东、z向下不统一。路径输出给飞控前必须做坐标变换和单位换算。这个步骤看似简单但忘记做的后果是无人机起飞后直接朝反方向飞。后来我的习惯是所有路径输出都封装成一个独立函数负责把规划结果转成目标坐标系的航点格式任何项目都复用这一个函数。再聊一个正在做的扩展方向把A星和速度规划联合起来。三维路径规划管的是飞过的空间路线但无人机实际飞行还有速度、加速度限制。现在我在仿真里给路径每个航点附带速度约束比如转弯处限速然后基于路径重新做时间维度的速度优化。这个方向改起来不难——A星只管空间速度层单独做——但整体效果非常显著生成的航迹已经能满足真实飞控的几个基本运动学约束了。后续可以再做基于动力学模型的可行性校验那就是另一个深度的话题了。回头看你手头的项目如果目标是做课程设计或毕设我建议把你现有的A星三维代码先完整跑通再挑一个改进点启发函数优化、双向搜索、路径平滑、动态重规划任选其一深做下去加上对比实验工作量就很扎实了。如果在做工程落地记住我上面提到的分层规划、地图膨胀和坐标变换三件事这三点比算法本身的微调更决定最终效果。MATLAB这个生态做三维路径规划确实有独特优势可视化调试顺手矩阵运算快工具箱齐全。把A星的主体逻辑吃透之后你会发现往RRT、RRT*、人工势场法那些算法迁移很多代码结构都能复用——搜索框架是相通的区别主要在于采样策略和代价定义。这篇里给的代码骨架就是替你把这些共性逻辑打好底子接下来就看你的地图长什么样了。 基于A星算法的无人机三维路径规划MATLAB实现
1. 为什么二维A星在无人机场景里不够用——三维路径规划的起点我最早接触无人机路径规划这个需求是在做多旋翼巡检项目的时候。地面机器人跑二维栅格地图跑得好好的A星算法一搜一条折线路径就出来了。但换成无人机问题立刻变味飞行器不是贴着地面跑的它要绕开楼宇、爬升越过障碍、在高低起伏的地形里穿梭单纯在x-y平面里规划根本不成立。所以无人机路径规划这件事本质上是一个三维空间搜索问题——这也是为什么很多同学拿着教科书上的二维A星改三维时觉得哪哪都不对劲。先说清楚这件事的适用场景。基于A星算法的无人机三维路径规划解决的是已知三维环境模型给定起点和终点求一条避开障碍、长度尽量短或者满足某种代价最小的可飞航迹这个问题。它在航拍巡检的航线设计、工业园区的低空物流、灾后侦察的预规划里都挺常见。MATLAB做这件事有个天然优势矩阵运算和可视化工具太成熟了栅格地图直接就是三维数组画出来的路径能直观叠在环境模型上调试起来比C和ROS那一套轻量太多。但要提醒一句A星算法本身是全局静态规划算法它假定环境模型事先知道、且规划期间不会变。这在真实无人机场景里通常用作离线航迹预规划或者作为在线规划器的底层搜索组件。真正飞起来之后传感器实时感知到的动态障碍物得靠局部避障算法比如人工势场法、动态窗口法、或者再次触发A星局部重规划去处理。这篇文章我们先把A星三维规划这件事做扎实再谈怎么在工程里扩展它。A星算法的核心思路其实一句话就能讲完在搜索空间中维护一个开放列表候选节点和一个关闭列表已考察节点每次从开放列表里取出估价函数值最小的节点扩展直到终点被取出来。估价函数是f(n) g(n) h(n)g(n)是从起点走到当前节点n已经付出的实际代价h(n)是从当前节点n到终点的启发式估计代价。f(n)就是这整条路径的总代价估计。这里的精髓在于h(n)的设计决定了搜索效率也决定了能不能找到最优解。如果h(n)始终不大于真实代价专业说法叫可采纳性admissibleA星就一定返回最优路径如果h(n)偏大搜索会更快但可能牺牲最优性。三维场景里这个平衡点怎么拿捏我放在后面专门讲。把A星从二维搬到三维最直观的变化是节点定义。二维是(x, y)三维变成(x, y, z)。但对应的概念更重要邻域、代价、启发函数、碰撞检测这四个维度全部要重写。二维的八邻域扩展变成了三维的26邻域甚至更多二维的曼哈顿距离启发函数在三维里会高估代价不满足可采纳性二维的格子是否被占据判断在三维里要小心沿着网格中心走会造成穿墙这类隐性问题。这些我都踩过坑下面逐个拆开讲。2. A星算法原理回顾与三维扩展的核心改动2.1 启发函数的选择为什么曼哈顿距离在三维里会翻车启发函数是整个A星算法里最值得花心思的地方。二维栅格里很多人习惯用曼哈顿距离因为四邻域场景下这样移动的代价非常直白横着走一格代价为1竖着走一格代价为1总代价恰好等于x方向步数加y方向步数。但三维栅格哪怕你只用六邻域上下左右前后曼哈顿距离依然是一种低估型启发函数它可以工作但搜索效率不高——低估意味着开放列表里会有更多节点被反复考察搜索范围膨胀得很厉害。如果允许斜向移动26邻域情况就变了。从(0,0,0)走到(5,5,5)曼哈顿距离是15。实际走对角线路径每步在对角线方向移动一格代价是√3三维空间体对角线走5步代价才8.66。也就是说曼哈顿距离15远远大于真实代价8.66它变成了高估型启发函数破坏了可采纳性A星会放弃最优路径、加速冲向终点——你可能拿到一条好像还行但明显绕了的路径。三维场景正确的启发函数选择是若只允许六邻域移动步长1用三维曼哈顿距离|dx| |dy| |dz|它是可采纳的。若允许26邻域移动含对角线和体对角线用三维欧几里得距离sqrt(dx² dy² dz²)这是最保守的可采纳选择但搜索范围偏大。想兼顾效率可以按移动代价的max值设计max(|dx|, |dy|, |dz|)乘以对角步长代价这在26邻域下是正好等于理想代价的搜索效率最好。我在代码里默认用了三维欧几里得距离代价函数的g(n)也按真实空间距离累计。这样整个算法的逻辑最干净代价单位统一为米启发函数不估高最优性有保证。如果你追求速度可以把h(n)写成max(|dx|, |dy|, |dz|)乘以√3搜索会明显快代价是路径可能长几个百分点工程上经常这么做。2.2 邻域扩展从8邻域到26邻域的取舍二维A星教材里常用的8邻域到了三维对应的是当前格子周围26个格子都算邻居x方向±1共3种取值y方向±1共3种取值z方向±1共3种取值去掉自身后恰好26个。不过这里有个特别容易理解错的地方26邻域并不等于所有邻居的移动代价相同。分三类看沿坐标轴移动的6个邻居面邻居步长为网格分辨率res代价res。沿面对角线移动的12个邻居边邻居步长为res×√2代价按实际空间距离计。沿体对角线移动的8个邻居角邻居步长为res×√3代价按实际空间距离计。有些简化实现为了省事把26个邻居的代价全部设为1或者全部设为res这在二维8邻域里勉强能忍因为只差√2倍放到三维就太粗糙了——√3和1差了1.73倍路径会明显偏好轴向移动导致折线路径绕来绕去。我建议按空间直线距离算代价代码里就一行distance norm(neighborPos - currentNodePos)不费事结果却扎实很多。另一种取舍是如果你的无人机对转弯半径敏感可以进一步限制邻域比如只允许同一高度面的16邻域垂直升降避免出现先斜飞再猛爬升这种对真实飞行器不友好的轨迹。这一步属于应用层面的约束算法层面不必过度设计先把26邻域的搜索实现干净后续加运动学约束再滤波。2.3 三维障碍物建模与碰撞检测的简化处理这一步决定了代码的实用上限。很多人建三维栅格地图很简单zeros(Nx, Ny, Nz)然后把障碍物占据的格子置1。比如要模拟一栋楼就在某个(x, y)范围、z从地面到楼顶的格子全置1。这做起来非常顺手调试可视化也直观——用MATLAB的scatter3或者slice画一下环境一眼就看明白了。但碰撞检测必须比只看当前节点是否在障碍格子里更进一步。A星的搜索过程是沿着网格中心点走的节点本身不穿障碍不代表节点之间的连线不穿障碍。想象一个薄墙只有一格厚起点和终点分别在墙的两侧相邻格A星检查这两个格子都是自由空间直接就把路径连过去了但实际飞行轨迹是穿墙而过的。解决办法是在判断从当前节点扩展到邻居节点这一步时不仅检查邻居格是否自由还要检查两个节点连线经过的所有格子是否自由。三维里做这件事可以用布雷森汉姆直线算法的三维推广或者更简单地在连线上按小步长采样、逐点查占据状态。采样步长取网格分辨率的四分之一到三分之一就够了精度足够且开销可控。我在MATLAB实现里默认用采样检测法两个节点连线长度为L按step res/4采样了大约4个点逐个查询。地图规模不太大时比如100×100×50的体素空间A星主循环里每次扩展做一次采样检测性能完全扛得住。3. MATLAB代码实现数据结构、主流程与关键函数3.1 第一步搭建三维栅格地图我用一个三维数组map3D表示环境0代表自由空间1代表障碍物。数组三个维度的下标分别对应x、y、z方向的网格编号。配套一个结构体env保存元信息网格分辨率res、地图尺寸nx、ny、nz、地图原点坐标origin。为什么单独存原点因为无人机路径规划最终要输出经纬度或局部坐标系下的坐标网格下标只是索引规划完后要换算回实际坐标。这个换算放最后一步做就行。% 初始化地图尺寸和分辨率 nx 100; ny 100; nz 50; res 1; % 每个网格代表1m % 三维地图矩阵 map3D zeros(nx, ny, nz); % 造两个长方体障碍物和一个圆柱障碍简单示意 map3D(20:30, 30:45, 1:25) 1; % 第一个障碍物 map3D(60:75, 50:65, 1:40) 1; % 第二个障碍物 [X, Y] meshgrid(1:nx, 1:ny); cylinderMask ((X - 80).^2 (Y - 25).^2) 100; % 半径10m的圆柱 map3D(:,:,1:20) map3D(:,:,1:20) | repmat(cylinderMask, 1, 1, 20);这里的写法有个小地方要注意MATLAB的meshgrid生成的X、Y矩阵维度与三维数组的维序容易搞混。我建议全程坚持以数组第1维是x、第2维是y、第3维是z这个约定来写画图时再对应到坐标系。如果你用惯imagesc这类二维函数很容易被行列与xy反转坑到三维里这种坑会放大务必统一约定。3.2 第二步节点结构体与开放列表的取舍每一个被搜索的节点需要记录五样东西三维坐标、g值、h值、f值、父节点索引。MATLAB最直观的做法是用结构体数组但结构体数组在频繁增删时性能很差。我建议这样设计用五个单独的数组来存g值、h值、f值、父节点坐标、是否在开放列表/关闭列表的标记。这样虽然代码没那么面向对象但性能至少快一个数量级。% 用三维矩阵直接记录每个格子的搜索状态 gScore inf(nx, ny, nz); % 实际代价 fScore inf(nx, ny, nz); % 估计总代价 parent zeros(nx, ny, nz, 3); % 父节点坐标(x,y,z)数组存3个分量 inOpen false(nx, ny, nz); inClosed false(nx, ny, nz); % 起点初始化 startIdx sub2ind([nx, ny, nz], sx, sy, sz); gScore(sx, sy, sz) 0; fScore(sx, sy, sz) h_func(sx, sy, sz, tx, ty, tz); inOpen(sx, sy, sz) true;关于开放列表的数据结构教科书都会说用优先队列。但如果你的地图是中等规模几十万格以内用每次从inOpen标记的所有节点中找fScore最小者这种朴素线性扫描完全能跑代码也更不容易出错。我第一版就是这么写的100×100×50的地图迷宫类环境跑几秒钟出结果够用了。要做大规模高分辨率地图再用min-heap或者bucket队列优化那是后话先把逻辑跑通。每次找最小f节点我建议存一个openList的线性索引数组避免每次全图扫描bool矩阵。中间的性能对比我放到第4章细说。3.3 第三步A星主循环与邻居扩展核心主循环的伪码逻辑如下直接看MATLAB代码更容易理解% 26个邻域的偏移量 offsets []; for dx -1:1 for dy -1:1 for dz -1:1 if dx 0 dy 0 dz 0 continue; end offsets(end1, :) [dx, dy, dz]; end end end while ~isempty(openList) % 找到f值最小的开放节点 [~, minIdx] min(fScore(openList)); currentIdx openList(minIdx); [cx, cy, cz] ind2sub([nx, ny, nz], currentIdx); % 到达终点则退出 if cx tx cy ty cz tz break; end % 从开放列表移到关闭列表 openList(minIdx) []; inOpen(currentIdx) false; inClosed(currentIdx) true; % 遍历26个邻居 for i 1:26 nx2 cx offsets(i,1); ny2 cy offsets(i,2); nz2 cz offsets(i,3); % 边界检查 if nx2 1 || nx2 nx || ny2 1 || ny2 ny || nz2 1 || nz2 nz continue; end % 障碍物检查 if map3D(nx2, ny2, nz2) 1 continue; end % 连线碰撞检测 if ~isLineCollisionFree(cx, cy, cz, nx2, ny2, nz2, map3D, res) continue; end % 关闭列表检查 if inClosed(nx2, ny2, nz2) continue; end % 计算移动代价 moveCost norm([nx2- cx, ny2 - cy, nz2 - cz]) * res; tentativeG gScore(cx, cy, cz) moveCost; neighborIdx sub2ind([nx, ny, nz], nx2, ny2, nz2); % 如果找到更好的g值更新节点信息 if tentativeG gScore(nx2, ny2, nz2) gScore(nx2, ny2, nz2) tentativeG; hVal h_func(nx2, ny2, nz2, tx, ty, tz); fScore(nx2, ny2, nz2) tentativeG hVal; parent(nx2, ny2, nz2, :) [cx, cy, cz]; if ~inOpen(nx2, ny2, nz2) inOpen(nx2, ny2, nz2) true; openList(end1) neighborIdx; end end end end这里有个细节openList(minIdx) []在MATLAB里删除元素会造成数组重排小地图无所谓大地图会比较慢。替代方案是逻辑删除定期压缩或者记录一个openList数组并且维护一个inOpenLastPos做swap-pop。追求性能时可以考虑但第一版先用直观写法可读性优先。3.4 第四步路径回溯与可视化找到终点后从终点一路查parent回溯到起点得到的节点序列就是规划路径。注意回溯的坐标是网格下标记得乘上res、加上origin换算到实际坐标pathIdx [tx, ty, tz]; while ~(pathIdx(end,1) sx pathIdx(end,2) sy pathIdx(end,3) sz) p parent(pathIdx(end,1), pathIdx(end,2), pathIdx(end,3), :); pathIdx(end1, :) [p(1), p(2), p(3)]; end pathIdx flipud(pathIdx); % 从起点到终点 path (pathIdx - 1) * res origin; % 转换到实际坐标可视化部分我习惯用两个图叠加第一个图用slice或patch画出障碍物表面第二个图用plot3画出路径。再把起点标成绿点、终点标成红点。这样每一帧都能直观看到算法是否绕了路、有没有钻障碍。figure; % 画障碍物表面简化为画占据格子的散点或用isosurface都可以 [fx, fy, fz] ind2sub([nx, ny, nz], find(map3D 1)); scatter3(fx*res, fy*res, fz*res, 1, [0.6 0.6 0.6], .); hold on; plot3(path(:,1), path(:,2), path(:,3), b-, LineWidth, 2); plot3(path(1,1), path(1,2), path(1,3), go, MarkerSize, 10); plot3(path(end,1), path(end,2), path(end,3), r^, MarkerSize, 10); xlabel(x (m)); ylabel(y (m)); zlabel(z (m)); axis equal; grid on;障碍物数量太大时用scatter3会卡这时候改用isosurface提取表面网格性能会好很多但代码相对长一点。工程里我用过一种折中方案只画障碍物外包络的关键切片面比如等间隔取三个高度画二维占据图直观且轻量。4. 算例测试与结果分析同一张地图下的对比4.1 启发函数对比欧几里得距离 vs 对角线距离为了让结论有说服力我在同一张地图上跑了三组实验h0等价于Dijkstra算法、h三维欧几里得距离、hmax(|dx|,|dy|,|dz|)×√3。地图是100×100×50起点在(10,10,5)终点在(90,90,30)中间随机撒了300个大小不一的长方体障碍物。结果很典型启发函数搜索节点数开放列表峰值路径长度m运行时间秒h0Dijkstra49587152.33.8三维欧几里得31824152.32.4对角距离max×√312673156.11.1解读一下欧几里得启发函数没有牺牲路径长度节点数和耗时却下降了四成。对角距离进一步省了一半时间但路径长度多了2.5%。如果你的航线是巡检用的这2.5%无所谓追求响应速度完全可以选对角距离如果你做的是精确物流、每米都得算能耗就老老实实用欧几里得。网格分辨率对运行时间的影响也很大。分辨率从1m变成0.5m体素数量变成8倍A星的搜索节点数往往涨得更猛三维环境路径搜索复杂度近似与体素数量成正比。所以规划超大范围时不要直接加密网格更聪明的做法是两层规划先粗网格规划一条走廊再在走廊内用细网格精修。这个思路跟工程里的分层路径规划完全一致。4.2 邻域碰撞检测的必要性验证我把碰撞检测关掉跑了一次路径长度显著变短短到不真实。打开路径可视化才发现轨迹斜着穿过了好几个薄障碍物的角落。这正好印证了我前面说的那个坑只看节点是否自由、不做线段级碰撞检测A星很容易钻空子。由此得到的路径在仿真里看起来没问题但转成无人机实际航线必撞无疑。碰撞检测的采样步长也有讲究。步长为res时基本没用因为采样点和节点本身重合步长为res/2时已经能挡住多数斜穿我用res/4在测试里表现稳定再加密到res/8并没有明显改善反而让单次扩展耗时翻倍。所以我的建议是采样步长取0.25倍网格分辨率这是性价比很高的经验值。4.3 死胡同环境下的表现A星不是万能钥匙有一次我把地图设置成一个U形障碍物起点在U形内部、终点在外部。A星的行为非常鲜明先把开放列表里的入口方向节点搜个底朝天确认走不通才掉头找U形开口那一侧。这就是A星与贪心算法的本质区别——它宁可慢也要保证能找到路只要确实存在通路。但这个特性也意味着如果地图里有很多凹形障碍物A星的节点数会暴涨。我后来测过把障碍物从规则长方体换成不规则凹形搜索节点数直接翻了2.6倍。这个现象值得记住。真实城区环境里楼群形成的凹坑非常多A星预规划在那种地图上耗时不可控。工程上的对策是栅格地图预处理先把自由空间做形态学膨胀处理缩小窄道减少凹坑或者把地图做分块连通性分析提前排除不可达区域。MATLAB的Image Processing Toolbox里有imdilate函数对三维数组也有效一行代码就能把障碍物膨胀对降低A星搜索压力效果显著。5. 在基础A星上做改进的几条实用路线5.1 双向搜索和JPS跳点搜索的启示三维A星的改进方向其实很清晰。双向A星是首选起点和终点同时向对方搜索每次扩展两个frontier中f值较小的一方相遇时路径就拼接完成。思路上很简单但MATLAB实现有个坑两个方向搜索用的是同一个地图但各自的gScore/parent/inOpen/inClosed要独立维护等于把内存占用翻倍。我试过在中等地图下双向A星能把节点数再省30%左右但代码复杂度上升明显。你要是赶论文或做毕设先把单向的实现搞扎实双向作为加分项。JPSJump Point Search在二维栅格里效果惊艳但三维的JPS实现要比二维复杂得多——需要定义强制邻居的三维判定条件推导的规则集比二维长好几倍。网上有人做过三维JPS但对栅格地图的规则性要求很苛刻不规则障碍物场景下提升有限。我的态度是三维JPS适合当作研究课题去深入不适合急着写进工程代码里。5.2 粗轨迹的后处理平滑三次样条与B样条A星输出的原始路径是沿着网格中心走的折线段即使代价函数用了欧几里得依然有大量锐角拐弯。无人机飞控通常期望轨迹至少是曲率连续的所以路径平滑是三维路径规划里绕不开的一步。我常用的办法是把A星出来的路径节点当作控制点用参数化三次样条做插值再按弧长重新采样。注意直接用spline会过冲可能让平滑后的轨迹重新撞上障碍物。所以平滑后必须再做一次碰撞检查如果有冲突就把那个区域附近的控制点加密重新平滑。这个平滑-检查-加密的迭代过程是我在实际项目里最依赖的套路。% 以路径节点为控制点做B样条拟合 knots 1:length(path); sp spap2(knots, 4, path); % 4阶B样条 smoothedPath fnplt(sp); % 密集采样平滑曲线B样条的好处是局部性——改一个控制点只影响附近一段曲线方便反复迭代修正。三次样条总长更贴近原始折线但全局性强改一处动全身。我偏好B样条。做完平滑再按无人机最大爬升角和最小转弯半径筛选一遍滤掉不可飞的弯道航线基本具备直接给飞控用的条件了。5.3 面对动态环境的局部重规划前文说了A星是全局静态算法但工程上可以用分时重规划的方式让它适应动环境无人机沿预规划航迹飞行时机载传感器探测到前方某个区域新增了障碍物就把当前无人机位置作为新起点、航迹终点不变在局部范围比如前方200米范围内重新执行A星替换掉冲突段航迹。这个方案比重新全图规划快得多也不会破坏全局航迹的合理性。做局部重规划时有个经验新起点的g值不应当从0开始而应该把原全局路径从真正起点到当前位置的累计代价继承下来。这样局部替换后的路径总代价才与全局路径一致不会出现局部最优值看起来比全局还短这种数据矛盾。MATLAB里实现继承只需把新的gScore初始化为该值即可代码改动非常小。6. 我在调试这段代码时踩过的坑和正在做的扩展6.1 三个高频坑死循环、内存暴涨、路径锯齿第一个坑是死循环。新手版代码经常在开放列表为空这个终止条件上忽略处理。当地图有缺陷、起点终点并不连通时开放列表会被搜空程序却还在while里转圈。解决方法是每次循环开头检查isempty(openList)为空就提示没有可行路径直接返回失败状态。这个检查一行代码但能省掉无数排查时间。第二个坑是内存暴涨。三维地图的gScore和fScore用双精度数组存储100×100×50的地图就是50万个元素每个元素8字节两个数组80MB起步。再加parent数组存三维坐标分量内存直奔150MB以上。MATLAB里这已经能感到卡顿了。对策一是尽量用single精度gScore inf(nx,ny,nz,single)内存直接减半对策二是parent不必用三维矩阵存坐标可以压缩成线性索引一个整数就能表达父节点位置回溯时再转坐标。这两个小改动内存量级可以降到一个零头。第三个坑是路径锯齿。前面说了启发函数或者邻域代价设计不当路径会出现先爬升再下降的无意义折返。排查时把fScore和hScore的分布画出来看很直观如果搜索明显集中在一条狭窄走廊里反复试探多半是障碍物膨胀不够或者采样碰撞步长太粗。处理方案是在地图预处理阶段把障碍物膨胀一圈形态学膨胀给路径留出安全余量顺便减少锯齿。6.2 往真实无人机系统延展的接口思路很多时候做完MATLAB仿真下一步就是跟真机对接。市面上常见的做法有两种一是ROS系统里用Python或C复写一遍A星核心逻辑照搬只用改一下地图数据格式二是MATLAB直接生成C代码部署到机载计算机。第二种对算法验证阶段非常友好但要注意A星里的动态数组操作比如删除开放列表元素在生成C代码时可能不支持得提前把数据结构改成堆或固定容量数组。我在做巡检项目时还踩过一个数据格式坑MATLAB仿真环境里的地图坐标系x向右、y向前、z向上和飞控实际使用的NED坐标系x向北、y向东、z向下不统一。路径输出给飞控前必须做坐标变换和单位换算。这个步骤看似简单但忘记做的后果是无人机起飞后直接朝反方向飞。后来我的习惯是所有路径输出都封装成一个独立函数负责把规划结果转成目标坐标系的航点格式任何项目都复用这一个函数。再聊一个正在做的扩展方向把A星和速度规划联合起来。三维路径规划管的是飞过的空间路线但无人机实际飞行还有速度、加速度限制。现在我在仿真里给路径每个航点附带速度约束比如转弯处限速然后基于路径重新做时间维度的速度优化。这个方向改起来不难——A星只管空间速度层单独做——但整体效果非常显著生成的航迹已经能满足真实飞控的几个基本运动学约束了。后续可以再做基于动力学模型的可行性校验那就是另一个深度的话题了。回头看你手头的项目如果目标是做课程设计或毕设我建议把你现有的A星三维代码先完整跑通再挑一个改进点启发函数优化、双向搜索、路径平滑、动态重规划任选其一深做下去加上对比实验工作量就很扎实了。如果在做工程落地记住我上面提到的分层规划、地图膨胀和坐标变换三件事这三点比算法本身的微调更决定最终效果。MATLAB这个生态做三维路径规划确实有独特优势可视化调试顺手矩阵运算快工具箱齐全。把A星的主体逻辑吃透之后你会发现往RRT、RRT*、人工势场法那些算法迁移很多代码结构都能复用——搜索框架是相通的区别主要在于采样策略和代价定义。这篇里给的代码骨架就是替你把这些共性逻辑打好底子接下来就看你的地图长什么样了。