人工势场法路径规划算法（APF）

本文主要对人工势场法路径规划算法进行介绍，主要涉及人工势场法的简介、引力和斥力模型及其推导过程、人工势场法的缺陷及改进思路、人工势场法的Python与MATLAB开源源码等方面

一、人工势场法简介

人工势场法是由Khatib于1985年在论文《Real-Time Obstacle Avoidance for Manipulators and Mobile Robots》中提出的一种虚拟力法。它的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。应用势场法规划出来的路径一般是比较平滑并且安全，但是这种方法存在局部最优点问题。

二、引力与斥力模型及公式推导

1、基础知识补充：梯度

梯度的本意是一个向量（矢量），表示某一函数在该点处的方向导数沿着该方向取得最大值，即函数在该点处沿着该方向（此梯度的方向）变化最快，变化率最大（为该梯度的模）。

2、引力与斥力模型

引力势场主要与机器人和目标点间的距离有关，距离越大，机器人所受的势能值就越大;距离越小，机器人所受的势能值则越小，所以引力势场的函数为:

Uatt(q)=12ηρ2(q,qg)U_{att}\left(q\right)=\dfrac{1}{2}\eta\rho^2\left(q,q_g\right) Uatt(q)=21ηρ2(q,qg)

其中η为正比例增益系数，ρ(q,qg)为一个矢量，表示机器人的位置q和目标点位置qg之间的欧几里德距离|q-qg|。矢量方向是从机器人的位置指向目标点位
置。

相应的引力Fatt(X)F_{att}\left(X\right)Fatt(X)为引力场的负梯度:

Fatt(X)=−∇Uatt(X)=ηρ(q,qg)F_{att}\left(X\right)=-\nabla U_{att}\left(X\right)=\eta\rho\left(q,q_{g}\right) Fatt(X)=−∇Uatt(X)=ηρ(q,qg)

决定障碍物斥力势场的因素是机器人与障碍物间的距离，当机器人未进入障碍物的影响范围时，其受到的势能值为零;在机器人进入障碍物的影响范围后，两者之间的距离越大,机器人受到的斥力就越小，距离越小，机器人受到的斥力就越大,斥力势场的势场函数为：

Ure1(X)={12k(1ρ(q,qo)−1ρo)20≤ρ(q,qo)≤ρo0ρ(q,qo)≥ρoU_{r e_1}(X)=\begin{cases}\dfrac{1}{2}k(\dfrac{1}{\rho(q,q_o)}-\dfrac{1}{\rho_o})^2&0\leq\rho(q,q_o)\leq\rho_o\\ 0&\rho(q,q_o)\geq\rho_o\end{cases} Ure1(X)=⎩⎨⎧21k(ρ(q,qo)1−ρo1)200≤ρ(q,qo)≤ρoρ(q,qo)≥ρo

其中k为正比例系数，ρ(q,qo)为一矢量，方向为从障碍物指向机器人，大小为机器人与障碍物间的距离|q-qo|，ρo为一常数，表示障碍物对机器人产生作用的最大距离。相应的斥力为斥力场的负梯度:

Fn0(X)={k(1ρ(q,q0)−1ρ0)1ρ2(q,q0)∇ρ(q,q0)0≤ρ(q,q0)≤ρ00ρ(q,q0)≥ρ0F_{n_0}(X)=\begin{cases}k(\dfrac{1}{\rho(q,q_0)}-\dfrac{1}{\rho_0})\dfrac{1}{\rho^2(q,q_0)}\nabla\rho(q,q_0)&0\leq\rho(q,q_0)\leq\rho_0\\ 0&\rho(q,q_0)\geq\rho_0\end{cases} Fn0(X)=⎩⎨⎧k(ρ(q,q0)1−ρ01)ρ2(q,q0)1∇ρ(q,q0)00≤ρ(q,q0)≤ρ0ρ(q,q0)≥ρ0

其中∇ρ(q,q0)\begin{aligned}\nabla\rho(q,q_0)\end{aligned}∇ρ(q,q0)表示从q0q_0q0指向q的单位向量

∇ρ(q,q0)=q−q0∥q−q0∥\nabla\rho(q,q_0)=\dfrac{q-q_0}{\|q-q_0\|}∇ρ(q,q0)=∥q−q0∥q−q0

3、引力与斥力公式的推导过程

设机器人位置为(x, y)，障碍物位置为(xg, yg)，则引力势场函数可由下式

Uatt(q)=12ηρ2(q,qg)U_{att}\big(q\big)=\dfrac{1}{2}\eta\rho^2\big(q,q_g\big)Uatt(q)=21ηρ2(q,qg)

改写为：

Uatt(x,y)=12η[(x−xg)2+(y−yg)2]U_{a t t}\big(x,y\big)=\frac{1}{2}\eta\biggl[\bigl(x-x_{g}\bigr)^{2}+\bigl(y-y_{g}\bigr)^{2}\biggr]Uatt(x,y)=21η[(x−xg)2+(y−yg)2]

所以根据梯度下降法。在这种情况下，势场U的负梯度可被认为是位形空间中作用在机器人上的一个广义力， F=−∇F=-\nablaF=−∇

−gradUatt(x,y)=−∇Uatt(x,y)=−Uatt,x′(x,y)i→−Uatt,y′(x,y)j→-gradU_{att}\left(x,y\right)=-\nabla U_{att}\left(x,y\right)=-U_{att,x}^{'}\big(x,y\big)\overset{\rightarrow}{i}-U_{att,y}^{'}\big(x,y\big)\overset{\rightarrow}{j}−gradUatt(x,y)=−∇Uatt(x,y)=−Uatt,x′(x,y)i→−Uatt,y′(x,y)j→

=−η(x−xg)i→−η(y−yg)j→=η[(xg−x)i→+(yg−y)j→]=-\eta\bigl(x-x_g\bigr)\overrightarrow i-\eta\bigl(y-y_g\bigr)\overrightarrow j=\eta\bigg[\big(x_g-x\big)\overrightarrow{i}+\big(y_g-y\big)\overrightarrow{j}\bigg] =−η(x−xg)i−η(y−yg)j=η[(xg−x)i+(yg−y)j]

=η(x−xg)2+(yg−y)2=ηρ(q,qg)=\eta\sqrt{\left(x-x_g\right)^2+\left(y_g-y\right)^2}=\eta\rho\Big(q,q_g\Big) =η(x−xg)2+(yg−y)2=ηρ(q,qg)

斥力势场函数可由下式:

Unq(q)=12k(1ρ(q,q0)−1ρ0)2U_{nq}(q)=\dfrac{1}{2}k\bigg(\dfrac{1}{\rho\left(q,q_0\right)}-\dfrac{1}{\rho_0}\bigg)^2\ Unq(q)=21k(ρ(q,q0)1−ρ01)2

改写为：

Unqq(x,y)=12k[1(x−x0)2+(y−y0)2−1ρ0]2U_{nqq}\left(x,y\right)=\dfrac{1}{2}k\bigg[\dfrac{1}{\sqrt{\left(x-x_0\right)^2+\left(y-y_0\right)^2}}-\dfrac{1}{\rho_0}\bigg]^2 Unqq(x,y)=21k[(x−x0)2+(y−y0)21−ρ01]2

所以：

−∇Umq(x,y)=−Uatt,x(x,y)i→−Uatt,y(x,y)j→-\nabla U_{mq}\bigl(x,y\bigr)=-U_{att,x}\bigl(x,y\bigr)\overrightarrow{i}-U_{att,y}\bigl(x,y\bigr)\overrightarrow{j} −∇Umq(x,y)=−Uatt,x(x,y)i−Uatt,y(x,y)j

其中对x求偏导部分过程如下：

−Uatt,x′(x,y)i⃗=−k[1(x−x0)2+(y−y0)2−1ρ0][1(x−x0)2+(y−y0)2−1ρ0]′i⃗-U_{att,x}^{'}\left(x,y\right)\vec{i}=-k\left[\dfrac{1}{\sqrt{\left(x-x_0\right)^2+\left(y-y_0\right)^2}}-\dfrac{1}{\rho_0}\right]\left[\dfrac{1}{\sqrt{\left(x-x_0\right)^2+\left(y-y_0\right)^2}}-\dfrac{1}{\rho_0}\right]^{'}\vec{i}−Uatt,x′(x,y)i=−k(x−x0)2+(y−y0)21−ρ01(x−x0)2+(y−y0)21−ρ01′i
=−k[1(x−x0)2+(y−y0)2−1ρ0]{−12[(x−x0)2+(y−y0)2]−32[(x−x0)2+(y−y0)2]′}i⃗=-k\bigg[\dfrac{1}{\sqrt{\left(x-x_0\right)^2+\left(y-y_0\right)^2}}-\dfrac{1}{\rho_0}\bigg]\bigg\{-\dfrac{1}{2}\bigg[\left(x-x_0\right)^2+\left(y-y_0\right)^2\bigg]^{-\dfrac{3}{2}}\bigg[\left(x-x_0\right)^2+\left(y-y_0\right)^2\bigg]^{'}\bigg\}\vec{i} =−k[(x−x0)2+(y−y0)21−ρ01]{−21[(x−x0)2+(y−y0)2]−23[(x−x0)2+(y−y0)2]′}i
=k[1(x−x0)2+(y−y0)2−1ρ0]{1(x−x0)2+(y−y0)2[(x−x0)2+(y−y0)2]−12(x−x0)}i⃗=k\bigg[\dfrac{1}{\sqrt{\left(x-x_0\right)^2+\left(y-y_0\right)^2}}-\dfrac{1}{\rho_0}\bigg]\bigg\{\dfrac{1}{\left(x-x_0\right)^2+\left(y-y_0\right)^2}\bigg[\left(x-x_0\right)^2+\left(y-y_0\right)^2\bigg]^{-\frac{1}{2}}\left(x-x_0\right)\bigg\}\vec{i} =k[(x−x0)2+(y−y0)21−ρ01]{(x−x0)2+(y−y0)21[(x−x0)2+(y−y0)2]−21(x−x0)}i
=k(1ρ(q,q0)−1ρ0)⋅1ρ2(q,q0)⋅1ρ(q,q0)⋅(x−x0)i→=k\bigg(\dfrac{1}{\rho\big(q,q_0\big)}-\dfrac{1}{\rho_0}\bigg)\cdot\dfrac{1}{\rho^2\big(q,q_0\big)}\cdot\dfrac{1}{\rho\big(q,q_0\big)}\cdot\big(x-x_0\big)\stackrel{\rightarrow}{i}=k(ρ(q,q0)1−ρ01)⋅ρ2(q,q0)1⋅ρ(q,q0)1⋅(x−x0)i→

同理，可得对y求偏导部分结果如下：

−Uatt,y′(x,y)j⃗=k(1ρ(q,q0)−1ρ0)⋅1ρ2(q,q0)⋅1ρ(q,q0)⋅(y−y0)i→-U_{att,y}^{'}\left(x,y\right)\vec{j}=k\bigg(\dfrac{1}{\rho\big(q,q_0\big)}-\dfrac{1}{\rho_0}\bigg)\cdot\dfrac{1}{\rho^2\big(q,q_0\big)}\cdot\dfrac{1}{\rho\big(q,q_0\big)}\cdot\big(y-y_0\big)\stackrel{\rightarrow}{i}−Uatt,y′(x,y)j=k(ρ(q,q0)1−ρ01)⋅ρ2(q,q0)1⋅ρ(q,q0)1⋅(y−y0)i→

所以，斥力函数为：

−∇Uqq(x,y)=k(1ρ(q,q0)−1ρ0)⋅1ρ2(q,q0)⋅∇ρ(q,q0)-\nabla U_{qq}\big(x,y\big)=k\bigg(\dfrac{1}{\rho\big(q,q_0\big)}-\dfrac{1}{\rho_0}\bigg)\cdot\dfrac{1}{\rho^2\big(q,q_0\big)}\cdot\nabla\rho\big(q,q_0\big) −∇Uqq(x,y)=k(ρ(q,q0)1−ρ01)⋅ρ2(q,q0)1⋅∇ρ(q,q0)

三、算法缺陷

1、不能到达目标点的问题

当存在某个障碍物与目标点距离太近时，机器人到达目标点时，根据势场函数可知，目标点的引力降为零，而障碍物的斥力不为零，此时机器人虽到达目标点，但在斥力场的作用下不能停下来，从而导致目标点不可达的问题，机器人容易出现在目标点附近震荡的现象。

2、陷入局部最优的问题
机器人在某个位置时，如果若干个障碍物的合斥力与目标点的引力大小相等、方向相反，则合力为0，这将导致机器人受力为0，而停止运动，故无法继续向目标点移动，下图列举了其中的一种情况。

3、可能碰撞障碍物的问题

当物体离目标点比较远时，引力将变的特别大，相对较小的斥力在可以忽略的情况下，物体路径上可能会碰到障碍物

四、改进思路

1、优化斥力场函数

以上缺陷1和2的一种改进思路是，改变斥力场函数，传统的斥力场函数的大小由机器人与障碍物直接的距离决定，方向由障碍物指向机器人，为避免以上缺陷，我们可以把斥力场函数改为由两部分组成，第一部分像传统的斥力场函数一样由障碍物指向机器人，大小由机器人与障碍物之间距离和机器人与目标点距离共同决定，如下图中的蓝色箭头Freq1所示，第二部分斥力由机器人指向目标点，大小由机器人与障碍物之间距离和机器人与目标点之间距离共同决定，如下图中的绿色箭头Freq2所示，这两部分斥力共同组成了合斥力Freq，如下图中黄色箭头所示，再与红色箭头所示的引力Fatt求合力，得到最终机器人的受力，如紫色的箭头F所示。

上述改进思想的斥力函数如下，其中n为任意常数：

{Freq1=k(1ρ(q,qo)−1ρo)ρgnρ2(q,qo)Freq2=n2k(1ρ(q,qo)−1ρo)2ρgn−1\begin{cases}F_{_{r eq1}}=k(\dfrac{1}{\rho(q,q_o)}-\dfrac{1}{\rho_o})\dfrac{\rho_g^n}{\rho^2(q,q_o)}\\ F_{_{r eq2}}=\dfrac{n}{2}k(\dfrac{1}{\rho(q,q_o)}-\dfrac{1}{\rho_o})^2\rho_g^{^{n-1}}\end{cases}⎩⎨⎧Freq1=k(ρ(q,qo)1−ρo1)ρ2(q,qo)ρgnFreq2=2nk(ρ(q,qo)1−ρo1)2ρgn−1

上述改进后，当机器人到达目标点时斥力也变为了0

2、针对陷入局部最小值问题的改进思路

针对陷入局部极小值问题，可以引入随机的虚拟力使机器人跳出局部极小值的状态。在障碍物密集的情况下，机器人易陷入局部最小值，此时，也可以对密集的障碍物进行处理，将多个障碍物看出一个整体来求斥力。此外，也可以通过设置子目标点的方式来使机器人逃出极小值。

五、开源程序

1、来自白菜丁的Python版本程序

（1）程序来源：局部路径规划-人工势场法【点击可跳转】

（2）程序如下所示：

import numpy as np
import matplotlib.pyplot as plt# 初始化车的参数
d = 3.5  # 道路标准宽度
W = 1.8  # 汽车宽度
P0 = np.array([0, -d / 2, 1, 1])  # 车辆起点位置，分别代表x,y,vx,vy
Pg = np.array([99, d / 2, 0, 0])  # 目标点位置
Pobs = np.array([[15, 7 / 4, 0, 0],  # 障碍物位置[30, -3 / 2, 0, 0],[45, 3 / 2, 0, 0],[60, -3 / 4, 0, 0],[80, 7 / 4, 0, 0]])
P = np.array([[15, 7 / 4, 0, 0],[30, -3 / 2, 0, 0],[45, 3 / 2, 0, 0],[60, -3 / 4, 0, 0],[80, 7 / 4, 0, 0],[99, d / 2, 0, 0]])Eta_att = 5  # 计算引力的增益系数
Eta_rep_ob = 15  # 计算斥力的增益系数
Eta_rep_edge = 50  # 计算边界斥力的增益系数d0 = 20  # 障碍影响距离
n = len(P)  # 障碍物和目标总计个数
len_step = 0.5  # 步长
Num_iter = 200  # 最大循环迭代次数Pi = P0
Path = []  # 存储路径
delta = np.zeros((n, 2))  # 存储每一个障碍到车辆的斥力方向向量以及引力方向向量
dist = []  # 存储每一个障碍到车辆的距离以及目标点到车辆距离
unitvector = np.zeros((n, 2))  # 存储每一个障碍到车辆的斥力单位向量以及引力单位向量
F_rep_ob = np.zeros((n - 1, 2))  # 存储每一个障碍到车辆的斥力
F_rep_edge = np.zeros((1, 2))
while ((Pi[0] - Pg[0]) ** 2 + (Pi[1] - Pg[1]) ** 2) ** 0.5 > 1:# a = ((Pi[0] - Pg[0]) ** 2 + (Pi[1] - Pg[1]) ** 2) ** 0.5Path.append(Pi.tolist())# 计算车辆当前位置和障碍物的单位方向向量for j in range(len(Pobs)):delta[j, :] = Pi[0:2] - P[j, 0:2]dist.append(np.linalg.norm(delta[j, :]))unitvector[j, :] = [delta[j, 0] / dist[j], delta[j, 1] / dist[j]]# 计算车辆当前位置和目标点的单位方向向量delta[n - 1, :] = P[n - 1, 0:2] - Pi[0:2]dist.append(np.linalg.norm(delta[n - 1, :]))unitvector[n - 1, :] = [delta[n - 1, 0] / dist[n - 1], delta[n - 1, 1] / dist[n - 1]]# 计算斥力for j in range(len(Pobs)):if dist[j] >= d0:F_rep_ob[j, :] = [0, 0]else:# 障碍物的斥力1,方向由障碍物指向车辆F_rep_ob1_abs = Eta_rep_ob * (1 / dist[j] - 1 / d0) * dist[n - 1] / dist[j] ** 2  # 回看公式设定n=1F_rep_ob1 = np.array([F_rep_ob1_abs * unitvector[j, 0], F_rep_ob1_abs * unitvector[j, 1]])# 障碍物的斥力2，方向由车辆指向目标点F_rep_ob2_abs = 0.5 * Eta_rep_ob * (1 / dist[j] - 1 / d0) ** 2F_rep_ob2 = np.array([F_rep_ob2_abs * unitvector[n - 1, 0], F_rep_ob2_abs * unitvector[n - 1, 1]])# 改进后的障碍物和斥力计算F_rep_ob[j, :] = F_rep_ob1 + F_rep_ob2# 增加的边界斥力if -d + W / 2 < Pi[1] <= -d / 2:# 这个边界斥力只作用在y方向，因此x方向为0F_rep_edge = [0, Eta_rep_edge * np.linalg.norm(Pi[2:4]) * np.exp(-d / 2 - Pi[1])]elif -d / 2 < Pi[1] <= -W / 2:F_rep_edge = [0, 1 / 3 * Eta_rep_edge * Pi[1] ** 2]elif W / 2 < Pi[1] <= d / 2:F_rep_edge = [0, -1 / 3 * Eta_rep_edge * Pi[1] ** 2]elif d / 2 < Pi[1] <= d - W / 2:F_rep_edge = [0, Eta_rep_edge * np.linalg.norm(Pi[2:4]) * np.exp(Pi[1] - d / 2)]# 计算合力和方向F_rep = [np.sum(F_rep_ob[:, 0]) + F_rep_edge[0], np.sum(F_rep_ob[:, 1]) + F_rep_edge[1]]  # 合并边界斥力和障碍舞斥力F_att = [Eta_att * dist[n - 1] * unitvector[n - 1, 0], Eta_att * dist[n - 1] * unitvector[n - 1, 1]]  # 引力向量F_sum = np.array([F_rep[0] + F_att[0], F_rep[1] + F_att[1]])UnitVec_Fsum = 1 / np.linalg.norm(F_sum) * F_sum# 计算车的下一步位置Pi[0:2] = Pi[0:2] + len_step * UnitVec_Fsum
# 将目标添加到路径中
Path.append(Pg.tolist())# 画图
x = []  # 路径的x坐标
y = []  # 路径的y坐标
for val in Path:x.append(val[0])y.append(val[1])plt.plot(x, y, linewidth=0.6)
plt.axhline(y=0, color='g', linestyle=':',linewidth=0.3)
plt.axis([-5,100,-4,4])
plt.gca().set_aspect('equal')
plt.plot(0, -d / 2, 'ro', markersize=1)
plt.plot(15, 7 / 4, 'ro', markersize=1)
plt.plot(30, -3 / 2, 'ro', markersize=1)
plt.plot(45, 3 / 2, 'ro', markersize=1)
plt.plot(60, -3 / 4, 'ro', markersize=1)
plt.plot(80, 7 / 4, 'ro', markersize=1)
plt.plot(99, d / 2, 'ro', markersize=1)
plt.xlabel('x')
plt.ylabel('y')
plt.show()

（3）程序分析：

首先是程序的初始化部分，对道路宽度、汽车宽度、起点和目标点位置、障碍物的位置、引力的增益系数、斥力的增益系数、边界增益系数、障碍物的影响距离、步长、最大循环次数（程序中未用到）等参数进行设定。

进入循环，只要机器人当前位置与目标点之间的距离大于1就循环执行以下程序，将机器人当前状态（位置和速度）（也可以是模拟的当前位置和速度）添加到路径中，然后计算机器人当前位置与障碍物和目标点之间的单位方向向量，然后计算斥力和引力的大小、计算边界带来的斥力，计算合力的大小和方向，更新机器人的状态（位置和速度），以此循环。

当机器人当前位置与目标点之间的距离小于1时，循环结束，绘制路径，程序结束。

（4）上述程序运行结果示例：

2、来自 AtsushiSakai 的Python版本程序

（1）程序来源（GitHub链接）：potential_field_planning.py【点击可跳转】

（2）程序如下所示：

""" Potential Field based path planner author: Atsushi Sakai (@Atsushi_twi) Ref: https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf """from collections import deque
import numpy as np
import matplotlib.pyplot as plt# Parameters
KP = 5.0  # attractive potential gain
ETA = 100.0  # repulsive potential gain
AREA_WIDTH = 30.0  # potential area width [m]
# the number of previous positions used to check oscillations
OSCILLATIONS_DETECTION_LENGTH = 3show_animation = Truedef calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy):""" 计算势场图 gx,gy: 目标坐标 ox,oy: 障碍物坐标列表 reso: 势场图分辨率 rr: 机器人半径 sx,sy: 起点坐标 """# 确定势场图坐标范围：minx = min(min(ox), sx, gx) - AREA_WIDTH / 2.0miny = min(min(oy), sy, gy) - AREA_WIDTH / 2.0maxx = max(max(ox), sx, gx) + AREA_WIDTH / 2.0maxy = max(max(oy), sy, gy) + AREA_WIDTH / 2.0# 根据范围和分辨率确定格数：xw = int(round((maxx - minx) / reso))yw = int(round((maxy - miny) / reso))# calc each potentialpmap = [[0.0 for i in range(yw)] for i in range(xw)]for ix in range(xw):x = ix * reso + minx   # 根据索引和分辨率确定x坐标for iy in range(yw):y = iy * reso + miny  # 根据索引和分辨率确定x坐标ug = calc_attractive_potential(x, y, gx, gy)  # 计算引力uo = calc_repulsive_potential(x, y, ox, oy, rr)  # 计算斥力uf = ug + uopmap[ix][iy] = ufreturn pmap, minx, minydef calc_attractive_potential(x, y, gx, gy):""" 计算引力势能：1/2*KP*d """return 0.5 * KP * np.hypot(x - gx, y - gy)def calc_repulsive_potential(x, y, ox, oy, rr):""" 计算斥力势能： 如果与最近障碍物的距离dq在机器人膨胀半径rr之内：1/2*ETA*(1/dq-1/rr)**2 否则：0.0 """# search nearest obstacleminid = -1dmin = float("inf")for i, _ in enumerate(ox):d = np.hypot(x - ox[i], y - oy[i])if dmin >= d:dmin = dminid = i# calc repulsive potentialdq = np.hypot(x - ox[minid], y - oy[minid])if dq <= rr:if dq <= 0.1:dq = 0.1return 0.5 * ETA * (1.0 / dq - 1.0 / rr) ** 2else:return 0.0def get_motion_model():# dx, dy# 九宫格中 8个可能的运动方向motion = [[1, 0],[0, 1],[-1, 0],[0, -1],[-1, -1],[-1, 1],[1, -1],[1, 1]]return motiondef oscillations_detection(previous_ids, ix, iy):""" 振荡检测：避免“反复横跳” """previous_ids.append((ix, iy))if (len(previous_ids) > OSCILLATIONS_DETECTION_LENGTH):previous_ids.popleft()# check if contains any duplicates by copying into a setprevious_ids_set = set()for index in previous_ids:if index in previous_ids_set:return Trueelse:previous_ids_set.add(index)return Falsedef potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):# calc potential fieldpmap, minx, miny = calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy)# search pathd = np.hypot(sx - gx, sy - gy)ix = round((sx - minx) / reso)iy = round((sy - miny) / reso)gix = round((gx - minx) / reso)giy = round((gy - miny) / reso)if show_animation:draw_heatmap(pmap)# for stopping simulation with the esc key.plt.gcf().canvas.mpl_connect('key_release_event',lambda event: [exit(0) if event.key == 'escape' else None])plt.plot(ix, iy, "*k")plt.plot(gix, giy, "*m")rx, ry = [sx], [sy]motion = get_motion_model()previous_ids = deque()while d >= reso:minp = float("inf")minix, miniy = -1, -1# 寻找8个运动方向中势场最小的方向for i, _ in enumerate(motion):inx = int(ix + motion[i][0])iny = int(iy + motion[i][1])if inx >= len(pmap) or iny >= len(pmap[0]) or inx < 0 or iny < 0:p = float("inf")  # outside areaprint("outside potential!")else:p = pmap[inx][iny]if minp > p:minp = pminix = inxminiy = inyix = minixiy = miniyxp = ix * reso + minxyp = iy * reso + minyd = np.hypot(gx - xp, gy - yp)rx.append(xp)ry.append(yp)# 振荡检测，以避免陷入局部最小值：if (oscillations_detection(previous_ids, ix, iy)):print("Oscillation detected at ({},{})!".format(ix, iy))breakif show_animation:plt.plot(ix, iy, ".r")plt.pause(0.01)print("Goal!!")return rx, rydef draw_heatmap(data):data = np.array(data).Tplt.pcolor(data, vmax=100.0, cmap=plt.cm.Blues)def main():print("potential_field_planning start")sx = 0.0  # start x position [m]sy = 10.0  # start y positon [m]gx = 30.0  # goal x position [m]gy = 30.0  # goal y position [m]grid_size = 0.5  # potential grid size [m]robot_radius = 5.0  # robot radius [m]# 以下障碍物坐标是我进行修改后的，用来展示人工势场法的困于局部最优的情况：ox = [15.0, 5.0, 20.0, 25.0, 12.0, 15.0, 19.0, 28.0, 27.0, 23.0, 30.0, 32.0]  # obstacle x position list [m]oy = [25.0, 15.0, 26.0, 25.0, 12.0, 20.0, 29.0, 28.0, 26.0, 25.0, 28.0, 27.0]  # obstacle y position list [m]if show_animation:plt.grid(True)plt.axis("equal")# path generation_, _ = potential_field_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_radius)if show_animation:plt.show()if __name__ == '__main__':print(__file__ + " start!!")main()print(__file__ + " Done!!")

（3）上述程序运行结果示例：

3、来自 ShuiXinYun的Python版本程序

（1）程序来源（GitHub链接）：Artificial_Potential_Field【点击可跳转】

（2）程序Original_APF.py如下所示：

"""
人工势场寻路算法实现
最基本的人工势场，存在目标点不可达及局部最小点问题
"""
import math
import random
from matplotlib import pyplot as plt
from matplotlib.patches import Circle
import timeclass Vector2d():"""2维向量, 支持加减, 支持常量乘法(右乘)"""def __init__(self, x, y):self.deltaX = xself.deltaY = yself.length = -1self.direction = [0, 0]self.vector2d_share()def vector2d_share(self):if type(self.deltaX) == type(list()) and type(self.deltaY) == type(list()):deltaX, deltaY = self.deltaX, self.deltaYself.deltaX = deltaY[0] - deltaX[0]self.deltaY = deltaY[1] - deltaX[1]self.length = math.sqrt(self.deltaX ** 2 + self.deltaY ** 2) * 1.0if self.length > 0:self.direction = [self.deltaX / self.length, self.deltaY / self.length]else:self.direction = Noneelse:self.length = math.sqrt(self.deltaX ** 2 + self.deltaY ** 2) * 1.0if self.length > 0:self.direction = [self.deltaX / self.length, self.deltaY / self.length]else:self.direction = Nonedef __add__(self, other):"""+ 重载:param other::return:"""vec = Vector2d(self.deltaX, self.deltaY)vec.deltaX += other.deltaXvec.deltaY += other.deltaYvec.vector2d_share()return vecdef __sub__(self, other):vec = Vector2d(self.deltaX, self.deltaY)vec.deltaX -= other.deltaXvec.deltaY -= other.deltaYvec.vector2d_share()return vecdef __mul__(self, other):vec = Vector2d(self.deltaX, self.deltaY)vec.deltaX *= othervec.deltaY *= othervec.vector2d_share()return vecdef __truediv__(self, other):return self.__mul__(1.0 / other)def __repr__(self):return 'Vector deltaX:{}, deltaY:{}, length:{}, direction:{}'.format(self.deltaX, self.deltaY, self.length,self.direction)class APF():"""人工势场寻路"""def __init__(self, start: (), goal: (), obstacles: [], k_att: float, k_rep: float, rr: float,step_size: float, max_iters: int, goal_threshold: float, is_plot=False):""":param start: 起点:param goal: 终点:param obstacles: 障碍物列表，每个元素为Vector2d对象:param k_att: 引力系数:param k_rep: 斥力系数:param rr: 斥力作用范围:param step_size: 步长:param max_iters: 最大迭代次数:param goal_threshold: 离目标点小于此值即认为到达目标点:param is_plot: 是否绘图"""self.start = Vector2d(start[0], start[1])self.current_pos = Vector2d(start[0], start[1])self.goal = Vector2d(goal[0], goal[1])self.obstacles = [Vector2d(OB[0], OB[1]) for OB in obstacles]self.k_att = k_attself.k_rep = k_repself.rr = rr  # 斥力作用范围self.step_size = step_sizeself.max_iters = max_itersself.iters = 0self.goal_threashold = goal_thresholdself.path = list()self.is_path_plan_success = Falseself.is_plot = is_plotself.delta_t = 0.01def attractive(self):"""引力计算:return: 引力"""att = (self.goal - self.current_pos) * self.k_att  # 方向由机器人指向目标点return attdef repulsion(self):"""斥力计算:return: 斥力大小"""rep = Vector2d(0, 0)  # 所有障碍物总斥力for obstacle in self.obstacles:# obstacle = Vector2d(0, 0)t_vec = self.current_pos - obstacleif (t_vec.length > self.rr):  # 超出障碍物斥力影响范围passelse:rep += Vector2d(t_vec.direction[0], t_vec.direction[1]) * self.k_rep * (1.0 / t_vec.length - 1.0 / self.rr) / (t_vec.length ** 2)  # 方向由障碍物指向机器人return repdef path_plan(self):"""path plan:return:"""while (self.iters < self.max_iters and (self.current_pos - self.goal).length > self.goal_threashold):f_vec = self.attractive() + self.repulsion()self.current_pos += Vector2d(f_vec.direction[0], f_vec.direction[1]) * self.step_sizeself.iters += 1self.path.append([self.current_pos.deltaX, self.current_pos.deltaY])if self.is_plot:plt.plot(self.current_pos.deltaX, self.current_pos.deltaY, '.b')plt.pause(self.delta_t)if (self.current_pos - self.goal).length <= self.goal_threashold:self.is_path_plan_success = Trueif __name__ == '__main__':# 相关参数设置k_att, k_rep = 1.0, 100.0rr = 3step_size, max_iters, goal_threashold = .2, 500, .2  # 步长0.5寻路1000次用时4.37s, 步长0.1寻路1000次用时21sstep_size_ = 2# 设置、绘制起点终点start, goal = (0, 0), (15, 15)is_plot = Trueif is_plot:fig = plt.figure(figsize=(7, 7))subplot = fig.add_subplot(111)subplot.set_xlabel('X-distance: m')subplot.set_ylabel('Y-distance: m')subplot.plot(start[0], start[1], '*r')subplot.plot(goal[0], goal[1], '*r')# 障碍物设置及绘制obs = [[1, 4], [2, 4], [3, 3], [6, 1], [6, 7], [10, 6], [11, 12], [14, 14]]print('obstacles: {0}'.format(obs))for i in range(0):obs.append([random.uniform(2, goal[1] - 1), random.uniform(2, goal[1] - 1)])if is_plot:for OB in obs:circle = Circle(xy=(OB[0], OB[1]), radius=rr, alpha=0.3)subplot.add_patch(circle)subplot.plot(OB[0], OB[1], 'xk')# t1 = time.time()# for i in range(1000):# path planif is_plot:apf = APF(start, goal, obs, k_att, k_rep, rr, step_size, max_iters, goal_threashold, is_plot)else:apf = APF(start, goal, obs, k_att, k_rep, rr, step_size, max_iters, goal_threashold, is_plot)apf.path_plan()if apf.is_path_plan_success:path = apf.pathpath_ = []i = int(step_size_ / step_size)while (i < len(path)):path_.append(path[i])i += int(step_size_ / step_size)if path_[-1] != path[-1]:  # 添加最后一个点path_.append(path[-1])print('planed path points:{}'.format(path_))print('path plan success')if is_plot:px, py = [K[0] for K in path_], [K[1] for K in path_]  # 路径点x坐标列表, y坐标列表subplot.plot(px, py, '^k')plt.show()else:print('path plan failed')# t2 = time.time()# print('寻路1000次所用时间:{}, 寻路1次所用时间:{}'.format(t2-t1, (t2-t1)/1000))

（3）程序improved_APF-1.py如下所示：

"""
人工势场寻路算法实现
改进人工势场，解决不可达问题，仍存在局部最小点问题
"""
from Original_APF import APF, Vector2d
import matplotlib.pyplot as plt
import math
from matplotlib.patches import Circle
import randomdef check_vec_angle(v1: Vector2d, v2: Vector2d):v1_v2 = v1.deltaX * v2.deltaX + v1.deltaY * v2.deltaYangle = math.acos(v1_v2 / (v1.length * v2.length)) * 180 / math.pireturn angleclass APF_Improved(APF):def __init__(self, start: (), goal: (), obstacles: [], k_att: float, k_rep: float, rr: float,step_size: float, max_iters: int, goal_threshold: float, is_plot=False):self.start = Vector2d(start[0], start[1])self.current_pos = Vector2d(start[0], start[1])self.goal = Vector2d(goal[0], goal[1])self.obstacles = [Vector2d(OB[0], OB[1]) for OB in obstacles]self.k_att = k_attself.k_rep = k_repself.rr = rr  # 斥力作用范围self.step_size = step_sizeself.max_iters = max_itersself.iters = 0self.goal_threashold = goal_thresholdself.path = list()self.is_path_plan_success = Falseself.is_plot = is_plotself.delta_t = 0.01def repulsion(self):"""斥力计算, 改进斥力函数, 解决不可达问题:return: 斥力大小"""rep = Vector2d(0, 0)  # 所有障碍物总斥力for obstacle in self.obstacles:# obstacle = Vector2d(0, 0)obs_to_rob = self.current_pos - obstaclerob_to_goal = self.goal - self.current_posif (obs_to_rob.length > self.rr):  # 超出障碍物斥力影响范围passelse:rep_1 = Vector2d(obs_to_rob.direction[0], obs_to_rob.direction[1]) * self.k_rep * (1.0 / obs_to_rob.length - 1.0 / self.rr) / (obs_to_rob.length ** 2) * (rob_to_goal.length ** 2)rep_2 = Vector2d(rob_to_goal.direction[0], rob_to_goal.direction[1]) * self.k_rep * ((1.0 / obs_to_rob.length - 1.0 / self.rr) ** 2) * rob_to_goal.lengthrep +=(rep_1+rep_2)return repif __name__ == '__main__':# 相关参数设置k_att, k_rep = 1.0, 0.8rr = 3step_size, max_iters, goal_threashold = .2, 500, .2  # 步长0.5寻路1000次用时4.37s, 步长0.1寻路1000次用时21sstep_size_ = 2# 设置、绘制起点终点start, goal = (0, 0), (15, 15)is_plot = Trueif is_plot:fig = plt.figure(figsize=(7, 7))subplot = fig.add_subplot(111)subplot.set_xlabel('X-distance: m')subplot.set_ylabel('Y-distance: m')subplot.plot(start[0], start[1], '*r')subplot.plot(goal[0], goal[1], '*r')# 障碍物设置及绘制obs = [[1, 4], [2, 4], [3, 3], [6, 1], [6, 7], [10, 6], [11, 12], [14, 14]]print('obstacles: {0}'.format(obs))for i in range(0):obs.append([random.uniform(2, goal[1] - 1), random.uniform(2, goal[1] - 1)])if is_plot:for OB in obs:circle = Circle(xy=(OB[0], OB[1]), radius=rr, alpha=0.3)subplot.add_patch(circle)subplot.plot(OB[0], OB[1], 'xk')# t1 = time.time()# for i in range(1000):# path planif is_plot:apf = APF_Improved(start, goal, obs, k_att, k_rep, rr, step_size, max_iters, goal_threashold, is_plot)else:apf = APF_Improved(start, goal, obs, k_att, k_rep, rr, step_size, max_iters, goal_threashold, is_plot)apf.path_plan()if apf.is_path_plan_success:path = apf.pathpath_ = []i = int(step_size_ / step_size)while (i < len(path)):path_.append(path[i])i += int(step_size_ / step_size)if path_[-1] != path[-1]:  # 添加最后一个点path_.append(path[-1])print('planed path points:{}'.format(path_))print('path plan success')if is_plot:px, py = [K[0] for K in path_], [K[1] for K in path_]  # 路径点x坐标列表, y坐标列表subplot.plot(px, py, '^k')plt.show()else:print('path plan failed')# t2 = time.time()# print('寻路1000次所用时间:{}, 寻路1次所用时间:{}'.format(t2-t1, (t2-t1)/1000))

（4）上述程序运行结果示例：

4、来自 jubobolv369 的MATLAB版本程序

（1）程序来源：人工势场法–路径规划–原理–matlab代码【点击可跳转】

（2）程序如下所示：

clc;
clear;
close all;
%% 基本信息、常数等设置
eta_ob=25;          %计算障碍物斥力的权益系数
eta_goal=10;        %计算障目标引力的权益系数
eta_border=25;      %车道边界斥力权益系数
n=1;                 %计算障碍物斥力的常数
border0=20;          %斥力作用边界
max_iter=1000;        %最大迭代次数
step=0.3;            %步长car_width=1.8;         %车宽
car_length=3.5;      %车长
road_width=3.6;      %道路宽
road_length=100;      %道路长%% 起点、障碍物、目标点的坐标、速度信息P0=[3 1.3 1 1];               %横坐标 纵坐标 x方向分速度 y方向分速度
Pg=[road_length-4 5.4 0 0];
Pob=[15 1.8;30 5.4;46 1.6;65 5.0;84 2.7;]%% 未达目标附近前不断循环Pi=P0;i=1;while sqrt((Pi(1)-Pg(1))^2+(Pi(2)-Pg(2))^2)>1if i>max_iterbreak;end%计算每个障碍物与当前车辆位置的向量(斥力)、距离、单位向量for j=1:size(Pob,1)vector(j,:)=Pi(1,1:2)-Pob(j,1:2);dist(j,:)=norm(vector(j,:));unit_vector(j,:)=[vector(j,1)/dist(j,:) vector(j,2)/dist(j,:)];end%计算目标与当前车辆位置的向量(引力)、距离、单位向量max=j+1;vector(max,:)=Pg(1,1:2)-Pi(1,1:2);dist(max,:)=norm(vector(max,:));unit_vector(max,:)=[vector(max,1)/dist(max,:) vector(max,2)/dist(max,:)];%计算每个障碍物的斥力for j=1:size(Pob,1)if dist(j,:)>=border0Fre(j,:)=[0,0];else%障碍物斥力指向物体Fre_abs_ob=eta_ob*(1/dist(j,:)-1/border0)*(dist(max)^n/dist(j,:)^2);Fre_ob=[Fre_abs_ob*unit_vector(j,1) Fre_abs_ob*unit_vector(j,2)];%障碍物斥力 指向目标Fre_abs_g=n/2*eta_ob*(1/dist(j,:)-1/border0)^2*dist(max)^(n-1);Fre_g=[Fre_abs_g*unit_vector(max,1) Fre_abs_g*unit_vector(max,2)];Fre(j,:)=Fre_ob+Fre_g;end end  if Pi(2)>=(road_width-car_width)/2 && Pi(2)< road_width/2   %下绿色下区域Fre_edge=[0,eta_border*norm(Pi(1,3:4))*(exp(road_width/2)-Pi(2))];elseif Pi(2)>= road_width/2 &&  Pi(2)<= (road_width+car_width)/2   %下绿色上区域Fre_edge=[0,-1/3*eta_border*Pi(2)^2];elseif Pi(2)>=(3*road_width-car_width)/2 && Pi(2)<= 3*road_width/2   %上绿色下区域Fre_edge=[0,1/3*eta_border*(3*road_width/2-Pi(2))^2];elseif Pi(2)>=3*road_width/2 && Pi(2)<= (3*road_width+car_width)/2    %上绿色上区域Fre_edge=[0,eta_border*norm(Pi(1,3:4))*(exp(Pi(2)-3*road_width/2))];else Fre_edge=[0 0];endFre=[sum(Fre(:,1))+Fre_edge(1) sum(Fre(:,2))+Fre_edge(2)]; %计算引力Fat=[eta_goal*dist(max,1)*unit_vector(max,1) eta_goal*dist(max,1)*unit_vector(max,2)];F_sum=[Fre(1)+Fat(1),Fre(2)+Fat(2)];unit_vector_sum(i,:)=F_sum/norm(F_sum);Pi(1,1:2)= Pi(1,1:2)+step*unit_vector_sum(i,:);path(i,:)= Pi(1,1:2);i=i+1;end%% 画图
figure(1)
%下车道下红色区域
fill([0,road_length,road_length,0],[0,0,(road_width-car_width)/2,(road_width-car_width)/2],[1,0,0]);
hold on
%下车道上红色区域，上车道下红色区域
fill([0,road_length,road_length,0],[(road_width+car_width)/2,(road_width+car_width)/2,...(3*road_width-car_width)/2,(3*road_width-car_width)/2],[1,0,0]);
%下车道绿色区域
fill([0,road_length,road_length,0],[(road_width-car_width)/2,(road_width-car_width)/2,...(road_width+car_width)/2,(road_width+car_width)/2],[0,1,0]);%上车道绿色区域
fill([0,road_length,road_length,0],[(3*road_width-car_width)/2,(3*road_width-car_width)/2,...(3*road_width+car_width)/2,(3*road_width+car_width)/2],[0,1,0]);
%上车道上红色区域
fill([0,road_length,road_length,0],[ (3*road_width+car_width)/2,(3*road_width+car_width)/2,2*road_width,2*road_width],[1,0,0]);
%路面中心线、边界线
plot([0,road_length],[road_width,road_width],'w--','linewidth',2);
plot([0,road_length],[2*road_width,2*road_width],'w','linewidth',2);
plot([0,road_length],[0,0],'w','linewidth',2);%障碍物、目标
plot(Pob(:,1),Pob(:,2),'ko');
plot(Pg(1),Pg(2),'mp');
%车
fill([P0(1)-car_length/2,P0(1)+car_length/2,P0(1)+car_length/2,P0(1)-car_length/2],...[P0(2)-car_width/2,P0(2)-car_width/2,P0(2)+car_width/2,P0(2)+car_width/2],[0,0,1]);plot(path(:,1),path(:,2),'w.');axis equal
set(gca,'XLim',[0 road_length])
set(gca,'YLim',[0 2*road_width])

（3）上述程序运行结果示例：

人工势场法路径规划算法（APF）相关推荐

基于人工势场法的二维平面内无人机的路径规划的matlab仿真,并通过对势场法改进避免了无人机陷入极值的问题
目录 1.算法描述 2.matlab算法仿真效果 3.MATLAB核心程序 4.完整MATLAB 1.算法描述人工势场法原理是:首先构建一个人工虚拟势场,该势场由两部分组成,一部分是目标点对移动机器 ...
路劲规划与轨迹跟踪学习4——人工势场法
本文参考(85条消息) [路径规划]局部路径规划算法--人工势场法(含python实现 | c++实现)_CHH3213的博客-CSDN博客_人工势场法路径规划路径规划与轨迹跟踪系列算法学习_第6讲 ...
路径规划算法3 改进的人工势场法（Matlab）
目录传统人工势场引力势场斥力势场合力势场传统人工势场法存在的问题改进的人工势场函数 Matlab代码实现参考链接: [1]朱伟达. 基于改进型人工势场法的车辆避障路径规划研究[D]. 江 ...
基于人工势场法的路径规划
基于人工势场法的路径规划 \qquad 路径规划是移动机器人领域的一个重要组成部分,人工势场法是机器人路径规划算法中一种简单有效的方法. \qquad 势场法的基本思想是在移动机器人的工作环境中构造一 ...
基于人工势场法和果蝇优化算法的路径规划（Matlab代码实现）
目录 1 概述 2 运行结果 2.1 算例1 2.2 算例2 3 Matlab代码实现 4 参考文献 1 概述近年来,智能机器人逐渐应用于医疗服务﹑航空等众多领域.路径规划作为机器人实现智能自 ...
【全局规划】人工势场法（APF）
人工势场法APF clc clear close all%% 初始化车的参数 d = 3.5; % 道路标准宽度 W = 1.8; % 汽车宽度 L = 4.7; % 车长P0 = [0,-d/2,1 ...
机器人路径规划_人工势场法
机器人路径规划_人工势场法原理人工势场法是由Khatib提出的一种虚拟力法.原理是:将机器人在环境中的运动视为一种机器人在虚拟的人工受力场的运动.障碍物对机器人产生斥力,目标点对机器人产生引力,引 ...
matlab人工势场法三维演示图,运动规划入门 | 5. 白话人工势场法，从原理到Matlab实现...
如何利用人工势场进行运动规划? 1.1 引力势场(Attractive Potential Field) 人工势场这个特殊的势场并不是一个单一的场,其实它是由两个场叠加组合而成的,一个是引力场,一个是 ...
多机器人编队人工势场法协同避障算法原理及实现
多机器人编队(二)多机器人编队人工势场法协同避障算法原理及实现避障算法原理避障算法仿真多机器人协同编队需要将理论和实践紧密地结合起来,其应用包括编队队形生成.保持.变换和路径规划与避障等等都是基 ...

人工势场法路径规划算法（APF）

人工势场法路径规划算法（APF）相关推荐

最新文章

热门文章