高动态环境下基于随机可及集的Path-Guided APF算法的Motion Planning

这是一篇ICRA2015的文章,也是我第二个看到进行精读的顶会文章(第一个是FRL).嘿嘿,倍儿有面子.

摘要

高动态环境下如何motion planning是一个挑战，因为需要持续的估计和验证规划的时效性。但是由于广泛的应用，一个能够在动态移动物体环境下安全规划路径的算法是十分必要的。在这篇文章中我们提出了一个在这种静态和动态环境下计算上十分高效的规划解决方案。Path-Guided APF-SR通过首先应用一个基于采样的技术来identify一个在静态环境下有效的，无碰撞的路径。然后，利用路径作为一个attractive的中间目标，利用人工势场规划方法安全通过移动障碍物。为了提高人工势场的安全性，利用随机可达集stochastic reachable sets对移动障碍物周围的排斥性势场进行了计算，该方法在高动态环境中显著提高了规划的成功率。事实证明，我们的算法比起其他的算法有更高的规划成功率，在300个随机移动的障碍物环境下。不仅如此，在以前开发的方法，失败的环境下，我们的方法也是可以成功规划的。

引言

路径规划包括为机器人从起点到终点找到一个有效的无碰撞的道路，由于动态障碍物的存在，路径规划需要不断的调整来躲避动态障碍物，坐在飞行环境下以及自动驾驶环境下，可能是十分致命的。在这些动态的环境下，如何高效的计算一条轨迹以高成功率躲避静态以及动态障碍物是十分重要的。
基于抽样的方法已经在静态障碍物的复杂规划问题中成功地识别了有效路径[1], [2], [3].我们扩展了这项工作以适应动态障碍[4]，并在最近的研究中表明，在高度动态环境[5]中，人工势场(APF)方法优于这些基于采样的方法。APF的工作原理是遵循障碍物周围的排斥力梯度和目标附近的吸引力梯度。然而，与所有APF方法一样，这种方法也存在局部极小问题。虽然有一些扩展算法解决了这个已知的限制[6]、[7]，但是它们要么在计算上很昂贵，要么在某些环境中执行得很糟糕。研究人员已经开始去探索如何基于采样的方法和APF在群体机器人[8]与群体行为模拟[9]来解决这种高维组合空间。

图1：狭窄的实验环境，灰色的条状物体是静态障碍物，黑色的和灰色的方框是300个移动的障碍物，黑色方框以线性移动，灰色方框以弧形移动，S代表的是起点，G代表的是终点，红色的线条是我们的算法，短划线是PRM一开始得到的路线。作为比较，蓝色的线是APF-SR得到的（陷入局部最优，在一开始就走向墙壁自闭了），而绿色的线是SR-Query。
这篇论文中我们提出了一种由路径引导的APF-SR算法，这是一种计算运行时间成本较低的方法，可以灵活地处理任何生成路径的采样方法，并且在有数百个移动障碍物的拥挤环境中成功地避免了碰撞。我们关注的问题是在高度动态和复杂的环境下，单个机器人的安全导航问题，包括静态和动态的随机障碍(不可控的)。
这项工作的主要贡献是:1)通过抽样方法生成路径（其实就是APM算法嘛）与通过APF方法的动态随机避障相结合，APF方法由随机可达集（这个是个啥东西？）加权。2)在动态随机环境下可以有效的导航。3）提出来路径导向的APF算法。4）在多种环境以及多种采样方法下测试了我们的方法。
我们的方法是首先应用一种基于采样的技术来识别一条相对于静态障碍物没有碰撞的路径。在此基础上，利用人工势场规划方法，将上面识别的运动轨迹作为有吸引力的中间目标，安全通过运动障碍物。为了提高人工势场的安全性，我们在预先计算的随机可达集pre-computed stochastic reachable (SR) sets的基础上，为每个移动障碍物建立了一个排斥势场。These sets provide a pairwise assurance of a likelihood of safety, for known relative robot-obstacle stochastic dynamics, at exponential computational cost in the number of relative degrees of freedom. （这句话的意思可能是，这些SR set为已知的移动障碍物，比如人或者机器人，提供了双向的保护，随着障碍物自由度的上升，这些set的的计算复杂度计以指数上升）。APF-SR组合的算法消除了这种运行时的指数级计算小号，并且在高动态环境下保持高规划成功率[5]。（这个[5]看起来很有必要看一看哦）
我们将路径引导的APF-SR与我们之前开发的两种算法进行了比较，这两种算法用于在有动态随机障碍物(但没有任何静态障碍物)的环境中导航。首先，SR- query[4]是一种基于PRM的方法，它使用SRset对边进行甲醛，动态调整路径选择，当障碍物移动时，这些边的权值会被更新。该方法已被证明在动态避障方面优于一般的基于PRM的[10]方法。第二,APF-SR【5】是一种使用SR作为偏执的APF方法，其实验表明有比起标准的APF方法有更高的成功率。并且我们还比较了路径导向的APF-SR算法和ORCA算法[11]，后者是一种利用对某区域的预测碰撞、速度的多机器人路径查找的算法。（也很值得看一下，说不定看一下就能把这篇文章改进成多机器人动态避障呢）
我们在300个动态移动障碍物的环境下进行来测试，既在没有静态障碍物的环境测试过，也在有特别大挑战性的静态障碍物的环境下进行了测试。测试中，我们的算法避免了local minima并且有超过90%的成功率。此外我们的算法还能够解决APF-SR和SR-Query无法解决的场景。我们还使用两种不同的采样方法(PRM和EST)的路径评估了路径引导的APF-SR，结果显示成功率没有显著变化。随附的视频提交包含实验和可视化的模拟。

相关工作

APF[12]是一种功能强大的路径规划方法，结构简单，适合快速在线规划，易于处理动态和非完整约束，适用于无人机[13]、[14]、机器人足球[15]、移动机器人[16]、[17]、[18]、[19]等。APF已经扩展了一些方法来解决由于障碍接近[16]而无法达到目标的问题，并且需要[17]在狭窄的通道中导航。其他最近的工作集中在通过模糊[19]和进化的[18]APFs来修改势场的计算。基于排斥和吸引概念的APFs可以与其他路径规划方法[20]、[21]相结合。在[20]中，用户定义的成本图影响快速探索随机树(RRT)算法中节点的位置。成本图为每个区域规定了一个排斥和吸引的factor。相似的，navigation fields[21]为agent分配了一个gradient，应用于群体建模。一种基于可得性概念的多智能体规划方法在[22]提出。
APF方法的主要缺点是陷入局部极小值的可能性。虽然提出了若干办法，但这仍然是一个困难的问题。随机路径规划器[6]规定了在局部最小值时的随机行走，这在某些环境中是有效的，但通常需要很长时间才能从复杂的环境中逃脱(例如，具有长而窄的逃脱路径的bug trap)。导航函数方法[7]在计算上是昂贵的，而且常常是受限的。
路径引导的APF被应用于机器人集群[8][9]通过沿着预先计算好的路径从中间目标产生有吸引力的潜力，允许大量的机器人以协调的方式在复杂的环境中有效地导航。通过沿着预先计算好的路径从中间目标产生有吸引力的潜力，允许大量的机器人以协调的方式在复杂的环境中有效地导航。Path-Guided APF-SR重点是在一个复杂的环境中，在许多不可控的随机移动的障碍安全导航单个机器人。这需要一个新的路径引导方案来避免局部极小值和一个SR-based的排斥力来提高安全性。
（下面这两段主要描述了可行性分析在路径规划中的工作，有一点看不懂，但是如果说涉及理论的话，可以去看一看相关的论文）
Reachability analysis has been used to inform motion planning decisions for collision avoidance, based on two approaches. One approach allows for a control and distur- bance input, and generates the maximal set of initial states within which a collision is guaranteed, assuming the worst case disturbance input [23], [24]. This set, known as the reachable set, can be obtained by solving a Hamilton-Jacobi- Isaacs (HJI) equation [25]; its complement assures collision avoidance. In [26], the reachable set (for the robot to reach target while avoiding a single obstacle) is computed by assuming the obstacle actively attempts to collide with the robot. A similar approach is used in [27], with reachable sets computed iteratively to enable the robot to modify its actions. Multiple obstacles are avoided in an online fashion in [28], based on precomputed invariant sets.
基于两种方法的可达性分析已被用于指导避免碰撞的运动规划决策。一种方法允许控制和干扰输入，并产生最大的初始状态集，在其中一个碰撞是有保证的，假设在最坏的情况下干扰输入[23]，[24]。通过求解哈密顿-雅可比-艾萨克斯(HJI)方程[25]可以得到这个集合，称为可达集合;它的补充保证避免碰撞。在[26]中，通过假设障碍物主动尝试与机器人碰撞来计算可达集(机器人在避开单个障碍物的同时达到目标)。在[27]中使用了类似的方法，迭代地计算可达集，使机器人能够修改其操作。在[28]中，基于预先计算的不变集，以在线方式避免了多个障碍。
Another approach incorporates stochastic relative dynamics（随机相对动力学）. Probabilistic SR sets [29] describe the set of states in which a collision is guaranteed with a certain probability.（概率性SR集[29]描述了以一定概率保证碰撞的一组状态。） In [30], the obstacles are modeled as random sets that evolve over time, whereas the desired target set is known and unchanging（障碍被建模为随时间演化的随机集，而期望的目标集是已知的和不变的）. A disturbance input that acts in opposition to the robot’s objective is considered in [31], and the reachable sets are generated using a two-player game formulation（【31】考虑了一个走向机器人并且与机器人目的地相反方向的干扰输入，其使用双人博弈formulation来产生reachable sets）（PS:我一直不太懂这个sr set的意思，现在我感觉可能是速度和角速度set）

问题假设

动态障碍物

我们认为障碍物而是一个2d的网格状物体，形状是一个方形，其中心是
我们假定障碍物遵循着以直线或者一个固定弧度描述的轨迹，其线速度或者角速度是随机的速度。（这里估计是说 v=random_p(w)() w=random_p(w)() random_p(w)=p(w)总之就是线速度和角速度都是服从同一个分布出来的随机数）但是更复杂的动态模型可以在直线和固定的曲线运动中交换，这样简单的组合。运动障碍物的轨迹可以被欧拉近似方法离散化表示为一个以一个时间步长在内的表达式。

直线的轨迹，是直线的斜率，；在固定的弧线轨迹中r是半径，，。

Relative robot-obstacle dynamics（运动学）

我们假定机器人是一个有动力的质点

我感觉去翻译这段话就有点不太专业了，但是这里面的描述呢，都特别的好，应该是用到了向量空间方面的知识，并且还把机器人移动的这个问题考虑成了一个概率问题。（貌似）

SR Sets for Collision Avoidance

这主要讲的是他们通过上面的描述去计算出一组可以避免碰撞的k，然后又计算了机器人在经过根部的时间之后，仍然在k中的概率，既the complement of hat(K)，具体的内容可以看32和29的文件，如果想知道更多的内容，可以看4和5文献（估计是他们之前的文章）
As in [29], the SR set is generated by iterating the value function backwards in time with
and

这一部分应该讲的是如何计算机器人和其他的障碍物有没有相撞的数学表达，如果没有collision，1k(x)=1，否则1k(x)=0，在之后要用到的时候再看。 这里的这方框应该说的是所有的可行驶范围的空间，也就是（X,Y）X∈R，Y∈R，这样（大概），那么这个个人感觉就是从现在到之后，所有的轨迹在没有碰撞之前的一个得分情况之和，怎么说呢，和强化学习里面的value function有点像，但是不知道为什么，这里就说，from time n=N到n=0。

这里说从最开始位置的value function，也就是n等于0时候的，为了方程描述了在n步之后躲避了撞击的概率，这时候，最优的控制策略（输入u），相当于去解决上面这个优化问题。

---

方法！（重点来啦）

路径引导的APF-SR将规划分为离线阶段和运行时阶段。在离线阶段，采用基于采样的方法，只在有静态障碍物的情况下生成目标的路径。然后，在运行阶段使用这条路径作为启发来指导APF- SR方法(考虑移动的障碍物)。这消除了静态障碍物所产生的局部最小值，并保持了APF-SR的高成功率。因此，路径引导的APF-SR能够在有大量移动障碍物的环境中进行快速的实时规划。

A. Offline Phase: Computation

预先计算stochasticreachable set and a path.
（文中说式子（11）是随机可达集合的表达式，这里我有点不懂，我怎么感觉那个是一个最优化函数呀，随机可达集合的表达式难道不是上面的V）
Since the stochastic reachable set(11) can have discontinuities and plateaus, which are detrimental to a gradient-following APF method, we convolve the stochastic reachable set with a 2D Gaussian of width σ for smoothing (Figure 2).
这里的意思应该是说随机可达集，它的特点是不连续，并且有高原现象，我也不知道这个高原现象是什么（可能就是说斥力断崖式生长），但是为了解决这个特点，文中使用了一个2D的高斯分布用来平滑。

B. Run Time Phase: Planning

算法的输入是移动物体以及其预计算好的平滑SRsets,和静态物体,以及sampling-based 生成的path,机器人r的起点S和重点G.

Path-Guided APF-SR (Algorithm 1) first updates the obstacle positions(line 3-5, updateObstacle).
一开始这个updateObstacle函数应该是预先计算这些障碍物按照原先的路径,接下来的最大时间内的位置集合
The repulsive potential has two primary components, due to moving and static obstacles. The moving obstacle repulsive potential is queried by using the pre-computed SR sets (line 7-12, $queryAPFGradient$). 排斥力的来源有两个，一个是静态障碍物，一个是动态障碍物，使用预先计算好的SR集来查询移动障碍物的排斥力.

这里一开始的APF力为(0,0)估计是xy方向的力

每一个动态障碍物都对APF有作用力
The static obstacle repulsive potential is computed (line 13-16, $calcAPFGradient$).16~13行是计算静态障碍物的排斥力作用，

The attractive potential is computed by finding the next node on the path which is required to generate a path-guided potential (lines 15-17, $getNewTarget$ and $getPathGuidedGradient$). 这里是说假如检测到现在机器人的位置比较接近于我们现在的目标的话，那我们就去把目标进行更新，得到下一个path的目标位置，

这里是计算这个路径给多少的吸引力，这个路径的吸引力其实是一个点的吸引力，因为它路径有很多个点组成，我们是一个点一个点的去去寻找.

The attractive and repulsive potentials are combined to compute and execute the robot’s action (line 22-23, $getControl$). 这里22~23号就说我排斥力和吸引力全部都计算完了之后去计算我机器人现在要执行的动作，也就是我的速度.

The subroutines are described in more detail below.下面将更详细地描述其中的函数

updateObstacle

Subroutine $updateObstacle$ moves the obstacle with its current velocity or angular velocity. Velocities are sampled every interval $T$ from a set of velocities $W$ identical to the offline SR set calculation.这个updateObstacle子程序呢，他把移动障碍物从当前的位置以他的速度和角速度进行移动，角速度和速度与sr set的速度与角速度相同
For every moving obstacle within a distance, $d_{min}$, from the robot, $queryAPFGradient$ computes the repulsive APF gradient by finding the relative position from the obstacle to the robot then querying the smoothed SR set collision probability for the APF value (Section III)在这个系统中，机器人与移动物体每移动一小步就要去计算这个物体和机器人之间的相对位置，并且将这个相对位置送到之前计算好的sr set中来计算这些障碍物对机器人的斥力作用. The gradient is then calculated by the second order central difference method然后用二阶中心差分法计算梯度 and summed into the vector $APFgradient$ for each obstacle.并且将所有的向量想加

calcAPFGradient

Subroutine $calcAPFGradient$ calculates the gradient of a repulsive APF ($U_S$) generated by a static obstacle. $U_S$ can be calculated in the fashion similar to [12] based on the distance $d$ to the obstacle. To implement the same APF decay around both static and moving obstacles, we use:

这个函数计算的是静态障碍物对机器人的排斥力大小，排斥力的大小可以和文献[12]的差不多，也就是基于距离。文中说为了在动态和静态在，我们的衰减效果，他用了上面这个函数。
The convolution of a Gaussian with variance $\sigma$ and a square of width $w$, where $w\gg \sigma$, results in an complementary error function ($erfc$) type decay on all four edges. 这句话可能说的是，之前sr set计算的高斯函数，以及现在这个us函数，会导致形成一个互补的Error function.以至于其会变成一个扁平的分布？

Given a small $\sigma$ (e.g., compared to the obstacle size), the moving and static obstacles generate comparable APFs.

getNewTarget

In order to compute the attractive potential, subroutine $getNewTarget$ finds the next node in the path. With a PRM, the path is found by running Dijkstra’s algorithm.为了去计算它的吸引力大小，这个get new target函数找到了下一个路径节点，并且这个路径是用杰杰斯特拉算法来得到的。For other methods, such as a tree-based method, there is a single path, so the next path node is simply used对于其他的算法，比如说基于数的算法，比如rrt，这里只有一条路径，所以下一个节点是直接使用的. As shown in Figure 3, subroutine $getPathGuidedGradient$ computes the Edge Following Gradient and the Next Node Following gradient. The Edge Following Gradient is a unit vector perpendicular to the edge that points toward the edge. The Next Node Following Gradient is a unit vector that points toward the next node $V_{nextnode}$. The sum of these two gradients forms the Path-Guided Gradient. This gradient pulls the robot toward the path as close as possible, with deviations from the path due to moving obstacles.这里估计讲的就是道路如何对我们机器人做出吸引力，一部分是垂直于当前道路连线的吸引力，另一部分是指向下一个目标点的吸引，两个建立合成就成为了这个道路对机器人的吸引力，

getControl

Finally, the $getControl$ subroutine finds the control closest to the combined gradient of all obstacles and goal最后这个get control就给出了结合所有吸引力和排斥力得到的控制方向, ($APFgradient$). For example, in the case of a holonomic robot, the control input vector $u$ is collinear with $APFgradient$. On the other hand, for nonholonomic, e.g., unicycle robots, $getControl$ returns control inputs that attempt to turn the robot toward $APFgradient$ at the max turn rate and accelerate/decelerate the robot so that the inner product of $u$ and $APFgradient$ is maximized.然后讲着在完整性约束机器人和非完整性约束机器人下是如何计算
这里我稍微总结一下，这里就是说我们用sr set计算出每一个移动障碍物，它对机器人有多少的排斥力，以及我们一开始会先有一个预先的道路，用来指引我们机器人去前进，然后在前进过程中会实时的计算我们的排斥力和吸引力的总和，最后以这个来规划机器人的移动方向，使得其既能良好的避障又能到达终点。
所以说如果要很好的复现这篇文章的话，要对sr set有个很不错的了解，并且对机器人运动学中完整性约束和非完整性约束的数学，有一个比较好的了解

实验

机器人的输入是受到最大线性速度每秒0.36个单位和π/ 5的最大角速度弧度每秒。这里设置了三个场景，第1个场景是没有任何静态障碍物，第2个场景是有一个框住机器人（有一个小门）的静态障碍物，所以需要机器人从静态障碍物中逃脱，第3个是有狭窄门的静态障碍物。
At runtime, the robot replans every $∆$ seconds. The environment has 300 randomly placed moving obstacles, with 150 following straight-line dynamics (2), and 150 following constant-arc dynamics (3). The moving obstacles are squares with width dobs = 1. Smoothed SR sets are computed for each obstacle with $\sigma =0.15$; for each of the straight-line obstacles, $\alpha \in [0,2\pi )$ was selected randomly. The set of possible linear velocities is$ W = {0.1, 0.2, 0.5, 0.7}$ with probability $p(w)=0.3,0.2,0.3,0.2$; the set of possible angular velocities is $W=0.17,0.26,0.39,0.52/r$ with probability $p(w)=0.2,0.2,0.3,0.3$, with $r\in 5,10,15$.
这里就说了一些动态障碍物，它的参数设定，
To maintain the constant density of moving obstacles, we restrict the robot and moving obstacles to lie in a circle with radius 50. 为了让这个动态障碍物不乱跑，他们给他的运动范围设定了一个半径为50的小圆，When an obstacle hits the boundary of the circle, it is transported to the antipodal position on the circle and continues evolving from this new position.当移动的障碍物撞到半径为50的小圆的时候，他就将移动障碍物移动到圆的另一端，重新开始演化，（个人认为这里有可能说的是改变它的方向而不是位置，要不然就变成闪现了） The resulting density of moving obstacles is similar to that in [5]最后得到的移动障碍物的密度像【5】论文一样，. We presume complete information, that is, that the robot has access to all obstacle positions within radius $d_{min}=3$ units 这里说的是机器人能够知道它方圆三米以内的障碍物信息。
We compare our method to SR-Query [4], APF-SR [5] modified with a standard APF static obstacle repulsion, ORCA [11] downloaded from [34] and modified for a single robot and moving obstacles. All experiments are imple- mented with MATLAB on an i7-3615QM with 16GB RAM.
我们比较了4 5论文中的方法效果和ORCA（11论文）中的效果（将其变成单机器人的运功规划），并且所有的实验都是在一个16g显存i7CPU的电脑上MATLAB仿真、
A. Obstacle-Free Environment
无障碍环境(图4)为路径引导的APF-SR与APF-SR的直接比较提供了一个基线，并证明了PRM制导不会干扰APF-SR移动避障。机器人从(- 25,0)开始，目标是(25,0)
B. Bug Trap Environment
这里比较了，在有障碍的情况下传统APF-SR的到不了终点，是路径引导的算法，他能够到得了终点，这里使用的是prm算法生成路径，
C环境不做讨论，其实大概表明现在这个算法比其他的算法好的意思，没有什么多需要注意的，如果需要查看，请到论文中查看，

讨论

这一的部分为接下来的算法改进有很大的帮助
路径引导的APF-SR失败的主要原因似乎是多个移动障碍物的相互作用。考虑几个障碍物从不同方向汇聚到机器人上的情况。障碍物的排斥力自从sr集中单独的查询的，没有考虑到全部，多重障碍物的存在可能会导致当时机器人陷入一个局部最小值，因为sr set这个集，他是没有对高维的障碍物做一个计算的。（只不过这种情况的发生与单一障碍物遭遇情况是比较罕见的，所以对成功率没有很大的影响）。我们研究了使用SR- query找到的路径来引导APF- SR机器人远离移动障碍物密度高的区域。虽然SR-Query可以引导机器人进入低密度区域，但这并不一定会排除多障碍场景，并且通常会导致每个规划步骤的路径长度和运行时间都要长得多。
我们没有观察到任何碰撞之间的机器人和静态障碍使用路径引导。这是预期的，因为随机移动障碍物的避障比静态避障本质上更困难。然而，在仅使用下一个节点计算梯度的初步实验中，移动避障的作用偶尔会导致相对引导路径较大的便宜，使机器人陷入局部极小值。

Roadmap quality (Table II) has an effect on the success rate of SR-Query, since longer edges increases the proba- bility of collision while traveling on an edge. Path-Guided APF-SR is robust to roadmap quality, so long as it is possible to connect the start position to the goal position路线图质量(表II)对SR-Query的成功率有影响，因为较长的边增加了在边上移动时碰撞的概率。只要能够将起始位置与目标位置连接起来，路径引导的APF-SR对于路线图质量来说是健壮的
Path-Guided APF-SR can be used with sampling-based
methods besides PRM. We implement an Expansive Space Tree (EST) (Figure 7) with 50000 nodes and maximum expansion per edge that is 10 seconds, using with holonomic robot dynamics. While the resulting path is more jagged and about 1.4 times longer than the PRM path shown in Figure 1, the success rate is 92±6% (e.g., comparable to Path-Guided APF-SRwith PRM). This demonstrates the flexibility of our method, in that it is not restricted to PRM for the offline planner for static obstacles. Indeed, the sampling-based part of our method can be chosen as appropriate for the particular problem at hand.这一段话很有意思，他主要说的是他还用了其他的算法对生成的路径进行生成，并且其实虽然它的路径长度长了，但是它的成功率还是很高的，所以说路径生成的算法其实有很大的可拓展性。
此外，我们认为我们的方法在APF[12]、[35]中也可能具有灵活性。虽然[5]证明了APF-SR优于APF的其他选择，但它不可避免地继承了SR集在离线计算时的维数诅咒，假如在人数不多的情况下，碰撞是不致命的，可在线计算的轻量级APF可能优于查询预先计算的SR集，从而为高自由度机器人实现在线APF规划方法，同时仍然避免了局部极小值。
最后，我们的方法确实展示了GNRON(目标没有)
到达由于附近的障碍)问题，特别是当航行通过一个非常狭窄的走廊。但是，可以集成[16]和[17]等技术来缓解这个问题。