人工势场法：机器人路径规划的理论与代码实现

文章目录

• • 1. 引言
  • 2. 梯度的定义
  • 3. 人工势场法的基本原理
  • 4. 势场函数的构建
  • • 4.1 引力势场的推导
    • 4.2 斥力势场的推导
  • 5. 合力的计算与位置更新
  • • 5.1 合力的计算
    • 5.2 位置更新
  • 6. 算法缺陷与改进方法
  • • 6.1 目标不可达问题
    • 6.2 局部最优问题
    • 6.3 乒乓效应问题
  • 7. 代码实现
  • 8. 结论
  • 参考文献

1. 引言

人工势场法（Artificial Potential Field, APF）是一种经典的机器人路径规划算法，广泛应用于移动机器人导航、自动驾驶等领域。其核心思想是通过构建虚拟势场来引导机器人避开障碍物并朝向目标点移动。目标点产生引力，障碍物产生斥力，机器人在势场的合力作用下完成路径规划。

尽管人工势场法具有计算简单、实时性强的优点，但它也存在一些固有缺陷，例如目标不可达问题和局部最优问题。本文将详细介绍人工势场法的理论基础、势场函数构建、算法实现及其改进方法，并通过仿真实验验证改进后的算法性能。

2. 梯度的定义

梯度是多元函数在某一点处的方向导数最大的方向，其大小等于该方向导数的最大值。梯度指向函数值增长最快的方向，其模长表示函数值增长的速率。

对于一个二元函数 f ( x , y ) f(x, y) f(x,y)，其梯度 ∇ f \nabla f ∇f 定义为：

∇ f = ( ∂ f ∂ x , ∂ f ∂ y ) \nabla f = \left( \frac{\partial f}{\partial x}, \frac{\partial f}{\partial y} \right) ∇f=(∂x∂f​,∂y∂f​)

对于三元函数 $f(x, y, z) $，其梯度 ∇ f \nabla f ∇f 定义为：

∇ f = ( ∂ f ∂ x , ∂ f ∂ y , ∂ f ∂ z ) \nabla f = \left( \frac{\partial f}{\partial x}, \frac{\partial f}{\partial y}, \frac{\partial f}{\partial z} \right) ∇f=(∂x∂f​,∂y∂f​,∂z∂f​)

在人工势场法中，梯度用于计算引力和斥力，从而指导机器人的运动方向。

3. 人工势场法的基本原理

人工势场法的基本思想是将目标点视为吸引子，障碍物视为排斥子，通过构建势场函数来引导机器人避开障碍物并朝向目标点移动。具体来说：

• 引力势场：目标点对机器人产生吸引力，势场函数通常设计为与距离的平方成正比。
• 斥力势场：障碍物对机器人产生排斥力，势场函数通常设计为与距离的倒数成正比。

机器人在势场中受到的合力为引力和斥力的矢量和，根据合力更新机器人的位置，从而实现路径规划。

4. 势场函数的构建

4.1 引力势场的推导

引力势场 $ U_{att}(q) $ 通常定义为与机器人和目标点之间的距离的平方成正比的函数：

U a t t ( q ) = 1 2 η ρ 2 ( q , q g ) U_{att}(q) = \frac{1}{2} \eta \rho^2(q, q_g) Uatt​(q)=21​ηρ2(q,qg​)

其中：

• η \eta η 是引力系数。
• ρ ( q , q g ) \rho(q, q_g) ρ(q,qg​) 是机器人当前位置 q q q 与目标位置 q g q_g qg​ 之间的欧几里得距离，即：

ρ ( q , q g ) = ( x − x g ) 2 + ( y − y g ) 2 \rho(q, q_g) = \sqrt{(x - x_g)^2 + (y - y_g)^2} ρ(q,qg​)=(x−xg​)2+(y−yg​)2 ​

根据梯度的定义，引力 F a t t F_{att} Fatt​ 是引力势场的负梯度：

F a t t = − ∇ U a t t ( q ) F_{att} = -\nabla U_{att}(q) Fatt​=−∇Uatt​(q)

计算 $ U_{att}(q) $ 的梯度：

∇ U a t t ( q ) = ( ∂ U a t t ∂ x , ∂ U a t t ∂ y ) \nabla U_{att}(q) = \left( \frac{\partial U_{att}}{\partial x}, \frac{\partial U_{att}}{\partial y} \right) ∇Uatt​(q)=(∂x∂Uatt​​,∂y∂Uatt​​)

具体计算如下：

∂ U a t t ∂ x = η ( x − x g ) , ∂ U a t t ∂ y = η ( y − y g ) \frac{\partial U_{att}}{\partial x} = \eta (x - x_g), \quad \frac{\partial U_{att}}{\partial y} = \eta (y - y_g) ∂x∂Uatt​​=η(x−xg​),∂y∂Uatt​​=η(y−yg​)

因此，引力 F a t t F_{att} Fatt​ 为：

F a t t = − η ( x − x g ) i − η ( y − y g ) j F_{att} = -\eta (x - x_g) \mathbf{i} - \eta (y - y_g) \mathbf{j} Fatt​=−η(x−xg​)i−η(y−yg​)j

4.2 斥力势场的推导

斥力势场 U r e p ( q ) U_{rep}(q) Urep​(q) 通常定义为与机器人和障碍物之间的距离的倒数成正比的函数。为了确保在安全距离 ρ 0 \rho_0 ρ0​ 外斥力势场为零，修正后的斥力势场定义如下：

U r e p ( q ) = { 1 2 k ( 1 ρ ( q , q 0 ) − 1 ρ 0 ) 2 if  ρ ( q , q 0 ) < ρ 0 0 if  ρ ( q , q 0 ) ≥ ρ 0 U_{rep}(q) = \begin{cases} \frac{1}{2} k \left( \frac{1}{\rho(q, q_0)} - \frac{1}{\rho_0} \right)^2 & \text{if } \rho(q, q_0) < \rho_0 \\ 0 & \text{if } \rho(q, q_0) \geq \rho_0 \end{cases} Urep​(q)=⎩ ⎨ ⎧​21​k(ρ(q,q0​)1​−ρ0​1​)20​if ρ(q,q0​)<ρ0​if ρ(q,q0​)≥ρ0​​

其中：

• k k k 是斥力系数。
• ρ ( q , q 0 ) \rho(q, q_0) ρ(q,q0​) 是机器人当前位置 q q q 与障碍物位置 q 0 q_0 q0​ 之间的欧几里得距离。
• ρ 0 \rho_0 ρ0​ 是安全距离。

斥力 F r e p F_{rep} Frep​ 是斥力势场的负梯度：

F r e p = − ∇ U r e p ( q ) F_{rep} = -\nabla U_{rep}(q) Frep​=−∇Urep​(q)

计算 U r e p ( q ) U_{rep}(q) Urep​(q) 的梯度：

∇ U r e p ( q ) = ( ∂ U r e p ∂ x , ∂ U r e p ∂ y ) \nabla U_{rep}(q) = \left( \frac{\partial U_{rep}}{\partial x}, \frac{\partial U_{rep}}{\partial y} \right) ∇Urep​(q)=(∂x∂Urep​​,∂y∂Urep​​)

具体计算如下：

∂ U r e p ∂ x = − k ( 1 ρ ( q , q 0 ) − 1 ρ 0 ) ( 1 ρ 3 ( q , q 0 ) ) ( x − x 0 ) \frac{\partial U_{rep}}{\partial x} = -k \left( \frac{1}{\rho(q, q_0)} - \frac{1}{\rho_0} \right) \left( \frac{1}{\rho^3(q, q_0)} \right) (x - x_0) ∂x∂Urep​​=−k(ρ(q,q0​)1​−ρ0​1​)(ρ3(q,q0​)1​)(x−x0​)

∂ U r e p ∂ y = − k ( 1 ρ ( q , q 0 ) − 1 ρ 0 ) ( 1 ρ 3 ( q , q 0 ) ) ( y − y 0 ) \frac{\partial U_{rep}}{\partial y} = -k \left( \frac{1}{\rho(q, q_0)} - \frac{1}{\rho_0} \right) \left( \frac{1}{\rho^3(q, q_0)} \right) (y - y_0) ∂y∂Urep​​=−k(ρ(q,q0​)1​−ρ0​1​)(ρ3(q,q0​)1​)(y−y0​)

因此，斥力 $ F_{rep} $ 为：

F r e p = { k ( 1 ρ ( q , q 0 ) − 1 ρ 0 ) ( 1 ρ 3 ( q , q 0 ) ) [ ( x − x 0 ) i + ( y − y 0 ) j ] if  ρ ( q , q 0 ) < ρ 0 0 if  ρ ( q , q 0 ) ≥ ρ 0 F_{rep} = \begin{cases} k \left( \frac{1}{\rho(q, q_0)} - \frac{1}{\rho_0} \right) \left( \frac{1}{\rho^3(q, q_0)} \right) \left[ (x - x_0) \mathbf{i} + (y - y_0) \mathbf{j} \right] & \text{if } \rho(q, q_0) < \rho_0 \\ 0 & \text{if } \rho(q, q_0) \geq \rho_0 \end{cases} Frep​={k(ρ(q,q0​)1​−ρ0​1​)(ρ3(q,q0​)1​)[(x−x0​)i+(y−y0​)j]0​if ρ(q,q0​)<ρ0​if ρ(q,q0​)≥ρ0​​

5. 合力的计算与位置更新

5.1 合力的计算

机器人所受的总力 F t o t a l F_{total} Ftotal​ 是引力和所有障碍物产生的斥力的矢量和：

F t o t a l = F a t t + ∑ i F r e p , i F_{total} = F_{att} + \sum_i F_{rep,i} Ftotal​=Fatt​+i∑​Frep,i​

5.2 位置更新

根据合力 F t o t a l F_{total} Ftotal​，更新机器人的位置：

q n e w = q + α F t o t a l ∥ F t o t a l ∥ q_{new} = q + \alpha \frac{F_{total}}{\| F_{total} \|} qnew​=q+α∥Ftotal​∥Ftotal​​

其中 α \alpha α 是步长，控制机器人每次移动的距离。

6. 算法缺陷与改进方法

6.1 目标不可达问题

问题描述：
当机器人接近目标点时，目标点附近的障碍物可能产生过大的斥力，导致机器人无法到达目标点。

原因分析：
斥力势场在目标点附近仍然存在，可能抵消引力，使机器人无法继续向目标点移动。

解决办法：目标点斥力衰减
引入衰减系数，当机器人接近目标点时，逐渐减小斥力的影响。衰减系数定义如下：

KaTeX parse error: Expected 'EOF', got '_' at position 13: \text{decay_̲factor} = \begi…

其中， d t h r e s h o l d d_{threshold} dthreshold​ 是目标点斥力衰减的阈值。

6.2 局部最优问题

问题描述：
机器人在势场中可能陷入局部最小值，导致路径规划失败。

原因分析：
引力和斥力在某些位置可能相互抵消，导致合力为零，机器人停滞不前。

解决办法：随机扰动
当机器人陷入局部最小值时，引入随机扰动，使机器人跳出局部最小值区域。随机扰动定义如下：

KaTeX parse error: Expected 'EOF', got '_' at position 55: … + \text{random_̲vector}

其中，随机向量 $ \text{random_vector} $ 可以定义为：

KaTeX parse error: Expected 'EOF', got '_' at position 14: \text{random_̲vector} = \text…

6.3 乒乓效应问题

问题描述：
当机器人在障碍物附近时，可能会在障碍物周围反复振荡，无法有效避开障碍物。

原因分析：
斥力的方向和大小在障碍物附近变化剧烈，导致机器人无法稳定地远离障碍物。

解决办法：动态调整步长
当机器人接近障碍物时，将步长调整为随机步长，避免机器人陷入局部振荡。具体实现如下：

1. 步长调整条件：

   • 当机器人到最近障碍物的距离小于某个阈值时，触发步长调整。

2. 随机步长范围：

   • 随机步长的范围可以设置为当前步长的 0.5 倍到 1.5 倍，以保持一定的随机性。

3. 步长更新公式：
   KaTeX parse error: Expected 'EOF', got '_' at position 14: \text{random_̲step_size} = \t…

7. 代码实现

以下是改进后的人工势场法 Python 实现：

import numpy as np
import matplotlib.pyplot as plt
from matplotlib import cm
import random
import imageio.v2 as imageio


# 计算引力
def calculate_attraction(position, goal, attraction_coefficient):
    """
    计算引力，引力与机器人和目标点之间的距离成正比。

    参数:
    - position: 机器人当前位置，格式为 [x, y]
    - goal: 目标点位置，格式为 [x, y]
    - attraction_coefficient: 引力系数

    返回值:
    - 引力向量，格式为 [Fx, Fy]
    """
    return -attraction_coefficient * (np.array(position) - np.array(goal))


# 计算斥力
def calculate_repulsion(position, obstacles, repulsion_coefficient, repulsion_threshold, goal, d_threshold):
    """
    计算斥力，斥力与机器人和障碍物之间的距离成反比。

    参数:
    - position: 机器人当前位置，格式为 [x, y]
    - obstacles: 障碍物位置列表，每个障碍物格式为 [x, y]
    - repulsion_coefficient: 斥力系数
    - repulsion_threshold: 斥力的作用范围阈值
    - goal: 目标点位置，格式为 [x, y]
    - d_threshold: 目标点斥力衰减的阈值

    返回值:
    - 斥力向量，格式为 [Fx, Fy]
    """
    force = np.array([0.0, 0.0])  # 初始化斥力向量
    dist_to_goal = np.linalg.norm(position - goal)  # 机器人到目标点的距离

    # 计算目标点斥力衰减因子
    decay_factor = min(1.0, dist_to_goal / d_threshold)  # 距离目标点较近，斥力逐渐减小

    for obstacle in obstacles:
        delta = np.array(position) - np.array(obstacle)  # 机器人到障碍物的向量
        dist = np.linalg.norm(delta)  # 机器人到障碍物的距离
        if dist < repulsion_threshold:  # 如果距离小于斥力作用范围
            # 计算斥力并叠加
            force += repulsion_coefficient * (1 / dist - 1 / repulsion_threshold) * (
                    1 / dist ** 3) * delta * decay_factor
    return force


# 添加随机扰动
def add_random_perturbation(position, perturbation_range):
    """
    添加随机扰动，用于帮助机器人跳出局部最小值。

    参数:
    - position: 机器人当前位置，格式为 [x, y]
    - perturbation_range: 随机扰动的范围

    返回值:
    - 添加扰动后的位置，格式为 [x, y]
    """
    perturbation = np.array([random.uniform(-perturbation_range, perturbation_range),
                             random.uniform(-perturbation_range, perturbation_range)])
    return position + perturbation


# 绘制引力和斥力
def draw_forces(position, attraction, repulsion, force):
    """
    绘制引力、斥力和合力箭头。

    参数:
    - position: 机器人当前位置，格式为 [x, y]
    - attraction: 引力向量，格式为 [Fx, Fy]
    - repulsion: 斥力向量，格式为 [Fx, Fy]
    - force: 合力向量，格式为 [Fx, Fy]
    """
    # 缩放因子，用于调整箭头长度
    scale_factor = 0.2

    # 绘制引力箭头（亮绿色，实线）
    attraction_length = np.linalg.norm(attraction) * scale_factor
    if attraction_length > 0:
        plt.arrow(position[0], position[1], attraction[0] * scale_factor, attraction[1] * scale_factor,
                  color='lime', width=0.05, label='Attraction', length_includes_head=True, linestyle='-', alpha=1.0)

    # 绘制斥力箭头（红色，虚线）
    repulsion_length = np.linalg.norm(repulsion) * scale_factor
    if repulsion_length > 0:
        plt.arrow(position[0], position[1], repulsion[0] * scale_factor, repulsion[1] * scale_factor,
                  color='red', width=0.05, label='Repulsion', length_includes_head=True, linestyle='--', alpha=1.0)

    # 绘制合力箭头（蓝色，点划线）
    force_length = np.linalg.norm(force) * scale_factor
    if force_length > 0:
        plt.arrow(position[0], position[1], force[0] * scale_factor, force[1] * scale_factor,
                  color='blue', width=0.05, label='Total Force', length_includes_head=True, linestyle='-.', alpha=1.0)


# 计算总势场
def calculate_potential_field(x, y, goal, obstacles, attraction_coefficient, repulsion_coefficient, repulsion_threshold,
                              d_threshold):
    """
    计算地图上每个点的总势场值。

    参数:
    - x, y: 地图网格点的坐标
    - goal: 目标点位置，格式为 [x, y]
    - obstacles: 障碍物位置列表，每个障碍物格式为 [x, y]
    - attraction_coefficient: 引力系数
    - repulsion_coefficient: 斥力系数
    - repulsion_threshold: 斥力的作用范围阈值
    - d_threshold: 目标点斥力衰减的阈值

    返回值:
    - 总势场值，格式为 2D 数组
    """
    potential_field = np.zeros_like(x)
    for i in range(x.shape[0]):
        for j in range(x.shape[1]):
            position = np.array([x[i, j], y[i, j]])
            # 计算吸引势场
            attraction = calculate_attraction(position, goal, attraction_coefficient)
            U_att = 0.5 * attraction_coefficient * np.linalg.norm(position - goal) ** 2
            # 计算排斥势场
            U_rep = 0.0
            for obstacle in obstacles:
                dist = np.linalg.norm(position - obstacle)
                if dist < repulsion_threshold:
                    U_rep += 0.5 * repulsion_coefficient * (1 / dist - 1 / repulsion_threshold) ** 2
            # 总势场
            potential_field[i, j] = U_att + U_rep
    return potential_field


# 绘制三维势场图
def plot_3d_potential_field(x, y, potential_field):
    """
    绘制三维势场图。

    参数:
    - x, y: 地图网格点的坐标
    - potential_field: 总势场值，格式为 2D 数组
    """
    fig = plt.figure(figsize=(12, 8))
    ax = fig.add_subplot(111, projection='3d')

    # 绘制三维表面图
    surf = ax.plot_surface(x, y, potential_field, cmap='viridis', alpha=0.8, linewidth=0, antialiased=False, vmin=0,
                           vmax=100)

    # 添加颜色条
    cbar = fig.colorbar(surf, ax=ax, shrink=0.5, aspect=5, label='Potential Field Value')

    # 绘制等势线
    contour = ax.contour(x, y, potential_field, levels=20, colors='black', linewidths=0.5,
                         offset=np.min(potential_field))

    # 设置标签和标题
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Potential')
    ax.set_title('3D Potential Field with Contours')

    plt.show()


# 主函数
def main():
    # 初始化参数
    start = np.array([1.0, 1.0])  # 起始点
    goal = np.array([9.0, 9.0])  # 目标点
    obstacles = [np.array([5.0, 5.0]), np.array([5.0, 7.0]), np.array([3.0, 4.0]), np.array([6.0, 5.0]),
                 np.array([9.0, 5.0])]  # 障碍物列表
    attraction_coefficient = 0.8  # 引力系数
    repulsion_coefficient = 5  # 斥力系数
    repulsion_threshold = 10.0  # 斥力作用范围阈值
    d_threshold = 1.0  # 斥力衰减的阈值
    step_size = 0.1  # 步长
    max_iterations = 1000  # 最大迭代次数
    perturbation_range = 0.5  # 随机扰动范围
    position = start.copy()  # 机器人当前位置
    path = [start.copy()]  # 路径记录

    # 初始化图形
    plt.figure(figsize=(12, 6))

    i = 0
    image_list = []  # 存储图片

    # 绘制势场
    x = np.linspace(-1, 11, 100)
    y = np.linspace(-1, 11, 100)
    x, y = np.meshgrid(x, y)
    potential_field = calculate_potential_field(x, y, goal, obstacles, attraction_coefficient, repulsion_coefficient,
                                                repulsion_threshold, d_threshold)

    # 绘制势场填充图
    plt.contourf(x, y, potential_field, levels=500, cmap='plasma', vmin=0, vmax=50, alpha=0.6)  # 使用浅色颜色映射
    plt.colorbar(label='Potential Field Value')

    # 绘制等势线（描边）
    contour = plt.contour(x, y, potential_field, levels=500, colors='black', linewidths=0.5)
    plt.clabel(contour, inline=True, fontsize=8)  # 在等势线上添加数值标签

    # 绘制目标点和障碍物
    plt.plot(goal[0], goal[1], 'r*', label='Goal')  # 绘制目标点
    for idx, obstacle in enumerate(obstacles):
        if idx == 0:
            plt.plot(obstacle[0], obstacle[1], 'ks', label='Obstacle')  # 第一个障碍物带标签
        else:
            plt.plot(obstacle[0], obstacle[1], 'ks')  # 其他障碍物不带标签

    plt.legend()

    # 主循环
    for _ in range(max_iterations):
        # 计算引力和斥力
        attraction = calculate_attraction(position, goal, attraction_coefficient)
        repulsion = calculate_repulsion(position, obstacles, repulsion_coefficient, repulsion_threshold, goal,
                                        d_threshold)
        force = attraction + repulsion  # 合力

        # 如果合力很小，添加随机扰动以避免局部最小值
        if np.linalg.norm(force) < 0.1:
            force += add_random_perturbation(position, perturbation_range)

        # 动态调整步长
        min_dist_to_obstacle = min(np.linalg.norm(position - obstacle) for obstacle in obstacles)
        if min_dist_to_obstacle < repulsion_threshold:
            # 在障碍物附近，使用随机步长
            random_step_size = step_size * random.uniform(0.9, 1.1)
        else:
            # 远离障碍物，使用固定步长
            random_step_size = step_size

        # 更新机器人位置
        position += random_step_size * force / np.linalg.norm(force)
        path.append(position.copy())  # 记录路径

        # 动态显示
        plt.clf()  # 清除当前图形
        plt.contourf(x, y, potential_field, levels=500, cmap='plasma', vmin=0, vmax=50, alpha=0.6)  # 使用浅色颜色映射
        plt.colorbar(label='Potential Field Value')

        # 绘制等势线（描边）
        contour = plt.contour(x, y, potential_field, levels=500, colors='black', linewidths=0.5)
        plt.clabel(contour, inline=True, fontsize=8)  # 在等势线上添加数值标签

        plt.plot(goal[0], goal[1], 'r*', label='Goal')  # 绘制目标点
        for idx, obstacle in enumerate(obstacles):
            if idx == 0:
                plt.plot(obstacle[0], obstacle[1], 'ks', label='Obstacle')  # 第一个障碍物带标签
            else:
                plt.plot(obstacle[0], obstacle[1], 'ks')  # 其他障碍物不带标签

        plt.plot(np.array(path)[:, 0], np.array(path)[:, 1], '-o', label='Path')  # 绘制路径
        plt.plot(position[0], position[1], 'bo', label='Robot')  # 绘制机器人当前位置

        # 绘制引力和斥力
        draw_forces(position, attraction, repulsion, force)

        # 显示力的数值
        plt.text(position[0] + 0.2, position[1] - 1.0,
                 f"Attraction: {np.linalg.norm(attraction):.2f}\nRepulsion: {np.linalg.norm(repulsion):.2f}\nTotal Force: {np.linalg.norm(force):.2f}",
                 fontsize=8, bbox=dict(facecolor='white', alpha=0.5))

        plt.legend()

        # 保存当前图形到临时文件
        # plt.savefig("temp.png")
        # i += 1
        # if (i % 5) == 0:
        #     image_list.append(imageio.imread("temp.png"))

        plt.pause(0.001)  # 暂停 0.001 秒以显示动态效果

        # 如果机器人接近目标点，结束循环
        if np.linalg.norm(position - goal) <= 0.1:
            break

    plt.show()
    # imageio.mimsave("display.gif", image_list, duration=0.1)

    # 绘制三维势场图
    plot_3d_potential_field(x, y, potential_field)


if __name__ == "__main__":
    main()

运行结果如下：

8. 结论

人工势场法在路径规划中具有简单直观的优点，但也存在目标不可达和局部最优等问题。通过引入目标点斥力衰减和随机扰动等改进方法，可以有效克服这些问题，提升算法的鲁棒性和实用性。改进后的算法在实际应用中表现出更好的性能，能够适应复杂环境下的路径规划需求。

参考文献

1. Khatib, O. (1986). Real-time obstacle avoidance for manipulators and mobile robots. The International Journal of Robotics Research, 5(1), 90-98.
2. Koren, Y., & Borenstein, J. (1991). Potential field methods and their inherent limitations for mobile robot navigation. Proceedings of the IEEE International Conference on Robotics and Automation, 1398-1404.
3. Latombe, J. C. (1991). Robot Motion Planning. Kluwer Academic Publishers.