人工势场法(Artificial Potential Fields)规划方法

  1. 引力函数
  2. 斥力函数
  3. 梯度下降
  4. 海森矩阵
  5. 对象距离计算
  6. 局部极小值问题
  7. 参考实现

人工势场法是由Khatib1986年提出的,其思想是通过障碍物的斥力场和目标位置的引力场共同作用形成一个虚拟的人工势场,再搜索一条势函数下降的方向,通过这种方式来寻找一条无碰撞的最优路径。实现该思想的具体方法是在机器人运动的环境中首先创建一个势场,记为$U$,这个势场分为两个部分:

整个势能$U$是引力部分和斥力部分势能的叠加。移动机器人在势场合力的作用下,绕开它运动线路上的障碍物,向目标移动。

人工势场法的规划,类似一种机器人在虚拟力场下运动。如图所示,机器人在一个二维环境下运动,图中指示了机器人自身、障碍物、和目标点之间的相对位置。

这个图比较清晰地说明了人工势场法的作用,物体在初始点的一个较高“山头”上,要达到的目标点在“山脚”下,这就形成了一种势场,物体在这种势的引导下,避开障碍物,到达目标点。

人工势场包括引力场和斥力场,其中目标点对物体产生引力,引导物体朝向其运动(类似A*算法中的启发函数Heuristic Functions)。障碍物对物体产生斥力,避免物体与之发生碰撞。

物体在路径上的每一点所受的合力等于这一点所有引力与斥力的和。所以问题的关键是如何构建引力场和斥力场。

引力函数

人工势场中的函数与距离有一定关系,引力函数受移动机器人与目标点之间距离的影响,当目标点与移动机器人的距离增大时,其所受的势能增大;当目标点与机器人的距离越近时,其所受的势能减小。当机器人势能为零时,表明机器人达到目标点(另外一种情况是陷入局部最优,后面会提及)。常用的引力函数:

Conial Potenial:
$$\\ U(q) = \zeta d(q, q_{goal}) \tag{1}$$

$$\\ \nabla U(q) = \frac \zeta {d(q, q_{goal})}(q - q_{goal}) \tag{2}$$

Quadratic Potential:
$$\\ U_{att}(q) = \frac 12 \zeta d^2(q, q_{goal}) \tag{3}$$
这里$\zeta$ (zeta)是尺度因子,$d^2(q*q_{goal})$表示物体当前状态与目标的距离。(不同文章中使用参数有所不同,有使用$\eta$(zeta)的情况,但笔者认为因为表示的是距离,还是使用参数d更适合一些。)根据引力场,引力就是引力场对距离的导数,作为梯度存在:
$$\\ \begin{eqnarray} & & F_{att}(q) = \nabla U_{att}(q) \tag{4.1}\\ &=& \nabla (\frac 12 \zeta d^2(q, q_{goal})) \tag{4.2} \\ &=& \frac 12 \zeta \nabla d^2(q, q_{goal}) \tag{4.3}\\ &=& \zeta (q- q_{goal}) \tag{5} \end{eqnarray}$$

Combined Potential

$$\\ U_{att}(q)=\left\{ \begin{aligned} \frac 12 \zeta d^2(q, q_{goal}), d(q,q_{goal}) \le d_{goal}^* \\ d_{goal}^* \zeta d(q, q_{goal}) - \frac 12 \zeta (d_{goal}^*), d(q,q_{goal}) >d_{goal}^* \end{aligned} \right.\tag{6}$$ $$\\ \nabla U_{att}(q)=\left\{ \begin{aligned} \zeta (q-q_{goal}), d(q,q_{goal}) \le d_{goal}^* \\ \frac {d_{goal}^* \zeta d(q, q_{goal})}{d(q, q_{goal})}, d(q,q_{goal}) >d_{goal}^* \end{aligned} \right.\tag{7}$$

斥力函数

在一个势场中,障碍物所形成额势场会对机器人产生排斥作用,且它们之间距离越近,障碍物对机器人的排斥作用越强烈,反之则越小。这种势场的原理与电势场具有相似之处,都与其之间的距离成反比。

$$\\ U_{rep}(q)=\left\{ \begin{aligned} \frac 12 \eta (\frac i{D(q)} - \frac 1{Q^*})^2, D(q) \le Q^* \\ 0, D(q) > Q^* \end{aligned} \right.\tag{8}$$

whose gradient is :
$$\\ \nabla U_{rep}(q)=\left\{ \begin{aligned} \eta (\frac 1{Q^*} - \frac 1{D(q)}) \frac 1{D^2(q)} \nabla D(q), D(q) \le Q^*\\ 0, D(q) >Q^* \end{aligned} \right.\tag{9}$$

总势能

$$\\ U(q) = U_{att}(q) + U_{rep}(q) \tag{10}$$ $$\\ F(q) = - \nabla U(q) \tag{11}$$

梯度下降

梯度下降法(Gradient descent)是一个一阶最优化算法,通常也称为最速下降法。要使用梯度下降法找到一个函数的局部极小值,必须向函数上当前点对应梯度(或者是近似梯度)的反方向的规定步长距离点进行迭代搜索。如果相反地向梯度正方向迭代进行搜索,则会接近函数的局部极大值点;这个过程则被称为梯度上升法。

上图示例了这一过程,这里假设函数定义在平面上,并且函数图像是一个碗形。蓝色的曲线是等高线(水平集),即函数F为常数的集合构成的曲线。红色的箭头指向该点梯度的反方向。(一点处的梯度方向与通过该点的等高线垂直)。沿着梯度下降方向,将最终到达碗底,即函数F值最小的点。

一个简单获取势场底部的方法:
$$\\ \dot c(t) = - \nabla U(c(t)) \tag{12}$$

关于梯度下降的部分之后继续补充。

海森矩阵

考虑这样的一个问题,在一个一维方程中,我们怎样知道在一个最优解?

海森矩阵是一个多变量实值函数的二阶偏导数组成的方块矩阵。被应用与牛顿法解决大规模优化问题。此处,为二阶导数的m×m矩阵。

如果Hessian是非奇异的(Det(H)≠0),临界点是a唯一的点:

Gradient Descent:

– q(0)=qstart
– i = 0
– while || ∇ U(q(i)) || > ε do
• q(i+1) = q(i) - α(i)∇ U(q(i))
• i=i+1

在空间$\mathcal{C}$中构建势场,所以点代表了机器人被目标点$q_g$所吸引,并被障碍区域($\mathcal{CO}$)重新驱动。

对象距离计算


$$\\ d_i(q) = \min_{c\in \mathcal{CO_i}} d(q,c) \tag{13}$$

$$\\ \nabla d_i(q) = \frac {q-c}{d(q,c)} \tag{14}$$ $$\\ U_{rep_i}(q)=\left\{ \begin{aligned} \frac 12 \eta (\frac 1{d_i(q)} - \frac 1{Q_i^*})^2, if d_i(q) \le Q_i^*\\ 0, if d_i(q) >Q_i^* \end{aligned} \right.\tag{15}$$ $$\\ U_{rep}(q) = \sum_{i=0}^n U_{rep_i}(q) \tag{16}$$

从传感器的部署位置,计算与障碍物的距离。

可以使用网格作为计算距离的一个参考。使用离散版本的空间并从那里开始工作,使用Brushfire 算法是一种做法:

局部极小值问题

人工势场法局部极小值:当机器人运行到某一点是,机器人所受斥力等于所受引力,使得全局合力等于零,从而导致机器人停在当前位置,无法移动。如何解决?当机器人陷入局部极小点时可以暂时建立一个中间目标点,使得机器人移动到该中间目标点,逃出陷阱,然后继续向最终目标点移动。如何设置中间目标点,这篇文章提供了一种方法:徐飞.基于改进人工势场法的机器人避障及路径规划研究,供参考。

参考实现

一份供参考的基于人工势场法的代码实现 https://gist.github.com/mickeyouyou/f32aadbaf067e2ed368886b3f9bfd5c5

Reference

如果你有不同看法?
script>