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

这篇具有很好参考价值的文章主要介绍了【路径规划】局部路径规划算法——人工势场法(含python实现 | c++实现)。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

参考资料

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

1. 算法简介

  • 1986 年 Khatib 首先提出人工势场法,并将其应用在机器人避障领域, 而现代汽车可以看作是一个高速行驶的机器人,所以该方法也可应用于汽车的避障路径规划领域。
【路径规划】局部路径规划算法——人工势场法(含python实现 | c++实现)
  • 人工势场法的基本思想是在障碍物周围构建障碍物斥力势场,在目标点周围构建引力势场,类似于物理学中的电磁场

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

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

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

2. 算法精讲

2.1 引力势场

引力势场主要与汽车和目标点间的距离有关, 距离越大, 汽车所受的势能值就越大; 距离越小, 汽车所受的势能值则越小, 所以引力势场的函数为:
U a t t ( q ) = 1 2 η ρ 2 ( q , q g ) (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 , q g ) \rho\left(q, q_{g}\right) ρ(q,qg) 为一个矢量, 表示汽车的位置 q q q 和目标点位置 q g q_{g} qg 之间的欧式距离 ∣ q − q g ∣ \left|q-q_{g}\right| qqg, 矢量方向是从汽车的位置指向目标点位置。

相应的引力 F att  ( q ) F_{\text {att }}(q) Fatt (q) 为引力场的负梯度,代表引力势场函数 U a t t ( q ) U_{att}(q) Uatt(q)的变化最快方向。
F a t t ( q ) = − ∇ U a t t ( q ) = − η ρ ( q , q g ) (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 斥力势场

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

  • 斥力势场的势场函数为:
    U r e q ( q ) = { 1 2 k ( 1 ρ ( q , q 0 ) − 1 ρ 0 ) 2 , 0 ≤ ρ ( q , q 0 ) ≤ ρ 0 0 , ρ ( q , q 0 ) ≥ ρ 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)

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

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

  • 相应的斥力为斥力势场的负梯度作用力:
    F r e q ( q ) = { k ( 1 ρ ( q , q 0 ) − 1 ρ 0 ) 1 ρ 2 ( q , q 0 ) , 0 ≤ ρ ( q , q 0 ) ≤ ρ 0 0 , ρ ( q , q 0 ) ≥ ρ 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 ) = U a t t ( q ) + U r e q ( q ) (5) \tag{5} U(q)=U_{att}(q)+U_{req}(q) U(q)=Uatt(q)+Ureq(q)(5)
所受合力为
F ( q ) = − ∇ U ( q ) = F a t t ( q ) + F r e q ( 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),障碍物位置为 ( x g , y g ) (x_g, y_g) (xg,yg)

根据公式(1),引力势场函数为
U a t t ( q ) = 1 2 η ρ 2 ( q , q g ) ⇒ U a t t ( x , y ) = 1 2 η [ ( x − x g ) 2 + ( y − y g ) 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η[(xxg)2+(yyg)2](7)
故引力势场的负梯度有
− grad ⁡ 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 ⃗ = − η ( x − x g ) i ⃗ − η ( y − y g ) j ⃗ = η [ ( x g − x ) i ⃗ + ( y g − y ) j ⃗ ] ⇒ 引 力 大 小 = η ( x − x g ) 2 + ( y g − y ) 2 = η ρ ( q , q g ) (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 =η(xxg)i η(yyg)j =η[(xgx)i +(ygy)j ]=η(xxg)2+(ygy)2 =ηρ(q,qg)(8)

同理,斥力势场函数为
U r e q ( q ) = 1 2 k ( 1 ρ ( q , q 0 ) − 1 ρ 0 ) 2 ⇒ U r e q ( x , y ) = 1 2 k [ 1 ( x − x 0 ) 2 + ( y − y 0 ) 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)2Ureq(x,y)=21k(xx0)2+(yy0)2 1ρ012(9)
斥力势场的负梯度为
− ∇ U r e q ( x , y ) = − U r e q , x ′ ( x , y ) i ⃗ − U r e q , 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)各项分别展开:
− U r e q , 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 ⃗ = − 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 [ 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 ( 1 ρ ( q , q 0 ) − 1 ρ 0 ) ⋅ 1 ρ 2 ( q , q 0 ) ⋅ 1 ρ ( q , q 0 ) ⋅ ( x − x 0 ) 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(xx0)2+(yy0)2 1ρ01(xx0)2+(yy0)2 1ρ01i =k(xx0)2+(yy0)2 1ρ01{21[(xx0)2+(yy0)2]23[(xx0)2+(yy0)2]}i =k(xx0)2+(yy0)2 1ρ01{(xx0)2+(yy0)21[(xx0)2+(yy0)2]21(xx0)}i =k(ρ(q,q0)1ρ01)ρ2(q,q0)1ρ(q,q0)1(xx0)i (11-1)
− U r e q , y ′ ( 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 ] ′ j ⃗ = − 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 ] ′ } j ⃗ = 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 ( y − y 0 ) } j ⃗ = k ( 1 ρ ( q , q 0 ) − 1 ρ 0 ) ⋅ 1 ρ 2 ( q , q 0 ) ⋅ 1 ρ ( q , q 0 ) ⋅ ( y − y 0 ) 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(xx0)2+(yy0)2 1ρ01(xx0)2+(yy0)2 1ρ01j =k(xx0)2+(yy0)2 1ρ01{21[(xx0)2+(yy0)2]23[(xx0)2+(yy0)2]}j =k(xx0)2+(yy0)2 1ρ01{(xx0)2+(yy0)21[(xx0)2+(yy0)2]21(yy0)}j =k(ρ(q,q0)1ρ01)ρ2(q,q0)1ρ(q,q0)1(yy0)j (11-2)

化简后得斥力大小为
− ∇ U r e q ( x , y ) = k ( 1 ρ ( q , q 0 ) − 1 ρ 0 ) ⋅ 1 ρ 2 ( q , q 0 ) (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 目标不可达的问题

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

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

4.2 陷入局部最优的问题

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

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

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

  • 如下图,汽车受到的障碍物的斥力和目标点的引力之间的夹角近似为180°,几乎在同一条直线上,就会出现汽车在障碍物前陷入局部最优的问题。
【路径规划】局部路径规划算法——人工势场法(含python实现 | c++实现)
  • 如果若干个障碍物的合斥力与目标点的引力大小相等、方向相反,则合力为0,智能汽车自身判断到达势能极小值的位置,但没有到达期望的目标点位置。由于合力为零,汽车就会陷在势能极小的位置,无法继续前进和转向,以致无法到达期望的目标点。
【路径规划】局部路径规划算法——人工势场法(含python实现 | c++实现)

4.3 解决方案

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

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

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

改进后的斥力场函数为:
U req  ( q ) = { 1 2 k ( 1 ρ ( q , q 0 ) − 1 ρ 0 ) 2 ρ g n , 0 ≤ ρ ( q , q 0 ) ≤ ρ 0 0 , ρ ( q , q 0 ) > ρ 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)
ρ g n \rho_{g}^{n} ρgn 为汽车与目标点的距离,式中 n n n为可选的正常数。

{ F r e q = F r e q 1 + F r e q 2 F r e q 1 = k ( 1 ρ ( q , q 0 ) − 1 ρ 0 ) ρ g n ρ 2 ( q , q 0 ) F r e q 2 = n 2 k ( 1 ρ ( q , q 0 ) − 1 ρ 0 ) 2 ρ g n − 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ρgn1(14)

F r e q 1 F_{req1 } Freq1的方向为障碍物指向车辆; F r e q 2 F_{req2} Freq2的方向为车辆指向目标点。

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

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

4.3.2 道路边界斥力势场

如图,假设每一条车道宽度为 d d d,有2条车道(故道路宽度为 2 d 2d 2d)。车辆宽度为 w w w,故车辆在每一条车道内允许调整的横向移动范围为 d − w d-w dw

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

通过建立道路边界斥力势场以限制汽车的行驶区域,并适当考虑车辆速度对斥力场的影响
F req,edge  = { η edge  ⋅ v ⋅ e ( − d 2 − y ) , − d + w / 2 < y ≤ − d / 2   车道1 1 3 η edge  ⋅ y 2 , − d / 2 < y ≤ − w / 2   车道1 − 1 3 η edge  ⋅ y 2 , w / 2 < y ≤ d / 2   车道2 η edge  ⋅ v ⋅ e ( y − d 2 ) , d / 2 < y ≤ d − w / 2   车道2 (15) \tag{15} F_{\text {req,edge }}= \begin{cases}\eta_{\text {edge }} \cdot v \cdot e^{\left(\frac{-d}{2}-y\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 ve(2dy),31ηedge y2,31ηedge y2,ηedge ve(y2d),d+w/2<yd/2 车道1d/2<yw/2 车道1w/2<yd/2 车道2d/2<ydw/2 车道2(15)

式中 η edge  \eta_{\text {edge }} ηedge 是常数, v v v为车辆速度, y y y为车辆横向坐标。.

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,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, 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=1
    
    Num_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:
            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')
    
    

效果如下:

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

代码仓库请移步github

6. c++实现

由于在自动驾驶中算法实现一般使用C++,所以我也使用C++实现了相关功能,代码结构相比python代码封装得更好一些,更加清晰,这边就不再做相关代码解释了。完整代码详见另一个github仓库。文章来源地址https://www.toymoban.com/news/detail-407545.html

到了这里,关于【路径规划】局部路径规划算法——人工势场法(含python实现 | c++实现)的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 基于人工势场算法的机器人避障路径规划(Matlab实现)

    路径规划是机器人导航领域的关键任务之一,它涉及到如何在复杂环境中找到一条安全、高效的路径,使机器人能够避开障碍物并到达目标位置。人工势场算法是一种常用的路径规划方法,它模拟了物体之间的相互作用力,并通过计算机模拟来实现机器人的避障行为。本文将

    2024年02月07日
    浏览(51)
  • 【路径规划】人工势场算法机器人避障路径规划【含Matlab源码 2731期】

    获取代码方式1: 完整代码已上传我的资源:【路径规划】基于matlab人工势场算法机器人避障路径规划【含Matlab源码 2731期】 获取代码方式2: 付费专栏Matlab路径规划(初级版) 备注: 点击上面蓝色字体付费专栏Matlab路径规划(初级版),扫描上面二维码,付费29.9元订阅海神

    2024年02月02日
    浏览(41)
  • 【APF三维路径规划】人工势场算法无人机三维路径规划【含Matlab源码 168期】

    获取代码方式1: 完整代码已上传我的资源:【三维路径规划】基于matlab人工势场算法无人机三维路径规划【含Matlab源码 168期】 获取代码方式2: 付费专栏Matlab路径规划(初级版) 备注: 点击上面蓝色字体付费专栏Matlab路径规划(初级版),扫描上面二维码,付费29.9元订阅

    2024年01月18日
    浏览(46)
  • 多机器人协同避障路径规划 — 一致性算法和人工势场算法

    多机器人协同避障路径规划 — 一致性算法和人工势场算法 现代机器人技术的发展给人们的生产和生活带来了巨大的影响。在复杂环境下,多个机器人通过协同控制可以完成更复杂的任务。但是,在多机器人协同运动时,避免碰撞及实现合理路径规划是一个重要的问题。因此

    2024年02月10日
    浏览(45)
  • 基于人工势场算法的机器人避障路径规划(MATLAB代码)

    在机器人的导航和路径规划领域,人工势场算法是一种常用的方法。该算法通过将机器人周围的环境建模为势场,并根据势场的梯度信息引导机器人避开障碍物,从而规划出安全的路径。本文将介绍基于人工势场算法的机器人避障路径规划的MATLAB代码,并解释其实现原理。

    2024年02月07日
    浏览(58)
  • 【路径规划】局部路径规划算法——贝塞尔曲线法(含python实现 | c++实现)

    路径规划与轨迹跟踪系列算法 曲线杂谈(二):Bezier曲线的特殊性质 贝塞尔曲线的特性总结 贝塞尔曲线于1962年由法国工程师皮埃尔·贝塞尔( Pierre Bézier)发表,他运用贝塞尔曲线来为汽车的主体进行设计。 贝塞尔曲线是应用于二维图形应用程序的数学曲线,由一组称为

    2024年02月14日
    浏览(98)
  • 【路径规划-二维路径规划】基于人工势场结合快速搜索树APF+RRT实现机器人避障规划附matlab代码

    在机器人路径规划领域,人工势场方法(Artificial Potential Field,APF)和快速搜索树(Rapidly-exploring Random Tree,RRT)是两种常用的算法,用于实现机器人避障规划。这两种方法可以结合使用,以在复杂环境中生成安全有效的路径。 人工势场方法是一种基于力的路径规划方法,通

    2024年01月19日
    浏览(45)
  • 轨迹规划 | 图解分析人工势场算法APF(附ROS C++/Python/Matlab仿真)

    🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备!详细介绍全局规划(图搜索、采样法、智能算法等);局部规划(DWA、APF等);曲线优化(贝塞尔曲线、B样条曲线等)。 🚀详情:图解自动驾驶中的运动规划(Motion Planning),附几十种规划算法 传统的避障方法通常基

    2024年02月08日
    浏览(44)
  • 局部路径规划 DWA 算法完全解析(理论推导+代码实现,包你看懂!)

    转载请注明出处,谢谢 前面学习的全局路径规划方法,Dijkstra、Best-First-Search、A*算法都属于 状态采样 (State Sampling)方法,而 DWA 局部路径规划则属于典型的 动作采样 (action sampling)方法 DWA 算法(Dynamic Window Approach)的原理主要是以一定的 分辨率 在 速度空间 (v, w) 中 采样

    2024年02月07日
    浏览(51)
  • 【路径规划】人工势场结合快速搜索树APF+RRT机器人避障规划【含Matlab源码 3778期】

    ✅博主简介:热爱科研的Matlab仿真开发者,修心和技术同步精进,Matlab项目合作可私信。 🍎个人主页:海神之光 🏆代码获取方式: 海神之光Matlab王者学习之路—代码获取方式 ⛳️座右铭:行百里者,半于九十。 更多Matlab仿真内容点击👇 Matlab图像处理(进阶版) 路径规划

    2024年01月20日
    浏览(49)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包