APF 人工势场：把“走向目标”和“远离障碍”写成一组力

摘要｜本文介绍人工势场法在移动机器人局部规划中的基本思想，结合 MATLAB 实现解析吸引力、排斥力、目标点选择、速度控制与运动学更新，并讨论局部极小和参数敏感性等实际问题。

前言

全局规划解决的是“从起点到终点大体应该走哪条路”，局部规划更关心机器人下一时刻应该怎么动。人工势场法（Artificial Potential Field，APF）就是一种非常直观的局部规划思想：把目标点看成一个持续吸引机器人的“引力源”，把障碍物看成会推开机器人的“斥力源”，机器人最终沿着合力方向不断运动。

这种方法的优势是结构简单、计算量小，而且和“避障”这件事的直觉非常一致。它也有明显短板，例如局部极小、目标附近障碍引起的振荡以及参数对轨迹形态影响较大。理解这些优缺点以后，再看 DWA、RPP 等算法，会更容易明白为什么后续局部规划方法需要引入速度空间、曲率约束或预测模型。

原理讲解

1. 吸引势场与吸引力

设机器人当前位置为

局部目标点为

最简单的吸引势函数可以写成：

其中 为吸引增益。对势函数取负梯度，就得到指向目标的吸引力：

当前实现没有直接使用距离大小，而是先把方向向量归一化：

attr_force = tgt_pos - cur_pos;
if ~all(attr_force == 0)
    attr_force = attr_force ./ norm(attr_force);
end

这样做意味着吸引项主要提供“方向”，而不是让距离越远、吸力越大。

2. 排斥力

设机器人到某个障碍点的距离为 ，障碍影响半径为 。经典人工势场常使用：

相应排斥力大小与

有关。代码中的 getRepulsiveForce 正是按照这一形式计算所有障碍点贡献，只保留影响半径 d_0 内的障碍，再把各方向排斥力求和并归一化。

最终合力写成：

当前参数为：

zeta = 1.0;
eta  = 0.8;
d_0  = 1.5;

这意味着吸引项权重略高于排斥项，障碍物只有进入 1.5 个地图距离单位以内才会产生排斥作用。

3. 为什么还需要全局路径

这份 APF 实现并不是直接始终把最终 goal 当作唯一吸引源，而是读取全局规划器输出的 path，再沿路径向前寻找一个局部目标点。

代码使用：

p_window = 0.5;

当当前路径点距离机器人已经小于窗口范围时，plan_idx 会继续向前移动，直到找到一个更合适的跟踪点。这样做很重要，因为纯 APF 如果只看最终终点，遇到大型障碍时很容易被障碍势场带入不理想区域；引入全局路径后，局部控制只需要在全局路线附近完成跟踪和避障。

4. 从“力”转换成速度方向

合力不是直接当作控制量，而是先和机器人当前速度矢量相加：

随后归一化，并限制到最大线速度：

max_v = 0.4;

再通过：

得到期望朝向。

因此 APF 实际上完成的是“给出下一步应该朝哪个方向走”，而最终的线速度、角速度仍由两个简单控制器生成。

代码详解

1. 输入接口

函数接口为：

function [pose, traj, flag] = apf_plan(start, goal, varargin)

其中 start、goal 不只是二维坐标，而是包含姿态角的三维状态：

[x, y, theta]

额外参数需要提供：

'path', global_path
'map',  grid_map

这里的 path 来自全局规划，map 用于提取障碍物。

2. 障碍物坐标提取

代码通过：

obs_index = find(map==2);

找到障碍栅格，再转换成二维坐标集合 obstacle。后续排斥力并不是针对障碍轮廓计算，而是对所有障碍栅格中心计算距离。

这会带来一个实际影响：障碍物越大，附近参与排斥力叠加的栅格点越多，因此障碍几何尺寸本身会间接影响排斥方向。

3. 速度控制并不是完整 PID

虽然 linearController 和 angularController 都写出了 P、I、D 三项，但当前参数中线速度控制实际为：

k_v_p = 1.00;
k_v_i = 0.00;
k_v_d = 0.00;

因此线速度部分本质上是比例控制。角速度部分为：

k_w_p = 1.00;
k_w_i = 0.00;
k_w_d = 0.01;

只有很小的微分项。

这说明 APF 决定的是“运动方向”，低层速度跟踪只是相对简单的辅助控制。

4. 大角度误差时先原地转向

如果：

abs(theta_d - robot.theta) > pi / 2

机器人不会继续向前，而是：

u = [0, w];

先原地旋转。这一逻辑可以避免机器人在朝向误差过大时一边快速前进一边急转，从而减小明显的横向偏离。

5. 运动学模型

代码采用差速/独轮车式二维运动模型：

因此控制器最终输出始终是：

u = [v, w]

即线速度与角速度。

完整 MATLAB 实现说明

对应源码文件为：

local_planner/apf_plan.m

核心函数包括：

apf_plan
├─ getAttractiveForce
├─ getRepulsiveForce
├─ linearController
├─ angularController
└─ f

阅读时建议重点关注 getRepulsiveForce、局部路径点选择逻辑以及从合力到 theta_d 的转换过程。本文说明依据当前 apf_plan.m 的实际实现展开。

总结与思考

APF 最大的优点是直觉清楚：目标负责“拉”，障碍负责“推”，最终方向由合力决定。它也说明了局部规划的一个基本问题——机器人不是只要知道路径就够了，还必须在当前姿态、障碍分布和速度限制下不断生成下一步控制量。

但人工势场并不天然保证全局收敛。吸引力和排斥力可能在某些位置相互抵消，形成局部极小；在狭窄通道或目标附近有障碍时，也可能出现往复振荡。因此 APF 更适合作为理解局部避障的起点，而不是所有场景下的万能方案。