Apollo 规划方法

技术难点

图片.png
第一，车辆所处的环境复杂度高。因此传感器检测返回的数据复杂，障碍物个数种类多；路况信息复杂，在高速上一秒钟车辆能跑出30米，情况瞬息万变。

第二，系统对规划模块的要求高。规划模块需要覆盖所有的ADAS场景，对模块计算的实时性和稳定性有着很高的要求。

第三，这是一个全新的未知领域，能够从外界获得的信息有限。虽然有过去长期的积累，但是依然有多重难题需要解决。
整体思路

图片.png
开发者面临的是一个三维空间中的优化问题，包括路面的二维平面，也包括时间维度。这是一个N立方难度的问题，Apollo 2.5的解决方法是，把这个N立方级别的问题，分拆成两个N平方级别的问题。也就是在x-y维度上求解，进行路径规划；在路径规划的基础上，以规划出来的路径为s轴，在s-t维度上进行速度规划。
这种降低维度，以退为进的方法，是对规划模块认识上的一个飞跃。通过放弃最优解的追求转而追求较优解，换来系统运算速度、稳定性的极大提高，同时满足无人驾驶系统的整体需求。这一点可以类比机器学习中SGD的思路，放弃对精确梯度方向的寻找，转而寻找近似梯度的方向，从而换来学习速率上的提升。
整体架构
图片.png
规划模块的架构从整体上来说，可以分为两个部分：一部分负责对数据的监听、获取和预处理；另一部分负责管理各个优化模块。数据进入后，对其综合处理为规划模块的内部数据结构，由任务管理器调度合适的优化器进行各个优化任务。综合优化的结果，经过最终的验证后，输出给控制模块。

在设计上，实现了策略的可插拔，使得各个优化器可以灵活配置不同策略，提升迭代效率。

相对于Apollo 2.0，规划模块在延续了其架构上统一、灵活、快捷的优点上，在保持已开发算法持续更新优化的同时，又新增了局部坐标系以及一种新的速度和路径同时优化的Lattice planner。
apollo规划器

图片.png
pollo 1.0中开始开放的RTK，也就是循迹Planner；Apollo 1.5中开始开放的EM Planner，也就是基于动态规划Dynamic Programming和二次规划Quadratic Programming的路径规划器与速度规划器；以及Apollo 2.5中开始开放的Lattice Planner，一种路径和速度同时规划的规划器。
RTK Planner
RTK Planner是规划预先设置的轨迹，为循即算法使用。
图片.png
EM Planner
图片.png
EM Planner可以通过动态规划进行路径和速度规划。比如说，路径规划会在范围内进行机动撒点，离散化解空间进行求解。
图片.png
要想合理的选择采样点，首先应该直观的感受一下车辆和道路。以最常用的林肯MKZ 2016款车款为例，此车宽度2米1，那么假设道路宽度为3.5米，在当前道路行驶的时候就留下了一共1.4米左右的机动范围。

比如说，采样器在垂直于Lane的方向选择5个点，这种选法每两个点间隔0.35米，对于车辆调节来说是合适的；而在沿着道路方向，点的选取不需要特别密集，过于密集的选择白白浪费了计算资源，并且在下一步使用光滑曲线连接点的时候，造成求解的不稳定。所以，往往会根据当前的车速和路况进行选择，比如每隔15-20米选一组点，共选择4-5组。这样选点，结合了车辆与道路的特点，并且所有参数是可以配置的，具有高度的灵活性。

图片.png

撒点后，通过Cost Function一层一层的进行动态规划，从而对问题进行了有效的简化。然后再通过平滑的曲线连接各层采样点得到最终的路径。其优势是明显的：

灵活性：采样点从左到右覆盖了整个可行区间，避免陷入局部最优

可靠性：其利用Cost Function 避免无解

可拓展：低速到高速全覆盖

图片.png

EM Planner中的速度规划，也可以采用和路径规划相似的方法。对解空间进行离散化以后，然后通过动态规划进行求解。从图中可以看到，在将与主车轨迹有重叠的障碍物映射到ST图以后，速度规划可以通过加速减速匀速等操作，从ST图中搜索到一条可通行区间并生成速度规划。从最后生成出来的图里面，可以解读出来相对于每一个障碍物的纵向决策。

比如说，需要避让或者跟随一个障碍物，车辆就从障碍物ST图中下方通过如果需要超越一个障碍物，那么在ST中车辆就从其上方通过。

图片.png

Lattice Planner和EM Planner在设计上最大的区别在于，Lattice是横向纵向综合求解的，而EM是分开求解的。Lattice Planner源于斯坦福大学参加DARPA无人驾驶大赛的规划算法，Apollo 2.5吸收了其精华，并针对原有算法进行了改进和升级。

图片.png

和EM planner一样，Lattice Planner也将轨迹规划问题分解成横向和纵向两个1维空间的独立的轨迹规划问题, 降低规划难度。横向仍然是SL问题，也就是Station Lateral，纵向上也还是ST，也就是Station Time问题。

Lattice Planner会根据起点和终点的状态，在位置空间和时间上同时进行撒点。撒点的起始状态和终止状态各有6个参数，包括了3个横向参数，即横向位置、横向位置的导数也就是Heading、Heading的导数；3个纵向参数，即纵向位置、纵向位置的一阶导数也就是速度、纵向位置的二阶导数（也就是加速度）。

图片.png

起点的参数是车辆当时真实的状态，或者Stitch的状态来进行设计，终止状态则是撒点枚举的各个情况。在确定了终点和起点状态以后，再通过五阶或者四阶的多项式连接起始状态和终止状态，从而得到规划的横向和纵向轨迹。

这个步骤是Lattice算法的精髓所在，它直接决定了算法的效率以及解空间。举个例子来说，在撒点的时候可以的加入车辆物理动力学性能的约束，同时根据障碍物进行可行域的预筛选等，有效的裁剪无效空间，从而达到更优的性能。

在生成所有横向和纵向的一维轨迹以后，将其排列组合起来，合成大量的二维轨迹，然后根据损失函数筛选出最好的合成轨迹。和EM Planner类似，Lattice Planner的损失函数也可以分为，安全相关、体感相关以及目标完成度相关三类。
各种规划方法对比

图片.png
Lattice Planner的优点是很明显的，同时产生轨迹的几何形状与速度分配，直接考虑到车辆的动力学状态，尤其是横向动力学状态，产生更适合高速运动的轨迹，简单可靠，参数少于10个。它利用了撒点算法避免了轨迹陷入局部最优，性能也非常的优秀，生成超过1000轨迹，轨迹的生成加上选择，用时少于100ms，确保了能做到10赫兹的Planning规划。
轨迹后处理
图片.png
在轨迹生成以后，往往还可以通过一些后处理，对轨迹进行平滑和验证。可以通过几何的算法或者更加高粒度的采样，对于生成轨迹每个点，进行额外的碰撞检查。也可以同时验证规划的轨迹是否符合车辆的物理学动力特性，比如看其是否有超过车辆加减速度上下界限，以及转弯半径界限的时候。
回退机制
图片.png
秉着安全第一的原则，Apollo 2.5同时提供冗余机制以保证整个系统的稳定性。车辆在路上行驶的过程中，遇到的情况千差万别，如果真的遇到了无论是人工驾驶还是机器驾驶都无法解决的特殊恶劣情况，如何保证Planning不会崩溃，并且输出一条相对最安全的轨迹，以保证生命和财产的安全，就显得尤其的重要了。

因此，无论是使用DP还是QP的EM planner，还是同时进行速度和路径优化的Lattice Planner，Apollo 2.5针对路径规划和速度规划都加入了后备规划。在正常的逻辑并不能在给定假设情况下计算出一条合理的路径的时候，针对当时的情况，对车辆进行合理的减速最后停车，以确保安全。
高效的测试与迭代

图片.png
如何帮助大家进行能够高效的迭代也是Apollo所关心的一个重要问题。因此，Apollo的Planning有一套推荐的测试迭代流程。即单元测试，单周期的集成测试，模拟器测试，最后再实车测试的流程。让开发者可以在开发迭代的过程中，随时进行回归，保证开发的质量以及效率。

坐标系

图片.png
Apollo 2.5又为大家打开了另外一扇窗--相对坐标系。此前的绝对坐标系是以世界为背景的一种全局定位，世界上每一个点，都会有一个相应的唯一的坐标。它的好处很明显，因为是全局唯一，所以说表达一个物体的真实位置的时候非常的容易，做多时刻位置对齐的时候也会比较的容易。

车身相对坐标系也是一种常见的坐标系，它是以车辆自身位置坐标原点，以车辆的朝向为x，车辆左方为y的坐标系。其优点也是非常明显的，可以去除开发者们对于地图定位的强依赖，反馈的数值也更加的直观方便开发者的理解。

图片.png

助力于相对坐标系这个新feature，在Apollo 2.5中同时推出了L3的高速方案。也就是基于车道线的感知方案和不基于地图和定位的规划方案。可以理解成为是类似于Tesla Autopilot或者Mobileye的方案，利用相对坐标系下的车道线代替全局坐标系的地图与定位，进行L3级别的自动驾驶功能。