人工势场 python 代码

### 回答1：
人工势场是一种路径规划算法，在机器人导航领域中广泛应用。人工势场算法的原理是将机器人当前位置视为受到势场作用的质点，根据“能量最小”的原则，计算机器人在势场中的运动方向和速度。其中，正向势场吸引机器人走向目标位置，反向势场避免机器人碰触障碍物。

以下是一段Python代码，实现了简单的人工势场算法：

import math

class Robot:
    def __init__(self, r, pos, speed):
        self.r = r # 机器人半径
        self.pos = pos # 机器人当前位置
        self.speed = speed # 机器人速度向量

    def compute_speed(self, target_pos, obstacles_pos):
        force = [0, 0] # 力
        for obstacle_pos in obstacles_pos:
            # 计算反向势场
            distance = math.sqrt((self.pos[0]-obstacle_pos[0])**2 + (self.pos[1]-obstacle_pos[1])**2)
            if distance < 2*self.r: # 碰撞
                force[0] += (self.pos[0]-obstacle_pos[0])*2*self.r/(distance**2)
                force[1] += (self.pos[1]-obstacle_pos[1])*2*self.r/(distance**2)
        # 计算正向势场
        distance = math.sqrt((self.pos[0]-target_pos[0])**2 + (self.pos[1]-target_pos[1])**2)
        if distance > 10: # 在距离目标较远时受到吸引力
            force[0] += (target_pos[0]-self.pos[0])/distance
            force[1] += (target_pos[1]-self.pos[1])/distance
        else: # 在距离目标较近时停止运动
            force[0] = 0
            force[1] = 0
        # 根据力计算机器人速度
        norm = math.sqrt(force[0]**2 + force[1]**2)
        if norm > 0:
            self.speed = [force[0]/norm, force[1]/norm]
        else:
            self.speed = [0, 0]

这段代码中，我们定义了一个Robot类，其中包含机器人半径、当前位置和速度向量三个属性，并定义了一个compute_speed方法，用于计算机器人下一步的速度向量。在compute_speed方法中，我们计算了机器人当前受到的正向势场和反向势场，并将二者叠加作为机器人总的势场。根据力学中的第二定律，我们可以根据机器人所受的总的力，计算机器人下一步的速度向量。最后，我们将计算得到的速度向量作为机器人当前的速度向量，并根据该速度向量更新机器人位置。

### 回答2：
人工势场法是一种常见的路径规划算法，可以用于无人机、机器人等自主导航领域。Python语言在机器学习、人工智能等领域有着广泛的应用，下面就介绍一下人工势场法在Python下的实现代码。

1、引入库

import numpy as np
import matplotlib.pyplot as plt

使用numpy库进行矩阵运算，matplotlib库进行图像绘制。

2、定义目标点和障碍物

gx, gy = 50.0, 50.0   #目标点位置
ox, oy = [30.0, 30.0, 40.0, 40.0], [10.0, 40.0, 10.0, 40.0]   #四个障碍物的位置

gx、gy为目标点的x、y坐标值，ox、oy分别存储四个障碍物的x、y坐标值。

3、绘制障碍物和目标点

plt.plot(ox, oy, 'ro', markersize=10)
plt.plot(gx, gy, 'go', markersize=10)

绘制障碍物和目标点，障碍物用红色圆点表示，目标点用绿色圆点表示。

4、定义势能函数

def calc_attractive_potential(x, y, gx, gy):
    return 0.5 * k_att * np.hypot(x - gx, y - gy)**2

def calc_repulsive_potential(x, y, ox, oy, k_rep):
    U = 0
    for i, _ in enumerate(ox):
        U += 0.5 * k_rep * ((x - ox[i])**2 + (y - oy[i])**2)**2
    return U

势能函数分为引力势和斥力势。引力势指向目标点，斥力势来自障碍物，用高斯势能函数表示。k_att、k_rep分别为引力常数和斥力常数，根据实际问题进行设置。

5、定义起点

sx, sy = 10.0, 10.0   #起点位置

设置起点的x、y坐标值。

6、定义算法参数

k_att = 5.0   #引力常数
k_rep = 100.0   #斥力常数
max_iter = 1000   #最大迭代次数
step_size = 0.1   #步长

设置k_att、k_rep的值，max_iter表示最大迭代次数，step_size表示每次移动的步长。

7、算法实现

rx, ry = [sx], [sy]   #轨迹x、y坐标列表
for i in range(max_iter):
    #计算当前点势能
    pot_total = calc_attractive_potential(rx[-1], ry[-1], gx, gy)
    pot_total += calc_repulsive_potential(rx[-1], ry[-1], ox, oy, k_rep)
    #计算梯度
    gradx, grady = np.gradient(pot_total)
    dx, dy = -gradx, -grady
    #更新位置
    x, y = rx[-1] + step_size * dx, ry[-1] + step_size * dy
    #检查是否越界或碰撞障碍物
    if x < 0 or x > 100 or y < 0 or y > 100:
        break
    if calc_repulsive_potential(x, y, ox, oy, k_rep) > 1e5:
        break
    #将更新后的位置添加到轨迹列表中
    rx.append(x)
    ry.append(y)

根据当前点的位置计算势能和梯度，用梯度更新当前点的位置，检查是否越界或碰撞障碍物，将更新后的位置添加到轨迹列表中。

8、绘制路径

plt.plot(rx, ry, 'b-')
plt.title('Artificial Potential Field')
plt.xlabel('X')
plt.ylabel('Y')
plt.show()

绘制轨迹，轨迹用蓝色实线表示，将图像的标题、x坐标轴标签和y坐标轴标签设置好，最后显示图像。