技术文档丨二次规划样条ST坐标速度优化

知识点

敲黑板，本文需要学习的知识点有 

路径优化的目的是在满足给定的约束条件下，通过优化技术确定路径方案的最优位置。

智能自动化驾驶技术是一门新兴的技术,在汽车自动驾驶的过程中,如何让汽车避开障碍物并以最短的路径到达指定的位置,是智能自动化驾驶汽车的关键性问题。

近年来，基于路径优化的技术已经成为最先进的AV路径规划方法，这项技术的核心是将路径规划问题表述为一个考虑多约束和预期车辆性能的优化问题。模型预测控制（MPC）已被证明非常适合解决路径规划问题，因为它们能够处理多约束和凸问题。此外，MPC以递归方式解决路径优化问题，同时考虑到规划过程中环境状态的更新。

二次规划是非线性规划中的一类特殊数学规划问题，在很多方面都有应用，如投资组合、约束最小二乘问题的求解、序列二次规划在非线性优化问题中应用等。

以下，ENJOY

在QP-Spline-Path（二次规划样条路径）中找到路径后，Apollo将路径上所有的障碍物和ADV（自动驾驶车辆）转换为路径时间（ST）图，该图表示Station沿路径随时间变化。速度优化的任务是在ST图上找到无碰撞且舒适的路径。

Apollo使用样条线段来表示速度曲线，它是ST图中ST点列表。Apollo利用二次规划方法求解最优曲线。QP问题的标准形式如下所示：

将ST曲线划分为n段。每个分段轨迹由多项式定义。

沿着参考线，每一分段i都有一个累积距离增量。默认情况下，该段的轨迹由一个5次多项式定义，如下所示。多项式的次数可通过配置参数调整。

Apollo 首先定义cost1 表示轨迹平滑：

然后，Apollo定义cost2 表示最终S-T轨迹和巡航S-T轨迹之间的差异 (给定速度限制- m个点)：

类似的，Apollo 定义cost3。它表示第一个S-T路径和跟随S-T路径之间的差异 (o 个点)：

最后，目标函数定义如下：

假设起点为，在规划的路径 ，，and（分别代表位置，速度和加速度）上。Apollo将这些约束转换为QP等式约束，如下所示：  

路径必须是单调的，例如，车辆只能向前行驶。

在路径上采样m个点， 对于j 和j-1点对 (j∈ [1,...,m])：

如果这两个点在同一个样条分段k中，满足下列约束：

如果这两个点分别在样条分段k和l中，需满足下列约束： 

该约束主要是为了平滑采样分段的连接处。假设分段和相连，路段的s值为 ，Apollo计算如下约束公式

即: 

等价于

在公式中，t0=0。

可以使用类似的方法求解下列等式约束方程：

沿着路径均匀地采样m个点，并检查这些采样点的障碍物边界。使用以下方法将约束转换为QP不等式约束：

首先，根据道路宽度和周围的障碍物, 找到这些点的下界，其中j∈[0,m]。求解如下所示的不等式约束：

类似的，对于上界，计算如下不等式约束方程：

Apollo还建立了一个速度限制界限。

在st曲线上采样m个点， 每个点j的速度限制由一个上界和一个下界共同定义，例如和。约束条件定义如下：

即

以及

即