人工势场法路径规划示例

人工势场法是一种常用的路径规划算法，它通过模拟物体在势场中的运动来实现路径规划。下面是一个人工势场法路径规划的示例：

import numpy as np
import matplotlib.pyplot as plt

# 定义目标点和障碍物
goal = np.array([10, 10])  # 目标点坐标
obstacles = np.array([[5, 5], [7, 8], [9, 6]])  # 障碍物坐标

# 定义势场参数
k_att = 1  # 引力系数
k_rep = 1  # 斥力系数
rep_range = 2  # 斥力作用范围

# 计算引力
def attractive_force(position):
    return k_att * (goal - position)

# 计算斥力
def repulsive_force(position):
    force = np.zeros(2)
    for obstacle in obstacles:
        dist = np.linalg.norm(position - obstacle)
        if dist < rep_range:
            force += k_rep * (1 / dist - 1 / rep_range) * (1 / dist**2) * (position - obstacle) / dist
    return force

# 计算总力
def total_force(position):
    return attractive_force(position) + repulsive_force(position)

# 进行路径规划
def path_planning(start, max_iter=100, step_size=0.1):
    path = [start]
    for _ in range(max_iter):
        current_pos = path[-1]
        force = total_force(current_pos)
        next_pos = current_pos + step_size * force
        path.append(next_pos)
        if np.linalg.norm(next_pos - goal) < step_size:
            break
    return np.array(path)

# 测试路径规划
start = np.array([0, 0])  # 起始点坐标
path = path_planning(start)

# 绘制路径规划结果
plt.figure()
plt.plot(path[:, 0], path[:, 1], 'b-')
plt.plot(obstacles[:, 0], obstacles[:, 1], 'ro')
plt.plot(start[0], start[1], 'go')
plt.plot(goal[0], goal[1], 'yo')
plt.xlabel('X')
plt.ylabel('Y')
plt.title('Path Planning with Artificial Potential Field')
plt.grid(True)
plt.show()

这个示例中，我们首先定义了目标点和障碍物的坐标。然后，我们定义了引力和斥力的计算函数，并根据势场参数计算总力。接下来，我们使用总力来进行路径规划，直到达到目标点或达到最大迭代次数。最后，我们使用matplotlib库将路径规划结果可视化。