改进型人工势场法

1、人工势场法基本原理

人工势场法的基本原理就是将无人车假设成一个点，该点在一个虚拟力场中运动，虚拟力场是由目标点对无人车的引力场和障碍物对无人车的斥力场组成。引力场由目标点产生，斥力场是由所有的障碍物产生的和力场组成。因此，人工势场法的势场函数定义为引力场与斥力场的和，如下面的式子所示。无人车的移动方向为势场函数下降的方向。这样，无人车就可以找到一条无碰撞的路径。
$U(q)=U_{att}(q)+U_{rep}(q)$U(q)=Uatt​(q)+Urep​(q)
在上式中，q是无人车的坐标点，$U_{att}(q)$Uatt​(q)是引力场，$U_{rep}(q)$Urep​(q)是斥力场，U(q)是势场和。
势场函数的负梯度定义为人工力，人工力由引力和斥力组成。引力为引力场的负梯度，斥力为斥力场的负梯度。因此，无人车在势场中所受力为：
$F(q)=-\nabla U_{att}(q)-\nabla U_{rep}(q)=F_{att}(q)+F_{rep}(q)$F(q)=−∇Uatt​(q)−∇Urep​(q)=Fatt​(q)+Frep​(q)
上式中，F(q)为无人车所受合力，$F_{att}$Fatt​为引力，引导无人车走向目标点，$F_{rep}$Frep​为斥力，使无人车远离障碍物点。
引力场的计算公式如下：
$U_{att}=\frac {1}{2}K_{att}(q-q_g)^2$Uatt​=21​Katt​(q−qg​)2
上式中， $K_{att}$Katt​为引力增益常数， $q_g$qg​为目标点的位置。根据上式可以计算出引力的大小为：
$F_{att}=-\nabla U_{att}(q)=-K_{att}(q-q_g)$Fatt​=−∇Uatt​(q)=−Katt​(q−qg​)
$F_{att}$Fatt​的方向指向目标点，与无人车当前位置到目标点位置的距离成线性关系。$F_{att}$Fatt​可以分解为x轴方向的力与y轴方向的力：
$F_{attx}=-K_{att}(x-x_g)$Fattx​=−Katt​(x−xg​)
$F_{atty}=-K_{att}(y-y_g)$Fatty​=−Katt​(y−yg​)
斥力场的计算公式如下：
$U_{rep}=\begin {cases} 0, &amp; \text {if $ \rho_{obs}(q)\geq\rho_{0}$} \\ \frac {1}{2}K_{rep}(\frac {1}{\rho_{obs}(q)}-\frac {1}{\rho_{0}})^2, &amp; \text{if $ \rho_{obs}(q)&lt;\rho_{0}$} \end{cases}$Urep​={0,21​Krep​(ρobs​(q)1​−ρ0​1​)2,​if ρobs​(q)≥ρ0​if ρobs​(q)<ρ0​​
其中$K_{rep}$Krep​为斥力增益常数，$\rho_{obs}(q)=||q-q_{obs}||$ρobs​(q)=∣∣q−qobs​∣∣，$q_{obs}$qobs​为障碍物坐标。$\rho_0$ρ0​为最大影响距离，若障碍物与无人车距离大于$\rho_0$ρ0​，则将斥力场视为0。设置$\rho_0$ρ0​可以消除离无人车较远的障碍物对无人车轨迹的影响，$\rho_0$ρ0​的值取决于无人车速度与加速度大小。与引力计算相似，斥力的计算公式为：
$F_{rep}=-\nabla U_{rep}(q)=\begin {cases} 0, &amp; \text {if $ \rho_{obs}(q)\geq\rho_{0}$} \\ K_{rep}(\frac {1}{\rho_{obs}(q)}-\frac {1}{\rho_{0}})(\frac{1}{\rho_{obs}^2(q)})\frac{q-q_{obs}}{||q-q_{obs}||}, &amp; \text{if $ \rho_{obs}(q)&lt;\rho_{0}$} \end{cases}$Frep​=−∇Urep​(q)={0,Krep​(ρobs​(q)1​−ρ0​1​)(ρobs2​(q)1​)∣∣q−qobs​∣∣q−qobs​​,​if ρobs​(q)≥ρ0​if ρobs​(q)<ρ0​​
$F_{rep}$Frep​的方向指向障碍物点的反方向，$F_{rep}$Frep​可以分解为x轴方向的力与y轴方向的力，如下所示：
$F_{repx}=\begin {cases} 0, &amp; \text {if $ \rho_{obs}(q)\geq\rho_{0}$} \\ K_{rep}(\frac {1}{\rho_{obs}(q)}-\frac {1}{\rho_{0}})(\frac{1}{\rho_{obs}^2(q)})\frac{x-x_{obs}}{||q-q_{obs}||}, &amp; \text{if $ \rho_{obs}(q)&lt;\rho_{0}$} \end{cases}$Frepx​={0,Krep​(ρobs​(q)1​−ρ0​1​)(ρobs2​(q)1​)∣∣q−qobs​∣∣x−xobs​​,​if ρobs​(q)≥ρ0​if ρobs​(q)<ρ0​​
$F_{repy}=\begin {cases} 0, &amp; \text {if $ \rho_{obs}(q)\geq\rho_{0}$} \\ K_{rep}(\frac {1}{\rho_{obs}(q)}-\frac {1}{\rho_{0}})(\frac{1}{\rho_{obs}^2(q)})\frac{y-y_{obs}}{||q-q_{obs}||}, &amp; \text{if $ \rho_{obs}(q)&lt;\rho_{0}$} \end{cases}$Frepy​={0,Krep​(ρobs​(q)1​−ρ0​1​)(ρobs2​(q)1​)∣∣q−qobs​∣∣y−yobs​​,​if ρobs​(q)≥ρ0​if ρobs​(q)<ρ0​​
以上的斥力计算是针对单个障碍物而言，但是环境中往往有多个障碍物，所以斥力场与斥力应该是多个障碍物对无人车的斥力场和与斥力和，则：
$U(q)=U_{att}(q)+\sum_{i=1}^n U_{rep}(q)$U(q)=Uatt​(q)+i=1∑n​Urep​(q)
$F(q)=F_{att}(q)+\sum_{i=1}^n F_{rep}(q)$F(q)=Fatt​(q)+i=1∑n​Frep​(q)
其中n为障碍物个数。无人车受力情况如下图所示，其中F为无人车所受合力。

2、人工势场法缺陷

(1)容易陷入局部极小值。当无人车所受引力与斥力大小相等、方向相反时，无人车所受合力为0，此时无人车陷入了局部极小值点。在局部极小值点周围的各个位置的合力都指向局部极小值，导致无人车在局部极小值点周围振荡，无法自行走出该区域。
(2)统人工势场法引力部分与无人车和目标点位置距离成正比，当无人车距离目标点较远时，引力部分过大，可能导致无人车与障碍物相撞。
(3)目标点附近有障碍物时，无人车可能无法到达目标点。

3、人工势场法改进方法

3.1、人工势场法引力场改进

传统人工势场法引力部分与无人车和目标点位置距离成正比，当无人车距离目标点较远时，引力部分过大，可能导致无人车与障碍物相撞。针对这一问题，对引力场公式进行改进，具体如下。
引力场的大小由无人车与目标点的距离决定，当值非常大时，引力将会变得非常大，则斥力相对于引力而言就会变得非常小，这就会导致无人车与障碍物相撞。因此，有必要对引力场加以改进，公式如下：
$U_{rep}=\begin {cases} \frac{1}{2}K_{att}(q-q_g)^2, &amp; \text {if $(q-q_g)\leq d$} \\ dK_{att}(q-q_g), &amp; \text{if $ (q-q_g)&gt; d$} \end{cases}$Urep​={21​Katt​(q−qg​)2,dKatt​(q−qg​),​if (q−qg​)≤dif (q−qg​)>d​
$U_{rep}=\begin {cases} -K_{att}(q-q_g), &amp; \text {if $(q-q_g)\leq d$} \\ -dK_{att}\frac {q-q_g}{||q-q_g||}, &amp; \text{if $ (q-q_g)&gt; d$} \end{cases}$Urep​={−Katt​(q−qg​),−dKatt​∣∣q−qg​∣∣q−qg​​,​if (q−qg​)≤dif (q−qg​)>d​
其中，d为阈值，当无人车与目标点距离小于等于d时，引力与距离成正比；当无人车与目标点距离大于d时，将引力视为常数；

3.2、模拟退火法

3.2.1、模拟退火法原理

当人工势场法陷入局部极小值时，在当前点$x$x处选择一个随机点$x_1$x1​，然后根据上面计算势场函数的公式，分别计算出点$x$x与点$x_1$x1​处的势场$U(x)$U(x)与$U(x_1)$U(x1​)，如果满足$U(x_1)\leq U(x)$U(x1​)≤U(x)，则接受点$x_1$x1​作为下一个点；如果满足$U(x_1)&gt;U(x)$U(x1​)>U(x)，则计算概率$P=e^{-\frac{\Delta}{T}},\Delta=U(x_1)-U(x)$P=e−TΔ​,Δ=U(x1​)−U(x)，并以概率$P$P接受该点作为下一个点。同时$T$T以一定的方式减小,一般$T(t)=\alpha T(t-1),0.85&lt;\alpha&lt;1$T(t)=αT(t−1),0.85<α<1。重复以上步骤直到逃离局部极小值。
模拟退火法的算法步骤如下：
1、设置$x=S$x=S($S$S局部极小值或者起点)。
2、选择退火策略，设置$T=T_0$T=T0​。
3、While($T\geq T_f$T≥Tf​并且没有逃离局部极小值)
3.1、产生一个随机点 $x_1=x+\Delta x$x1​=x+Δx($\Delta x$Δx为点$x$x附近一个随机点，距离点$x$x一个步长)。
3.2、计算$U(x_1)$U(x1​)，即点$x_1$x1​处的势能。
3.3、设置$\Delta=U(x_1)-U(x)$Δ=U(x1​)−U(x)。
3.4、如果$\Delta \leq 0$Δ≤0，设置$x=x_1$x=x1​。
3.5、如果$\Delta&gt;0$Δ>0，以概率P设置$x=x_1$x=x1​，其中$P=e^{-\frac{\Delta}{T}},\Delta=U(x_1)-U(x)$P=e−TΔ​,Δ=U(x1​)−U(x)（具体为： 设置一个随机概率a，当P>a时，设置$x=x_1$x=x1​，反之，这个随机点为失败的随机点，再重新设置随机点）
3.6、如果$U(x_1)\leq U(S)$U(x1​)≤U(S)，成功逃离局部极小值。
4、如果没有逃离局部极小值，返回失败；否则，返回成功。

在实际的编程过程中，不是以$U(x_1)\leq U(S)$U(x1​)≤U(S)为标志判断是否程序逃离局部极小值，而是计算当前点与进入局部极小值时的点之间的距离，若该距离大于设定值，则认为逃离局部极小值。通常设定值为步长的5-10倍，本程序设定值为步长的5倍。当无人车陷入局部极小值时启用模拟退火法。而判断无人车是否陷入局部极小值是根据无人车是否陷入振荡或者所受合力是否为0，判断是否陷入振荡则是根据第j次与第j-2次小车的位置判断，若这两个点距离小于步长的0.1倍，则认为小车陷入振荡。
基于模拟退火法的人工势场法算法流程图如下：

3.2.2、仿真结果

在仿真实验中，将障碍物看成半径为0.5的圆，其他参数的设置如下表所示：

下图为没有改进前的人工势场法：

下图为改进后的人工势场法，其中蓝色部分为传统人工势场法规划出的路径，绿色部分为基于模拟退火法的人工势场法：

4、代码解释与链接

1、运行main.m即可，代码里面有详细的备注，大家应该都可以看得懂，且在main.m文件开头备注部分有我的QQ号，欢迎大家一起交流。
2、在main.m最下面部分有对路径的优化算法，路径优化有两个算法，分别check_lineA.m和check_lineB.m。为由于楼主最近比较忙，所以没有时间更新对优化路径算法的解释，有兴趣的小伙伴可以自己先看看。
3、没有改进前的人工势场法：https://download.****.net/download/chengchang1234/11139704
基于模拟退火法的人工势场法：https://download.****.net/download/chengchang1234/11239864