文章目录

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

参考资料

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

1. 算法简介

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

人工势场法的基本思想是在障碍物周围构建障碍物斥力势场，在目标点周围构建引力势场，类似于物理学中的电磁场
被控对象在这两种势场组成的复合场中受到斥力作用和引力作用，斥力和引力的合力指引着被控对象的运动，搜索无碰的避障路径。
更直观而言，势场法是将障碍物比作是平原上具有高势能值的山峰，而目标点则是具有低势能值的低谷。

2. 算法精讲

2.1 引力势场

引力势场主要与汽车和目标点间的距离有关, 距离越大, 汽车所受的势能值就越大; 距离越小, 汽车所受的势能值则越小, 所以引力势场的函数为:
Uatt(q)=12ηρ2(q,qg)(1)\tag{1} U_{a t t}(q)=\frac{1}{2} \eta \rho^{2}\left(q, q_{g}\right) Uatt(q)=21ηρ2(q,qg)(1)
其中 η\etaη 为正比例增益系数, ρ(q,qg)\rho\left(q, q_{g}\right)ρ(q,qg) 为一个矢量, 表示汽车的位置 qqq 和目标点位置 qgq_{g}qg 之间的欧式距离 ∣q−qg∣\left|q-q_{g}\right|∣q−qg∣, 矢量方向是从汽车的位置指向目标点位置。

相应的引力 Fatt (q)F_{\text {att }}(q)Fatt (q) 为引力场的负梯度，代表引力势场函数Uatt(q)U_{att}(q)Uatt(q)的变化最快方向。
Fatt(q)=−∇Uatt(q)=−ηρ(q,qg)(2)\tag{2} F_{a t t}(q)=-\nabla U_{a t t}(q)=-\eta \rho\left(q, q_{g}\right) Fatt(q)=−∇Uatt(q)=−ηρ(q,qg)(2)

2.2 斥力势场

决定障碍物斥力势场的因素是汽车与障碍物间的距离, 当汽车未进入障碍物的影响范围时, 其受到的势能值为零; 在汽车进入障碍物的影响范围后, 两者之间的距离越大, 汽车受到的势能值就越小, 距离越小, 汽车受到的势能值就越大。
斥力势场的势场函数为:
Ureq(q)={12k(1ρ(q,q0)−1ρ0)2,0≤ρ(q,q0)≤ρ00,ρ(q,q0)≥ρ0(3)\tag{3} U_{r e q}(q)=\left\{\begin{array}{lc} \frac{1}{2} k\left(\frac{1}{\rho\left(q, q_{0}\right)}-\frac{1}{\rho_{0}}\right)^{2} ,& 0 \leq \rho\left(q, q_{0}\right) \leq \rho_{0} \\ 0 ,& \rho\left(q, q_{0}\right) \geq \rho_{0} \end{array}\right. Ureq(q)={21k(ρ(q,q0)1−ρ01)2,0,0≤ρ(q,q0)≤ρ0ρ(q,q0)≥ρ0(3)

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

由公式(3)可知，斥力势场不同于引力势场，智能汽车不总是受到障碍对它的斥力作用。当汽车与障碍物之间的相对距离超过ρ0\rho_{0}ρ0时，就判定此障碍对汽车不再有影响作用。当汽车进入障碍物的影响范围之后,即汽车与障碍的相对距离小于ρ0\rho_{0}ρ0时，汽车开始受到障碍物的斥力影响。汽车与障碍物的相对距离越小，斥力影响越大,自身势能升高。汽车与障碍物的相对距离越大，斥力影响越小，自身势能降低。
相应的斥力为斥力势场的负梯度作用力:
Freq(q)={k(1ρ(q,q0)−1ρ0)1ρ2(q,q0),0≤ρ(q,q0)≤ρ00,ρ(q,q0)≥ρ0(4)\tag{4} F_{r e q}(q)= \begin{cases}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 \leq \rho\left(q, q_{0}\right) \leq \rho_{0} \\ 0 ,& \rho\left(q, q_{0}\right) \geq \rho_{0}\end{cases} Freq(q)={k(ρ(q,q0)1−ρ01)ρ2(q,q0)1,0,0≤ρ(q,q0)≤ρ0ρ(q,q0)≥ρ0(4)

2.3 合力势场

根据上述定义的引力场函数和斥力场函数，可以得到整个运行空间的复合场，机器人的合力势场大小为机器人所受的斥力势场和引力势场之和，故合力势场总函数为：
U(q)=Uatt(q)+Ureq(q)(5)\tag{5} U(q)=U_{att}(q)+U_{req}(q) U(q)=Uatt(q)+Ureq(q)(5)
所受合力为
F(q)=−∇U(q)=Fatt(q)+Freq(q)(6)\tag{6} F(q) =-\nabla U(q)= F_{a t t}(q)+F_{r e q}(q) F(q)=−∇U(q)=Fatt(q)+Freq(q)(6)

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

3. 引力斥力推导计算

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

根据公式(1)，引力势场函数为
Uatt(q)=12ηρ2(q,qg)⇒Uatt(x,y)=12η[(x−xg)2+(y−yg)2](7)\tag{7} U_{a t t}(q)=\frac{1}{2} \eta \rho^{2}\left(q, q_{g}\right) \Rightarrow U_{a t t}(x, y)=\frac{1}{2} \eta\left[\left(x-x_{g}\right)^{2}+\left(y-y_{g}\right)^{2}\right] Uatt(q)=21ηρ2(q,qg)⇒Uatt(x,y)=21η[(x−xg)2+(y−yg)2](7)
故引力势场的负梯度有
−grad⁡att(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⃗]⇒引力大小=η(x−xg)2+(yg−y)2=ηρ(q,qg)(8)\tag{8} \begin{aligned} -\operatorname{grad}_{a t t}(x, y)&=-\nabla U_{a t t}(x, y) \\ &=-U_{a t t, x}^{\prime}(x, y) \vec{i}-U_{a t t, 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{aligned} −gradatt(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]=η(x−xg)2+(yg−y)2=ηρ(q,qg)(8)

同理，斥力势场函数为
Ureq(q)=12k(1ρ(q,q0)−1ρ0)2⇒Ureq(x,y)=12k[1(x−x0)2+(y−y0)2−1ρ0]2(9)\tag{9} \begin{aligned} U_{r e q}(q)=\frac{1}{2} k\left(\frac{1}{\rho\left(q, q_{0}\right)}-\frac{1}{\rho_{0}}\right)^{2} \Rightarrow U_{r e q}(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{aligned} Ureq(q)=21k(ρ(q,q0)1−ρ01)2⇒Ureq(x,y)=21k⎣⎡(x−x0)2+(y−y0)21−ρ01⎦⎤2(9)
斥力势场的负梯度为
−∇Ureq(x,y)=−Ureq,x′(x,y)i⃗−Ureq,y′(x,y)j⃗(10)\tag{10} -\nabla U_{r e q}(x, y)=-U_{r e q, x}^{\prime}(x, y) \vec{i}-U_{r e q, y}^{\prime}(x, y) \vec{j} −∇Ureq(x,y)=−Ureq,x′(x,y)i−Ureq,y′(x,y)j(10)
将公式(10)各项分别展开：
−Ureq,x′(x,y)i⃗=−k[1(x−x0)2+(y−y0)2−1ρ0][1(x−x0)2+(y−y0)2−1ρ0]′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[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(1ρ(q,q0)−1ρ0)⋅1ρ2(q,q0)⋅1ρ(q,q0)⋅(x−x0)i⃗(11-1)\tag{11-1} \begin{aligned} -U_{r e q, 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]' \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]'\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{aligned} −Ureq,x′(x,y)i=−k⎣⎡(x−x0)2+(y−y0)21−ρ01⎦⎤⎣⎡(x−x0)2+(y−y0)21−ρ01⎦⎤′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⎣⎡(x−x0)2+(y−y0)21−ρ01⎦⎤{(x−x0)2+(y−y0)21[(x−x0)2+(y−y0)2]−21(x−x0)}i=k(ρ(q,q0)1−ρ01)⋅ρ2(q,q0)1⋅ρ(q,q0)1⋅(x−x0)i(11-1)
−Ureq,y′(x,y)i⃗=−k[1(x−x0)2+(y−y0)2−1ρ0][1(x−x0)2+(y−y0)2−1ρ0]′j⃗=−k[1(x−x0)2+(y−y0)2−1ρ0]{−12[(x−x0)2+(y−y0)2]−32⋅[(x−x0)2+(y−y0)2]′}j⃗=k[1(x−x0)2+(y−y0)2−1ρ0]{1(x−x0)2+(y−y0)2[(x−x0)2+(y−y0)2]−12(y−y0)}j⃗=k(1ρ(q,q0)−1ρ0)⋅1ρ2(q,q0)⋅1ρ(q,q0)⋅(y−y0)j⃗(11-2)\tag{11-2} \begin{aligned} -U_{r e q, 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]' \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]'\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{aligned} −Ureq,y′(x,y)i=−k⎣⎡(x−x0)2+(y−y0)21−ρ01⎦⎤⎣⎡(x−x0)2+(y−y0)21−ρ01⎦⎤′j=−k⎣⎡(x−x0)2+(y−y0)21−ρ01⎦⎤{−21[(x−x0)2+(y−y0)2]−23⋅[(x−x0)2+(y−y0)2]′}j=k⎣⎡(x−x0)2+(y−y0)21−ρ01⎦⎤{(x−x0)2+(y−y0)21[(x−x0)2+(y−y0)2]−21(y−y0)}j=k(ρ(q,q0)1−ρ01)⋅ρ2(q,q0)1⋅ρ(q,q0)1⋅(y−y0)j(11-2)

化简后得斥力大小为
−∇Ureq(x,y)=k(1ρ(q,q0)−1ρ0)⋅1ρ2(q,q0)(12)\tag{12} \begin{aligned} -\nabla U_{r e q}(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{aligned} −∇Ureq(x,y)=k(ρ(q,q0)1−ρ01)⋅ρ2(q,q0)1(12)

4. 算法缺陷与改进

4.1 目标不可达的问题

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

4.2 陷入局部最优的问题

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

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

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

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

4.3 解决方案

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

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

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

改进后的斥力场函数为:
Ureq (q)={12k(1ρ(q,q0)−1ρ0)2ρgn,0≤ρ(q,q0)≤ρ00,ρ(q,q0)>ρ0(13)\tag{13} U_{\text {req }}(q)= \begin{cases}\frac{1}{2} k\left(\frac{1}{\rho\left(q, q_{0}\right)}-\frac{1}{\rho_{0}}\right)^{2} \rho_{g}^{n}, & 0 \leq \rho\left(q, q_{0}\right) \leq \rho_{0} \\ 0, & \rho\left(q, q_{0}\right) > \rho_{0}\end{cases} Ureq (q)=⎩⎨⎧21k(ρ(q,q0)1−ρ01)2ρgn,0,0≤ρ(q,q0)≤ρ0ρ(q,q0)>ρ0(13)
ρgn\rho_{g}^{n}ρgn 为汽车与目标点的距离，式中nnn为可选的正常数。

{Freq=Freq1+Freq2Freq1=k(1ρ(q,q0)−1ρ0)ρgnρ2(q,q0)Freq2=n2k(1ρ(q,q0)−1ρ0)2ρgn−1(14)\tag{14} \left\{\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}\right. ⎩⎨⎧Freq=Freq1+Freq2Freq1=k(ρ(q,q0)1−ρ01)ρ2(q,q0)ρgnFreq2=2nk(ρ(q,q0)1−ρ01)2ρgn−1(14)

Freq1F_{req1 }Freq1的方向为障碍物指向车辆；Freq2F_{req2}Freq2的方向为车辆指向目标点。

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

4.3.2 道路边界斥力势场

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

通过建立道路边界斥力势场以限制汽车的行驶区域，并适当考虑车辆速度对斥力场的影响
Freq,edge ={ηedge ⋅v⋅e(−d2−y),−d+w/2<y≤−d/2车道113ηedge ⋅y2,−d/2<y≤−w/2车道1−13ηedge ⋅y2,w/2<y≤d/2车道2ηedge ⋅v⋅e(y−d2),d/2<y≤d−w/2车道2(15)\tag{15} F_{\text {req,edge }}= \begin{cases}\left.\eta_{\text {edge }} \cdot v \cdot e^{\left(\frac{-d}{2}-y\right.}\right), & -d+w / 2<y \leq -d / 2 \text{ \quad \quad车道1} \\ \frac{1}{3} \eta_{\text {edge }} \cdot y^{2}, & -d / 2<y\leq -w / 2 \text{ \quad \quad车道1} \\ -\frac{1}{3} \eta_{\text {edge }} \cdot y^{2}, & w / 2<y\leq d / 2 \text{ \quad \quad车道2} \\ \eta_{\text {edge }} \cdot v \cdot e^{\left(y-\frac{d}{2}\right)}, & d / 2<y\leq d-w / 2\text{ \quad \quad车道2}\end{cases} Freq,edge =⎩⎨⎧ηedge ⋅v⋅e(2−d−y),31ηedge ⋅y2,−31ηedge ⋅y2,ηedge ⋅v⋅e(y−2d),−d+w/2<y≤−d/2 车道1−d/2<y≤−w/2 车道1w/2<y≤d/2 车道2d/2<y≤d−w/2 车道2(15)

式中ηedge \eta_{\text {edge }}ηedge 是常数，vvv为车辆速度，yyy为车辆横向坐标。.

5. python实现

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

初始化参数设置

import numpy as np
import matplotlib.pyplot as plt
import copy
from 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=0
for i in range(Num_iter):if ((Pi[0] - Pg[0]) ** 2 + (Pi[1] - Pg[1]) ** 2) ** 0.5 < 1:breakdists=[]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)]## 计算斥力# 在原斥力势场函数增加目标调节因子（即车辆至目标距离），以使车辆到达目标点后斥力也为0for 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_edgeF_sum = F_att+F_repUnitVec_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

【路径规划】局部路径规划算法——人工势场法（含python实现）相关推荐

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

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