简介：这份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(end+1, :) = newPt; parent(end+1) = 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 px<1||px>nx||py<1||py>ny||pz<1||pz>nz, 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的比值决定行为：引力太大容易撞障碍，斥力太大到不了目标。我一般从attGain=1, repGain=100开始试。局部极小是这套方法的死穴，常见逃逸策略是加一个随机扰动或者切换到 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(i+1,:)/res)+1) warning('平滑轨迹在第 %d 段碰撞，需重新规划', i); end end % 导出为航点表（经纬高需要根据起飞点做局部坐标转换） writematrix(smoothPath, 'waypoints.csv');minsnappolytraj的第二个参数是时间采样点，第三个参数是多项式阶数，7 阶是最小加加速度的标准选择。这里的关键是平滑后必须重新做碰撞检测，因为多项式拟合可能让曲线「鼓」出去蹭到障碍。我一般会在导出前把平滑轨迹和原始障碍地图叠在一起画一遍，肉眼确认没有擦边。
还有一个实用技巧：航点导出时不要直接把所有采样点都发给飞控，200 个点太多了，飞控的航点缓冲区可能装不下。我通常按曲率做抽稀，直线段每 5 米一个点，转弯段每 1 米一个点，这样既保证轨迹精度又不会撑爆飞控内存。
最后说个血泪教训：仿真里用的地图和真实环境的坐标系一定要对齐。我有一次忘了做原点偏移，仿真里障碍在 (40,40)，实际场地障碍在 (40,40) 但起飞点在别处，结果整个路径偏了十几米。现在我养成的习惯是，任何路径规划脚本的第一行注释都写清楚坐标系定义和原点位置，导出航点前再核对一遍。希望帮到你。
本文还有配套的精品资源，点击获取
