多伦多大学自动驾驶运动规划笔记-全-

多伦多大学自动驾驶运动规划笔记（全）

002：课程概述与结构 🚗

在本节课中，我们将要学习自动驾驶汽车运动规划课程的整体概述与结构安排。运动规划是自动驾驶系统的核心，它负责生成从起点到终点的安全、平滑的路径和速度。

课程概述

大家好，欢迎来到自动驾驶汽车专项课程的第四门课程。在本课程中，我们将聚焦于运动规划问题，以及可以用来解决这些问题的一些技术。

从宏观层面看，运动规划问题的任务是生成自动驾驶汽车从A点移动到B点所需的路径和速度。为了实现这一点，我们将需要前序课程中描述的感知与传感信息，因为运动规划需要了解周围环境中的其他车辆、行人以及其他障碍物。

由于问题的复杂性，通常会将规划问题分解为更小的子问题。在本课程中，我们将聚焦于一个三阶段的任务规划层次结构，它包括：

地图级导航：你可能在使用在线地图工具规划行程时对此很熟悉。
行为规划：确保我们的驾驶行为遵守道路规则。
局部规划：确保我们的路径平滑且无碰撞。

课程结构详解

在模块1介绍了运动规划问题及规划层次结构之后，我们将在课程其余部分重点讲解解决每个规划子问题的方法。

模块2：地图与静态障碍物

上一节我们介绍了规划的整体框架，本节中我们来看看规划的基础——环境表示。我们将讨论自动驾驶背景下的地图绘制，以及如何表示规划过程中必须避开的静态障碍物。这两种地图都是为自动驾驶汽车生成安全轨迹的必要组成部分。

模块3：任务规划

接下来，我们将聚焦于模块3中的任务规划，这是我们规划问题中最高层的部分。我们将使用模块2中生成的地图，以尽可能高效的方式导航至目的地。

模块4：与动态智能体的交互

从全局导航转向具体交互，模块4将重点讲解自动驾驶汽车如何与驾驶场景中的其他移动智能体（如其他车辆、行人）进行交互。这是一项复杂的任务，对于确保规划过程中的安全至关重要。

模块5：行为规划

理解了交互的复杂性后，模块5将聚焦于行为规划这一子问题。其任务是确定自动驾驶汽车应根据其周围环境表现出哪些行为（例如跟车、变道、停车）。

模块6：局部规划

在确定了高层行为之后，模块6将转向局部规划。我们将介绍一种反应式规划器，为我们在课程1中介绍的自行车模型生成轨迹。

模块7：解耦的路径与速度规划

最后，在模块7中，我们将创建一个解耦的路径规划器和速度剖面生成器作为总结。它建立在我们的反应式规划器之上，能够生成可用于自动驾驶汽车道路驾驶的路径和速度剖面。

实践与项目

在整个课程中，我们将提供作业来测试你实现分层运动规划器各个子组件的能力。这将为你提供必要的实践经验，以便在最终的课程项目中开发一个完全集成的运动规划系统。在该项目中，你将在CARLA模拟器中开发一个完整的运动规划系统。

总结

本节课中，我们一起学习了自动驾驶运动规划课程的整体框架。我们了解到运动规划的核心任务是生成安全、可行的路径与速度，并通常将其分解为任务规划、行为规划和局部规划三个层次。后续课程将依次深入讲解地图构建、导航、交互、行为决策以及具体的轨迹生成方法，最终通过实践项目整合所有知识。希望你对深入探索运动规划问题感到兴奋，让我们开始吧。

003：认识讲师史蒂文·瓦斯兰德

概述

在本节课中，我们将通过讲师史蒂文·瓦斯兰德的个人经历与职业发展，了解他如何从一名广泛的兴趣爱好者成长为自动驾驶与机器人领域的研究者。他的故事揭示了将个人兴趣与实际问题解决相结合的重要性，并为我们理解自动驾驶技术的现实挑战与未来方向提供了背景。

童年与广泛的兴趣

我童年时兴趣广泛，热爱各种事物。我曾认为自己可以成为一名科学家，或许是物理学家，也可能是化学家，但当时对未来并没有明确的想法。

我成长过程中并未过多担忧未来，拥有一个田园诗般的童年。我是一名重度运动爱好者，热爱越野滑雪和山地自行车，这些活动占据了我的大部分时间。

发现挑战与兴趣的结合

大约在16至18岁时，我开始思考需要做一些有趣的事情来度过人生。正是在这个阶段，我开始接受各种挑战，并逐渐认识到是什么在持续驱动着我。

我热爱解决数学问题，喜欢在放学后与朋友们一起研究化学或微积分挑战。这些活动都是我课余时间自发进行的，并没有明确的目标。

直到我开始尝试选择一项事业时，我才发现，我喜欢的这些事情在特定的应用领域中频繁出现。

职业生涯的起点：航空航天工业

大学毕业后，我在普拉特·惠特尼加拿大公司工作，这是一家小型喷气发动机制造商。在普拉特·惠特尼的工作经历令人着迷。

我们有机会测试喷气发动机控制系统，这让我充分接触了航空学的乐趣。然而，航空学领域的挑战在于其极其严格的安全要求。

你所做的每一件事都必须严谨无误，并且每次都必须精确工作。因此，该领域的发展实际上受制于经过反复验证的、已有30年历史的技术。

作为一名试图推动变革、改变世界的年轻工程师，这让我感到非常沮丧。于是，我带着从航空航天工业中获得的美好经验和对安全的专注，进入了研究生阶段的学习。

我选择了航空与航天学专业，并开始建造无人机。

转向机器人学与问题解决

是的，这就是问题解决和算法开发。具体来说，是处理机器人任务。

例如，让无人机降落在行驶的汽车上，或者让自动驾驶汽车自行泊车、在障碍物区域中导航。所有这些问题都非常实际、具有应用性，并且你在构建过程中就能看到结果。

你可以看到它的工作效果如何。如果无人机错过了汽车，它就会坠毁并失败。这其中有一种令人兴奋的、非常有趣的成分。

构建自动驾驶汽车的基础步骤

要制造一辆自动驾驶汽车，首先必须将一辆普通汽车改造成线控驾驶车辆。这意味着你可以将计算机接入汽车，并向油门、转向和刹车发送指令，汽车将实际执行这些操作。

一旦具备了这种能力，你就可以开始编程让它执行任何你想要的指令。

接下来，你必须在后备箱中添加一整套传感器和计算设备。拥有了所有这些部件，你就可以开始构建自己的自动驾驶车辆。

研究实验室的哲学与目标

我的研究实验室围绕这样一个理念建立：如果我们拥有一个真实的测试平台，并将其置于真实情境中，我们就能更快地了解当前方法的局限性。

我们真正关注的是寻找特别适合学术界的研究问题。例如，处理恶劣天气条件、处理一些边缘案例，或者试图从理论上证明我们所使用的方法具有可量化的安全性。

在学术界，我们确实有很多具体问题可以研究。我们可以尝试许多大胆的想法，可以比工业界更快速地、或许更少约束地探索可能的解决方案空间。

自动驾驶的未来与挑战

我们肯定会看到这些车辆的性能不断提升。例如，特斯拉的自动驾驶系统每年都会变得越来越好。我们还将看到真正自主的车队在受限领域内工作，这些领域被认为是安全的运营环境。

但我仍然认为，要实现最终的、在任何你想驾驶的地方都能实现的完全自主，我们还有很多工作要做。这仅仅是因为汽车可能失败的方式太多，它们必须理解的情境太多，以及许多无法提前预测的奇怪条件。

给新学习者的建议

如果你是自动驾驶领域的新手，认为这是一个美妙的领域并想尝试，我认为你首先应该完成这个Coursera专项课程。

除此之外，还有数百种方法可以丰富你的知识，并更好地了解如何进入这个领域。整个领域依赖于机器人学基础、计算机视觉基础、神经网络与深度学习基础。

这些领域你可以进行广泛的学习，其有趣之处在于它们也适用于其他美妙的应用。想想机器人操纵器、手机摄像头系统及其视频拍摄的改进功能、无人机飞行与物体跟踪等，你可以用这些技术做很多事情。

对于那些正在思考自动驾驶是否是一个值得探索方向的人，可以明确的是，这个市场将持续增长。对我们项目中毕业生的需求是巨大的，这非常惊人。

总结

回顾过去，从放学后研究随机数学问题，到现在从事自动驾驶汽车研究并多年来一直与无人机打交道，我只是一个幸运的人。本节课中，我们一起了解了史蒂文·瓦斯兰德如何将广泛的兴趣、工程实践与学术研究相结合，最终投身于自动驾驶这一前沿领域，并探讨了该领域的基础、挑战与未来方向。

自动驾驶汽车的运动规划：P04：认识讲师乔纳森·凯利

在本节课中，我们将认识本课程的讲师乔纳森·凯利教授，了解他的背景、职业动机以及对自动驾驶技术安全性的深刻见解。

我叫乔纳森·凯利，是加拿大多伦多大学航空航天研究所的助理教授。

童年时期，我每月都喜欢去杂志店，阅读《大众机械》或《大众科学》。这些杂志包含了从太空旅行到新型交通工具，再到新计算设计的各种有趣文章。正是这些内容，最终促使我开始将科学技术视为一个可能的职业选择。

在我还很小的时候，发生了第一起航天飞机灾难，即挑战者号事故。我至今仍清楚记得那一刻，心想一定是出了什么严重的问题，其后果是多么可怕。当时看到巨大的爆炸，我震惊地呆坐着，因为我曾认为技术总是可靠的——技术不会失败，汽车不会无缘无故爆炸。但电视上的这一幕让我深刻认识到，扎实的工程学在这些关乎生死或可能改变人生的情境中至关重要。

上一节我们了解了凯利教授的职业启蒙，本节中我们来看看他对自动驾驶领域的看法。

自动驾驶汽车在许多方面与此非常相似。作为工程师，我们有责任将这些系统设计得尽可能安全可靠。这是近期最令人兴奋的技术革命之一。每天都有新的想法、技术和设计涌现。我常常坐在办公桌前，看到人们提出的众多创新理念和技术时，都会感到眼前一亮。

这种兴奋感也来自于它正在改变技术格局，并可能造福许多人。

本节课中，我们一起认识了乔纳森·凯利教授，了解了他从童年兴趣到职业选择的历程，以及他对工程责任和自动驾驶技术安全性与创新性的核心观点。他强调，可靠的技术和严谨的工程是保障安全的基础，而该领域日新月异的创新则带来了巨大的潜力与责任。

005：驾驶任务、场景与行为

在本节课中，我们将学习自动驾驶运动规划的基本问题。我们将了解自动驾驶的任务目标，识别一些重要的道路驾驶场景，并学习处理这些场景所需的关键驾驶行为。

自动驾驶任务概述

自动驾驶任务的核心是从地图上的一个点（当前位置）导航到另一个点（最终目的地）。在此过程中，系统需要考虑不同道路的连接及其相关的交通状况，以在道路网络中找到最高效（时间或距离最短）的路径。任务规划将许多重要的底层变量（如道路结构、障碍物和其他交通参与者）抽象化以简化问题，但这些变量对于运动规划至关重要。

道路结构相关场景

上一节我们介绍了自动驾驶的总体任务，本节中我们来看看由道路结构定义的一些常见驾驶场景。道路结构指的是车道边界以及与驾驶员相关的交通管制元素。

以下是几种核心场景：

车道保持：这是最简单的场景，车辆需要跟随当前车道的中心线行驶。目标是最小化与路径中心线的横向偏差，同时达到参考速度（通常是限速）。
变道：当车辆需要更换车道时，即使相邻车道没有动态障碍物，也需要在横向和纵向加速度约束、道路限速以及执行时间范围内，优化变道轨迹的形状。参数选择会影响轨迹，导致缓慢保守或激进不适的变道。
左转/右转：车辆通过路口时经常需要执行转弯。与变道类似，转弯的轨迹形状和激进程度会因情况而异。此外，自动驾驶车辆可采取行动的可行性受周围环境状态影响，例如，在红灯时即使路口空旷也不能左转。
掉头：在某些需要高效改变方向的场景中，掉头很重要。其轨迹形状同样受参数影响，并且高度依赖于周围环境状态（例如，在某些国家的某些路口，掉头可能不合法）。

静态与动态障碍物的影响

道路结构并非影响场景的唯一因素。静态和动态障碍物会极大地改变场景结构，并增加确定所需行为的难度。

静态障碍物：指不移动的物体，如停放车辆、隔离带和路缘。它们限制了自动驾驶车辆在规划路径时可以占据的位置，因为车辆占据的空间不能与障碍物重叠。
动态障碍物：指在自我车辆规划工作空间中移动的参与者，对速度曲线和驾驶行为影响更大。例如，在车道保持时前方有前车。如果前车速度比我们的参考速度慢，我们需要在保持安全时距（以当前速度到达前车当前位置所需的时间）和尽可能接近参考速度之间取得平衡。

动态障碍物也会影响转弯和变道场景。根据它们的位置和速度，自我车辆可能只有特定的时间窗口来执行这些操作。这些窗口是基于对其他交通参与者行为的预测估算的。例如，在左转前等待对向车流出现空档。

动态障碍物不仅包括汽车，还有自行车、卡车和行人等。它们各有其行为模式和道路规则。

高层驾驶行为

尽管驾驶任务场景丰富多样，但大多数场景所需的行为可以看作是一些高层动作或操作的简单组合。

以下是一组核心的高层操作：

速度跟踪：这是常态驾驶行为。我们有一个参考速度或限速，在车道内向前行驶时保持该速度。
减速至停止：如果前方有停车标志，我们需要平稳减速至停止，以保证舒适性。
保持停止：某些交通管制元素要求此行为。例如，在红灯前，我们需要保持停止直到绿灯亮起。
让行：某些管制元素也要求让行。最明显的是在让行标志处，如果有优先级更高的车辆，我们需要减速并等待，直到可以安全通过。
紧急停车：当自动驾驶汽车检测到问题时，车辆需要立即停止并靠边。

这些高层行为还可以通过导航行为（如执行变道和转弯）来增强。结合这些操作，我们可以覆盖最基本的驾驶场景。但需要注意的是，这个行为列表并不详尽，可以通过增加行为复杂性来处理更多有趣的场景。

边缘案例与规划挑战

就驾驶场景而言，我们仅仅触及了表面。许多异常情况使得自动驾驶成为一个极具挑战性的任务。

例如，假设有行人乱穿马路。这就有一个违反道路规则的参与者，从运动规划的角度看，其行为变得难以预测。另一个例子是摩托车进行车道分割（在车流中穿行），这取决于所在国家/地区可能合法也可能不合法。这种行为可能会让自动驾驶汽车感到困惑，因为自动驾驶汽车通常使用车道边界来预测道路上其他参与者的行为。

由此可见，解决运动规划问题是一项复杂的任务。如果不进行任何形式的抽象和简化，直接求解最优运动方案是难以处理的。

运动规划的层次结构

为了解决这个问题，我们将任务分解为一个层次化的优化问题。通过这样做，我们可以为每个优化问题定制适当抽象级别的输入和输出，从而能够实时进行运动规划。

在这个层次结构中，越高层意味着优化问题的抽象级别越高。

任务规划：位于层次结构顶端，专注于解决地图级别的自动驾驶任务，即导航到目的地。
行为规划：决定自动驾驶车辆应根据当前驾驶场景采取哪种行为。
局部规划：基于所选行为，计算一条无碰撞的路径和速度曲线以达到目标状态。在我们的课程中，为了提高性能，我们将此过程解耦为路径规划和速度曲线生成。
控制器跟踪：最后，计算出的运动计划将交给控制器（我们在第一门课程中设计过）进行跟踪。

每个优化问题都将有不同的目标和约束，我们将在后续课程中详细讨论。

总结

本节课中我们一起学习了自动驾驶运动规划的基础知识。

我们明确了自动驾驶运动规划的任务是从自我车辆的当前位置导航到指定目的地。我们看了一些常见的道路驾驶场景，以及道路结构和存在的障碍物如何决定场景的性质。然后，我们讨论了一些有用的驾驶行为来应对这些场景。最后，我们描述了一个运动规划优化问题的层次结构，我们将在本课程的剩余部分详细探讨。

希望本节课让你对作为自动驾驶工程师在设计运动规划器时会遇到的不同场景有了一些了解。关键要点是，虽然我们列举了许多自动驾驶所需的基本场景和行为，但如何在所有潜在驾驶场景中规划安全、鲁棒的行为，仍然存在许多开放性问题。

在下一课中，我们将探讨在解决运动规划层次结构中的优化子问题时，会遇到的一些常见约束。

006：运动规划约束

在本节课中，我们将要学习运动规划中涉及的一些最重要的约束条件。这些约束对于维持车辆的稳定性和舒适性，以及确保特定驾驶场景中所有交通参与者的安全至关重要。

具体来说，你将了解车辆的运动学和动力学模型如何约束我们的运动规划。你将学习静态和动态障碍物如何影响车辆的运动规划。最后，你将看到交通规则元素如何在运动规划过程中影响我们可用的行为选择。

让我们开始吧。

车辆运动学约束 🚲

上一节我们介绍了运动规划的基本概念，本节中我们来看看第一个约束条件：车辆运动学。在自动驾驶的运动规划中，我们通常使用自行车模型来简化自车的运动学。

选择这个模型的一个原因是，自行车在固定速度 V 下，其转向角 δ 有一个可接受的范围，这个范围对应于车辆可以遵循的一系列允许的曲率 κ。这意味着，在使用自行车模型进行运动规划时，车辆在遵循给定路径时所能执行的最大曲率是有限制的，我们称之为 κ_max。

对于自动驾驶汽车也是如此。这意味着我们规划的任何路径的曲率都必须考虑这个最大曲率限制。不幸的是，这是一种非完整约束。直观地说，这意味着约束不仅取决于机器人的状态，还取决于机器人如何到达当前状态。非完整约束减少了机器人在其工作空间中任意给定点可以移动的方向数量。通常，非完整约束会使规划问题变得更加复杂。

现在你可能想知道，曲率如何影响我们运动计划中给定路径的形状？为了让你直观地理解曲率是什么，这里有一条任意曲线。

对于路径上的每个点，我们可以根据该点处曲线在空间中的瞬时变化率，拟合一个圆。这类似于我们最初推导自行车模型时使用的瞬时旋转中心的概念。

基于图形的形状，不同点的弯曲程度似乎不同。因此，我们的直觉应该是，曲线上的不同点将具有不同的曲率。

例如，这里有一个与曲线上特定点拟合的圆。如你所见，它的半径 R 相当大。曲率的一个直观解释是，它是沿曲线该点拟合圆的半径的倒数。由于这个半径很大，该点的曲率相对较低，从图中可以看出该点的弯曲更平缓。

如果我们看这条曲线上的第二个点，可以看到它的半径比前一个点小。这反过来对应于一个曲率更高的点。从图中可以看出，这个点比前一个点的弯曲更剧烈。

虽然这种圆拟合方法作为一种几何解释很有用，但这里给出了曲率更精确的数学公式。它可以使用给定路径的 X 和 Y 分量相对于弧长的一阶和二阶导数来计算。这些弧长导数由 x'、y'、x'' 和 y'' 表示。

车辆动力学约束 ⚙️

接下来我们将讨论的约束源于车辆动力学。这些动态约束侧重于保持汽车处于稳定、安全的状态。

第一个动力学约束是由所谓的汽车摩擦椭圆施加的。如果你还记得，我们在第一门课程中讨论过轮胎滑移和摩擦椭圆。摩擦椭圆表示汽车轮胎与路面之间可以产生的摩擦力的最大大小。如果汽车发动机施加的力超过轮胎的摩擦力，轮胎将在路面上打滑。车辆的转向功能依赖于轮胎抓地力，因此为了保持控制和稳定性，汽车必须保持在摩擦圆内。

归根结底，这归结为横向和纵向加速度约束。然而，通常加速度约束远高于车内乘客在物理上感到舒适的水平。因此，在非紧急情况下，我们通常关注加速度的舒适矩形。

这些值将横向和纵向加速度各自限制在一个舒适范围内，分别表示在纵向加速度 a_long 轴和横向加速度 a_lat 轴上。如下图所示，蓝色的舒适矩形完全位于绿色的摩擦椭圆内部。这导致对规划可行的加速度约束比摩擦椭圆的要求更严格。

速度约束 🏎️

从横向加速度约束以及路径的曲率，我们现在间接地约束了汽车的速度。汽车的速度 V 是汽车横向加速度 a_lat 以及路径瞬时转弯半径 R 的函数。

回想一下，沿路径的瞬时曲率 κ 是汽车转弯半径的倒数。如果我们现在组合这些方程，可以看到速度的平方受到最大横向加速度 a_lat_max（一个常数）以及路径曲率 κ（沿路径每个点都在变化）的约束。

因此，很明显，当我们为自动驾驶车辆生成速度曲线时，必须考虑路径的曲率以及车辆的最大横向加速度。

静态障碍物约束 🚧

静态障碍物也对我们的路径规划过程施加约束。由于静态障碍物（如停放的汽车或施工路锥）具有不随时间变化的固定位置，它们通常通过阻挡自车工作空间的某些部分来建模。

这个概念将在本课程的第2模块中更详细地介绍，届时我们将讨论占据栅格地图。本质上，静态障碍物限制了汽车在其路径上可以占据的位置。

有多种方法可以执行静态碰撞检查。一种方法是检查车辆沿给定路径行驶时的扫掠区域。这个扫掠区域是自车在遍历路径时，其车身占据的所有位置的并集。如果扫掠区域与静态障碍物重叠，则该路径不可行。

另一种快速碰撞检查的选择是用一组圆来近似汽车的车身，并计算汽车沿其路径移动时这些圆的位置。如果任何圆内存在静态障碍物，则该路径被视为不可行。这些圆的并集通常比车辆车身大，确保了保守的近似。我们将在未来的课程中更详细地讨论这一点。

动态障碍物约束 🚶‍♂️🚗

另一方面，动态障碍物对运动规划问题提出了一些最具挑战性的约束。不同类别的动态障碍物，如汽车、自行车和行人，都将具有不同的行为和运动模型。基于这些其他交通参与者的行为来约束我们的运动计划通常涉及预测，而预测存在不确定性。

然而，如果我们采取保守的方法，将自己约束在所有交通参与者的所有可能行为上，我们的运动规划问题很快就会变得过度约束，无法有意义地解决。我们在保守主义的安全性和前进所需的激进性之间取得平衡的程度，是自动驾驶研究的一个活跃领域。

举一个简单的例子，动态障碍物会约束我们运动的一个场景是在十字路口。如果两辆车正交地进入十字路口，那么我们就有一个潜在的碰撞场景。检查是否会发生碰撞的一种方法是跟踪自车行驶方向与从自车位置到其他交通参与者位置的向量所形成的角度。如果这个角度随时间推移保持不变，那么自车将与其他交通参与者发生碰撞，我们的运动规划器需要减速以防止这种情况发生。

因此，动态障碍物迫使我们根据障碍物在驾驶场景中的进展来修改我们的行为。我们在上一课讨论的另一个例子是，当自车前方存在一辆前导车辆时。这辆前导车辆对自车的纵向速度施加了上限约束。因为如果我们在同一条车道上超过他们的速度，最终会发生碰撞。

如你所见，动态障碍物将同时约束我们的行为规划过程（我们在此做出机动决策）和我们的局部规划过程（它将影响我们的速度曲线规划）。我们将在后续模块中进一步详细讨论这一点。

交通规则与监管元素约束 🚦

我们将讨论的最后一个约束包括道路规则，以及自车工作空间中存在的监管元素。虽然道路规则为规划问题提供了一些约束，但它们也帮助我们做出关于环境中其他交通参与者行为的明智决策。例如，对向交通极有可能保持在它的车道内，而不会试图与我们的自车正面碰撞。这有助于在尝试预测其他交通参与者行为时减少搜索空间。

道路规则施加的最常见约束之一是车道约束。简单来说，车道约束是为了防止我们的运动计划让自车离开其当前车道，除非这样做是合法的。车道约束也告知自车在何处执行转弯操作是安全的。

我们还需要遵守其他软性的道路规则，例如保持自车与我们车道中前导车辆之间的时间间隔。时间间隔是指自车以当前速度行驶到达前导车辆当前位置所需的时间量。保持足够的时间间隔有助于确保运动规划期间的安全，为自车提供充足的反应时间来应对工作空间中的事件。

工作空间中的监管元素也会影响我们的驾驶行为。自车显然需要遵守红灯和停车标志以确保安全，但也需要了解不同区域的限速以及其他动态监管元素，例如施工场地带来的限制。

总的来说，监管元素将对我们执行运动规划时可用的行为产生重大影响，并将在第5模块中进一步详细讨论。

总结 📝

现在我们已经讨论了许多自动驾驶运动规划的约束，让我们总结一下到目前为止所学的内容。

在本视频中，我们首先回顾了自行车模型，并研究了模型的运动学和动力学以及路径曲率如何约束我们的运动规划问题。接下来，我们研究了静态障碍物如何限制我们汽车可以安全占据的位置，从而限制了我们的可行运动规划工作空间，以及动态障碍物如何影响自车可用的机动操作及其速度曲线。最后，我们讨论了监管元素的作用以及它们如何影响我们的驾驶行为。

希望本节课让你对自动驾驶运动规划问题的一些约束有了一些了解。在下一个视频中，我们将深入探讨用于解决运动规划问题的一些优化目标。下次见。

007：自动驾驶目标函数

在本节课中，我们将探讨自动驾驶运动规划问题中最常用的一些目标函数。目标函数为我们提供了一种评估当前运动计划优劣的方式，并允许我们优化运动计划，使其具备理想的特性。

完成本课后，你将能够列举一些可用于自动驾驶运动规划的有用目标函数，并理解每个目标函数为运动规划过程带来的益处。

效率目标函数

上一节我们介绍了目标函数的基本概念，本节中我们来看看与行驶效率相关的目标函数。

在进行路径规划时，最简单直观的目标是希望尽可能高效地到达目的地。在路径规划中，这通常转化为最小化规划路径的弧长。

弧长直观上是指车辆沿路径行驶所累积的总距离。在许多情况下，路径会以弧长为参数进行参数化。因此，这可以表述为一个与弧长成比例的惩罚项。在其他情况下，我们需要通过数值积分来计算路径的弧长。

通用的弧长积分公式如下：

S_F = ∫_{x_I}^{x_F} √(1 + (dy/dx)^2) dx

其中，S_F 是累积的总弧长，x_I 是路径起点的x坐标，x_F 是路径终点的x坐标。通过最小化弧长，我们计算的是从当前位置到目标位置的最短路径。

当优化速度曲线时，我们通常希望最小化到达目的地所需的时间。从弧长的角度看，这等同于沿路径对纵向速度的倒数进行积分，积分变量为弧长。直观上，这相当于将总距离除以沿曲线每个点的行驶速度，但由于速度沿路径变化，我们将其表述为一个积分。

该积分公式如下：

T_F = ∫_{0}^{S_F} (1 / v(s)) ds

其中，T_F 是速度曲线的总时间。通过最小化这个积分，我们可以在遵循给定规划路径的同时，最小化到达目的地的时间。

以下是两个路径示例：

路径A：弧长较长，效率较低。
路径B：弧长较短，效率较高，目标函数值更低，更符合驾驶任务需求。

跟踪参考目标函数

在自动驾驶路径规划中，我们可能会得到一个希望遵循的参考路径。这通常由地图模块提供，例如道路的中心线或路口转弯的必需路径。

为了确保尽可能紧密地跟随参考路径，我们引入一个称为差值积分 的项。本质上，它惩罚我们计算出的路径与输入的参考路径 x_ref 之间的偏差。

路径跟踪的差值积分公式如下：

J_path = ∫ (x(s) - x_ref(s))^2 ds

类似地，在进行速度曲线优化时，我们通常希望最小化整个路径上我们的速度与某个参考速度 v_ref（可能是限速或其他方式确定的速度）之间的差异。

速度跟踪的差值积分公式如下：

J_speed = ∫ (v(s) - v_ref(s))^2 ds

虽然这可以表述为绝对差值惩罚，但通常我们希望对超速的惩罚比对低于限速的惩罚更严厉。因此，我们可以在目标函数中添加一个铰链损失 项。

铰链损失项公式如下：

J_hinge = ∫ max(0, v(s) - v_limit(s))^2 ds

本质上，只有当速度曲线超过限速（即我们的速度与限速之差为正）时，该项才生效；否则该项为0。这允许我们更严厉地惩罚超速行为，同时仍鼓励自动驾驶车辆达到所需的参考速度。

舒适性目标函数

接下来，我们探讨与乘坐舒适性相关的目标函数。

回顾上一课，为了保持稳定性和舒适性，我们限制了最大加速度大小。当在速度曲线的目标函数中优化舒适性时，我们将重点转向最小化轨迹上的加加速度。

加加速度 是加速度随时间的变化率，即位置的三阶导数。车辆轨迹上的加加速度极大地影响车内乘客的舒适度。因此，在规划速度曲线时，我们希望尽可能减小加加速度绝对值的累积量。

加加速度惩罚项公式如下：

J_jerk = ∫ (da/dt)^2 dt

让我们看两个不同速度曲线的例子：

左侧线性斜坡曲线：在曲线开始和结束时加加速度最大，在图中表现为开始和结束时的巨大尖峰。这对乘客来说可能相当不舒服。
右侧双二次曲线：没有加加速度尖峰，将为乘客带来更平稳的乘坐体验。

由于舒适性上的这种差异，尽可能选择能减少乘客感受到的加加速度的曲线非常重要。

在规划路径时，我们必须注意路径结构也会影响速度曲线的舒适性。回顾上一课，高曲率的路径会限制沿路径的最大速度，以保持在车辆的摩擦椭圆内。

为了确保避免路径上的高曲率点，我们需要对大的绝对曲率值制定某种惩罚。一个常用的表示这种惩罚的目标函数称为路径的弯曲能量。

弯曲能量公式如下：

J_curvature = ∫ κ(s)^2 ds

其中，κ 表示曲率。这个目标函数使曲率沿路径分布更均匀，防止路径上任何一点的曲率达到过高的总值。

以下是两个路径示例：

左侧路径：左转路径在到达目标状态前有一个相当急的弯道。根据我们的弯曲能量目标，这将受到高度惩罚，因为路径上存在一个曲率峰值点。如果为此路径规划速度曲线，我们到达曲线弯道时需要大幅减速，否则将牺牲乘客舒适度甚至车辆稳定性。
右侧路径：左转路径的曲率分布更广，整体曲率更低。这将带来更平稳、更舒适的转弯，并允许我们在优化速度曲线时，在保持在舒适矩形内的同时，维持合理的速度。

总结

本节课中我们一起学习了多种用于运动规划的目标函数。

我们首先讨论了与效率相关的目标函数，以及它们如何减少路径长度和通行时间。接着，我们探讨了如何鼓励轨迹跟踪参考路径和速度曲线。最后，我们学习了如何通过惩罚高路径曲率和加加速度来提高给定轨迹的舒适性。

现在，你应该对优化路径和轨迹时需要考虑的一些目标函数类型有了基本了解。在设计一个总体的目标函数时，这些项中的每一项通常需要以加权和的形式组合成一个函数。你选择的具体权重组合是一个工程决策，需要根据特定应用进行微调。

当然，对于路径和轨迹规划的优化目标这个丰富的话题，我们只是浅尝辄止。随着你成长为一名自动驾驶工程师，相信你自己也能想出有趣且有用的目标函数。

在下一个视频中，我们将讨论如何将运动规划问题解耦为更简单、更小的子问题，并概述解决每个子问题的不同方法。下节课见。

008：分层运动规划详解

在本节课中，我们将详细探讨如何将复杂的整体运动规划问题分解为更小的子问题。我们将深入了解任务规划、行为规划和局部规划这三个层次的具体范围、目标以及常见的解决方法。

概述

到目前为止，我们已经描述了规划问题的概貌，介绍了用于解决复杂规划问题的分层结构，并阐述了使规划变得困难的约束与目标。本节课程将更详细地讨论如何将整体规划问题分解为更小的子问题。通过本视频的学习，你将理解任务规划、行为规划和局部规划子问题的范畴，能够列举解决每个问题的一些方法，并理解为何这种分层结构被广泛应用于自动驾驶系统，以实现对环境运动计划的逐步细化。

规划层次回顾

在模块的第一课中，我们介绍了用于自动驾驶运动规划的分层结构。我们描述了规划层次中的每一级都定义了其自身的优化问题，每一级在相关目标函数和约束方面都具有特定的抽象程度。

任务规划 是最高级别的优化问题，专注于从车辆当前位置到给定目的地的地图级导航。
行为规划 则专注于当前的驾驶场景，根据工作空间中的其他智能体和道路规则来决定执行何种操作。
最后是局部规划，它将专注于以解耦的方式生成运动学上可行且无碰撞的路径，以及舒适的纵向速度曲线。

随后，这些规划结果将输出到我们在第一门课程中讨论过的车辆控制器。

通过将规划问题分解为子问题，我们限制了运动规划过程每个部分所需的知识和计算量，从而允许进行更高效的计算。然而，这需要权衡取舍，因为当我们分解问题时，每个子问题都会丢失一些领域信息。与同时考虑规划过程所有方面的运动计划相比，我们最终得到的是一个次优的运动计划。对于自动驾驶而言，实时运行运动规划器至关重要，因此这种权衡几乎总是值得的。一些运动规划方法在不同程度上整合了不同的子问题，但在本课中，我们将重点讨论这种特定的分层分解。

接下来，让我们详细探讨这些步骤。

任务规划

如前所述，我们分层运动规划器的第一个也是最高级别的步骤是任务规划。

任务规划专注于从自车当前位置到所需目的地的地图级导航。由于任务规划的尺度通常在公里级别，我们需要高度的抽象来使问题易于处理。从这个意义上说，我们通常会忽略运动规划问题的某些方面，例如障碍物和交通规则元素，转而关注问题的宏观方面，例如交通流量和道路连接。通过这样做，我们可以将问题简化到能够在大的空间尺度上规划所需驾驶任务的程度。

这里展示了一个例子，我们在加利福尼亚州伯克利市的地图上规划两点之间的路径，考虑了地图上的各种交叉路口和道路连接，以及每条道路的方向性。这样，我们就能计算出道路网络中到达目的地的最短路径。

这可以通过多种方式实现。如果我们关注道路网络中道路的空间长度，我们可以从道路网络构建一个图，然后使用 Dijkstra算法 或 A*搜索 来找到最短路径。如果我们将最小化到达目的地的时间作为目标，问题会变得更复杂，但仍然可以使用相同的算法解决。我们将在模块3中更详细地讨论这些方法。

行为规划

我们分层规划器的下一步是行为规划。什么是行为规划？

行为规划是运动规划的一部分，专注于遵循道路规则所需的高级决策，并识别在给定驾驶场景中哪些操作是安全的。我们在最初的课程中讨论了许多这样的场景。例如，回想一下我们讨论过的在交叉路口转弯的场景。

假设自车已进入交叉路口准备左转，并且交通灯即将变绿。行为规划器应向路径规划器和速度生成器明确，只有在自车位置与交叉路口的对向车流和行人之间有足够的时间间隙时，继续行驶才是安全的。根据我们在第一课中讨论的操作分类，行为规划器应向运动规划的其余部分输出一个“让行”操作，这将确保它们只在确认安全后才继续行驶。

当然，我们的分类法并非通用标准。在决定自动驾驶汽车应表现出何种类型的操作和行为范围时，存在许多可能性。

现在，你可能想知道，行为规划器应该如何接收关于自动驾驶汽车周围环境的所有信息，并以高效的方式计算它应该执行什么操作。这是一个活跃的研究领域，目前还没有公认的最佳方法来实现这一点。总的来说，解决行为规划问题似乎有三种主流架构。

以下是三种主要的行为规划方法：

基于有限状态机的方法：有限状态机的核心概念是，它们由代表自车所需操作的状态和决定状态如何根据输入演变的转换组成。这些输入可能包括车辆位置、交通信号灯转换或当前驾驶场景中任何其他感兴趣的元素。例如，可以想象一个处理停车标志的状态机，它可能包含诸如“减速至停车”、“停车”和“跟踪速度”等状态。当汽车遇到停车标志时，它会首先进入“减速至停车”状态，此时行为规划器会发出减速信号。一旦在停车位置停下，行为规划器将在“停车”状态保持设定的时间。一旦时间超过且交叉路口畅通，行为规划器将进入“跟踪速度”状态，这意味着可以安全地继续行驶。我们将在本课程后面的一个模块中更详细地讨论这个例子。需要注意的是，在决定有限状态机每一步进行哪种转换时，我们完全专注于当前状态和有限状态机的输入。本质上，这意味着有限状态机是无记忆的，其过去的状态不会影响当前的转换或输出。这是一个有用的特性，使得有限状态机易于实现，但对于动作序列确实很重要的系统，可能会导致许多相似的状态。
基于规则的系统：本质上，这类系统由规则层次结构组成，其中层次结构代表了每条规则的相对重要性。每条规则可能对应一条道路规则，例如在红灯时停车，也可能对应一个驾驶最佳实践，例如保持自车与前车之间两秒的间隙。自车及其周围环境的状态将以逻辑谓词的形式输入到基于规则的系统中。然后，这些谓词将根据每条规则进行评估，基于规则优先级，与最高优先级规则对应的操作将输出给运动规划器的其余部分。例如，假设我们有以下两条规则：第一条规则检查前方是否有绿灯，并发出继续行驶的决策；第二条规则检查自车车道中是否有行人，并发出执行紧急停车操作的决策。在我们的系统中，为前方的行人停车比在绿灯时通过具有更高的优先级。现在，假设我们正在行驶，观察到前方交叉路口有绿灯，同时有一个行人正在违章横穿马路。在这种情况下，两条规则都会被触发，但由于紧急停车的优先级更高，基于规则的系统将输出该操作。可以想象，确保基于规则的系统在逻辑上一致既重要又具有挑战性，否则它们可能会表现出不稳定的行为。
基于机器学习的方法：特别是，强化学习是这类方法中一个有趣的例子。强化学习是确定一个最优决策策略的过程，该策略最大化某个奖励函数 R。这个奖励函数评估给定时间步长内一系列动作的质量，对未来状态的折扣比当前状态更重。强化学习的过程要求智能体在通常由模拟提供的环境中执行动作。然后，根据其与环境的交互给予奖励，从而使其能够通过连续的交互收敛到最优策略。这类行为规划方法是一个快速扩展的研究领域，遗憾的是，深入探讨超出了本课程的范围。

局部规划

我们分层规划器中下一个也是最后一个模块是局部规划。如前所述，局部规划的目标是为自车生成运动学上可行且无碰撞的路径，以及舒适安全的纵向速度曲线。

我们将局部规划问题分解为两个子问题：路径规划和速度曲线生成。开发一个好的路径规划算法的关键创新在于减少优化的搜索空间。

路径规划器主要有三类：基于采样的规划器、变分规划器和栅格规划器。

以下是三类主要的路径规划器：

基于采样的规划器：这类规划器均匀地随机采样汽车的控制输入，以生成汽车可能行驶的潜在路径。最具代表性的基于采样的算法之一是快速探索随机树及其变种。这些算法通过在随机采样的位置生成点，并从树中最近的点规划一条到达该点的路径来构建路径树的分支。如果该路径与任何静态障碍物无碰撞，则该路径被添加到树中。这棵树能快速探索工作空间，生成许多潜在路径。当到达目标区域时，终止于该区域的路径将被返回。基于采样的算法通常速度极快，但代价是在运行周期较短时可能生成质量较差、不稳定的路径。
变分规划器：这类规划器依赖于变分法来优化轨迹函数，该函数根据某个考虑障碍物和机器人动力学的成本泛函，将时间点映射到工作空间中的位置。变分规划器通常是轨迹规划器，这意味着它们将路径规划和速度规划结合到一个步骤中，因此不必解耦路径规划和速度规划问题。例如，假设我们从一个给定的空间路径离散采样开始。起初，它与一个障碍物发生碰撞。变分规划器将迭代地移动离散点，使其相对于机器人的动力学是平滑的，同时保持无碰撞。最终，轨迹将既无碰撞，又相对于机器人的动力学更加平滑。然而，变分方法通常速度较慢，更复杂，并且其收敛到可行路径对初始条件敏感。变分方法的一个例子是Chomp算法。变分规划器超出了本课程的范围，但对于感兴趣的学习者，Chomp算法提供了一个极好的进一步探索的例子，补充材料中的链接包含了相关信息。
栅格规划器：这类规划器通过限制自车在工作空间中任何一点可以采取的动作来约束搜索空间。这组动作被称为栅格规划器的控制集。这个控制集与工作空间的离散化相结合，隐式地定义了一个图。然后可以使用图搜索算法（如Dijkstra或A*）搜索该图，从而实现路径的快速计算。障碍物可以将穿过它们的边设置为无限成本，因此图搜索也允许我们执行碰撞检查。虽然栅格规划器通常速度相当快，但路径的质量对所选控制集很敏感。栅格规划器的一个常见变体称为共形栅格规划器，其中目标点被选在汽车前方一定距离处，相对于道路方向横向偏移。然后对到达这些点的每一条路径进行优化。最能满足某些目标同时保持无碰撞的路径被选为要执行的路径。在之前的视频中，我们概述了一些可以影响路径规划过程的目标，例如弧长和弯曲能量。

我们局部规划的第二部分是速度曲线生成。速度曲线生成通常被设置为一个约束优化问题。通常，我们会结合上一视频中描述的许多速度曲线目标，例如最小化加加速度或最小化与期望参考值的偏差。我们还在第二课中讨论了一些重要的约束，其中之一是舒适加速度矩形。这绝不是目标或约束的详尽列表，在你作为自动驾驶工程师的职业生涯中会遇到更多。一旦目标和约束被形式化，就变成了如何高效解决问题的事情。一种方法是计算优化域和目标的凸近似，这有助于确保我们的优化器不会陷入局部最小值。

总结

现在我们已经讨论了分层规划器中的每个级别，让我们总结一下本课涵盖的内容。

我们首先介绍了任务规划，以及我们的规划器如何在地图中导航以到达目的地。接着，我们讨论了行为规划，我们需要根据当前情况选择驾驶行为。最后，我们讨论了局部规划，我们将其分为路径规划和速度曲线生成。我们给出了几种路径规划算法的例子，并讨论了如何为给定的规划路径和参考速度优化速度曲线。

恭喜你，你已经完成了我们自动驾驶专项课程第四门课程的第一个模块。希望你现在对设计分层运动规划器时的不同方法有了清晰的认识。在本课程的其余部分，我们将介绍解决运动规划问题每个级别所需的工具。但在那之前，我们将在下一个模块中学习如何构建占据栅格地图以进行有效的碰撞避免检测。下节课见。

009：占据栅格地图 🗺️

在本节课中，我们将学习环境地图创建模块。我们将详细讨论两种关键的地图：占据栅格地图和高清路线图。它们对于自动驾驶汽车的运动规划任务至关重要。

我们将从详细定义占据栅格地图开始，并理解如何在自动驾驶汽车上创建它。接着，我们将探讨用于创建地图的测量数据中固有的噪声问题。最后，你将学习如何通过贝叶斯更新来处理这些噪声，以完善对栅格占据状态的“信念”。

占据栅格地图的定义

上一节我们介绍了本模块的学习目标，本节中我们来看看占据栅格地图的具体定义。

占据栅格是一个围绕当前自车位置进行离散化的网格。这种离散化可以在二维或三维空间中进行，我们讨论的方法也适用于两者。然而，为了简化解释并降低本模块的计算需求，我们将只关注二维版本。

占据栅格的每个方格表示该网格位置是否存在静态或静止物体。如果存在，则该网格位置被分类为“被占据”。会被归类为占据网格单元的静态物体示例包括树木、建筑物、路标和灯杆。在自动驾驶领域，一些你可能不认为是障碍物的物体也应被分类为占据空间，包括所有不可行驶的表面，如草坪或人行道。

每个由 m_i 表示的栅格方格映射到一个二进制值，其中 1 表示该方格被静态物体占据，0 表示未被占据。在这张地图中，我们可以看到有树木和草地覆盖的方格被标记为 1，而道路被标记为 0。生成的地图如下所示：所有被占据的方格显示为紫色，其余对应于可行驶表面的区域保持透明。

创建占据栅格的假设

为了创建准确的占据栅格，需要做出以下一组假设。

以下是创建占据栅格地图所依赖的三个核心假设：

用于创建此占据栅格的环境测量数据仅对应于静态物体。这意味着所有动态或移动物体在传感器数据用于占据栅格建图之前必须被移除。
每个栅格单元独立于所有其他单元。做出此假设是为了简化创建占据栅格所需的更新函数。
在每个时间步，车辆相对于占据地图的当前状态是确切已知的。

传感器数据与预处理

在自动驾驶汽车领域，最常用于观测汽车与世界当前状态之间距离的传感器是激光雷达。快速回顾一下，激光雷达传感器使用光脉冲来测量到汽车周围所有物体的距离，并在其视野范围内返回一个测量点云。

在将激光雷达数据用于构建占据栅格之前，需要过滤掉其中的几个组成部分。

以下是处理激光雷达数据的三个关键步骤：

过滤所有构成地平面的激光雷达点。在本例中，地平面是自动驾驶汽车可以安全行驶的路面。
过滤掉出现在车辆最高点以上的所有点。这组激光雷达点可以忽略，因为它们不会阻碍自动驾驶汽车的行驶。
移除激光雷达捕获的所有非静态物体。这包括所有车辆、行人、自行车和动物。

一旦完成所有激光雷达数据的过滤，就需要将3D激光雷达数据投影到2D平面上，以用于构建我们的占据栅格。在本模块后面的视频中，将介绍如何通过过滤和压缩激光雷达数据来为自动驾驶汽车创建准确的占据栅格。

经过过滤和压缩的激光雷达数据，类似于来自高清晰度2D测距传感器的数据，能准确测量车辆周围平面上到所有静态物体的距离。

处理噪声与概率化占据栅格

然而，即使在所有过滤完成后，仍然存在一个问题：由于数据使用的过滤方法、手头数据的复杂性以及最主要的环境和传感器噪声，地图仍然存在很大的不确定性。

为了处理这种噪声，占据栅格将被修改为概率形式。现在，每个单元 i 存储的不再是表示占据的二进制值，而是一个介于 0 和 1 之间的概率，对应于该方格被占据的确定程度。存储的值越高，该方格被占据的概率就越高。

为了使用这组概率，占据栅格现在可以表示为一个信念地图，用术语 bel 或 b 表示。为了保持符号简单，m_i 表示占据栅格中的一个方格，其信念可以根据测量值 y 和车辆位置 x 来构建。

m_i 的信念等于给定为该单元位置收集的传感器测量值，当前单元 m_i 被占据的概率。要转换回二进制地图，可以设定一个阈值，当某个信念值足够高时，即可确信其被分类为“被占据”。任何低于设定阈值的值都将被设置为“空闲”。

例如，左侧图中的被占据方格概率为 0.94，这将其分类为被占据。另一方面，街道上的方格被占据的概率仅为 0.12，因此将被分类为空闲位置。

贝叶斯更新与信念融合

可以将从时间 1 到时间 T 的多组测量结果结合起来，以获得更准确的占据信念。事实上，我们可以以递归的方式更新信念，以便在每个时间步 T，我们使用从时间 1 开始的所有先前信息来定义我们的信念。

在时间 T 对地图单元 m_i 的信念定义为，给定从时间 1 到 T 的所有测量值和车辆位置，m_i 被占据的概率。

为了将多个测量结果合并到单个信念地图中，可以应用贝叶斯定理。在占据栅格的情况下，我们得到一个采用以下形式的贝叶斯更新步骤：

bel_t(m_i) = η * p(y_t | m_i) * bel_{t-1}(m_i)

其中，给定 m_i 的 y_t 的分布 p(y_t | m_i) 是在单元 m_i 被占据的情况下获得特定测量值的概率。这被称为测量模型，你在课程2中已经详细学习过。

时间 t-1 时 m_i 的信念对应于从前一个时间步存储在我们的占据栅格中的先验信念。我们依赖于马尔可夫假设，即估计单元占据所需的所有必要信息都在每个时间步的信念地图中捕获，因此在单元更新方程中不需要考虑更早的历史。

最后，此处的 η 对应于应用于信念地图的归一化常数。这是为了缩放结果以确保其仍然是一个概率分布所必需的。

实例演示

让我们看一个实际运行中的占据栅格。在这个视频中，我们将跟随自动驾驶汽车驶出车道并沿道路行驶，同时占据栅格正在实时更新。颜色较浅的栅格单元表示空闲方格，而黑色的栅格单元表示被占据的方格。我们还可以看到红色的原始激光雷达数据和橙色的过滤输出。注意地图是如何跟踪车辆运动的，这是使用与我们在课程2中介绍的相同技术进行估计的。

在此视频中，将一个物体分类为障碍物所需的信念阈值设置得非常高。因此，只有大型静态物体被识别为被占据。降低此阈值将导致更多单元被标记为被占据，但也会导致地图噪声更大。

总结

本节课中我们一起学习了占据栅格地图的基础知识。

总结一下，在本视频中，你学习了占据栅格地图的基本定义，并看到了激光雷达传感器数据如何被过滤和压缩以创建占据栅格。然后，你学习了如何将占据栅格表示为信念地图，并应用贝叶斯更新来在占据栅格中整合新的测量结果。

在下一个视频中，我们将讨论信念空间地图表示中的一些数值问题，并引入对数几率函数作为此问题的解决方案。我们还将研究一个逆测量模型，该模型是使用对数几率函数从2D激光雷达数据创建占据栅格地图所必需的。下节课见。

010：占据栅格更新与对数几率表示 🧮

在本节课中，我们将学习如何高效且稳定地更新占据栅格地图。我们将首先分析直接使用贝叶斯概率更新存在的问题，然后引入对数几率表示法来解决这些问题，并最终推导出用于更新占据栅格的对数几率贝叶斯更新公式。

贝叶斯更新存在的问题

上一节我们介绍了如何应用贝叶斯定理，将先前的占据信念图与当前的传感器测量信息相结合，以在每个时间步生成高精度的占据栅格。这通过以下公式实现：

P(M_i | y_{1:t}) = η * P(y_t | M_i) * P(M_i | y_{1:t-1})

其中，η 是归一化常数，P(y_t | M_i) 是当前测量值，P(M_i | y_{1:t-1}) 是先前的信念图。

然而，这种简单的贝叶斯更新方法存在一个问题。为了演示这个问题，我们来看一个例子：一个原本空闲的栅格单元收到了一个新的“空闲”测量值。

假设一个单元先前的占据信念值很低，为 6.38 × 10^{-4}。新的测量结果概率也很低，为 1.2 × 10^{-5}。这意味着更新后的信念值将非常低，约为 8.0 × 10^{-7}。可以看到，所有这些概率值都非常接近零。

在计算机上对浮点数进行乘法运算时，当乘数是很小的数字时，会导致显著的舍入误差，进而导致概率估计的不稳定。此外，概率乘法本身也是一种低效的信念更新方式。因此，我们直接应用贝叶斯规则来更新占据单元信念的方法并不理想。

解决方案：对数几率表示法

不过，存在一个解决方案。我们不再存储范围在0到1之间的信念值，而是可以使用 Logit函数 将我们的信念转换为 对数几率 概率。

我们首先在第三门课程中见过这个Logit函数。对数几率表示法将单元格的值域映射到负无穷到正无穷的整个实数轴，从而避免了数字接近零的问题。

Logit函数取概率 P 与 1-P 之比的自然对数。因此，它将0到1的概率值映射到整个实数轴。

logit(p) = ln( p / (1-p) )

也可以从对数几率域转换回概率域，这通过以下公式实现：

p = e^{logit(p)} / (1 + e^{logit(p)})

现在我们有了单元格概率的另一种表示方法，接下来看看这如何影响我们的贝叶斯更新方程。

对数几率贝叶斯更新公式推导

我们将从对 P(M_i | y_{1:t}) 应用贝叶斯规则开始推导，其中 M_i 是位置 i 的当前占据栅格地图单元，y_{1:t} 是从时间1到时间 t 对该单元的传感器测量值。

写出完整的贝叶斯更新方程以将最新测量值纳入我们的占据信念，我们得到以下等式。分子中的第一项是在给定单元格状态和所有先前测量的情况下获得测量值 y_t 的概率。第二项是给定直到时间 t-1 的所有测量值，单元格被占据的概率。分母是在给定直到时间 t-1 的所有先前测量的情况下获得测量值 y_t 的概率。

接下来，我们将应用马尔可夫假设，该假设确保如果地图状态 M_i 已知，则当前测量值独立于先前的测量值。

然后，我们再次应用贝叶斯规则来扩展测量模型 P(y | M)。这导致当前测量值下地图单元 M_i 的概率乘以获得该测量值的概率，再除以栅格单元 M_i 被占据的概率。

将这个扩展的测量模型代入主贝叶斯推理方程后，我们得到分子中有三项，分母中有两项。我们现在将这个扩展形式通过Logit函数，然后开始简化结果表达式。

经过一系列代数运算和同类项抵消，我们最终得到一个简化的几率比表达式，其中分子和分母各只剩下三项。这个方程可以更好地视为三个比率：概率与其补概率（1-概率）之比。

第一个比率是给定测量值 y 时单元格被占据的概率。第二个是单元格未被占据的概率。第三个是给定直到时间 t-1 的所有测量值时单元格被占据的先验信念。

最后，我们对这一系列概率比取对数，得到三个Logit函数的加法。这就是我们最终的更新方程，它具有一个很好的特性：当需要新测量值时，只需要进行加法运算。

这三个项可以用一个方便的简写表示：

L_{t-1, i} 是时间 t-1 时单元格 i 被占据的信念的logit值。
L_{0, i} 是时间 0 时的初始信念。
L_{t, i} 是时间 t 时的信念。

我们现在得到了用于占据栅格地图贝叶斯推理的便捷的对数几率更新规则：

L_{t, i} = logit(P(M_i | y_t)) + L_{t-1, i} - L_{0, i}

它由三项组成，在每个时间步根据最新的测量数据组合而成：

逆测量模型：logit(P(M_i | y_t)) 是使用新测量信息形成的logit值。概率分布 P(M_i | y_t) 被称为逆测量模型，我们将在下一个视频中学习如何为激光雷达数据构建它。
先验信念：L_{t-1, i} 是单元格 i 在时间 t-1 的先前信念。
初始信念：L_{0, i} 是同一单元格在时间零的初始信念。初始信念代表栅格单元被占据的基线信念，通常统一设置为50%，因为我们不期望有能改进此值的先验信息。它在每个时间步都出现在这个方程中，这有点令人惊讶，但这只是我们本节推导的结果，它调整了前两项的加法，以确保更新后的信念与对数几率形式一致。

对数几率表示法的优势

与直接更新概率相比，贝叶斯对数几率表示法有两个显著优势：

数值稳定：由于logit将0-1的概率映射到整个实数轴，更新过程数值稳定。
计算高效：它完全依赖加法来完成占据栅格的所有更新，计算效率显著更高。

本节总结

本节课中我们一起学习了：

我们首先指出了使用直接的贝叶斯概率更新来存储和更新占据栅格时存在的一些问题。
然后，我们看到了如何通过采用概率空间的对数几率表示法来解决这个问题。
最后，我们看到了贝叶斯对数几率更新是如何从贝叶斯规则推导出来的。

在下一个视频中，我们将学习如何应用贝叶斯对数几率更新，并构建所需的逆测量模型，以使用过滤后的激光雷达数据来更新占据栅格。

011：构建占据栅格（第二部分）🚗

在本节课中，我们将学习如何为激光雷达数据定义并构建一个逆测量模型，这是执行占据栅格贝叶斯更新的关键步骤。我们还将探讨如何使用光线追踪算法来显著提升计算效率。

概述

上一节我们介绍了占据栅格的基本概念和贝叶斯更新规则。本节中，我们来看看如何具体实现更新所需的核心组件——逆测量模型。

逆测量模型的定义

为了执行占据栅格的贝叶斯更新，我们需要计算 P(m_i | y_t)，即在给定当前激光雷达测量值 y_t 的情况下，栅格单元 m_i 被占据的概率。然而，我们之前见过的测量模型形式是 P(y_t | m_i)，即给定单元被占据时获得特定测量的概率。

因此，对于占据栅格更新，我们需要将这个测量模型“翻转”过来。也就是说，我们必须为每一个新接收到的测量值，构建一个能计算出 P(m_i | y_t) 的逆测量模型。

激光雷达数据表示

为简化起见，本课程中我们将激光雷达数据视为2D距离数据，包含两组信息：

每个激光束发射的方位角。
激光束在碰到物体前传播的距离。

以下是数据的数学表示：

方位角：由一组在传感器最小和最大视角范围内均匀分布的角度 φ 表示。我们假设方位角覆盖完整的360度。
距离：每个光束传播的距离，记为 r_1 到 r_J，在最小距离 r_min（通常为0）和最大距离 r_max 之间测量。如果光束未返回回波，则表示该方向上探测范围内没有物体。

我们在此做一个假设：一次完整的激光雷达扫描（传感器旋转一周的测量值）是在同一时刻获得的。对于高速移动的车辆，这并非精确模型，实际中需要使用状态估计进行运动校正。

构建临时测量栅格

逆测量模型按以下步骤工作。假设车辆在如下环境中使用激光雷达收集不同方位角的距离测量值。

我们构建一个临时的占据栅格，其范围覆盖所有方向上光束的最大探测距离。该测量栅格的坐标系使用占据栅格地图的坐标系。我们定义传感器在栅格坐标系中的位置 (x1_t, x2_t) 和朝向 x3_t。

基于当前的激光雷达数据，我们可以在测量栅格上定义三个不同的区域：

无信息区：所有激光束均未到达的区域。当前扫描未提供此区域的新信息。
低概率区：所有激光束都穿过但未碰到任何物体的区域。
高概率区：激光束接触到物体并返回了非最大距离值的区域。

为了将这些区域映射到测量栅格上，每个栅格单元都会被分配一个相对于车辆当前位置的距离和方位角。

单元的相对距离 r_i 是传感器到该单元的欧几里得距离：
r_i = sqrt( (mx_i - x1_t)^2 + (my_i - x2_t)^2 )
其中 (mx_i, my_i) 是栅格单元中心的坐标。
单元的相对方位角 φ_i 使用反正切函数计算：
φ_i = atan2( (my_i - x2_t), (mx_i - x1_t) ) - x3_t

对于每个单元，我们通过寻找其方位角与激光束角度误差最小的测量值，来关联最相关的激光雷达光束。

概率分配规则

我们定义两个参数 α 和 β，它们定义了围绕每个光束的一个扇形区域，该区域内单元的占据概率将根据该光束的距离测量值来确定。

现在，我们可以根据接收到的激光雷达测量值，为任何单元分配被占据的概率。以下是三种类型单元的概率分配规则：

以下是具体的概率分配步骤：

无信息区：对应所有距离大于测量距离、或位于关联测量值的 β 角锥形区域之外的单元。我们为其分配一个等于先验概率的占据概率，通常为 0.5。
高信息区：对应距离落在测量值 ±α/2 范围内、且角度落在关联测量值 ±β/2 范围内的单元。我们为这些单元分配一个 大于 0.5 的占据概率。
低信息区：对应距离小于测量值减去 α/2、且位于关联测量值 β 角锥形区域内的单元。我们为这些单元分配一个 小于 0.5 的占据概率。

例如，假设激光雷达扫描在红色 X 标记的位置返回了一个距离值。受影响的区域（将被标记为高概率）以红色显示。增加 α 的值会增大受影响区域的范围，而增加 β 的值会影响受影响区域的角度。

使用光线追踪优化计算

然而，这个简单的逆测量模型在计算上可能非常昂贵。它需要更新测量地图中的每一个单元，并依赖多次浮点运算来确定哪些测量值与哪些单元对应。

一种替代方案是使用光线追踪算法，例如计算机科学经典——Bresenham直线算法。该算法最初设计于20世纪60年代早期，旨在当时可用的硬件上高效解决显示和打印中的直线绘制问题。

通过沿着激光雷达扫描的光束进行光线追踪，我们可以减少需要处理的单元数量，并更快地识别它们。该算法主要依赖整数加法、减法和位移操作来沿着激光雷达光束在网格中移动。

有趣的是，许多光束会穿过车辆附近的相同单元，这大大增加了对附近测量值的置信度。

总结

本节课中我们一起学习了为对数几率占据栅格映射构建激光雷达逆测量模型的关键步骤。我们首先定义了逆测量模型的概念及其必要性，然后详细讲解了如何基于激光雷达的距离和方位角数据，在临时栅格上划分无信息区、低概率区和高概率区，并为每个栅格单元分配相应的占据概率。最后，我们探讨了使用Bresenham光线追踪算法来优化计算过程，减少计算负担。

在下一个视频中，我们将深入研究将3D激光雷达扫描数据用于占据栅格映射所需的诸多准备步骤。

012：占据栅格更新

在本节课中，我们将学习如何将自动驾驶汽车采集的激光雷达扫描数据，转换为可用于构建占据栅格的过滤后表示。我们将首先介绍几种必须应用于3D扫描的过滤方法，然后了解如何将3D激光雷达数据投影到2D空间并转换为置信度地图，最后讨论调整占据栅格精度所需的几个参数。

从激光雷达到占据栅格的过滤流程

上一节我们介绍了占据栅格的基本概念，本节中我们来看看如何将原始的激光雷达数据转化为适合构建占据栅格的输入。这里展示的是一个典型的自动驾驶汽车在道路上行驶时获取的激光雷达扫描点云。

以下是必须应用于激光雷达扫描的几种关键过滤方法：

下采样：为了确保更新操作能够实时运行，通常需要将激光雷达扫描的点数减少到一个更小的数量。
移除车顶以上点：为了移除不影响驾驶的物体（如树木、电线、路牌），我们过滤掉所有高于自动驾驶汽车高度的激光雷达点。
移除可行驶表面点：我们不希望将可行驶表面标记为被占据，因此需要移除所有击中可行驶表面或地面的激光雷达点。
移除动态物体点：最后，我们移除所有动态或移动的物体（如行驶中的汽车或行人）的点。

过滤方法详解

下采样

下采样是通过移除或忽略冗余的激光雷达点来减少待处理点数的过程。自动驾驶中常用的激光雷达每秒可产生多达120万个点，这为车辆周围的所有物体提供了非常丰富的几何描述。然而，由于许多点都围绕着同一物体，捕获的是相同的信息，因此许多生成的点实际上是冗余的。

例如，一个路标被密集的激光雷达点覆盖，但实际上只需一小部分点就足以在占据栅格地图中指示障碍物的位置。最重要的是，每秒处理120万个点在计算上是不切实际的，因此必须移除一些点以提高后续所有操作的计算效率。

下采样可以通过多种方式实现，最简单的方法是使用一个系统性过滤器，沿着激光雷达扫描环保留每第 n 个点。也可以在距离图像中应用图像下采样技术，或在3D网格中进行空间搜索，用单个占据测量值替换点集合。这些方法在开源点云库（如PCL）或计算机视觉库（如OpenCV）中都很容易实现。

移除车顶以上点

第二个过滤器相对简单，即移除车辆高度以上的点（例如，高于2.4米的点）。这个过滤器通常假设地面是平坦的，但请注意，盲目应用这个假设是危险的。然而，过滤掉悬垂的树木、电线、桥梁和标志物等价值显著，因此值得在此讨论。

移除可行驶表面点

下一个过滤器涉及移除被认为落在可行驶表面上的点。由于激光雷达扫描的特性，图像中看到的许多同心圆环是由于激光雷达击中了可行驶表面，这些点不应与占据栅格地图中的被占据单元混淆。然而，事实证明这是一个具有挑战性的任务，因为会出现几个复杂情况。

首先，所有道路都有不同的几何形状，包括用于排水的可变凹度、不同的坡度和倾斜角、曲率等。这种变化很难预测。如果不移除地面点，占据栅格可能会出现伪影，导致死锁，即汽车认为道路被阻塞而无法继续行驶。

第二个问题是路缘在不同位置有不同的高度，并且道路边界在激光雷达数据中并不总是清晰定义的。这些变化可能导致部分路缘或不可行驶区域被当作地面的一部分而移除。

最后，需要检测道路上的小物体（如足球或乌龟），这意味着在点云上工作的简单几何方法很难解析前方道路的真实状态。处理这类问题的最佳方法是利用您在课程3中学到的，通过语义分割来利用视觉和深度神经网络。这可以在分割图像中看到，其中地面平面显示为深紫色。

然后，任务就变成了将视觉数据中检测到的可行驶表面映射到激光雷达点云，并屏蔽掉所有落在可行驶表面投影边界内的点。

移除动态物体点

所需的最后一个过滤器是移除所有动态物体，如移动的汽车或行人。这可以再次依靠感知模块来完成，该模块必须检测并跟踪场景中的所有动态物体。检测到的动态物体的3D边界框用于移除受影响区域内的所有点。还会在边界框的大小上增加一个小的阈值，以应对感知算法在物体位置估计中的任何小错误，从而提高点移除过滤器的鲁棒性。

然而，很多时候这并不令人满意，原因有二。首先，并非所有检测到的动态物体类别的实例都在实际移动。有些车辆可能停在路边，因此可以被视为占据栅格的一部分，因为它们确实是静态的。为了处理这个问题，感知模块需要使用动态物体轨迹来识别当前静止的物体，以便占据栅格构建器避免移除它们。

其次，由于感知模块所需的计算时间，通常动态物体在检测到后会有些延迟，这导致使用过时的物体位置来更新占据栅格，从而导致边界框遗漏动态车辆上的大部分激光雷达点。相反，我们可以基于物体轨迹预测移动物体的运动。将边界框沿着预测路径向前移动，可以从最新的激光雷达扫描中移除更多动态物体的点。

从3D到2D的投影与置信度地图生成

经过所有这些过滤后，激光雷达数据终于准备好从3D投影到2D。现在让我们看看实现这最后一步的一个简单而有效的技术。

首先，存储激光雷达点高度的 Z 值被设置为 0，从而将点云压缩到一个平面上。然后，将2D扫描平面划分为与占据栅格相同的网格模式。对于占据栅格的每个单元，统计落入其中的所有激光雷达点的数量。这个值将被用作占据置信度的度量。一个单元中的点越多，表示在该单元中测量到静态物体的可能性就越大。

参数调整与总结

本节课中，我们一起学习了将激光雷达点云转换为占据栅格所需的过滤技术。这些技术包括下采样、移除车顶以上的点、地面平面移除以及动态物体移除。然后，我们看到了一个将3D激光雷达数据转换为2D置信度地图的简单而有效的方法，正如第1课所解释的，这可用于创建占据栅格。最后，我们了解了占据栅格必须仔细调整和校准的所有不同方面。

在下一个视频中，我们将讨论高精地图，特别关注车道级地图及其在运动规划中的用途。下次见。

014：创建道路网络图

概述

在本节课中，我们将学习自动驾驶中的任务规划问题及其解决方法。我们将介绍图这一数学概念，探讨如何用它来表示需要导航的道路网络，并学习广度优先搜索算法如何应用于任务规划。

任务规划回顾

上一模块中，我们介绍了自动驾驶任务是运动规划的最高层级。本节中，我们来看看任务规划的具体目标。

自动驾驶任务的目标是：在道路网络中，为自车从其当前位置到指定目的地找到最优路径，同时抽象掉低层级的细节，如交通规则和场景中的其他交通参与者。在本模块中，我们将最优性理解为汽车到达目的地所需的时间或距离。

对于自动驾驶而言，任务规划被视为最高层级的规划问题。这是因为任务规划的空间规划尺度在公里级别。任务规划不关注低层级的规划约束，例如障碍物或车辆动力学。相反，任务规划在规划时将关注道路网络的各个方面，例如限速、道路长度、交通流量和道路封闭情况。

基于地图施加的这些约束，任务规划器需要找到到达所需目的地的最优路径。

关于道路网络，需要注意的一点是它具有高度结构化的特点，我们可以在规划过程中利用这一点来简化问题。通过利用这种结构，我们可以根据给定的地图，高效地找到通往目的地的最优路径。为此，我们需要使用一种称为“图”的数学结构，我们已将其叠加到这里的道路网络上。

什么是图？

图是一种离散结构，由一组顶点（记为 V）和一组边（记为 E）组成。

对于任务规划器，V 中的每个顶点对应于道路网络上的一个给定点。每条边 E 对应于连接道路网络中任意两点的路段。从这个意义上说，图中一系列连续的边对应于从一点到另一点穿过道路网络的路径。

这里展示了一个图的示例。目前，我们假设每个路段长度相等，因此该图的边都是无权重的。然而，在后续课程中，我们将放宽这一限制。

要生成这样的图，需要对道路网络进行离散采样，这将为我们提供图的顶点。然后，边将由根据道路规则连接每个采样点的路段来定义。

请注意，一般来说，仅仅因为点 A 通过一个路段与点 B 相邻，并不意味着点 A 可以从点 B 通过同一路段到达。这是因为在许多情况下，一个路段只能合法地沿一个方向通行。从这个意义上说，我们图的边是有向的，即边只能沿一个方向通行。我们通过在图中使用箭头表示边来显示其方向性。

使用广度优先搜索寻找最优路径

现在我们有了有向图。如何找到通往目的地的最优路径？

首先，我们在图中定位对应于自车当前位置的顶点（记为 S）和期望的目的地顶点（记为 T）。这两个顶点在图中显示如下。

一旦有了这两个顶点，我们就可以使用高效的图搜索算法来找到通往目的地的最优或最短路径。由于我们当前的图公式是无权重的，广度优先搜索是一个很好的候选算法。

从高层次看，BFS 可以被认为是遍历图中的所有顶点，但以这样一种方式进行：在深入图之前，首先评估所有相邻的顶点。从这个意义上说，图搜索就像在图中以移动波前的方式或“广度优先”地进行。

让我们逐步了解 BFS 算法的步骤。

我们构建三个数据结构来辅助搜索：

开放队列：待评估的顶点。
封闭集合：已被搜索算法评估过的顶点。
前驱字典：存储搜索结果。

队列是一种先进先出的数据结构，第一个被推入或添加到队列的顶点是第一个被弹出或从队列返回的顶点。字典是一组无序的键值对，对于封闭集合中的每个节点，存储一个前驱顶点，我们稍后会进行标识。

以下是算法的具体步骤：

算法首先将我们的起始顶点 S 添加到开放队列。
然后，当开放队列包含顶点时，我们从开放队列中取出第一个元素，并检查它是否是目标位置。
- 如果是，我们就找到了最短路径。
- 如果不是，我们则将当前顶点的所有相邻顶点（尚未在开放队列或封闭集合中的）添加到开放队列。这可以防止我们在图搜索中陷入循环。请注意，所谓“相邻”，我们指的是可以从当前顶点到达的所有顶点。
由于我们使用队列来存储开放顶点，我们确保在深入图之前，先处理当前搜索深度的所有相邻顶点。因此，所有距离起始顶点一步之遥的顶点将在处理距离两步之遥的顶点之前被处理。
当一个顶点被添加到开放队列时，我们在前驱字典中存储它的前驱顶点。这将帮助我们在找到目标后重建最优路径。
最后，我们将当前活动的顶点添加到封闭集合，并返回到开放队列的下一个元素进行处理。

由于 BFS 算法的广度优先特性，当我们到达目标顶点时，我们已经处理了目标顶点所有可能的前驱顶点，这些前驱顶点都比目标顶点更接近起始顶点。这意味着当我们到达目标顶点时，我们已经找到了通往目标顶点的最短路径，可以终止算法。

算法示例

为了巩固我们对这个算法的理解，让我们看一个具体例子。

假设我们的任务规划器需要通过图中的一组顶点（现已标记）找到从点 S 到目的地 T 的最优路径。

第一步是处理起点 S，并将所有相邻顶点添加到我们的队列，并将它们的前驱设置为 S。通向相邻顶点的出边用蓝色高亮显示。将这些添加到队列后，我们将 S 添加到封闭列表。

接下来，我们弹出顶点 A。A 的出边通向 D 和 B，但 B 已经在我们的队列中，因此我们只将 D 添加到队列，并将 A 标记为其前驱。我们用红色高亮显示了这条通往 B 的重复路径，以表明我们不会将 B 两次添加到队列。我们现在已经处理了 A 的所有相邻边，并将其移动到封闭集合。

我们对 B 重复相同的过程，将 E 添加到队列，并将 B 作为其前驱。接着处理 C，它没有新的相邻顶点，因此不会向开放队列添加顶点。

接下来，我们从队列中处理 D，它只将 T 添加到队列，并将 D 作为其前驱，因为 E 已经被添加。当 E 被弹出时，它不会将 C 或 D 添加到队列，因为这两个顶点都已被处理并存在于封闭集合中。

最后，我们从队列中弹出 T。这是我们的目标顶点。因此，我们现在通过从 T 回溯到 S 的前驱链来重建路径。完成后，我们就找到了通往目的地的最优路径，用绿色高亮显示。

图中对应于我们最优路径的边序列，可以使用我们的地图转换为道路网络上的路线。然后，该路线可用于在我们规划层级的后续层中指导更详细的运动规划。

总结与展望

在本节课中，我们一起学习了任务规划问题，以及如何构建和使用图作为我们规划领域的地图层级表示。此外，您现在应该能够熟练使用 BFS 搜索来导航无权图，以找到通往给定目的地的最短路径。

在结束本视频之前，我们应该注意到，除了 BFS，还有高度相关的深度优先搜索算法等。深度优先搜索对开放集合使用后进先出的栈而不是队列。这一变化意味着评估的是最近添加的顶点，而不是最早的顶点。结果是搜索会快速深入图中，然后最终回溯到更早添加的顶点。

在下一课中，我们将通过为图中的边添加不同的权重来使图变得更复杂，以更好地反映使用不同路段的不同成本。我们将介绍迪杰斯特拉算法，一种处理这种新复杂性的方法。下次见。

016：启发式搜索与A*算法

在本节课中，我们将基于迪杰斯特拉算法，引入一种可能更快的路径搜索方法。我们将通过利用启发式信息来优化搜索过程，从而提高效率。

概述

在上一节中，我们介绍了迪杰斯特拉算法，用于解决带权图（更贴合实际道路网络）的最短路径问题。然而，该算法需要搜索图中几乎所有的边，这在处理大规模城市路网时效率较低。本节我们将学习如何利用启发式搜索，特别是A*搜索算法，来更高效地解决任务规划问题。

什么是搜索启发式？

搜索启发式是对从图中任意给定顶点到达目标顶点所需剩余成本的估计。在我们的场景中，顶点对应空间中的点，边对应道路段，其权重对应道路长度。因此，一个有用的成本估计是两点之间的直线距离或欧几里得距离。

对于任意顶点V，其到目标T的启发式估计值H(V)即为V与T之间的欧几里得距离。这个估计值总是小于或等于到达目标的真实最短距离，因为两点之间直线最短。满足此条件的启发式被称为可采纳启发式。

启发式计算示例

假设起点A坐标为(0,0)，目标点C坐标为(2,2)。则启发式估计值为：
H(A) = sqrt((2-0)^2 + (2-0)^2) = 2.828
假设实际路径为A->B->C，边权分别为2和2.6，则真实成本为4.6。可见，启发式估计2.828是真实成本4.6的一个下界。

A*搜索算法

A*算法与迪杰斯特拉算法结构相似，但有一个关键区别：它使用累计成本 + 启发式估计成本来指导搜索优先级，而不仅仅是累计成本。

以下是A*算法的核心伪代码片段（关键区别已高亮）：

将起点S加入最小堆，键值为：g(S) + h(S)
当最小堆非空时：
    弹出当前键值最小的顶点V
    如果V是目标点，则结束搜索
    将V加入“已关闭”集合
    对于V的每个邻接顶点U：
        计算新的累计成本：g_new = g(V) + cost(V, U)
        如果U未访问过或 g_new < g(U)：
            更新 g(U) = g_new
            计算 f(U) = g(U) + h(U) // f为估计总成本
            将U以键值f(U)加入最小堆（或更新堆中U的键值）
            记录U的前驱顶点为V

其中：

g(V)：从起点到顶点V的实际累计成本。
h(V)：从顶点V到目标点的启发式估计成本。
f(V) = g(V) + h(V)：通过V点到达目标点的估计总成本。

最小堆根据f(V)进行排序，这使得搜索偏向于估计总成本更低的顶点，从而更快地导向目标。

重要说明：如果将启发式h(V)始终设为0，A*算法即退化为迪杰斯特拉算法。

算法应用示例

让我们将A*算法应用于我们的道路网络示例图。图中顶点已标注其空间位置，边权代表道路长度。

以下是搜索过程的逐步解析：

初始化：将起点S加入最小堆，f(S) = g(S) + h(S) = 0 + 4.472 = 4.472。
处理S：弹出S。处理其邻接点A、B、C，计算各自的f值并加入堆。此时堆顺序为：C(5.6), A(8), B(9)。将S加入关闭集。
处理C：弹出C（当前f最小）。处理其邻接点E，计算f(E)=11.4并入堆。堆顺序更新为：A(8), B(9), E(11.4)。将C加入关闭集。
处理A：弹出A。处理其邻接点B和D。
- 对于B，发现经A到B的新路径g(B)=5，优于之前的7，因此更新B在堆中的f值为8，并将B的前驱更新为A。
- 将D以f(D)=7加入堆。
- 堆顺序更新为：D(7), B(8), E(11.4)。将A加入关闭集。
处理D：弹出D。处理其邻接点E和T。
- 对于E，新路径的f(E)=15.4，差于堆中已有的11.4，因此忽略此路径。
- 对于目标点T，计算f(T)=7并入堆。
- 堆顺序更新为：T(7), B(8), E(11.4)。将D加入关闭集。
找到目标：弹出T，发现是目标点，搜索结束。

最终，我们得到了从S到T的最短路径：S -> A -> D -> T。值得注意的是，通过使用A*算法，我们避免处理了顶点B和E，从而提高了搜索效率。

扩展到更复杂的问题

在我们的示例中，边权重是道路的物理长度。但在实际任务规划中，我们可能需要考虑交通状况、限速、天气等因素，此时更合适的边权重应该是穿越路段所需的估计时间。

然而，这导致欧几里得距离无法直接作为时间成本的启发式估计。为了解决这个问题，我们可以使用一个时间成本的下界作为可采纳启发式：

h_time(V) = (V到T的欧几里得距离) / (全路网最高允许速度)

这个公式假设汽车以最大允许速度直线驶向目标，这给出了可能的最短时间，因此是真实时间成本的一个可采纳下界。

时间启发式示例

假设全路网最高速度为100公里/小时，边权重为穿越所需秒数。计算从A到C的启发式：
h_time(A) = (欧几里得距离 2.828 km) / (100 km/h) ≈ 101.8 秒
而实际路径A->B->C的时间成本可能为165.6秒。启发式估计仍然是下界，尽管这个下界可能比较“宽松”。启发式下界越紧，通常能更有效地引导搜索。

对于大规模、基于时间的路径规划问题，还有更高级的方法（如预计算、改进的启发式定义等）来提升搜索效率。

总结

本节课中，我们一起学习了：

启发式的概念：作为剩余路径成本的估计，用于引导搜索方向。
可采纳启发式：要求启发式估计值永不高于真实成本。欧几里得距离是路径长度问题的可采纳启发式。
A*搜索算法：结合了实际累计成本g(n)和启发式估计成本h(n)，通过最小化f(n)=g(n)+h(n)来高效地寻找最短路径。它通常比迪杰斯特拉算法探索更少的顶点。
问题扩展：讨论了当边权重为时间而非距离时，如何构造可采纳的时间启发式（基于最大速度的直线时间）。

通过引入启发式信息，A*算法使我们能够以更高的计算效率解决大规模的道路网络任务规划问题。

恭喜你完成本任务规划模块的学习！在本模块中，你学会了将任务规划问题定义为有向图上的最短路径搜索，并应用迪杰斯特拉和A*算法来高效地寻找最短路径。在下一个模块中，你将学习动态物体交互及其与运动规划层级中第二阶段——行为规划器的关系。下次见！

017：运动预测概述 🚗

在本节课中，我们将学习运动规划中一个至关重要的环节：动态物体的运动预测。我们将了解其定义、重要性、所需信息、面临的挑战，并从一个简单的预测模型开始实践。

运动预测旨在估计环境中所有动态物体在未来一段时间内的位置、航向和速度。这对于运动规划问题至关重要，因为它允许我们基于其他物体的预期运动来规划自动驾驶车辆的未来动作和机动。预测的路径还能确保自车计划执行的路径不会在未来与任何物体发生碰撞。

为了能够预测运动物体的轨迹，我们必须获取关于周围环境的一些信息，特别是与动态物体相关的信息。

以下是预测所需的基本信息：

物体类别：这是至关重要的信息，因为大多数预测模型对车辆和行人的算法处理方式不同。
当前状态：动态物体在环境中的当前位置和速度。这通常用一个向量表示，向量原点等于物体位置，大小等于其速度，方向等于其当前航向或行驶方向。没有这个最基本的信息，就无法对动态物体的未来状态进行预测。

此外，还有许多其他信息虽然不是预测所必需的，但可以极大地提高预测的准确性。

以下是一些能够改进预测的主要额外信息来源：

状态历史（轨迹）：动态物体在环境中移动时的状态历史记录。这非常有用，可以帮助我们更好地了解物体是如何在环境中机动的。
高精度地图：可以作为额外的信息来源，用于确定动态物体的未来行为。例如，车辆在道路上行驶时倾向于跟随各自的车道，这为改进预测准确性提供了强有力的线索。
当前图像：动态物体当前状态的图像也是一个有用的信息来源。对于车辆，图像可以提供关于当前转向灯或刹车灯状态的信息。对于行人，图像可以显示人的当前朝向，这有助于预测未来的行进方向，即使行人当前是静止的。

尽管运动预测任务的复杂性相当大，但我们可以使用一些假设来简化问题。我们将首先看看针对车辆的简化假设，然后再讨论行人。

对于车辆的假设可以分为三类：

基于物理的假设：车辆必须遵循支配其运动的一套物理约束。我们可以将同样的车辆动力学应用于环境中的其他车辆来预测其运动。
基于机动的假设：假设车辆在道路上的几乎所有运动都由环境中受限区域内的一组有限机动动作组成。例如，车辆很可能会保持在车道内，遵守停车标志等规则，不太可能驶上人行道或穿过障碍物。
交互感知的假设：在基于机动假设的基础上，进一步假设动态物体会相互反应和交互。例如，在车辆并道时，目标车道内的车辆可能会减速以保持安全距离。

对于行人的假设也可以归纳为类似的三类：

基于物理的假设：行人最高速度较低，但可以非常快速地改变运动方向和速度。这使得仅使用基于物理的假设来可靠地预测行人变得相当具有挑战性。
基于机动的假设：行人倾向于使用人行道或其他行人专用区，进入可能与车辆接触的可行驶区域时主要使用人行横道。虽然将行人运动限制在这些区域是合理的假设，但并非硬性约束。
交互感知的假设：行人拥有路权，自动驾驶汽车有责任在必要时停车。心不在焉的行人可能会毫无预警地走入车道，但在受到来车威胁时通常会停下。

现在我们对运动预测有了更好的理解，让我们来看一个简单、计算效率高的算法，它可以同等地应用于行人和车辆。该算法只对动态物体的运动做了一个极端的假设：所有动态物体将保持其当前速度，包括大小和方向。

理解了这一点，现在让我们看看这个简单的恒定速度模型的算法实现。

该算法接受三个基本输入：

T：预测时间范围，即预测物体未来位置的时间长度。
DT：更新率或路径模拟频率，即状态预测之间的时间间隔。
X_object：当前物体状态，包括动态物体的位置和速度。

该算法从当前时间0开始迭代，直到时间范围T结束，步长为DT。其输出是预测时间范围内每个时间步的预测物体状态（位置和速度）列表。

为了看看这些预测的效果如何，让我们看一个简单的例子。我们使用三秒的预测范围，一秒的更新步长，以及图中红色箭头所示的当前车辆状态。正如预期的那样，车辆的预测位置以恒定方向移动，步长固定，这与具有恒定限速的直线路段非常吻合。简而言之，这是因为在该路段上恒定速度假设是有效的。

然而，恒定速度估计失效的地方在于所有其他情况。虽然这个算法勉强属于基于物理假设的范畴，但它未能捕捉车辆动力学模型的全部复杂性，甚至无法体现车辆加速、减速或施加非零转向指令的能力。恒定速度假设的另一个重大缺陷是，它未能考虑车辆跟随道路形状变化的趋势。在这个弯曲道路的例子中，恒定速度模型预测路径将继续进入对向车道，这些预测完全不适合行为规划。同样，恒定速度预测也未能考虑道路标志以进行速度调整。接近停车标志的车辆往往会减速，而离开停车线的车辆往往会加速。该算法所做的假设非常强，并不适用于我们可能观察到动态物体的大多数情况。

运动预测的关键挑战实际上是，根据可用信息，为车辆或行人模型选择最可能的输入。尽管如此，恒定速度模型是一个极好的起点，有助于清晰地定义运动预测的概念。它依赖于关于动态物体的最少信息来形成预测，并且可以在完全无法获得额外线索的任何地方使用。

总结

在本节课中，我们学习了动态物体运动预测的任务及其对自动驾驶的重要性。然后，我们定义了为车辆和行人创建有效运动预测算法所需的最低限度和可选信息要求。接着，我们研究了一个用于预测物体未来位置的简单恒定速度算法，并指出了它的许多局限性。

在下一个视频中，我们将探讨通过使用高精度地图来增强运动预测的方法。

018：具备地图意识的运动预测

在本节课中，我们将学习如何利用高清地图信息来改进动态物体的运动预测。我们将探讨基于地图的算法所做的假设，并学习如何将这些假设应用于位置和速度预测，以生成更准确、更符合现实场景的轨迹。

概述：从恒定速度预测到地图感知预测

在上一节中，我们探讨了恒定速度运动预测，但这种方法仅在非常有限的场景中有效。为了改进预测，特别是针对车辆，基于地图的算法会做出两大类假设：一类用于改进车辆状态的位置分量，另一类用于改进速度分量。

基于地图的位置预测假设

以下是两种用于改进位置预测的主要假设。

车道跟随假设

第一个假设是，在给定车道上行驶的车辆通常会跟随该车道。例如，在弯曲的道路上，车辆很可能会沿着道路的曲率转弯。为了应用这一假设，我们可以使用车道中心线作为车辆的预测路径，而不是恒定速度预测生成的直线路径。

车道中心线被定义为一条折线，其上的每个点与车道左右边界的距离相等。虽然车辆的实际路径可能与中心线有微小偏差，但中心线可以作为一个很好的运动预测近似。

基于转向灯的预测

第二个假设是，可以根据车辆转向灯的状态来预测其车道变更或转向意图。如果感知系统检测到车辆的转向灯亮起，预测模型就可以纳入这一信息，调整预测路径。

多假设预测：处理不确定性

然而，将路径预测限制在单一车道中心线会带来两个主要问题。第一，驾驶员在正常驾驶中会变换车道。第二，在某些情况下（如路口），可能存在多条可选的中心线。

为了解决这些问题，我们需要考虑基于场景中其他智能体可能行为的多种假设。这被称为多假设预测。

在多假设预测方法中，我们会考虑车辆基于其在高清地图当前位置所有可能性的每一种名义行为。例如，在一个三岔路口，我们可以考虑三种可能性：左转、右转或保持静止。

基于转向灯位置、车辆在路口的状态等佐证信息，我们可以评估智能体在预测时间范围内执行每种假设的可能性。这些概率可以通过从大量类似路口的车辆行驶数据中学习获得，也可以通过实际测试进行工程设计和优化。

这种方法为行为规划器提供了更多模糊信息，要求规划过程同时考虑多种场景，并处理信念的概率表示。但如果处理得当，这种方法可以显著提高安全性，实现防御性驾驶策略，并在新信息改变概率时快速重新规划。

基于地图的速度预测改进

现在我们已经了解了如何利用地图改进预测轨迹的位置分量，接下来让我们深入探讨速度分量的改进。

基于道路曲率的速度调整

第一个改进基于已知的道路几何形状或曲率，以及对其他车辆如何应对曲率的预测。所有车辆在进入急弯或转弯时都会降低速度。我们可以使用一个预期的最大横向加速度（通常在 0.5 到 1 米/秒² 的范围内）来改进沿曲线行驶的速度估计。

整合交通规则元素

第二个更重要的改进是整合交通规则元素来改进速度估计。给定车辆的预期路径，任何道路元素，如停车标志、让行标志、限速变化或交通信号灯，都可以为速度预测提供信息。

对于交通信号灯，还需要知道其当前状态。在每种情况下，都可以基于高清地图中定义的规则元素线来预测停车位置。然后，可以沿其路径对车辆速度预测应用平滑的减速。

实际上，给定高清地图，可以预先处理每条道路的名义轨迹，并基于名义驾驶行为定义特定于车道的多假设先验。这既可以为自车规划其行为和轨迹提供指导，也可以用于改进对其他智能体的运动预测。

地图感知预测的局限性与挑战

当然，车道前方的障碍物、路口到达的优先顺序信息以及车道中的前导车辆都可以被整合进来，以改进对其他车辆的预测。然而，为单一自动驾驶汽车做决策的复杂性，限制了可用于改进运动预测的变量数量以及信息和逻辑的深度。

我们也不能完全依赖关于预期动态物体行为的假设来预测未来动作。动态物体并不总是按照驾驶员的预期名义行为行事。它们不会精确跟随中心线，不会以限速行驶，并且会以不同的速率加速和减速。此外，它们可能对预测系统尚未获得的信息做出反应，或者可能根本没有注意到某个规则元素。

所有这些变化都必须被考虑在内，多假设方法可以在一定程度上做到这一点。因此，最好的方法是跟踪这组假设上信念的演变，并在每个时间步基于感知系统提供的证据进行更新。

总结

本节课中，我们一起学习了地图感知的运动预测。我们描述了一组用于改进车道车辆运动预测的地图算法假设，定义了基于位置和速度的方法来改进运动预测，将多假设预测描述为维持关于其他车辆未来动作多种信念的一种方式，并最后指出了完全依赖地图感知运动预测存在的一些问题。

希望下次课程你能继续加入我们，届时我们将学习如何使用预测路径来计算动态物体对之间的碰撞时间。下次见。

020：行为规划系统概述 🚗

在本节课中，我们将学习行为规划系统的基本概念。我们将定义行为规划系统的要求，探讨其典型的输入与输出，并介绍有限状态机及其在构建行为规划系统中的应用。

行为规划系统的定义与要求

行为规划系统负责规划一系列高级驾驶动作或策略，以在各种驾驶情境下安全地完成驾驶任务。这些策略必须考虑交通规则，并处理与环境中所有静态和动态物体的交互。规划器所做的高级决策必须确保车辆安全，并在环境中高效移动。

行为规划器需要在计算上高效地执行此类决策，以便快速响应环境变化，并能在自动驾驶车辆的硬件上部署。此外，行为规划器还应能够处理不准确（受测量噪声影响）和错误（受感知误差影响，如误检和漏检）的输入。

基础驾驶行为列表

以下是本模块将使用的五个基础驾驶行为列表：

跟踪速度：在开放道路上无约束驾驶，唯一的限制是遵守速度限制。
跟随前车：匹配前车速度，并保持安全跟车距离。
减速至停止：在规划视野内，本车车道中存在停车点，车辆应减速至该点完全停止。
保持停止：车辆应保持静止一段固定时间。例如，在停车标志处至少停止三秒。
并线：车辆应向左或向右车道并线。

这个基础策略列表有助于我们理解行为规划的原理。但在实际应用中，需要考虑更多行为，行为规划器的整体复杂性也会随之增加。

行为规划器的输出

行为规划器的主要输出是当前环境下应执行的驾驶策略。除了驾驶策略，行为规划器还输出一组约束条件，用于限制局部规划问题。本模块将使用和填充的约束包括：

默认路径：从车辆当前位置到目标目的地的路径。对于许多行为，这通常是本车道的中心线。
速度限制：沿默认路径的速度限制。
车道边界：在正常驾驶条件下应保持的当前车道边界。
未来停车点：车辆需要到达的任何未来停车位置。仅当选择相关策略时才会填充此约束。
高关注动态物体：局部规划器应关注的动态物体集合。这些物体可能因距离近或预测路径而重要。

行为规划器的输入

为了使行为规划器能够产生所需的输出，它需要从自动驾驶软件栈的许多其他系统中获取大量信息：

道路网络知识：来自高清地图，了解车辆附近完整的道路网络。
任务路径：以道路网络图上的任务路径形式提供，指示应遵循哪些道路以到达目标位置。
车辆定位：来自定位系统的精确定位信息，以便在车辆周围局部环境中正确放置高清地图元素。
感知信息：包括所有观察到的环境动态物体（如汽车、行人、自行车）及其当前状态、预测路径、碰撞点和碰撞时间；所有观察到的静态物体（如停放车辆、施工锥、交通灯）及其状态；以及定义可执行策略安全区域的局部占据栅格地图。

有限状态机方法

拥有所有必要信息后，行为规划器必须选择适当的行为并定义必要的伴随约束，以保持车辆安全高效地移动。为此，我们将构建一套规则来考虑所有交通规则以及与其他动态物体的交互。

传统上，用于表示解决行为选择所需规则集的一种方法是有限状态机。在本模块中，我们将逐步构建一个基于有限状态机的行为规划器，并讨论这种方法的局限性。

为了更好地理解有限状态机方法，让我们通过一个简单示例来了解如何处理单个场景：一个无交通的停车标志交叉路口。

有限状态机的第一组组件是状态集合。对于行为规划系统，状态代表可能遇到的每种可能的驾驶策略。在我们的示例中，只需要两种可能的策略或状态：跟踪速度 和 减速至停止。行为规划器定义的策略决策由有限状态机的状态设定。

每个状态都有一个与之关联的进入动作，即首次进入状态时要执行的动作。对于我们的行为规划器，这些进入动作涉及设置伴随行为决策所需的约束输出。例如，一旦进入减速至停止状态，我们还必须定义沿路径的停车点。同样，跟踪速度状态的进入条件会设置要跟踪的速度限制。

有限状态机的第二组组件是转移，它定义了从一个状态到另一个状态的移动。在我们的两状态示例中，可以从跟踪速度转移到减速至停止，也可以从减速至停止转移回跟踪速度。注意，也可以存在返回当前状态的转移，从而触发该状态的进入动作重复执行。

每次转移都伴随一组转移条件，这些条件需要在切换到下一个状态之前得到满足。在处于某个状态时，会监控这些转移条件以确定何时应发生转移。对于我们简单的示例，从跟踪速度转移到减速至停止的转移条件包括检查当前车道中是否存在一个在阈值距离内的停车点。同样，如果我们在停车点速度已降至零，则可以从减速至停止转移回跟踪速度。

这个两状态示例突出了基于有限状态机的行为规划器最重要的方面。随着场景和行为的增加，所需的有限状态机会变得显著更复杂，包含更多的状态和转移条件。

有限状态机的优势与局限

有限状态机可以是行为规划中非常简单有效的工具。我们可以将其视为行为规划定义的直接实现，该定义要求我们定义满足交通规则的策略（状态）和局部规划约束（进入动作），并检查与环境中的其他动态和静态物体的安全交互（转移条件）。

通过跟踪当前策略和驾驶环境状态，只需要考虑从当前状态出发的相关转移，这大大减少了每次迭代需要检查的条件数量。由于行为规划被分解为一组具有相互转移的状态，所需的各个规则保持相对简单。这导致了直接的实现，不同行为之间有清晰的划分。

然而，随着状态数量的增加，定义所有可能的转移及其条件的复杂性会爆炸式增长。也没有明确的方法来处理输入数据中的不确定性和错误。这些挑战意味着，随着我们接近完全的L5级自动驾驶，有限状态机方法往往会遇到困难。但对于具有受限运行设计域的系统来说，它是一个极好的起点，因为它允许管理可控数量的状态。我们将在本模块的最后一个视频中探讨这些局限性以及行为规划的替代方法。

总结

本节课我们一起学习了行为规划问题及其在整个运动规划系统中的角色。我们讨论了行为规划模块的标准输入和输出，介绍了有限状态机及其组件，并将其应用于一个两状态的行为规划问题。

接下来，我们将开始为我们的有限状态机行为规划器添加更多功能。在下一课中，我们将学习如何处理没有任何动态物体的交叉路口场景中的所有相关规则。

021：处理无动态物体的交叉路口场景

在本节课中，我们将学习如何构建一个行为规划系统，以处理一个更完整的交叉路口场景。我们将从定义场景细节开始，然后学习如何将交叉路口地图离散化，以建立清晰的状态转换。接着，我们将定义安全高效完成给定场景所需的状态和转换。最后，我们将介绍验证行为规划系统正确性所需的测试流程。

场景定义

上一节我们介绍了行为规划器的概念和有限状态机的基础知识。本节中，我们来看看如何构建系统来处理一个更完整的交叉路口场景。

我们首先要定义将要处理的具体场景。该场景是一个四向交叉路口，每个方向都有两条车道和一个停车标志。下图展示了这样的交叉路口，其中红线代表车辆必须在其后方停车的停止线。

到本节结束时，车辆应能在此交叉路口左转、右转或直行。目前我们暂时不考虑动态物体，将在下一节视频中讨论它们。

交叉路口区域离散化

我们已经为行为规划器定义了一个非常受限的操作设计域。现在开始实现行为规划器。首先，我们看看如何将交叉路口离散化，以便在环境中更简单地做出决策。

交叉路口区域可以划分为三个部分：

接近区：车辆应开始安全制动的区域，在图中以红色高亮显示。
停车区：车辆必须停车并等待合适时机继续行驶的区域，在图中以绿色高亮显示。
通过区：车辆实际穿过交叉路口的区域，在图中以橙色高亮显示。

每个区域的大小基于两个主要因素动态变化：

自车速度：速度越高，安全舒适停车所需的距离越长。
交叉路口大小：交叉路口越大，每个区域也必须越大。

定义状态与转换

为了处理这个场景，我们需要三种高级驾驶操作。

以下是三种操作状态及其定义：

保持车速：此状态仅受道路当前限速约束。通常，这是在进入交叉路口任何区域之前，或安全进入“通过区”之后执行的操作。其进入动作是设置速度限制。
减速至停车：此状态强制物体的未来轨迹在到达停止点前停车。其进入动作是定义停止点位置。
停车：此状态指示车辆在其当前位置保持停止。其进入动作是启动一个计时器，在继续通过交叉路口前等待固定时间。

现在，我们来看看自车在此场景中会遇到的不同情况，并找出编码正确行为规划解决方案所需的有限状态机元素。

让我们从自车进入交叉路口区域之前开始看起，此时它只有一个约束：遵循道路限速。这个约束是基于“保持车速”状态的进入动作设置的。

当自车进入“接近区”（红色区域）时，它必须开始减速至停车标志处，因此将转换到“减速至停车”状态。从“保持车速”转换到“减速至停车”的转换条件就是进入“接近区”。

然后，一旦开始减速，自动驾驶车辆必须执行的下一个操作是在停止线前或交叉路口的“停车区”内完全停止。为确保这一点，车辆保持在“减速至停车”状态，直到其速度为零且位置在“停车区”内。“减速至停车”状态的进入动作是建立一个安全的停车位置。由于场景简单，这是一个单一的停车位置，即高精地图提供给规划器的停止线。然而，我们将在下一课中看到，一旦有其他动态物体与自车交互，这将变得困难。

一旦完全停止，车辆进入“停车”状态。作为进入动作，会启动一个计时器，以确保车辆在继续行驶前（根据典型的驾驶规则）在停止状态停留三秒钟。

计时器完成后，规划器自动转换到“保持车速”状态，并遵循任务规划器提供的路线通过交叉路口，无论是左转、右转还是直行。以上就是使用有限状态机处理简单四向交叉路口所需的全部计算。

处理输入噪声与测试

在整个过程中，至关重要的是我们要理解我们作为人类专家如何分析场景，以及由此产生的行为规划器的具体能力是什么。这些需要被捕获在操作设计域定义中，并且我们需要确保创建一个完整的状态机，能够处理给定场景中可能出现的所有情况。

一个对状态机性能有重大影响的特定问题是输入中的噪声。上面定义的状态转换条件是精确的，依赖于车辆精确到达停止点并达到零速度。然而，即使没有其他动态物体需要检测，车辆状态的定位估计也可能包含噪声，无法精确满足这些条件。为了处理这种类型的输入噪声，我们可以引入噪声阈值超参数。这是一个小的阈值，允许接近零的速度被接受为停止状态。在下一课处理更复杂的动态物体场景时，我们将继续看到这些超参数。

现在我们已经完成了有限状态机的创建，如何测试它是否有效？传统上，要确认行为规划系统的功能，需要进行四个阶段的测试，这遵循了我们在第一门课程中对整车安全评估的讨论。

以下是四个测试阶段：

基于代码的测试：用于确认代码逻辑是否正确。例如，基于代码的测试可以告诉程序员，地图中设置的限速是否会是有限状态机产生的限速。然而，这些检查无法确认状态转换是否正确，或者状态是否能够处理给定场景中的所有情况。
仿真测试：在模拟环境（如Carla）中执行，状态机执行其设计要处理的场景。这种类型的测试能够确认状态机转换和状态覆盖是否正确。仿真中执行的测试数量应能代表驾驶该场景时可能遇到的所有可能情况，以捕获程序员可能遗漏的任何边界情况。通常，选择一个有代表性的测试集并非易事，尤其是随着场景复杂性的增加。
封闭场地测试：一旦确信状态机在仿真中按预期运行，就进行此类测试。这种测试针对那些在仿真中难以精确确认的特定场景，例如真实环境中的参数调优以及感知输出中的噪声和错误。
道路验证测试：所有之前的测试都是在高度受控的环境中进行的，而道路测试可能高度不可预测，并且经常以工程师未曾想象的方式使系统崩溃。然后，可以将场景的新变体纳入测试过程的早期阶段。

总结

本节课中我们一起学习了以下内容：

我们首先定义了要处理的场景和操作设计域：一个所有方向都有停车标志的单交叉路口场景。
然后我们看到了如何将交叉路口离散化，以便在制定状态图转换时使用每个区域。
接着，我们构建了系统的状态和转换，以正确定义整个交叉路口所需的行为。
最后，我们回顾了可用于确认场景中正确行为的四个测试阶段。

在下一节视频中，我们将展示如何在与其他动态物体交互的情况下处理相同的交叉路口场景。正如我们将看到的，这会使状态机变得复杂得多，也更有趣。我们下节再见。

022：处理有动态物体的交叉路口场景 🚦

在本节课中，我们将学习如何扩展行为规划器，使其能够在四向停车路口场景中安全地与动态物体（其他车辆）进行交互。我们将回顾关键的距离测量概念，扩展有限状态机以包含新的状态和转换，并简要讨论处理动态物体时可能遇到的复杂边缘情况。

回顾与动态物体的交互

上一节我们介绍了无交通情况下的基本行为规划器。本节中，我们将重点关注如何与动态物体进行交互。

为了安全地处理其他车辆，我们需要测量与这些物体关键交互点的距离。以下是三个核心概念：

到动态物体的距离：定义为自车当前位置到环境中任何动态物体中心的欧几里得距离。
到碰撞点的距离：到达与另一个动态物体潜在碰撞点的距离。
碰撞时间：到达给定碰撞点所需的时间。其计算方法在之前的视频中已讨论。

扩展有限状态机

现在，让我们扩展有限状态机，以应对动态物体带来的额外复杂性。我们需要将所需的机动动作增加到四个，以正确处理此场景中与车辆的所有交互。

首先，回顾我们第一版有限状态机中包含的机动动作：

保持车速：唯一约束是给定道路的速度限制。
减速停车：要求自车在环境中特定位置减速至停车点。
停车：保持在停车位置。

我们需要添加的新机动动作是：

跟随前车：此状态要求自车跟随前车速度并保持安全距离。前车是指同一车道中正前方的任何车辆。安全距离与速度相关，安全速度和安全距离在行为规划器的每次迭代中都会根据交互情况进行更新。

定义状态转换

定义了扩展有限状态机的所有状态后，我们开始填充状态转换。我们将采用与上一课类似的方式，从给定状态开始，识别所有从该状态出发的转换，然后再处理下一个状态。

以下是构建状态转换的过程：

1. 从“保持车速”状态开始

自车位于交叉路口外，处于“保持车速”状态。与上一课类似，一旦进入接近区，自车将转换到“减速停车”状态。

然而，这里有一个额外情况：当另一辆车出现在自车前方或驶入自车所在车道时。在这种情况下，我们将通过执行跟随检查，尝试转换到跟随该车辆的状态。

跟随检查可分解为两个要素：距离检查和同车道检查。

距离检查确认车辆足够近，应该被跟随。
同车道检查确认车辆确实与自车在同一车道。

对于距离检查，我们可以使用到动态物体的距离（到物体的欧几里得距离）并与一个阈值进行比较。
对于同车道检查，为简化起见，我们将检查前车是否在车道界限内，以及动态物体的航向是否在自车航向的给定阈值内，以表明其行驶方向相同。

2. “跟随前车”状态的转换

在“跟随前车”状态，我们只定义两个转换：

前车可能在自车接近或到达交叉路口之前驶离车道，导致切换到“保持车速”状态。
或者，当前车驶出当前车道（通常是进入交叉路口），而自车已处于“到达区”或“接近区”时，直接转换到“减速停车”状态。我们添加的转换条件是：当自车处于接近区或到达区时，到碰撞点的距离大于到停车点的距离。在这种情况下，自车应转换到“减速停车”状态，并将停车点设置为交叉路口停车线的正确位置。

3. “减速停车”状态的考虑

在“减速停车”状态，自车持续减速，直到在停车线处完全停止，这将导致切换到“停车”状态。

然而，我们必须再次考虑到，车辆有可能在交叉路口处切入自动驾驶汽车前方（例如从车道驶出或超车机动）。与之前类似，当自车处于接近区或到达区时，我们在整个“减速停车”状态期间执行跟随检查。如果检查返回为真，则转换到“跟随前车”状态。不过，在这种情况下，距离检查考虑的是前车的到碰撞点的距离是否小于停车点距离，以便优先进入跟随前车模式。

4. 完成扩展：从“停车”状态出发

最后，为了完成有限状态机的扩展，让我们看看一旦停车后可能面临的情况。为简化起见，我们假设一种非常保守的驾驶风格：我们的自动驾驶汽车会等待所有车辆清空交叉路口的接近区、到达区或通过区后，再继续通过。

我们将为四向交叉路口定义简单的检查，用四个简单的标签识别所有车辆的行驶方向，并且只考虑其他直行通过交叉路口的车辆。

相对于自车车身坐标系，相对航向在 -45度到45度 之间的车辆物体将被标记为与自车同向行驶。
相对航向在 45度到135度 之间的车辆将被标记为向右行驶。
相对航向在 135度到-135度 之间的车辆将被标记为朝向自车行驶（对向）。
相对航向在 -45度到-135度 之间的车辆将被标记为向左行驶。

可以使用动态物体的预测路径来改进这种简化，特别是在有额外的感知信息可用于改进对通过交叉路口行驶方向的预测时。

现在我们可以定义从“停车”状态出发的转换。我们将有两组转换：要么是“跟随前车”，要么是“保持车速”，这取决于车道中是否存在满足跟随检查的车辆。

在每种情况下，转换条件根据我们的任务计划所需的通过交叉路口的路径而变化：

当自车需要左转时，任何从左方、右方接近的车辆或任何对向车辆都必须清空交叉路口通过区，自车才能继续行驶。
当自车需要直行时，只有从左方或右方接近的车辆需要清空交叉路口。
当自车需要右转时，只有从左方接近的车辆需要清空交叉路口。

从这个简单的“停车”状态转换定义中，希望你能体会到处理因到达交叉路口顺序而产生的优先权问题，以及其他车辆也可能左转或右转的可能性所需的额外检查。

边缘情况讨论

到目前为止，在创建状态机的过程中，我们对动态物体的行为做了一个特别强的假设：即所有动态物体都遵守交通规则。然而，情况并非总是如此，这种困难导致了许多也需要考虑的边缘情况。

让我们快速讨论几个例子，说明这个强假设可能出错的情况：

驾驶员无意中驶入对向车道，进入自车车道。
激进驾驶员在自车已开始通过交叉路口时仍加速冲过路口。
车辆未在交叉路口停车。
车辆停放在交叉路口附近。如果该车辆未被标记为停放（从而可能被当作静态物体而非动态物体处理），我们的行为规划器可能会陷入死锁状态，等待这辆停放车辆移动。

这远非所有可能性的完整集合。行为安全评估和测试的主要目标之一就是尽可能多地发现意外行为的变化，以便它们也能被检测、分类并纳入行为规划设计过程。

然而，仅仅发现边缘情况是不够的。我们必须定义自动驾驶汽车应如何应对每一种情况。对于这些紧急机动（如急转弯或急刹车），需要定义更多的转换和条件。这是一个活跃的研究和开发领域。

总结

本节课中，我们一起学习了如何创建一个能够与遵守交通规则的动态物体进行交互的有限状态机。通过观察动态物体可能引发的众多可能边缘情况中的几个，我们看到了处理动态物体为何如此具有挑战性。

在下一课中，我们将看到如何继续构建我们的行为规划器，使其能够同时处理多个场景。下次再见。

023：处理多重场景

欢迎来到行为规划模块的第四个视频。在上一个视频中，我们为有限状态机行为规划器引入了动态对象。在本视频中，我们将开发一个更强大的行为规划器，它能够同时处理多种驾驶场景的决策过程。

我们将通过引入分层有限状态机的概念来实现这一点。同时，我们还将开发一种方法，用于在不同驾驶场景发生时进行切换。

让我们开始吧。

单一状态机的局限性

到目前为止，在本模块中，我们已经开发了一个能够处理单一动态四向停车路口场景的有限状态机行为规划器。

然而，在实际驾驶中，会遇到许多类似的场景。例如，三向停车路口、交通灯控制的路口以及直行道路场景等。虽然许多场景由于处理逻辑相似而具有共性，但每个场景本质上都需要不同的驾驶行为。

处理这些多重场景的一种方法是，在一个不断扩大的行为网络中，为单一状态机添加更多的状态、转换和条件。但是，这种方法存在几个问题。

以下是这种方法的主要问题：

规则爆炸：我们需要开发额外的逻辑来区分当前场景，以及处理每个场景所需的规则。这很快会导致“规则爆炸”问题，即添加新场景所需的规则数量变得难以想象地庞大。
计算负担：由于每个时间步都需要评估大量额外的规则和状态转换，所需的计算时间会显著增加。
复杂性与可维护性：随着需要考虑的情况数量迅速增加，这样的系统变得复杂且难以创建和维护。

本质上，随着复杂性的增加，我们失去了在第一课中强调的使用有限状态机的所有优势。

分层有限状态机

不过，我们可以采用另一种方法：将每个高级场景表示为单一状态。这允许我们在高级场景状态之间创建更简单的转换。

在这个例子中，我们可以看到状态代表了一个直行道路场景和我们之前处理的四向路口场景。对于每一个这样的高级场景状态，我们将存储一个低层级状态机，用于独立处理该场景。

例如，对于路口场景，低层级状态机就是我们在过去两节课中构建的那个。这种表示方法被称为分层状态机，其中超级状态代表每个场景，子状态代表在每个场景中要处理的驾驶操作。

高级场景状态机之间的转换，将是一个基于高清路线图和动态车辆信息来定义何时进入新场景的规则。

场景切换机制

假设我们有一个简单的分层状态机，包含两个场景：一个是带有行人横道的直行道路，另一个是基于停车标志的路口。

要从直行道路切换到停车标志路口，自动驾驶车辆必须接近该路口。这可以通过检查沿任务计划到下一个路口的距离来实现。

我们在这里讨论的所有转换都发生在超级状态层面。接下来要回答的问题是：在处理子状态操作时，我们如何在场景之间切换？

一种方法是在当前场景的超级状态中，为关键操作子状态引入转换出口。

让我们继续以路口场景为例。首先，确定哪个状态将是退出该场景的关键出口状态。我们能够退出路口的唯一方式，是在“跟踪速度”或“跟随前车”状态中，并且已经通过了路口。因此，让我们将出口转换放在那里。

转换的条件是确认该场景确实已完成。在这个例子中，我们可以使用到下一个停车标志路口的距离。如果这个距离大于给定的阈值，我们就可以退出路口场景。

通过这种转换方法，我们能够在场景切换之间保持操作。在这种情况下，路口超级状态中的“跟踪速度”状态将连接到我们进入的下一个场景的“跟踪速度”状态。

扩展性与优缺点

通过创建一个新的超级状态场景节点，并配备一个能够处理该场景的子状态状态机，就可以将额外的多车道场景添加到这个分层状态机中。

这种类型的有限状态机有其固有的优点和缺点。

以下是这种方法的优点：

计算效率：由于其在每个超级状态内创建独立状态机的细分特性，这种方法通过重构搜索空间，限制了任何单个时间步所需的计算时间。
易于测试和编程：这种细分特性也使得分层状态机的测试和编程变得显著更容易。

然而，分层状态机也存在问题。虽然它限制了所需规则的数量，但这种方法仍然无法完全解决规则爆炸问题。规则数量庞大的一个原因是，所有场景都有完全分离的状态机，因此许多规则在不同场景中为了处理相似行为而彼此重复。

最终，分层方法允许系统设计者处理比扁平的单层有限状态机更大量的复杂性。只要能在每一层定义关键状态，这个想法可以扩展到多层。

实际上，有限状态机所强化的“在任何给定时间只有一个行为是活动的，并且所有到其他行为的转换都可以明确定义”的观点，对其能处理的场景集合大小存在限制。尽管如此，它仍为自动驾驶汽车中的典型行为规划提供了一个有用的工具。

总结

在本节课中，我们一起学习了以下内容：

我们首先介绍了能够同时处理多个场景的分层有限状态机。接着，我们讨论了如何在超级状态和子状态层面，在不同驾驶场景之间进行转换。最后，我们探讨了使用分层有限状态机进行行为规划的能力与局限。

请加入我们的下一个视频，在那里我们将讨论一系列先进的研究方法，这些方法能够解决我们目前在行为规划中遇到的一些问题。

下次见。

024：行为规划高级方法

欢迎来到行为规划模块的第五个，也是最后一个视频。在本视频中，我们将讨论基于状态机的行为规划方法的局限性。然后，我们将概览一些旨在解决这些局限性的替代性行为规划系统。通过本视频的学习，您应该对行为规划中使用的主要方法有一个全面的了解。

让我们首先指出状态机模型存在的问题。

我们在整个模块开发过程中发现的第一个问题是规则爆炸问题。这意味着，随着我们开发出更完整的场景和处理策略集合，所需规则的数量会迅速增长。这个限制意味着，虽然可以开发一个有限状态机行为规划器来处理有限的操作设计域，但用有限状态机开发一个具备完整四级或五级自动驾驶能力的车辆几乎是不可能的。

处理噪声环境是下一个问题。虽然我们在第2课和第3课中看到了如何通过添加超参数来处理某些噪声，但这种噪声处理方法只能应对非常有限的情况。要处理所有类型的输入不确定性，需要不同的方法。

关于这一点，我想指出的下一个问题是超参数的实际调优。随着所需行为变得更加复杂，用于离散化环境和处理某些低级噪声的超参数数量迅速增长。所有这些超参数都必须非常仔细地调整，这可能是一个漫长的过程。

最后，我们将讨论的最后一个问题是处理未遇到过的情况。由于这种方法是基于编程的，很可能出现一种情况，即系统的编程逻辑会以不正确或非预期的方式做出反应。

基于状态机的方法被归类为专家系统，即由人类专家设计和开发的系统。然而，状态机并非唯一设计的此类专家系统。

还有一些方法依赖于规则数据库，这些规则在每个时间步都应用于输入数据。规则被组织成层次结构，其中安全关键规则优先于乘坐舒适性规则或防御性驾驶规则。每条规则只有在适当的条件出现时才会被激活，以影响输出策略的选择。

这种基于规则的系统不会遇到我们在分层状态机中遇到的规则重复问题，因为规则可以应用于整个操作设计域或其大部分区域。然而，基于规则的系统必须非常仔细地制定规则，以确保它们不会相互产生负面影响，或在多条规则同时激活时导致意外结果。

由于这两种专家系统通常都依赖专家用户来设计所有可能的场景，因此它们都面临着上述相同的问题。

针对我们上面指出的部分问题，一个可能的解决方案是扩展人类专家型规划器，使其融入模糊逻辑。模糊逻辑是一种系统，它使用一组清晰、明确定义的值来创建一组更连续的模糊状态。

为了简单了解基于模糊的系统如何工作，让我们看一下车辆跟随行为的例子。

之前我们设置了一个参数化的距离，将空间划分为跟随车辆或不跟随车辆。而在模糊系统中，我们能够拥有一个连续的空间，在这个空间上可以应用不同的规则。

例如，一个模糊系统可能对非常接近的前车做出强烈反应，但当距离较远时，对速度匹配或距离跟随的要求则不那么严格。

虽然基于模糊的规则系统比传统的离散系统更能处理环境噪声，但规则爆炸和超参数调优仍然是模糊系统面临的问题。实际上，模糊系统可能导致更严重的规则爆炸，因为处理模糊输入集需要更多的逻辑。伴随规则爆炸而来的是超参数调优的巨大挑战。

接下来我想讨论的一个令人兴奋的研究方法是使用强化学习进行行为规划。强化学习是机器学习的一种形式，其中智能体通过采取行动并接收连续奖励来学习如何与给定环境互动。

智能体首先在模拟环境中进行“游戏”，这意味着开始时智能体并不擅长特定任务。然而，随着时间的推移，随着智能体最大化其奖励，它们最终会掌握任务。通过在模拟环境中学习，在学习过程中经历的许多失败不会产生现实世界的后果。

更具体地说，我们可以说智能体正在尝试学习一个由 P 表示的策略，该策略将给定的环境 S 映射到给定的动作 a。为了将强化学习的概念与行为规划联系起来，假设我们正试图教自动驾驶汽车如何跟随前车。

在这种情况下，我们的策略 P 将尝试执行车辆跟随。我们的动作可以明确定义为要执行的行为，也可以通过选择期望的加速度和转向率来隐式定义行为。环境可以由一组连续的变量表示，这些变量告诉我们相关信息，例如到所有物体的距离和我们的任务路径。最后，奖励函数可以基于最佳跟随距离，在首选距离处给予最高奖励，对距离过近的惩罚比对距离过远的惩罚更重。

由于自动驾驶车辆可能遇到的情景和输入种类极其繁多，直接使用强化学习进行行为规划不太可能成功。因此，通常会应用一些进一步的调整。其中一种调整称为分层强化学习，我们将问题划分为策略空间中的低级策略和场景中的高级策略，这类似于分层有限状态机。然后，每个低级策略独立学习，只有在成功学习后，才能学习高级策略来完成一个场景。

经常应用的第二种技术称为基于模型的强化学习。在基于模型的强化学习中，智能体不仅尝试学习策略 P，还尝试学习智能体周围当前环境的模型。这种方法如何应用于自动驾驶汽车行为规划的一个例子是包含动态物体运动模型。如果智能体理解动态物体的运动模式，它就可以创建更有效的环境规划。

虽然强化学习是一个非常令人兴奋且前景广阔的研究领域，但它也有自身的局限性。许多用于学习自动驾驶所需策略的模拟环境过于简化，由于其简单性，学到的策略可能无法转移到现实世界环境中。过于逼真的模拟器则会导致严重的计算需求问题，尤其是在为自动驾驶学习运行数千次变化广泛的场景重复时。

第二个是关于安全的问题。虽然强化学习中有一些技术试图确保强化学习器创建的轨迹满足安全约束，但对于学习到的系统仍然无法进行严格的安全评估，因为它们在决策方式上大多是“黑箱”。

研究界目前还在探索许多其他有趣的想法。其中许多都试图从人类驾驶行为中学习。例如，在逆强化学习中，不是尝试在给定奖励函数的情况下获得策略，而是使用人类驾驶数据作为策略，并尝试学习人类使用的奖励函数。一旦学习了奖励函数，算法就可以像人类驾驶员一样执行驾驶操作。

最后，端到端方法将原始传感器数据作为输入，并尝试通过学习人类驾驶命令（在模仿学习方法中）来输出油门、刹车和转向指令。这种方法由NVIDIA的研究人员在其论文《End to End Learning for Self-Driving Cars》中开创。虽然这没有被明确归类为行为规划，但端到端方法在其输出选择过程中仍然隐式地执行了行为选择的任务。

这些简要的解释只是触及了这个庞大且快速发展的研究领域的表面。行为规划仍然是自动驾驶研究的热点之一，也是实现现实世界五级自动驾驶最棘手的瓶颈之一。我们在补充材料中为有兴趣深入了解的读者提供了一份该领域近期工作的阅读清单。

让我们总结一下本视频讨论的内容。

我们介绍了基于有限状态机规划的一些问题，并提出了多种解决行为规划问题复杂性的高级方法。

至此，我们的行为规划模块已经结束。在本模块中，我们定义了使行为规划系统能够实现自动驾驶所需的要求。然后，我们逐步创建了针对四路交叉口特定场景的这样一个系统。接着，我们添加了动态车辆交互，并扩展到包含多个场景。最后，我们简要回顾了行为规划研究界涌现的一些令人兴奋的方法。

请加入我们的下一个模块，在那里我们将使用参数化曲线和非线性优化构建一个完整的局部规划解决方案。

025：轨迹传播与反应式规划

在本节课中，我们将学习如何基于运动学自行车模型，构建一个反应式运动规划器。我们将介绍路径预测和碰撞检查的概念，并专注于静态环境下的规划，作为实现自动驾驶汽车行为与路径规划的第一步。

运动学模型与动力学模型

上一节我们介绍了本模块的目标，本节中我们来看看机器人运动模型的基础知识。

首先，我们需要区分运动学模型和动力学模型。

运动学模型描述了机器人的运动方程，但忽略了质量和惯性对其运动的影响。其输入通常是线速度、角速度及其导数。
动力学模型则考虑了质量和惯性的影响，因此更为复杂。其输入通常是力和力矩。

为了说明这一点，我们可以对比两个粒子模型：

运动学粒子模型：x(t+Δt) = x(t) + v * Δt
考虑摩擦的动力学粒子模型：m * a = F - μ * m * g

在路径规划和轨迹优化中，我们通常关注运动学模型，以使运动规划问题在计算上更易处理，而将简化动力学所引发的问题留给控制器去解决。

自行车模型回顾

现在，让我们回顾在第一门课程中介绍过的自行车模型。该模型是描述汽车运动的常用简化模型。

以下是其运动方程：

ẋ = v * cos(θ)
ẏ = v * sin(θ)
θ̇ = v / L * tan(δ)

这些方程的含义是：

机器人的状态由其位置 (x, y) 和相对于x轴的航向角 θ 定义。
模型的输入是速度 v 和前轮转向角 δ。
根据这些运动学方程，当前的机器人状态和输入共同决定了轨迹随时间的变化。

需要明确的是，我们通常无法直接控制机器人的状态（如直接指定其到达某个XY位置）。但我们可以设计一系列控制输入 U_n，根据运动学方程，使机器人到达目标位置。这一系列输入就对应了机器人将遵循的轨迹。

轨迹生成：从连续到离散

了解了运动学方程的例子后，你可能会问：我们如何为给定的输入序列实际计算出一条轨迹？

我们给出的运动学方程本质上是一个连续时间微分方程组。为了生成轨迹，我们将关注这些方程的离散化形式。离散化模型使我们能够轻松高效地为给定的控制输入序列传播轨迹。

有多种方法可以离散化连续时间微分方程，这里我们使用简单的零阶保持法。离散化的一个结果是，我们可以递归地实现序列中所有状态更新的求和运算。

以下是离散化后的递归更新公式：

x_{k+1} = x_k + v_k * cos(θ_k) * Δt
y_{k+1} = y_k + v_k * sin(θ_k) * Δt
θ_{k+1} = θ_k + v_k / L * tan(δ_k) * Δt

这种递归求解的优势在于计算效率。我们无需为轨迹上的每个点重新计算之前所有更新的总和，而只需使用前一个计算出的点，增量地计算下一个点。在本模块中，我们将一直使用这种递归解法。

轨迹传播的应用

如果我们将这种离散化方法应用于整个控制输入序列，就能得到机器人将遵循轨迹的精确近似。

这不仅对轨迹规划有用，也对运动预测非常有用。例如，在驾驶场景中，如果我们知道其他交通参与者的运动学模型，并对其可能采取的控制输入有合理的猜测，就可以估算出他们未来的轨迹，从而帮助我们规划自身的运动以避免碰撞。

以下是两个应用示例：

示例一：恒定输入
假设我们对自行车模型施加恒定的速度输入和恒定的转向角输入。如下图所示，这将导致机器人以恒定速度沿圆弧行驶，同时其航向角随时间步长缓慢变化。

示例二：避障机动
如果我们根据某个转向函数，在路径上改变转向角，就可以执行更复杂的机动，这对于避障等任务至关重要。这本质上是局部规划问题的核心：计算所需的控制输入，以安全导航到给定目标点。

如下图所示，如果像之前一样以恒定转向角行驶，将会导致碰撞。为了避免碰撞，我们给机器人一系列不同的转向角输入，使其成功绕开障碍物。

总结与预告

本节课中我们一起学习了运动学模型与动力学模型的区别，并回顾了自行车模型。接着，我们讨论了如何利用轨迹传播算法，为给定模型和控制输入集计算轨迹。该算法将在我们开发本模块其余运动规划器时派上用场。

希望本视频让你对如何根据机器人的运动学模型和任意输入生成轨迹有了深入理解。在下一个视频中，我们将讨论一些高效的碰撞检查方法，用于评估不同规划路径的安全性。下次见。

027：轨迹展开算法

在本节课中，我们将结合前两课的知识，开发一种称为“轨迹展开规划器”的反应式运动规划算法。这将为我们介绍轨迹规划的任务，并为后续学习模块7中更高级的规划方法打下基础。

概述

轨迹展开规划器是一种反应式规划方法。它通过在当前工作空间点生成一组候选轨迹，利用局部障碍物信息筛选无碰撞路径，并选择能最大化目标函数（通常包含向目标前进的奖励项）的路径。通过反复执行此过程，我们得到一个能对环境做出反应、同时稳步向目标前进的“滚动时域”规划器。

算法步骤详解

上一节我们介绍了算法的整体思路，本节中我们来看看实现轨迹展开规划器的具体步骤。

1. 轨迹生成

第一步是在每个时间步生成一组轨迹。在轨迹展开算法中，每条轨迹对应在固定时间范围内，对机器人持续施加一个固定的控制输入。

我们通过在可用输入值范围内均匀采样这些固定输入，来生成各种潜在的候选轨迹。通过覆盖输入频谱上的多种轨迹，我们提高了轨迹搜索的质量和机器人的可操作性，因为我们在探索更广泛的候选路径集。

如果只使用小范围的输入，计算时间会减少，但可能会错过计算中潜在的候选轨迹，从而降低规划器生成的轨迹质量。然而，采样过多的候选轨迹意味着每一步都有额外的计算开销，因为每条额外的轨迹都需要生成、检查碰撞并进行评分。

选定输入集后，我们需要使用车辆的运动学模型（如第一课所述）向前传播状态，从而生成沿轨迹的未来状态。

以自行车模型为例，两个输入是速度和转向角。如果我们保持速度恒定，但让转向角在 -π/4 到 π/4 的范围内变化，我们就得到一组弧线作为候选轨迹。这些弧线是通过递归计算运动学方程生成的。

2. 碰撞检查

现在我们有了弧线集，可以检查哪些是无碰撞的。对于碰撞检查算法，我们假设给定一个表示车辆工作空间离散化的占据栅格。

这种离散化以矩阵形式存储，矩阵中的每个值表示工作空间中对应位置是否被占据。然后，我们可以使用上一视频讨论的基于“扫掠区域”的方法进行碰撞检查。

回忆一下，我们通过将机器人的车身沿路径扫掠，并取给定轨迹每个时间步所有足迹的并集来生成扫掠区域。汽车的足迹对应于占据栅格中的一组索引，因此沿路径每个旋转和平移的点也对应于占据栅格中的不同索引。这些索引存储在集合数据结构中以消除重复项。

然后，我们可以检查扫掠区域的每个点，看哪些点与占据栅格中被占据的元素重叠。具体做法是遍历扫掠区域集合中的每个点，并检查占据栅格中相关索引是否存在障碍物。如果扫掠区域中任何点被占据，则该轨迹包含碰撞。

3. 路径选择与目标函数

完成上一步所有轨迹的遍历后，我们将得到一组无碰撞、运动学可行的轨迹，然后可以使用目标函数对它们进行评分。

每个目标函数都需要一个主要元素，即某种奖励向目标点或区域前进的方式，这是运动规划问题的最终目标。一个简单有效的方法是在目标函数中加入一个与候选轨迹末端到目标点距离成正比的项。

然而，有时我们也希望鼓励反应式规划器产生其他行为。正如我们在模块1中看到的，目标函数的例子包括最小化到车道中心线的距离或惩罚路径的曲率。有时我们也希望奖励那些最大化到最近障碍物距离的路径，以增加规划器未来时间步可用可行路径的灵活性。

正如模块1所讨论的，没有完美的目标函数，你需要根据应用场景设计自己的目标函数。对于我们的反应式规划器，我们将使用到目标的距离作为要最小化的目标函数，即此处显示的第一个公式 J = distance_to_goal。

一旦有了目标函数，我们就可以遍历无碰撞轨迹，并选择能最大化目标函数（或最小化惩罚，取决于目标函数的表述方式）的路径。

算法实例演示

让我们通过一个具体例子将整个算法串联起来。

假设占据栅格中的障碍物为红色，目标区域为黄色，汽车的初始点为 (0,0,0)。假设我们的转向角范围在 -π/4 到 π/4 之间，步长为 π/8。假设我们使用恒定速度 0.5 米/秒。此外，假设规划器使用 0.1 秒的时间步长，且每条规划轨迹总持续时间为 2 秒。

如果我们应用第一课概述的、使用自行车运动学模型的轨迹传播算法，就可以为转向角范围内每个选定的转向角生成一组路径。

第一条轨迹的转向角为 -π/4，因此轨迹向右弯曲。
下一条轨迹的转向角为 -π/8，生成的轨迹曲率比第一条小。
接下来是转向角为零的轨迹，导致汽车直线前进。
正转向角轨迹与负转向角轨迹对称，并使汽车向左转。

现在我们有了候选轨迹集，需要使用上一视频概述并在本节回顾的碰撞检查算法检查每条轨迹是否无碰撞。将足迹沿每条轨迹平移和旋转后，我们检查结果扫掠区域中每个占据栅格索引是否存在障碍物。如果任何索引包含障碍物，则该路径被标记为有碰撞（我们用红色表示）。所有无碰撞路径用绿色表示，然后我们可以使用目标函数进行评估以找到最佳路径。

回忆一下，我们的目标函数是到目标的距离。最小化此距离的路径现在用黑色标出。这就完成了我们的第一次规划迭代。

滚动时域规划

此时，我们有了供车辆执行的轨迹。然而，我们不会在下一次规划周期之前完全执行这条轨迹。相反，车辆只执行该周期中的前几个点。具体数量取决于规划频率，我们的规划时域将根据进度向前移动。

这正是您在本专项第一门课程中应用于车辆控制的滚动时域方法。

总结一下，在每个时间步，我们规划一条 2 秒的轨迹，但每次只执行其中的 1 秒。这样做，我们的规划时域的结束时间在每个规划周期向前移动 1 秒。这被称为滚动时域规划器，因为在每个规划周期，我们都有一个固定的规划时间范围，其结束时间慢慢向到达目标的时间点“后退”。

一旦下一个规划周期开始，我们重复整个过程。我们继续这个过程，直到计算出到达目标区域的轨迹（我们在每次迭代结束时检查）。在我们的示例问题中，您现在可以看到规划器到达目标区域所采取的其余步骤。

关于这个规划器需要注意的一点是，它是“短视”的，即它不直接规划一条通往目标的路径，而是根据子路径能让机器人多接近目标来贪婪地采样子路径。这可能导致规划器目光短浅，陷入死胡同，并且通常会导致规划器找到次优路径。然而，这个规划器大大降低了到达目标区域的规划问题的复杂性，并且速度足够快，可以用作在线规划器。

总结

本节课中我们一起学习了轨迹展开运动规划算法的步骤，结合了前两课介绍的关于轨迹传播和碰撞检查的概念。为了巩固这个概念，我们详细讲解了一个该算法运行的示例。最后，我们介绍了滚动时域规划器的概念，并讨论了它们在规划到目标区域时可能如何目光短浅。

现在，您应该对轨迹展开算法的工作原理有了很好的了解。在下一课中，我们将讨论动态窗口法，以及它如何帮助我们的轨迹展开算法生成更舒适的轨迹。

028：动态窗口法

在本节课中，我们将学习如何为上一课开发的轨迹展开算法增加一项名为“动态窗口”的技术。动态窗口法允许我们对车辆轨迹施加线性和角加速度约束，以提升车辆在规划周期之间行驶时的乘坐舒适性。

具体来说，学完本课后，你将能够为课程一中推导的自行车模型添加加速度约束，并能够使用动态窗口法修改轨迹展开算法以适应这些新约束。

回顾自行车模型

首先，让我们回顾第一课中讨论的自行车模型运动学方程。本质上，自行车模型的两个输入是线速度 v 和转向角 δ，它们会随时间改变机器人的位置和航向。

这套运动学方程的一个特点是，它没有考虑加速度或加加速度等高阶项。这些高阶项正是导致车内乘客不适的原因，因此我们应该尝试在运动学模型中解决这个问题。

引入加速度约束

即使不将课程一中讨论的完整动力学模型纳入轨迹规划过程，我们也可以通过限制所选输入来考虑快速变化对乘坐舒适性的影响。我们可以为自行车模型允许的线性和角加速度范围添加约束。

这样做将限制车辆在行驶其规划轨迹时，乘客所体验到的力和扭矩的大小。然而，这需要权衡取舍：我们的运动规划器在每次规划迭代中会失去一些机动性。

具体来说，在添加角加速度约束后，我们可能无法移动到转向角集合中的每一个可能的转向角，因为它们可能引起过高的角加速度。此外，我们也无法在规划迭代之间过快提升或降低速度。

推导角加速度约束

让我们专注于角加速度约束，并推导由此产生的转向角限制。

回想一下，自行车模型的角速度由公式 ω = v * tan(δ) / L 给出。因此，角加速度的大小近似等于起始和结束转向角的角速度之差的绝对值除以所使用的时间步长 Δt。

利用规划器中 v 和 L 始终为正的事实重新整理项，我们得到约束条件：|tan(δ₂) - tan(δ₁)| ≤ ω̇_max * L / v。

实例分析

为了帮助理解这对规划器的影响，让我们分析一个具体例子。

假设我们的自行车模型在所有候选轨迹上以恒定的线速度 v = 1 m/s 移动，当前转向角 δ₁ = π/8。假设该机器人的最大和最小转向角分别为 π/4 和 -π/4，转向角步长为 π/8。此外，假设我们的轨迹以 Δt = 0.1 s 的时间分辨率采样，自行车模型机器人长度为 L = 1 m。最后，将角加速度约束为 ω̇_max = 0.6 rad/s²。

将每个潜在的转向角代入我们的转向角集合，我们发现，如果我们将转向角改为 -π/8 或 -π/4，则会违反角加速度约束。然而，根据新约束，将其改为 π/4 或保持为 π/8 都是有效的选择。

为了说明这一点，我们将不允许的轨迹标记为红色。剩余的绿色轨迹仍然可供我们在上一课开发的反应式规划器的后续步骤使用。

这说明了，一般来说，添加的约束会在一定程度上降低机器人的机动性，同时促进更舒适的轨迹。约束集越严格，机器人的机动性就越差。

扩展到线性加速度约束

我们也可以将类似的逻辑应用于线性加速度约束和我们可用的线速度输入范围的情况，甚至可以同时将两种约束应用于机器人。

总的来说，动态窗口方法允许我们在规划过程中对轨迹的演变施加更多限制，从而产生能更好地同时满足广泛目标的运动。

总结

本节课中，我们首先为自行车模型引入了额外的加速度约束。然后，我们推导了动态窗口法的过程，以在每个时间步限制我们的轨迹集合，从而满足轨迹展开算法的这些新约束。

恭喜你完成了本模块的学习。让我们回顾一下所涵盖的内容。

我们首先发展了轨迹传播的概念，以便为给定的运动模型生成轨迹。接着，我们讨论了碰撞检查，这对于为自动驾驶汽车制定无碰撞运动规划是必要的。然后，我们将这两个概念结合到轨迹展开规划器中，并使用动态窗口法对其进行增强以处理加速度约束。

现在，你应该对以轨迹展开算法为核心的反应式规划有了坚实的基础。这是一种紧凑而有效的通用运动规划策略，适用于具有各种目标和约束的广泛规划任务。

在下一个模块中，我们将开始讨论动态环境，并使用轨迹传播和碰撞检测来预测其他物体的运动并判断是否可能发生碰撞。我们下节课见。

029：路径规划问题与参数曲线

在本节课中，我们将学习分层运动规划器的最底层——局部规划器。我们将介绍路径规划问题的基本要求、边界条件，并深入探讨用于表示路径的参数曲线，特别是样条曲线和螺旋曲线的区别与优劣。

概述

欢迎来到课程4的最后一个模块。本模块将讨论分层运动规划器的最底层，即局部规划器。局部规划器负责以无碰撞、高效且舒适的方式，执行行为规划器所请求的机动动作。其输出结果可以是一条轨迹（在给定时间点的一系列空间位置），也可以是一条路径及其速度剖面（一系列空间点及其对应的所需速度）。这个计划随后可以作为参考输入，传递给在第一门课程中开发的控制器。

本模块将基于我们在模块4中介绍的响应式规划器进行构建，使其能够处理自动驾驶运动规划问题中的一些细微差别。具体来说，我们将从离散时间转向连续时间，以生成平滑、参数化的路径，便于控制器进行跟踪。

路径规划问题与边界条件

理解路径规划问题的第一步是理解其最基本的要求。对于路径规划问题，给定起始位置、航向和曲率，需要找到一条满足车辆运动学约束、到达目标位置、航向和曲率的路径。

在优化问题的背景下，起始值和结束值可以表述为问题的边界条件，而车辆的运动学则表述为优化变量的连续时间约束。边界条件是指，对于任何可行的优化解，路径两端点必须满足的条件。如果违反了这些边界条件，无论路径本身多么优秀，我们都没有实现到达目标点的核心目标，因此该路径对我们无用。

参数曲线简介

为了简化优化问题的表示，我们将路径定义为一条参数曲线。什么是参数曲线？参数曲线是一组可以用特定参数描述的方程。这些参数通常表示路径的遍历过程，无论是通过弧长还是简单地从0变化到1。

例如，这里有一个用于描述路径X和Y位置的三次参数方程集合。方程的参数u从0变化到1，对应我们从路径起点到终点的行进过程。向量值函数R包含了对应于给定u值的每个点的x和y位置。

对于自动驾驶，我们通常（但不总是）要求路径是参数曲线。为什么？我们通常关注根据边界条件、运动学约束和目标函数来优化给定路径的规划方法。拥有路径的参数化表示使得建立优化问题更简单，因为我们有一个可以直接提供给目标函数F的函数。

两种常见的路径参数化方法

在自动驾驶领域，有两种常见的路径参数化类型。

五次样条曲线

第一种是五次样条曲线，它是车辆X和Y位置的五阶多项式函数。五次样条由两个方程给出，一个用于描述x沿样条的进展，一个用于y。它共有12个参数，6个用于x方程，6个用于y方程。这些参数对应于构成曲线形状的多项式系数。

五次样条的一个优点是，对于给定的位置、航向和曲率边界条件，存在一个满足这些条件的样条系数的封闭形式解。这非常理想，因为它允许我们立即生成满足边界条件的可行解，然后可以进一步优化。然而，五次样条的缺点是通常难以将曲率约束在特定范围内，而这在自动驾驶中经常是必需的。其曲率作为弧长的函数通常不是多项式，这可能导致样条中出现曲率的尖峰甚至不连续性，使得在整个样条域内近似满足曲率约束变得困难。

多项式螺旋线

作为替代方法，我们也可以使用多项式螺旋线来表示路径。这些曲线提供了沿其弧长每个点的曲率的封闭形式方程。对于自动驾驶，通常选择三次多项式作为弧长的曲率函数，当然高阶函数也是可以接受的。

使用多项式螺旋线的主要优点是，其结构非常有利于满足路径规划问题中经常要求的近似曲率约束。由于螺旋线是曲率的多项式函数，其曲率值不会像在五次样条中那样发生极端快速的变化。这意味着我们只需约束螺旋线上少数几个点的曲率，螺旋线就很可能在整个曲线上都满足曲率约束。这在执行路径优化时非常有用，因为约束数量会大大增加每次优化步骤的计算量。

多项式螺旋线的缺点在于，与五次样条不同，它没有位置和航向的封闭形式解。因此，我们必须执行迭代优化才能生成满足边界条件的螺旋线。位置方程会导致菲涅尔积分，而该积分没有封闭形式解，因此需要使用数值近似技术来计算螺旋线的最终端点。

样条与螺旋线的对比

在比较样条和螺旋线的优缺点时，我们几乎可以看到一种对偶性：一方的弱点正是另一方的强项。样条仅基于起点和终点就提供了封闭形式的解，而螺旋线则不能。螺旋线确保了沿路径的平滑曲率变化，而样条则不能。因此，您需要根据具体的应用场景来确定哪种方法更合适。简而言之，样条带来计算效率，而螺旋线则使曲率约束的实现更容易。

在本模块中，由于我们强烈关注确保局部规划器生成的路径能够被车辆平滑、安全地执行，因此我们将重点使用多项式螺旋线来开发我们的路径规划器。不过，后续描述的许多技术同样可以应用于五次样条。

总结

本节课我们概述了自动驾驶路径规划中使用的边界条件和参数曲线。我们介绍了样条曲线和螺旋曲线作为替代的路径表示方法，并讨论了它们在路径规划背景下的差异。

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

030：路径规划优化

在本节课中，我们将讨论如何将模块1中介绍的目标与约束，与上一课引入的三次螺旋线和边界条件相结合，从而构建一个路径规划优化问题。通过求解此问题，我们将能够生成满足所有约束的平滑、可行的路径。

在本视频结束时，你将能够：识别使用多项式螺旋线进行平滑路径规划所需的边界条件和约束；近似处理某些必需的约束以提高优化问题的可处理性；了解如何映射所需参数，以使优化问题能快速收敛到一个可行解。

边界条件与约束

上一节我们介绍了三次螺旋线的基本概念，本节中我们来看看如何为其添加约束。回顾上一课，边界条件描述了规划两点之间路径所需满足的最低要求。本质上，它们要求对于给定的起始位置、航向和曲率，规划的路径必须在特定的位置、航向和曲率结束。这为我们提供了优化问题的第一组约束，即边界条件。

然而，正如上一课所讨论的，三次螺旋线没有关于螺旋线末端位置的封闭形式解。为了用螺旋线参数来写出约束，我们需要使用数值积分技术。我们将应用辛普森法则。

应用辛普森法则

辛普森法则是一种常用的数值积分技术，通常比其他简单方法更精确。这是因为它评估的是给定函数二次插值的积分，而非某些方法（如中点法则和梯形法则）中的线性插值积分。

以下是辛普森法则的公式：

积分 ≈ (Δx/3) * [f(x0) + 4f(x1) + 2f(x2) + 4f(x3) + ... + 2f(x_{n-2}) + 4f(x_{n-1}) + f(x_n)]

其中，n 为偶数，Δx = (b - a) / n。

在我们的具体规划问题中，如果取 n = 8，该近似对于我们将要执行的优化来说足够精确，同时计算成本不会太高。

由于航向角 θ 是三次螺旋函数的积分，我们可以为其显式定义一个封闭形式的解，即一个四阶多项式。

然后，我们可以使用每个分割点处的 θ 值，通过辛普森法则近似计算三次螺旋线的 x 和 y 位置。x 和 y 的积分分别是 cos(θ(s)) 和 sin(θ(s))，将其代入辛普森法则中的 f，形成以下表达式。

现在，我们有了一个有用的近似，可以通过弧长参数 s 定义的任何弧长点来计算螺旋线的 x 和 y 位置。我们将使用辛普森法则计算的 x 和 y 近似值记为 x_s 和 y_s。

回到边界条件，我们现在有了路径终点位置的近似值，并且可以用螺旋线的已知参数写出边界条件。

运动学约束

在定义目标函数之前，我们需要考虑运动学约束。对于自动驾驶路径规划，我们将重点关注曲率约束。汽车有绝对的最小转弯半径，并且需要保持在横向加速度限制内，以维持车轮牵引力和乘坐舒适性。

现在假设我们的汽车可以实现的最小转弯半径为 2 米，这对应于最大曲率 κ_max = 0.5 弧度/米。

在螺旋线上的每个点都写出这个曲率约束相当困难。然而，由于螺旋线的多项式性质，我们只需要约束曲线上几个均匀分布的点。因为曲率的多项式函数是连续且行为良好的，在执行优化时，我们很可能生成满足曲率要求的螺旋线。

为简化起见，让我们约束曲线在三分之一和三分之二弧长点处的曲率。起点和终点的曲率已在边界条件中约束。

完成这一步后，我们现在有了作为螺旋线参数函数的曲率约束。我们拥有了求解优化问题所需的所有约束。

目标函数：弯曲能量

拼图的最后一块是我们希望最小化的实际目标函数。我们希望鼓励规划路径的平滑性和舒适性，一种方法是将绝对曲率均匀分布在路径上。这可以通过最小化规划参数曲线的弯曲能量来实现。

曲线的弯曲能量是其曲率平方沿整个路径弧长的积分。由于我们有一个描述三次螺旋线的曲率多项式函数，弯曲能量积分在螺旋线参数方面有一个封闭形式的解。此外，其梯度也有封闭形式的解。

现在我们已经有了目标函数，可以将所有内容整合到我们的路径规划优化问题中。

优化问题的简化与重构

出于我们的目的，我们将假设初始边界条件为零。这意味着我们在车辆坐标系中定义局部规划问题，并导致本视频中定义的、使用辛普森法则的航向及 x、y 近似表达式得以简化。这意味着初始边界值约束可以被移除，因为它们已经包含在我们的积分计算中。

然而，这个优化问题的设置存在一个实际问题，可能导致在使用标准非线性规划求解器求解时速度变慢或根本不收敛。主要问题与最终位置和航向的等式约束有关。因为等式约束必须被精确满足，对于数值优化器来说，从一个不可行的起点（通常是任意问题实例给优化器的起点）生成可行解是相当困难的。

为了缓解这个问题，在优化中通常将等式约束软化以提高优化器性能。软约束将严格的约束转换为目标函数中惩罚严重的项。通过“严重惩罚”，我们的意思是约束惩罚项系数应至少比一般优化目标大一个数量级。

我们还将假设初始曲率已知且通常设为 0，这对应于 a0 = 0。这减少了一个优化变量。

软化这些约束后，我们的新优化问题如下。

我们需要解决的最后一个问题与优化参数有关。虽然使用三次螺旋系数在我们的目标函数中更有直观性，但考虑到最终的曲率边界约束，我们实际上可以减少搜索的参数数量。

让我们使用一组不同的参数（记为向量 P）来重新定义三次螺旋线，其中 P 有五个元素：P0 到 P3 表示起点、三分之一点、三分之二点和终点的曲率。最后一项 P4 是路径的最终弧长。

方便的是，我们在曲率参数和螺旋参数之间有一个封闭形式的映射。因此，我们可以轻松地将所有约束和目标项计算为这些新 P 变量的函数，而不是螺旋线的系数。优化解决后，我们可以使用这里的方程将结果映射回螺旋系数。

由于我们已经知道起点和终点的曲率，我们可以消除两个变量：P0 和 P3。这使我们的优化问题只剩下三个变量：P1、P2 和 P4。通过利用边界条件，我们减少了优化问题的维度，这将显著提高计算速度。

最终的优化问题如下。我们已经用映射到 P 参数后的等效函数替换了螺旋参数函数。请注意，初始和最终路径曲率 P0 和 P3 是常数，因此优化变量现在仅为 P1、P2 和 P4。由于路径起点和终点的曲率边界条件是已知的，这种简化才成为可能。

总结

本节课中我们一起学习了如何构建平滑路径规划的优化问题。

首先，我们回顾了此问题所需的边界条件和约束。然后，讨论了如何使用辛普森法则数值计算螺旋线的终点位置。接着，引入了弯曲能量目标以鼓励平滑性，并形成了一个通用的螺旋优化问题。最后，我们重新映射了优化函数的参数，以确保快速收敛到可行解。

虽然有很多新信息需要消化，但我们希望本节课让你深入了解如何执行平滑路径规划。本节课将我们在模块1和模块4中讨论的许多主题联系在一起。如果你觉得有些内容具有挑战性，可以在重新学习本课之前复习那些模块。在下一课中，我们将讨论如何在Python中执行优化，为你实现完整的路径规划器做准备。

031：Python中的优化实现 🐍

在本节课中，我们将学习如何在Python中实现优化，以巩固本模块及整个专项课程中讨论过的优化概念。我们将重点介绍如何使用SciPy优化库来解决通用的非线性优化问题。

通过本视频的学习，你将掌握如何使用该库来设置和调用一个带约束的优化问题。具体来说，你将学会如何向优化器传递雅可比矩阵，以及如何定义优化问题中所需的参数边界。

优化库简介

上一节我们介绍了优化的基本概念，本节中我们来看看如何在Python中实现它们。优化是一个内容极其丰富的领域，我们无法在本专项课程中深入探讨所有细节。

SciPy优化库涵盖了一些最流行的优化算法，使其易于访问并保证了合理的实现效率。许多已实现的优化方法在所需参数类型上具有相似的结构。为了将其抽象成一个简单的接口，SciPy优化库包含了一个通用的minimize函数。

以下是该库中可用的一些优化方法示例：

共轭梯度法
内尔德-米德法
狗腿法
BFGS法

关于这些方法的更多细节，请参阅补充材料中的链接。库将运行的具体优化算法取决于你传递给此函数的method参数。method参数还将决定优化算法需要哪些额外参数。

设置优化问题

例如，在我们将要使用的L-BFGS-B算法中，我们不仅需要最小化的模型，还需要模型的雅可比矩阵和变量边界。当模型是单标量值函数时，雅可比矩阵简化为梯度。这个雅可比矩阵通过jac参数传递给minimize函数，如函数调用所示。

我们希望最小化的实际函数是minimize函数的第一个参数。约束通过constraints变量作为约束字典或对象的列表传入。此外，还有一个可选的options参数，高级用户可以用它来自定义优化器的输出内容。这些优化算法还需要为模型或目标函数的优化变量提供一个初始猜测值，这在函数调用中由x0给出。

具体示例：BFGS算法

让我们以BFGS算法为例，具体说明如何使用SciPy实现优化。本质上，对于BFGS算法，我们需要传入我们希望最小化的实际目标函数的函数指针，以及一个评估目标函数雅可比矩阵的函数的函数指针。这些函数将接收所有优化变量的向量，以便在特定点评估目标函数和雅可比矩阵。

优化完成后，minimize函数将返回一个result变量。结果中名为x的成员变量将返回达到局部最小值时的最终优化变量向量。

定义约束

正如我们之前提到的，我们也可以为大多数算法的优化问题指定约束。这些约束以列表或字典的形式给出。最简单的约束类型是目标变量的不等式约束，称为边界。

边界在L-BFGS-B算法中指定为一个列表的列表，其中每个子列表长度为2，包含每个优化变量的上下界。换句话说，第一个子列表对应x0的边界，第二个子列表对应x1的边界，依此类推。然后，这些边界被传递给minimize函数的constraints可选参数。

线性和非线性约束也可以传递给优化器，但目前我们将重点放在使用边界作为优化约束。更多细节，你可以查看在线的SciPy优化文档。

你还可以通过传入一个Python列表来组合多种类型的约束，列表中包含你希望在优化器函数中使用的每个约束对象。

总结

本节课中我们一起学习了如何使用SciPy优化库来设置优化问题。具体来说，我们讨论了如何将用户定义的目标函数、雅可比矩阵以及参数边界传递给优化器。你现在应该对如何使用这个Python库解决一般优化问题有了很好的了解。更多信息，你可以查阅SciPy优化库的文档。

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

032：共形格点规划

在本节课中，我们将运用前几课开发的优化技术，推导一个完整的路径规划器——共形格点规划器。学完本视频后，你将能够实现一个共形格点规划器来解决路径规划问题。你将理解如何沿道路采样点，以及如何使用第二课开发的优化公式为每个点规划路径。你还将能够判断路径是否无碰撞，并选择最能跟踪所需道路的最佳路径。

概述与目标

上一节我们介绍了路径规划的基本概念，本节中我们来看看共形格点规划器的核心思想。

与所有路径规划器一样，其目标是从自动驾驶汽车的当前位置规划一条可行且无碰撞的路径到达给定的目标状态。共形格点规划器利用道路的结构化特性来加速规划过程，同时避开障碍物。它专注于那些在目标路径左右轻微转向的平滑路径选项，因此生成的规划结果非常接近人类驾驶行为。

在道路上规划路径时，除非遇到紧急停车情况，汽车通常不应考虑离开道路。因此，共形格点规划器选择一个中心目标状态，以及一系列替代目标状态，这些替代状态是通过相对于道路航向从中心目标状态横向偏移形成的。

下图展示了一个共形格点的例子，其中每条路径的终点都从中心路径横向偏移，而中心路径对应于道路上的一个目标点（以金色高亮显示）。

我们可以这样做，是因为通常汽车应该沿着车道向前行驶。我们不关心那些不会导致前进的路径，因此可以大大减少搜索空间，使共形格点规划器在计算上易于处理。

目标状态选择

那么，我们如何选择这个目标状态呢？一般来说，为路径规划选择目标状态时存在一个关键的权衡。

如果你选择一个靠近当前车辆位置的目标状态，可以减少找到通往目标点路径所需的计算时间。然而，这也会降低规划器以平滑舒适的方式避开路径前方更远处障碍物的能力。这在高速行驶时可能成为问题，因为汽车在规划周期之间会覆盖更长的距离。

通常，我们规划中使用的目标范围是基于汽车速度和天气条件等因素动态计算的。然而，为了本模块的简洁性，我们将使用一个固定的目标范围。我们将目标点取为沿车道中心线、位于我们前方一个目标范围距离的点。

下图展示了这一点，金色点对应选定的目标位置，蓝色点对应横向偏移的目标点，它们将用作格点中每条螺旋线的替代终点约束。车道中心线的黑色部分弧长等于我们选定的目标范围。

在每个规划步骤中，我们将基于相同的范围重新计算目标点，并沿着车道向前推进。

路径生成与采样

一旦找到这些目标状态，我们就可以计算到达每个目标所需的螺旋线。此时，我们暂不关心路径是否无碰撞，只希望得到到达每个目标状态的、运动学上可行的路径。

因此，我们可以使用第二课开发的优化公式来求解从当前位置到每个终点位置的三次螺旋线。如果任何螺旋线在运动学上不可行或无法到达所需的目标状态，我们就丢弃这些螺旋线，不再将它们视为潜在路径。

请注意，一旦优化问题得到解决，我们只有结果参数向量 P。然后，我们必须撤销最初对螺旋系数执行的变换，以便从 P 向量中检索它们。

获得螺旋系数后，我们可以沿着螺旋线采样点，以获得整个路径的离散表示。由于我们没有沿螺旋线位置的封闭形式解，我们再次需要进行数值积分。然而，由于这次我们需要在整个螺旋线上的多个点处评估积分，因此需要一种更有效的方法来解决这些积分。

这里我们应用线性插值方法——梯形法则。梯形法则在此上下文中比辛普森法则高效得多，因为曲线上的每个后续点都可以从前一个点构造出来，所以我们只需要对螺旋线进行一次扫描即可获得所有需要的点。而辛普森法则则需要为每个点求解积分近似，效率要低得多。在Python中，我们可以使用累积梯形函数来实现这一点。

应用梯形法则后，我们现在有了每个目标点对应螺旋线的离散表示。重要的是，我们要跟踪每个点的曲率以及位置和航向，因为这将有助于我们后续的速度剖面规划。曲率和航向有封闭形式的解，因此不需要数值积分。

应用梯形法则后，我们现在得到了为每个目标状态生成的一组路径，如下图所示。

碰撞检测

现在我们有了完整的路径集，需要看看哪些是无碰撞的。为此，我们可以使用模块4中讨论的任何一种碰撞检测技术。最通用的方法是使用二进制占据栅格，如果单元格被占据则值为1，否则为0。

然后，我们可以获取汽车在占据栅格单元格中的足迹，并将该足迹扫过螺旋线上的每个点，以生成路径的覆盖区域。如果覆盖区域中某个单元格的占据栅格包含障碍物，则该路径将与障碍物发生碰撞，应标记为有碰撞。如果沿着整个路径的覆盖区域中的所有单元格从未发生这种情况，则该路径被视为无碰撞。

或者，如果自车和车道中的每个障碍物都可以用圆形近似包围，我们可以使用圆形检查。然后，我们将自车的圆形放置在路径上的每个点，并检查与落在自车车道内的每个障碍物的碰撞。

为了说明碰撞检查的结果，我们在路径中添加了一辆停放的车辆，现在我们需要规划绕行。在将覆盖区域扫过每条螺旋线后（类似于我们在模块4中的做法），我们将无碰撞路径标记为绿色，将与障碍物碰撞的路径标记为红色。

路径选择

此时，我们有一组可行且无碰撞的路径，但需要一种方法来选择最佳路径进行跟踪。选择过程在很大程度上是一个设计选择，因为对于给定的规划应用，可能有多个有用的标准。

例如，我们可能希望选择尽可能远离障碍物的路径，因此我们可能会为过于接近占据栅格中障碍物的路径添加惩罚项。我们可能还希望惩罚偏离最近车道中心线太远的项，因为我们不想在执行变道时长时间跨线行驶。

目前，我们将使用一个简单的度量标准，即偏向规划器从路径集中选择尽可能接近中心目标状态的路径。只要惩罚随着你远离中心目标而增加，具体的惩罚函数并不重要。

通过偏向中心路径，我们鼓励规划器遵循参考路径，只有当参考路径不可行或与障碍物碰撞时才允许其偏离参考路径。为简单起见，我们可以将此函数取为从中心目标状态到我们正在检查的路径的目标状态的位移。

然后，我们可以遍历路径集中的每条路径，找到最小化此惩罚的路径，并选择它作为最终路径发布。在我们的示例中，选定的路径以蓝色高亮显示。

如果现在随着汽车沿路径移动，我们从多个时间步重复此过程，我们的规划器就能够规划出一条在避开障碍物的同时收敛到目标状态的路径。类似于我们在模块4中开发的反应式规划器，该规划器以后退视野的方式朝着车道尽头的目标前进。

总结

本节课中我们一起学习了构建一个完整路径规划器的关键步骤。

在本视频中，我们首先定义了共形状态格点规划器方法，该方法选择沿道路在我们前方某个目标点横向偏移的点。然后，我们使用第二课开发的螺旋优化方法为每个点规划路径。接下来，我们讨论了如何修剪这组路径使其无碰撞，以及如何选择剩余的最佳路径。最后，我们集成了后退视野方法来完成我们的路径规划器。

现在你已经对整个路径规划算法有了概览，希望你能够看到本课程的许多内容如何最终形成了一种有效解决路径规划问题的方法。在下一个视频中，我们将讨论如何获取由路径规划器计算出的输入路径，并为汽车沿该路径移动时生成要遵循的速度剖面。然后，我们将拥有一套完整的参考信号传递给控制器执行，从而完成分层运动规划过程的所有三个阶段。

035：最终项目解决方案详解 🚗

在本节课中，我们将详细解析课程4最终项目的解决方案。该项目综合了本课程及专项课程中涵盖的多个核心概念。我们将回顾完成项目所需的四个主要任务，并逐一解释其实现方法。

概述

最终项目要求实现一个功能完整的运动规划器，以应对Carla模拟器中的三阶段场景。解决方案主要围绕四个核心步骤展开：路径生成、路径碰撞检查、速度曲线生成以及行为规划。接下来，我们将深入探讨每个步骤。

路径生成 🛣️

上一节我们概述了项目目标，本节中我们来看看第一个任务：路径生成。本项目已提供优化螺旋线至目标点所需的全部代码。因此，您需要完成的主要工作是从给定的航点序列中提取目标点。

以下是实现步骤：

迭代航点：遍历所有航点，找到距离自车最近的航点。
选择前瞻点：从最近航点开始，继续向前遍历，直至找到一个距离自车至少为“前瞻距离”的航点。这个距离代表了我们在每个规划周期内为车辆规划的前方距离。
生成目标状态集：选定目标航点后，沿该航点道路航向的垂直方向（旋转90度）进行横向偏移，生成一组目标点。这为车辆提供了多个可选路径，以便规避障碍物。横向偏移确保了规划的路径集符合道路结构。
坐标变换：将这些目标点从全局坐标系转换到自车坐标系。在自车坐标系中，自车位于原点且航向角为零。
路径优化：将转换后的目标点传递给螺旋优化器，计算得到可用于后续规划步骤的路径。

完成路径生成后，我们得到了一组候选路径。

路径碰撞检查与选择 🚧

现在，我们需要从路径集中计算并选择一条最佳、可行且无碰撞的路径供自车跟随。

我们采用在模块4中介绍的基于圆形的方法进行碰撞检查。

具体实现如下：

覆盖圆形：沿着路径上的每个点，在车辆位置上方覆盖圆形。这生成了一组圆形，保守地近似了自车行驶的扫掠区域。
碰撞检测：检查每个障碍物点是否位于路径上任何圆形内部。
标记碰撞路径：如果有任何障碍物点落入圆形内，则将该路径标记为存在碰撞。
筛选路径：对每条路径重复此过程，将所有与障碍物发生碰撞的路径从后续考虑中移除。

碰撞检查完成后，我们从无碰撞的路径中，选择终点最接近车道中心线的那条作为最佳路径。这样选择是为了让我们能够沿着车道中心线跟踪全局航点，即使因为路径上的障碍物而需要暂时偏离路线。需要注意的是，这种选择可能导致车辆尽可能贴近障碍物行驶，因此我们必须确保碰撞检查是保守的，并留有一定的缓冲空间。

速度曲线生成 ⚡

选定合适的路径后，我们需要为其生成速度曲线。根据要执行的行为，我们会获得一个参考速度。

速度规划逻辑如下：

如果正在减速停车或保持停止状态，目标速度应为 0。
否则，目标速度应为道路限速。
然而，我们必须考虑前方车辆。如果前车速度比我们慢，我们可能会撞上它。为了处理这种情况，我们将目标速度设置为 min(参考速度，前车速度)。
我们还需要确保在到达前车原始位置时，速度已降至前车速度，否则仍有碰撞风险。

基于以上信息，我们现在可以沿着路径构建一个从当前速度到目标速度的斜坡轨迹。

如果存在前车，斜坡曲线的终点将是 前车位置 - 时间间隔缓冲。
否则，斜坡曲线的终点将是路径的终点。

行为规划 🚦

本项目中将面临的唯一交通管制元素是停车标志，因此这将是行为规划器的重点。

我们的行为规划器将包含三个状态：

正常车道跟随状态
减速至停止状态
保持停止状态

以下是状态机的运作流程：

在正常驾驶过程中，车辆保持在 车道跟随状态。
一旦规划路径的终点穿过停车标志的停止线，我们就从正常驾驶切换到 减速至停止状态，目标是在该停止线定义的停止点停车。为此，必须确保规划的路径长度能提供足够的时间进行舒适减速。
车辆完全停止后，转换到 保持停止状态，确保车辆在停车标志处完全停止一段固定时间（本项目为2秒）。
时间结束后，转换回 车道跟随状态，继续进行正常驾驶。

这样就完成了状态机的转换循环，使我们能够处理路径上的停车标志。

总结

本节课中，我们一起学习了最终项目解决方案的四个核心组成部分：路径生成、碰撞检查与路径选择、速度曲线生成以及行为规划状态机。完成所有这些步骤后，我们就得到了一个功能完整的运动规划器，能够处理本项目提出的场景。

如果您在项目的任何部分遇到困难，可以尝试按照本视频讨论的方法重新实现该部分，以查看是否能提升其性能。当然，课程中对本项目的许多方面都定义了多种方法，您选择使用哪种方法并无限制。如果您找到了其他非常有效的方法，欢迎在论坛中与我们分享。

现在您已经完成了课程项目，应该对课程4中涵盖的运动规划概念有了扎实的理解。在最后两个视频中，我们将结束本课程以及整个专项课程。感谢您的辛勤付出，祝您在未来的所有自动驾驶项目中一切顺利！

037：课程总结与未来展望 🚗

在本节课中，我们将对多伦多大学自动驾驶汽车专项课程进行总结，回顾所学内容，并探讨未来的学习方向与职业机会。

大家好。Stephen和我本人想亲自花时间感谢大家为完成这个专项课程所付出的所有努力，并祝贺大家出色地完成了任务。

你们已经完成了多伦多大学的自动驾驶汽车专项课程。

你们应该已经为进入自动驾驶汽车领域的职业生涯做好了准备。

无论是在学术界还是工业界，都有大量的机会可以继续在这一领域发展和贡献力量。

我们需要像你们这样的人来帮助我们共同构建自动驾驶汽车的未来。

课程内容回顾 📚

在这个专项课程中，我们涵盖了大量内容。

从基础的车辆控制，到状态估计，再到感知，最后到路径规划。

这些部分中的每一个对于整个自动驾驶汽车软件栈都至关重要。

现在，你们已经将这些动态的部分整合起来，创建了一个功能完整的自动驾驶系统。

补充学习资源 💡

在你们离开之前，我们想提醒大家几个额外的项目，这些可能是在学习主要课程内容时没有时间仔细查看的。

以下是课程中包含的补充资源：

专家访谈：我们收录了多位来自工业界和学术界的同事的精彩访谈，为你们正在学习的许多方面提供了额外的视角。我们采访了安全保证专家 Kristof Shanetsky、Oxbotica 联合创始人兼首席执行官兼牛津大学教授 Paul Newman，以及在湾区领先公司积极参与自动驾驶领域的三位工程师 Diana Burges、Winston Liu 和 Andy Wong。
延伸阅读材料：我们在整个专项课程中包含了延伸阅读的链接。这些材料经过精心挑选，旨在帮助你们在深入研究课程涵盖的任何主题时，能轻松找到一个好的起点。

我们希望你们能花时间查看这些优秀的资源，并在论坛中进行讨论。

职业前景与未来方向 🛣️

你们所学习的内容应该已经为你们进入自动驾驶汽车行业的职业生涯铺平了道路。

这个行业正在蓬勃发展，对具备你们所掌握技能的工程师和开发人员有着巨大的需求。

从这里出发，你们可以朝许多方向发展。以下是几个可能的方向：

性能优化：你们可以选择进一步探索如何调整和改进自动驾驶汽车软件以获得更好的性能。
高级功能开发：或者，例如，使车辆能够执行更具挑战性的机动动作。
深入理论研究：你们也可以选择更详细地研究课程中描述的某项技术背后的数学原理，以更好地掌握所涉及的原则。

可能性与机遇是无穷无尽的。最终，选择哪条道路取决于你们自己。

但你们现在已经掌握了基础知识，可以为一个将对世界产生巨大影响的领域做出贡献。

你们可以帮助塑造自动驾驶汽车将为我们社会带来的积极变化，并从根本上改变我们的互动方式。

结语与祝福 🌟

带领大家走过我们讨论过的关于如何构建自动驾驶汽车的所有不同主题的旅程，对我们来说绝对是一段激动人心的经历。

我们希望你们和我们一样享受这个过程。

这对 Stephen 和我自己来说都是一次巨大的学习经历。

我们很期待看到你们从课程中学到了什么，以及你们未来将如何运用所学的知识。

路上见。

posted @ 2026-03-26 12:27 布客飞龙III 阅读(14) 评论(0) 收藏举报

刷新页面返回顶部

龙哥盟