在上个视频中，我们介绍了同步定位和映射。正如它的名字所暗示的那样，我们最终以二元占用网格的形式绘制了一个环境地图。在这个视频中，我们将探索一些使用像这样的地图进行运动规划的方法，也就是在这个环境中找到一条轨迹，将机器人的起始状态与某个目标状态连接起来。我们将简要介绍运动规划的含义以及如何使用图来解决这个规划问题，然后我们将介绍创建图的两种流行方法——基于搜索的算法(如a *)和基于采样的算法(如RRT和RRT*)。我想这个视频会很好地建立一个基本的理解，我们如何使用图形来规划通过已知环境的轨迹。当你走出去学习更多的规划方法时，你就可以在这个基础上有所建树。所以，我希望你能坚持下去。我是Brian，欢迎来到MATLAB技术讲座。
我们想要找到一条从开始姿势到目标姿势的路径。如果我们讨论的是一个在地面上移动的机器人，可能有三种状态构成它的姿态、x和y位置以及它的方向。
路径是平稳连接起点和目标的一系列姿态状态。确定这个序列称为路径规划。现在，路径规划只是运动规划问题的一个子集。在运动规划中，我们不仅要考虑姿势的序列，还要考虑它们的导数，比如速度、加速度、旋转速率等等。因此，通过运动规划，我们试图精确地规定机器人如何在环境中移动。对于路径规划，我们只关心它所走的路径，而不是它加速或移动的速度有多快。
当然，姿态向量的大小取决于您的系统和环境的具体情况。不是三种状态，它可能只有两种状态，x和y如果机器人是全方位的，方向不重要。或者，在具有多个执行器的机械臂的情况下，姿势可能由数十个状态组成。
在这个视频中，我要用到的例子主要是路径规划对于一个全方位的机器人，只有两种姿态状态。这将简化解释，希望你能看到这些技巧可以外推到高维系统。
好的，让我们从一个简单的地图开始，它与我们在上个视频中生成的相似。它只是一个矩形。
假设，开始姿势在这里，目标姿势在这里。距离最小的解决方案可以直接解决，只要没有障碍，就可以用一条直线连接起点和目标。如果机器人沿着这条路径移动，它将在尽可能短的距离内到达目标。
现在，像这样解析地求解最短路径对于我们简单的环境来说是很简单的。这种类型的解决方案甚至可以适用于有一些障碍和限制的环境。
但对于许多问题，系统的障碍和动力学太复杂，无法用解析方法生成最优解。所以，我们用数值方法来解决这个问题。就像我在这集视频开始时说的，我们将专注于基于图形的方法来数值地找到最短长度的路径。
在我们进入任何特定的算法之前，让我用这个简单的地图向您展示基于图形的解决方案背后的一般思想。万博 尤文图斯基于图的算法通过将环境离散化(即将其分解为离散的点或节点)，然后仅考虑这些节点，找到到目标的最短距离。
让我们用随机的方法来解决这个问题。起始位置是图中的第一个节点，它的代价为0，因为我们已经在那里了。所以我在节点里面放一个0。然后我们可以向随机方向移动，并在图中我们的新位置放置一个节点。节点间的边是我们走过的距离到达这个节点的代价是这条边的长度。现在我们在随机方向上再走一步，放置另一条边，这个节点的代价总共是3个单位。我们可以继续这样做，以一种随机漫步的方式，直到我们达到目标。这条随机路径的代价是10个单位。我们找到了一条可行的路径，尽管它肯定不是最短路径。
所以，我们可以重新开始，采取新的随机步骤，添加一个节点，并将它与一条边连接起来，并记录它的代价。如果我们恰好到达了我们之前访问过的一个节点，我们可以比较到达那个节点的两条不同路径的代价并保留最小的那条。基本上，就是修正我们对到达那个节点需要多少单位的估计。
现在我们有了这个节点图，或者地图上的位置，以及到达每个节点的花费。我们应该认识到，我们实际上不需要构建一个完全互联的图，只需要一个树，它是图的一个子集。图可以有任意连接的节点，但在树中每个节点只有一个父节点。如果你可以用两种不同的方式到达一个节点，如果你在寻找最短路径，那么保留较长的路径是没有意义的。你可以去掉长路径的边，保持树形结构。
通过这种方式，树将从机器人的位置开始，分支将冒险到其他状态，但永远不会重新组合。
现在，为了找到离目标更近的距离，我们可以在环境中随机漫游，更新树，直到我们找到一个代价足够低的到达目标的分支。这并不能保证最优路径，但当节点数趋于无穷大时，它会继续趋于最优。
当然，通过随机漫游建立树并不是最好的解决方案。这就是路径规划算法的用武之地，它们提供了更有效的方法来构建这棵树。
我想从所谓的基于搜索的方法开始，它通过在有序模式中添加节点来构建树。在实践中实现这一点的一种方法是从一个基于网格的地图开始，就像我们拥有的占用网格一样，一个单元一个单元地走，确定成本，或者机器人为了到达那个单元必须走的距离。这里，相邻移动数是10对角线移动数是14。这与我们刚才所做的随机方法类似，只是我们有条不紊地遍历每个单元格，并计算到达它的代价，如果它是到达该节点的最短路径，则更新代价和树。一旦我们覆盖了网格中的每个单元格，最优路径就是在目标上产生最小成本的单元格序列。
这将产生一个最优解决方案，至少在网格的分辨率上是最优的，但您可以看到这将是计算开销很大的，因为这是一种检查每个可能节点的暴力方法。为了改善这一点，研究人员在1968年提出了A*算法，使机器人Shakey能够自己决定去哪里，这是通用移动机器人的第一次。这种基于搜索的方法仍然以有序的方式添加节点，但它是通过对更有可能产生最优路径的节点进行优先级排序，并首先在那里进行搜索来实现的。它通过跟踪一些其他启发式方法来做到这一点，例如从节点到目标的直线距离，以及节点的成本。这两个数字的和就是路径的绝对最小代价。如果有一条通往目标的直线，那么你可以想象，这个节点的总路径长度是48。我们已经做了10个了，至少还有38个要做。因此，应该优先考虑另一个节点，即使它当前的代价是14，因为可能只剩下28个，总共是42，所以继续尝试这条路径更有意义。
因此，通过这种方式，A*允许我们在节点中进行搜索从而达到目标而不必将每个节点都添加到树中。事实上，一旦我们到达目标我们就知道我们选择了最优路径因为其他路径的代价加距离都大于我们找到的路径。
现在，这是a *的一个快速介绍，我要做一个动画，展示它如何在障碍存在的情况下仍然很好地工作，但老实说，我不能做得比塞巴斯蒂安·拉格(腿-你)在他的YouTube频道上已经有了更好的，我在下面链接了。他关于A*的视频和动画非常棒，如果你想了解更多关于这个方法的知识，我建议你去看看。
好了，基于搜索的算法给我们提供了一种方法通过在某种有序的模式中添加节点来构建树。但是这些类型的算法有一个问题，即使是像A*这样高效的算法，随着状态空间的大小和维数的增加，它们的计算成本会变得非常昂贵。您可以想象网格点的数量是如何随着维数的增加而呈指数增长的。这会让一切都变慢。所以，它们往往不用于高维状态空间，比如确定多关节机器人的路径，或者用于真正大的低维状态空间，一个可能有数百万或更多网格单元的空间。这就是所谓的基于抽样的算法有用的地方。
为了理解基于采样的算法是如何工作的，我认为首先有必要认识到，在我们的地图中，可能在大多数地图中，在需要转弯之前，路径可以在一个方向上继续一段距离。对于A*，我们必须计算这两点之间的每一个网格单元——也就是树中的多个节点。但是，如果我们只检查距离较远的节点，并且没有任何障碍，那么我们就可以计算出这一个节点的直线代价。这减少了节点的数量，从而减少了总计算的数量。
那么问题就变成了，我们如何选择这些稀疏节点的位置，使我们仍然达到目标?其中一个答案是随机选择他们，或对他们进行抽样，因此得名。现在，我知道我说过随机选择节点并不是构建树的最佳方法，但是我们选择一个随机节点的位置有点不同。而不是通过某种随机游走来扩展路径，这可能会让路径绕回自己，并花费很长时间探索目标的方向，我们将更聪明地对待随机化，并专注于快速探索随机树(RRT)，它的一个版本可以接近一个称为RRT*的最优解决方案。
让我们来看看基本的RRT算法是如何工作的。从起始节点开始，我们需要在树中放置一个新节点。RRT通过在状态空间的任意位置随机选择一个节点来实现这一点。一旦我们有了这个随机节点，我们就想把它和树中离它最近的节点连接起来。因为我们刚刚开始，所以这是第一个节点。但我们不想把它放在太远的地方，因为路径穿过障碍物或在错误的方向上走得太远的机会越大，边缘越长。因此，我们指定新节点到最近节点的最大距离。
现在，说句题外话。如果随机节点比最大距离更近，那么我们就把新节点放在这里。此外，如果最近的节点和我们想要放置新节点的位置之间存在障碍，那么这个障碍将被完全忽略，没有任何东西被添加到树中，我们继续前进。这可以防止树试图穿过任何墙壁或其他障碍。
好，我们继续。我们随机选择一个新节点，并找出离它最近的现有节点。同样，这是起始节点。我们在树中已经有了两条路径它们都进入了状态空间的不同部分。一个新的随机节点，我们再次将它连接到最近的节点上，这个节点向开放空间延伸了一条路径。这就是为什么这个算法被称为快速探索随机树。当有很大的未开发区域时，就像我们所拥有的一样，很有可能在该区域中选择一个随机节点。这将在某些地方添加新节点，从而导致路径在一开始便快速进入未探索的区域，从而快速探索。一个与目标方向相反的随机节点不会影响它通往目标的路径，因为另一个节点离它最近。通过这种方式，一条路径总是快速地朝着目标前进，而其他路径则快速地进入状态空间的其他开放区域。 And then later on, as the unexplored area starts to fill up, the random node selection tends to just fill the tree out with more branches.
我们可以继续做这个过程，直到路径达到目标的某个阈值。在这一点上，我们有一个可行的路径，尽管可能不是最优的，因为这个方法往往是曲折的。但我们确实找到了一个解决方案，而且重要的是，这个解决方案使用的节点可能比a *所需的节点少得多，因为节点可以间隔得更远。同样，我们设置最大连接距离。
RRT可以很好地用于寻找有效路径而不一定是最短距离的情况。只要起始节点和目标节点是可达的，该方法就保证在样本数量接近无穷大时找到一条路径——在大多数情况下，样本数量有限得多。
现在，如果我们确实想要一个更接近最优的解决方案，我们必须在RRT算法中添加一些额外的步骤，我们得到RRT*。对于RRT*，节点选择过程完全相同。我们选择一个随机节点，找到最近的邻居，如果没有障碍，我们在随机节点或最大连接距离处放置一个新节点-以较小者为准。不同之处在于我们将这个节点连接到现有树的位置。我们不一定要把它连接到最近的节点。相反，我们在某个指定的搜索半径内检查其他节点，并确定是否可以以一种既保持树结构又最小化总路径长度的方式重新连接这些本地节点。在这里，我将它连接到另一个节点，因为这样会产生一条更短的回到起点的路径。
让我们看看RRT*在稍微复杂一点的环境中如何工作。在这里，我使用MATLAB和导航工具箱中的RRT*函数来进行可视化。一开始，你可以看到它正在快速探索环境。而且只用了几十个节点。到目前为止，它与RRT没有什么不同，但让我暂停一下。下一个节点会出现在这里。如果这是RRT，它将把这个节点连接到这个节点，因为它最近。但看看会发生什么。这个节点出现了，它一直连接到这里，因为这是一条回到起点的更短的路径。它重新连接了搜索范围内的其他节点。 So RRT* is always trying to shorten the paths. Let’s continue.
在这里，它到达了目标，就像RRT一样，最初的路径有点曲折。但随着我们继续添加更多节点，现有的路径重新连接，您可以看到它们是如何随着时间的推移而改进的，变得更短更直。只要我们对结果满意，就可以停止添加节点。我们不仅有接近目标的最优路径，而且我们的树生成了接近环境中任何地方的最优路径。至少，只要环境不改变。这就是RRT*的力量。
再说一次，这是对基于抽样的算法的一个超级快速的介绍，我在这个视频的描述中留下了更好的信息来源。这就是我现在要做的简单介绍。这里的想法并不是要告诉您关于路径规划的所有知识，因为仅RRT就有几十种变化，但希望您了解树如何帮助我们在环境中规划路径，以及我们可以构建树的一些基于搜索和基于采样的方法。
在这个视频中，我们讨论了如何规划通过静态环境的路径。然而，其他物体和障碍物也经常在环境中移动，规划算法需要对这些变化做出反应。解决这个问题的部分方法是跟踪扩展对象——这些对象足够大，传感器可以返回多次检测到的对象。这就是我们下个视频要讲的内容。
如果你不想错过这个或其他未来的技术讲座视频，不要忘记订阅这个频道。你想看看我的频道，控制系统讲座，我也会讲更多的控制理论话题。谢谢收看，我们下期见。
