简介：这份MATLAB无人机路径规划源码包面向无人机自主导航方向的学习者与研究者，聚焦栅格地图构建、激光雷达扫描模拟与A算法改进三大核心问题，适合具备一定MATLAB编程基础、希望动手复现路径规划全流程的读者。包内共14个文件，以m脚本为主，涵盖A路径探索、地图绘制、动态路径调整与GIF生成等模块，另含Python脚本、说明文档及png、gif效果图，压缩包约107KB，结构紧凑便于快速上手。已有89人学习下载。读者可从中获得完整的算法实现逻辑，包括障碍物标记、启发式评估函数设计以及飞行过程中依据新环境信息动态重规划的处理思路，并借助动态显示直观观察无人机避障与路径优化效果，为感知与控制方向的课题研究或课程设计提供可运行的参考范例。
1. 从一次炸机复盘说起：这套 MATLAB 无人机路径规划源码到底能干什么
去年帮一个做农业植保的朋友看他的巡检机，起飞十分钟就在一片果园里撞了树。事后导出日志一看，航线是手绘的，拐点全凭感觉，压根没考虑过障碍物膨胀和最小转弯半径。这事让我意识到，很多做无人机应用的人，卡的不是飞控调参，而是路径规划这一环——怎么在已知环境里算出一条既短又安全、还符合动力学约束的航线。这次拆的这套 MATLAB 无人机路径规划源码，核心就是解决这个问题：它把 A* 算法从二维栅格地图一路做到三维空间，附带完整的代价函数、启发式设计和可视化脚本，适合做算法验证的学生、做巡检/测绘航线预规划的工程师，以及想从零手撸一遍规划流程的开发者。源码是纯 MATLAB 实现，不依赖额外工具箱，拿到就能跑，改几个参数就能套到自己的场景里。
2. A* 算法在栅格地图上的实现：从 openList 到路径回溯
2.1 为什么选 A* 而不是 Dijkstra 或 RRT
路径规划算法一抓一大把，选型这事得看场景。Dijkstra 能保证最短路径，但它是盲目搜索，节点一多就慢得让人想砸键盘；RRT 适合高维空间和动力学约束强的场景，但它是采样式算法，每次跑出来的路径都不一样，做航线预规划时这种不确定性很要命。A* 的好处在于它用启发式函数把搜索方向"拽"向目标点，在栅格地图这种离散环境里，既能保证最优性（启发式可采纳的前提下），速度又比 Dijkstra 快一个量级。
这套源码选 A* 作为主干，我认为是合理的。它的应用场景是已知静态环境下的全局规划——比如提前知道地块边界、电线杆位置、禁飞区范围，然后离线算出一条航线再上传给飞控。这种场景不需要在线重规划，A* 的确定性和最优性正好对上。
启发式函数用的是对角距离（Octile Distance），比曼哈顿距离更贴合八邻域移动的实际代价。如果换成四邻域移动，曼哈顿距离就够了；但八邻域下用曼哈顿会高估代价，导致路径不够优。这个细节源码里处理得对，后面讲参数时我会展开。
2.2 核心数据结构与主循环拆解
A* 的骨架不复杂，难的是边界处理。下面这段是源码里astar_plan.m的主循环简化版，我加了注释方便对照：
function [path, cost] = astar_plan(map, start, goal) % map: 二值栅格地图，0 可通行，1 障碍 % start/goal: [row, col] 格式的起终点坐标 [rows, cols] = size(map); % openList 用结构体数组存候选节点，closedList 用逻辑矩阵标记 openList = struct('pos', {}, 'g', {}, 'h', {}, 'parent', {}); closed = false(rows, cols); % 起点入 openList，g=0，h 用对角距离估算 openList(1).pos = start; openList(1).g = 0; openList(1).h = octile_dist(start, goal); openList(1).parent = [0, 0]; % 八邻域方向向量，前四个是直线，后四个是对角 dirs = [0 1; 1 0; 0 -1; -1 0; 1 1; 1 -1; -1 1; -1 -1]; moveCost = [1 1 1 1 sqrt(2) sqrt(2) sqrt(2) sqrt(2)]; while ~isempty(openList) % 找 f = g + h 最小的节点 [~, idx] = min([openList.g] + [openList.h]); current = openList(idx); openList(idx) = []; % 到达目标，回溯路径 if isequal(current.pos, goal) path = backtrack(current); cost = current.g; return; end closed(current.pos(1), current.pos(2)) = true; for k = 1:8 nb = current.pos + dirs(k, :); % 边界检查 + 障碍检查 + closed 检查 if nb(1) < 1 || nb(1) > rows || nb(2) < 1 || nb(2) > cols continue; end if map(nb(1), nb(2)) == 1 || closed(nb(1), nb(2)) continue; end % 对角移动时检查是否穿角（两个相邻直线格都是障碍） if k > 4 if map(current.pos(1), nb(2)) == 1 && map(nb(1), current.pos(2)) == 1 continue; end end tentative_g = current.g + moveCost(k); % 如果该邻居已在 openList 中且新 g 更小，更新它 existing = find(arrayfun(@(x) isequal(x.pos, nb), openList), 1); if isempty(existing) node.pos = nb; node.g = tentative_g; node.h = octile_dist(nb, goal); node.parent = current.pos; openList(end+1) = node; elseif tentative_g < openList(existing).g openList(existing).g = tentative_g; openList(existing).parent = current.pos; end end end path = []; cost = inf; % 无解 end逻辑上分四步：初始化、选最优节点、扩展邻居、回溯路径。参数上最需要注意的是moveCost——直线代价 1，对角代价 √2，这是欧氏距离下的真实移动代价。如果你把对角代价也设成 1，路径会倾向于走对角线，实际飞行距离会比预估的长，这是新手常犯的错。
octile_dist的实现是max(dr, dc) + (sqrt(2)-1) * min(dr, dc)，其中 dr、dc 是行列差。这个公式保证启发式不会高估实际代价，A* 的最优性才成立。
2.3 路径回溯与平滑处理
主循环找到目标后，backtrack函数从目标节点顺着 parent 指针往回走，直到起点，然后翻转数组。源码里这部分写得很直白，但有个坑：如果 parent 存的是坐标而不是索引，在节点重复入 openList 时可能追到错误的父节点。这套源码用坐标存储，配合"更新已有节点"的逻辑，实测没出问题，但如果你的地图很大、节点重复率高，建议改成存索引。
回溯出来的路径是栅格序列，直接拿去飞会有很多 90 度和 45 度硬拐。源码附了一个smooth_path.m，用简单的滑动平均加角度约束做后处理：
function sp = smooth_path(path, max_turn_deg) % 先做三点滑动平均，去掉锯齿 sp = path; for i = 2:size(path, 1)-1 sp(i, :) = round((path(i-1,:) + path(i,:) + path(i+1,:)) / 3); end % 再按最大转弯角约束剔除过密点 keep = true(size(sp, 1), 1); for i = 2:size(sp, 1)-1 v1 = sp(i,:) - sp(i-1,:); v2 = sp(i+1,:) - sp(i,:); ang = atan2d(v1(1)*v2(2)-v1(2)*v2(1), v1(1)*v2(1)+v1(2)*v2(2)); if abs(ang) > max_turn_deg keep(i) = false; end end sp = sp(keep, :); endmax_turn_deg一般设 30 到 45 度，看机型。多旋翼可以放宽到 60 度，固定翼最好压在 30 度以内。平滑后的路径点数会减少，但要注意别把避障的关键拐点也平滑掉了——如果平滑后路径穿过障碍，得回退到原始路径重新处理。
3. 从二维到三维：高度层设计与代价函数扩展
3.1 三维栅格地图的构建方式
二维规划只能解决平面避障，实际飞行还得考虑高度。这套源码的三维版本把地图从rows × cols扩展成rows × cols × layers，每个高度层是一张独立的二值栅格图。构建方式常见有两种：一种是从点云或 DEM 数据体素化，另一种是手动分层——比如 10 米一层，每层标注该高度上的障碍物投影。
源码用的是第二种，build_3d_map.m里读入一组二维障碍图，按高度堆叠：
function map3d = build_3d_map(obstacle_list, layer_height, max_alt) % obstacle_list: cell 数组，每个元素是一层的二维障碍图 % layer_height: 每层代表的高度间隔（米） % max_alt: 最大飞行高度（米） num_layers = floor(max_alt / layer_height); [rows, cols] = size(obstacle_list{1}); map3d = zeros(rows, cols, num_layers); for L = 1:num_layers if L <= length(obstacle_list) map3d(:, :, L) = obstacle_list{L}; end end end参数上，layer_height决定了高度分辨率。设太小，层数多、搜索慢；设太大，可能漏掉两层之间的障碍。我一般建议按无人机的最小安全高度余量来定——比如你要求离障碍物至少 5 米，那层高设 5 米就够。max_alt别超过法规允许的限高，也别超过电池能支撑的爬升高度。
3.2 三维 A* 的邻居扩展与代价设计
三维 A* 的邻居从 8 个变成 26 个——上下左右前后各 6 个面邻居，12 个棱邻居，8 个角邻居。代价计算也跟着变：面移动代价 1，棱移动 √2，角移动 √3。源码里用了一个 26×3 的方向矩阵，配合对应的代价向量，逻辑和二维版一致。
% 26 邻域方向与代价 dirs3 = []; costs3 = []; for dx = -1:1 for dy = -1:1 for dz = -1:1 if dx==0 && dy==0 && dz==0, continue; end dirs3(end+1, :) = [dx, dy, dz]; costs3(end+1) = sqrt(dx^2 + dy^2 + dz^2); end end end启发式函数换成三维欧氏距离或三维对角距离。这里有个容易翻车的地方：如果高度层的物理间距和平面栅格间距不一样（比如平面 1 米一格，高度 5 米一层），代价计算必须做尺度归一化，否则算法会倾向于在同一层里绕远路，也不愿意爬升。源码里用了一个scale_factor参数来处理，默认[1, 1, layer_height/plane_res]，这个细节很多人会忽略。
代价函数还可以叠加额外项，比如禁飞区惩罚、爬升能耗惩罚。源码留了一个extra_cost回调接口，你可以传入自定义函数给特定区域加权。常见做法是给禁飞区设一个很大的正数，让算法主动绕开；给爬升动作加一点惩罚，避免频繁变高。
3.3 三维路径的可视化与验证
算完路径得能看。源码附了plot_3d_path.m，用plot3画航线，用patch画障碍物包围盒。跑完一遍，我一般会检查三件事：路径有没有穿障碍、高度变化是否平滑、总航程和预估电量是否匹配。
figure; hold on; grid on; axis equal; % 画障碍物 for i = 1:size(obs_boxes, 1) draw_box(obs_boxes(i, :), [0.8 0.2 0.2]); end % 画路径 plot3(path(:,2), path(:,1), path(:,3)*layer_height, 'b-o', 'LineWidth', 1.5); xlabel('X (格)'); ylabel('Y (格)'); zlabel('高度 (米)'); view(45, 30);如果发现路径贴着障碍物边缘走，说明障碍物膨胀没做够。实际飞行要留安全余量，膨胀半径至少是机身半径加定位误差。源码里inflate_map.m用形态学膨胀实现，strel('disk', r)的 r 就是膨胀像素数，对应实际距离要按地图分辨率换算。
4. 避坑与排查：那些让路径"看起来对、飞起来炸"的细节
4.1 路径贴边飞行，安全余量不足
现象：规划出的航线紧贴障碍物边缘，仿真里能过，实飞时因为定位漂移撞上去。
原因：栅格地图里障碍物只占一个格，A* 把相邻可通行格当作安全区域，但实际机身有尺寸，GPS 有误差。
解决：规划前先对障碍物做膨胀，膨胀半径 = 机身半径 + 定位误差 + 安全余量。多旋翼一般取 1.5 到 3 米。源码里inflate_map的半径参数按地图分辨率换算，别直接填米数。
4.2 对角穿角导致路径不可飞
现象：路径在两个对角障碍之间斜穿过去，仿真显示可通行，实际那个缝隙根本过不了机身。
原因：八邻域扩展时只检查了对角目标格是否障碍，没检查两个相邻直线格。
解决：在对角移动前加穿角检查，两个相邻直线格任一为障碍就跳过该方向。源码里已经处理了，但如果你自己改邻居逻辑，这个检查很容易漏。
4.3 启发式函数高估代价，路径不是最优
现象：A* 跑出来的路径明显绕远，换 Dijkstra 一对比发现短了 15%。
原因：启发式函数用了曼哈顿距离或欧氏距离但没乘系数，在八邻域下高估了实际代价，破坏了可采纳性。
解决：八邻域用对角距离，三维用三维对角距离。如果为了加速允许次优解，可以把启发式乘一个大于 1 的系数（加权 A*），但要清楚这是在拿最优性换速度。
4.4 高度层间距与平面分辨率不匹配
现象：三维规划结果要么一直不爬升，要么频繁上下跳，路径看着很别扭。
原因：代价计算里高度方向的尺度和平面尺度没统一，算法对爬升的"贵贱"判断失真。
解决：在代价函数里引入尺度因子，把高度差换算成等效平面距离。源码的scale_factor参数就是干这个的，默认值按实际分辨率算，别偷懒用 1。
4.5 openList 线性查找导致大图卡死
现象：地图超过 500×500 后，规划时间从几秒涨到几分钟。
原因：每次循环用min遍历 openList 找最小 f，复杂度 O(n)，节点多了就爆。
解决：换成二叉堆或 MATLAB 的priorityqueue（如果有对应工具箱）。源码在小图上够用，大图建议自己替换数据结构。我一般会加一个节点数阈值提示，超过就警告。
5. 进阶技巧：把规划结果导出成飞控能吃的航线文件
源码默认输出的是栅格坐标矩阵，离实际可用还差一步——转成飞控认识的航线格式。常见的是 MAVLink 的 mission item 或 KML。我一般写个转换脚本，把栅格坐标按地图原点和分辨率换算成经纬高，再按航点间距抽稀。
function export_mission(path, origin_lat, origin_lon, res, layer_h, filename) % path: 栅格路径 [row, col, layer] % origin_lat/lon: 地图左上角对应的经纬度 % res: 每格代表的实际距离（米） % layer_h: 每层高度（米） lat = origin_lat - path(:,1) * res / 111320; lon = origin_lon + path(:,2) * res / (111320 * cosd(origin_lat)); alt = path(:,3) * layer_h; % 抽稀：相邻点距离小于 2 米就合并 keep = [true; sqrt(diff(lat).^2 + diff(lon).^2) * 111320 > 2]; T = table(lat(keep), lon(keep), alt(keep), ... 'VariableNames', {'lat', 'lon', 'alt'}); writetable(T, filename); end参数上，res必须和建图时用的分辨率一致，否则经纬度全错。111320是每度纬度对应的米数，经度方向要乘cosd(lat)修正。抽稀阈值 2 米是个经验值，太密飞控处理不过来，太疏航线变形。
导出后我习惯用地面站软件回放一遍，确认航点顺序、高度、转弯半径都合理。有一次忘了抽稀，导了 3000 多个航点，飞控直接拒收。从那以后我每次导出前都强制走一遍抽稀和范围检查，确认航点数在飞控限制以内、高度没超限、经纬度落在地图范围内。希望这些经验帮到你，少走点我踩过的弯路。
本文还有配套的精品资源，点击获取
