简介面向农业自动化与智慧植保领域的MATLAB路径优化资源聚焦无人机植保作业中的航线规划问题涉及农田地图读取、地块边界绘制、障碍物规避、最短路径求解等完整流程帮助中高级开发者掌握优化工具箱与图搜索算法的工程化应用。压缩包共26个文件以23个m脚本为核心覆盖Dijkstra算法、点到线段距离、直线相交判断、坐标转换等功能模块另附README说明与docx文档便于理解代码结构与复现实验。包体仅33KB轻量精简。已有700人学习下载。通过研读主要脚本可对比不同航路规划策略的建模差异调整无人机速度、转弯半径等参数并观察优化结果是学习无人机任务规划与MATLAB数学建模的实用参考。1. 植保路径优化不只是一个最短路径问题接手这个项目时我第一个反应是“这不就是旅行商问题的变体吗”。真正把routesPlanning.m跑完一遍才发现农业植保的路径优化和普通巡检、物流配送有一个本质差别你的首要目标不是找一条最短路径而是用尽可能少的转弯次数把整块农田全覆盖扫完。无人机每转一次弯就要减速、悬停、转向再加速这个时间成本在小地块上可能占到总作业时间的 30% 以上。所以这个项目里真正值钱的不是“找最短路径”的算法部分而是“如何把全覆盖扫描和点对点转移这两层逻辑叠在一起”。项目给的routesPlanning.m和routesPlanning2.m两套脚本其实就是两种不同的航点生成策略配合UAV2.m、UAV3.m里的无人机运动模型你能把电池约束、转弯半径约束、障碍物约束同时拉进优化目标里。做农业自动化或者无人机任务规划的人都能从这套代码里拆出可复用的框架。2. 农田建模与坐标换算plantMap.m和geography2coordinate.m是后续所有计算的地基2.1 为什么植保路径规划不能直接用经纬度算如果你直接用经纬度坐标去做距离计算和碰撞检测第一个坑就是单位不一致。经度 0.0001 度在地球不同纬度上对应的实际米数完全不同而无人机植保的作业幅宽通常只有 3 到 8 米这种误差会直接导致漏喷或者重喷。所以geography2coordinate.m这个文件存在的意义就是把 GPS 采集到的经纬度点对转换成以米为单位的平面坐标后续的路径长度、转弯半径、障碍物距离才都能在同一个度量体系里计算。function [X, Y] geography2coordinate(lon, lat, lon0, lat0) % 使用等距圆柱投影做近似转换适合小范围农田 % lon0, lat0 为地块基准点通常是地块中心或起点 EARTH_RADIUS 6371000.0; x EARTH_RADIUS * deg2rad(lon - lon0) * cos(deg2rad(lat0)); y EARTH_RADIUS * deg2rad(lat - lat0); X x; Y y; end这段转换代码的核心是取地块中心点作为基准然后用等距圆柱投影把经纬度偏移量换算成弧长。注意cos(deg2rad(lat0))这个因子它修正了不同纬度下经度线间距的差异。对于几公顷的农田这种近似转换的误差在厘米级完全够用。如果你的地块特别大或者纬度跨度超过 0.5 度就不建议用这种方式应该换成 UTM 分区投影。这个函数输出的是相对坐标原点设在基准点所以后面所有路径规划算法都不需要再关心绝对值大小只需要处理相对位移。2.2plantMap.m如何把农田变成算法能读的栅格地图拿到平面坐标之后下一步就是建图。plantMap.m做的事情本质上是一个栅格化过程把连续的地理空间切分成均匀的网格单元每个单元标记为“可作业”或者“不可作业”。栅格大小直接决定了路径规划的精度和计算量我一般建议设置为无人机作业幅宽的 1/2 到 1/3。比如你的植保机有效喷幅是 6 米栅格设 2 米左右比较合适——太小了计算量指数增长太大了边界区域会漏喷。function map plantMap(boundary, obstacles, gridSize) % boundary: 地块边界N x 2 的顶点数组 % obstacles: 障碍物列表每个元素是 M x 2 的顶点数组 % gridSize: 栅格大小米 minX min(boundary(:,1)); maxX max(boundary(:,1)); minY min(boundary(:,2)); maxY max(boundary(:,2)); cols ceil((maxX - minX) / gridSize); rows ceil((maxY - minY) / gridSize); map zeros(rows, cols); % 先用边界多边形填充地块内部 [Xq, Yq] meshgrid(minX:gridSize:maxX, minY:gridSize:maxY); inBoundary inpolygon(Xq, Yq, boundary(:,1), boundary(:,2)); map(inBoundary) 1; % 再扣掉障碍物区域 for i 1:length(obstacles) obs obstacles{i}; inObs inpolygon(Xq, Yq, obs(:,1), obs(:,2)); map(inObs) 0; end end这段代码的逻辑很直白先生成一个全零矩阵代表“不可飞行区”然后先填充边界内部再把障碍物区域清零。gridSize是唯一的超参数它的选择影响边界精度也影响优化求解速度。实际使用中我建议你在脚本里加一行fprintf(地图尺寸: %d x %d\n, rows, cols)打印一下如果行列数超过 300就要考虑增大栅格尺寸否则后面的路径规划计算会很吃力。这个栅格地图本身就是一个 0-1 矩阵后续的所有路径生成都可以在矩阵层面操作不需要反复调用inpolygon这是性能上的一个关键设计。3. 双层路径规划routesPlanning.m里的全覆盖扫描与 Dijkstra 转移3.1 为什么单层规划搞不定植保任务植保路径规划的本质矛盾在于全覆盖扫描需要“之”字形逐行扫但农田里的障碍物电线杆、树、灌溉设施会把整块区域切碎。如果只用一套全局规划算法去处理要么因为过度绕行导致效率急剧下降要么算法复杂度高到无法在无人机机载电脑上实时运行。所以routesPlanning.m和routesPlanning2.m采用了双层规划结构这是植保路径规划场景中的主流方案。第一层是行扫描规划生成横向的弓字形路径覆盖整个可作业区域。routesPlanning.m用的是最简单的逐行覆盖策略扫描方向默认沿地图的 Y 轴即南北方向。第二层是转移路径规划负责连接两条相邻的扫描行或者绕开单个障碍物。这一步直接调用了项目里的dijkstra.m在栅格地图上计算最短转移路径。分层设计的最大好处是调试方便——你单独跑通行扫描后再单独调转移逻辑不会出现一改参数全局崩掉的情况。function [waypoints, distance] routesPlanning(map, startPos, spacing) % map: plantMap 生成的 0-1 栅格地图 % startPos: 起点坐标 [row, col] % spacing: 航线间距栅格单位 [rows, cols] size(map); waypoints []; totalDist 0; currentPos startPos; direction 1; % 1 表示从上到下-1 表示从下到上 % 按列扫描即沿 X 方向逐行推进 col startPos(2); while col cols % 在当前列中找到可飞行的上下边界 colData map(:, col); validIdx find(colData 1); if ~isempty(validIdx) if direction 1 targetY max(validIdx); else targetY min(validIdx); end % 生成当前行的航点序列 rowPoints generateRowPoints(validIdx, col, direction); waypoints [waypoints; rowPoints]; totalDist totalDist abs(targetY - currentPos(1)) ... abs(col - currentPos(2)); currentPos [targetY, col]; end direction -direction; col col spacing; end distance totalDist; end注意这个版本的实现有几个工程上很实在的细节。第一spacing参数的单位是栅格数它决定了航线之间的间隔实际间隔等于spacing * gridSize你应该根据无人机有效喷幅来反推这个值。第二每一列中找到可飞行区域的上下边界后用direction变量实现蛇形扫描这样无人机永远不会从地块外部绕路回来省下大量无效飞行。第三generateRowPoints这个辅助函数负责把连续的有效栅格压缩成线段端点——它输出的是一串结构化航点而不是每个栅格都来一个点这对后续的转弯半径约束非常重要。3.2 Dijkstra 做转移路径的三个细节dijkstra.m在这个项目里不是用来做全覆盖主路径的它在转移阶段——也就是无人机需要从一个扫描行的末端飞到下一个相邻扫描行起点的时候——负责计算最短避障路径。这样说你可能就理解了如果全覆盖扫描是战术层面的“如何走最优”那么 Dijkstra 是战略层面的“如何最省地机动到这个位置”。这里我对dijkstra.m的实现做了一个简化版抽离让你看清它的核心结构function [path, cost] dijkstra(map, startIdx, goalIdx) % map: 0-1 栅格地图1 表示可通行 % startIdx/goalIdx: 起终点的一维线性索引 [rows, cols] size(map); N rows * cols; dist inf(N, 1); prev zeros(N, 1); visited false(N, 1); dist(startIdx) 0; for i 1:N % 选择未访问且距离最小的节点 minDist inf; u -1; for j 1:N if ~visited(j) dist(j) minDist minDist dist(j); u j; end end if u -1 || u goalIdx break; end visited(u) true; % 获取四邻域邻居 [r, c] ind2sub([rows, cols], u); neighbors []; if r 1, neighbors [neighbors, sub2ind([rows, cols], r-1, c)]; end if r rows, neighbors [neighbors, sub2ind([rows, cols], r1, c)]; end if c 1, neighbors [neighbors, sub2ind([rows, cols], r, c-1)]; end if c cols, neighbors [neighbors, sub2ind([rows, cols], r, c1)]; end for k 1:length(neighbors) v neighbors(k); if ~visited(v) map(v) 1 % 权重水平或垂直移动为 1这里没有走对角线 alt dist(u) 1; if alt dist(v) dist(v) alt; prev(v) u; end end end end有一个很关键的工程取舍这里用的是四邻域而不是八邻域。也就是说无人机转移时只能上下左右走不能斜穿。原因很简单——植保机在低速转移状态下斜向运动会带来不必要的偏航角变化而且四邻域搜索出来的路径天然是曼哈顿式的转弯点更规整方便后续用plotCircle.m做平滑。代价是路径长度会略长一些但在农田场景下这个代价完全值得。如果某个区域可行但走不过去去看map(v) 1这个条件是否把所有该通的路径都标成 1 了——这种情况最常见的问题出在plantMap栅格化时把窄通道误杀掉了。3.3routesPlanning2.m的升级点代价函数里加了转弯惩罚routesPlanning.m和routesPlanning2.m的差异是这个项目里最值得对比研究的地方。routesPlanning.m的优化目标是路径总长度最短它不区分直行和转弯。routesPlanning2.m则在路径规划时显式引入了代价函数给每个转弯动作增加了一个惩罚项。这个惩罚项模拟的是植保无人机转弯时的时间损耗和能量损耗进入转弯需要减速、完成转弯后需要加速恢复作业速度这两个过程耗费的时间和功率都远高于稳定直飞。代价函数的核心结构长这样function totalCost pathCost(trajectory, turnPenalty) % trajectory: 航点序列 % turnPenalty: 转弯惩罚权重单位米 totalCost 0; for i 2:length(trajectory) - 1 % 计算当前点到下一个点的方向向量 v1 trajectory(i, :) - trajectory(i-1, :); v2 trajectory(i1, :) - trajectory(i, :); % 向量归一化后求夹角 cosTheta dot(v1, v2) / (norm(v1) * norm(v2) eps); theta acos(max(-1, min(1, cosTheta))); % 夹角度数 % 转弯惩罚与夹角成线性关系 totalCost totalCost norm(v1) turnPenalty * (theta / pi); end endturnPenalty的单位是米它从算法视角回答了“一个 90 度转弯等价于多少米直飞成本”这样一个策略性问题。我把turnPenalty从 0 调到 10、再从 10 调到 30 做过对比实验惩罚系数为 0 时规划出来的路径会频繁做小角度转向来规避障碍物惩罚系数偏大时路径会倾向于绕远路来换取更少的转弯次数导致总飞行距离明显上升。一个经验取值是对于轴距 450 级别的植保机turnPenalty设为 815 效果最好这个区间内路径总长度和转弯次数能达到平衡。注意如果路径中出现了接近 180 度的转向也就是掉头说明这地方需要单独检查UAV2.m里的转弯半径参数是否和路网宽度匹配。4. 障碍物几何检测与避障修正obstacleAvoidance.m与线段相交函数族4.1 几何工具箱的拆分逻辑distanceOfTwoLines.m、dotInLine.m、intersectionOrNot.m、crossBarrierOrNot.m这四个文件单独看都很小但它们拼在一起就是一个完整的避障判定链。在 MATLAB 里做路径规划的初学者经常犯的一个错误是遇到“判断路径是否穿过障碍物”这个需求时直接在图纸上肉眼判断或者简单比较坐标范围。这种方式在障碍物形状不规则、路径斜着穿过地块的时候完全不可靠。这个项目把几何判定拆分成四个独立函数逻辑清晰且可以单独做单元测试这是很成熟的工程习惯。intersectionOrNot.m负责判断两条线段是否相交这是最底层的几何原语。crossBarrierOrNot.m则在这一基础上判断一条线段也就是无人机的一段路径是否穿越一个障碍物多边形——方法很直接把障碍物的每条边拿出来和路径线段做一次intersectionOrNot只要有一条边相交就算穿越。dotInLine.m判断一个点是否在线段上主要用来处理边界情况比如路径刚刚擦过障碍物的顶点。distanceOfTwoLines.m计算两条线段的最短距离这个函数在路径微调时非常有用——你希望新的路径和障碍物保持一个安全距离不是“不碰就行”而是“至少离 5 米”。function isCross crossBarrierOrNot(p1, p2, obs) % p1, p2: 路径线段的两个端点平面坐标 % obs: 障碍物多边形顶点M x 2 矩阵 isCross false; M size(obs, 1); for i 1:M q1 obs(i, :); q2 obs(mod(i, M) 1, :); if intersectionOrNot(p1, p2, q1, q2) isCross true; return; end end end遍历障碍物每一条边这是最保守也最可靠的做法。确保obs是闭合多边形也就是说第一个顶点和最后一个顶点按顺序排列即可mod函数在这里保证了最后一条边是从最后一个顶点绕回第一个顶点。如果障碍物数量多这个函数会被频繁调用性能瓶颈很明显——每判断一条路径就要遍历所有障碍物的所有边。优化思路是在调用前先做一个包围盒粗筛只对路径线段 bounding box 有重叠的障碍物做精确求交。这样可以把单次判定的时间复杂度从 O(N×M) 降到接近 O(N)。4.2obstacleAvoidance.m的迭代优化机制obstacleAvoidance.m是这个项目里最核心的避障文件它的工作方式不是一步到位生成一条全新的路径而是在已有的全覆盖扫描路径上做局部修正。这个设计思路非常聪明——全覆盖扫描的主框架是统一的弓字形结构如果因为某个障碍物就把整条路径推倒重排代价太大而且会产生大量不可预测的碎片路径。正确的做法是只把穿过障碍物的那些局部航段找出来用绕行路径替换。function newPath obstacleAvoidance(waypoints, obstacles, safeDist) % waypoints: 原始路径航点序列 % obstacles: 障碍物列表 % safeDist: 期望的安全距离米 newPath []; i 1; while i size(waypoints, 1) p1 waypoints(i, :); p2 waypoints(i1, :); crossed false; for j 1:length(obstacles) if crossBarrierOrNot(p1, p2, obstacles{j}) crossed true; % 找到路径与障碍物的交点然后做绕行 detourPoints planDetour(p1, p2, obstacles{j}, safeDist); newPath [newPath; detourPoints]; break; end end if ~crossed newPath [newPath; p1]; end i i 1; end newPath [newPath; waypoints(end, :)]; end这个算法最值得注意的地方是当检测到一段路径穿越障碍物时它只修改这一段其他航点全都保持不动。planDetour函数做的事情是找到线段和障碍物的两个交点然后沿着障碍物边界外扩safeDist生成一个半圆形的绕行弧线。safeDist的建议取值是无人机翼展的 1.5 倍——太小了喷出的药液会打到障碍物上或者无人机下洗气流会把障碍物附近的药液吹散太大了会让路径长度显著增加。我一般在这个项目里跑的时候用safeDist 1.5 * 机架对角线跑完以后用distanceOfTwoLines.m验证一下最小间距有没有小于安全阈值如果有就自动把局部绕行点向外推。还有一招可以看plotCircle.m——它会把障碍物画成一个圆这个圆的内切半径其实就是safeDist你可以在面板上直观地确认路径与障碍物的距离是不是符合预期。5. 验证与调参技巧从仿真曲线反推算法参数的边界这个项目最容易让新手迷惑的不是写代码而是拿到结果后如何判断“这个路径好还是不好”。我提供一个通用做法用航点坐标跑一遍完整的仿真循环用UAV2.m和UAV3.m的运动学模型去模拟无人机沿着轨迹飞行的真实运动然后输出一条时间-位置曲线。如果你看到曲线在某个位置出现剧烈的速率跳变说明路径生成时没有考虑到无人机的最小转弯半径或者spacing设置得过小导致转弯太急。最常见的处理是调大routesPlanning.m里的spacing或者调大pathCost里的turnPenalty。验证覆盖率是另一个容易出问题的地方。我在实践里常用这样一个验证脚本把规划出来的航点序列恢复成一条连续轨迹以此检查有没有漏掉的地块。这在植保领域叫“覆盖率验证”也就是说要确定每个栅格单元至少被航迹覆盖一次。function coverage checkCoverage(map, waypoints, sprayWidth) % map: 0-1 栅格地图 % waypoints: 规划出的航点序列 % sprayWidth: 无人机有效喷幅米 covered zeros(size(map)); for i 1:size(waypoints, 1) - 1 p1 waypoints(i, :); p2 waypoints(i1, :); dist norm(p2 - p1); numSamples ceil(dist / (sprayWidth / 4)); for t 0:numSamples pt p1 (p2 - p1) * (t / numSamples); r round(pt(1)); c round(pt(2)); if r 1 r size(map, 1) c 1 c size(map, 2) % 以喷幅为半径把覆盖圆内的栅格标记为已覆盖 [rows, cols] meshgrid(max(1,r-2):min(size(map,1),r2), ... max(1,c-2):min(size(map,2),c2)); covered(rows, cols) 1; end end end coverage sum(covered(:) map(:)) / sum(map(:)); end这个脚本的关键在于numSamples的密度设置——它是根据喷幅动态调整的喷幅越大采样点越少。覆盖率低于 98% 时优先检查是不是障碍物绕行把某块区域隔离出来了这种情况下光调参数没用得手动在路径里补一个航点。覆盖率达到 100% 反而要警惕因为那通常意味着你设置的安全距离过小路径紧贴着障碍物边缘实际飞行中一个侧风就把航线吹偏了。一个比较稳妥的目标是覆盖率 98%99.5%同时最小离障距离不小于安全阈值的 1.2 倍。想观察到离障距离可以在最后打印一下distanceOfTwoLines.m输出的最小距离值与safeDist摆在对比表里。确认无误后再导出航点文件交给地面站执行。本文还有配套的精品资源点击获取
