简介A算法在三维空间中的航路规划MATLAB实现源码面向路径规划学习者、无人机与机器人导航方向的开发者和研究者针对3D环境中计算复杂、障碍物多维表示与碰撞检测等问题提供一套可运行的启发式搜索解决方案。压缩包共16个文件以8个.m源码文件为核心覆盖空间网格化、A主循环、启发式代价估计、邻域扩展与最优路径回溯等模块另含2个.mat格式的地图与地形数据、1份详细说明文档及3张辅助示意图整体约883KB轻量且结构清晰适合直接阅读和二次开发。已有593人学习/下载。通过这份资源可以完整理解A*算法在三维栅格模型上的落地流程包括FGH代价迭代、障碍物避让和路径平滑输出借助自带地形数据与可视化结果可快速复现演示也可在此基础上针对不同启发函数或搜索策略开展对比实验为无人机航路规划等实际工程场景提供算法基础。1. 三维航路规划为什么值得自己实现一次Astar拿到的这份 Astar_3Der.zip 是很典型的 MATLAB 课程级实现主题是 Astar 算法做航路 3D 规划。不是二维栅格的简化版节点坐标直接落在三维空间里配合 TerrainData.mat 这类地形高程数据和 MakeData.m 生成的三维场景跑出的路径能直接用于无人机航线预研和机器人导航验证。适合正在学路径规划的硕士生也适合想快速验证三维寻路思路的工程师。A* 在二维地图上选路很容易理解一旦把高度维加进来节点扩展方式、启发式函数设计、碰撞检测范围都会同步变化。这套代码的价值在于把三维寻路的完整链路——地图生成、算法主循环、节点管理、结果可视化——都摊开放在你面前你可以顺着调用链看明白每个环节也可以直接改参数观察路径变化。拿到代码先在 MATLAB 里把 Main.m 跑通再看 A_star.m 里每一轮迭代发生了什么这套代码能给你讲清楚的细节比大多数只放二维示例的教程多得多。2. 拆文件Main.m、A_star.m 与辅助函数的调用链2.1 从压缩包结构看设计思路解压之后能看到的文件不多但每一个都在路径规划流程里有明确位置。先按职责分个组文件职责调用关系Main.m程序入口配置起点终点、加载地图、启动搜索并调用可视化顶层脚本MakeData.m生成三维场景数据产出 TerrainData.mat 与 MapData.mat独立运行结果供 Main 使用A_star.m算法主循环管理 open 列表与 closed 集合的迭代被 Main.m 调用min_fn.m从 open 列表中选出 F 值最小的节点被 A_star.m 调用expand_array.m对当前节点做邻域扩展生成可供搜索的相邻节点集合被 A_star.m 调用insert_open.m将新节点写入 open 列表并维护节点索引被 A_star.m 调用node_index.m根据节点坐标返回其在 open 列表中的索引位置被 expand_array 与 A_star 协同使用distanced.m计算节点间的三维欧几里得距离作为启发式代价与移动代价基础被多个函数引用Result 文件夹存放运行结果图或航迹数据由 Main.m 导出这个结构是很标准的「入口 算法 辅助函数」三层模式。Main.m 做成脚本而不是函数说明设计时是面向实验场景而不是工程集成。这种做法的好处是你改起参数来非常直接坏处是一旦搜索空间变大所有变量都堆在工作区里内存占用会变得不可控。对于学习和验证来说这个代价可以接受。2.2 主流程的执行顺序跑通的顺序应该是这样先运行 MakeData.m 生成地形数据然后运行 Main.m。Main.m 内部做的事情基本可以概括为从 MapData.mat 和 TerrainData.mat 加载地图环境定义起点、终点坐标初始化 open 列表然后进入 A_star.m 的循环最后把路径画出来。打开 Main.m 你会看到类似下面的结构% 加载三维地图数据 load(TerrainData.mat); load(MapData.mat); % 定义起点与终点索引坐标 start [10, 10, 5]; goal [45, 40, 25]; % 初始化 open 列表第一行存节点自身状态 open_list [1; start(1); start(2); start(3); 0; heuristic(start, goal); 0]; closed_list []; % 调用 A* 主函数 [path, node_count] A_star(start, goal, MapData, TerrainData); % 可视化为三维航迹 plot3(path(:,1), path(:,2), path(:,3), r-, LineWidth, 2);load 语句负责把三维地形栅格和障碍物标记读进工作区open 列表的初始化格式决定了后面所有函数读写的列顺序这点非常关键。观察这份代码时先确认每一列分别存的是什么——常见约定是 x、y、z 坐标、G 值、H 值、F 值、父节点索引。你在阅读 insert_open.m 和 min_fn.m 时要时刻带着这个列约定否则很容易把节点坐标和代价字段搞混。2.3 一个容易忽略的调用细节MakeData.m生成的数据格式直接影响 A_star.m 中的栅格判定逻辑。常见的设计是用一个三维 0/1 矩阵表示可通行与不可通行1 代表障碍物0 代表自由空间。但地形数据里通常存的是高度值不是障碍标记判断一条路径是否穿山不是查一个点是否在障碍物里而是要判断这个点的 Z 坐标是否低于该 XY 位置的地形高度。这两者在代码实现上有本质区别前者是查表后者是逐点比较。我一开始读这份代码时踩过这个坑MapData.mat 里存的是障碍物栅格TerrainData.mat 里存的是地形高程。如果你在扩展节点时只查了 MapData 而忽略了 TerrainData规划出来的路径会在视觉上穿过山峰但代码本身不报错。后面第 5 章会讲怎么设计一个验证环节专门抓这种问题。% 检查节点是否在山体内部 function is_collision check_collision(node, MapData, TerrainData) % 先查障碍物栅格 if MapData(node(1), node(2), node(3)) 1 is_collision true; return; end % 再查地形高度如果节点高度低于地表高度则撞山 if node(3) TerrainData(node(1), node(2)) is_collision true; return; end is_collision false; end这个双重判定是三维航路和二维栅格最关键的区别。二维路径只要避开关闭网格三维路径必须同时处理空间障碍与地形约束。这段逻辑在后续分析 expand_array.m 时也要反复用到因为邻域扩展的每个候选节点最终都要经过这一关。3. 三维节点扩展、启发式函数与检测逻辑3.1 26 邻域扩展的空间代价A* 在二维栅格中通常做 4 邻域或 8 邻域扩展三维场景下自然对应 6 邻域和 26 邻域。这份代码在 expand_array.m 里使用的是 26 邻域也就是对当前节点的 x、y、z 三个方向各做 ±1 偏移组合出 26 个候选位置再逐一过滤掉越界、在 closed 列表内、撞障碍物的节点。一个典型的 26 邻域生成逻辑长这样function expanded expand_array(current, closed_list, MapData, TerrainData) expanded []; % 三组偏移x、y、z 各取 -1, 0, 1 for dx -1:1 for dy -1:1 for dz -1:1 if dx 0 dy 0 dz 0 continue; % 跳过自身 end neighbor current(1:3) [dx, dy, dz]; % 越界检查 if any(neighbor 1) || neighbor(1) size(MapData,1) ... || neighbor(2) size(MapData,2) ... || neighbor(3) size(MapData,3) continue; end % 碰撞检查 if check_collision(neighbor, MapData, TerrainData) continue; end % 已在 closed 列表中则跳过 if is_in_closed(neighbor, closed_list) continue; end expanded [expanded; neighbor]; end end end end这里的循环看起来直接但在 MATLAB 里逐条 append 到 expanded 上效率很低节点规模上万后延迟会很明显。常见优化是先预分配一个 26 行三列的零矩阵用计数器写入最后截断多余行。这个技巧可以用在这段代码上不需要改动算法逻辑但能把扩展阶段的时间压掉 20% 以上。3.2 启发式函数选型欧氏距离与 octile 距离的取舍这份代码里启发式函数用的是 distanced.m 计算的三维欧几里得距离。三维空间下欧氏距离是最直观的估计它始终满足可采纳性也就是不会高估到目标的实际代价所以用它做启发式能够保证找到最优路径。但要注意的是欧氏距离只对完全自由的空间精确。在 26 邻域中真实移动代价可能是 1轴向移动、sqrt(2)面对角移动或者 sqrt(3)体对角移动欧氏距离作为启发式此时依然可采纳但信息量偏低搜索会扩展较多无效节点。如果想让搜索更快可以把启发式替换成三维 octile 距离function h octile_distance(node, goal) dx abs(node(1) - goal(1)); dy abs(node(2) - goal(2)); dz abs(node(3) - goal(3)); % 三维 octile 距离轴向 1面对角 sqrt(2)体对角 sqrt(3) h (dx dy dz) (sqrt(2) - 2) * min(dx, dy) ... (sqrt(3) - sqrt(2)) * min(max(dx, dy), dz); endoctile 距离在邻域扩展允许对角移动时比欧氏距离更贴近真实代价搜索收敛更快。代价是它不再严格可采纳最终路径可能比最优路径长 1% 到 3%。具体怎么选取决于你的场景无人机在开阔空域飞行欧氏距离就够在密集城市环境或地形起伏大的区域建议用 octile 距离减少无效搜索。改法也很简单把 A_star.m 里调用 distanced.m 的那行替换为 octile_distance 即可。3.3 G 值的移动代价设计G 值代表从起点到当前节点的实际已付代价。在三维 26 邻域中移动代价取决于当前节点与父节点的相对方向。轴向移动代价取 1面对角移动取 sqrt(2)体对角移动取 sqrt(3)这样才能让斜向移动不会被白白占便宜。function g_cost move_cost(parent, current) diff abs(current(1:3) - parent(1:3)); if sum(diff) 1 g_cost 1; % 轴向移动 elseif sum(diff) 2 g_cost sqrt(2); % 面对角移动 else g_cost sqrt(3); % 体对角移动 end end这里要提醒一个容易忽略的点——Z 轴的移动代价应该比其他轴更高。现实中无人机改变高度需要额外的能量消耗机器人在斜坡上移动也有额外代价。很多三维 A* 实现把 Z 轴当作和其他轴一模一样的维度处理这会让规划出的航迹频繁上下起伏。如果你想得到更符合动力学特性的路径可以把轴向代价改为 dx1、dy1、dz1.2然后在路径平滑阶段做补偿。3.4 一个关于数据类型的坑注意 TerrainData.mat 和 MapData.mat 在读取时的数据类型。如果它们存的是单精度浮点MATLAB 的 find 函数和逻辑索引行为会和双精度一样但内存占用减半。如果存的是 uint8在检查 MapData(node(1), node(2), node(3)) 1 时不会有问题但一旦参与矩阵运算或与地形高度做比较MATLAB 会隐式转换类型带来额外的时间开销。我在处理大规模地图时通常会在 load 之后统一转成 double虽然内存翻倍但后续的逐点比较和插值计算会更省时间。4. 网格粒度、权重与 open 列表的性能取舍4.1 三维空间复杂度带来的边界三维 A* 的计算量随栅格规模呈立方增长。一个 100x100x30 的栅格就有 30 万个节点每个节点扩展 26 个邻域一轮搜索可能要访问百万级节点。这比二维 100x100 栅格多了两个数量级。因此在改动这份代码之前务必要先量化地图规模。地图规模节点总数26 邻域扩展量级建议用途50x50x2050000130 万算法验证、教学演示100x100x30300000780 万小型无人机任务区200x200x4016000004160 万工程级预研需配合优化500x500x100250000006.5 亿必须改用分层或采样式规划这份代码默认的 TerrainData 规模大致在几十万节点量级直接跑没有问题。但如果把 Main.m 里的地图换成分辨率更高的数据第一步要感受的变化就是 min_fn.m 的扫描开销会突增见 4.2。4.2 min_fn 的线性扫描是隐形瓶颈min_fn.m 负责从 open 列表中找出 F 值最小的节点。常见实现如下function i_min min_fn(open_list) f_values open_list(6, :); % 假设第 6 行存 F 值 [~, i_min] min(f_values); end看起来这段代码足够简单高效min 函数在 MATLAB 里是高度优化的 C 实现扫描百万级数组很快。但问题不在 min 本身而在 open 列表的动态增删。每次迭代结束后当前节点要从 open 列表中删除新节点要由 insert_open.m 添加。MATLAB 矩阵的删除和拼接都是整体拷贝操作数组越大单次增删的成本越高。搜索结果跑几百上千次迭代后open 列表的维护时间会远超 min_fn 的计算时间。三个实用的解决方向用稀疏矩阵维护 closed 列表比逐行 ismember 判断快一个量级。把 open 列表预分配大数组用计数器追踪有效长度避免每次拼接新行。如果节点规模超过百万放弃 MATLAB 自带数据结构直接用 Java 的 PriorityQueue 接口。% 使用 Java PriorityQueue 维护 open 列表MATLAB 内嵌 Java pq java.util.PriorityQueue(); pq.add([f_value, node_id]); while ~pq.isEmpty() item pq.poll(); % 取出 F 值最小的节点 f_val item(1); node_id item(2); endMATLAB 对 Java 类有原生支持这块是很多做算法验证的人不知道或者不用的。Java PriorityQueue 的插入和弹出都是 O(log n)和 min_fn 线性扫描的 O(n) 相比节点规模越大优势越明显。缺点是要自己处理节点与队列元素的映射关系代码可读性稍有下降。4.3 启发式权重的调整空间很多人在跑通这份代码后会想加快搜索速度最简单的方式是在启发式函数前乘一个权重系数 w。当 w1 时是可采纳的 A*保证最优路径w1 时变成 Weighted A*搜索速度提升路径质量下降。这份代码没有内置权重参数但改起来非常容易% 在计算 F 值时引入权重 f g 1.2 * h;权重取 1.2 到 1.5 之间时大多数场景路径长度只会增加 2% 到 5%但扩展节点数可能减少 40% 以上。如果你做的是实时航线重规划这个改动几乎是无本万利。如果做离线最优路径权重保持 1.0 即可。一个更细的技巧是动态权重搜索初期用较大权重快速接近目标当 open 列表中最小 F 值连续多轮不变时把权重逐步降到 1.0保证终点附近的搜索精度。这样可以在保留 A* 最优性的同时缩短前期搜索时间。% 动态权重示例 if iteration 200 w 1.5; elseif min_f_no_change_count 50 w 1.0; end4.4 实际改动的效果判断改完参数后建议在 Result 文件夹里存一张路径总长与迭代次数的记录表每次调整参数时对比这两个数字。路径总长用 distanced.m 对路径逐段累加即可迭代次数在 A_star.m 的主循环里加一个计数器。如果你发现路径长度没变但迭代次数大幅减少说明启发式权重调对了方向。如果路径表面出现明显的锯齿说明权重过大搜索过度偏向启发式估计而忽视了实际地形约束。5. 路径平滑、碰撞复核与三维航迹验证5.1 对 A* 输出路径做平滑A* 搜索出来的路径是栅格节点连线在三维地形上看起来会有明显的折线感尤其在坡度变化大的区域。我一般会用三次样条对路径做平滑让航迹连续可飞% 对路径的 x、y、z 分量分别做样条插值 t 1:size(path, 1); tt linspace(1, size(path, 1), 500); sx spline(t, path(:, 1), tt); sy spline(t, path(:, 2), tt); sz spline(t, path(:, 3), tt); smooth_path [sx; sy; sz];样条平滑有一个风险插值点可能落在山体内部尤其是在路径急转弯处。所以平滑后必须做碰撞复核逐点检查每个插值点是否满足 check_collision 条件。% 逐点复核平滑路径是否安全 safe true; for i 1:size(smooth_path, 1) node round(smooth_path(i, :)); if check_collision(node, MapData, TerrainData) safe false; fprintf(平滑路径在点 %d 处穿越障碍\n, i); break; end end如果出现碰撞不要把样条阶数降太低更好的做法是先用 A* 的原始路径分段做样条只在安全区间内平滑转弯点附近保留原节点。5.2 生成可交互的三维可视化MATLAB 的 plot3 能显示航迹但没有真实地形的对比很难判断路径质量。把地形面绘制在同一坐标系下% 绘制三维地形表面 [X, Y] meshgrid(1:size(TerrainData,1), 1:size(TerrainData,2)); surf(X, Y, TerrainData); alpha(0.5); % 半透明显示 colormap(jet); hold on; plot3(path(:,1), path(:,2), path(:,3), w-, LineWidth, 2);半透明地形上叠加白色航迹可以直观看到路径是否沿着山谷走、是否贴着山峰绕行、是否存在无意义的上下起伏。这一步比任何参数指标都更能暴露问题。5.3 航迹质量量化验证除了可视化还要算三个量化指标来横向比较参数变更路径总长度、总爬升量、平均离地高度。总爬升量很容易算对路径 Z 坐标做差分把所有正值加起来就是累计爬升。这个数字对无人机航线尤其重要同样的路径长度爬升越少能耗越低。平均离地高度则反映飞行安全性离地越近越危险。把这三个指标写进 Result 文件夹下的一个文本或表格里以后每次调参都有据可查。这样处理之后这套代码就不再是跑一次就忘的课程作业而是可以持续对比参数效果的三维航线验证平台。本文还有配套的精品资源点击获取 三维航路规划A*算法MATLAB代码拆解与调优指南
简介A算法在三维空间中的航路规划MATLAB实现源码面向路径规划学习者、无人机与机器人导航方向的开发者和研究者针对3D环境中计算复杂、障碍物多维表示与碰撞检测等问题提供一套可运行的启发式搜索解决方案。压缩包共16个文件以8个.m源码文件为核心覆盖空间网格化、A主循环、启发式代价估计、邻域扩展与最优路径回溯等模块另含2个.mat格式的地图与地形数据、1份详细说明文档及3张辅助示意图整体约883KB轻量且结构清晰适合直接阅读和二次开发。已有593人学习/下载。通过这份资源可以完整理解A*算法在三维栅格模型上的落地流程包括FGH代价迭代、障碍物避让和路径平滑输出借助自带地形数据与可视化结果可快速复现演示也可在此基础上针对不同启发函数或搜索策略开展对比实验为无人机航路规划等实际工程场景提供算法基础。1. 三维航路规划为什么值得自己实现一次Astar拿到的这份 Astar_3Der.zip 是很典型的 MATLAB 课程级实现主题是 Astar 算法做航路 3D 规划。不是二维栅格的简化版节点坐标直接落在三维空间里配合 TerrainData.mat 这类地形高程数据和 MakeData.m 生成的三维场景跑出的路径能直接用于无人机航线预研和机器人导航验证。适合正在学路径规划的硕士生也适合想快速验证三维寻路思路的工程师。A* 在二维地图上选路很容易理解一旦把高度维加进来节点扩展方式、启发式函数设计、碰撞检测范围都会同步变化。这套代码的价值在于把三维寻路的完整链路——地图生成、算法主循环、节点管理、结果可视化——都摊开放在你面前你可以顺着调用链看明白每个环节也可以直接改参数观察路径变化。拿到代码先在 MATLAB 里把 Main.m 跑通再看 A_star.m 里每一轮迭代发生了什么这套代码能给你讲清楚的细节比大多数只放二维示例的教程多得多。2. 拆文件Main.m、A_star.m 与辅助函数的调用链2.1 从压缩包结构看设计思路解压之后能看到的文件不多但每一个都在路径规划流程里有明确位置。先按职责分个组文件职责调用关系Main.m程序入口配置起点终点、加载地图、启动搜索并调用可视化顶层脚本MakeData.m生成三维场景数据产出 TerrainData.mat 与 MapData.mat独立运行结果供 Main 使用A_star.m算法主循环管理 open 列表与 closed 集合的迭代被 Main.m 调用min_fn.m从 open 列表中选出 F 值最小的节点被 A_star.m 调用expand_array.m对当前节点做邻域扩展生成可供搜索的相邻节点集合被 A_star.m 调用insert_open.m将新节点写入 open 列表并维护节点索引被 A_star.m 调用node_index.m根据节点坐标返回其在 open 列表中的索引位置被 expand_array 与 A_star 协同使用distanced.m计算节点间的三维欧几里得距离作为启发式代价与移动代价基础被多个函数引用Result 文件夹存放运行结果图或航迹数据由 Main.m 导出这个结构是很标准的「入口 算法 辅助函数」三层模式。Main.m 做成脚本而不是函数说明设计时是面向实验场景而不是工程集成。这种做法的好处是你改起参数来非常直接坏处是一旦搜索空间变大所有变量都堆在工作区里内存占用会变得不可控。对于学习和验证来说这个代价可以接受。2.2 主流程的执行顺序跑通的顺序应该是这样先运行 MakeData.m 生成地形数据然后运行 Main.m。Main.m 内部做的事情基本可以概括为从 MapData.mat 和 TerrainData.mat 加载地图环境定义起点、终点坐标初始化 open 列表然后进入 A_star.m 的循环最后把路径画出来。打开 Main.m 你会看到类似下面的结构% 加载三维地图数据 load(TerrainData.mat); load(MapData.mat); % 定义起点与终点索引坐标 start [10, 10, 5]; goal [45, 40, 25]; % 初始化 open 列表第一行存节点自身状态 open_list [1; start(1); start(2); start(3); 0; heuristic(start, goal); 0]; closed_list []; % 调用 A* 主函数 [path, node_count] A_star(start, goal, MapData, TerrainData); % 可视化为三维航迹 plot3(path(:,1), path(:,2), path(:,3), r-, LineWidth, 2);load 语句负责把三维地形栅格和障碍物标记读进工作区open 列表的初始化格式决定了后面所有函数读写的列顺序这点非常关键。观察这份代码时先确认每一列分别存的是什么——常见约定是 x、y、z 坐标、G 值、H 值、F 值、父节点索引。你在阅读 insert_open.m 和 min_fn.m 时要时刻带着这个列约定否则很容易把节点坐标和代价字段搞混。2.3 一个容易忽略的调用细节MakeData.m生成的数据格式直接影响 A_star.m 中的栅格判定逻辑。常见的设计是用一个三维 0/1 矩阵表示可通行与不可通行1 代表障碍物0 代表自由空间。但地形数据里通常存的是高度值不是障碍标记判断一条路径是否穿山不是查一个点是否在障碍物里而是要判断这个点的 Z 坐标是否低于该 XY 位置的地形高度。这两者在代码实现上有本质区别前者是查表后者是逐点比较。我一开始读这份代码时踩过这个坑MapData.mat 里存的是障碍物栅格TerrainData.mat 里存的是地形高程。如果你在扩展节点时只查了 MapData 而忽略了 TerrainData规划出来的路径会在视觉上穿过山峰但代码本身不报错。后面第 5 章会讲怎么设计一个验证环节专门抓这种问题。% 检查节点是否在山体内部 function is_collision check_collision(node, MapData, TerrainData) % 先查障碍物栅格 if MapData(node(1), node(2), node(3)) 1 is_collision true; return; end % 再查地形高度如果节点高度低于地表高度则撞山 if node(3) TerrainData(node(1), node(2)) is_collision true; return; end is_collision false; end这个双重判定是三维航路和二维栅格最关键的区别。二维路径只要避开关闭网格三维路径必须同时处理空间障碍与地形约束。这段逻辑在后续分析 expand_array.m 时也要反复用到因为邻域扩展的每个候选节点最终都要经过这一关。3. 三维节点扩展、启发式函数与检测逻辑3.1 26 邻域扩展的空间代价A* 在二维栅格中通常做 4 邻域或 8 邻域扩展三维场景下自然对应 6 邻域和 26 邻域。这份代码在 expand_array.m 里使用的是 26 邻域也就是对当前节点的 x、y、z 三个方向各做 ±1 偏移组合出 26 个候选位置再逐一过滤掉越界、在 closed 列表内、撞障碍物的节点。一个典型的 26 邻域生成逻辑长这样function expanded expand_array(current, closed_list, MapData, TerrainData) expanded []; % 三组偏移x、y、z 各取 -1, 0, 1 for dx -1:1 for dy -1:1 for dz -1:1 if dx 0 dy 0 dz 0 continue; % 跳过自身 end neighbor current(1:3) [dx, dy, dz]; % 越界检查 if any(neighbor 1) || neighbor(1) size(MapData,1) ... || neighbor(2) size(MapData,2) ... || neighbor(3) size(MapData,3) continue; end % 碰撞检查 if check_collision(neighbor, MapData, TerrainData) continue; end % 已在 closed 列表中则跳过 if is_in_closed(neighbor, closed_list) continue; end expanded [expanded; neighbor]; end end end end这里的循环看起来直接但在 MATLAB 里逐条 append 到 expanded 上效率很低节点规模上万后延迟会很明显。常见优化是先预分配一个 26 行三列的零矩阵用计数器写入最后截断多余行。这个技巧可以用在这段代码上不需要改动算法逻辑但能把扩展阶段的时间压掉 20% 以上。3.2 启发式函数选型欧氏距离与 octile 距离的取舍这份代码里启发式函数用的是 distanced.m 计算的三维欧几里得距离。三维空间下欧氏距离是最直观的估计它始终满足可采纳性也就是不会高估到目标的实际代价所以用它做启发式能够保证找到最优路径。但要注意的是欧氏距离只对完全自由的空间精确。在 26 邻域中真实移动代价可能是 1轴向移动、sqrt(2)面对角移动或者 sqrt(3)体对角移动欧氏距离作为启发式此时依然可采纳但信息量偏低搜索会扩展较多无效节点。如果想让搜索更快可以把启发式替换成三维 octile 距离function h octile_distance(node, goal) dx abs(node(1) - goal(1)); dy abs(node(2) - goal(2)); dz abs(node(3) - goal(3)); % 三维 octile 距离轴向 1面对角 sqrt(2)体对角 sqrt(3) h (dx dy dz) (sqrt(2) - 2) * min(dx, dy) ... (sqrt(3) - sqrt(2)) * min(max(dx, dy), dz); endoctile 距离在邻域扩展允许对角移动时比欧氏距离更贴近真实代价搜索收敛更快。代价是它不再严格可采纳最终路径可能比最优路径长 1% 到 3%。具体怎么选取决于你的场景无人机在开阔空域飞行欧氏距离就够在密集城市环境或地形起伏大的区域建议用 octile 距离减少无效搜索。改法也很简单把 A_star.m 里调用 distanced.m 的那行替换为 octile_distance 即可。3.3 G 值的移动代价设计G 值代表从起点到当前节点的实际已付代价。在三维 26 邻域中移动代价取决于当前节点与父节点的相对方向。轴向移动代价取 1面对角移动取 sqrt(2)体对角移动取 sqrt(3)这样才能让斜向移动不会被白白占便宜。function g_cost move_cost(parent, current) diff abs(current(1:3) - parent(1:3)); if sum(diff) 1 g_cost 1; % 轴向移动 elseif sum(diff) 2 g_cost sqrt(2); % 面对角移动 else g_cost sqrt(3); % 体对角移动 end end这里要提醒一个容易忽略的点——Z 轴的移动代价应该比其他轴更高。现实中无人机改变高度需要额外的能量消耗机器人在斜坡上移动也有额外代价。很多三维 A* 实现把 Z 轴当作和其他轴一模一样的维度处理这会让规划出的航迹频繁上下起伏。如果你想得到更符合动力学特性的路径可以把轴向代价改为 dx1、dy1、dz1.2然后在路径平滑阶段做补偿。3.4 一个关于数据类型的坑注意 TerrainData.mat 和 MapData.mat 在读取时的数据类型。如果它们存的是单精度浮点MATLAB 的 find 函数和逻辑索引行为会和双精度一样但内存占用减半。如果存的是 uint8在检查 MapData(node(1), node(2), node(3)) 1 时不会有问题但一旦参与矩阵运算或与地形高度做比较MATLAB 会隐式转换类型带来额外的时间开销。我在处理大规模地图时通常会在 load 之后统一转成 double虽然内存翻倍但后续的逐点比较和插值计算会更省时间。4. 网格粒度、权重与 open 列表的性能取舍4.1 三维空间复杂度带来的边界三维 A* 的计算量随栅格规模呈立方增长。一个 100x100x30 的栅格就有 30 万个节点每个节点扩展 26 个邻域一轮搜索可能要访问百万级节点。这比二维 100x100 栅格多了两个数量级。因此在改动这份代码之前务必要先量化地图规模。地图规模节点总数26 邻域扩展量级建议用途50x50x2050000130 万算法验证、教学演示100x100x30300000780 万小型无人机任务区200x200x4016000004160 万工程级预研需配合优化500x500x100250000006.5 亿必须改用分层或采样式规划这份代码默认的 TerrainData 规模大致在几十万节点量级直接跑没有问题。但如果把 Main.m 里的地图换成分辨率更高的数据第一步要感受的变化就是 min_fn.m 的扫描开销会突增见 4.2。4.2 min_fn 的线性扫描是隐形瓶颈min_fn.m 负责从 open 列表中找出 F 值最小的节点。常见实现如下function i_min min_fn(open_list) f_values open_list(6, :); % 假设第 6 行存 F 值 [~, i_min] min(f_values); end看起来这段代码足够简单高效min 函数在 MATLAB 里是高度优化的 C 实现扫描百万级数组很快。但问题不在 min 本身而在 open 列表的动态增删。每次迭代结束后当前节点要从 open 列表中删除新节点要由 insert_open.m 添加。MATLAB 矩阵的删除和拼接都是整体拷贝操作数组越大单次增删的成本越高。搜索结果跑几百上千次迭代后open 列表的维护时间会远超 min_fn 的计算时间。三个实用的解决方向用稀疏矩阵维护 closed 列表比逐行 ismember 判断快一个量级。把 open 列表预分配大数组用计数器追踪有效长度避免每次拼接新行。如果节点规模超过百万放弃 MATLAB 自带数据结构直接用 Java 的 PriorityQueue 接口。% 使用 Java PriorityQueue 维护 open 列表MATLAB 内嵌 Java pq java.util.PriorityQueue(); pq.add([f_value, node_id]); while ~pq.isEmpty() item pq.poll(); % 取出 F 值最小的节点 f_val item(1); node_id item(2); endMATLAB 对 Java 类有原生支持这块是很多做算法验证的人不知道或者不用的。Java PriorityQueue 的插入和弹出都是 O(log n)和 min_fn 线性扫描的 O(n) 相比节点规模越大优势越明显。缺点是要自己处理节点与队列元素的映射关系代码可读性稍有下降。4.3 启发式权重的调整空间很多人在跑通这份代码后会想加快搜索速度最简单的方式是在启发式函数前乘一个权重系数 w。当 w1 时是可采纳的 A*保证最优路径w1 时变成 Weighted A*搜索速度提升路径质量下降。这份代码没有内置权重参数但改起来非常容易% 在计算 F 值时引入权重 f g 1.2 * h;权重取 1.2 到 1.5 之间时大多数场景路径长度只会增加 2% 到 5%但扩展节点数可能减少 40% 以上。如果你做的是实时航线重规划这个改动几乎是无本万利。如果做离线最优路径权重保持 1.0 即可。一个更细的技巧是动态权重搜索初期用较大权重快速接近目标当 open 列表中最小 F 值连续多轮不变时把权重逐步降到 1.0保证终点附近的搜索精度。这样可以在保留 A* 最优性的同时缩短前期搜索时间。% 动态权重示例 if iteration 200 w 1.5; elseif min_f_no_change_count 50 w 1.0; end4.4 实际改动的效果判断改完参数后建议在 Result 文件夹里存一张路径总长与迭代次数的记录表每次调整参数时对比这两个数字。路径总长用 distanced.m 对路径逐段累加即可迭代次数在 A_star.m 的主循环里加一个计数器。如果你发现路径长度没变但迭代次数大幅减少说明启发式权重调对了方向。如果路径表面出现明显的锯齿说明权重过大搜索过度偏向启发式估计而忽视了实际地形约束。5. 路径平滑、碰撞复核与三维航迹验证5.1 对 A* 输出路径做平滑A* 搜索出来的路径是栅格节点连线在三维地形上看起来会有明显的折线感尤其在坡度变化大的区域。我一般会用三次样条对路径做平滑让航迹连续可飞% 对路径的 x、y、z 分量分别做样条插值 t 1:size(path, 1); tt linspace(1, size(path, 1), 500); sx spline(t, path(:, 1), tt); sy spline(t, path(:, 2), tt); sz spline(t, path(:, 3), tt); smooth_path [sx; sy; sz];样条平滑有一个风险插值点可能落在山体内部尤其是在路径急转弯处。所以平滑后必须做碰撞复核逐点检查每个插值点是否满足 check_collision 条件。% 逐点复核平滑路径是否安全 safe true; for i 1:size(smooth_path, 1) node round(smooth_path(i, :)); if check_collision(node, MapData, TerrainData) safe false; fprintf(平滑路径在点 %d 处穿越障碍\n, i); break; end end如果出现碰撞不要把样条阶数降太低更好的做法是先用 A* 的原始路径分段做样条只在安全区间内平滑转弯点附近保留原节点。5.2 生成可交互的三维可视化MATLAB 的 plot3 能显示航迹但没有真实地形的对比很难判断路径质量。把地形面绘制在同一坐标系下% 绘制三维地形表面 [X, Y] meshgrid(1:size(TerrainData,1), 1:size(TerrainData,2)); surf(X, Y, TerrainData); alpha(0.5); % 半透明显示 colormap(jet); hold on; plot3(path(:,1), path(:,2), path(:,3), w-, LineWidth, 2);半透明地形上叠加白色航迹可以直观看到路径是否沿着山谷走、是否贴着山峰绕行、是否存在无意义的上下起伏。这一步比任何参数指标都更能暴露问题。5.3 航迹质量量化验证除了可视化还要算三个量化指标来横向比较参数变更路径总长度、总爬升量、平均离地高度。总爬升量很容易算对路径 Z 坐标做差分把所有正值加起来就是累计爬升。这个数字对无人机航线尤其重要同样的路径长度爬升越少能耗越低。平均离地高度则反映飞行安全性离地越近越危险。把这三个指标写进 Result 文件夹下的一个文本或表格里以后每次调参都有据可查。这样处理之后这套代码就不再是跑一次就忘的课程作业而是可以持续对比参数效果的三维航线验证平台。本文还有配套的精品资源点击获取