人工势场法路径规划算法（APF）
本文主要对人工势场法路径规划算法进行介绍，主要涉及人工势场法的简介、引力和斥力模型及其推导过程、人工势场法的缺陷及改进思路、人工势场法的Python与MATLAB开源源码等方面
一、人工势场法简介
人工势场法是由Khatib于1985年在论文《Real-Time Obstacle Avoidance for Manipulators and Mobile Robots》中提出的一种虚拟力法。它的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。应用势场法规划出来的路径一般是比较平滑并且安全，但是这种方法存在局部最优点问题。
二、引力与斥力模型及公式推导
1、基础知识补充：梯度
梯度的本意是一个向量（矢量），表示某一函数在该点处的方向导数沿着该方向取得最大值，即函数在该点处沿着该方向（此梯度的方向）变化最快，变化率最大（为该梯度的模）。
2、引力与斥力模型
引力势场主要与机器人和目标点间的距离有关，距离越大，机器人所受的势能值就越大;距离越小，机器人所受的势能值则越小，所以引力势场的函数为:
U a t t ( q ) = 1 2 η ρ 2 ( q , q g ) 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|。矢量方向是从机器人的位置指向目标点位
置。
相应的引力 F a t t ( X ) F_{att}\left(X\right) Fatt(X)为引力场的负梯度:
F a t t ( X ) = − ∇ U a t t ( X ) = η ρ ( q , q g ) F_{att}\left(X\right)=-\nabla U_{att}\left(X\right)=\eta\rho\left(q,q_{g}\right) Fatt(X)=−∇Uatt(X)=ηρ(q,qg)
决定障碍物斥力势场的因素是机器人与障碍物间的距离，当机器人未进入障碍物的影响范围时，其受到的势能值为零;在机器人进入障碍物的影响范围后，两者之间的距离越大,机器人受到的斥力就越小，距离越小，机器人受到的斥力就越大,斥力势场的势场函数为：
U r e 1 ( X ) = { 1 2 k ( 1 ρ ( q , q o ) − 1 ρ o ) 2 0 ≤ ρ ( q , q o ) ≤ ρ o 0 ρ ( q , q o ) ≥ ρ o U_{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为一常数，表示障碍物对机器人产生作用的最大距离。相应的斥力为斥力场的负梯度:
F n 0 ( X ) = { k ( 1 ρ ( q , q 0 ) − 1 ρ 0 ) 1 ρ 2 ( q , q 0 ) ∇ ρ ( q , q 0 ) 0 ≤ ρ ( q , q 0 ) ≤ ρ 0 0 ρ ( q , q 0 ) ≥ ρ 0 F_{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 , q 0 ) \begin{aligned}\nabla\rho(q,q_0)\end{aligned} ∇ρ(q,q0)表示从 q 0 q_0 q0指向q的单位向量
∇ ρ ( q , q 0 ) = q − q 0 ∥ q − q 0 ∥ \nabla\rho(q,q_0)=\dfrac{q-q_0}{\|q-q_0\|} ∇ρ(q,q0)=∥q−q0∥q−q0
3、引力与斥力公式的推导过程
设机器人位置为(x, y)，障碍物位置为(xg, yg)，则引力势场函数可由下式
U a t t ( q ) = 1 2 η ρ 2 ( q , q g ) U_{att}\big(q\big)=\dfrac{1}{2}\eta\rho^2\big(q,q_g\big) Uatt(q)=21ηρ2(q,qg)
改写为：
U a t t ( x , y ) = 1 2 η [ ( x − x g ) 2 + ( y − y g ) 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=-\nabla F=−∇
− g r a d U a t t ( x , y ) = − ∇ U a t t ( x , y ) = − U a t t , x ′ ( x , y ) i → − U a t t , 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 − x g ) i → − η ( y − y g ) j → = η [ ( x g − x ) i → + ( y g − 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 − x g ) 2 + ( y g − y ) 2 = η ρ ( q , q g ) =\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)
斥力势场函数可由下式:
U n q ( q ) = 1 2 k ( 1 ρ ( q , q 0 ) − 1 ρ 0 ) 2 U_{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
改写为：
U n q q ( x , y ) = 1 2 k [ 1 ( x − x 0 ) 2 + ( y − y 0 ) 2 − 1 ρ 0 ] 2 U_{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)2 1−ρ01]2
所以：
− ∇ U m q ( x , y ) = − U a t t , x ( x , y ) i → − U a t t , 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求偏导部分过程如下：
− U a t t , x ′ ( x , y ) i ⃗ = − k [ 1 ( x − x 0 ) 2 + ( y − y 0 ) 2 − 1 ρ 0 ] [ 1 ( x − x 0 ) 2 + ( y − y 0 ) 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)2 1−ρ01 (x−x0)2+(y−y0)2 1−ρ01 ′i
= − k [ 1 ( x − x 0 ) 2 + ( y − y 0 ) 2 − 1 ρ 0 ] { − 1 2 [ ( x − x 0 ) 2 + ( y − y 0 ) 2 ] − 3 2 [ ( x − x 0 ) 2 + ( y − y 0 ) 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)2 1−ρ01]{−21[(x−x0)2+(y−y0)2]−23[(x−x0)2+(y−y0)2]′}i
= k [ 1 ( x − x 0 ) 2 + ( y − y 0 ) 2 − 1 ρ 0 ] { 1 ( x − x 0 ) 2 + ( y − y 0 ) 2 [ ( x − x 0 ) 2 + ( y − y 0 ) 2 ] − 1 2 ( x − x 0 ) } 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)2 1−ρ01]{(x−x0)2+(y−y0)21[(x−x0)2+(y−y0)2]−21(x−x0)}i
= k ( 1 ρ ( q , q 0 ) − 1 ρ 0 ) ⋅ 1 ρ 2 ( q , q 0 ) ⋅ 1 ρ ( q , q 0 ) ⋅ ( x − x 0 ) 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求偏导部分结果如下：
− U a t t , y ′ ( x , y ) j ⃗ = k ( 1 ρ ( q , q 0 ) − 1 ρ 0 ) ⋅ 1 ρ 2 ( q , q 0 ) ⋅ 1 ρ ( q , q 0 ) ⋅ ( y − y 0 ) 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→
所以，斥力函数为：
− ∇ U q q ( x , y ) = k ( 1 ρ ( q , q 0 ) − 1 ρ 0 ) ⋅ 1 ρ 2 ( q , q 0 ) ⋅ ∇ ρ ( q , q 0 ) -\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为任意常数：
{ F r e q 1 = k ( 1 ρ ( q , q o ) − 1 ρ o ) ρ g n ρ 2 ( q , q o ) F r e q 2 = n 2 k ( 1 ρ ( q , q o ) − 1 ρ o ) 2 ρ g n − 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.5
Path.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=1
F_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) ** 2
F_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
（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 time
class Vector2d():
"""
2维向量, 支持加减, 支持常量乘法(右乘)
"""
def __init__(self, x, y):
self.deltaX = x
self.deltaY = y
self.length = -1
self.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.deltaY
self.deltaX = deltaY[0] - deltaX[0]
self.deltaY = deltaY[1] - deltaX[1]
self.length = math.sqrt(self.deltaX ** 2 + self.deltaY ** 2) * 1.0
if self.length > 0:
self.direction = [self.deltaX / self.length, self.deltaY / self.length]
else:
self.direction = None
else:
self.length = math.sqrt(self.deltaX ** 2 + self.deltaY ** 2) * 1.0
if self.length > 0:
self.direction = [self.deltaX / self.length, self.deltaY / self.length]
else:
self.direction = None
def __add__(self, other):
"""
+ 重载
:param other:
:return:
"""
vec = Vector2d(self.deltaX, self.deltaY)
vec.deltaX += other.deltaX
vec.deltaY += other.deltaY
vec.vector2d_share()
return vec
def __sub__(self, other):
vec = Vector2d(self.deltaX, self.deltaY)
vec.deltaX -= other.deltaX
vec.deltaY -= other.deltaY
vec.vector2d_share()
return vec
def __mul__(self, other):
vec = Vector2d(self.deltaX, self.deltaY)
vec.deltaX *= other
vec.deltaY *= other
vec.vector2d_share()
return vec
def __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_att
self.k_rep = k_rep
self.rr = rr # 斥力作用范围
self.step_size = step_size
self.max_iters = max_iters
self.iters = 0
self.goal_threashold = goal_threshold
self.path = list()
self.is_path_plan_success = False
self.is_plot = is_plot
self.delta_t = 0.01
def attractive(self):
"""
引力计算
:return: 引力
"""
att = (self.goal - self.current_pos) * self.k_att # 方向由机器人指向目标点
return att
def repulsion(self):
"""
斥力计算
:return: 斥力大小
"""
rep = Vector2d(0, 0) # 所有障碍物总斥力
for obstacle in self.obstacles:
# obstacle = Vector2d(0, 0)
t_vec = self.current_pos - obstacle
if (t_vec.length > self.rr): # 超出障碍物斥力影响范围
pass
else:
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 rep
def path_plan(self):
"""
path plan
:return:
"""
while (self.iters self.goal_threashold):
f_vec = self.attractive() + self.repulsion()
self.current_pos += Vector2d(f_vec.direction[0], f_vec.direction[1]) * self.step_size
self.iters += 1
self.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.rr): # 超出障碍物斥力影响范围
pass
else:
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.length
rep +=(rep_1+rep_2)
return rep
if __name__ == '__main__':
# 相关参数设置
k_att, k_rep = 1.0, 0.8
rr = 3
step_size, max_iters, goal_threashold = .2, 500, .2 # 步长0.5寻路1000次用时4.37s, 步长0.1寻路1000次用时21s
step_size_ = 2
# 设置、绘制起点终点
start, goal = (0, 0), (15, 15)
is_plot = True
if 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 plan
if 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.path
path_ = []
i = int(step_size_ / step_size)
while (i
（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)>1
if i>max_iter
break;
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,:)>=border0
Fre(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 && Pi(2)=(3*road_width-car_width)/2 && Pi(2)=3*road_width/2 && Pi(2)
