简介：这份MATLAB程序面向机器人路径规划的学习者与研究者，针对人工势场法（APF）中角度计算偏差和障碍物附近震荡两大痛点给出改进实现。程序通过优化角度转换函数提升方向判断精度，并调整引力、斥力权重与势场衰减因子，引入平滑策略抑制反复摆动，使机器人更平稳地逼近目标。压缩包共4个文件，均为m脚本，整体约2KB，涵盖主程序、斥力计算、角度计算与引力计算等核心模块，结构精简便于逐行研读算法逻辑。资源已有8414人学习下载，读者可据此理解势场构建、角度校正与运动控制的完整流程，并在此基础上扩展多机器人协作或动态避障等功能，适合具备一定MATLAB基础、希望深入掌握APF算法实现细节的读者参考。

1. 从一次机器人原地打转说起：这份 MATLAB 人工势场法改进程序到底改了什么

去年帮一个做移动机器人课程设计的朋友看代码，他跑的是最经典的人工势场法（Artificial Potential Field，APF），结果小车在离目标点还有半米的地方开始原地画圈，角度来回抖，速度指令像抽风一样。这不是个例，几乎所有手写过 APF 的人都会撞上两个坎：一是目标不可达（Goal Non-Reachable with Obstacles Nearby，GNRON），二是局部极小值导致的震荡。这份 MATLAB 程序的核心价值，就是针对角度震荡和路径抖动做了工程化的修正，把一份“能跑但没法看”的 demo 级代码，改成了轨迹平滑、角度收敛的可用版本。它适合正在做路径规划课程设计、毕设，或者想把 APF 作为对比基线塞进论文里的同学。你拿到手不需要从头推导势场函数，直接替换参数就能看到效果差异，但前提是你得先搞明白它到底动了哪几刀。

2. 人工势场法的数学底子：引力、斥力与合力怎么算才不会翻车

2.1 引力场与斥力场的基本形式

经典 APF 的出发点很朴素：目标点对机器人产生引力，障碍物产生斥力，合力方向就是下一步的运动方向。引力场通常取二次型：

$$U_{att}(q) = \frac{1}{2} k_{att} \cdot \rho_{goal}^2(q)$$

其中 $k_{att}$ 是引力增益，$\rho_{goal}(q)$ 是机器人到目标点的欧氏距离。对 $q$ 求负梯度就得到引力：

$$F_{att}(q) = -k_{att} \cdot \rho_{goal}(q) \cdot \nabla \rho_{goal}(q)$$

斥力场用的是分段函数，只在障碍物影响半径 $\rho_0$ 内生效：

$$U_{rep}(q) = \begin{cases} \frac{1}{2} k_{rep} \left( \frac{1}{\rho_{obs}(q)} - \frac{1}{\rho_0} \right)^2 & \rho_{obs} \leq \rho_0 \ 0 & \rho_{obs} > \rho_0 \end{cases}$$

对应的斥力是 $U_{rep}$ 对 $q$ 的负梯度。这里有个细节：原始斥力只考虑障碍物距离，没考虑目标点距离，这正是 GNRON 问题的根源——当目标点离障碍物很近时，斥力会把机器人往外推，永远到不了。

2.2 改进思路：把目标距离塞进斥力函数

这份程序的关键改动之一，是在斥力项里乘上一个与目标距离相关的因子。常见做法是把斥力函数改成：

$$U_{rep}(q) = \begin{cases} \frac{1}{2} k_{rep} \left( \frac{1}{\rho_{obs}} - \frac{1}{\rho_0} \right)^2 \cdot \rho_{goal}^n & \rho_{obs} \leq \rho_0 \ 0 & \rho_{obs} > \rho_0 \end{cases}$$

$n$ 一般取 1 或 2。这样当机器人靠近目标时，$\rho_{goal}$ 变小，斥力被自然削弱，目标点附近的合力不再被障碍物主导。程序里 $n$ 默认设成 2，实测比 $n=1$ 收敛更稳，但 $n$ 太大会让机器人在障碍物附近过于“勇敢”，容易擦碰。我一般建议先在 $n=1$ 和 $n=2$ 之间试，看轨迹是否贴障碍物太近。

2.3 角度震荡的根源：合力方向在临界点反复横跳

角度震荡的本质是合力方向在障碍物影响边界附近不连续。当机器人刚好在 $\rho_0$ 边缘时，斥力从有到无突变，合力方向瞬间跳变，控制器输出的角速度就会来回摆。程序里用了两个手段压制：一是对斥力做平滑过渡，把硬阈值改成软阈值；二是对输出的角度做一阶低通滤波。下面这段是核心计算片段：

% 计算引力（目标点方向） dist_goal = norm(robot_pos - goal_pos); if dist_goal > 0 F_att = k_att * (robot_pos - goal_pos); % 指向目标 else F_att = [0; 0]; end % 计算斥力（遍历所有障碍物） F_rep = [0; 0]; for i = 1:num_obstacles dist_obs = norm(robot_pos - obs_pos(i, :)); if dist_obs <= rho0 && dist_obs > 0 % 改进项：乘以目标距离的 n 次方 rep_mag = k_rep * (1/dist_obs - 1/rho0) * (dist_goal^n) / (dist_obs^2); % 方向：从障碍物指向机器人 dir_vec = (robot_pos - obs_pos(i, :)) / dist_obs; F_rep = F_rep + rep_mag * dir_vec; end end % 合力 F_total = F_att + F_rep; % 角度平滑：一阶低通滤波 if norm(F_total) > 0 desired_angle = atan2(F_total(2), F_total(1)); % 角度差归一化到 [-pi, pi] angle_diff = wrapToPi(desired_angle - prev_angle); smooth_angle = prev_angle + alpha * angle_diff; % alpha 是滤波系数 prev_angle = smooth_angle; else smooth_angle = prev_angle; end

逻辑说明：k_att和k_rep是增益系数，rho0是障碍物影响半径，n是目标距离因子的指数。alpha是低通滤波系数，取值在 0 到 1 之间，越小越平滑但响应越慢。程序里默认alpha = 0.3，这个值是我试过在 0.1 到 0.5 之间比较均衡的点。参数说明：k_att太大会让机器人冲过头，太小则靠近目标慢；k_rep太大会导致机器人不敢靠近障碍物，太小则容易撞。常见做法是先固定k_att = 1，调k_rep到轨迹刚好不碰障碍物，再回头微调k_att。

3. 把程序跑起来：参数配置、仿真循环与轨迹可视化

3.1 环境初始化与参数表

拿到程序后，第一步不是直接 run，而是先把参数区看清楚。程序开头一般有一个params结构体或一组独立变量。下面这张表是我整理的关键参数及其作用，你照着改的时候心里有数：

初始化部分通常长这样：

% 参数初始化 k_att = 1.0; k_rep = 100; rho0 = 1.5; n = 2; alpha = 0.3; step_size = 0.1; max_iter = 2000; % 起点、终点、障碍物 start_pos = [0; 0]; goal_pos = [10; 10]; obs_pos = [3, 3; 5, 6; 7, 4; 8, 8]; % 每行一个障碍物坐标 num_obstacles = size(obs_pos, 1); % 状态初始化 robot_pos = start_pos; prev_angle = 0; path = robot_pos';

这段没什么玄学，但要注意obs_pos的格式：每行一个障碍物，列数是维度。程序默认是二维，如果你要改成三维，所有向量和atan2都要换成对应的三维角度计算，工作量不小，建议先二维跑通再说。

3.2 主循环与终止条件

主循环负责迭代更新位置，终止条件一般是“到目标点距离小于阈值”或“达到最大迭代次数”。程序里还加了一个“连续多步位置变化极小”的判定，用来提前退出死循环：

for iter = 1:max_iter % 计算合力（调用上一节的函数） [F_total, prev_angle] = compute_force(robot_pos, goal_pos, obs_pos, ... k_att, k_rep, rho0, n, alpha, prev_angle); % 如果合力为零，说明卡住了，直接退出 if norm(F_total) < 1e-6 disp('合力为零，可能陷入局部极小值'); break; end % 沿合力方向移动一步 move_dir = F_total / norm(F_total); robot_pos = robot_pos + step_size * move_dir; path = [path; robot_pos']; % 到达目标判定 if norm(robot_pos - goal_pos) < 0.2 disp(['到达目标，迭代次数：', num2str(iter)]); break; end % 位置变化极小判定（连续 20 步移动距离小于阈值） if iter > 20 && norm(path(end,:) - path(end-20,:)) < 0.05 disp('位置几乎不变，可能震荡或卡住'); break; end end

逻辑说明：move_dir是合力单位方向，step_size控制每步走多远。到达阈值 0.2 是我一般用的值，太小会导致迭代次数暴涨，太大则轨迹末端粗糙。位置变化极小判定里的 20 步和 0.05 阈值可以根据你的场景调，如果机器人正常走但速度慢，这个判定可能误杀，建议先注释掉，确认轨迹正常后再打开。

3.3 轨迹可视化与角度曲线

跑完仿真不画图等于白跑。程序里一般会画两张图：一张是二维轨迹图，标出起点、终点、障碍物和机器人走过的路径；另一张是角度随时间的变化曲线，用来检查震荡是否被压制：

figure(1); plot(path(:,1), path(:,2), 'b-', 'LineWidth', 1.5); hold on; plot(start_pos(1), start_pos(2), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g'); plot(goal_pos(1), goal_pos(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r'); for i = 1:num_obstacles viscircles(obs_pos(i,:), rho0, 'Color', [0.5 0.5 0.5], 'LineStyle', '--'); end axis equal; grid on; xlabel('X'); ylabel('Y'); title('APF 路径轨迹'); legend('路径', '起点', '终点', '障碍影响范围'); figure(2); angles = atan2(diff(path(:,2)), diff(path(:,1))); plot(angles, 'k-'); grid on; xlabel('迭代步'); ylabel('运动方向角 (rad)'); title('角度变化曲线');

逻辑说明：viscircles画的是障碍物影响半径的虚线圆，方便你判断机器人是不是在边界附近抖。角度曲线如果出现密集的上下尖刺，说明滤波系数alpha还是太大，或者rho0设置导致合力方向频繁跳变。我一般会先把alpha降到 0.1 看效果，如果曲线平滑了但机器人绕大圈，再往回加。

4. 避坑与排查：角度震荡、目标不可达、局部极小值的现场记录

4.1 现象：机器人快到目标时开始画圈，角度曲线高频抖动

原因：目标点附近有障碍物，原始斥力没有目标距离因子，导致合力方向在障碍物影响边界反复跳变。解决：确认程序里斥力项是否乘了dist_goal^n，如果没有，加上去并把n设为 2。同时把alpha从默认值降到 0.2 以下，观察角度曲线是否变平滑。如果还抖，检查rho0是不是太大，导致机器人在目标附近仍然被多个障碍物影响。

4.2 现象：机器人停在离目标一段距离的地方不动，合力为零

原因：典型的局部极小值，引力被斥力完全抵消。解决：程序里虽然有“合力为零退出”的判定，但退出不等于解决。常见做法是加一个随机扰动，当检测到连续多步位置变化极小时，给合力方向加一个小的随机角度偏转，让机器人“抖”出极小值区域。扰动幅度一般取 0.1 到 0.3 弧度，太大轨迹会乱，太小出不来。

4.3 现象：机器人贴着障碍物边缘走，最近距离小于安全阈值

原因：k_rep太小或者rho0太小，斥力不足以在安全距离外把机器人推开。解决：先把k_rep调大 20% 到 50%，看轨迹是否远离障碍物。如果调大后机器人绕远路太夸张，说明rho0也需要同步调大，让斥力更早介入。注意k_rep和rho0是耦合的，不要单独猛调一个。

4.4 现象：迭代次数跑满 max_iter 还没到目标，但轨迹看起来在靠近

原因：step_size太小，或者到达阈值 0.2 太严格。解决：先把step_size从 0.1 提到 0.15 或 0.2，看迭代次数是否下降。如果轨迹开始跳过障碍物，说明步子太大，需要折中。到达阈值可以放宽到 0.3，但别超过 0.5，否则轨迹末端会明显偏离目标。

4.5 现象：改了障碍物坐标后程序报错或轨迹异常

原因：obs_pos的维度或格式不对，比如把二维坐标写成了行向量导致norm计算出错。解决：确认obs_pos是N×2矩阵，每行一个障碍物。如果从文件读取，检查有没有表头或空行。另外，障碍物坐标如果和起点或终点重合，dist_obs会为零，除零会导致NaN，程序里虽然有dist_obs > 0的判断，但最好在初始化时检查一遍。

5. 进阶调参与验证：用指标说话，别靠眼睛猜

5.1 三个量化指标：路径长度、最大曲率、到达时间

眼睛看轨迹平滑不够，我一般会算三个数：路径总长度、路径上相邻三点形成的最大曲率、以及到达目标的迭代步数。路径长度反映绕路程度，最大曲率反映角度震荡的剧烈程度，迭代步数反映收敛速度。程序里加几行就能算：

% 路径长度 path_len = sum(sqrt(sum(diff(path).^2, 2))); % 最大曲率（用相邻三点外接圆半径近似） max_curv = 0; for i = 2:size(path,1)-1 p1 = path(i-1,:); p2 = path(i,:); p3 = path(i+1,:); a = norm(p2-p1); b = norm(p3-p2); c = norm(p3-p1); if a*b*c > 0 % 三角形面积 s = (a+b+c)/2; area = sqrt(max(s*(s-a)*(s-b)*(s-c), 0)); if area > 1e-6 curv = 4*area / (a*b*c); max_curv = max(max_curv, curv); end end end fprintf('路径长度: %.2f\n', path_len); fprintf('最大曲率: %.4f\n', max_curv); fprintf('迭代步数: %d\n', size(path,1)-1);

逻辑说明：曲率用外接圆半径的倒数近似，area是三角形面积，curv越大说明转弯越急。参数说明：max_curv如果超过 0.5，基本可以判定角度震荡没压住，需要回去调alpha或n。路径长度和迭代步数用来对比不同参数组合，别只看单次结果。

5.2 参数扫描：固定两个，调一个

调参最忌讳三个参数一起动。我一般固定k_att = 1、step_size = 0.1，然后扫k_rep从 50 到 200，步长 25，记录每组的三项指标。下面是一个简单的扫描框架：

k_rep_list = 50:25:200; results = zeros(length(k_rep_list), 3); for idx = 1:length(k_rep_list) % 这里调用你的主仿真函数，返回 path % [path, iter] = run_apf(k_rep_list(idx), ...); % 然后计算三项指标填入 results end % 找出最大曲率最小且路径长度可接受的那组

逻辑说明：扫描的目的是找到“曲率小、路径不过长”的平衡点。常见做法是先把k_rep扫一遍，选曲率最小的区间，再在这个区间里微调rho0。注意每次扫描前把随机种子固定，否则扰动项会让结果不可比。

5.3 验证方法：换一组障碍物布局再跑

参数调好后，别只在一组障碍物上验证。我习惯至少换三组布局：稀疏障碍、密集障碍、目标点靠近障碍物。如果三组都能在 500 步内到达且最大曲率低于 0.3，这套参数才算稳。程序里可以把障碍物坐标写成几个预设场景，用循环跑：

scenarios = { [3,3; 5,6; 7,4; 8,8], ... % 稀疏 [2,2; 3,4; 4,3; 5,5; 6,4; 7,6], ... % 密集 [9,9; 8,8; 7,7] ... % 目标附近有障碍 }; for s = 1:length(scenarios) obs_pos = scenarios{s}; % 跑仿真并记录指标 end

逻辑说明：scenarios是元胞数组，每个元素是一组障碍物坐标。跑完后对比三组指标，如果某一组明显变差，说明参数对该布局过拟合，需要放宽rho0或降低n。从那以后我每次调完 APF 参数，都强制走一遍三场景验证，不然不敢把结果写进报告。希望帮到你。

本文还有配套的精品资源，点击获取