一文掌握百度Apollo Udacity自动驾驶课程笔记

LZ第一次使用幕布完成了对Apollo课程的课后笔记，欢迎大家观看。

自动驾驶的六大模块：高精地图、定位、感知、预测、规划、控制，本次课程都有涉及。毕竟是入门课程，作为了解自动驾驶机制是很不错的课程。LZ今天用一天时间，从早晨起床开始2倍速在B站刷视频，一共有100集，每一小集也就2-3分钟，上午刷完，下午对整篇课程进行整理，现在脑壳要爆炸。。。哈哈哈哈，还是挺有成就感的。接下来打算对Apollo的进阶课程进行观看整理，看看在实际项目中无人驾驶具体面临的问题是什么。

目录

一、高精地图

1、高精地图与传统地图

2、高精地图与定位、感知、规划的关系

3、Apollo高精地图与构建

二、定位（厘米级）

1、GNSS RTK

2、惯性导航

3、激光雷达定位

4、视觉定位

5、Apollo定位

三、感知

1、计算机视觉

2、Camera图像

3、Lidar图像

4、机器学习

5、神经网络

6、反向传递

7、卷积神经网络CNN：

8、检测与分类：

9、跟踪：

10、分割：

11、Apollo感知：

12、传感器比较：

13、感知融合策略：

四、预测

1、简介

2、不同的预测方式

3、基于车道序列的预测

4、障碍物状态

5、预测目标车道

6、递归神经网络RNN在目标车道预测的应用

7、轨迹生成

五、规划

1、路径规划使用三个输入

2、将地图转为图形

3、路径查找算法A*

4、轨迹生成

5、Frenet坐标系

6、路径-速度解耦规划

7、路径生成与选择

8、ST图

9、速度规划

10、路径-速度规划的轨迹生成

11、Lattice 规划

12、ST轨迹的终止状态

13、SL轨迹的终止状态

14、Lattice规划的轨迹生成

六、控制

1、控制流程

2、PID控制

3、PID优劣对比

4、线性二次调节器LQR

5、模型控制预测

6、总结

 

---

一、高精地图

• 前言：目前 Apollo 内部高精地图主要应用在高精定位、环境感知、决策规划、仿真运行四大场景，帮助解决林荫道路GPS信号弱、红绿灯是定位与感知以及十字路口复杂等导航难题。

• 1、高精地图与传统地图

  • 1.1 高精地图是当前无人驾驶车技术不可或缺的一部分。它包含了大量的驾驶辅助信息，最重要是包含道路网的精确三维表征，例如交叉路口布局和路标位置

    

  • 1.2高精地图还包含很多语义信息，地图上可能会报告交通灯不同颜色的含义，也可能指示道路的速度限制，及左转车道开始的位置。

    

  • 1.3 高精地图最重要的特征之一是精度，GPS导航只能到达米级精度，而高精地图可以使车辆能够达到厘米级的精度，这对确保无人车的安全性至关重要。

• 2、高精地图与定位、感知、规划的关系

  • 2.1 定位：首先车辆可能会寻找地标，我们可以使用从各类传感器收集的数据，如摄像机图像数据、激光雷达收集的三维点云数据来查找地标。车辆将其收集的数据与其在高精地图上的已知地标进行比较，这一匹配过程是需要预处理、坐标转换、数据融合的复杂过程。横向：单目相机拍摄实虚线和地图对比，知道在那个车道。纵向：借助交通信号灯、路灯、灯杆等实现定位

    

  • 2.2 感知：1）Camera、激光雷达超过一定距离受到限制，地图保证提前5-10公里感知。2）恶劣天气或夜间、遇到障碍物，传感器无法识别物体。3）即使传感器尚未检测到交通信号灯，高精地图也可以将交通信号灯的位置提供给软件栈的其余部分，帮助车辆做下一个决策。4）高精地图可帮助传感器缩小检测范围，ROI区域提高检测精度和速度。

    

  • 2.3 规划：1）高精地图可帮助车辆找到合适的行车空间，还可以帮助规划器确定不同的路线选择，来帮助预测模块预测道路上其他车辆将来的位置。2）在具有低速限制、人行横道或减速带的区域，高精地图可以使车辆能够提前查看并预先减速

    

• 3、Apollo高精地图与构建

  • 1、高精地图包含道路定义、交叉路口、交通信号灯精确位置、车道规则。不仅减小计算需求，还可以提供驾驶环境详细信息。

    

  • 2.高精度地图的构建由五个过程组成：数据采集、数据处理、对象检测、手动验证和地图发布。

    

    • 1、数据采集：300辆Apollo测试车辆负责收集用于制作地图的源数据，多种传感器，如GPS、IMU、激光雷达、摄像机。Apollo定义了一个硬件框架，将这些传感器集成到单个自主系统中，通过支持多种类的传感器，Apollo收集各类数据将这些数据融合，最终生成高精度地图。

      

    • 2、数据处理：对收集到的数据进行整理、分类和精简，以获得没有任何语义信息或注释的初始地图模板。

      

    • 3、对象检测：使用人工智能来检测静态对象并对其进行分类，其中包括车道线、交通标志、甚至是电线杆，手动验证可确保自动地图创建过程正确进行并及时发现问题。

      

    • 4.在构建和更新地图的过程中，Apollo使用众包向公众发布其数据采集工具，以便任何人都可以参与制作高精度地图的任务，这加快了高精地图制作和维护的过程。

      

    • 二、定位（厘米级）

    • 1、GNSS RTK

      • 1、GPS 4颗卫星定位。除了三颗卫星三角测量，再加一颗定位高度的卫星。接收端与卫星距离通过TOF原理测量飞行时间，但是光速很大，少量时间误差导致巨大误差。每颗卫星都配备了高精确度的原子钟。
      • 2、为进一步减小误差，使用实时运动定位RTK。RTK涉及的地面基站，用来确定系统误差，然后传递给GPS接收器来调整。在 RTK 的帮助下，GPS 可以将定位误差限制在 10 厘米以内。

        

      • 评价：1）存在高楼和其他障碍物可能阻挡 GPS 信号的问题，这使定位变得困难或根本无法定位。2）GPS 的更新频率很低，大约为 10 赫兹。由于无人驾驶车在快速移动，可能需要更频繁地更新位置。

    • 2、惯性导航

      • 1、加速度需要三轴加速度计的传感器来测量，加速度计根据车辆的坐标系记录测量结果，还需要陀螺仪传感器将这些测量值转换为全局坐标系测量值。
      • 2、三轴陀螺仪的三个外部平衡环一直在旋转，但三轴陀螺仪中的旋转轴始终固定在世界坐标系中。在坐标系中的位置是通过测量旋转轴和三个外部平衡环的相对位置来计算的。

        

      • 3、评价：1）IMU 可以以高频率更新，可达 1000 赫兹。2）运动误差随时间增加而增加，我们只能依靠惯性测量单元在很短的时间范围内进行定位。

    • 3、激光雷达定位

      • 1、迭代最近点法ICP 是匹配点云常见方法，假设我们对两次点云扫描进行匹配。我们的目标是通过点云旋转和平移来最大限度地降低这一平均距离误差

        

      • 2、滤波算法是一种LiDAR 定位方法，滤波算法可消除冗余信息并在地图上找到最可能的车辆位置。直方图滤波算法，将通过传感器扫描的点云滑过地图上的每个位置，在每个位置上计算扫描的点与高精度地图上的对应点之间的误差或距离然后对误差的平方求和，求得的和越小扫描结果与地图之间的匹配越好。

        

      • 3、卡尔曼滤波是另一种LiDAR 定位方法。卡尔曼滤波使用了预测更新周期，首先我们根据之前的状态以及对移动距离和方向的来估计我们的新位置。当然运动估计并不完美，所以需要通过使用传感器测量我们的位置并加以纠正。一旦传感器测量了我们的新位置，我们便可以使用概率规则将不完美的测量结果与现有位置匹配起来。

    • 4、视觉定位

      • 粒子滤波：车道线在许多道路上却很常见，可以使用相同的粒子滤波原理对车道线进行拍照，然后使用拍摄的图像来确定车辆在道路中的位置，可以将道路摄像头图像与地图进行比较。

        

    • 5、Apollo定位

      • Apollo定位模块依赖于IMU、GPS、激光雷达、雷达、高精地图，这些传感器同时支持 GNSS 定位和LiDAR 定位，GNSS 定位输出位置和速度信息，LiDAR 定位输出位置和行进方向信息。融合框架通过卡尔曼滤波将这些输出结合在一起。

        

      • 惯性导航解决方案用于卡尔曼滤波的预测步骤，GNSS 和 LiDAR 定位用于卡尔曼滤波的测量结果更新步骤。

三、感知

• 1、计算机视觉

  • 无人驾驶车有四个感知世界的核心任务：检测——指找出物体在环境中的位置；分类——指明确对象是什么；跟踪——指随时间的推移观察移动物体；语义分割——将图像中的每个像素与语义类别进行匹配如道路、汽车、天空。

    

    

• 2、Camera图像

  • RGB图像，深度为3

    

• 3、Lidar图像

  • 激光雷达传感器创建环境的点云表征，提供了难以通过摄像头图像获得的信息如距离和高度。点云中的每个点代表反射回传感器的激光束，可以告诉我们关于物体的许多信息例如其形状和表面纹理。

    

• 4、机器学习

  • 1、监督式学习：使用数据和相关的真值标记来进行模型训练
  • 2、非监督学习：没有真值标记的车辆与行人图像，让计算机自行决定哪些图像相似、哪些图像不同
  • 3、半监督学习：使用少量的标记数据和大量的未标记数据来训练模型
  • 4、强化学习：允许模型通过尝试许多不同的方法来解决问题，然后衡量哪种方法最为成功。以多种不同的方向和速度进行实验性驾驶，如果车辆实际完成了右转，智能体会提高奖励即得分。

• 5、神经网络

  • 受到构成人类神经系统的生物神经元启发，生物神经元通过相互连接构成了神经元网络或神经网络，通过类似的方式将人工神经元层连接起来以创建用于机器学习的人工神经网络。

• 6、反向传递

  • 三步循环组成——前馈、误差测定、反向传播。每个人工神经元都对其值进行微调，这是基于通过神经网络后向传播的误差，可生成更准确的网络。

    

    

    

• 7、卷积神经网络CNN：

• CNN通过维持输入像素之间的空间关系来解决这个问题，CNN通过将过滤器连续滑过图像来收集信息，每次收集信息时，只对整个图像的一小部分区域进行分析，称为卷积。当在整个输入图像上对一个过滤器进行卷积时，将该信息与下一个卷积层相关联。

  

• 8、检测与分类：

• 计算机首先需要知道障碍物的位置，然后进行分类。另一个示例为交通信号灯检测分类，首先将使用计算机视觉对图像中的交通信号灯进行定位，然后根据灯光显示颜色对交通信号灯进行分类。可以先使用检测CNN来查找图像中的对象的位置，在对图像中的对象进行定位后，可以将图像发送给另一个CNN进行分类。经典体系结构为 R-CNN、Fast R-CNN、Faster R-CNN、YOLO、SSD等

• 9、跟踪：

• 追踪可以解决遮挡问题。追踪的第一步为确认身份，通过查找特征相似度最高的对象，将在之前的帧中检测到的所有对象与在当前的帧中检测到的对象进行匹配。

  

• 10、分割：

• 语义分割涉及对图像的每个像素进行分类，用于尽可能详细地了解环境并确定车辆可驾驶区域。语义分割依赖于一种特殊类型的CNN，被称为全卷积网络FCN。1）FCN用卷积层来替代传统CNN体系结构末端的平坦层，现在网络中的每一层都是卷积层，因此名称为“全卷积网络”。2）为了分割像素，输出尺寸必须与原始图像的尺寸相匹配。为了完成，可以对中间输出进行上采样处理，直到最终输出的大小与原始输出图像的大小相匹配。编码器 和解码器。

  

• 11、Apollo感知：

  • 1）Apollo将ROI过滤器应用于点云和图像数据，以缩小搜索范围并加快感知。

    

  • 2）然后通过检测网络馈送已过滤的点云，输出用于构建围绕对象的三维边界框。

    

  • 3）最后使用被称为检测跟踪关联的算法来跨时间步识别单个对象，该算法先保留在每个时间步要跟踪的对象列表，然后在下一个时间步中找到每个对象的最佳匹配。

    

  • 4）对于交通信号灯的分类，Apollo先使用高精度地图来确定前方是否存在交通信号灯。如果前方有交通信号灯，则高精度地图会返回灯的位置。后续根据位置，摄像头搜索范围，在摄像头捕获到交通信号灯图像后。Apollo使用检测网络对图像中的灯进行定位，然后从较大的图像中提取交通信号灯。将裁剪的交通灯图像提供给分类网络以确定灯颜色，如果有许多灯则系统需要选择哪些灯与其车道相关。

    

  • 5）Apollo使用 YOLO 网络来检测车道线、动态物体其中包括车辆、卡车、骑自行车的人、行人。在经过YOLO网络检测后，在线检测模块会并入来自其他传感器的数据对车道线预测进行调整，车道线最终被并入名为“虚拟车道”的单一数据结构中。同样也通过其他传感器的数据对YOLO网络所检测到的动态对象进行调整，以获得每个对象的类型、位置、速度、前进方向。虚拟通道和动态对象均被传递到规划与控制模块。

    

• 12、传感器比较：

• Camera适应于分类，交通信号灯分类、车道检测。激光雷达：适用于障碍物检测；

• 13、感知融合策略：

• 卡尔曼滤波。实际上有两种测量结果更新步骤：同步和异步。同步融合同时更新来自不同传感器的测量结果，而异步融合则逐个更新所收到的传感器测量结果。传感器融合可提高感知性能，因为各传感器相辅相成，融合也可以减少跟踪误差。

  

四、预测

• 1、简介

  • 1、无人车需要预测这些物体的行为，这样才能确保做出最佳决策。在环境中为所有其他的物体做出预测，这些共同形成了在一段时间内的预测路径。这些预测路径为无人车在规划阶段做出决策提供了必要信息。
  • 2、预测路径：实时性、准确性、预测模块应该有学习新的行为。

• 2、不同的预测方式

  

  • 1、基于模型的预测：构建两个候选模型，观察移动车的运动，看它与那一条轨迹更匹配。
  • 2、基于数据驱动预测：使用机器学习算法，通过观察结果来训练模型。数据驱动方法的优点是训练数据越多，模型效果越好。

• 3、基于车道序列的预测

  • 1、为了建立车道序列，首先将道路分成多个部分，每一部分都覆盖了一个易于描述车辆运动的区域。为了预测，我们更关心车辆如何在这些区域内转换，而不是在某个区域内的具体行为。例如直行汽车的运动可以描述为车道序列是 0-1-3-7。

    

• 4、障碍物状态

  • 为了预测物体运动也需要知道障碍物状态（位置、朝向、速度、加速度、其在车道段内物体的位置），预测模块会考虑从物体到车道线段边界的纵向和横向距离，还包含之前时间间隔的状态信息以便做出更准确的预测。

    

• 5、预测目标车道

  • 现在所要做的就是选择车辆最有可能采取的车道顺序，可以通过计算每个车道序列的概率来进行选择。随着记录随着时间的增加，模型可以自我迭代更新，精确度不断提升。

    

• 6、递归神经网络RNN在目标车道预测的应用

  • 1、递归神经网络RNN是一种利用时间序列数据特征的一种预测方法。建立像这样的多重结构的递归神经网络称之为 MLP单元，从数据序列中提取出高级特征，每个MLP单元将序列的一个元素作为输入并预测序列的下一个元素作为输出。为了对元素之间的顺序关系建立模型，在每个单元之间建立一个额外的连接，这意味着每个单元根据原始输入和前一个单元的输出进行预测，这 RNN的基本结构。

    

  • 2、Apollo使用RNN建立一个模型来预测车辆的目标车道，为车道序列提供一个RNN模型，为相关对象状态提供另一个RNN模型。连接这两个RNN的输出并将它们馈送到另一个神经网络，该神经网络会估计每个车道序列的概率，具有最高概率的车道序列是我们预测目标车辆将遵循的序列。

    

• 7、轨迹生成

  • 如何预测最有可能的轨迹？可以先通过设置约束条件来去除大部分候选轨迹，首先假定汽车将与目标车道的中心对齐，继续去除车辆无法实际执行的轨迹。通过考虑车辆当前的速度和加速度从剩余的轨迹中进行选择，实际上我们并没有实际列出所有可能的轨迹并逐一去除它们。

    

五、规划

• 1、路径规划使用三个输入

• ，第一个输入为地图，Apollo提供的地图数据包括公路网和实时交通信息。第二个输入为我们当前在地图上的位置。第三个输入为我们的目的地，目的地取决于车辆中的乘客。

• 2、将地图转为图形

  • 该图形由“节点”(node)和“边缘”(edge)组成。节点代表路段，边缘代表这些路段之间的连接。我们可以对一个节点移动到另一个节点所需的成本进行建模。

    

• 3、路径查找算法A*

  • 从初始节点开始，我们需要确定8个相邻节点中，哪个是最有希望的候选节点。对于每个候选节点，我们考虑两件事情。首先，我们计算从开始节点到候选节点的成本。然后，我们估计从候选节点前往目的地的成本。对于每个候选节点，我们通过添加g值和h值来计算总和，即f值。最佳候选节点是f值最小的节点。

    

    

• 4、轨迹生成

  • 1、我们可能需要与试图在我们前面掉头的汽车互动，或者我们可能希望超过一辆在公路上行驶的慢车。这些场景需要更低级别、更高精确度的规划。我们将这一级别的规划称为轨迹生成。轨迹生成的目标是生成一系列路径点所定义的轨迹。
  • 2、每个路径点分配了一个时间戳和速度，这些时间戳和空间上的两个维度（2D position）共同创建了一个三维轨迹（3D  Trajectory）。我们还为每个路径点指定了一个速度，用于确保车辆按时到达每个路径点。

    

  • 3、成本函数为每个轨迹分配了一个“成本”，我们选择成本最低的轨迹。轨迹“成本”由各种犯规处罚组成，例如：偏离道路中心，有可能产生碰撞，速度限制，轨迹的曲率和加速度让乘客感到不舒服等。

    

• 5、Frenet坐标系

  • 1、我们通常使用笛卡尔坐标系描述物体的位置，但笛卡尔坐标系对车辆来说并不是最佳选择。即使给出了车辆位置(x,y)，如果我们不知道道路在哪，就很难知道车辆行驶了多远也很难知道车辆是否偏离了道路中心。

    

  • 2、Frenet坐标系描述了汽车相对于道路的位置。s代表沿道路的距离，也被称为纵坐标。d表示与纵向线的位移，也被称为横坐标。在道路的每个点上，横轴和纵轴都是垂直的。纵坐标表示道路中的行驶距离，横坐标表示汽车偏离中心线的距离。

    

• 6、路径-速度解耦规划

  • 路径-速度解耦规划将轨迹规划分为两步：路径规划、速度规划。1）首先在路径规划步骤中生成候选曲线，这是车辆可行驶的路径，然后按成本对路径进行排名并选择成本最低的路径。；2）确定沿这条路线行进的速度，

• 7、路径生成与选择

  • 为了在路径-速度解耦规划中生成候选路径，首先将路段分割成单元格。。然后对这些单元格中的点进行随机采样。通过从每个单元格中取一个点并将点连接，我们创建了候选路径。通过重复此过程可以构建多个候选路径。使用成本函数对这些路径进行评估并选择成本最低的路径，成本函数可能考虑以下因素：与车道中心的偏离、与障碍物的距离、速度和曲率的变化、对车辆的压力、或希望列入的任何其他因素。

    

• 8、ST图

  • 选择路径后的下一步是选择与该路径关联的速度曲线，在ST图中，“s”表示车辆的纵向位移、“t”表示时间。ST 图上的曲线是对车辆运动的描述，因为它说明了车辆在不同时间的位置。由于速度是位置变化的速率，所以可以通过查看曲线的斜率从 ST 图上推断速度。斜坡越陡则表示在更短的时间段内有更大的移动，对应更快的速度。

    

• 9、速度规划

  • 1、为构建最佳速度曲线需要将 ST 图离散为多个单元格。单元格之间的速度有所变化，但在每个单元格内速度保持不变，该方法可简化速度曲线的构建并维持曲线的近似度。在 ST 图中可以将障碍物绘制为在特定时间段内阻挡道路的某些部分的矩形。例如，假设预测模块预测车辆将在 t0 到 t1 的时间段内驶入的车道。由于该车将在此期间占据位置 s0 到 s1，因此在 ST 图上绘制了一个矩形，它将在时间段 t0 到 t1 期间阻挡位置 s0 到 s1。为避免碰撞，速度曲线不得与此矩形相交。既然有了一张各种单元格被阻挡的 ST 图，便可以使用优化引擎为该图选择最佳的速度曲线。优化算法通过复杂的数学运算来搜索受到各种限制的低成本解决方案。这些限制可能包括：法律限制，如速度限制；距离限制，如与障碍物的距离；汽车的物理限制，如加速度限制。

    

  • 2、路径-速度解耦规划在很大程度上取决于离散化。路径选择涉及将道路划分为单元格，速度曲线构建涉及将 ST 图划分为单元格。尽管离散化使这些问题更容易解决，但该解决方案生成的轨迹并不平滑。为了将离散解决方案转换为平滑轨迹，可使用“二次规划”技术（Quadratic Programming)。二次规划将平滑的非线性曲线与这些分段式线性段拟合。尽管二次规划背后的数学运算很复杂，但对于我们的目的而言，细节并不是必需的。

    

• 10、路径-速度规划的轨迹生成

  • 回顾一下端到端路径-速度解耦规划。假设我们正在路上行驶，感知系统观察到一辆缓慢行驶的车辆离我们越来越近。首先，在这辆车的周围生成多条候选路线，使用成本函数对这些候选路径进行评估并选择成本最低的路径。然后使用 ST 图来进行速度规划，根据其他车辆随时间变化的位置阻挡了 ST 图的部分区域。优化引擎可帮助确定该图的最佳速度曲线，该曲线受制于约束和成本函数。我们可以使用二次规划让路径和速度曲线变平滑。最后，将路径和速度曲线合并构建轨迹。这里的轨迹在速度较快时为红色，在速度较慢时为蓝色。我们使用该轨迹来安全地绕开其他车辆并继续我们的旅程。

    

• 11、Lattice 规划

  • 1、通过使用 Frenet 坐标可以将环境投射到纵轴和横轴上，目标是生成三维轨迹：纵向维度、横向维度、时间维度。将三维问题分解成两个单独的二维问题，这是通过分离轨迹的纵向和横向分量来解决的。其中一个二维轨迹是具有时间戳的纵向轨迹称之为 ST 轨迹，另一个二维轨迹是相对于纵向轨迹的横向偏移称之为 SL 轨迹。

    

  • 2、Lattice 规划具有两个步骤即先分别建立 ST 和 SL 轨迹，然后将它们合并为生成纵向和横向二维轨迹。先将初始车辆状态投射到 ST 坐标系和 SL 坐标系中，通过对预选模式中的多个候选最终状态进行采样。来选择最终车辆状态。对于每个候选最终状态构建了一组轨迹将车辆从其初始状态转换为最终状态，使用成本函数对这些轨迹进行评估并选择成本最低的轨迹。

    

• 12、ST轨迹的终止状态

  • 1、根据情况可以将状态分成 3 组：巡航 、跟随、停止。巡航意味着车辆将在完成规划步骤后定速行驶，实际上在对图上的点进行采样，在图中横轴代表时间，纵轴代表速度。对于该图上的点，这意味着汽车将进入巡航状态，在时间 t 以 s 点的速度巡航，对于这种模式，所有最终状态的加速度均为零。

    

  • 2、下一个要考虑的模式为跟随车辆，在这种情况下要对位置和时间状态进行采样，并尝试在时间t出现在某辆车后面，在跟随车辆时，需要与前方的车保持安全距离，这时速度和加速度将取决于要跟随的车辆，这意味着在这种模式下，速度和加速度都会进行修正。

    

  • 3、最后一种模式是停止，对于这种模式只需对汽车何时何地停止进行抽样，这里速度和加速度会被修正为 0 

    

• 13、SL轨迹的终止状态

  • 根据这样一个假设来进行SL 规划，即无论车辆进入怎样的终止状态，车辆都应该稳定地与车道中心线对齐。这意味着只需要在一个小区域内，对横向终止位置进行采样。具体来说采样的是道路上相邻车道中心线周围的位置。为了确保稳定性，汽车驶向的终止状态应该与车道中心一致。当用横向位置与纵向位置作图时 ，想要的候选轨迹应该以车辆与车道对齐并直线行驶而结束。为了达到这种终止状态，车的朝向和位置的一阶和二阶导数都应该为零。这意味着车辆既不是横向移动的，那是一阶导数；也不是横向加速，那是二阶导数。这意味着车辆正沿着车道直行。

    

• 14、Lattice规划的轨迹生成

  • 一旦同时拥有了 ST 和 SL 轨迹，就需要将它们重新转换为笛卡尔坐标系。然后可以将它们相结合构建由二维路径点和一维时间戳组成的三维轨迹。ST 轨迹是随时间变化的纵向位移，SL 轨迹是纵向轨迹上每个点的横向偏移。由于两个轨迹都有纵坐标 S，所以可以通过将其 S 值进行匹配来合并轨迹。

    

  • 六、控制

  • 1、控制流程

    • 1、控制器预计有两种输入：目标轨迹与车辆状态。目标轨迹来自规划模块，在每个轨迹点，规划模块指定一个位置和参考速度。在每个时间戳都对轨迹进行更新。我们还需要了解车辆状态，车辆状态包括：通过本地化模块来计算的车辆位置、从车辆内部传感器获取的数据（如速度、转向和加速度）。我们使用这两个输入来计算目标轨迹与实际行进轨迹之间的偏差。
    • 2、控制器的输出是控制输入（转向、加速和制动）的值。当偏离目标轨迹时，我们希望采取行动来纠正这种偏差。对于普通汽车，我们使用方向盘控制行驶方向（即转向）、使用油门加速、使用刹车减速（即制动）。这也是无人驾驶汽车所做的。一旦将这三个值传递给车辆，汽车实际上已经开始无人驾驶了。之后将介绍不同的控制算法，如何计算这三个输出-转向、加速和制动。

      

  • 2、PID控制

    • 1、P代表“比例”(Proportional)P控制器在车辆开始偏离时立即将其拉回目标轨迹。比例控制意味着，车辆偏离越远，控制器越难将其拉回目标轨迹。

      

    • 2、在实践中P控制器的一个问题在于，它很容易超出参考轨迹。当车辆越来越接近目标轨迹时，我们需要控制器更加稳定。PID控制器中的D项致力于使运动处于稳定状态，D代表“微分”（Derivative）。PD控制器类似于P控制器，它增加了一个阻尼项，可最大限度地减少控制器输出的变化速度

      

    • 3、PID控制器中的最后一项I代表积分（Integral），该项负责纠正车辆的任何系统性偏差。例如，转向可能失准，这可能造成恒定的转向偏移。在这种情况下，我们需要稍微向一侧转向以保持直行。为解决这一问题，控制器会对系统的累积误差进行惩罚。我们可以将P、I和D组件结合构成PID控制器。

      

  • 3、PID优劣对比

    • PID控制器很简单，但它在很多情况下的效果很好。对于PID控制器，你只需要知道你的车辆与目标轨迹之间的偏差。但是PID控制器只是一种线性算法，对于非常复杂的系统而言，这是不够的。例如，为控制具有多个关节的四轴飞行器或机器人，我们需要建立机器人的物理模型。对无人驾驶而言，我们需要应用不同的PID控制器来控制转向和加速，这意味着很难将横向和纵向控制结合起来。另一个问题在于PID控制器依赖于实时误差测量，这意味着受到测量延迟限制时可能会失效。

  • 4、线性二次调节器LQR

    • 1、线性二次调节器（Linear Quadratic Regulator 或LQR）是基于模型的控制器，它使用车辆的状态来使误差最小化。Apollo使用LQR进行横向控制。横向控制包含四个组件：横向误差、横向误差的变化率、朝向误差和朝向误差的变化率。变化率与导数相同，我们用变量名上面的一个点来代表。我们称这四个组件的集合为X，这个集合X捕获车辆的状态。除了状态之外，该车有三个控制输入：转向、加速和制动。我们将这个控制输入集合称为U。

      

    • 2、LQR处理线性控制，这种类型的模型可以用等式来表示（详见下图）。x（上方带点）=Ax+Bu，x（上方带点）向量是导数，或X向量的变化率。所以x点的每个分量只是x对应分量的导数。等式x点=Ax+Bu，该等式捕捉状态里的变化，即x点是如何受当前状态 x 和控制输入 u 的影响的。这个等式是线性的，因为我们用∆x来改变x时，并用∆u来改变u。x点的变化也会让这个等式成立（见下图等式）。现在我们了解了LQR中的L。

      

      

    • 3、接下来我们学习LQR中的Q。这里的目标是为了让误差最小化，但我们也希望尽可能少地使用控制输入。由于使用这些会有成本，例如：耗费气体或电力。为了尽量减少这些因素，我们可以保持误差的运行总和和控制输入的运行总和。当车往右转的特别厉害之际，添加到误差总和中。当控制输入将汽车往左侧转时，从控制输入总和中减去一点。然而，这种方法会导致问题。因为右侧的正误差只需将左侧的负误差消除即可。对控制输入来说也是如此。相反，我们可以让x和u与自身相乘，这样负值也会产生正平方，我们称这些为二次项。我们为这些项分配权重，并将它们加在一起。最优的u应该最小化二次项的和随时间的积分。在数学中我们将这个积分值称为成本函数（形式见下图）。我们经常以紧凑的矩阵形式表示加权二次项的总和。这里的Q和R代表x和u的权重集合。xT和uT是转置矩阵，这意味着它们几乎与x和u相同，只是重新排列以便矩阵相乘。x乘以xT，u乘以uT，实质上是将每个矩阵乘以它自己。最小化成本函数是一个复杂的过程，但通常我们可以依靠数值计算器为我们找到解决方案。Apollo就提供了一个这样的求解方案。在LQR中，控制方法被描述为u=-Kx。其中，K代表一个复杂的skeme，代表如何从x计算出u。所以找到一个最优的u就是找到一个最优的K。许多工具都可以轻松地用来解决K，尤其当你提供了模拟车辆物理特征的A、B，以及x和u的权重Q、R。

      

      

  • 5、模型控制预测

    • 1、模型预测控制（或MPC）是一种更复杂的控制器，它非常依赖于数学优化，但基本上可以将MPC归结为三个步骤：1、建立车辆模型。2、使用优化引擎计算有限时间范围内的控制输入。3、执行第一组控制输入。MPC是一个重复过程，它着眼未来，计算一系列控制输入，并优化该序列。但控制器实际上只实现了序列中的第一组控制输入，然后控制器再次重复该循环。为什么我们不执行整个控制输入序列呢？那是因为我们只采用了近似测量与计算。如果实现了整个控制输入序列，实际产生的车辆状态与我们的模型有很大差异，最好在每个时间步不断地重新评估控制输入的最优序列。
    • 2、MPC的第一步为定义车辆模型，该模型近似于汽车的物理特性，该模型估计了假如将一组控制输入应用于车辆时会发生什么。接下来，我们决定MPC预测未来的能力。预测越深入，控制器就越精确，不过需要的时间也越长。所以，我们需要在准确度与快速获取结果之间做出取舍。获取结果的速度越快，越能快速地将控制输入应用到实际车辆中。

      

    • 3、下一步是将模型发送到搜索最佳控制输入的优化引擎。该优化引擎的工作原理是通过搜索密集数学空间来寻求最佳解决方案。为缩小搜索范围，优化引擎依赖于车辆模型的约束条件。优化引擎可间接评估控制输入，它通过使用以下方法对车辆轨迹进行建模：通过成本函数对轨迹进行评估。成本函数主要基于与目标轨迹的偏差；其次，基于其他因素，如加速度和提升旅客舒适度的措施。

      

  • 6、总结

    • 控制实际上是无人驾驶汽车实现自动移动的方式。在控制中，我们使用转向、加速和制动来运行我们的目标轨迹。我们研究了几种不同类型的控制器。PID控制是一种简单而强大的控制算法，线性二次调节器和模型预测控制是另外两种类型的控制器，它们更复杂，但也更强大、更准确。Apollo支持所有这三种控制器，而你也可以选择最适合自己的控制器！

参考：

Apollo开发者社区的分享文章