简介这份MATLAB无人机路径规划源码包面向无人机自主导航方向的学生、科研人员与算法爱好者聚焦栅格地图构建、激光雷达扫描模拟与A算法改进三大核心问题可用于课程设计、毕业课题或算法验证场景。资源共14个文件以7个m脚本为主体涵盖A路径探索、障碍物标记、动态路径调整与地图绘制等模块另含1个Python脚本、2张png结果图、1个gif动态演示、1份md说明及inscode、gitignore等配置项压缩包约107KB结构紧凑便于直接运行调试。已有89人学习下载。读者可从中获得完整的A*算法实现逻辑、雷达扫描机制设计思路与动态路径规划方案并通过动态显示直观观察无人机避障飞行过程为感知与控制研究提供可复用的代码基础与实践参考。1. 从一次炸机说起MATLAB 无人机路径规划到底在算什么去年帮一个做农业植保的朋友看他的仿真结果三维图里无人机轨迹漂亮得像一条丝带结果导出航点灌进真机刚起飞就撞了田边的防风林。问题不在飞控在于他的路径规划只算了水平面的最短距离高度是拍脑袋定的常数。这件事让我意识到很多人搜「MATLAB无人机路径规划源码」真正想要的不是一段能跑的代码而是一套能把「环境建模—约束定义—代价函数—轨迹平滑」串起来的可复现流程。MATLAB 在这个方向上的价值是它把矩阵运算、优化工具箱和三维可视化揉在一个环境里你改一个权重就能立刻在图上看到轨迹怎么变。这篇文章面向的是已经会写 MATLAB 基础脚本、但对路径规划还停留在「调个 A* 就完事」阶段的同学我会把栅格法、RRT、人工势场这几条常见路线的源码骨架拆开讲重点放在参数怎么设、坑在哪。2. 环境建模与栅格地图生成把连续世界塞进矩阵路径规划的第一步永远不是算法是地图。MATLAB 里最省事的做法是把三维空间离散成栅格矩阵0 表示可飞1 表示障碍。但这里有个新手最容易翻车的地方栅格分辨率设得太细内存直接爆掉设得太粗窄缝直接消失规划出来的路径穿墙。2.1 用 meshgrid 和逻辑索引构建三维占据栅格假设飞行区域是 100m × 100m × 30m我一般先用 1m 的分辨率试水确认算法逻辑通了再降到 0.5m。下面这段代码生成一个带圆柱障碍和地面禁飞区的栅格地图。% 三维栅格地图生成示例 res 1; % 栅格分辨率单位米 x 0:res:100; y 0:res:100; z 0:res:30; [X, Y, Z] meshgrid(x, y, z); map false(size(X)); % false 表示可通行 % 地面以下禁飞z 2m 视为地面效应区 map(Z 2) true; % 圆柱形障碍中心 (40,40)半径 8m高度 25m cyl ((X - 40).^2 (Y - 40).^2) 8^2 Z 25; map(cyl) true; % 第二个障碍中心 (70,60)半径 6m cyl2 ((X - 70).^2 (Y - 60).^2) 6^2 Z 20; map(cyl2) true; % 可视化切片确认障碍位置 slice(X, Y, Z, double(map), 40, 40, 15); shading flat; colormap(gray); axis equal;这段代码的逻辑很直白meshgrid把三个坐标轴展开成三维矩阵false(size(X))初始化全可通行然后用逻辑索引批量标记障碍。参数上唯一需要斟酌的是res它直接决定矩阵大小——100×100×30 在 1m 分辨率下是 30 万个体素MATLAB 处理起来毫无压力但如果降到 0.2m体素数量涨到 375 万A* 的开放列表会迅速吃满内存。我的经验是先用粗分辨率验证路径拓扑对不对再在最终输出航点时用细分辨率做局部修正。2.2 从栅格到可搜索图邻接关系的两种建法栅格建好后算法需要知道「从一个体素能走到哪些邻居」。常见做法有两种26 邻域和 6 邻域。26 邻域允许对角移动路径更短但容易贴着障碍角飞实际飞行中风险高6 邻域只允许上下左右前后路径保守但安全。% 生成 6 邻域偏移量 neighbors6 [ 1 0 0; -1 0 0; 0 1 0; 0 -1 0; 0 0 1; 0 0 -1 ]; % 生成 26 邻域偏移量 [Xn, Yn, Zn] meshgrid(-1:1, -1:1, -1:1); neighbors26 [Xn(:), Yn(:), Zn(:)]; neighbors26(all(neighbors26 0, 2), :) []; % 去掉自身这里有个细节26 邻域的对角移动代价不是 1而是 √2 或 √3代价函数里必须体现否则 A* 会退化成偏好对角线的贪心搜索。我一般会在代价计算时用norm(offset)动态算而不是硬编码。注意如果你的障碍物是薄壁结构比如电线1m 分辨率下它可能只占一个体素26 邻域搜索时容易被「挤」过去。这种情况要么加密栅格要么在障碍标记时做膨胀处理。3. 三种主流规划算法的 MATLAB 实现与选型环境准备好之后选哪个算法取决于你的场景。A* 适合静态已知地图、要求最优路径RRT 适合高维空间、障碍复杂但允许次优解人工势场法适合实时避障、但容易陷入局部极小。下面把三条路线的核心代码骨架都过一遍。3.1 A* 在三维栅格上的开放列表管理A* 的 MATLAB 实现难点不在算法本身在于开放列表的查找效率。新手常犯的错是用find遍历整个矩阵找最小 F 值30 万体素下每轮搜索都卡成幻灯片。function path astar3d(map, startIdx, goalIdx) [nx, ny, nz] size(map); gScore inf(nx, ny, nz); gScore(startIdx) 0; fScore inf(nx, ny, nz); fScore(startIdx) heuristic(startIdx, goalIdx); openSet false(nx, ny, nz); openSet(startIdx) true; cameFrom zeros(nx, ny, nz, 3); % 记录父节点坐标 neighbors [1 0 0; -1 0 0; 0 1 0; 0 -1 0; 0 0 1; 0 0 -1]; while any(openSet(:)) % 找 F 最小的节点用逻辑索引而非 find 全扫 fVals fScore; fVals(~openSet) inf; [~, minIdx] min(fVals(:)); [cx, cy, cz] ind2sub([nx ny nz], minIdx); if minIdx goalIdx path reconstruct(cameFrom, startIdx, goalIdx); return; end openSet(minIdx) false; for k 1:size(neighbors, 1) nx_ cx neighbors(k,1); ny_ cy neighbors(k,2); nz_ cz neighbors(k,3); if nx_1 || nx_nx || ny_1 || ny_ny || nz_1 || nz_nz continue; end if map(nx_, ny_, nz_), continue; end % 障碍跳过 tentative_g gScore(cx,cy,cz) norm(neighbors(k,:)); if tentative_g gScore(nx_, ny_, nz_) cameFrom(nx_, ny_, nz_, :) [cx cy cz]; gScore(nx_, ny_, nz_) tentative_g; fScore(nx_, ny_, nz_) tentative_g heuristic([nx_ ny_ nz_], goalIdx); openSet(nx_, ny_, nz_) true; end end end path []; % 无解 end function h heuristic(a, b) h norm(double(a) - double(b)); % 欧氏距离可换曼哈顿 end这段代码里最值得说的是fVals(~openSet) inf这一行。它把不在开放列表里的节点 F 值设为无穷然后直接min取全局最小避免了find加循环的开销。在 30 万体素规模下这个改动能把单次搜索从十几秒压到两秒以内。启发函数用欧氏距离是保守的不会高估保证最优性如果你追求速度、能接受次优可以乘一个大于 1 的权重比如 1.2路径会稍微绕一点但搜索快很多。3.2 RRT 的采样策略与步长参数RRT 的核心是随机采样加树扩展。MATLAB 里没有现成的三维 RRT 函数得自己写。关键参数有两个步长stepSize和目标偏置概率goalBias。function path rrt3d(map, startPt, goalPt, stepSize, goalBias, maxIter) tree startPt; % 每行一个节点坐标 parent 0; % 根节点无父 [nx, ny, nz] size(map); for iter 1:maxIter % 目标偏置有一定概率直接采样目标点 if rand goalBias sample goalPt; else sample [randi(nx), randi(ny), randi(nz)]; end % 找树上最近节点 dists vecnorm(tree - sample, 2, 2); [~, nearIdx] min(dists); nearPt tree(nearIdx, :); % 朝采样点扩展固定步长 dir (sample - nearPt) / norm(sample - nearPt); newPt nearPt dir * stepSize; newPt round(newPt); % 边界与碰撞检查 if any(newPt 1) || newPt(1)nx || newPt(2)ny || newPt(3)nz continue; end if map(newPt(1), newPt(2), newPt(3)) continue; end if ~collisionFree(map, nearPt, newPt) continue; end tree(end1, :) newPt; parent(end1) nearIdx; % 到达目标附近则回溯 if norm(newPt - goalPt) stepSize path backtrack(tree, parent, size(tree,1)); return; end end path []; endstepSize设大了树长得快但容易跳过窄通道设小了迭代次数暴涨。我的经验值是栅格分辨率的 3 到 5 倍比如 1m 栅格用 3 到 5m 步长。goalBias一般取 0.05 到 0.1太高会让树过早朝目标生长、失去探索能力太低则收敛慢。collisionFree函数需要做线段与栅格的相交检测简单做法是沿线段等间隔采样若干点逐一查map采样密度至少是栅格分辨率的两倍。3.3 人工势场法的引力斥力函数与局部极小逃逸人工势场法代码最短但坑最深。引力指向目标斥力来自障碍合力方向就是下一步方向。function nextPt apfStep(curr, goal, map, attGain, repGain, repRange) attForce attGain * (goal - curr); % 引力 repForce [0 0 0]; [nx, ny, nz] size(map); % 在斥力影响范围内搜索障碍 for dx -repRange:repRange for dy -repRange:repRange for dz -repRange:repRange px curr(1)dx; py curr(2)dy; pz curr(3)dz; if px1||pxnx||py1||pyny||pz1||pznz, continue; end if ~map(px,py,pz), continue; end obs [px py pz]; d norm(curr - obs); if d repRange d 0 repForce repForce repGain * (1/d - 1/repRange) * (1/d^2) * (curr - obs)/d; end end end end totalForce attForce repForce; if norm(totalForce) 1e-6 nextPt curr; % 局部极小需要逃逸策略 else nextPt curr totalForce / norm(totalForce); end endattGain和repGain的比值决定行为引力太大容易撞障碍斥力太大到不了目标。我一般从attGain1, repGain100开始试。局部极小是这套方法的死穴常见逃逸策略是加一个随机扰动或者切换到 A* 走一段。实际项目中我很少单独用 APF更多是把它作为 A* 的局部避障补充。4. 避坑与排查那些让轨迹「看起来对、飞起来炸」的细节这一章是我自己踩过的坑每一条都对应真金白银的教训。4.1 路径穿墙碰撞检测的采样密度不够现象A* 输出的路径在可视化里贴着障碍边缘看起来没穿但导出航点后发现某两个航点之间的连线穿过了障碍角。原因A* 只在体素中心判断可通行两个相邻体素中心连线可能擦过障碍体素的角。解决在路径后处理阶段做线段碰撞检测沿每段路径以 0.5 倍栅格分辨率采样发现碰撞就插入中间航点或重新规划。4.2 高度震荡代价函数缺少平滑项现象规划出的路径在高度方向上下跳动无人机飞起来像坐过山车。原因A* 的代价只算了距离没有惩罚高度变化。解决在代价函数里加一项w * abs(dz)w取 0.5 到 2 之间强制算法偏好平飞。这个改动会让路径稍微变长但飞行品质提升明显。4.3 搜索超时开放列表用矩阵全扫现象地图稍微大一点A* 跑十分钟不出结果。原因每轮用find或min全矩阵扫描找最小 F 值。解决改用二叉堆或 MATLAB 的priorityqueue如果有对应工具箱或者至少用逻辑索引把非开放节点排除后再取min。我在 3.1 的代码里已经用了后者实测提速 5 到 8 倍。4.4 RRT 路径锯齿没有做后平滑现象RRT 输出的路径是一段段折线航向角变化剧烈。原因RRT 本质是随机树节点之间没有考虑转弯半径约束。解决用三次 B 样条或多项式拟合对路径做平滑同时检查平滑后的曲线是否仍然避障。MATLAB 的cscvn和fnval可以快速实现。4.5 势场法目标不可达障碍在目标附近现象无人机在目标附近来回振荡就是落不下去。原因目标点落在障碍的斥力影响范围内斥力把无人机推走。解决当距离目标小于一定阈值时逐步减小斥力增益或者直接把目标点附近的斥力置零。5. 从仿真到航点轨迹平滑与导出验证的一个实用技巧仿真跑通只是第一步真正要落地还得把路径变成飞控能吃的航点。这里我分享一个自己常用的后处理流程先用 A* 或 RRT 拿到粗路径再用minsnappolytraj做最小加加速度轨迹生成最后导出为经纬高航点表。% 假设 path 是 N×3 的栅格坐标先转成米制坐标 waypoints (path - 1) * res; % res 是栅格分辨率 % 用最小加加速度多项式生成平滑轨迹 tSamples linspace(0, 10, 200); % 10 秒飞完200 个采样点 [qx, qv, qa] minsnappolytraj(waypoints(:,1), tSamples, 7); [qy, ~, ~] minsnappolytraj(waypoints(:,2), tSamples, 7); [qz, ~, ~] minsnappolytraj(waypoints(:,3), tSamples, 7); % 检查平滑后轨迹是否碰撞 smoothPath [qx qy qz]; for i 1:size(smoothPath,1)-1 if ~collisionFree(map, round(smoothPath(i,:)/res)1, ... round(smoothPath(i1,:)/res)1) warning(平滑轨迹在第 %d 段碰撞需重新规划, i); end end % 导出为航点表经纬高需要根据起飞点做局部坐标转换 writematrix(smoothPath, waypoints.csv);minsnappolytraj的第二个参数是时间采样点第三个参数是多项式阶数7 阶是最小加加速度的标准选择。这里的关键是平滑后必须重新做碰撞检测因为多项式拟合可能让曲线「鼓」出去蹭到障碍。我一般会在导出前把平滑轨迹和原始障碍地图叠在一起画一遍肉眼确认没有擦边。还有一个实用技巧航点导出时不要直接把所有采样点都发给飞控200 个点太多了飞控的航点缓冲区可能装不下。我通常按曲率做抽稀直线段每 5 米一个点转弯段每 1 米一个点这样既保证轨迹精度又不会撑爆飞控内存。最后说个血泪教训仿真里用的地图和真实环境的坐标系一定要对齐。我有一次忘了做原点偏移仿真里障碍在 (40,40)实际场地障碍在 (40,40) 但起飞点在别处结果整个路径偏了十几米。现在我养成的习惯是任何路径规划脚本的第一行注释都写清楚坐标系定义和原点位置导出航点前再核对一遍。希望帮到你。本文还有配套的精品资源点击获取