【宾夕法尼亚大学机器人课程学习】Motion Planning

一、定义
二、Grassfire Algorithm
三、Dijkstra's Algorithm狄克斯特拉算法
四、A_star Algorithm A*算法
五、构形空间configuration space
六、Visibility Graph可视图
七、trapezoidal decomposition梯形分割
八、collision detection function碰撞检测函数
九、probabilistic road map(PRM)随机路标法
十、the rapidly exploring random tree(RRT)快速扩展随机树
十一、construct artificial potential fields

以下均是宾夕法尼亚大学机器人专项课程视频的学习笔记：

一、定义

运动规划问题的重点在于如何让一个机器人从一个位置移动到另一个。

举例说明，比如要从绿格子到红格子去，不能超越边界，不能去到黑格子中，那么将其抽象成一个图，如下：

用V表示这些节点，用E表示连接节点的边。很多情况下都可以将图中的边与各数字关联起来，比如上图可以将折线图中单位距离或代价值与节点的边关联起来。

可以发现有很多路径都可以完成这样的效果，但关注点在于找到一条总成本最少或距离最短的路径。

二、Grassfire Algorithm

这里介绍一下Grassfire Algorithm，用0作为距离值标记目标节点，然后给所有距离目标节点1步的点标上1，将距其2步的点都标上2，以此类推，直到抵达起始点。

标上的距离值代表的是从这个点走到目标点的最少步数。（广度优先搜索BFS）

然后从起点START开始，递归地移动到临近单元格而花费最小步数的方式，来构建起抵达目的地的最短路径。如果有两个相邻点含有相同的距离标记值，可以任意选择。

对于以下这种情况：

因为起始点并没有被标记，所以认为起始点和终点不存在可行路径。

在以上例子中，每个网格单元最多被标记一次这种算法的计算量，也就是说，在网格上运行grassfire algorithm所需要的计算量与节点数量呈线性相关。可以进行如下表示：

三、Dijkstra’s Algorithm狄克斯特拉算法

目标：在下图中找到从节点A到节点E的最短路径，即找到一条从起点到终点权重和最小的路

用距离0对起点A进行标记，同时用红色表示它已经被访问或处理。然后对节点A相邻的各点进行标记，即点B、D、F。用蓝色标记目前讨论的列表中的节点，这时已经将某个数和这些节点关联起来了，这些数表示从起始点到该点的最短路径长度。

在A的三个邻点中，B点对应的距离最短，所以接着会访问B点，然后给C一个值8。然后访问节点D的所有相邻节点，发现C的值可以更小，为2+3<8，所以更新C的值为5.然后访问F的邻点… 一旦抵达终点，就可以通过将现在的终点设为起点来重建路径。

Dijkstra’s Algorithm的伪码(pseudo code)：

该算法的计算复杂度为：

四、A_star Algorithm A*算法

上述两种算法都可以找到存在的最短路径，若路径不存在时，它们就会报错。然而，两种算法在执行中都要访问表中的所有的节点，当节点数量庞大时，就会出现问题。

A*算法通过搜索式的代价函数来划分各条路径，探索各自的可能性，然后按照顺序来检查这些选择。和其它两种算法不同的是，它保留了正在探索的节点。

Grassfire Algorithm和Dijkstra’s Algorithm都是均匀地向各个方向去探索，而既然终点的位置已经知晓，就可以大概知道哪个方向离终点更近。

A*算法引入了探索式距离的概念，即给定节点与目标节点之间的距离估计。这样就可以利用这个信息将搜索方向引到我们认为更快到达目标的方向上。

在最短路径问题上使用启发式探索函数（H），它会给定一个节点n并返回一个非负值，这个值代表从当前节点到起点的距离。

假设H(终点)=0；对于图中相邻的任意两个节点x和y，H(x) <= H(y) + d(x, y)。

A*算法的伪代码为：

它与Dijkstra算法有非常相似的结构。对于图表中的每个节点，相应地将两个数值属性关联起来，g值表示当前节点与起点之间的距离，f值表示节点的g值和H(n)值的和，也就是当前节点与起点之间的距离以及估计从当前节点到终点之间距离的求和。

在此算法的每次迭代中，系统选择具有最小f值的节点。一旦目标被选定，对应它各个相邻节点的f和g值就会进行相应的更新。该算法往往按预期更直接地集中在更靠近目标的点上。

五、构形空间configuration space

the configuration space是指所有机器人能够达到的位置所组成的空间的一个集合。

用[tx, ty]坐标可以将二维平面上的点和机器人的构形空间对应起来。

当出现左图黑色处的障碍物时，右边黑色的区域就是机器人的[tx, ty]无法到达的区域，成为构形空间的障碍。障碍构形空间是指机器人由于与障碍物碰撞而无法到达的空间集合，它的形状是由障碍物的形状和机器人的外形（通过Minkowski sum）共同得到的。
反之，机器人可以到达的构形区域称为自由空间。

在此设置中，路径规划的任务转化为在构形空间中从起点到目的地寻找一条路径。只是给一个点规划一条穿过构形空间的路径，并避免其中的障碍，所有和机器人的几何外形以及障碍相关的约束都转移到构形空间中的障碍上。这就是将问题转化到构形空间的优势。而路径规划的实质就是在构形空间内，引导机器人从一个点前往另一个点。

六、Visibility Graph可视图

目标仍然是在二维的构形空间内找到一个从起点到终点的轨迹。在本例中，将构形空间中的障碍物简化为多边形。

将起始点和每个构形空间光学顶点两两相连，如下图，然后只需要计算这个可见的线图中的数据。也就是说，在任意两个顶点之间画一条直线，这两个点可以用直线连接，而且这些直线完全处于自由空间。其中也包括同个障碍上的相邻顶点间的边。

这样为了生成一条从起点到终点距离最近的路径，使用Dijkstra’s Algorithm即可，如下：

那么，其实这个可视图算法（将两顶点间的路径看成一条线）实际上就是构建两点间的最短路径。

但其实机器人紧贴着障碍物的方法并不太好，所以一般在考虑的时候，会把机器人的尺寸考虑得稍微大一些。

七、trapezoidal decomposition梯形分割

基于x轴对障碍物顶点进行划分，从左到右将自由空间划分为不同的区域。最终将二维自由空间划分为了一系列由梯形或三角形组成的区域。这些区域都是凸的，这意味着机器人可以安全地在任一单元内的任意两点间作直线运动。

下图中用绿色的点表明这些自由空间的梯形区域，边代表哪些区域是相邻的，然后找出哪些单元格包含起点和目标点，规划一条通过这两个节点之间的图形的路径。

八、collision detection function碰撞检测函数

为了简化，通常用碰撞函数来表达障碍即可。定义如下，令x表示构形空间中点的位置，当没有障碍物时，CollisionCheck(x)函数的值为0，否则为1。

如上图所示，可以用一系列的三角形表示机器人和障碍物，机器人和障碍物是否相撞问题就转换为了机器人的任何三角形是否与任何障碍物的三角形重叠的问题。

因为三角形是凸变形，所以可以通过检查两个三角形的所有边，看看能不能找到一条分割线，使一个三角形的所有边都在分割线的一边，另一个三角形的所有边都在分割线的另一边。如果能找到这样一条分割线，就说明没有重叠；反之存在重叠。

在更高维，可以看有没有分割面，甚至超平面。

九、probabilistic road map(PRM)随机路标法

每次迭代中，系统会在构形空间中随机选择一种构形，并用碰撞函数检查它是否位于自由空间。

对于新的随机点（绿点），如果它处于自由空间，就尝试查看此绿点是否与其它样本点有可通路径，其中绿色的实现对应于给系统新添加的边，绿色虚线表示由于与障碍物碰撞而连接失败的路径。

这个基本过程的伪代码如下：

其中，pseudocode概率反映过程和路标图的随机性。

PRM过程包含两部分：
①距离函数是关于x1和x2两点的构形空间坐标，并返回一个实数，反映这两点间的距离；也可以通过计算标准距离函数完成，如L1或L2距离。

②在图上规划，确定x1和x2之间是否存在可行路径。比如，可以在两点之间的直线上建立一组均匀的样本，然后用碰撞函数检查两点连线中的位置是否会与障碍产生交错。
一旦PRM程序生成了足够的有效构形空间样本，就可以尝试在指定的起点和终点之间生成路径。
先尝试将起点和终点，跟它们位于路线网上附近的点进行连接，如果此步骤成功，就可以找到一条可行路径。

梳理一下：下图是通过随机抽样构建的原始概率路线图，此时并没有加起点和终点，所以可以在不同的起点或终点的情况下重复使用：

下图中，图中标出了起点和终点，并用绿色的边将它们接入路线中：

最终版本如下，显示出通过该扩充图规划出的路径：

But!!!使用PRM算法，可能会出现即使存在路径，算法也无法找到的情况。比如下图：

当提高样本密集数，就可以找到一条可行路径，如下：

所以只要路径存在，不断添加随机样本点，总能找到路径。但这可能需要很长的时间才能生成这些样本点。所以说，这些基于随机样本的算法在概率上是完整的。并且，路标图生成的样本数量是此算法过程的重要参数。

为了解决上述问题，提出了很多可以让算法有目的进行采样的方法，从而增加了在这些情况下找到路径的可能性。其中一种方法是，多在靠近构形空间障碍的位置采点，从而构建出一条可以绕过障碍物表面的路径。

另外一个问题就是由于样本点是随机的，所以可能会产生非常不合理形状的轨迹。对此需要对生成的轨迹采取一定的措施，以尝试使粗糙的边缘变平整。

PRM的最大优势在于可以应用于高自由度的系统。

十、the rapidly exploring random tree(RRT)快速扩展随机树

和随机路标法类似，系统会生成随机样本并尝试将它们连接成网。只不过采样方法不同，RRT采样过程通过构造一种特殊树图形来进行，其中每个节点都连接到一个父节点，并且树以给定的起点为根。如下图所示：

RRT算法的伪代码如下：

这是原始tree：

如下图所示，红色的点表示系统随机生成的新点，y表示树中最近的现有节点。接下来在x和y的连线上有一个新点z，其与点y的距离为delta。

就会生成如下图的tree，即为更新后，加了z点的tree：

若要构造从起点到终点的路径，那么就需要两个tree，两棵trees都以相同的速度朝向彼此生成，伪代码如下：

步骤梳理如下：
①生成一个随机样本点，让蓝色的树朝该点的方向生长

②然后，查看是否可以将刚添加的新点连接到红色树中最近的节点

在上图中，由于存在障碍，这种连接失败。

③生成一个新的随机样本点，并尝试将红色的树向该点生长

④连接该点到蓝树

成功连接，所以就找到了从起点到终点的一条路径。

在高维构形空间中，用RRT方法构建路径是非常有效的。RRT的另一个重要特点是可以在具有动态约束的系统上使用，从而限制了系统的移动方式。

十一、construct artificial potential fields

如何基于人工势场法(Artificial Potential Field Method)引导机器人在充满障碍物的环境中运动？

基本思路就是在构形空间内构造一个平滑函数，当机器人靠近障碍物时，该函数值较高；远离障碍物时，函数值较低。还希望此函数值在终点时最小，若机器人移动到离终点更远的位置，函数值就会增加。

如图为一个二维的构形空间：

其中，黑色表示构形空间障碍物，红点表示目标位置。

将机器人引导到终点的函数可以构建一个简单的二次函数，如下，函数值在终点为0，该值随着到终点的距离的增加而增加。

还需要构造第二个函数fr，使机器人绕过构形空间的障碍。