Apollo速度规划都有什么？详解阿波罗动态规划&轨迹优化！

作者 | 無印燁  编辑 | 汽车人

原文链接：https://zhuanlan.zhihu.com/p/656399342

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

点击进入→自动驾驶之心【规划控制】技术交流群

本文只做学术分享，如有侵权，联系删文

Apollo速度规划分为：DP（动态规划）+ QP（轨迹优化）两部分。

DP给出一个粗解，为了开辟一个凸空间，然后再由osqp/ipopt来做优化平滑。

1. ST坐标系中的S：

1. 路径规划中的SL坐标系：S是参考线上的s；

2. 速度规划中的ST坐标系：S是路径规划输出的路径上的s；

2. 基于动态规划的速度规划

task：SPEED_HEURISTIC_OPTIMIZER

Apollo自动驾驶planning纵向速度规划之DP详解 - 知乎 (zhihu.com)

Planning 基于动态规划的速度规划 - 知乎 (zhihu.com)

基于处理好的ST图，通过采样构建一个二维的cost_table，并对每一格计算cost值，从而通过动态规划算法，得到一条粗的最优ST轨迹。

一般动态规划问题包括以下几个步骤：

• 确定状态变量

• 设计状态转移方程

• 初始化边界

• 返回结果

1. 状态变量

状态变量的定义就是StGraphPoint, 从代码的定义我们可以看到，每个状态点保存了：

• 当前栅格的位置信息point_，

• 优化后的速度（optimal_speed_），

• 参考速度cost （reference_cost_），

• 障碍物cost(obstacle_cost_)，

• 空间距离cost （spatial_potential_cost_），

• 当前点总的cost（total_cost_）

1. 状态转移方程

总体状态转移方程如下：

其中k表示了在最大加减速度约束下，从i时间j位置向i+1时间转移可达范围内的所有点

遍历从（0,0）点开始，向（1，k）逐步遍历。对于遍历点C（i+1，j+k）会比较其本身cost和从C（i,j）转移到C(i+1，j+k)的cost中最小cost作为其新的cost。

3. 基于osqp的二次速度规划

task：PIECEWISE_JERK_SPEED_OPTIMIZER

apollo PiecewiseJerkSpeedProblem 速度优化 QP 数学推导 - 知乎 (zhihu.com)

Planning 基于二次规划的速度规划 - 知乎 (zhihu.com)

1. 优化变量

1. 目标函数

第4项的 ：根据决策制定的速度分配确定的 i 时刻的曲率确定的权重；是曲率关于时间t的函数

但是这里如果曲线优化后，在对应时间的s就不是参考位置的s了，曲率的惩罚也不是优化后对应位置的曲率，所以apollo还提供了一直非线性规划方法。

4. 基于ipopt的非线性速度规划

task：PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER

osqp二次规划存在的问题

二次规划的曲率规划的惩罚pi是曲率关于时间t的函数，但实际上路径的曲率是和s相关的。

二次规划的速度规划通过对动态规划粗糙的st曲线，将关于s的区域惩罚转化为关于t的区域的惩罚，此时如果平滑后的曲线和原来动态规划的st曲线相差不大的时候，其惩罚位置是正确的，如果平滑后的曲线和原始的动态规划的st曲线相差较大，实际的惩罚区间就和设定的不一样。

比如上图中应该惩罚的区间为橙色区间，而二次规划算法实际对于曲率惩罚的区域是在绿色的区域。还有比如地图的限速约束也是和s相关的，所以这样的问题就在于，限速或者曲率的函数是关于s的函数，而s又是待求的量(优化量)，就无法对目标函数施加速度约束或者曲率的约束，只能通过动态规划的st曲线进行转化，转化成t的函数再施加约束，但这样就会导致二次规划算法的st曲线的速度约束不精确的问题。

ipopt非线性规划求解

f(x)为代价函数，g(x)为限制条件，f(x)和g(x)都可以为非线性或者是非凸的函数，但必须为二次连续可微（及二阶导数是连续的）；

非线性优化器求解主要是考虑路径曲率对速度规划的影响，因为曲率约束是一个非线性的函数。

Planning 基于非线性规划的速度规划 - 知乎 (zhihu.com)

1. 速度点集的osqp优化平滑

2. 道路曲率的osqp优化平滑

3. 道路限速的osqp优化平滑

等间隔的时间采样，s，s一阶导数，s二阶导数，以及s的两组松弛变量，用于避免求解失败

额外加入了横向加速度和松弛变量的优化。

横向加速度和曲率有关，由于曲率是关于s的关系式，这里还要对该曲率进行进一步的平滑 ，因为对于非线性规划求解器，无论是目标函数还是约束都是要求函数能够二阶可导：

曲率之前是怎么来的呢？

上节课讲到了，规划路径是通过分段多项式的二次规划算法求得的，每一段的s和l都是保持的三阶导为定值的一个多项式的曲线关系，它可以保证sl是二阶可导，但是求得s的曲率实际上并不是二阶可导的，所以这里需要对曲率曲线重新做一个拟合，生成s关于曲率二阶可导的一个关系式。这里apollo同样采用分段多项式的方法获得s关于曲率kappa的关系式。首先对s进行采样，然后求解s关于曲率的分段多项式的关系式。

限速函数既不连续也不可导，所以要对限速曲线进行平滑；

对于速度规划问题，我们如何计算一个初始解：

分段多项式二次求解作为非线性规的初值

① 全网独家视频课程

BEV感知、毫米波雷达视觉融合、多传感器标定、多传感器融合、多模态3D目标检测、点云3D目标检测、目标跟踪、Occupancy、cuda与TensorRT模型部署、协同感知、语义分割、自动驾驶仿真、传感器部署、决策规划、轨迹预测等多个方向学习视频（扫码即可学习）

视频官网：www.zdjszx.com

② 国内首个自动驾驶学习社区

近2000人的交流社区，涉及30+自动驾驶技术栈学习路线，想要了解更多自动驾驶感知（2D检测、分割、2D/3D车道线、BEV感知、3D目标检测、Occupancy、多传感器融合、多传感器标定、目标跟踪、光流估计）、自动驾驶定位建图（SLAM、高精地图、局部在线地图）、自动驾驶规划控制/轨迹预测等领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频，期待交流！

③【自动驾驶之心】技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多模态感知、Occupancy、多传感器融合、transformer、大模型、点云处理、端到端自动驾驶、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向。扫码添加汽车人助理微信邀请入群，备注：学校/公司+方向+昵称（快速入群方式）

④【自动驾驶之心】平台矩阵，欢迎联系我们！