11_Artificial Potential Field_宾夕法尼亚大学机器人运动规划专项课程【学习笔记】_artificial_potential_fields的setapfconfig.h

1. Constructing Artificial Potential Field

本节将讲解如何在人工势场中引导机器人在充满障碍物的环境中运动，基本思路是在位形空间内构造一个平滑函数【smooth function】，当机器人靠近障碍物时，该函数值较高；远离障碍物时，该函数值较低；还希望在终点此函数值最小；若机器人移动到（离终点）更远的位置，函数值就会增加，如果建立出这样的函数方程，就可以用它的（下降）梯度【gradient】将机器人引导到所要求的位置。

先举个二维位形空间的简单例子，可以更容易观察到发生了什么，下图是一个典型的二维位形空间，黑色区域表示位形室间障碍物，红点表示目标位置。

构建一个势场函数，它将引导机器人到达终点，有很多实现方法，但是一个流行办法就是构造一个简单的二次函数【quadratic function】。如图所示，该函数值在终点为零，该值随着到终点的距离的增加而增加。

在该表达式中，向量x表示机器人在二维位形空间中的坐标，xg表示目标位置的坐标，只是用来缩放函数的常数.

下图显示了该函数在二维工作空间中的变化情况，函数值在目标位置为零，并随着到（目标）的距离而迅速增加。
注意，碗状是二次函数的特征。
如果机器人在下坡时遵循此函数的坡度，它会直接引导机器人朝着目标点前进，我们将此功能称为fa，以提醒我们，这是一种将机器人引导到终点的函数。

那么，除了到达目标位置以外，我们还希望机器人能够绕开环境中的障碍。为此，将构建第二个函数fr，使得机器人绕过位形空间中的障碍。
ρ(x)表示一个函数，向其输入二维位形空间中点坐标，该函数会返回最近位形空间障碍的距离值，如果位形空间障碍能明确的（用某个值、变量或参数）表示，则可以通过解析构造这样一个函数；或者，如果我们愿意将二维工作空间变成离散的网格，我们可以采用图像处理中一种叫距离变换的方法，基于这种距离变换的方法，使用距离函数来建立排斥函数fr。当靠近障碍物时。函数值变大；而远离障碍物时，函数值变小。

参数d0控制该函数对整体函数的影响，如果机器人与障碍物之间的距离大于d0，该函数不起作用；因此，仅当机器人接近障碍物时，排斥函数才会有（特定的）值。然后，当靠近障碍物的表面时，该值会迅速增加。

还有很多方法可以建立平滑的排斥函数去引导机器人远离障碍，下图是一种特别热门且方便的方法。
这是个曲面图，从此可以看出排斥函数是如何在例子中的二维空间内变化的。

有了这两个（到达终点和避开障碍）要素，存在吸引电势就会将机器人引导到目标位置，若存在排斥电势就会使机器人远离障碍物，我们可以简单的将它们加起来以构造同时具有吸引、排斥功能的函数

下面这个曲面图就是两个函数（吸引、排斥函数）的总和

有了这势场函数，那么如何使用它来指导机器人?这里我们使用微积分计算函数的梯度来操纵机器人，具体步骤如下图，根据势函数的梯度选择机器人v的速度

此处的比例关系表示了机器人的运动方向，但是机器人的速度取决于程序里速度矢量的大小，例如你可以选择(让机器人)以某个恒定的速度一直移动到距离目标足够近为止；或者，你可以根据(机器人)与目标的距离来选择切换速度。这样一来，机器人在接近目标时会减速，避免冲过头或产生其他不良影响。

总结该算法，机器人会不断评估人工势场的梯度，并朝该方向逐步移动，直到足够接近目标为止。

更具体一点，该算法或该控制策略的运行过程，与石头从势能面向下滚动，到达目标的情形差不多，下图显示的算法的运行过程：

那么下图则是以另一种方式来描述这个过程，箭头图，其中箭头表示位形空间中各个位置处的梯度场方向

红线表示通过该梯度的引导（机器人）从起点到终点的运动轨迹

这里还有一些其他的轨迹实例，都是在这个势场中得出的

该过程吸引人的地方在于，通过机器人的运动轨迹就能大概判断出人工势场函数f的梯度。
更具体地说，这意味着该机器人仅使用它的位置和本地传感器的信息就能完成程序，利用自身位置就能构建势场吸引函数的机器人，这种机器人可以测量距当前位置d0范围内的所有障碍物，也可以估算出排斥函数，通过评估机器人当前位置附近位置的势场函数，然后应用简单的有限差分法，可以估算势场梯度。

事实上，最初，这些势场方法是通过精确（计算）开发出来的，因为它们提供了一种简单、直接的方法来引导机器人前往目标点，同时(机器人)根据本地传感器数据躲避障碍物，所以有时将它称作基于传感器的路径规划，机器人位置传感器的读数将会引导它向目标前进，同时它的距离传感器（例如激光扫描仪或立体声系统）的测量信息会让它远离环境中的障碍物

2. Issues with Local Minima

人工势场法操作起来相对简单，它们可以使用本地传感器数据，以几十赫兹的刷新速度来对机器人进行实时控制。但是，这些方法的缺点是很难保证它们不会失效。理想情况下，人工势场函数只在终点处出现全局最小值【global minimum】。

实际上，除了终点之外，在其它位置也可能出现引力和斥力合成后，有局部最小值【lacal minimum】的情况。

以上图位形空间为例，当构造人工势场的时候 会得到下图所示的曲面图。其中，有两个最小值，一个在目标点处，另一个在我们建立的涵洞里。

这在实践中意味着，简单的梯度下降的控制策略，会不会在终点处结束，取决于机器人起点的位置。回到前面石头（滚落）的比喻，在这些目标点以外对应局部最小值的波谷中，机器人很可能会被卡住，由下图可以看到，机器人的轨迹终结于局部最小值而不是终点处。

而且，实际中，这类局部极小值问题很难消除。某些障碍和参数设置会导致局部最小值，但并不是全部；而在实践中，很难事先知道，这个问题(陷入局部最小点)什么时候会发生。

有一种检查这些人工势场方案的思路，挺有用的，或许能启发到你，在很多情况下，他们能够成功引导机器人到所需的位置，但它们也可能会陷入死胡同，这就往往需要回溯程序来检测这些情况，然后再切换到不同的规划策略。

感谢 B 站 up 主 Here_Kin 的翻译和分享！
【自制中英】宾夕法尼亚大学机器人专项课程二 运动规划 - Robotics:Motion and Planning