本文参考(85条消息) 【路径规划】局部路径规划算法——人工势场法（含python实现 | c++实现）_CHH3213的博客-CSDN博客_人工势场法路径规划
路径规划与轨迹跟踪系列算法学习_第6讲_人工势场法_哔哩哔哩_bilibili
1. 算法简介
人工势场法是一种原理比较简单的移动机器人路径规划算法，它将目标点位置视做势能最低点，将地图中的障碍物视为势能高点，计算整个已知地图的势场图，然后理想情况下，机器人就像一个滚落的小球，自动避开各个障碍物滚向目标点。
2. 算法精讲
2.1 引力势场
引力势场主要与汽车和目标点间的距离有关, 距离越大, 汽车所受的势能值就越大; 距离越小, 汽车所受的势能值则越小, 所以引力势场的函数为:
2.2 斥力势场
-
决定障碍物斥力势场的因素是汽车与障碍物间的距离, 当汽车未进入障碍物的影响范围时, 其受到的势能值为零; 在汽车进入障碍物的影响范围后, 两者之间的距离越大, 汽车受到的势能值就越小, 距离越小, 汽车受到的势能值就越大。
-
斥力势场的势场函数为:
相应的斥力为斥力势场的负梯度作用力:
2.3 合力势场
根据上述定义的引力场函数和斥力场函数，可以得到整个运行空间的复合场，机器人的合力势场大小为机器人所受的斥力势场和引力势场之和，故合力势场总函数为：
所受合力为：
合力的方向决定汽车的行驶朝向，合力的大小决定汽车的行驶加速度。
3. 引力斥力推导计算
化简后得斥力大小为
4. 算法缺陷与改进
4.1 目标不可达的问题
由于障碍物与目标点距离太近，当汽车到达目标点时，根据势场函数可知，目标点的引力降为零，而障碍物的斥力不为零，此时汽车虽到达目标点， 但在斥力场的作用下不能停下来，从而导致目标不可达的问题。
4.2 陷入局部最优的问题
车辆在某个位置时，无法向前搜索避障路径。
出现局部最优主要有两种情况：
- 如下图，汽车受到的障碍物的斥力和目标点的引力之间的夹角近似为180°，几乎在同一条直线上,就会出现汽车在障碍物前陷入局部最优的问题。
- 如果若干个障碍物的合斥力与目标点的引力大小相等、方向相反，则合力为0，智能汽车自身判断到达势能极小值的位置，但没有到达期望的目标点位置。由于合力为零，汽车就会陷在势能极小的位置，无法继续前进和转向，以致无法到达期望的目标点。
4.3 解决方案
4.3.1 改进障碍物斥力势场函数
通过改进障碍物斥力势场函数来解决局部最优和目标不可达的问题；在传统人工势场法的障碍物斥力场模型中加入调节因子
，使汽车只有到达目标点时, 斥力和引力才同时减小到零, 从而使局部最优和目标不可达的问题得到解决。
改进后的斥力场函数为:
为汽车与目标点的距离，式中n为可选的正常数。
的方向为障碍物指向车辆；
的方向为车辆指向目标点。
改进的斥力场函数中加入了汽车与目标点间的距离，这样使汽车在驶向目标的过程中，受到的引力和斥力同时在一定程度上减小，且只有在汽车到达目标点时，引力和斥力才同时减小为零，即目标点成为势能值的最小点，从而使局部最优和目标不可达的问题得到解决。
4.3.2 道路边界斥力势场
如图，假设每一条车道宽度为d，有2条车道（故道路宽度为2d）。车辆宽度为w，故车辆在每一条车道内允许调整的横向移动范围为d−w。
通过建立道路边界斥力势场以限制汽车的行驶区域，并适当考虑车辆速度对斥力场的影响
