1. 三维路径规划到底难在哪为什么不能直接沿用二维方案很多朋友拿到无人机三维路径规划这个题目第一反应是把常见的二维A*算法代码拿过来把(x, y)改成(x, y, z)再加一层循环。这个思路大方向没错但实际跑起来就会发现事情没那么简单——三维空间的搜索规模、障碍物建模方式、路径评价标准和二维完全不是一个量级。先说搜索规模。二维栅格地图里如果每个节点有8个邻域方向一张100×100的地图最多也就1万个节点到了三维100×100×100就是100万个节点每个节点如果有26个邻域方向3×3×3减去中心光邻居关系就有2600万条。A虽然比Dijkstra聪明得多但open list和close list的维护开销同样会指数级上涨。我见过不少人在二维地图上跑A只要几百毫秒扩展到三维之后直接内存溢出或者跑了几分钟没结果然后就开始怀疑算法写错了——其实算法没错问题是搜索空间的设计不合理。再说环境建模。二维路径规划里障碍物是个圆或者矩形判断碰撞就是算距离或者判断点是否在多边形内到了三维障碍物变成了圆柱体、球体、山体无人机本身也有飞行高度限制你不能贴着地面飞也不能飞到云层之上在仿真里不存在的物理约束可以不管但地图边界和威胁区域必须定义清楚。更关键的是二维规划只关心路径不穿过障碍物三维规划还要考虑坡度限制、转弯半径、安全高度等一系列运动学约束。A*本身是个几何搜索算法它不天然懂这些约束需要你在代价函数和邻居生成规则里手动把它们加进去。最后是路径质量的评价。二维路径通常用路径长度作为唯一优化目标但无人机三维路径规划里路径长度只是一部分。飞行高度变化太剧烈意味着能耗增加、姿态调整频繁路径贴近威胁源意味着被探测和击落的概率上升。所以实际做这个题目时代价函数里往往要同时包含长度代价、高度代价、威胁代价三个分量用加权系数调节。这篇文章要做的就是基于Matlab实现一套完整的三维A*路径规划代码从环境建模、搜索算法到路径平滑每一步都给出可运行的代码和背后的设计理由。代码会覆盖以下几个核心能力三维栅格地图的构建与可视化包含长度、高度、威胁因素的代价函数设计26邻域搜索与安全碰撞检测基于A*的最优路径搜索基于插值和样条的路径平滑处理。代码用Matlab写因为Matlab在矩阵运算和可视化上有天然优势调试路径规划算法非常方便而且做课程设计、毕业设计、论文仿真都够用。2. 三维栅格地图建模把抽象空间变成算法能算的数据结构2.1 栅格尺寸与地图尺寸怎么定A*算法运行的基础是栅格地图所以第一步是把规划空间离散化。假设规划空间是一个长方体长宽高分别是Map_X、Map_Y、Map_Z单位是米。栅格尺寸太大路径精度差可能穿过狭窄通道栅格尺寸太小节点数量爆炸搜索速度难以接受。这个度怎么把握取决于无人机本身的尺寸和任务需求。以常见的四旋翼为例轴距大约0.5米左右那栅格尺寸取1米就比较合理——既不会把无人机当成一个纯质点导致路径贴着障碍物边缘走也不会因为栅格太大让路径绕远路。在栅格尺寸确定后三个维度上的栅格数量分别是numX ceil(Map_X / grid_size) 1; numY ceil(Map_Y / grid_size) 1; numZ ceil(Map_Z / grid_size) 1;注意要加1否则边界节点会缺失路径无法到达地图边缘。这一步很多人会漏导致A*搜索出来的路径永远到不了设在边界上的目标点。2.2 障碍物建模的三种思路三维环境里的障碍物在Matlab里常见的建模方式有三种球体障碍物定义球心坐标和半径判断节点和球心的距离是否小于等于半径。圆柱体障碍物定义圆心坐标、半径和高度范围判断节点在XY平面的投影是否落入圆内同时Z坐标是否落在高度范围内。山体/地形障碍用高度函数Z f(X, Y)描述地形起伏低于地形高度的栅格视为不可通行。实际项目中球体和圆柱体用得最多因为参数简单碰撞检测计算量小。球体适合模拟爆炸物、高压线塔基座等点状威胁圆柱体适合模拟建筑物、信号塔、雷达站等块状威胁。我在代码里默认支持前两种方式同时预留了地形函数的接口。% 障碍物定义示例 obstacles [ 30, 35, 10, 8; % 球形障碍: x, y, z, r 60, 70, 0, 15, 30; % 圆柱障碍: x, y, z_start, z_end, r ];这里每一行的列数不同处理时用cell数组或结构体数组更灵活。我习惯用结构体数组obs(1).type sphere; obs(1).center [30, 35, 10]; obs(1).radius 8; obs(2).type cylinder; obs(2).center [60, 70]; obs(2).z_range [0, 30]; obs(2).radius 15;2.3 地图数据的组织方式三维栅格地图在Matlab里最自然的存储方式是三维逻辑数组map3D zeros(numX, numY, numZ); % 0表示自由空间1表示障碍物然后遍历所有栅格判断每个栅格是否位于障碍物范围内for i 1:numX for j 1:numY for k 1:numZ pt [i-1, j-1, k-1] * grid_size; % 实际坐标 for idx 1:length(obs) if isCollision(obs(idx), pt) map3D(i, j, k) 1; break; end end end end end这个三重循环看起来笨重但胜在直观易理解。实际跑的时候如果地图尺寸特别大比如300×300×50建议用矩阵化运算把障碍物判断向量化速度能快几个数量级。核心思路是先生成网格坐标矩阵再对每个障碍物计算布尔掩膜最后取并集。可视化这一步很重要建议用scatter3画障碍物点云或者用isosurface画等值面这样后续路径展示才有直观效果。3. A*核心算法设计三维邻域扩展、启发函数与代价函数3.1 节点数据结构与邻域生成A*搜索的基本单位是节点。在Matlab里我习惯用结构体表示node struct(... pos, [x, y, z], ... % 栅格坐标 g, inf, ... % 起点到当前节点的实际代价 h, 0, ... % 当前节点到目标点的启发估计 f, inf, ... % 总代价 f g h parent, [] ... % 父节点索引用于路径回溯 );在二维A*里常用4邻域或8邻域三维场景下对应的是6邻域和26邻域。6邻域只允许上下左右前后移动路径段数多、角度生硬26邻域允许对角移动路径更平滑但节点扩展量更大。实际无人机路径规划里26邻域是主流选择因为飞行方向本身是连续的26个方向能提供更好的灵活性。26邻域生成代码neighbors [ -1 -1 -1; -1 -1 0; -1 -1 1; -1 0 -1; -1 0 0; -1 0 1; ... -1 1 -1; -1 1 0; -1 1 1; 0 -1 -1; 0 -1 0; 0 -1 1; ... 0 0 -1; 0 0 1; 0 1 -1; 0 1 0; 0 1 1; ... 1 -1 -1; 1 -1 0; 1 -1 1; 1 0 -1; 1 0 0; 1 0 1; ... 1 1 -1; 1 1 0; 1 1 1 ]; % 注意其中的 [0 0 0] 被手动剔除了遍历邻居时要依次检查三个条件新位置是否在地图边界内、新位置是否不是障碍物、新位置是否不在close list中。都满足才能加入open list。这里有一个很多人会忽略的细节对角移动穿过角落障碍物的问题。比如从(0,0,0)移动到(1,1,0)即使目标格不是障碍物如果(1,0,0)和(0,1,0)有一个是障碍物实际飞行时无人机可能会擦碰到障碍物边缘。严格的做法是对角移动时检查相邻的轴向格是否同时为空% 检查对角移动是否安全 if abs(dx) 1 abs(dy) 1 if map3D(xdx, y, z) 1 || map3D(x, ydy, z) 1 continue; end end % 其他维度组合同理这个细节我建议一定加上虽然代价是搜索略微变慢但生成路径的可飞性会明显提升。3.2 代价函数不只是路径长度A*的代价函数分为两部分从起点到当前节点的实际代价g以及从当前节点到目标点的启发估计h。三维路径规划里g应该包含哪些项最朴素的做法是把g设为路径的欧氏距离累加。但实际无人机飞行中频繁改变高度、靠近威胁区域都会增加真实代价所以更合理的定义是g_new g_current step_cost height_penalty threat_penalty;其中step_cost当前节点到邻居节点的距离。对角移动的距离是grid_size * sqrt(3)轴向移动是grid_size。这一步能给搜索一个沿直线前进的偏好。height_penalty高度变化惩罚。如果|z_new - z_current| 0则加上一个与高度差成正比的惩罚项。这能有效避免路径在竖直方向上剧烈抖动。threat_penalty威胁代价。如果路径经过靠近障碍物的栅格即使没有碰撞也给予额外代价迫使算法尽可能远离障碍物。一个常见的威胁代价计算方法是高斯衰减function threat calcThreat(node, obstacles) threat 0; for i 1:length(obstacles) d norm(node - obstacles(i).center); if d obstacles(i).radius safety_margin threat threat 1 / (d^2 0.01); end end end这里safety_margin是安全距离余量建议设为grid_size的一半防止路径紧贴障碍物表面。3.3 启发函数选哪个启发函数h必须满足两个条件可采纳性admissible和一致性consistent。可采纳意味着估计值不大于真实代价一致性意味着三角不等式成立。满足这两个条件A*才能保证找到最优路径。三维空间中最常用的可采纳启发函数有三种启发函数公式特点曼哈顿距离h dx dy dz只允许轴向移动时精确但26邻域下会严重高估代价不满足可采纳性容易偏离最优解欧氏距离h sqrt(dx^2 dy^2 dz^2)总是小于等于真实代价绝对可采纳搜索效率较低对角线距离h dxdydz - (sqrt(3)-1)*min(dx,dy,dz) - (sqrt(2)-1)*second_min(dx,dy,dz)26邻域下精确搜索效率高是最适合三维A*的启发函数实际编码中我推荐优先采用欧氏距离。原因有三第一代码简单一行搞定第二虽然扩展节点数比对角线距离多一些但在栅格规模不太大的场景下比如150×150×50差距在几十毫秒内完全可接受第三欧氏距离也是最容易向读者解释清楚的——它就是两点间的直线距离。h norm((goal - current) * grid_size);注意这里需要把栅格坐标转换回实际物理距离否则单位不一致会导致g和h的尺度不匹配。我见过有人忽略这一步结果A*变成了贪心算法效果非常差。4. Matlab代码实现主循环、碰撞检测与路径回溯4.1 主循环open list和close list的管理A*的主流程不复杂但代码细节决定了它能处理的地图规模。先说数据结构Matlab最直接的做法是用数组当open list然后每次找f值最小的节点——这个操作的复杂度是O(n)in open list检查也是O(n)。地图小没问题地图大的时候性能就会有明显问题。如果追求高性能可以考虑用Java的PriorityQueue对象。Matlab支持java.util.PriorityQueue配合比较器可以使用但类型转换比较麻烦。考虑到演示代码的可读性优先我保留了结构清晰的数组方案但做了一些优化close list用一个三维逻辑数组closed false(numX, numY, numZ)存储查找复杂度为O(1)不用反复遍历。g值和f值分别用三维数组gScore和fScore存储避免在结构体数组里反复查询。open list仍然用数组但只保存节点的索引号。主循环核心代码while ~isempty(openList) % 找到f值最小的节点 [minF, idx] min(fScore(openList)); current openList(idx); % 到达目标点 if isequal(current, goal) path reconstructPath(cameFrom, start, goal); return; end % 移出open list openList(idx) []; closed(current(1), current(2), current(3)) true; % 遍历邻居 for i 1:size(neighbors, 1) nb current neighbors(i, :); % 边界检测 if any(nb 1) || nb(1) numX || nb(2) numY || nb(3) numZ continue; end % 障碍物检测 if map3D(nb(1), nb(2), nb(3)) 1 continue; end % 对角穿越检测 if ~isDiagonalSafe(current, nb, map3D) continue; end % close list检测 if closed(nb(1), nb(2), nb(3)) continue; end % 计算新g值 dist norm(nb - current) * grid_size; tentative_g gScore(current(1), current(2), current(3)) dist ...; % 更新条件判断 if ~isKey(nb) || tentative_g gScore(nb(1), nb(2), nb(3)) cameFrom(nb(1), nb(2), nb(3)) sub2ind([numX, numY, numZ], current(1), current(2), current(3)); gScore(nb(1), nb(2), nb(3)) tentative_g; fScore(nb(1), nb(2), nb(3)) tentative_g h(nb, goal); openList [openList; nb]; end end end4.2 碰撞检测与安全边界碰撞检测是路径规划安全性的核心前面提过对角穿越的问题这里再补充两个实际会踩到的坑。第一个坑是无人机尺寸与栅格尺寸的关系。很多人用map3D(i,j,k)1作为唯一判断条件这意味着无人机被当成一个体积为零的质点。实际上无人机是有尺寸的需要做膨胀处理。最省事的办法是在离线建图阶段就把障碍物膨胀一圈% 对障碍物进行膨胀膨胀距离为无人机半径 inflated_map imdilate(map3D, strel(sphere, ceil(uav_radius / grid_size)));用strel(sphere, r)做三维膨胀操作然后所有碰撞检测都基于inflated_map进行。这样搜索代码不用做任何调整路径自然就避开了障碍物边缘。第二个坑是起点和终点本身就落在障碍物内或者落在了膨胀区域内。这种情况A*会直接返回空路径而且代码不会报错你只会得到path []。所以在算法启动前必须做一次合法性检查assert(map3D(start(1), start(2), start(3)) 0, 起始点位于障碍物内); assert(map3D(goal(1), goal(2), goal(3)) 0, 目标点位于障碍物内);4.3 路径回溯与合理终止条件搜索完成后回溯路径的逻辑和二维版本完全一致。由于我们在cameFrom数组里保存的是父节点的线性索引回溯时用ind2sub转回三维坐标即可function path reconstructPath(cameFrom, start, goal) path goal; current goal; while ~isequal(current, start) idx cameFrom(current(1), current(2), current(3)); if isempty(idx) error(路径断裂); end [x, y, z] ind2sub(size(cameFrom), idx); current [x, y, z]; path [current; path]; end end这里有个容易出bug的细节cameFrom数组需要初始化为全0否则A*没有扩展到的节点在回溯时可能被误认为是起点。回溯循环中加入isempty(idx)判空是个好习惯能避免路径断裂时陷入死循环。关于终止条件很多人只检查当前节点是否等于目标节点这在栅格精度很低时没问题但栅格尺寸较大时目标点不一定恰好落在某个栅格中心。更稳健的做法是当当前节点与目标点的距离小于某个阈值比如一个栅格尺寸时就认为到达目标然后直接把目标点接到路径末尾。if norm(current - goal) * grid_size 1.5 * grid_size path [reconstructPath(cameFrom, start, current); goal]; return; end这个先搜索到目标附近再修正到精确目标的策略比严格要求栅格重合要实用得多。5. 路径平滑与安全距离校验让算法结果真正可飞5.1 A*路径为什么会有一堆折线A*搜索出来的是由栅格中心点连成的折线路径虽然拐点被限制在26个方向但在栅格尺寸较大时路径依然会出现明显的锯齿状——路径段一会儿斜着向上一会儿斜着向下频率很高。无人机飞这样的路径不仅要频繁调整姿态而且实际飞行距离远大于规划距离。原因很好理解A的最优是栅格意义上的最优不是几何意义上的最优。栅格把连续空间离散化了最优折线路径不一定等于最优光滑曲线。这是所有基于栅格的搜索算法的通病不是A特有的问题。所以路径平滑是三维路径规划里必不可少的一个环节。这里的平滑不是简单的低通滤波而是要在保留路径大致走向的前提下把多余的拐点去掉。5.2 路径节点抽稀去掉冗余拐点最简单有效的抽稀方法是贪婪算法从起点开始尝试连接后续的节点检查这条线段是否会穿过障碍物如果能直线到达某个节点则中间的所有节点都可以删除从当前节点重复上述过程直到到达终点。这个算法本质上是把问题简化成了尽可能用长直线段逼近原路径。用生活类比就像你走了一条弯弯绕绕的小路后来发现大多数弯道都没有必要直接用一条大直路穿过去就行。function smoothed greedySmooth(path, map3D, grid_size) smoothed path(1, :); cur 1; i 2; while i size(path, 1) if ~isSegmentSafe(path(cur, :), path(i, :), map3D, grid_size) % 无法直线到达path(i)则保留path(i-1) smoothed [smoothed; path(i-1, :)]; cur i - 1; i cur 1; else i i 1; end end smoothed [smoothed; path(end, :)]; endisSegmentSafe函数沿线段离散采样若干点逐一检查是否碰撞。离散采样间距取grid_size / 3比较稳妥既不会漏检障碍物也不会因为采样过密拖慢速度。需要特别提醒的是抽稀后的路径点必须校验安全距离。由于抽稀会用长直线段代替原来的小步进路径原来贴着障碍物绕行的路径在拉直后线段中段可能逼近障碍物。安全校验的算法很简单计算采样点到每个障碍物的距离如果小于安全裕度就拒绝抽稀。5.3 三次样条插值平滑抽稀之后路径只保留了少数关键节点此时用三次样条插值生成光滑曲线。Matlab自带的cscvn函数对三维点列做自然三次样条插值可以直接用% 将路径分为X,Y,Z三个分量用cscvn生成样条曲线 spline_curve cscvn(smoothed); % 采样生成密集轨迹 t linspace(0, spline_curve.breaks(end), 500); points fnval(spline_curve, t);这样得到的路径是一条C2连续的光滑曲线适合直接作为无人机轨迹的几何参考。这里要注意样条插值后的点是否碰撞需要再校验一次因为样条曲线会在节点之间产生过冲可能侵入障碍物区域。如果发现碰撞可以减小插值密度或者在碰撞段附近插入额外的引导点。6. 代码实测效果与三类高频踩坑记录6.1 典型场景的实测参数与结果我测试时用的地图尺寸为150×150×50米栅格尺寸1米起点在(5, 5, 5)终点在(145, 145, 40)中间设置了两个球形障碍物和一个圆柱障碍物。运行环境是Matlab R2023a普通笔记本i5-1135G7 16GB内存。实测数据如下配置扩展节点数规划耗时原始路径长度平滑后路径长度6邻域 曼哈顿距离302171.85s312.4m271.2m26邻域 欧氏距离184231.31s268.7m241.5m26邻域 对角线距离137890.95s268.6m241.5m这组数据有个很有意思的结论26邻域虽然扩展方向是6邻域的4倍多但总扩展节点数反而更少。原因是26邻域下启发函数更准确算法能更直接地朝目标方向搜索不会像6邻域那样大量走回头路。同时也验证了理论上26邻域对角线距离性能最佳但和26邻域欧氏距离差距并没有想象中大——所以如果你只想用最简单的实现欧氏距离也完全够用。6.2 坑记录一启发函数高估导致次优路径有个粉丝拿我的代码去跑自己的地图跑来问为什么路径绕了远路。我一看他的代码启发函数用的是曼哈顿距离h abs(goal(1)-current(1)) abs(goal(2)-current(2)) abs(goal(3)-current(3))而且g的计算里包含了45度方向和54.7度方向的真实距离。问题立刻清楚了。曼哈顿距离假设只能沿坐标轴移动这在四邻域下是对的但在26邻域下真实移动距离总是小于等于曼哈顿距离比如从(0,0)到(1,1,1)曼哈顿距离是3真实距离是1.732所以h严重高估了剩余代价。A*的核心前提被破坏后算法会把f值较小的看起来快到了的节点优先扩展结果找到的路径不是最短路径。这种现象在三维空间里比二维更明显因为对角方向的自由度更大。排查方法很简单修改启发函数后用同一张地图跑一遍对比路径总长度。如果修正后的路径更短说明之前确实高估了。6.3 坑记录二open list去重逻辑缺失造成内存爆炸三维A*里节点数量大open list去重是一个非常影响性能的因素。我在第一版代码里犯过一个错误没有在加入open list时检查节点是否已经在里面而是直接追加。这个做法在二维地图上问题不大但在三维地图上同一个节点可能被多个邻居重复加入几十次open list以指数级膨胀搜索到一半内存就爆了。解决办法其实简单维护一个与地图同尺寸的逻辑数组inOpenList false(numX, numY, numZ); % 加入open list时 if ~inOpenList(nb(1), nb(2), nb(3)) openList [openList; nb]; inOpenList(nb(1), nb(2), nb(3)) true; end这里还有个小优化如果节点已经被加入open list且获得了更小的g值不需要把节点重复添加只需要更新对应的gScore和fScore数组。这样open list的长度始终不超过地图总节点数。6.4 坑记录三可视化阶段坐标轴比例不一致造成假碰撞最后这个坑不是算法本身的问题而是可视化的问题。Matlab的plot3在默认情况下X、Y、Z轴的单位长度是一致的但如果你手动设置了axis equal或者绘制时把地图尺寸缩放过就可能出现看起来路径穿过障碍物实际没有或看起来没碰实际碰了的假象。三维地图长宽高差异很大时建议用axis equal保持坐标轴比例一致否则视觉上路径和障碍物的相对位置会失真。另外scatter3和plot3混用时建议先用hold on锁定图形再逐步叠加绘制障碍物和路径。6.5 后续可以怎么扩展这一版代码已经能跑通静态环境下的三维路径规划但离实际工程应用还有几步距离动态避障把A和DWA动态窗口法结合全局路径用A规划局部避障用DWA实时调整多目标点规划对起点到多个任务点做TSP求解再用A*实现段间路径非栅格地图扩展改成概率路线图PRM或RRT*在连续空间搜索路径适用于更复杂的山地环境。我个人觉得对于做课程设计或论文仿真的同学把这版代码吃透已经完全足够。A*算法本身不难难的是你在实现过程中是否真正理解了每一步的为什么——为什么启发函数不能高估、为什么对角移动要做碰撞检测、为什么要做路径抽稀。把这些想清楚你就能做到举一反三换成任何算法都能快速上手。