人工势场算法python

人工势场算法（Artificial Potential Field Algorithm）是一种基于物理原理的路径规划算法，主要用于机器人的自主导航和运动控制。

以下是一个简单的人工势场算法的 Python 实现：

import math

class Robot:
    def __init__(self, x, y, goal_x, goal_y):
        self.x = x
        self.y = y
        self.goal_x = goal_x
        self.goal_y = goal_y
        self.k_att = 0.5 # 引力系数
        self.k_rep = 100 # 斥力系数
        self.rep_range = 100 # 斥力范围
        self.speed = 1 # 机器人速度

    def calculate_potential_field(self, obstacles):
        # 计算机器人到目标点的距离和角度
        dx = self.goal_x - self.x
        dy = self.goal_y - self.y
        distance = math.sqrt(dx*dx + dy*dy)
        angle = math.atan2(dy, dx)

        # 计算引力
        att_dx = self.k_att * distance * math.cos(angle)
        att_dy = self.k_att * distance * math.sin(angle)

        # 计算斥力
        rep_dx = 0
        rep_dy = 0
        for obstacle in obstacles:
            dx = obstacle[0] - self.x
            dy = obstacle[1] - self.y
            dist = math.sqrt(dx*dx + dy*dy)
            if dist <= self.rep_range:
                angle = math.atan2(dy, dx)
                rep_dx -= self.k_rep * (self.rep_range - dist) * math.cos(angle)
                rep_dy -= self.k_rep * (self.rep_range - dist) * math.sin(angle)

        # 合并引力和斥力
        dx = att_dx + rep_dx
        dy = att_dy + rep_dy

        # 计算新的位置
        angle = math.atan2(dy, dx)
        self.x += self.speed * math.cos(angle)
        self.y += self.speed * math.sin(angle)
        return self.x, self.y

在上面的代码中，我们定义了一个 Robot 类，代表机器人。在初始化时，我们传入机器人的起点和终点坐标。我们还定义了一些参数，如引力系数、斥力系数、斥力范围和机器人速度。

在 `calculate_potential_field()` 方法中，我们先计算机器人到目标点的距离和角度，然后计算引力和斥力。我们遍历障碍物列表，对于每个障碍物，如果它在机器人的斥力范围内，我们就计算斥力。最后，我们将引力和斥力合并，计算新的位置，并返回机器人的新坐标。

下面是一个简单的示例，演示了如何使用人工势场算法实现机器人自主导航：

import matplotlib.pyplot as plt

# 创建机器人和障碍物列表
robot = Robot(0, 0, 100, 100)
obstacles = [(50, 50), (70, 70), (80, 20), (20, 80)]

# 绘制初始状态
plt.plot(robot.x, robot.y, 'bo')
for obstacle in obstacles:
    plt.plot(obstacle[0], obstacle[1], 'ro')
plt.plot(robot.goal_x, robot.goal_y, 'go')
plt.axis([-10, 110, -10, 110])

# 迭代计算新的位置
for i in range(100):
    x, y = robot.calculate_potential_field(obstacles)
    plt.plot(x, y, 'bo')

# 显示结果
plt.show()

在上面的示例中，我们创建了一个机器人和障碍物列表，并在初始状态下绘制了它们。然后，我们迭代计算机器人的新位置，并绘制它的轨迹。最后，我们显示了结果。

这是一个简单的例子，仅用于演示人工势场算法的基本原理。在实际应用中，还需要考虑更复杂的情况，如动态障碍物、局部最小值等。