引言
在机器人学和游戏AI中，路径规划（Path Planning）是使智能体从起点安全到达终点的核心问题。与A等基于图搜索的全局规划算法不同，人工势场法（Artificial Potential Field）*将环境建模为虚拟的”力场”：目标产生吸引力，障碍物产生排斥力，智能体在这些虚拟力的合力作用下向目标移动。
这一方法由Khatib于1986年提出，因其计算简单、实时性强、易于并行化而广泛应用于机器人导航、无人机控制和游戏角色移动。本文将用Java实现完整的人工势场路径规划系统，并深入分析其数学原理与局部极小值问题。
算法原理
势场建模
假设智能体在二维平面上运动，位置为 $\mathbf{p} = (x, y)$。
吸引力（Atttractive Potential）：目标位置 $\mathbf{p}_{goal}$ 产生吸引智能体的势场
$$U_{att}(\mathbf{p}) = \frac{1}{2} k_{att} |\mathbf{p} – \mathbf{p}_{goal}|^2$$
对应的吸引力为势场的负梯度：
$$\mathbf{F}{att}(\mathbf{p}) = -\nabla U{att} = -k_{att}(\mathbf{p} – \mathbf{p}_{goal})$$
排斥力（Repulsive Potential）：障碍物 $\mathbf{p}_{obs}$ 在距离 $d_0$ 内产生排斥
$$U_{rep}(\mathbf{p}) = \begin{cases} \frac{1}{2} k_{rep} \left(\frac{1}{d} – \frac{1}{d_0}\right)^2 & d \leq d_0 \ 0 & d > d_0 \end{cases}$$
其中 $d = |\mathbf{p} – \mathbf{p}_{obs}|$。对应的排斥力：
$$\mathbf{F}{rep}(\mathbf{p}) = k{rep} \left(\frac{1}{d} – \frac{1}{d_0}\right) \frac{1}{d^2} \frac{\mathbf{p} – \mathbf{p}_{obs}}{d}$$
合力：
$$\mathbf{F}{total} = \mathbf{F}{att} + \sum \mathbf{F}_{rep}$$
运动更新
根据合力方向更新智能体位置：
$$\mathbf{p}{t+1} = \mathbf{p}_t + \alpha \frac{\mathbf{F}{total}}{|\mathbf{F}_{total}|}$$
其中 $\alpha$ 是步长。重复迭代直到到达目标或达到最大步数。
Java实现
import java.util.*;
/**
* 人工势场法路径规划实现
* 支持多点障碍物避障与路径平滑
*/
public class ArtificialPotentialField {
// 二维向量类
static class Vector2D {
double x, y;
Vector2D(double x, double y) {
this.x = x;
this.y = y;
}
Vector2D add(Vector2D other) {
return new Vector2D(x + other.x, y + other.y);
}
Vector2D subtract(Vector2D other) {
return new Vector2D(x - other.x, y - other.y);
}
Vector2D multiply(double scalar) {
return new Vector2D(x * scalar, y * scalar);
}
double magnitude() {
return Math.sqrt(x * x + y * y);
}
Vector2D normalize() {
double mag = magnitude();
if (mag < 1e-10) return new Vector2D(0, 0);
return new Vector2D(x / mag, y / mag);
}
double distance(Vector2D other) {
return subtract(other).magnitude();
}
@Override
public String toString() {
return String.format("(%.3f, %.3f)", x, y);
}
}
// 圆形障碍物
static class Obstacle {
Vector2D center;
double radius;
Obstacle(Vector2D center, double radius) {
this.center = center;
this.radius = radius;
}
double distanceTo(Vector2D point) {
return point.distance(center) - radius;
}
}
// 路径规划器参数
static class APFConfig {
double kAtt = 1.0; // 吸引力系数
double kRep = 100.0; // 排斥力系数
double d0 = 5.0; // 障碍物影响距离
double stepSize = 0.3; // 运动步长
double goalThreshold = 0.5; // 到达目标的阈值距离
int maxSteps = 1000; // 最大迭代步数
double minForceThreshold = 0.001; // 最小力阈值（检测局部极小）
}
private final APFConfig config;
private final List<Obstacle> obstacles;
public ArtificialPotentialField(APFConfig config, List<Obstacle> obstacles) {
this.config = config;
this.obstacles = obstacles;
}
/**
* 计算吸引力（朝向目标）
*/
public Vector2D computeAttractiveForce(Vector2D current, Vector2D goal) {
Vector2D diff = goal.subtract(current);
return diff.multiply(config.kAtt);
}
/**
* 计算排斥力（远离障碍物）
*/
public Vector2D computeRepulsiveForce(Vector2D current) {
Vector2D totalRep = new Vector2D(0, 0);
for (Obstacle obs : obstacles) {
double d = obs.distanceTo(current);
if (d <= config.d0 && d > 0.01) {
// 排斥力方向：从障碍物指向当前位置
Vector2D direction = current.subtract(obs.center).normalize();
// 排斥力大小
double forceMag = config.kRep * (1.0 / d - 1.0 / config.d0) / (d * d);
totalRep = totalRep.add(direction.multiply(forceMag));
}
}
return totalRep;
}
/**
* 计算总合力
*/
public Vector2D computeTotalForce(Vector2D current, Vector2D goal) {
Vector2D fAtt = computeAttractiveForce(current, goal);
Vector2D fRep = computeRepulsiveForce(current);
return fAtt.add(fRep);
}
/**
* 执行路径规划
* @param start 起点
* @param goal 终点
* @return 规划的路径点列表
*/
public List<Vector2D> planPath(Vector2D start, Vector2D goal) {
List<Vector2D> path = new ArrayList<>();
Vector2D current = new Vector2D(start.x, start.y);
path.add(current);
int steps = 0;
Vector2D previousForce = new Vector2D(0, 0);
int oscillationCount = 0;
while (current.distance(goal) > config.goalThreshold && steps < config.maxSteps) {
Vector2D force = computeTotalForce(current, goal);
// 检测局部极小值：力的大小持续很小或震荡
if (force.magnitude() < config.minForceThreshold) {
System.out.println("警告：陷入局部极小值，位置 " + current);
break;
}
// 检测震荡
if (steps > 0) {
double dot = force.normalize().dotProduct(previousForce.normalize());
if (dot < -0.5) oscillationCount++;
else oscillationCount = Math.max(0, oscillationCount - 1);
if (oscillationCount > 10) {
System.out.println("警告：检测到震荡，尝试随机扰动");
force = randomPerturbation(force);
oscillationCount = 0;
}
}
// 更新位置
Vector2D direction = force.normalize();
current = current.add(direction.multiply(config.stepSize));
path.add(new Vector2D(current.x, current.y));
previousForce = force;
steps++;
}
return path;
}
/**
* 向量点积
*/
private double dotProduct(Vector2D other) {
return this.x * other.x + this.y * other.y;
}
// 辅助：在Vector2D中添加点积方法
private Vector2D randomPerturbation(Vector2D force) {
Random rand = new Random();
double angle = rand.nextDouble() * Math.PI * 2;
return new Vector2D(Math.cos(angle), Math.sin(angle)).multiply(force.magnitude());
}
/**
* 路径平滑：移除冗余的中间点
*/
public List<Vector2D> smoothPath(List<Vector2D> path) {
if (path.size() <= 2) return path;
List<Vector2D> smoothed = new ArrayList<>();
smoothed.add(path.get(0));
int i = 0;
while (i < path.size() - 1) {
// 寻找从path[i]可以直接到达的最远点
int farthest = i + 1;
for (int j = path.size() - 1; j > i; j--) {
if (isPathClear(path.get(i), path.get(j))) {
farthest = j;
break;
}
}
smoothed.add(path.get(farthest));
i = farthest;
}
return smoothed;
}
/**
* 检查两点之间是否有障碍物阻挡
*/
private boolean isPathClear(Vector2D from, Vector2D to) {
Vector2D dir = to.subtract(from);
double dist = dir.magnitude();
Vector2D unitDir = dir.normalize();
// 沿路径采样检查
int samples = (int) (dist / (config.stepSize * 0.5)) + 1;
for (int i = 0; i <= samples; i++) {
Vector2D point = from.add(unitDir.multiply(i * config.stepSize * 0.5));
for (Obstacle obs : obstacles) {
if (obs.distanceTo(point) < 0.1) {
return false;
}
}
}
return true;
}
/**
* 打印路径信息
*/
public static void printPathInfo(List<Vector2D> path, Vector2D goal) {
System.out.println("路径长度: " + path.size() + " 点");
System.out.println("起点: " + path.get(0));
System.out.println("终点: " + path.get(path.size() - 1));
System.out.println("距目标距离: " + path.get(path.size() - 1).distance(goal));
double totalDist = 0;
for (int i = 1; i < path.size(); i++) {
totalDist += path.get(i).distance(path.get(i - 1));
}
System.out.println("总移动距离: " + String.format("%.3f", totalDist));
}
/**
* 控制台可视化（简单ASCII）
*/
public static void visualizePath(List<Vector2D> path, List<Obstacle> obstacles,
Vector2D goal, int width, int height) {
char[][] grid = new char[height][width];
for (char[] row : grid) Arrays.fill(row, '.');
// 绘制障碍物
for (Obstacle obs : obstacles) {
int cx = (int) ((obs.center.x / 20.0) * width);
int cy = (int) ((obs.center.y / 20.0) * height);
int r = (int) ((obs.radius / 20.0) * Math.min(width, height));
for (int i = Math.max(0, cy - r); i < Math.min(height, cy + r); i++) {
for (int j = Math.max(0, cx - r); j < Math.min(width, cx + r); j++) {
if ((i - cy) * (i - cy) + (j - cx) * (j - cx) <= r * r) {
grid[i][j] = '#';
}
}
}
}
// 绘制路径
for (Vector2D p : path) {
int px = (int) Math.min(width - 1, Math.max(0, (p.x / 20.0) * width));
int py = (int) Math.min(height - 1, Math.max(0, (p.y / 20.0) * height));
grid[py][px] = '*';
}
// 绘制起点和终点
int gx = (int) Math.min(width - 1, Math.max(0, (goal.x / 20.0) * width));
int gy = (int) Math.min(height - 1, Math.max(0, (goal.y / 20.0) * height));
grid[gy][gx] = 'G';
// 绘制起点
Vector2D start = path.get(0);
int sx = (int) Math.min(width - 1, Math.max(0, (start.x / 20.0) * width));
int sy = (int) Math.min(height - 1, Math.max(0, (start.y / 20.0) * height));
grid[sy][sx] = 'S';
for (char[] row : grid) {
System.out.println(new String(row));
}
}
public static void main(String[] args) {
// 场景设置：20x20的地图
Vector2D start = new Vector2D(1, 1);
Vector2D goal = new Vector2D(18, 18);
List<Obstacle> obstacles = Arrays.asList(
new Obstacle(new Vector2D(10, 5), 2.5),
new Obstacle(new Vector2D(5, 12), 2.0),
new Obstacle(new Vector2D(15, 15), 2.0),
new Obstacle(new Vector2D(8, 8), 1.5)
);
APFConfig config = new APFConfig();
config.kAtt = 1.0;
config.kRep = 50.0;
config.d0 = 4.0;
config.stepSize = 0.2;
ArtificialPotentialField apf = new ArtificialPotentialField(config, obstacles);
System.out.println("=== 人工势场法路径规划 ===");
System.out.println("起点: " + start);
System.out.println("终点: " + goal);
System.out.println("障碍物数量: " + obstacles.size());
List<Vector2D> path = apf.planPath(start, goal);
printPathInfo(path, goal);
System.out.println("\n路径可视化:");
visualizePath(path, obstacles, goal, 40, 20);
// 路径平滑
System.out.println("\n平滑后路径:");
List<Vector2D> smoothed = apf.smoothPath(path);
printPathInfo(smoothed, goal);
visualizePath(smoothed, obstacles, goal, 40, 20);
}
}
算法分析
时间复杂度
每步迭代需要计算与所有障碍物的距离，设障碍物数量为 $m$，则每步时间为 $O(m)$。总时间复杂度为 $O(k \cdot m)$，其中 $k$ 是迭代步数。相比A*的 $O(V \log V)$，人工势场法在连续空间中通常更快，但可能无法找到最优路径。
空间复杂度
仅需存储障碍物列表和当前状态，空间复杂度为 $O(m)$，远小于基于网格的搜索算法。
局部极小值问题
人工势场法的最大缺陷是局部极小值陷阱：当吸引力与排斥力平衡时，合力为零，智能体停滞不前。
常见解决方案：
1. 随机扰动：在检测到局部极小时添加随机力
2. 模拟退火：逐步降低步长，允许 uphill 移动
3. 构造辅助势场：在局部极小点附近添加虚拟排斥力
4. 与全局规划结合：用A*生成全局路径点序列，人工势场负责局部避障
扩展应用
多智能体协调
在多机器人场景中，可以为每对机器人之间添加排斥势场，防止碰撞。这一方法被广泛应用于仓库机器人（如Amazon Kiva系统）的分布式调度中。
动态障碍物
对于移动障碍物，势场法天然支持动态更新：每帧重新计算排斥力即可。结合预测模型（如卡尔曼滤波），可以预判障碍物的未来位置，提前规避。
游戏AI中的移动
在RTS或MOBA游戏中，人工势场法常用于小兵和野怪的移动控制：
– 敌方防御塔产生强排斥
– 友方单位产生弱排斥（避免重叠）
– 敌方英雄产生中等吸引（攻击范围趋近）
与Navigation Mesh结合
现代游戏引擎（Unity、Unreal）通常将人工势场与NavMesh结合：NavMesh提供全局可行区域，人工势场处理动态避障，形成高效的混合路径规划系统。
总结
人工势场法以其数学简洁性和计算高效性，成为实时路径规划的重要工具。通过将复杂的避障问题转化为物理直觉清晰的力场计算，它实现了 $O(m)$ 每步的高效迭代。虽然局部极小值问题限制了其单独使用，但与全局规划算法的结合（如A*提供路标、势场负责局部避障）已成为工业界和游戏中的标准方案。掌握人工势场法，不仅是理解机器人运动规划的基础，更是设计游戏AI移动系统的实用技能。
