自动驾驶——Smooth Local Planning

Jack Ju

已于 2023-05-16 18:28:24 修改

阅读量1.4k

点赞数 2

分类专栏：自动驾驶算法资料翻译文章标签：自动驾驶人工智能机器学习

于 2023-05-08 22:27:33 首次发布

本文链接：https://blog.csdn.net/qq_40464599/article/details/130568134

版权

自动驾驶算法同时被 2 个专栏收录

76 篇文章 55 订阅

订阅专栏

资料翻译

7 篇文章 0 订阅

订阅专栏

7.1参数曲线

在本模块中，我们将讨论分层运动规划器的最低级别，即局部规划器。作为提醒，局部规划器是分层规划器的一部分，它以无碰撞、高效和舒适的方式执行行为规划器所要求的机动。这导致轨迹，即在给定时间空间中的一系列点，或路径和速度剖面，即在每个点具有所需速度的空间中的点序列。然后，这个计划可以作为您在第一课程中开发的控制器的参考输入。在本模块中，我们将以我们在模块四中开发的入门反应式规划器为基础，这样它就能够处理自动驾驶运动规划问题中存在的一些细微差别。特别是，我们将从离散时间转移到连续时间，以便生成易于使用控制器跟踪的平滑参数化路径。
在本视频中，我们将介绍路径规划问题及其相关的约束和边界条件。此外，我们还将讨论参数曲线，以及它们如何用于表示此问题的路径。特别是，您应该了解运动规划中样条曲线和缓和曲线之间的区别，以及每种曲线的优点和缺点。
在这里插入图片描述
理解路径规划问题的第一步是首先了解其最基本的要求。对于路径规划问题，这是给定一个起始位置、航向和曲率，找到一条到达终点位置的路径，航向和曲率满足我们的运动学约束。在优化的背景下，起始值和结束值可以被公式化为问题的边界条件，车辆的运动学运动可以被公式化成优化变量的连续时间约束。在这种情况下，边界条件是必须保持在路径的任意端点上的条件，以便给定的优化解被认为是可行的。如果违反了这些边界条件，无论路径有多大，我们都没有实现真正到达我们想要到达的点的核心目标。因此，路径对我们没有用处。这些边界条件将影响我们如何决定建立优化问题的底层结构。
在这里插入图片描述
让我们看看为什么会这样。对于我们的路径规划器来说，我们唯一的运动学约束是限制路径上的最大曲率。一般来说，这并不容易满足，因为沿着连续路径有无限多个点。相反，我们通常会对路径上不同点的曲率进行采样，并约束这些点中每个点的曲率。假设路径表现相对良好，这可能对应于满足约束的整个路径的曲率。
在这里插入图片描述

为了简化优化问题的表示，我们将把路径定义为参数曲线。什么是参数曲线？参数曲线是指可以描述为具有特定参数的一组方程的曲线。这些参数通常表示路径遍历，无论是通过弧长还是从零到一不等。例如，这里我们有一个三次样条曲线的参数方程组，用于路径的x和y位置。当我们从路径的起点行进到路径的终点时，方程u的参数从零到一不等。向量值函数r包含对应于给定u值的每个点的x和y位置。
在这里插入图片描述
对于自动驾驶，我们通常但并不总是要求路径是参数曲线。为什么会这样？我们经常关注的规划方法是根据边界条件优化给定的路径，边界条件如这里的Beta noth和Beta f所示，运动学约束如这里的Alpha所示，目标函数如这里的f所示。有了路径的参数表示，设置优化问题就更简单了，因为我们有一个函数可以直接给目标函数f。请注意，术语泛函指的是以函数为自变量并返回实值的映射，因此它可以用于定义函数空间或参数化曲线上的成本。
在这里插入图片描述
我们可以将这种参数曲线方法与模块四中的反应规划器进行对比，在模块四中，我们用空间中的一系列点来表示轨迹和路径。这被称为非参数路径，因为我们所遵循的曲线没有参数表示。

在自动驾驶领域，有两种常见的路径参数化类型。第一种是五次样条，它是汽车x和y位置的五阶多项式函数。第二种类型是多项式螺旋，由关于弧长的多项式曲率函数给出。图中是一个三阶多项式螺旋，一个三次螺旋，这两条参数化曲线都为我们提供了满足刚才讨论的边界条件的方法，也为我们提供在目标函数中使用的参数，以根据我们的要求绘制路径。选择其中任何一个都有相关的权衡。
在这里插入图片描述
所以，让我们更深入地探索每一个。首先，让我们讨论五次样条曲线。五次样条由两个方程给出，一个用于x沿着样条的级数，另一个用于y。这里我们可以看到，五次样条有12个参数，其中6个用于x方程，6个用于y方程。这些参数对应于形成曲线形状的多项式系数。遍历参数u在这里是相当任意的。为了简单起见，我们认为它在0到1的范围内。这意味着，u等于零对应于路径的开始，u等于1对应于结束。五次样条曲线的一个很好的性质是，对于给定的位置-航向和曲率边界条件，满足这些条件的样条曲线系数存在一个即时闭合形式的解。这个解决方案很长，所以我们不在这里列出，但它的评估仍然比使用迭代优化方法生成路径更便宜。有关完整列表，请参阅补充材料。还有额外的自由度，可以根据该应用进一步优化。这是可取的，因为它使我们能够立即生成边界条件的可行解，并可以随时进一步细化。
在这里插入图片描述
五次样条曲线的缺点是，通常很难像自动驾驶中经常需要的那样，将曲率约束在一定的范围内。如果我们观察参数曲线的曲率方程，我们可以看到，对于五次样条曲线，曲率作为弧长的函数通常不会是多项式。这有可能在样条曲线中引入曲率的尖端甚至不连续性，这使得很难在样条线的整个域上近似满足曲率约束。我们将在下一课中更详细地讨论这些曲率约束，在下一课中将设置路径规划的优化问题。

在这里插入图片描述
作为一种替代方法，我们也可以使用多项式螺旋来表示我们的路径。这些曲线提供了曲线沿其弧长的每个点的曲率的闭合形式方程。对于自动驾驶来说，选择三次多项式作为弧长的曲率函数是很常见的。然而，高阶函数也是可以接受的。使用多项式螺旋的主要优点是，它们的结构非常有利于满足路径规划问题经常需要的近似曲率约束。由于螺旋是曲率的多项式函数，因此曲率值不会像五次样条曲线那样变化极快。这意味着我们只能约束缓和曲线中几个点的曲率，并且缓和曲线很可能满足整个曲线的曲率约束。这在执行路径优化时非常有用，因为约束的数量大大增加了每个优化步骤的计算工作量。
在这里插入图片描述
使用多项式螺旋线的缺点是，与五次样条曲线的情况不同，螺旋线的位置和方向没有闭合形式的解。因此，我们必须进行迭代优化，以生成满足边界条件的螺旋。从这里可以看出，位置方程产生了菲涅耳积分，菲涅耳积分没有闭合形式的解。因此，我们需要使用数值近似技术来计算螺旋的最终端点。在本模块中，我们将使用第三行所示的Simpson规则来近似这些菲涅耳积分。Simpson规则比其他近似方法更准确，点更少，这在我们设置优化问题时会很有用。当谈到螺旋的长处和短处时，与样条曲线相比，我们几乎具有对偶性，每个样条曲线都有一个弱点，而另一个很强。样条曲线仅基于起点和终点提供闭合形式的解决方案，而缓和曲线则不提供。缓和曲线可确保曲率沿路径平滑变化，而样条曲线则不能。因此，您需要根据您的具体应用来确定哪种方法是合适的。简而言之，样条曲线可以提高计算效率，而螺旋曲线可以更容易地实现曲率约束。对于本模块，我们将在开发路径规划器时重点关注多项式螺旋，因为我们对确保本地规划器生成的路径能够由车辆平稳安全地执行非常感兴趣。然而，前面描述的许多技术也可以应用于五次样条曲线。
在这里插入图片描述
让我们总结一下这一课。我们概述了自动驾驶路径规划中使用的边界条件和参数曲线，并引入了样条曲线和螺旋线作为替代路径表示，并讨论了它们在路径规划中的差异。

在下一节课中，我们将讨论如何使用本课中讨论的三次螺旋参数化以及本课程前面定义的约束和目标函数来设置路径规划优化问题。

7.2路径规划优化

在本课中，我们将讨论如何将我们在模块一中讨论的一些目标和约束与我们在上一课中引入的边界条件中的三次螺旋相结合，以创建路径规划优化问题。通过解决这个问题，我们将能够生成满足所有约束的平滑可行的路径。在本视频结束时，您应该能够：识别使用多项式螺旋进行平滑路径规划所需的边界条件和约束，近似一些所需的约束以提高优化问题的可处理性，并知道如何映射所需的参数，使优化问题快速收敛到可行的解决方案。
在这里插入图片描述
如果您还记得上一课，我们的边界条件描述了两点之间规划路径的绝对最小要求。从本质上讲，它们要求对于给定的起始位置航向和曲率，我们计划的路径也在特定的位置航向和弯曲处结束。这将为我们提供优化问题的第一组约束，即边界条件，我们可以在这里看到。不幸的是，正如我们在上一课中所讨论的，三次螺旋对于螺旋末端的位置没有闭合形式的解。为了用螺旋的参数来描述我们的约束条件，我们需要使用数值积分技术。存在许多，但我们将应用辛普森规则，我们在上一课中简要提到了这一点。

在这里插入图片描述
让我们更仔细地看看辛普森定律。Simpson法则是一种常用的数值积分技术，通常比其他更简单的数值方法更精确。这是因为它评估给定函数的二次插值的积分，而不是像中点和梯形规则等一些方法中那样评估线性插值的积分。Simpson规则通过定义由n定义的积分域的多个等距划分来进行，然后在每个划分和边界点对项求和。例如，如果我们选择n等于4，那么我们将积分域拆分为四个大小相等的分段，因此我们有五个点要包括在我们的和中。总和中的每个项都是在除法点处评估的函数乘以适当的系数。在Simpson规则方程的内部，我们可以看到，除了系数为1的端点项外，每个项的系数为4和2。正如人们所期望的那样，随着n的增加，我们得到了积分的更精确近似。

在这里插入图片描述
让我们将此应用于我们的具体规划问题。如果我们在Simpson规则近似中取n等于8，那么我们的近似对于我们将要执行的优化来说将足够准确，而不会过于昂贵。由于标题Theta只是三次螺旋函数的积分，我们可以明确定义它的闭式解，它是一个四阶多项式，如图所示。然后，我们可以使用Simpson规则近似中每个分割点的Theta值来计算三次螺旋的x和y位置。x和y要积分的被积函数分别是s的Theta的余弦和s的Theta的正弦，它们在Simpson规则中被代入f，形成以下表达式。现在，我们对弧长参数s定义的任何给定弧长点处螺旋的X和Y位置有了一个有用的近似值。我们将使用Simpson规则计算的X和Y的近似值表示为X sub s和Y sub s。
在这里插入图片描述
回到我们的边界条件，我们现在有了路径结束位置的近似值，我们可以根据螺旋的已知参数写出边界条件。我们现在可以通过迭代优化螺旋的参数及其总弧长Sf来生成从一个点到另一个点的螺旋，该螺旋满足给定的边界条件。然而，在我们这样做之前，让我们回顾一下我们想要强制执行的运动学约束。
在这里插入图片描述
特别是对于自动驾驶路径规划，我们将重点关注曲率约束。汽车有一个绝对最小的转弯半径，需要保持在横向加速度限制范围内，以保持车轮牵引力和车辆的行驶舒适性。我们将在未来的课程中更详细地讨论这些限制。现在，让我们假设我们的汽车可以实现两米的最小转弯半径。这对应于0.5弧米的最大曲率。现在，很难在螺旋线上的每一个点上写出这个曲率约束。然而，由于螺旋线的多项式性质，我们只需要约束几个均匀分布的点。因为曲率的多项式函数是连续的并且表现良好，所以在进行优化时，我们很可能会生成一个满足曲率要求的螺旋。在这里插入图片描述
为了简单起见，让我们将曲率约束在曲线的三分之一点和三分之二点。起点和终点曲率已经在边界条件中受到约束。一旦我们做到了这一点，我们现在就有了作为螺旋参数函数的曲率约束，并且我们有了解决优化问题所需的所有约束。在这里插入图片描述
谜题的最后一块是我们希望最小化的实际目标函数。我们希望沿着我们计划的道路保持平稳和舒适。这样做的一种方法是沿着路径均匀地分布绝对曲率。这可以通过最小化我们计划的参数曲线的弯曲能量来实现。曲线的弯曲能量是其曲率平方沿路径的整个弧长的积分。由于我们有一个曲率多项式函数来描述我们的三次螺旋，弯曲能量积分就螺旋的参数而言有一个闭合形式的解。此外，它的梯度也有一个闭合形式的解。然而，这两个表达式都有许多术语，因此最好由符号解算器来创建它们。目标函数及其梯度具有闭式解这一事实使其成为一个非常有利于非线性规划的目标函数，我们将在下一课中讨论这一点。
在这里插入图片描述
现在我们有了目标函数，我们可以把所有的东西放在一起，解决这里所示的路径规划优化问题。出于我们的目的，我们将假设初始边界条件为零，这意味着我们正在定义车架中的局部规划问题，并使用我们在本视频中定义的Simpson规则得出航向和x和y近似的简化表达式。这意味着可以去除初始边值约束，因为它们已经在我们的积分计算中得到了考虑。现在我们可以到此为止，并将其用作我们的路径生成优化问题。然而，如何设置这个优化问题存在一个实际问题，当使用规范非线性规划求解器求解时，这可能会减慢它的速度或导致它根本不收敛。
在这里插入图片描述
让我们现在尝试解决这个问题。我们在这里看到的主要问题与最终位置和标题的平等约束有关。由于等式约束必须精确满足，因此数值优化器很难从不可行的起点生成可行的解决方案，而不可行的起始点通常是为任意问题实例提供给优化器的。为了缓解这个问题，在优化中通常使用软不等式约束来提高优化器的性能。软约束将严格约束转化为目标函数中的严重惩罚项。所谓严重惩罚，我们的意思是约束惩罚项系数应该至少比一般优化目标大一个数量级。尽管这允许优化器违反边界条件相等约束，但在弯曲能量惩罚项大到足以影响优化器之前，将强烈鼓励优化器收敛到尽可能接近边界条件的解决方案。我们还将假设我们的初始曲率是已知的，并且通常设置为零，这对应于等于零的零。这将优化变量的数量减少了一个。在软化这些约束之后，我们的新优化问题如下。在定义要实现的优化的最终版本之前，我们还有一个问题需要解决。
在这里插入图片描述
我们可以解决的最后一个问题与优化参数有关。虽然在我们的目标函数中使用三次螺旋系数有更多的直觉，但我们实际上可以通过考虑最终的曲率边界约束来减少我们正在搜索的参数的数量。让我们使用一组不同的参数来重新定义我们的三次螺旋，这些参数由向量p表示，其中p有五个元素。首先，我们有p零到p3，表示起点、三分之一点、三分之二点和终点的曲率。最终项p4是路径的最终弧长。方便的是，我们在曲率参数和螺旋参数之间有一个闭合形式的映射，如图所示。因此，我们可以很容易地将所有约束和目标项计算为这些新的p变量的函数，而不是螺旋的系数。一旦优化得到解决，我们就可以使用这里的方程将结果映射回螺旋系数。由于我们已经知道初始曲率和最终曲率，我们可以消除其中两个变量，p零和p3。这使得我们在优化问题p1、p2和p4中只有三个变量。通过使用边界条件，我们降低了优化问题的维数，这将导致显著的计算加速。
在这里插入图片描述
由此产生的最终优化问题如下。在重新映射到p参数后，我们用等效函数替换了螺旋参数的函数。请注意，初始和最终路径曲率p零和p3是常数。因此，优化变量现在只有p1、p2和p4。这种简化是可能的，因为在路径的开始和结束处曲率的边界条件是已知的。现在我们已经进行了这些修改，我们可以有效地解决路径规划问题，并且大大减少陷入较差的局部极小值的几率。

在这里插入图片描述
让我们总结一下我们在这个视频中学到的东西。首先，我们回顾了这个问题所需的边界条件和约束条件。然后，我们讨论了如何使用Simpson规则对螺旋的n个位置进行数值计算。然后，我们引入了弯曲能量目标以促进平滑，并形成了一个通用的螺旋优化问题。最后，我们重新映射优化函数的参数，以确保快速收敛到可行的解。好吧，这是很多新的信息。我们希望这节课能让你深入了解如何执行平滑路径规划。本课将我们在第一单元和第四单元中讨论的许多主题联系在一起。因此，如果你觉得这些材料中的一些很有挑战性，在再次学习本课之前，请随时复习这些模块。
在这里插入图片描述
在我们的下一课中，我们将讨论如何在Python中执行优化，以便为实现完整的路径规划器做好准备。

7.3 Python中的优化

在本课中，我们将复习Python中优化的基础知识，以帮助巩固我们在本模块的前几节课中讨论过的一些优化概念，事实上，在整个专业化过程中都有讨论。特别是，我们将使用SciPy优化库来研究解决一般非线性优化问题所需的一些函数。在本视频结束时，您应该知道如何使用此库设置和调用约束优化问题。特别是，您应该知道如何将雅可比矩阵传递给优化器，以及在优化问题中定义的任何所需参数边界。
在这里插入图片描述
所以让我们开始吧。优化领域是一个非常丰富的研究领域，我们无法在这个专业中详细探索。SciPy优化库涵盖了一些最流行的优化算法，使其易于访问，并确保其实现的合理效率。许多实现的优化方法在它们需要什么类型的参数方面具有类似的结构。因此，为了将其抽象为一个简单的接口，SciPy优化库包含一个通用的最小化函数。可用优化方法的一些例子包括共轭梯度、Nelder-Mead、狗腿和BFGS。有关这些方法的更多详细信息，请参阅补充材料中的链接。库运行的特定优化算法将取决于传递给此函数的方法参数。方法参数还将确定优化算法需要哪些附加参数。例如，在我们将使用的L-BFGS-B算法中，我们不仅需要模型最小化，还需要模型雅可比和变量边界。在模型是单个标量值函数的情况下，雅可比降为梯度。这个雅可比通过jac参数传递给最小化函数，如这个函数调用所示。我们希望最小化的实际函数是最小化函数的第一个参数。约束作为约束字典或对象的列表传递给约束变量。此外，还有一个可选的选项参数，高级用户可以使用它来自定义优化器输出的内容。这些优化算法还需要对模型或目标函数的优化变量进行初始猜测，这由函数调用中的x零给出。

在这里插入图片描述
让我们看看BFGS算法，以获得如何使用SciPy实现优化的具体示例。本质上，对于BFGS算法，我们需要传入指向我们希望最小化的实际目标函数的函数指针，以及指向评估目标函数的雅可比函数的函数的函数指示器。这些函数将采用所有优化变量的向量，以便在特定点评估目标函数和雅可比。在这里插入图片描述一旦优化完成，最小化函数将返回一个结果变量。由x表示的结果的成员变量将返回优化变量的最终向量，其中已经实现了局部最小值。
正如我们前面提到的，我们还可以为优化问题指定约束条件。对于大多数算法，这些约束是以列表或字典的形式给出的。最简单的约束类型是对称为边界的目标变量的不等式约束。L-BFGS-B算法将边界指定为列表列表，其中每个子列表的长度为2，并包含每个优化变量的上限和下限。换句话说，第一个子列表对应于x 0的边界，第二个子列表对应x 1等的边界。然后将这些边界传递给最小化函数的约束可选参数。
在这里插入图片描述
线性和非线性约束也可以传递给优化器，但现在我们将专注于使用优化约束的边界。有关更多详细信息，您可以在线查看SciPy优化文档。您还可以通过传入要在优化器函数中使用的每个约束对象的Python列表来组合多种类型的约束。

总之，在本视频中，我们介绍了如何使用SciPy优化库设置优化问题。特别是，我们讨论了如何将用户定义的雅可比目标函数以及参数边界传递给优化器。现在，您应该对如何使用Python库解决一般优化问题有了一个很好的想法。有关更多信息，您可以查阅SciPy优化库文档。

在这里插入图片描述
在这节课之后，我们有一个编程作业，让你有机会练习我们在这里讨论的概念，并为模块项目的结束做好准备

7.4保形格点规划 Conformal Lattice Planning

在本课中，我们将使用在前几节课中开发的优化技术，导出一个完整的路径规划器，称为共形晶格规划器。在本视频结束时，您应该能够实现保形晶格规划器，以解决我们的路径规划问题。您应该了解如何沿道路采样点，以及如何使用我们在第二课中开发的优化公式规划到每个点的路径。您还应该能够确定路径是否没有碰撞，并选择最能跟踪我们需要遵循的道路的路径。

在这里插入图片描述
作为介绍，让我们回顾一下共形格规划器的高级目标和结构。与所有路径规划器一样，目标是规划一条从自动驾驶汽车当前位置到给定目标状态的可行无碰撞路径。共形网格规划器利用道路的结构化特性，在避免障碍物的同时加快规划过程。通过只关注那些稍微转向目标路径左侧或右侧的平滑路径选项，保形网格规划器可以生成与人类驾驶非常相似的计划。在规划道路上的路径时，除非出现紧急停车情况，否则汽车通常不应考虑离开道路。正因为如此，共形格规划器选择了一个中心目标状态以及一系列备用目标状态，这些状态是通过相对于道路的航向从中心目标状态横向偏移而形成的。这在这个共形晶格的例子中进行了说明。其中每条路径的终点与中心路径横向偏移，中心路径对应于道路上的目标点。这个进球点用金色突出显示。我们之所以能做到这一点，是因为总的来说，汽车应该沿着自我车道向前行驶。我们不太关心不会导致前进的路径，所以我们可以大大减少搜索空间，保持共形晶格规划器在计算上的可处理性。
在这里插入图片描述
那么我们如何选择这个目标状态呢？一般来说，在选择路径规划的目标状态时，有一个关键的权衡。如果您选择了一个接近当前自我车辆位置的目标状态，则可以减少找到到达目标点的路径所需的计算时间。然而，你也会降低规划者以平稳舒适的方式避开道路上更远的障碍物的能力。在较高的速度下，这可能会产生问题，因为在规划周期之间，汽车将行驶更多的距离。通常，我们在计划中使用的目标范围是根据车速和天气条件等因素动态计算的。然而，为了简化本模块，我们将使用一个固定的目标范围。我们将把球门点作为车道中心线上的点，也就是说，我们前面的距离等于我们的球门地平线。这在这里沿着这条路径进行了说明，其中黄金点对应于所选的进球位置。蓝色点对应于横向偏移的目标点，这些目标点将用作晶格中每个缓和曲线的替代端点约束。车道中心线的黑色部分的弧长等于我们选择的球门地平线。在每一个规划步骤中，我们都会根据相同的地平线重新计算我们的目标点，并沿着车道向前推进。

在这里插入图片描述
一旦找到了这些目标状态，我们就可以计算达到每一个目标状态所需的螺旋。在这一点上，我们不担心路径是否是无碰撞的，我们只想要到达每个目标状态的运动学上可行的路径。因此，我们可以使用我们在第二课中开发的优化公式，来求解从当前位置到每个末端位置的三次螺旋。如果任何螺旋在运动学上不可行或无法达到所需的目标状态，我们将丢弃这些螺旋，使其不再被视为潜在路径。
在这里插入图片描述
请注意，一旦优化问题得到解决，我们就只有得到的参数向量p。然后，我们必须撤消最初对螺旋系数执行的变换，以便从p向量中检索它们。一旦我们有了螺旋系数，我们就可以沿着螺旋对点进行采样，以获得整个路径的离散表示。
在这里插入图片描述
由于我们没有螺旋线位置的闭合形式解，我们再次需要进行数值积分。然而，由于这一次，我们正在评估积分和整个螺旋上的许多点，需要一种更有效的方法来解决这些积分。这里我们采用了一种线性插值方法。梯形规则。在这种情况下，梯形规则明显比Simpson规则更有效，因为沿着曲线的每个后续点都可以从前一个点构造。所以我们只需要在螺旋中扫一次，就可以得到所有需要的点。另一方面，Simpson规则要求我们为每个点求解积分近似，这效率要低得多。在Python中，我们可以通过使用累积梯形函数来实现这一点。一旦应用了梯形规则，我们现在就有了每个目标点的每个螺旋的离散表示。重要的是，我们要跟踪每个点的曲率以及位置和航向，因为这将有助于我们以后的速度剖面规划。我们有曲率和航向的闭合形式解，因此不需要数值积分。
在这里插入图片描述
在使用梯形规则之后，我们现在为每个目标状态生成了一组路径，如图所示。

现在我们有了一整套路径，我们需要看看哪些是无碰撞的。要做到这一点，我们可以使用模块四中讨论的任何一种碰撞检查技术。通常，我们可以使用二进制占用网格，如果一个单元格被占用，则该网格包含一个，否则为零。然后，我们可以根据占用网格的单元来获取我们的汽车足迹，并将足迹扫过螺旋中的每个点，以生成路径的线束。如果线束中单元的占用网格包含障碍物，则有问题的路径将与障碍物碰撞，并且应标记为发生碰撞。如果在整个路径上，线束中的任何单元都从未发生这种情况，则该路径被视为无碰撞。或者，如果自我车辆和自我车道上的每个障碍物都可以被封闭在圆形近似中，我们可以使用圆形检查。然后，我们将ego车辆的圆圈放置在路径上的每个点上，并检查是否与ego车道内的每个障碍物发生碰撞。为了说明碰撞检查的结果，我们在道路上添加了一辆停放的车辆，现在需要对其进行规划。在每个螺旋上扫过线束后，类似于我们在模块四中的做法，我们将无碰撞路径标记为绿色，将与障碍物碰撞的路径标记为红色。
在这里插入图片描述
在这一点上，我们有一组可行且无碰撞的路径，但我们需要一种选择最佳路径的方法。选择过程在很大程度上是一种设计选择。因为可能存在多个对给定规划应用有用的标准。例如，我们可能希望选择尽可能远离障碍的道路。因此，我们可能会在占用网格中为过于靠近障碍物的路径添加一个惩罚项。我们可能还想惩罚偏离最近车道中心线太远的条款，因为我们不想在变道时分割车道太长时间。现在，我们将使用一个简单的度量，使规划器从路径集中选择尽可能接近中心目标状态的路径。只要点球增加，你离中心球门越远，实际的点球功能就无关紧要。通过偏向中心路径，我们鼓励我们的规划者遵循参考路径，只有在参考路径不可行或与障碍物碰撞时，才让它偏离参考路径。为了简单起见，我们可以将这个函数作为我们正在检查的路径从中心目标状态到目标状态的位移。然后，我们可以迭代路径集中的每个路径，找到最大限度地减少这种惩罚的路径，并选择它作为我们的最终路径发布。在我们的示例中，选定的路径以蓝色突出显示。
在这里插入图片描述
If we now repeat this process for multiple time steps as the car moves along the path, our planner is able to plan a path that converges to the goal state, while also avoiding the obstacles. Similar to the reactive planner we developed in module four, this planner proceeds in a receding horizon fashion towards the goal at the end of the lane. We now have all the pieces necessary to form smooth, collision-free paths through the environment, that favor forward progress in the lane.
在这里插入图片描述

在本视频中，我们首先定义了共形状态格规划器方法，该方法选择沿道路从我们前面的某个目标点横向偏移的点。然后，我们使用我们在第二课中开发的螺旋优化方法规划到这些点中的每一个的路径。接下来，我们讨论了如何修剪这组路径以避免冲突，以及如何选择最佳的剩余路径。最后，我们集成了一种后退地平线方法来完成我们的路径规划。既然您已经对整个路径规划算法进行了概述，我们希望您能够看到本课程中有多少课程达到了高潮，从而有效地解决了路径规划问题。
在这里插入图片描述
在我们的下一个视频中，我们将讨论如何根据路径规划器计算的输入路径，并在汽车沿路径移动时生成速度剖面。然后，我们将有一套完整的参考信号传递给我们的控制器来执行，完成分层运动规划过程的所有三个阶段。

Coding

%%%%a cubic curvature polynomial of the form :κ(s) = a + bs + cs2 + ds3

a = 0;
b = 33;
c = -82;
d =41.5;
Length = 1.5;

s = Length;

x0 = 0;
y0 = 0;
ph0 = 0;
%%%写一个点
s =0 ;
xArray = [];
yArray = [];
kArray = [];
times = 0;
while(s <0.1)
phi = ph0 + d*s^4/4  + c*s^3/3 + b*s^2/2 + a*s;
K   = a + b*s + c*s^2 + d*s^3;
xNew = x0 + s*( cos(ph0) + 4*cos(calFunction(a,b,c,d,s/8,ph0)) + ...
    2*cos(calFunction(a,b,c,d,2*s/8,ph0))+ 4*cos(calFunction(a,b,c,d,3*s/8,ph0))...
    + 2*cos(calFunction(a,b,c,d,4*s/8,ph0)) +  4*cos(calFunction(a,b,c,d,5*s/8,ph0))...
    + 2*cos(calFunction(a,b,c,d,6*s/8,ph0)) + 4*cos(calFunction(a,b,c,d,7*s/8,ph0)) + cos(calFunction(a,b,c,d,s,ph0)) )/24
yNew = y0 + s*( sin(ph0) + 4*sin(calFunction(a,b,c,d,s/8,ph0)) + ...
    2*sin(calFunction(a,b,c,d,2*s/8,ph0))+ 4*sin(calFunction(a,b,c,d,3*s/8,ph0))...
    + 2*sin(calFunction(a,b,c,d,4*s/8,ph0)) +  4*sin(calFunction(a,b,c,d,5*s/8,ph0))...
    + 2*sin(calFunction(a,b,c,d,6*s/8,ph0)) + 4*sin(calFunction(a,b,c,d,7*s/8,ph0)) + sin(calFunction(a,b,c,d,s,ph0)) )/24
xArray = [xArray xNew];
yArray = [yArray yNew];
kArray = [kArray K];


s = s+0.005;
x0  = xNew;
y0  = yNew;
ph0 = phi;
plot(xArray,yArray)
axis([-0.5,1,-1,1])
times = times +1
pause(0)

end


function  [phi] = calFunction(a,b,c,d,s,phi0)
    phi = phi0 + d*s^4/4  + c*s^3/3 + b*s^2/2 + a*s;
end