简介面向无人机路径规划与避障算法学习者这份matlab仿真代码利用势场法实现二维平面内的无人机避障与导航在终点施加引力、障碍物施加斥力改进后的算法可有效避免传统方法易陷入局部极值的问题适合算法初学者及机器人方向学生参考。资源共有6个文件以matlab脚本为主包含主程序Main.m、引力计算与斥力计算等核心函数另有1张效果图片和1个仿真操作录像avi格式压缩包仅259KB轻量但五脏俱全。目前已有676人学习下载足见其实用性。通过该资源读者可以获得完整的可运行代码与配套操作演示既能快速复现路径规划效果也能结合代码理解势场法的数学建模与改进策略对后续开展无人机或移动机器人路径规划研究有一定启发。1. 从避障到势场无人机二维路径规划的两个反直觉结论很多人在第一次跑完势场法的仿真后最困惑的不是代码而是无人机为什么会在半路“站住不动”。这通常不是程序bug而是传统人工势场法固有的局部极小值问题。这套基于势场法的无人机路径规划MATLAB仿真正好用改进后的算法把这个问题绕开了。它实现的是二维平面内的无人机避障路径规划给终点加引力给障碍物加斥力无人机在合力场里一步步飞到目标。仿真基于MATLAB 2022a附带完整的操作录像和四个核心函数文件。如果你在做课程设计、复现文献里的路径规划方法或者正在为局部极小值头疼这个工程值得拆一拆。它用很小的代码量展示了改进式势场法的关键思路比单纯看公式清晰得多。2. 势场法建模引力、斥力与合力方程的Matlab表达2.1 人工势场法的数学基础与三个关键参数人工势场法把二维空间看成一张由势场构成的地图。目标点产生“引力势场”吸引无人机靠近障碍物产生“斥力势场”把无人机推开。无人机在每一点受到的合力就是引力向量和斥力向量的叠加。只要合力的方向是下降最陡的方向无人机就会沿着一条平滑曲线向目标移动。引力势场一般用二次型来表示U_att(q) 0.5 * ξ * ||q - q_goal||²这里 q 是无人机当前位置q_goal 是目标点ξ 是引力增益。对这个势场求负梯度就得到引力向量F_att(q) -∇U_att -ξ * (q - q_goal)也就是说引力大小与无人机到目标的距离成正比方向始终指向目标点。这和弹簧模型很像离得越远拉力越大。斥力势场只作用在障碍物周围的一段范围以内常用形式是U_rep(q) 0.5 * η * (1/ρ - 1/ρ0)²当 ρ ≤ ρ0U_rep(q) 0当 ρ ρ0其中 ρ 是无人机到障碍物的最近距离η 是斥力增益ρ0 是障碍物影响半径。求负梯度后斥力方向从障碍物指向无人机距离越近斥力越强。在MATLAB里这三个参数分别是引力增益、斥力增益、斥力作用半径。它们直接决定了路径的形态。整理成一个表方便对照参数符号作用调大后的直接影响引力增益ξ代码中常写为 k_att控制无人机飞向目标的拉力强度路径更直、更短但可能切入障碍物斥力增益η代码中常写为 k_rep控制障碍物的排斥强度避障更安全但路径容易摆动斥力作用半径ρ0代码中常写为 d0控制斥力的最远影响范围范围过大则绕远路过小则反应迟钝这三个参数相互影响。例如调大引力增益后如果斥力增益不变无人机会更偏向目标忽视障碍物必须同步增大斥力增益或斥力作用半径才能让避障依旧有效。2.2 compute_angle.m 的作用角度计算不是可有可无的工具源码里有一个 compute_angle.m 文件很多初看代码的人不理解它存在的意义以为只是一个 atan2 的封装。实际上它在路径规划里有两个实际用途一是把力向量转换成方位角方便在图上绘制受力箭头二是判断当前力的方向是否指向目标区域辅助后续逻辑判断。这个函数的典型实现是function ang compute_angle(v) % v 是二维列向量 [x; y] % 返回向量与 x 轴正方向的夹角单位为弧度范围 [-pi, pi] ang atan2(v(2), v(1)); end使用 atan2 而不是 atan是因为 atan 无法区分向量位于第三象限还是第四象限。比如向量 (-1, -1) 和 (1, -1) 的 atan 结果都是 -π/4但实际上方向差了 90°。在无人机路径规划里力的方向稍有偏差轨迹就会偏出几个身位。所以这个文件虽然短却是后续所有方向判断的地基。在主循环里可以这样调用它来显示受力方向ang compute_angle(F_total); quiver(pos(1), pos(2), F_total(1), F_total(2), r, LineWidth, 1.5);这里的 quiver 是MATLAB自带的向量场绘制函数红色箭头表示当前合力方向。加入这一行后能清楚看到每一步受力变化比只看轨迹点直观很多。2.3 改进型斥力场为什么要在斥力里乘上目标距离因子传统势场法最大的坑是无人机在某个位置受到的引力和斥力大小相等、方向相反合力为零于是它永远停留在那里。这种情况在障碍物正对目标点时尤其常见。为了打破这个平衡项目采用的改进方式是在斥力势场中加入“无人机到目标点距离”的调节因子。改进后的斥力势场表达式为U_rep(q) 0.5 * η * (1/ρ - 1/ρ0)² * ||q - q_goal||ⁿ当 ρ ≤ ρ0U_rep(q) 0当 ρ ρ0新增加了一个乘数项 ||q - q_goal||ⁿ。无人机距离目标点越近这一项越小斥力被压得越低。当无人机到达目标点附近时斥力趋近于零引力占绝对主导从而避开“力的平衡陷阱”。指数 n 一般取 2也可以取 1区别在于对目标距离的敏感度。n2 时路径更平滑n1 时近距离下斥力削弱得不够明显路径可能还是有点绕。这个改进直接影响了 compute_repulsion.m 的输入参数。函数的参数里必须包含 goal 和 n而不只是无人机位置和障碍物位置。如果哪个版本里斥力函数没有这两个参数大概率还是传统方案局部极小值问题会重新出现。2.4 从连续公式到离散循环合力的归一化处理势场法的公式是连续时间域的设计但在MATLAB里仿真时只能按离散步长更新位置。每一步的计算公式通常写成pos_new pos_old step * F_total / ||F_total||其中 step 是步长F_total 是当前合力向量。把合力除以自身模长是为了让无人机每一步移动的距离固定为 step而不是被合力大小带飞。如果不做归一化当无人机靠近目标时引力很大步长会被拉得极长路径会在目标点附近来回振荡甚至越过目标点。这一步是“仿真不发散”的关键。很多初学者的代码不是算法错而是没有归一化合力导致一个障碍物附近的斥力瞬间达到 10 的 10 次方无人机一步飞出地图。下面的章节会在主循环里具体展示怎么写。3. 代码拆解主循环、引力函数与改进型斥力函数的实现3.1 Main.m 主循环骨架初始化、迭代、终止这个工程里的 Main.m 是整个仿真的调度中心。从文件名可以看出它负责初始化地图、设置起点终点、调用两个力的函数、更新无人机位置并把轨迹画出来。一个结构清晰的主循环通常长这样%% 基于势场法的二维无人机路径规划主程序 clear; clc; close all; pos [0; 0]; % 无人机初始位置二维列向量 [x; y] goal [10; 10]; % 目标点位置 obs_list [5 6 7; 5 4 5]; % 每列一个障碍物第一行x坐标第二行y坐标 k_att 1.0; % 引力增益 k_rep 0.8; % 斥力增益 d0 2.0; % 斥力作用半径 n 2; % 改进距离指数 step 0.1; % 单步步长 max_iter 2000; % 最大迭代次数 figure; hold on; axis equal; plot(goal(1), goal(2), g^, MarkerSize, 10); plot(pos(1), pos(2), ro, MarkerSize, 6); for i 1:size(obs_list,2) rectangle(Position, [obs_list(1,i)-0.3, obs_list(2,i)-0.3, 0.6, 0.6], ... FaceColor, [0.7 0.7 0.7]); end traj pos; % 轨迹记录矩阵第一列是起点 for iter 1:max_iter F_att compute_Attract(pos, goal, k_att); F_rep zeros(2,1); for i 1:size(obs_list,2) F_rep F_rep compute_repulsion(pos, obs_list(:,i), goal, k_rep, d0, n); end F_total F_att F_rep; % 归一化合力保证每一步移动距离为 step pos pos step * F_total / max(norm(F_total), 1e-6); traj [traj, pos]; plot(pos(1), pos(2), b., MarkerSize, 4); % 到达判断 if norm(pos - goal) 0.2 fprintf(到达目标点迭代次数: %d\n, iter); break; end end plot(traj(1,:), traj(2,:), k-, LineWidth, 1.5);这里的 obs_list 是一个 2×m 的矩阵m 是障碍物数量。循环里用 size(obs_list,2) 而不是 length是为了避免只有一个障碍物时 length 返回2的歧义。每个障碍物单独调用一次 compute_repulsion再把所有斥力累加。归一化那行代码是大关键。max(norm(F_total), 1e-6)这个写法是为了防止合力为零时出现除零错误。合力为零又没到达目标时说明无人机陷入了无力状态此时保持原位置不动等待后续策略介入。3.2 compute_Attract.m引力就是弹簧力这个源文件只有几行核心逻辑就是我在第二章里写的那条负梯度公式。常见实现如下function F_att compute_Attract(pos, goal, k_att) % pos : 2x1 无人机当前位置 % goal : 2x1 目标点位置 % k_att: 引力增益 % F_att: 2x1 引力向量方向指向目标点 F_att k_att * (goal - pos); end这行代码的含义是引力向量等于增益系数乘以“目标点减当前位置”。当无人机在 (0,0)目标在 (10,10) 时F_att k_att * [10;10]方向直接指向目标。距离越远向量模长越大。有的版本会把引力除以距离使引力变成恒力。那种写法适合需要控制最大飞行速度的场合。在配合归一化步长的主循环里线性引力已经够用而且不需要额外处理接近目标时除零的问题。3.3 compute_repulsion.m改进型斥力的完整实现这个文件是整套仿真里最值得细看的部分。它的实现直接关系到局部极小值问题能不能被真正解决。常见写法如下function F_rep compute_repulsion(pos, obstacle, goal, k_rep, d0, n) % pos : 2x1 无人机当前位置 % obstacle : 2x1 单个障碍物位置 % goal : 2x1 目标点位置 % k_rep : 斥力增益 % d0 : 斥力作用半径 % n : 目标距离改进指数 % F_rep : 2x1 斥力向量方向从障碍物指向无人机 d norm(pos - obstacle); F_rep zeros(2,1); if d d0 d 1e-6 direction (pos - obstacle) / d; rep_mag k_rep * (1/d - 1/d0) * (1/d^2); improve norm(pos - goal)^n; F_rep rep_mag * direction * improve; end end重点解释几行第一direction。它是从障碍物指向无人机的单位向量。斥力天然要推开无人机所以方向是“远离障碍物”即(pos - obstacle) / d。第二rep_mag。这一项来自传统斥力势场对距离求导包含(1/d - 1/d0)和1/d^2两个因子。距离 d 越接近01/d^2增长得越快斥力会迅速变得很大这是防碰撞的主要保障。第三improve。这是改进项也就是目标距离因子。无人机离目标越远这一项越大斥力越强离目标越近这一项越小斥力被压制。当无人机已经到达目标点时improve 为0斥力完全消失只有引力从根本上不会出现目标点附近的引力斥力平衡。3.4 多障碍物场景下的调用方式在 Main.m 里每个障碍物都要单独算一次斥力。这背后有一个潜在的问题如果无人机同时靠近多个障碍物斥力会叠加可能会把无人机“推”进一个狭小通道里出现明显的抖动。这时可以调整 d0 到较小值比如1.5让重叠的斥力场区域变小也可以改 k_rep让单个障碍物的斥力上限降低。顺便说明compute_angle.m 在主循环里不是必须的但它的调用时机通常是在合力计算完之后用来画箭头或记录航向角。例如ang compute_angle(F_total);这条语句本身不会影响轨迹但调试时非常有用。你可以把 ang 打印出来观察无人机在哪个位置出现了方向突变从而定位参数设置不合理的区域。4. 仿真运行与参数调优从“跑通”到“不抖、不困、不撞”4.1 在MATLAB 2022a里跑通这套代码的步骤拿到压缩包后先把所有文件解压到一个纯英文路径下比如D:\UAV_potential_field。然后启动MATLAB 2022a在“当前文件夹”窗口切换到该目录。注意一定要看左下角或顶部的当前路径因为MATLAB只在当前文件夹和搜索路径里找函数文件。如果当前文件夹不对运行 Main.m 时会直接报错“未定义函数 compute_Attract”这只是路径问题不是算法问题。在命令行窗口输入Main然后回车。此时会弹出一个二维坐标图红色圆点是起点绿色三角是目标点灰色方块是障碍物蓝色点迹就是无人机的运动轨迹。整个仿真迭代速度很快几百次迭代会在几秒内完成最后会有一条黑色实线把轨迹连起来。操作录像里演示的是Windows Media Player播放你完全可以按上面的命令手动复现一遍。录像还有一个好处它展示了修改参数后重新运行的过程能够直观看到不同 k_rep 对轨迹的影响。4.2 参数对路径形态的影响与调参顺序现象可能原因调整策略无人机直接穿过障碍物k_rep过小或d0过小增大k_rep到1.2以上或增大d0到3无人机在障碍物前大幅抖动k_rep过大或step过大减小k_rep到0.5以下step调为0.05无人机长期绕远路d0太大减小d0让斥力作用范围更收敛无人机停在半路不动陷入局部极小值检查改进项n是否为2或增加随机扰动轨迹在目标点来回震荡step过大把step降为0.1以内或改用动态步长我的调参习惯是先把 step 固定为0.1k_att 固定为1然后从 k_rep0.5 开始逐步增大每次增加0.1直到路径不再碰撞障碍物。接着再调 d0从2.0开始如果路径在障碍物附近转得太急就把d0调大让斥力场提前起作用如果路径绕得太远就调小。4.3 仿真发散与数值不稳定的排查仿真发散是初学者最常见的问题飞机要么一步冲出坐标系要么轨迹上出现NaN。通常由两个原因导致。第一目标点与无人机当前位置重合。如果 compute_Attract 返回的是零向量而你的代码还把合力做了归一化就会出现除零。所以主循环里的max(norm(F_total), 1e-6)就是为了防止这种情况。第二障碍物坐标与无人机坐标几乎一致。在 compute_repulsion 里如果 d 小于一个很大的值例如 0.001斥力会达到一个天文数字导致无人机一步弹飞。因此代码里加了d 1e-6的保护条件。即使这样d 在 1e-3 到 1e-5 之间时斥力依然非常大此时需要把 step 调得更小或者限制斥力上限。排查发散时可以在循环里加一行调试输出if mod(iter, 20) 0 fprintf(iter%d, F_total[%.3f, %.3f], pos[%.3f, %.3f]\n, ... iter, F_total(1), F_total(2), pos(1), pos(2)); end这段代码每20步打印一次合力大小和位置。如果看到合力数值突然从 1 跳到 1e8那就直接把断点设在那一行检查是哪一个障碍物造成的斥力爆破。5. 打破局部极小值改进势场法的三个实用策略5.1 策略一在斥力函数里加入目标距离因子项目采用的就是这个方法也就是 compute_repulsion.m 里的improve norm(pos - goal)^n。当无人机靠近目标点时斥力被削弱引力保持主导。这个改进的优点是代码简洁缺点是在距离目标很远但离障碍物很近时improve变得很大斥力可能比原方案还猛烈导致无人机在远离目标的地方躲避过于积极。解决办法是把 n 从2改小到0.5降低距离因子的放大效果。5.2 策略二在合力停滞时添加随机扰动如果无人机已经陷入局部极小值表现为若干步几乎没有位移可以在主循环里加入一个扰动计数器if iter 10 norm(pos - pos_prev) 0.02 F_total F_total 0.3 * randn(2,1); end这会让无人机在合力为零的位置获得一个随机方向的推力打破力的平衡。扰动系数0.3不是定死的如果扰动太弱无人机还是会靠回平衡点太强路径会变得杂乱。一般从0.1开始试着调。5.3 策略三修改斥力的作用方向改为障碍物切线方向更工程化的做法是完全放弃点状斥力场把障碍物看成边长为定值的矩形或圆形斥力方向始终指向远离障碍物边界的切线方向。这样在局部极小值位置无人机会被“推”到障碍物侧面而不是停在正前方。这个方法的代价是需要额外计算障碍物的几何边界代码量会翻倍。对单纯演示算法而言前两种策略已经可以应对绝大多数情况。最后如果你想把仿真结果保存下来做报告可以在主循环结束后运行save(trajectory.mat, traj, goal, obs_list);下次直接用load(trajectory.mat)就能重新绘制轨迹不需要再跑一遍完整迭代。本文还有配套的精品资源点击获取