【路径规划】局部路径规划算法——人工势场法（含python实现 | c++实现）

文章目录

• 参考资料
• 1. 算法简介
• 2. 算法精讲
• • 2.1 引力势场
  • 2.2 斥力势场
  • 2.3 合力势场
• 3. 引力斥力推导计算
• 4. 算法缺陷与改进
• • 4.1 目标不可达的问题
  • 4.2 陷入局部最优的问题
  • 4.3 解决方案
  • • 4.3.1 改进障碍物斥力势场函数
    • 4.3.2 道路边界斥力势场
• 5. python实现
• 6. c++实现

• 路径规划与轨迹跟踪系列算法
• 基于改进型人工势场法的车辆避障路径规划研究
• 基于改进人工势场法的车辆避障路径规划研究

• 1986 年 Khatib 首先提出人工势场法，并将其应用在机器人避障领域， 而现代汽车可以看作是一个高速行驶的机器人，所以该方法也可应用于汽车的避障路径规划领域。

• 人工势场法的基本思想是在障碍物周围构建障碍物斥力势场，在目标点周围构建引力势场，类似于物理学中的电磁场

• 被控对象在这两种势场组成的复合场中受到斥力作用和引力作用，斥力和引力的合力指引着被控对象的运动，搜索无碰的避障路径。

• 更直观而言， 势场法是将障碍物比作是平原上具有高势能值的山峰， 而目标点则是具有低势能值的低谷。

2.1 引力势场

引力势场主要与汽车和目标点间的距离有关, 距离越大, 汽车所受的势能值就越大; 距离越小, 汽车所受的势能值则越小, 所以引力势场的函数为:
$$U_{att}(q)=\frac{1}{2}\eta {\rho }^2\left(q,q_g\right)\text{\hspace{1em}(1)}$$
其中 $\eta$ 为正比例增益系数, $\rho \left(q,q_g\right)$ 为一个矢量, 表示汽车的位置 $q$ 和目标点位置 $q_g$ 之间的欧式距离 $\left|q-q_g\right|$, 矢量方向是从汽车的位置指向目标点位置。

相应的引力 $F_{\text{att }}(q)$ 为引力场的负梯度，代表引力势场函数$U_{att}(q)$的变化最快方向。
$$F_{att}(q)=-\nabla U_{att}(q)=-\eta \rho \left(q,q_g\right)\text{\hspace{1em}(2)}$$

2.2 斥力势场

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

• 斥力势场的势场函数为:
  $$U_{req}(q)=\{\begin{array}{lc}\frac{1}{2}k{\left(\frac{1}{\rho \left(q,q_0\right)}-\frac{1}{{\rho }_0}\right)}^2,& 0\le \rho \left(q,q_0\right)\le {\rho }_0\\ 0,& \rho \left(q,q_0\right)\ge {\rho }_0\end{array}\text{\hspace{1em}(3)}$$

  其中 $k$ 为正比例系数, $\rho \left(q,q_0\right)$ 为一矢量, 方向为从障碍物指向汽车, 大小为汽车与障碍物间的欧式距离 $\left|q-q_0\right|,{\rho }_0$ 为一常数, 表示障碍物对汽车产生作用的最大影响范围。

  由公式(3)可知，斥力势场不同于引力势场，智能汽车不总是受到障碍对它的斥力作用。当汽车与障碍物之间的相对距离超过${\rho }_0$时，就判定此障碍对汽车不再有影响作用。当汽车进入障碍物的影响范围之后,即汽车与障碍的相对距离小于${\rho }_0$时，汽车开始受到障碍物的斥力影响。汽车与障碍物的相对距离越小，斥力影响越大,自身势能升高。汽车与障碍物的相对距离越大，斥力影响越小，自身势能降低。

• 相应的斥力为斥力势场的负梯度作用力:
  $$F_{req}(q)=\{\begin{array}{ll}k\left(\frac{1}{\rho \left(q,q_0\right)}-\frac{1}{{\rho }_0}\right)\frac{1}{{\rho }^2\left(q,q_0\right)},& 0\le \rho \left(q,q_0\right)\le {\rho }_0\\ 0,& \rho \left(q,q_0\right)\ge {\rho }_0\end{array}\text{\hspace{1em}(4)}$$

2.3 合力势场

根据上述定义的引力场函数和斥力场函数，可以得到整个运行空间的复合场，机器人的合力势场大小为机器人所受的斥力势场和引力势场之和，故合力势场总函数为：
$$U(q)=U_{att}(q)+U_{req}(q)\text{\hspace{1em}(5)}$$
所受合力为
$$F(q)=-\nabla U(q)=F_{att}(q)+F_{req}(q)\text{\hspace{1em}(6)}$$

合力的方向决定汽车的行驶朝向，合力的大小决定汽车的行驶加速度。

不妨设车辆位置为$(x,y)$，障碍物位置为$(x_g,y_g)$。

根据公式(1)，引力势场函数为
$$U_{att}(q)=\frac{1}{2}\eta {\rho }^2\left(q,q_g\right)\Rightarrow U_{att}(x,y)=\frac{1}{2}\eta \left[{\left(x-x_g\right)}^2+{\left(y-y_g\right)}^2\right]\text{\hspace{1em}(7)}$$
故引力势场的负梯度有
$$\begin{array}{rl}-{\mathrm{grad}}_{att}(x,y)& =-\nabla U_{att}(x,y)\\ & =-U_{att,x}^{\prime }(x,y)\vec{i}-U_{att,y}^{\prime }(x,y)\vec{j}\\ & =-\eta \left(x-x_g\right)\vec{i}-\eta \left(y-y_g\right)\vec{j}\\ & =\eta \left[\left(x_g-x\right)\vec{i}+\left(y_g-y\right)\vec{j}\right]\\ \Rightarrow 引力大小& =\eta \sqrt{{\left(x-x_g\right)}^2+{\left(y_g-y\right)}^2}=\eta \rho \left(q,q_g\right)\end{array}\text{\hspace{1em}(8)}$$

同理，斥力势场函数为
$$\begin{array}{r}{U}_{req}(q)=\frac{1}{2}k{\left(\frac{1}{\rho \left(q,q_0\right)}-\frac{1}{{\rho }_0}\right)}^2\Rightarrow U_{req}(x,y)=\frac{1}{2}k{\left[\frac{1}{\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}}-\frac{1}{{\rho }_0}\right]}^2\end{array}\text{\hspace{1em}(9)}$$
斥力势场的负梯度为
$$-\nabla U_{req}(x,y)=-U_{req,x}^{\prime }(x,y)\vec{i}-U_{req,y}^{\prime }(x,y)\vec{j}\text{\hspace{1em}(10)}$$
将公式(10)各项分别展开：
$$\begin{array}{rl}-U_{req,x}^{\prime }(x,y)\vec{i}& =-k\left[\frac{1}{\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}}-\frac{1}{{\rho }_0}\right]{\left[\frac{1}{\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}}-\frac{1}{{\rho }_0}\right]}^{\prime }\vec{i}\\ & =-k\left[\frac{1}{\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}}-\frac{1}{{\rho }_0}\right]\left\{-\frac{1}{2}{\left[{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2\right]}^{-\frac{3}{2}}\cdot {\left[{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2\right]}^{\prime }\right\}\vec{i}\\ & =k\left[\frac{1}{\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}}-\frac{1}{{\rho }_0}\right]\left\{\frac{1}{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}{\left[{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2\right]}^{-\frac{1}{2}}\left(x-x_0\right)\right\}\vec{i}\\ & =k\left(\frac{1}{\rho \left(q,q_0\right)}-\frac{1}{{\rho }_0}\right)\cdot \frac{1}{{\rho }^2\left(q,q_0\right)}\cdot \frac{1}{\rho \left(q,q_0\right)}\cdot \left(x-x_0\right)\vec{i}\end{array}\text{\hspace{1em}(11-1)}$$
$$\begin{array}{rl}-U_{req,y}^{\prime }(x,y)\vec{i}& =-k\left[\frac{1}{\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}}-\frac{1}{{\rho }_0}\right]{\left[\frac{1}{\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}}-\frac{1}{{\rho }_0}\right]}^{\prime }\vec{j}\\ & =-k\left[\frac{1}{\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}}-\frac{1}{{\rho }_0}\right]\left\{-\frac{1}{2}{\left[{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2\right]}^{-\frac{3}{2}}\cdot {\left[{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2\right]}^{\prime }\right\}\vec{j}\\ & =k\left[\frac{1}{\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}}-\frac{1}{{\rho }_0}\right]\left\{\frac{1}{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}{\left[{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2\right]}^{-\frac{1}{2}}\left(y-y_0\right)\right\}\vec{j}\\ & =k\left(\frac{1}{\rho \left(q,q_0\right)}-\frac{1}{{\rho }_0}\right)\cdot \frac{1}{{\rho }^2\left(q,q_0\right)}\cdot \frac{1}{\rho \left(q,q_0\right)}\cdot \left(y-y_0\right)\vec{j}\end{array}\text{\hspace{1em}(11-2)}$$

化简后得斥力大小为
$$\begin{array}{r}-\nabla U_{req}(x,y)=k\left(\frac{1}{\rho \left(q,q_0\right)}-\frac{1}{{\rho }_0}\right)\cdot \frac{1}{{\rho }^2\left(q,q_0\right)}\end{array}\text{\hspace{1em}(12)}$$

4.1 目标不可达的问题

由于障碍物与目标点距离太近，当汽车到达目标点时，根据势场函数可知，目标点的引力降为零，而障碍物的斥力不为零，此时汽车虽到达目标点， 但在斥力场的作用下不能停下来，从而导致目标不可达的问题。

4.2 陷入局部最优的问题

车辆在某个位置时，无法向前搜索避障路径。

出现局部最优主要有两种情况：

• 如下图，汽车受到的障碍物的斥力和目标点的引力之间的夹角近似为180°，几乎在同一条直线上,就会出现汽车在障碍物前陷入局部最优的问题。

• 如果若干个障碍物的合斥力与目标点的引力大小相等、方向相反，则合力为0，智能汽车自身判断到达势能极小值的位置，但没有到达期望的目标点位置。由于合力为零，汽车就会陷在势能极小的位置，无法继续前进和转向，以致无法到达期望的目标点。

4.3 解决方案

解决方案可参考资料2和资料3，这两篇论文均给出了不一样的解决方案，但思路几乎差不多。下面以参考资料2给出的方案进行简单叙述。

4.3.1 改进障碍物斥力势场函数

通过改进障碍物斥力势场函数来解决局部最优和目标不可达的问题；在传统人工势场法的障碍物斥力场模型中加入调节因子 ${\rho }_g^n$, 使汽车只有到达目标点时, 斥力和引力才同时减小到零, 从而使局部最优和目标不可达的问题得到解决。

改进后的斥力场函数为:
$$U_{\text{req }}(q)=\{\begin{array}{ll}\frac{1}{2}k{\left(\frac{1}{\rho \left(q,q_0\right)}-\frac{1}{{\rho }_0}\right)}^2{\rho }_g^n,& 0\le \rho \left(q,q_0\right)\le {\rho }_0\\ 0,& \rho \left(q,q_0\right)>{\rho }_0\end{array}\text{\hspace{1em}(13)}$$
${\rho }_g^n$ 为汽车与目标点的距离，式中$n$为可选的正常数。

$$\{\begin{array}{l}{F}_{req}=F_{req1}+F_{req2}\\ \\ F_{req1}=k\left(\frac{1}{\rho \left(q,q_0\right)}-\frac{1}{{\rho }_0}\right)\frac{{\rho }_g^n}{{\rho }^2\left(q,q_0\right)}\\ F_{req2}=\frac{n}{2}k{\left(\frac{1}{\rho \left(q,q_0\right)}-\frac{1}{{\rho }_0}\right)}^2{\rho }_g^{n-1}\end{array}\text{\hspace{1em}(14)}$$

$F_{req1}$的方向为障碍物指向车辆；$F_{req2}$的方向为车辆指向目标点。

改进的斥力场函数中加入了汽车与目标点间的距离，这样使汽车在驶向目标的过程中，受到的引力和斥力同时在一定程度上减小，且只有在汽车到达目标点时，引力和斥力才同时减小为零，即目标点成为势能值的最小点，从而使局部最优和目标不可达的问题得到解决。

4.3.2 道路边界斥力势场

如图，假设每一条车道宽度为$d$，有2条车道（故道路宽度为$2d$）。车辆宽度为$w$，故车辆在每一条车道内允许调整的横向移动范围为$d-w$。

通过建立道路边界斥力势场以限制汽车的行驶区域，并适当考虑车辆速度对斥力场的影响
$$F_{\text{req,edge }}=\{\begin{array}{ll}{\eta }_{\text{edge }}\cdot v\cdot e^{\left(\frac{-d}{2}-y\right)},& -d+w/2<y\le -d/2\text{ 车道1}\\ \frac{1}{3}{\eta }_{\text{edge }}\cdot y^2,& -d/2<y\le -w/2\text{ 车道1}\\ -\frac{1}{3}{\eta }_{\text{edge }}\cdot y^2,& w/2<y\le d/2\text{ 车道2}\\ {\eta }_{\text{edge }}\cdot v\cdot e^{\left(y-\frac{d}{2}\right)},& d/2<y\le d-w/2\text{ 车道2}\end{array}\text{\hspace{1em}(15)}$$

式中${\eta }_{\text{edge }}$是常数，$v$为车辆速度，$y$为车辆横向坐标。.

下面简单实现改进后的人工势场法。

• 初始化参数设置

  import numpy as npimport matplotlib.pyplot as pltimport copyfrom celluloid import Camera  # 保存动图时用，pip install celluloid%matplotlib qt5## 初始化车的参数d = 3.5  #道路标准宽度W = 1.8  #  汽车宽度L = 4.7  # 车长P0 = np.array([0, - d / 2, 1, 1]) #车辆起点位置，分别代表x,y,vx,vyPg = 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, 3/2, 0, 0]])P = np.vstack((Pg,Pobs))  # 将目标位置和障碍物位置合放在一起Eta_att = 5  # 引力的增益系数Eta_rep_ob = 15  # 斥力的增益系数Eta_rep_edge = 50   # 道路边界斥力的增益系数d0 = 20  # 障碍影响的最大距离num = P.shape[0] #障碍与目标总计个数len_step = 0.5 # 步长n=1Num_iter = 300  # 最大循环迭代次数

• 数据存储变量定义

  path = []  # 保存车走过的每个点的坐标delta = np.zeros((num,2)) # 保存车辆当前位置与障碍物的方向向量，方向指向车辆；以及保存车辆当前位置与目标点的方向向量，方向指向目标点dists = [] # 保存车辆当前位置与障碍物的距离以及车辆当前位置与目标点的距离unite_vec = np.zeros((num,2)) #  保存车辆当前位置与障碍物的单位方向向量，方向指向车辆；以及保存车辆当前位置与目标点的单位方向向量，方向指向目标点F_rep_ob = np.zeros((len(Pobs),2))  # 存储每一个障碍到车辆的斥力,带方向v=np.linalg.norm(P0[2:4]) # 设车辆速度为常值

• 人工势场法核心代码

  ## ***************初始化结束，开始主体循环******************Pi = P0[0:2]  # 当前车辆位置# count=0for i in range(Num_iter):    if ((Pi[0] - Pg[0]) ** 2 + (Pi[1] - Pg[1]) ** 2) ** 0.5 < 1:        break    dists=[]    path.append(Pi)    # print(count)    # count+=1    #计算车辆当前位置与障碍物的单位方向向量    for j in range(len(Pobs)):        delta[j]=Pi[0:2] - Pobs[j, 0:2]        dists.append(np.linalg.norm(delta[j]))        unite_vec[j]=delta[j]/dists[j]    #计算车辆当前位置与目标的单位方向向量    delta[len(Pobs)]=Pg[0:2] - Pi[0:2]    dists.append(np.linalg.norm(delta[len(Pobs)]))    unite_vec[len(Pobs)] = delta[len(Pobs)]/dists[len(Pobs)]        ## 计算引力    F_att = Eta_att*dists[len(Pobs)]*unite_vec[len(Pobs)]        ## 计算斥力    # 在原斥力势场函数增加目标调节因子（即车辆至目标距离），以使车辆到达目标点后斥力也为0    for j in  range(len(Pobs)):        if dists[j] >= d0:            F_rep_ob[j] = np.array([0, 0])        else:            # 障碍物的斥力1，方向由障碍物指向车辆            F_rep_ob1_abs = Eta_rep_ob * (1 / dists[j] - 1 / d0) * (dists[len(Pobs)])**n / dists[j] ** 2  # 斥力大小            F_rep_ob1 = F_rep_ob1_abs*unite_vec[j]  # 斥力向量            # 障碍物的斥力2，方向由车辆指向目标点            F_rep_ob2_abs = n/2 * Eta_rep_ob * (1 / dists[j] - 1 / d0) **2 *(dists[len(Pobs)])**(n-1) # 斥力大小            F_rep_ob2 = F_rep_ob2_abs * unite_vec[len(Pobs)]  # 斥力向量            # 改进后的障碍物合斥力计算            F_rep_ob[j] = F_rep_ob1 + F_rep_ob2            # 增加道路边界斥力势场，根据车辆当前位置，选择对应的斥力函数    if Pi[1] > - d + W / 2 and Pi[1] <= - d / 2:         F_rep_edge = [0, Eta_rep_edge * v * np.exp(-d / 2 - Pi[1])]  # 下道路边界区域斥力势场，方向指向y轴正向    elif Pi[1] > - d / 2 and Pi[1] <= - W / 2:        F_rep_edge = np.array([0, 1 / 3 * Eta_rep_edge * Pi[1] ** 2])    elif Pi[1] > W / 2 and Pi[1] <= d / 2:        F_rep_edge = np.array([0, - 1 / 3 * Eta_rep_edge * Pi[1] ** 2])    elif Pi[1] > d / 2 and Pi[1] <= d - W / 2:        F_rep_edge = np.array([0, Eta_rep_edge * v * (np.exp(Pi[1] - d / 2))])            ## 计算合力和方向    F_rep = np.sum(F_rep_ob, axis=0)+F_rep_edge        F_sum = F_att+F_rep        UnitVec_Fsum = 1 / np.linalg.norm(F_sum) * F_sum    #计算车的下一步位置    Pi = copy.deepcopy(Pi+ len_step * UnitVec_Fsum)    # Pi[0:2] = Pi[0:2] + len_step * UnitVec_Fsum    # print(Pi)path.append(Pg[0:2]) # 最后把目标点也添加进路径中path=np.array(path) # 转为numpy

• 画图

  ## 画图fig=plt.figure(1)# plt.ylim(-4, 4)plt.axis([-10,100,-15,15])camera = Camera(fig)len_line = 100# 画灰色路面图GreyZone = np.array([[- 5, - d - 0.5], [- 5, d + 0.5],            [len_line, d + 0.5], [len_line, - d - 0.5]])for i in range(len(path)):          plt.fill(GreyZone[:, 0], GreyZone[:, 1], 'gray')     plt.fill(np.array([P0[0], P0[0], P0[0] - L, P0[0] - L]), np.array([- d /          2 - W / 2, - d / 2 + W / 2, - d / 2 + W / 2, - d / 2 - W / 2]), 'b')     # 画分界线     plt.plot(np.array([- 5, len_line]), np.array([0, 0]), 'w--')     plt.plot(np.array([- 5, len_line]), np.array([d, d]), 'w')     plt.plot(np.array([- 5, len_line]), np.array([- d, - d]), 'w')     # 设置坐标轴显示范围     # plt.axis('equal')     # plt.gca().set_aspect('equal')     # 绘制路径     plt.plot(Pobs[:,0],Pobs[:,1], 'ro') #障碍物位置     plt.plot(Pg[0],Pg[1], 'gv')  # 目标位置     plt.plot(P0[0],P0[1], 'bs')  # 起点位置     # plt.cla()     plt.plot(path[0:i,0],path[0:i,1], 'k')  # 路径点     plt.pause(0.001)#      camera.snap()# animation = camera.animate()# animation.save('trajectory.gif')

效果如下：

代码仓库请移步github

由于在自动驾驶中算法实现一般使用C++，所以我也使用C++实现了相关功能，代码结构相比python代码封装得更好一些，更加清晰，这边就不再做相关代码解释了。完整代码详见另一个github仓库。

来源地址：https://blog.csdn.net/weixin_42301220/article/details/125155505