apollo速度规划部分的论文阅读

apollo的速度规划理论基础主要来源于论文“Optimal Trajectory Generation for Autonomous Vehicles Under Centripetal Acceleration Constraints for In-lane Driving Scenarios”，而该论文最重要的部分为第三章问题定义以及第五章轨迹生成。

问题定义

在笛卡尔空间中具有微分约束的车辆的配置可以用三个变量来表示，$(x,y,\theta )$，其中，$x,y$指定了车辆的某个参考点的坐标，$\theta$指定了车辆在笛卡尔空间中的航向角。apoolo将一个新维度$\kappa$，即车辆转向产生的即时曲率加入到配置空间中，以进行更精确的建模，因此计算轨迹提供了额外的信息，可用于更好地设计反馈控制器。所以apollo的速度规划本质上是找到一个函数$\tau (t)$将时间$t$映射到特定配置$(x,y,\theta ,\kappa )$。给定一个平滑的驾驶导引导线，Frenet坐标系将车辆在笛卡尔空间中的运动解耦为两个独立的一维运动，沿引导线移动的纵向运动和垂直于引导线移动的横向运动。因此，将笛卡尔空间中的轨迹规划问题转化为Frenet坐标系中的二维低维独立规划问题。该坐标系利用了大多数车辆沿着车道移动的任务域，降低了规划的维数，大大简化了问题。

轨迹生成

最优性建模

自动驾驶汽车的轨迹生成的目标是计算出一个驱动乘客安全舒适地到达目的地的轨迹。以下是常用来衡量轨迹质量的关键因素：

是否完成任务

自动驾驶最重要的应用是让车辆将乘客运送到一个预定义的目的地。这个应用包含了两个任务：
1、巡航：车辆达到目标巡航速度。当存在低速的前障碍物时，车辆与前障碍物保持安全距离。
2、停车：车辆在某一点停车。当车辆到达预定目的地或中间目标，如红灯或停车标志等时，它将停车。
可以通过比较任务的特定目标状态与车辆状态的差异来评估任务是否成功。

安全性

apoolo限制了该算法仅在具有车道的驾驶场景中应用，为了避免与环境中障碍物的碰撞，使用了路径-时间-障碍图即$S-T$图的概念。假设自车的路径是预先确定的，并且可以准确地预测附近障碍物的轨迹。路径-时间-障碍物图的基本思想是计算哪个时间$t$，即预定的路径的哪一个部分被障碍物所阻塞。在给定的时间$t$内，防碰撞分析的输出为允许区域(s)$[s_{min},s_{max}]$。如果$t$ 时刻自车停留在了允许的区域内，则避免与障碍物产生碰撞。在复杂的情况下，给定时间$t$的允许区域可能不是唯一的。简单的启发式方法被用来通过考虑车辆的动态来唯一地确定一个区域，然而，一个具有更先进技术的正式情境推理模块肯定可以提供更好的决策。推理的结果可以被抽象为一个函数$s_{free}(t)$，它在给定任意时间$t$时返回一个区域$(s_{min},s_{max})$。自车还可以通过增加$s_{min}$或减少$s_{max}$来增加安全裕度，以保持与前方障碍物的一定距离。

舒适性

舒适度是衡量轨迹质量的一个重要指标。在apollo的方案中，舒适性模型将考虑以下几个因素：
纵向加速度：最小化纵向加速度，以减少频繁的油门或制动输入。
纵向加速度变化率即加加速度（jerk）：最小化纵向上的jerk，即最小化节气门或制动器的输入变化。
向心加速度：沿运动轨迹的向心加速度最小化。当车辆接近一个曲线时，它必须优雅地减速，以减少由该曲线产生的向心加速度。
时间：为了完成某项任务，如达到目标速度，时间最小。

约束

由于算法是用于结构化环境下的车辆驾驶，所以假设车辆总是单调地向前移动。根据车辆的动力学特性，在时间范围为${\mathrm{t}}_{\tau }$的轨迹的任意时间$t$处都必须满足以下约束条件，以达到安全和舒适性：
纵向速度边界
$\dot{s}(t)\in [0,{\dot{s}}_{\max }]$
纵向加速度边界
$\ddot{s}(t)\in [{\ddot{\mathrm{s}}}_{\min },{\ddot{s}}_{\max }]$
纵向jerk边界

向心加速度边界
${\mathrm{a}}_c(t)\in [-{\dot{a}}_{c_{\max }},{\dot{a}}_{c_{\max }}]$
除了动态约束外，为了实现避免碰撞，任何给定时间$t$的空间值$s$必须位于$s_{free}(t)$给出的允许区域内：
安全边界
$s(t)\in s_{free}(t)$

轨迹的公式和离散化

为了在实际应用中评价约束满意度的同时有效地计算出最优轨迹，一种常用的方法是根据某个参数离散轨迹$s(t)$。然后通过计算这些离散点来逼近目标函数，并对这些点进行约束满足度的检验。
根据用于轨迹离散化的参数，优化的公式，包括目标函数，约束公式，可以非常不同。离散化有两种参数的选择， 尽管对离散化参数的选择存在差异，但两种方法都将轨迹离散为$s$的二阶导数，即$\ddot{s}$，以实现对加速度水平的精确控制，并假定一个恒定的$s$三阶项即jerk连接连续的离散点。

空间参数离散化:

第一种选择是使用空间参数$s$将轨迹离散到一个预定义的分辨率$\Delta \mathrm{s}$。所得到的轨迹用以下变量表示，假设起点为$s=0$：

在这个公式中，连续离散点之间是一个相等的空间距离$\Delta \mathrm{s}$。优化变量包括速度$\dot{s}$、每个点的加速度$\ddot{s}$和连续点之间的时间间隔$\Delta \mathrm{t}$。导引线的曲率$\kappa$是空间参数的函数。
空间参数离散化的主要优点是每个离散点的空间值$s$是固定的，因此曲率是固定的。因此，可以预先计算由向心加速度约束产生的速度上限，并在整个优化过程中保持不变。然而，这种公式也有几个缺点：
难以进行优化的目标函数。对于最优性建模中的一项，纵向jerk最小化，jerk是使用连续点之间的差分来计算的。在这个公式中，目标函数中的优化变量之间会有一个分割项，这导致了一个困难的非凸优化问题。
难以强制执行安全约束约束。对于路径时间障碍分析，输出是一个函数${\mathrm{s}}_{free}(t)$，它将时间t映射到一个安全区域$(s_{min},s_{max})$。在这个公式中，由于时间$t$是一个变量，一个点的安全区域随着时间$t$的变化而变化。由于这些问题，尽管简化了向心加速度约束的公式，但利用空间参数离散轨迹并不是一个很好的选择。

时间参数离散化

时间参数离散化使用时间$t$将轨迹离散到给定的分辨率$\Delta \mathrm{t}$。得到的轨迹用以下变量表示，假设起点$t=0$：

在该公式中，优化变量包括每个离散点上的空间参数$s$、速度$\dot{s}$和加速度$\ddot{s}$。与空间离散化相比，该公式在计算纵向jerk时避免了变量间的划分问题。此外，由于在整个优化过程中每个点的安全边界保持固定，因此更容易将安全边界约束合并为安全边界。
与空间参数离散化相比，时间参数离散化的一个主要挑战是难以考虑涉及向心加速度的约束条件和目标。某一点的向心加速度由两个因素决定：速度$\dot{s}$和曲率$\kappa$。由于$\kappa$与空间参数$s$相关联，在优化过程中迭代变化，曲率$\kappa$相应变化。换句话说，一个离散点的速度极限现在是一个涉及$\kappa (s)$的函数。假设最大向心加速度为固定值$a_{c_{\max }}$，向心加速度极限的约束如下：$$-a_{c_{\max }}\le {\dot{s}}^2\ast \kappa (s)\le a_{c_{\max }}$$
为了将该约束应用到优化过程中，${\dot{s}}^2\ast \kappa (s)$必须是可微的。使用一系列闭式的spiral路径直接作为引导线的表示，可以使这一项可微。
时间参数离散化的另一个困难是建模最小时间偏好。为了近似这种偏好，在目标公式中引入了一个参考速度项可以做到这点。参考速度在不考虑障碍物和曲率的情况下模拟了偏好速度。目标函数中包含了当前速度和参考速度差的积分。这一项鼓励车辆以最少的时间达到参考速度。

完整优化公式

假设一个轨迹$s(t)$由最大时间长度为${\mathrm{t}}_{\tau }$的时间$t$参数化，目标函数设计如下：

其中，$\dot{\mathrm{s}}$，$\ddot{\mathrm{s}}$分别为线速度和加速度。$\kappa$是一个函数，它以一个空间参数$s$作为输入，并输出给定点上的曲率$\kappa$。第1-3项分别直接建模了纵向加速度、加加速度和向心加速度。第4项隐式地建模了完成特定任务的最小化时间。这一项鼓励轨迹快速达到参考速度${\dot{\mathrm{s}}}_{ref}$。第5-7项用于模拟任务偏好，它鼓励车辆在轨迹结束时达到一定的预定义状态。当使用时间参数离散轨迹时，目标函数可以通过计算$n$个离散点来逼近，即将积分形式写成离散形式：

其中加加速度项是通过加速度进行差分得到的：

对这些离散化的点也进行了约束检查：$\forall i\in Z,i\in [0,n-1]$，

然而，为了保持分段constant-jerk轨迹的连续性即连续性约束，连续点之间必须满足以下等式方程：