简介：基于人工势场法的路径规划Matlab项目提供了一套完整的机器人路径规划源码实现，面向学习移动机器人导航、无人机航线规划或自动驾驶技术的开发者与科研人员。程序将目标点建模为吸引力场、障碍物建模为排斥力场，通过合力引导智能体避开障碍并抵达目标，且支持用户自定义目标位置与障碍物分布，便于针对不同场景进行算法验证。压缩包共5个文件，其中4个为Matlab脚本、1张示例图片，整体仅68KB，核心包含主程序、排斥力计算函数、吸引力计算函数及角度计算辅助函数，结构简洁、逻辑清晰，易于阅读和二次开发。资源已有935人学习查看，尤其适合正在学习人工势场法原理、准备课程设计或开展路径规划相关课题的读者，通过阅读和修改源码可快速掌握APF的建模过程、参数调节思路与实际应用方法，也可作为教学演示的实用参考。
1. 人工势场法为什么适合先跑通Matlab路径规划
自动泊车、巡检机器人、无人机穿障碍这些场景，落地时都会先拿一张二维坐标图试算。基于人工势场法的路径规划Matlab，核心逻辑只有一句话：目标点产生引力，障碍物产生斥力，机器人沿合力方向走。引力场把机器人往终点拉，斥力场把机器人从障碍物旁边推开，叠加之后形成一条绕行轨迹。这个算法最早用于机械臂避障，后来被广泛应用在智能机器人路径规划算法教学中，是理解混合A*、强化学习方法之前更值得先上手的基线。
但真实跑起来大概率会遇到两种反直觉结果：路径在半路停住不动，或者到了目标附近来回抖。前者叫局部极小值，后者叫目标不可达，在泊车路径规划算法和动态避障小车路径规划里都很常见。理解这两个失效边界，比一开始就追求复杂算法更有价值。下面直接给出一套可复现的Matlab最小实现，把公式、参数和排错顺序一次讲清楚。
2. 人工势场法的势场公式与参数设计
2.1 引力场与斥力场的数学表达
在写代码之前，先把势场的数学形式固定下来。人工势场法把机器人当作势场中的一个质点，目标点是势场的最低谷，障碍物是势场的高峰。机器人每一步都朝“下降最快的方向”走，也就是负梯度方向。
最常用的引力势函数是二次型：
U_att(q) = 0.5 * k_att * d(q, q_goal)^2
其中d(q, q_goal)是当前位置q到目标点q_goal的欧氏距离，k_att是引力增益。这个式子的好处在于它的负梯度是线性力：
F_att = -k_att * (q - q_goal)
注意q - q_goal是从目标指向机器人的向量，加负号后方向反过来，变成从机器人指向目标。离目标越远，引力越大，机器人不会在远端磨蹭；接近目标时引力趋近于零，这正好配合后面要做的步长归一化。
斥力势函数我习惯用带影响半径的形式：
U_rep(q) = 0.5 * k_rep * (1/d_obs - 1/rho_0)^2，当 d_obs <= rho_0 时；否则为0。
d_obs是机器人到障碍物的距离，rho_0是斥力影响半径，k_rep是斥力增益。展开负梯度后得到斥力：
F_rep = k_rep * (1/d_obs - 1/rho_0) * (1/d_obs^2) * (q - q_obs)/d_obs
这个力的方向由(q - q_obs)决定，也就是从障碍物指向机器人，正好把机器人推开。注意里面有1/d_obs^2项，距离越近斥力增长得越猛，这是保证不碰撞的关键。如果直接拿1/d_obs，碰撞点附近的力不够大，在步长较大的情况下很容易穿过障碍物。
2.2 合力计算与参数表
得到引力斥力之后，合力就是：
F_total = F_att + F_rep
迭代更新则写成：
q_new = q_old + step * F_total / norm(F_total)
除以norm(F_total)意味着每一步走的长度正好是step，方向完全由合力方向决定。如果不做归一化，靠近目标时引力变小，步长会越来越短；障碍物附近斥力增大，又可能一步弹得很远。这两种情况都会让路径看起来很不稳定。
| 参数 | 含义 | 常见初值 | 调试建议 |
|---|---|---|---|
| k_att | 引力增益 | 0.5~2.0 | 增大使路径更贴直线，过大会切入障碍物 |
| k_rep | 斥力增益 | 10~100 | 增大避障更保守，过大会绕远路 |
| rho_0 | 斥力影响半径 | 2~3倍障碍物直径 | 过小可能漏避，过大会挤压可行空间 |
| step | 迭代步长 | 0.1~0.2 | 越小越平滑，越大越容易跳过窄缝 |
| max_iter | 最大迭代次数 | 1000~3000 | 留足余量应对局部极小值造成的死循环 |
这个表直接决定后文Matlab代码里的参数取值。我的经验是，先固定k_att=1.0、step=0.15，然后把k_rep从20往上试，每次只改一个量，能很快摸清当前地图的调试方向。
% 计算合力的核心子函数 function F = computeForce(q, goal, obstacles, params) k_att = params.k_att; F_att = -k_att * (q - goal); % 引力指向目标 F_rep = [0, 0]; for i = 1:size(obstacles, 1) obs = obstacles(i, :); d_obs = norm(q - obs); if d_obs < params.rho_0 % 斥力方向从障碍物指向机器人 dir = (q - obs) / d_obs; coeff = params.k_rep * (1/d_obs - 1/params.rho_0) ... / d_obs^2; F_rep = F_rep + coeff * dir; end end F = F_att + F_rep; end这里的computeForce返回的是二维平面上的合力向量，第2行直接利用负梯度公式，第8到第13行循环累加所有影响半径内的斥力。要把障碍物半径和机器人半径一起处理时，把d_obs替换成到膨胀后障碍物表面的距离即可，公式中不需要额外改动。
2.3 局部极小值与目标不可达
两个典型的失效场景值得在写代码前先有意识。第一个是局部极小值：当多个障碍物的斥力与引力在某个点互相抵消时，合力模长会变成零。机器人在数学上停在原地，仿真里表现为连续几个迭代几乎不移动。第二种是目标不可达：目标点附近存在障碍物时，目标点本身的引力被斥力压过，机器人停在一个距离目标还有半米左右的位置抖动。
这两个问题不是Matlab代码写法造成的，是势场法的固有缺陷。第4章会给出实用化改进，但在第一版实现里，先用max_iter和距离阈值兜住异常，能避免脚本死循环。
3. 用Matlab实现人工势场法路径规划的最小可运行代码
3.1 地图建模与障碍物表示
实际项目中，地图通常来自激光雷达或视觉建图的结果，格式可以是栅格地图、多边形的边，也可以直接在全局坐标里列出障碍物的中心点。为了复现代码方便，这里把障碍物表示成一个N x 2的坐标矩阵，每一行是某个障碍物在全局坐标系下的位置。这样不但兼容Matlab的plot和scatter绘图指令，也方便在调试时手动增删障碍物。
需要注意，机器人必须被当作有体积的对象。常见做法是先把机器人半径加到每个障碍物半径上，得到“膨胀后障碍物”，再让质心点在膨胀后的距离上计算斥力。最小实现里可以省略这一步，但如果你做的是泊车路径规划，这一步省略会导致规划出来的路径贴墙太近，真车执行时碰撞。
3.2 完整的Matlab规划函数与可视化脚本
直接给一个可运行的最小版本。先写规划函数，再写调用脚本。
function path = apf_planner(start, goal, obstacles, params) % APF_PLANNER 基于人工势场法的路径规划 % start: 起点坐标 [x, y] % goal: 目标点坐标 [x, y] % obstacles: 障碍物坐标矩阵，每行一个 [x, y] % params: 结构体，包含 k_att, k_rep, rho_0, step, % max_iter, d_goal_thres q = start; path = q; for i = 1:params.max_iter if norm(q - goal) < params.d_goal_thres break; end F = computeForce(q, goal, obstacles, params); if norm(F) < 1e-4 % 合力为零，说明陷入局部极小值，提前退出 warning('陷入局部极小值，iter=%d', i); break; end q = q + params.step * F / norm(F); path(end+1, :) = q; end end注意第9行的警告分支：如果合力模长小于1e-4，直接退出并提示，这比让脚本空转要好。第12行把新位置追加到path末尾，最终得到一条按步长离散的路径点序列。
%% 参数设置 clc; clear; close all; start = [0, 0]; goal = [10, 10]; % 障碍物坐标，单位米 obstacles = [ 3, 2; 4, 7; 7, 3; 6, 8; 8, 6 ]; params.k_att = 1.0; params.k_rep = 30; params.rho_0 = 2.5; params.step = 0.15; params.max_iter = 2000; params.d_goal_thres = 0.3; path = apf_planner(start, goal, obstacles, params); %% 可视化 figure('Color', 'w'); hold on; axis equal; grid on; plot(obstacles(:,1), obstacles(:,2), 'ks', ... 'MarkerFaceColor', 'k', 'MarkerSize', 12); plot(start(1), start(2), 'go', ... 'MarkerFaceColor', 'g', 'MarkerSize', 10); plot(goal(1), goal(2), 'ro', ... 'MarkerFaceColor', 'r', 'MarkerSize', 10); plot(path(:,1), path(:,2), 'b-', 'LineWidth', 1.5); xlabel('x (m)'); ylabel('y (m)'); legend('障碍物', '起点', '终点', '规划路径', ... 'Location', 'best');这段脚本把障碍物、起点、终点和规划出来的路径画在同一张图里。路径数组path第1列是x坐标，第2列是y坐标，plot函数直接把离散点连成折线。如果路径明显贴近某个障碍物，先检查rho_0和k_rep，再检查是否缺少膨胀处理。
3.3 参数怎么调：从能跑到跑得好
第一版跑通后，参数调整的优先级应该是：先保证不碰障碍物，再缩短路径长度，最后优化曲线平滑度。
我的习惯是保持k_att和step不变，把k_rep从20、40、60依次试。k_rep偏小时，路径会紧贴障碍物边缘擦过去，看起来很近但没碰；k_rep偏大时，路径会绕一个大弯，长度明显增加。rho_0则决定“多远就开始躲”：设成0.5米时，机器人要贴近障碍物才转弯，适合窄道通行；设成3米时，机器人老远就开始绕，全局路径变得曲折。
步长是很容易被忽略的一项。step设0.3以上，路径看起来会有明显折点，而且在窄通道里可能一步越过障碍物到另一侧，产生“穿墙”效果。step设0.05以下，路径光滑但迭代次数成倍增长，场景大时Matlab绘图都会变卡。对10米级别的二维地图，0.1到0.15是比较稳的区间。如果地图尺寸扩大到100米，step可以同步放大到0.5甚至1.0，但要同步检查障碍物最小间距是否大于step。
4. 动态避障与实用化：人工势场法的三种改进路线
4.1 动态障碍物场景的速度斥力接入
人工势场法最常被质疑的地方是不能应对动态障碍物。其实只要把障碍物坐标在每一帧更新，再用上一节的基础函数重新计算力和路径，就能实现最简单的动态避障小车路径规划。问题在于不考虑相对运动时，机器人可能被一个快速逼近的障碍物撞上，因为斥力只跟距离有关，跟接近速度没关系。
常见做法是给斥力加一项速度分量。机器人相对障碍物的速度v_rel = v_robot - v_obs，当v_rel在两者连线上投影为负，表示正在靠近，此时按接近速率放大斥力。实现上，在computeForce里多传入一个动态障碍物矩阵，每个元素包含位置和速度，然后累加一项：
F_rep_vel = k_vel * v_rel_proj * dir
其中v_rel_proj是相对速度在障碍物指向机器人方向上的投影，k_vel是速度斥力增益。这样，距离还远但在高速接近的障碍物也有足够大的推力。这个技巧在机器人路径规划的工程实现里很常见，Matlab原型里验证效果也直观，尤其适合做“小车遇人急停”的仿真。
4.2 局部极小值的三种常用解决策略
第一种是随机扰动。检测到连续N个迭代位移小于某个阈值时，给合力叠加一个随机方向的试探力，相当于在势场里推一把。优点是改动最小，缺点是有概率把机器人推进另一个陷阱。
第二种是改进斥力函数。在斥力中乘上目标距离因子d_goal^n，n通常取2。目标越近，斥力衰减得越明显，使得目标点的合力总是指向目标，解决目标不可达。这个改进对泊车路径规划效果明显，因为车位目标点经常贴着路沿和旁车。
% 带目标距离因子的斥力计算（改进版） d_goal = norm(q - goal); coeff = params.k_rep * (1/d_obs - 1/params.rho_0) / d_obs^2; coeff = coeff * d_goal^2; % 目标越近，斥力压得越低 F_rep = F_rep + coeff * (q - obs) / d_obs;对比第2章的基础公式，这里只多了一行：把coeff乘上d_goal^2。这个改动会带来一个副作用：远离目标时斥力被放大，路径可能变得曲折，所以实际项目中常把n取1.5，平衡避障能力和可达性。
第三种是虚拟中间目标点。当检测到局部极小值时，在陷阱区外围放一个临时目标点，让机器人先脱离当前凹陷区域，再恢复原始目标点继续规划。这类思路在ROS2路径规划的导航栈里也有对应实现，只是虚拟点设置逻辑更复杂。
提示：这三种策略不是互斥的。实际项目里我一般先加目标距离因子，再保留卡停检测，最后才考虑虚拟点。前两步足够覆盖大多数静态地图。
4.3 人工势场法与混合A星、强化学习的适用边界
经常有人问：既然人工势场法有局部极小值，为什么不直接上混合A*或者强化学习。这要回到场景约束来看。
| 维度 | 人工势场法 | 混合A* | 强化学习 |
|---|---|---|---|
| 全局最优 | 不保证 | 搜索可行域内较优 | 由奖励函数决定 |
| 计算资源 | 很低，单轮毫秒级 | 中等，依赖搜索规模 | 训练成本高，推理较快 |
| 动态环境 | 需手动加速度项 | 重新规划开销明显 | 适合高频在线决策 |
| 实现复杂度 | 低 | 中高 | 高 |
| 典型场景 | 快速原型、教学、轻量避障 | 自动驾驶泊车、窄路转弯 | 复杂博弈、未知环境 |
在自动驾驶泊车路径规划里，混合A*因为有前轮转向约束，能生成可执行的泊车轨迹，这是人工势场法做不到的。但在无人机路径规划算法原型验证和课堂演示里，人工势场法几分钟就能出一张轨迹图，用于理解合力、势场、避障这些核心概念足够。强化学习方法适合环境规则不固定、需要在线决策的场景，不过训练阶段的数据收集和奖励设计成本需要单独评估。
5. 人工势场法复现实验时先看哪几个量：验证与调参技巧
5.1 三分钟诊断：势能序列、目标距离与卡停日志
路径规划实验里，只看最终曲线很容易漏判问题。我在跑通最小实现后，会加一小段日志记录，把每一步的势能、目标距离和合力模长存下来。
% 记录并输出诊断信息 energy = zeros(params.max_iter, 1); goal_dist = zeros(params.max_iter, 1); for i = 1:params.max_iter F = computeForce(q, goal, obstacles, params); goal_dist(i) = norm(q - goal); energy(i) = 0.5 * params.k_att * goal_dist(i)^2; if norm(F) < 1e-4 && goal_dist(i) > 0.5 fprintf('[stuck] iter=%d q=(%.2f, %.2f) dist=%.2f\n', ... i, q(1), q(2), goal_dist(i)); break; end if goal_dist(i) < params.d_goal_thres break; end q = q + params.step * F / norm(F); end上面这段可以作为调试脚本独立保存，替换掉原函数里的循环部分。卡停、震荡、过冲都能从日志里看出时间点。
| 诊断信号 | 正常表现 | 异常表现 | 可能原因 |
|---|---|---|---|
| 势能序列 | 单调下降并趋平 | 周期起伏 | k_rep / k_att 比例失衡 |
| 目标距离 | 持续减小到阈值内 | 在阈值外抖动 | 目标不可达 |
| 合力模长 | 到达目标时收敛为0 | 中途突然为0 | 局部极小值 |
另一个技巧是画势场等高线。用meshgrid生成网格点，逐个计算总势能，再用contour画出来，能直接看到局部极小值的凹坑位置。把规划路径叠上去后，一眼就能判断卡停在哪个区域。等高线图建议只在调试时打开，因为网格点一多，Matlab绘图开销明显。
调参口诀可以记成：路径贴障碍就加k_rep，路径绕远就减k_rep，卡停先查合力是否归零，震荡先查目标距离是否停在阈值外。这套流程在动态避障小车路径规划里同样适用。
本文还有配套的精品资源，点击获取
