技术讲堂 | Apollo规划模块的算法解析

本文将为大家介绍Apollo规划模块的算法解析。首先会介绍下Planning的功能（生产安全舒适的驾驶轨迹），然后介绍基于Planning功能，Apollo所采用的解决方案。主要内容包括：

• 基于场景分类的规则和算法调用框架

• 基于Frenet Frame的解耦合路径速度规划

• 基于规则的路径和速度决策

• 基于数值优化算法的路径和速度轨迹生成

01  Planning 功能

功能：基于预测的结果、路由模块的全局路线指引和地图信息，生成安全舒适的驾驶轨迹。

这里有两个关键词，第一个关键词是安全，在第二个关键词是舒适。后面的算法，很多都是基于安全和舒适来考虑的。

• 安全：什么叫安全？我们至少要防止碰撞，如果前面有障碍物，我们能够避免撞上，需要有道路边际线的限制，不能撞到路肩（马路牙子），这就是安全。

• 舒适：我们的自动驾驶还有一个很重要的应用场景是Robotaxi（自动驾驶出租车），需要搭载乘客，所以舒适也是很重要的。

 

02  基于场景分类的规则和算法调用框架

在Apollo中，Planning的场景和任务管理是如何实现的？我们有一个双层的状态管理机，上层是一个场景管理器，叫做Scenario Manager，这里举了3个场景的例子，Pull Over、Parking、Junction，每个场景的配置文件/参数是不一样的。

有了场景之后，会基于不同的规划器（Planner）进行规划，Apollo中目前有俩个Planner，一个是On Lane Planning Tasks，一个是Open Space Planning Tasks。

On Lane Planning Tasks是默认的规划器，接下来会重点介绍。如果大家对Open Space Planning Tasks感兴趣，可以在GitHub找相关的论文和代码。

有了规划器之后，会通过Path decision，把道路边界测出来。主要是通过不同脚本的task拆分完成的，比如Path decision，可以拆分成Lane Borrow和Lane Change。当decision做好之后，还会进行Path Planning。

有了轨迹后，可以再进行速度决策，速度决策也是通过小的task进行的。这里列了两个：

• Cross Lane Safety：主要是我们进行交叉变道时，对安全上的考虑。

• Lane Merge Waiting：主要是进行车辆汇流时，我们如何决策的。比如车辆需要等待，就需要生成一个Stop Fence。

有了Speed decision之后，用Speed的Optimizer ，来进行速度的规划。

03 基于Frenet Frame的解耦合路径速度规划

• Frenet Frame

首先为什么要进行Frenet Frame？可以将笛卡尔坐标系下的运动状态进行了横纵向的分解。横向是指车辆l离我们的reference line的距离，纵向是沿着reference line走了多远，用s来表示。

通过这样的拆分，相当于把Planning降维了，可以大大的降低计算的复杂度。

• Reference line smoother

介绍下reference line smoother是如何做的：我们会直接优化离散点位置，用QCQP来解优化点间曲率，这样的运算时间大概是10ms。具体Cost Function如上图：

• 第一项表示俩个向量、之差，最小的情况是三个点在一条直线上，意味着这时的车是比较舒适的，没有左右摇摆。

• 第二项是离reference line的距离，距离越小越好。

• 第三项是指俩个相邻点之间的距离不要差的太远。

上式的约束条件：

•     不能超过boundary的约束范围。

• 符合曲率的限制。

重点介绍下曲率限制：

• 表示坐标。

• θ表示向量与之间的夹角。

• d表示平均离散化后的长度。

假设：

1. • 与俩点之间的距离是相等的。

   • θ足够小，方便计算时做近似。

   smoother前后的长度是相等的。

   接下来我们可以进行计算：

   两个向量之间的差值约等于，又因为θ足够小，所以约等于，再对θ进行曲率的转化，就可以变成，因为有最小的转弯半径，所以有了图中的不等式，也就是我们的约束条件。

   有了约束条件我们就可以对原来的Loss Function进行求解。

   04  基于规则的路径和速度决策

   接下来介绍下基于规则的路径和速度决策，即Path boundary和Speed boundary是如何生成的。

   Path Bound Decision

   如图所示，存在这样一个场景：车辆在正常行驶，在它的前方停了一个障碍车。

   这时，来看一下Path Bound Decision是如何做的。首先，车在起始状态时，会判断需不需要进行换道：

   • 如果不需要进行换道，还需要判断是否需要借道（向左变道，再变回来），如果不需要借道，boundary就会限制在本车道内。如果我们需要借道，需要判断借道是否安全，如果不安全，boundary还会限制在本车道内。如果判断是安全的，就会进行借道路径的限制。

   • 如果需要换道，首先还需要进行安全性判断。如果不安全，依然限制在本车道内，如果安全，就会进行换道的路径限制。

   02 Speed Bound Decision

   

   讲完Path Bound Decision，来看一下Speed Bound Decision是如何生成的。

   先来看一下速度的v-s图，也就是道路限速：当车辆行驶时会保持一个速度，当遇到减速带时会进行一个速度的限制，当侧向比较接近行人时，也会进行速度的限制。

   还有ST限制，以上面这个场景为例，我们车辆正常向前行驶，蓝色的障碍车想要进行一个切入的操作，在我们的ST限制图中反映的是蓝色区域。当我们进行Speed Bound Decision时，我们要考虑主车的ST线，不能跟蓝色区域有交汇，一旦交汇，就意味着发生了碰撞。

   当我们有了道路限速与ST限制之后，就可以得到Speed Bound Decision。

   05 基于数值优化算法的路径和速度轨迹生成

   Piecewise Jerk Path Optimization

   此算法Piecewise Jerk Path Optimization主要思想是将车辆横向位置运动当作一个三阶系统，应用模型预测的方法求解二次规划问题。这个方法的单帧运算时间大概是10ms。

   上图为Piecewise Jerk Path Optimization的全貌，左边为状态空间，右边第一部分为Cost Function，第二部分是Cost Function的展开，最下方是限制条件。

   详细为大家介绍下：

   01 States

         

首先将车辆横向位置运动建模成三阶系统，然后进行Multiple shooting formulation对△s进行采样，最后对状态进行泰勒展开，得到的结果就是我们的状态空间。

02 Objectives

接下来讨论下Cost Function优化函数是如何得到的：

左边是优化函数，做优化的目的是让车辆可以保持安全、舒适的行驶，生成一个相对安全和舒适的轨迹.

其中：

表示车辆位置与boundary中心的距离，表示车辆偏离s的距离，和表示boundary的左边界和有边界。

这俩部分，是出于对舒适性的考虑。表示速度，表示加速度，可以理解成横向不要走的太快，方向盘也不要打的太猛。可以理解成Jerk，即方向盘不要打的太猛，也不要总是左右的变化。

02 Constraints

因为车辆存在最小的转弯半径，所以我们要对车辆运动学进行限制。由于转弯半径是基于笛卡尔坐标系的，需要基于Frenet坐标系进行转换。

假设：

• ，车辆朝向与reference line平行。

• ，横向二阶导几乎为零。

这样，可以将

简化为：

这里需要对曲率K进行最大限制，否则转弯半径会过小，导致无法转弯。

再把的约束代入，可以得到最终的约束：

其中L是指车身的长度。

02 Piecewise Jerk Speed Optimization

Piecewise Jerk Speed Optimization算法的主要思想是将车辆纵向位置运动当做一个三阶系统，应用模型预测的方法求解非线性优化问题，单帧的运算时间大概是40ms。

图中依然是我们的算法全貌，左边是状态空间，右上为目标函数，然后是限制条件。

States

刚刚做Path Optimization时，变量是s和l，现在做Speed Optimization时，则变成t和s。然后我们先建模成三阶系统：

然后进行Multiple shooting formulation 对△t进行采样，生成状态空间。再对状态空间进行泰勒展开得到：

然后进行Multiple shooting formulation 对△t进行采样，生成状态空间。再对状态空间进行泰勒展开得到：

Objectives

接下来时速度规划的Cost Function，如左图，解释下每项的意义:

是指时间的Objective达到最小

后面三项依然时基于舒适性考虑的，第一项是横向的加速度，第二项是纵向的加速度，第三项是Jerk（加速度的变化是舒适性的重要考虑因素）。

03 Constraints

限制条件主要考虑：

 

属于free space，然后有一个限速，加速度和刹车有最大值，然后jerk也有限制，表示向心加速度的限制，也就是转弯时不要转太猛。

▲用vs图表示的速度限制

▲用ks图表示的向心加速度直限制

 

到这里，已经为大家介绍了Planning模块最常用的规划器算法，随着Apollo版本的迭代，我们开发了不用应用场景下的Planner，如Open Space Planner和Learning based Planning。