简介这份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); % 起点入 openListg0h 用对角距离估算 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(end1) 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(i1,:)) / 3); end % 再按最大转弯角约束剔除过密点 keep true(size(sp, 1), 1); for i 2:size(sp, 1)-1 v1 sp(i,:) - sp(i-1,:); v2 sp(i1,:) - 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 dx0 dy0 dz0, continue; end dirs3(end1, :) [dx, dy, dz]; costs3(end1) 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 多个航点飞控直接拒收。从那以后我每次导出前都强制走一遍抽稀和范围检查确认航点数在飞控限制以内、高度没超限、经纬度落在地图范围内。希望这些经验帮到你少走点我踩过的弯路。本文还有配套的精品资源点击获取