本文参考(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。
通过建立道路边界斥力势场以限制汽车的行驶区域,并适当考虑车辆速度对斥力场的影响