路劲规划与轨迹跟踪学习4——人工势场法

本文参考(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。

通过建立道路边界斥力势场以限制汽车的行驶区域，并适当考虑车辆速度对斥力场的影响