改进人工势场法 python 人工势场法路径规划

路径规划算法3.1 人工势场法APF

• 前言
• 电场与电势场
• 人工势场
• 人工势场的构建
• 梯度下降与局部最小问题
• 后记

前言

人工势场法APF(Artificial Potential Field)，是非场经典的寻路方法，常用于移动机器人的局部路径规划，其主要思想是通过目标的引力与障碍物的斥力共同引导机器人的移动。

电场与电势场

回顾电学中的正负电荷，同性电荷相斥，异性电荷相吸。如下图所示，黑色电荷位置固定。左边红色的正电荷一方面受到黑色正电荷的斥力，远离正电荷；另一方面受到负电荷的引力，靠近正电荷。

进一步，固定的黑色电荷在空间中形成了电场，场中的每个位置都存在电势。红色电荷受电场方向的力影响而运动，也就是向电势场的负梯度方向运动，势能下降。

人工势场

通过模仿电势场，我们制造了一个人工场，根据场景中的障碍物和目标点计算场景中每个位置的势能，然后让机器人沿着负梯度方向（势能下降的方向）前进，就能够到达目标点，如下图所示。

其中，峰值凸包表示障碍物及其附近，红色点线表示机器人移动的路径，路径的终点势能最低。

人工势场的构建

人工势场包括引力场(Attractive)和斥力场(Repulsive)两部分组成：
$U(q)=U_{attr}(q)+U_{rep}(q)$
引力场和斥力场可以自行设定，不过实际应用中通常需要遵守一定的设计准则。常用的引力场函数如下：
$U_{attr}(q)= \begin{cases} \frac{1}{2} \lambda d(q,q_{goal})^2, \quad \quad \quad \quad \quad \quad\quad d(q,q_{goal})\le d_{thres} \\ \lambda d(q,q_{goal})d_{thres} -\frac{1}{2}\lambda d_{thres}^2, \quad d(q,q_{goal})>d_{thres} \end{cases} \\ \quad \\ d(q,q_{goal})= ||q - q_{goal}||_2 \\$
其梯度可表示为：
$\nabla U_{attr} = \begin{cases} \lambda (q-q_{goal}), \quad d(q,q_{goal})\le d_{thres} \\ \frac{\lambda d_{thres}(q-q_{goal})}{d(q,q_{goal})} , \quad d(q,q_{goal})>d_{thres} \end{cases} \\ \quad \\$
在这个引力场设计中，考虑了机器人位置与目标位置的距离阈值，当前距离小于阈值时，距离越远梯度越大，机器人受到的引力越大；当前距离大于阈值时，距离越大梯度越小，机器人受到的引力越小，这就能避免由于距离过大导致引力太大导致碰撞障碍物的问题。

斥力场函数可设计为：
$U_{req}(q)= \begin{cases} \frac{1}{2} \mu (\frac {1}{D(q, q_{obst})}-\frac{1}{D_{thres}})^2, \quad D(q,q_{obst})\le D_{thres} \\ 0, \quad \quad \quad \quad \quad \quad \quad \quad \quad D(q,q_{obst}) > D_{thres} \end{cases} \\ \quad \\ D(q,q_{obst}) = ||q-q_{obst}||_2\\$
梯度为：
$\nabla U_{req} = \begin{cases} -\mu (\frac {1}{D(q, q_{obst})}-\frac{1}{D_{thres}})(\frac{1}{D_(q,q_{obst})})^2 \frac{(q-q_{obst})}{D(q,q_{obst})}, \quad D(q,q_{obst})\le D_{thres} \\ 0, \quad \quad \quad \quad \quad \quad \quad \quad \quad D(q,q_{obst}) > D_{thres} \end{cases} \\ \quad \\$
当前位置距离障碍物位置越近，梯度越大，机器人受到的斥力越大；当前位置与障碍物位置大于阈值时，机器人不再受到该障碍物的斥力。

将引力场与斥力场叠加获得人工势场：
$U(q)=U_{attr}(q)+\sum_{i=1}^n U_{req}^i(q) \\ F(q) = -\nabla U(q)$

梯度下降与局部最小问题

根据获得的人工势场，从起点开始，计算当前位置的梯度，向梯度的负方向前进，然后再次计算当前位置的梯度，再往梯度的负方向前进，直至到达终点。这就是通过梯度下降进行寻路的过程。

然而，我们知道，优化理论中的梯度下降法容易陷入局部最优解。相同地，路径规划也有容易陷入局部最优的缺点。

一个很简单的例子是，机器人的当前位置与障碍物，目标点共线，如下图所示。机器人将在直线上左右摇摆，无法到达终点。此时需要给机器人一个扰动，令其逃出局部最小。

后记

人工势场还有许多变种方法，后续有机会我还会对这些变种进行学习记录。