《Optimal Trajectory Generation for Autonomous Vehicles Under Centripetal Acceleration Constraints for In-lane Driving Scenarios》这篇文章是apollo 基于二次规划的速度规划思想的来源。

I. INTRODUCTION

主要介绍了两阶段的算法：第一阶段生成参考线；第二阶段进行以路径时间障碍图(path-time obstacle graph,就是ST boundary)分析得到的无碰撞区域和参考线为输入，生成纵向加速度、向心加速度和加速度最小的无碰撞舒适轨迹。

path-time obstacle graph（ST图）的这种方法通过将周围障碍物的预测轨迹投影到ST图上来对未来时刻是否会与自车发生碰撞进行判断。这种方法适用于结构化道路，能够较好地利用道路的信息(domain kownledge)。

优化类的方法较适用于低维的空间，对于高维空间，求解速度较慢，且容易陷入局部极小值。

II. RELATED WORK

和这篇文章《Optimal Vehicle Path Planning Using Quadratic Optimization for Baidu Apollo Open Platform》基本一致。

笔记：https://blog.csdn.net/sinat_52032317/article/details/132523043?spm=1001.2014.3001.5501

III. PROBLEM DEFINITION

$\kappa$的作用：车辆的即时转向曲率，若车辆的转向轮保持一定的角度，车辆将会做圆周运动，圆周运动的半径就是曲率半径，倒数为曲率$\kappa$。加入$\kappa$，有助于控制模块获得更准确的控制反馈，设计更加平稳的控制器。

在这篇文章中，默认车辆的轨迹会沿着参考线进行，可能的横向误差会被理想的控制器消除。

IV. PHASE I: SMOOTH DRIVING GUIDE LINEGENERATION

参考先是在Frenet坐标系下进行规划的前提。参考线的平滑程度会影响规划出轨迹的质量，依据文献《Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenét Frame》，参考线需要做到曲率微分连续，才能保证获得连续的加速度和转向角。

apollo中使用了一种非线性的优化算法生成参考线。

• 目标： 参考线几何特性的摆动会引起加速度的不平滑以及车辆转向角的摆动，因此需要最小化参考线长度$s$，参考线曲率$\kappa$，以及参考线曲率一阶微分$\dot{\kappa }$
• 参考线的表示： 采用离散-连续结合的方式。$n$个输入点，$n-1$段闭式曲线将点进行连接。
• 约束： 参考线不一定需要完全经过笛卡尔坐标系下的输入点，因为可能存在误差；但是需要约束在一定范围内。

在这篇论文中，作者使用五次多项式螺旋曲线作为道路的平滑表示，以便于在自动驾驶车辆生成轨迹时进行优化。这种曲线的优点是，可以通过直接对其导数求解来轻松推导出其几何性质，如曲线方向、曲率和曲率变化率。同时，与其他曲线表示法（如笛卡尔空间中的参数曲线）相比，螺旋曲线的目标函数可以大大简化。

螺旋线的示意图
$\theta$为切向角，$\kappa =\frac{d\theta }{ds}$,$\dot{\kappa }=\frac{d^2\theta }{{ds}^2}$

文章中给出的参考例子

优化变量：

$\Delta s$为连接两点之间曲线的长度

x = { θ 0 , θ 1 , … , θ n − 1 , θ 0 ˙ , θ 1 ˙ , … , θ n − 1 ˙ , θ 0 ¨ , θ 1 ¨ , … , θ n − 1 ¨ } x=\{\theta_0,\theta_1,\ldots,\theta_{n-1},\dot{\theta_0},\dot{\theta_1},\ldots,\dot{\theta_{n-1}},\ddot{\theta_0},\ddot{\theta_1},\ldots,\ddot{\theta_{n-1}}\} x={θ0​,θ1​,…,θn−1​,θ0​˙​,θ1​˙​,…,θn−1​˙​,θ0​¨​,θ1​¨​,…,θn−1​¨​}

几何特性变量${\theta }_i，{\dot{\theta }}_i，{\ddot{\theta }}_i，{\theta }_{i+1}，{\dot{\theta }}_{i+1}，{\ddot{\theta }}_{i+1}$以及曲线长度$\Delta s_i$唯一决定了五次螺旋线的各项系数

$${\mathbf{\theta }}_i(s)=\sum _{j=0}^5{c}_j\ast s^j$$

论文没有给出具体的目标函数形式以及相应的约束方程，我从网上找到了一个可能的目标函数形式：

$$cost=w_{length}\sum _{i=1}^{n-1}\Delta s_i+w_{kappa}\sum _{i=1}^{n-1}\sum _{i=0}^{m-1}({\dot{\theta }}_i(s_j){)}^2+w_{dkappa}\sum _{i=1}^{n-1}\sum _{i=0}^{m-1}({\ddot{\theta }}_i(s_j){)}^2$$

V. PHASE II: TRAJECTORY GENERATION

A. Optimality Modeling

衡量轨迹质量的几个评价指标：

1. Task achievement任务完成度：通过比较指定的目标状态与车辆在轨迹末端时的状态进行评估。对于in-lane的场景，这部分主要包含两个任务：

• Cruise巡航车辆能够以目标速度巡航，同时能够对出现的障碍物产生相应的决策。
• Stop车辆能够停在指定的目标点或遇到需要进行停止决策的障碍物。

2. safety安全性：
   这部分主要的思想是引入了ST图的概念。用ST图去判断何时会与障碍物发生碰撞。
   通过对ST图的分析，可得在给定时间$t$下避免碰撞的区域$[s_{min},s_{max}]$，同时通过还可以增加$s_{min}$或者减少$s_{max}$来提高自车行驶的安全裕度。（当遇见较为复杂的场景时，apollo采用简单的启发式方法去决定具体的区域，这也意味着可以用更先进的相关推理技术去改进）
   

旁车切入场景下的ST图。绿色：自车；红色：旁车。ST图纵坐标$s$代表了自车沿着中心参考线（本文假定车辆沿着参考线运动，而不像代码中沿着路径规划出来的轨迹进行运动）行驶的距离；$t$即为时间。红色四边形为障碍车在ST图上的投影。两条曲线所构成边界是考虑了自车速度、加速度边界后形成的。

Kant和Zucker首次引入将路径时间解耦来解决速度约束下沿给定路径的动态避障问题。

论文：Kant, Kamal, and Steven W. Zucker. “Toward Efficient Trajectory Planning: The Path-Velocity Decomposition.” The International Journal of Robotics Research, Aug. 1986, pp. 72–89, https://doi.org/10.1177/027836498600500304.

3. Comfort舒适度：
   在这方面，apollo主要考虑了以下因素：

• 纵向加速度：避免频繁刹车或油门的操作；
• 纵向加加速度：减少刹车或油门的变化量；
• 向心加速度：过弯的时候需要减少向心加速度；
• 时间：尽快完成任务。

B. Constraints

约束：
在算法中，假定车辆只会单调前进。基于车辆运动学特性，在轨迹的任意时刻$t$需要满足以下约束才能实现安全性、舒适性等目标：

1. 纵向速度边界：$\dot{s}\in [0,{\dot{s}}_{max}]$；
2. 纵向加速度边界：$\ddot{s}\in [{\ddot{s}}_{min},{\ddot{s}}_{max}]$；
3. 纵向$jerk$边界: $s^{{}^{\prime \prime \prime }}\in [s_{min}^{{}^{\prime \prime \prime }},s_{max}^{{}^{\prime \prime \prime }}]$；
4. 向心加速度边界：$a_c\in [-a_{c_{max}},a_{c_{max}}]$
5. 安全行驶边界：$s\in s_{free}$

C. Trajectory Formulation and Discretization

这部分介绍了两种离散化方式，这两种方式都是离散到$s$的二阶导，以满足相关动力学要求，同时都用了一个恒定的$s^{{}^{\prime \prime \prime }}$去连接两个离散点。

空间参数离散化：

采用固定空间距离$\Delta s$，优化变量包括$\dot{s},\ddot{s}$，$\Delta t$ 为两个离散点之间的时间。

优点：

1. 每一个点的$s$是固定的，也意味着曲率也是固定的，可以提前计算由向心加速度限制的最高速度限制，减少计算量。

缺点：
2. 构建目标函数比较困难。因为$jerk$是通过两个离散点$\ddot{s}$差分进行计算的，但两个点之间的$\Delta t$未知，需要再对目标函数进行划分，这样会导致问题变为一个非凸的问题。
3. 难以利用ST图的信息。因为ST boundary是和时间相关的信息。

时间参数离散化：

采用固定时间间隔$\Delta t$，优化变量包括$s,\dot{s},\ddot{s}$

优点：

1. 无需因为$jerk$的计算对目标函数进行划分。
2. ST boundary的信息容易被利用。

缺点：

1. 难以计算向心加速度以及相关约束。在这种方法下，离散点的速度限制变为了一个与$\kappa$相关的函数。假设最大向心加速度为$a_{c_{max}}$，则向心加速度限制如下：$$-a_{cmax}\le {\dot{s}}^2\ast \mathbf{\kappa }(s)\le a_{cmax}$$为了将这个约束应用到优化过程中，${\dot{s}}^2\ast \kappa (s)$需要是可微的。在这篇文章中，利用五次螺旋线解决这个问题。

2. 另一个问题便是难以衡量到达目标状态的最小时间。对此，作者在目标函数中引入了参考速度这一项进行解决。

D. Complete Optimization Formulation

目标函数

(4)使得车辆能够尽快达到预定义的速度；
(5)、(6)、(7)则是使得车辆能够在轨迹的末端抵达预先定义的状态。

离散化

约束

VI. IMPLEMENTATION AND EXPERIMENTS

主要是利用IPOPT这个库进行求解。

对巡航（舒适风格与运动激进风格）、停车、跟车几个场景进行仿真测试。

VII. CONCLUSION AND FUTURE WORK

主要对扩展到多车道场景的算法改进方向进行讨论：

1. 参考线能够动态适应周围静态障碍物。可以保证速度规划的部分几乎无需改变，但计算耗时。
2. 另一个方向是使用优化来生成横向轨迹/路径$d(s)$，它直接将投影的静态障碍物考虑到Frenet帧上。这个方向与 Frenet 框架规划中的框架更加一致。好处是平滑引导线可以离线生成和存储，并动态加载用于在线使用。此外，车道变化动作将更加干净地处理，因为可以通过简单地替换引导线来规划变道。

后者与apollo现在的方案更加类似。