上个月我在给一台六旋翼做电力巡检航线规划时撞上一堵看不见的墙地图上从起点拉到终点的直线航线看起来干干净净实际飞起来却会直接怼进山坡。传统航点规划只处理经纬度坐标高度层是固定值等飞控发现前方地形抬升避障传感器已经来不及反应。这个场景让我彻底放弃二维航线思路转向真正的三维空间搜索最终用A星算法在MATLAB里搭了一套三维路径规划仿真框架跑通了从地图建模、启发式搜索、路径输出到真机航点转换的完整链路。这篇内容分享一下我的完整实现思路覆盖三维环境建模、A星算法的三维扩展原理、MATLAB代码逐段拆解、调参实测数据以及从仿真结果落到真机时容易被忽略的工程细节。适合正在做无人机导航、路径规划算法验证或者准备把A星从二维理解延伸到三维的同学参考。1. 为什么A星算法必须从二维升维到三维1.1 固定高度飞行解决不了真实地形问题无人机实际作业场景中贴着山脊飞、穿树林飞、绕高楼飞都是常态。用二维A星规划时搜索空间是一个平面所有路径点共享同一个高度值。这种假设对平坦城区偶尔够用但遇到起伏地形就立刻露馅地形抬升导致实际飞行高度低于地面直接撞山固定在安全高度层飞行遇到高层建筑或线塔时只能水平绕大圈航时和能耗双高山谷、鞍部等特殊地形无法利用无人机自身的垂直机动优势当时我在巡检航线里遇到的就是第一种情况。山脊附近规划出的直线航线穿过了地形剖面飞行中如果气象条件导致高度保持偏差后果非常严重。二维算法在这个场景里本质上是在逃避高度这个维度而无人机恰恰是少数能自由改变高度的平台不利用这个自由度是巨大浪费。1.2 三维环境建模栅格法与八叉树的取舍进入三维A星之前必须先把环境变成计算机能搜索的数据结构。无人机三维路径规划里最常用的两种是栅格法和八叉树三维栅格法把空间切成等大小的立方体单元每个单元标记自由或障碍。数据组织就是一个三维数组直观、好实现、矩阵运算方便MATLAB里天生适合做这个。八叉树递归分割空间空旷区域用大立方体表示障碍密集区域用小立方体。内存占用小但实现复杂度高节点遍历逻辑繁琐。我建议在MATLAB仿真验证阶段直接用栅格法原因很简单你首先要验证的是算法本身的搜索逻辑而不是环境数据结构的工程性能。A星在三维栅格上的搜索逻辑清晰透明出了bug容易定位可视化也方便。八叉树留到真机嵌入式阶段再考虑那时候内存和实时性才真正构成瓶颈。栅格分辨率是第一个需要拍板的参数。无论取的网格边长是0.5米还是2米只要栅格尺寸小于无人机尺寸加上安全距离路径在物理世界就是可飞的。我当时取的是无人机轴距的1.5倍左右这样路径点间距留出了机动余量又不会因为栅格太细导致搜索节点数量爆炸。1.3 三维代价函数把能耗和威胁写进算法二维A星的代价通常就是路径长度三维场景里这个定义不够用了。无人机在三维空间里移动不同方向运动的代价并不相同。我给每步移动定义了三个代价分量水平距离代价基础代价对应电池能耗中与距离成正比的部分垂直爬升/下降代价无人机爬升的瞬时功率远大于平飞下降时虽然省电但需要控制约束时间综合来看垂直方向运动代价要额外加权威胁区代价经过禁飞区、高压线走廊、雷达探测范围等区域时叠加惩罚值代价函数的数学表达大致是cost_total w1 * d_horizontal w2 * d_vertical w3 * threat_value其中d_horizontal是水平移动距离d_vertical是高度变化量threat_value是当前栅格的威胁等级。w1、w2、w3是权重系数。实测中我把w2设为w1的1.2倍体现爬升能耗更高w3的取值需要根据具体任务标定如果威胁区必须绝对避让可以设成一个远大于其他代价的阈值让A星永远不会选择经过威胁栅格的节点。这一步是整个A星三维化的基石。二维A星只要搜索最短路径三维A星搜索的是综合考虑距离、能耗、安全后的最优路径。代价函数定义得好不好直接决定规划出来的路径能不能在真机上飞。2. A星算法三维扩展的核心机制从8邻域到26邻域2.1 估价函数 f(n) g(n) h(n) 在三维空间里的意义A星算法本身并不绑定维度它只是一个带启发式的优先搜索。核心公式就一行f(n) g(n) h(n)g(n)从起点到当前节点n已经消耗的实际代价h(n)从当前节点n到终点的估计剩余代价f(n)经过节点n的完整路径的总估计代价三维空间里g(n)的计算就是沿着搜索路径把每一段移动代价累加这个很直接。h(n)的设计才是关键它决定了A星是聪明还是笨。h(n)需要满足一个性质可采纳性admissible。也就是说h(n)的估计值必须小于等于实际最小代价。如果h(n)高估了A星会低估某些路径最终可能丢掉最优解。如果h(n)低估太多搜索又失去了方向性会退化成Dijkstra算法——所有方向均匀扩张搜索效率极差。三维空间里最典型的可采纳启发函数是三维欧氏距离h(n) sqrt((nx - gx)^2 (ny - gy)^2 (nz - gz)^2)它计算的是空间直线距离任何实际路径的长度都不可能短于直线距离所以它永远不会高估天然满足可采纳性。2.2 三种启发函数在三维场景下怎么选我在实际项目里对比了三种常见的启发函数结果差距比想象中大启发函数计算方式是否可采纳搜索效率适用场景曼哈顿距离x、y、z三个方向距离之和三维栅格中可采纳很低只允许沿坐标轴移动的场合对角距离考虑对角线移动的代价三维栅格中可采纳中等允许斜向但限制角度欧氏距离空间直线距离始终可采纳高无人机自由飞行的常规场景曼哈顿距离在三维里表现很差。因为它只统计沿坐标轴方向的距离之和低估了斜线移动的可能性启发信息太弱open list里的节点数量会急剧膨胀。用曼哈顿距离跑三维A星搜索空间能从几千个节点膨胀到几万个规划时间成倍增长。欧氏距离在无人机场景下是性价比最高的选择。它既能保证最优性搜索方向又足够聚焦。如果追求更极致的效率可以把h(n)乘以一个权重系数w一般取1.0到2.0之间变成加权A星。这样搜索会更快逼近终点但代价是可能丢弃最优解。关于这个我在第4章的实测部分会详细展开。2.3 邻居扩展策略6邻域、18邻域、26邻域二维A星从当前格子向上下左右和对角方向扩展一共8个邻居。三维空间里这个逻辑要重新设计常见的扩展方式有三种6邻域只向x、y、z轴正负方向移动。搜索出的路径黏在栅格轴线上转弯极其僵硬航迹很长。18邻域6个面邻居加12条边邻居。允许沿空间对角线方向移动路径灵活度大幅提升。26邻域18邻域再加上8个角邻居。无人机可以朝任意相邻栅格移动最贴合自由飞行的运动特性。我在三维搜索里直接用26邻域。理由很简单无人机在三维空间里没有只能沿固定方向飞的约束26个方向的扩展能够覆盖所有可能的相邻位置不至于因为方向限制而丢掉更优路径。但26邻域引入了一个新问题对角线移动的代价怎么算。沿x轴移动一个栅格代价是1沿面对角线移动距离是sqrt(2)沿体对角线移动距离是sqrt(3)。代价计算要是简单取1就会严重低估对角线路径的实际距离导致规划出的路径偏向斜穿。正确做法是代价 距离 * 单位栅格代价距离根据移动方向取1、sqrt(2)或sqrt(3)。这一步看似细节实际上直接影响g(n)的计算精度进而影响整个搜索的路径质量。3. MATLAB仿真框架搭建与核心代码逐段拆解3.1 地图生成随机障碍物加地形曲面地图模块的定位是把环境描述变成三维数组。我的做法分两步走先加随机柱状障碍物再加一个正弦曲面模拟真实地形起伏% 三维栅格地图生成50x50x300自由栅格1障碍栅格 map zeros(50, 50, 30); % 第一步随机柱状障碍物 rng(42); % 固定随机种子保证可重复 num_obs 80; for i 1:num_obs % 随机柱子的底面中心 cx randi([4, 47]); cy randi([4, 47]); % 柱子半径 r randi([2, 4]); % 柱子从地面延伸到此高度 H randi([15, 25]); for dx -r:r for dy -r:r if dx^2 dy^2 r^2 x cx dx; y cy dy; if x 1 x 50 y 1 y 50 map(x, y, 1:H) 1; end end end end end % 第二步正弦起伏地形 for x 1:50 for y 1:50 surface_h 5 4 * sin(x / 9) * cos(y / 9); surface_h max(1, floor(surface_h)); map(x, y, 1:surface_h) 1; end end这段代码生成的地图非常有代表性底层是连续的山体曲面中间层散布着柱状障碍。真实场景中的山体、建筑群、线塔群大致就是这种结构。固定随机种子这点值得强调做算法验证时如果每次运行地图都不同定位问题会非常痛苦。3.2 A星搜索主循环open list、closed list与回溯搜索核心我封装成一个函数输入地图、起点、终点输出路径点序列和扩展节点数。下面是去掉注释后直接可读的核心逻辑function [path, exp_count] astar3d(map, start, goal) [nx, ny, nz] size(map); % 方向向量表26邻域 dirs zeros(26, 3); idx 1; for dx -1:1 for dy -1:1 for dz -1:1 if dx 0 dy 0 dz 0 continue; end dirs(idx, :) [dx, dy, dz]; idx idx 1; end end end % open list用容器存储key是坐标字符串 openList containers.Map(KeyType, char, ValueType, any); closedList containers.Map(KeyType, char, ValueType, any); % 节点转key的工具函数 keyOf (p) sprintf(%d,%d,%d, p(1), p(2), p(3)); % 起点节点初始化 startNode.g 0; startNode.h norm(start - goal); startNode.f startNode.g startNode.h; startNode.pos start; startNode.parent []; openList(keyOf(start)) startNode; exp_count 0; path []; while ~isempty(openList) % 从open list中取f值最小的节点 keys keys(openList); fvals cellfun((k) openList(k).f, keys); [~, minIdx] min(fvals); currKey keys{minIdx}; currNode openList(currKey); remove(openList, currKey); % 到达终点的判断 if norm(currNode.pos - goal) 0.5 path backtrack(currNode); return; end closedList(currKey) currNode; exp_count exp_count 1; % 扩展26个邻居 for d 1:26 np currNode.pos dirs(d, :); % 越界检查 if np(1) 1 || np(1) nx || np(2) 1 || np(2) ny || np(3) 1 || np(3) nz continue; end % 障碍物检查 if map(np(1), np(2), np(3)) 1 continue; end nKey keyOf(np); if isKey(closedList, nKey) continue; end % 移动代价距离乘以栅格代价系数 dist norm(dirs(d, :)); tentative_g currNode.g dist; if isKey(openList, nKey) if tentative_g openList(nKey).g openList(nKey).g tentative_g; openList(nKey).f tentative_g openList(nKey).h; openList(nKey).parent currNode; end else newNode.g tentative_g; newNode.h norm(np - goal); newNode.f newNode.g newNode.h; newNode.pos np; newNode.parent currNode; openList(nKey) newNode; end end end end这个代码有两点值得细说。第一open list我用的是containers.Map加每轮全表扫描取最小值。当节点数过万时这个操作会变成性能瓶颈。MATLAB仿真阶段还能凑合跑但如果你想做更大规模的地图或更频繁的重规划建议改成二叉堆实现。好在MATLAB有heap类支持改起来不算太麻烦。第二回溯路径的backtrack函数本质上是沿着parent指针从终点往起点走反过来输出路径点序列。每个节点都保存了父节点引用这是A星能回溯出完整路径的基础。有些初学者会在节点入closed list之后忘记保存parent导致最后回溯出一段空路径排查起来非常隐蔽。3.3 路径可视化与结果导出三维路径规划的结果不能只看数字必须可视化验证。我用了plot3加scatter3的组合figure; % 绘制障碍物 [obs_x, obs_y, obs_z] ind2sub(size(map), find(map 1)); scatter3(obs_x, obs_y, obs_z, 3, [0.6 0.6 0.6], filled); hold on; % 绘制路径 path path_result; % astar3d返回的路径点 Nx3矩阵 plot3(path(:,1), path(:,2), path(:,3), r-, LineWidth, 2); scatter3(path(:,1), path(:,2), path(:,3), 30, r, filled); % 起点和终点 scatter3(start(1), start(2), start(3), 80, g, filled); scatter3(goal(1), goal(2), goal(3), 80, b, filled); view(45, 30); grid on; xlabel(X / 栅格); ylabel(Y / 栅格); zlabel(Z / 栅格);如果规划出的一条路径从障碍物中间穿过在三维图上会立刻暴露出来。我还有个习惯是输出一条穿地率指标即在路径点里统计有没有点落在地形曲面以下的栅格里用来做自动化回归验证比人眼盯图靠谱得多。4. 仿真结果分析与调参实战4.1 启发函数权重最优性与效率的博弈把启发函数h(n)乘以权重系数w是A星实战里最常用的调参手段。w1是标准A星保证最优路径w1是加权A星搜索更快但路径次优。我在地图上实测了三种权重结果很有代表性权重 w扩展节点数规划耗时秒路径长度栅格单位1.0148236.873.21.352172.475.81.818040.8584.6w1.3的场景下搜索效率提升了近3倍路径长度只损失了3.5%。这个性价比非常高实际项目中我经常直接用1.3作为默认值。w1.8虽然快到0.85秒但路径长度已经涨了15%以上对航时紧张的无人机来说不划算。这个实验说明一个道理三维A星的调参本质是在搜索速度和路径质量之间找一个平衡点没有绝对最优只有任务需求决定的选择。如果无人机航时充裕、任务对路径长度敏感用标准A星如果地图很大、需要快速重规划加权A星更合适。4.2 栅格分辨率带来的维度爆炸三维栅格的节点数随分辨率呈立方级增长。我同一个场景分别用50x50x30和100x100x50两套分辨率跑过地图尺寸栅格总数A星扩展节点规划耗时50x50x3075000约1.5万约7秒100x100x50500000约8万约40秒分辨率翻倍耗时翻近6倍。这不是A星的bug而是三维搜索空间固有的维度爆炸问题。真实无人机地图动辄几公里见方如果要做到1米分辨率节点数能到千万级别纯A星暴力搜索根本不现实。我的建议是分层规划先用低分辨率地图跑一次全局路径找出关键转折点再以转折点为中心在局部用高分辨率地图细化。全局给方向、局部给精度两者结合才能在真机上兼顾实时性和路径质量。这也是很多商用飞控里全局规划局部规划双层的底层逻辑。4.3 动态障碍物与三维A星的配合方式A星本质上是静态规划算法规划出一整条路径后如果环境变了它不会自己调整。真机场景里常见的处理手法是周期性重规划无人机沿着规划路径飞行每间隔一段时间或检测到前方障碍物突变时以当前位置为起点重新跑一遍A星。我在MATLAB里验证过这个逻辑的效果。把地图中的一个柱状障碍物在两次重规划之间随机移动模拟真实环境里的移动吊车或临时施工架。结果是静止A星规划出的路径第一次就会撞上移动障碍而每3秒重规划一次的方案路径虽然会出现蛇形摆动但整体能始终避开障碍物。这说明三维A星在动态环境里并没有失效它只要配上足够频率的重规划机制就能从静态路径规划器升级成动态路径规划器。频率的选择有讲究太频繁计算开销大太稀疏避障跟不上。具体的刷新周期需要结合无人机飞行速度和安全距离倒推一般取安全距离/飞行速度作为参考值。4.4 典型陷阱open list更新逻辑的错误写三维A星时最容易出错的地方不是算法框架而是open list里的节点更新逻辑。我在开发初期踩过一个很隐蔽的坑当一个新的更优路径找到某个已经在open list里的节点时我没有更新它的g值和parent导致最后回溯出来的路径不是最优。这种bug在二维小地图里很难暴露因为次优路径和最优路径差别不大。但在三维地图中路径长度的误差会被放大最终规划出的航线可能莫名其妙多绕一个弯。排查方法很简单在节点更新处打点日志检查每次g值更新是否真的降低了f值。如果发现某节点被反复更新且路径仍不光滑多半就是更新逻辑写错了。5. 从仿真到真机容易被忽略的工程盲区5.1 折线路径必须做平滑处理A星规划出的路径是栅格间的折线直接丢给飞控会触发非常剧烈的偏航指令。真机需要的是一条曲率连续、可跟踪的光滑轨迹。常用的做法是用B样条曲线或者贝塞尔曲线对路径点做平滑。B样条的优点是可以通过调整控制点权重来控制曲线与原始折线的贴合程度不会把路径推离障碍物太远。具体操作时把A星输出的路径点作为B样条的控制点再按一定间隔采样生成新的航点序列。平滑之后必须做一次安全校验检查曲线上是否有采样点落进障碍物区域。因为平滑过程可能把曲线甩进障碍物内部这种情况加一个校正项把曲面向外推即可。5.2 全局规划结果如何变成飞控能执行的航点规划出来的路径点要落地必须转成飞控认识的航点协议。主流开源飞控PX4和ArduPilot都支持MAVLink协议里的MISSION_ITEM消息每条消息包含经纬度、相对高度、航点动作等字段。转换时有一个细节经常被忽略A星在三维栅格里输出的是绝对坐标转成经纬度之前要先做坐标系变换通常是从UTM坐标系或者ENU局部坐标系转换到WGS84经纬度。直接拿栅格坐标送飞控飞控会以为你在另一个星球起降。另外飞控执行航点时默认是直线飞到目标点这意味着如果两个航点之间刚好有个凸起障碍物飞控不会自动规避。所以下发航点的密度不能太稀一般建议在规划出的路径上按安全距离加密采样让飞控沿着加密后的小段航迹飞行而不是长距离直线跨越。5.3 计算实时性与嵌入式移植MATLAB跑三维A星50x50x30的地图已经需要秒级到十秒级时间这个速度在真机上绝对不够。真机上的路径规划模块要么用C实现要么用基于同算法的优化库。移植到真机有几点需要注意地图大小与内存三维数组放进嵌入式设备内存占用很容易超过SDK限制。解决方法是改用稀疏存储只存障碍物信息自由空间用范围描述。open list数据结构C里用std::priority_queue或者自定义二叉堆保持取最小值操作的对数时间复杂度。这一步能省掉的耗时非常可观。多线程设计规划线程和执行线程分离规划过程中飞控仍然按旧航点飞行规划完成后再切换新航点。避免规划中飞控停住这种尴尬局面。我实际用的方案是离线全局规划加在线局部避障起飞前用MATLAB或地面站生成全局参考路径飞行中由机载轻量级避障算法处理突发障碍。全局A星负责大方向局部避障负责细节纠偏两者配合既有全局最优性又保住实时响应。最后再分享一个真实操作中的体会三维路径规划验证阶段一定要把随机地图的种子值固定下来然后做不同算法参数的批量对比实验。否则每次运行地图都不同你很难判断路径变好到底是参数调优的结果还是恰好这张地图更简单。固定种子、批量跑参、可视化对比三件套是调试三维路径规划效率最高的工作流我现在所有规划方案的验证都是这么做的。