【论文笔记】Optimal Vehicle Path Planning Using Quadratic Optimization for Baidu Apollo Open Platform

Summary

本文介绍了基于优化的自动驾驶路径规划方法，通过解耦为参考线生成和基于分段加加速度的优化两个阶段，有效地利用了道路结构，实现了更清晰的场景理解。其中，参考线生成采用二次规划进行解决，并且考虑了参考线曲率导数的连续性，路径边界生成部分考虑了交通规则、自车状态以及堵塞障碍物信息的决策树，路径优化采用连续性、安全性和曲率约束等方法，采用OSQP求解器进行求解。该方法已在百度阿波罗开放平台发布，并进行了多种场景的路测，具有较高的效率和鲁棒性。

I. 介绍

路径要求平顺且安全，符合车辆的非完整约束，同时还要在动态、复杂的场景中保持较好的鲁棒性。而要做到这些，需要面临不小的挑战。

对此，作者提供了一个基于优化的路径规划方法，将路径规划问题解耦成两个阶段：第一个阶段基于地图数据找到一条合适的参考线；第二个阶段基于Frenet坐标系进行基于分段加加速度的优化（Piecewise-Jerk）。

II. 相关工作

在这一部分介绍了过去的一些方法的缺陷，也介绍了基于优化的算法的优势。

作者指出了部分阐明传统的这些采样类规划算法(RRT,PRM等等)无法利用结构化道路环境中的domain knowledge；Discrete search类的方法由于其复杂度随着环境复变得杂而指数级增长，难以适用于复杂的环境之中；State lattice可以被视作一类特殊的Discrete Search，不过得到的路径比较依赖于采样的单元（比如单元的分布状况、数量等等，这些会显著影响计算量），而且需要对环境信息有一个较高程度的建模，对于动态环境适应程度一般。

优化类的算法能够实现最优性建模，同时由于将离散化后的路径/轨迹作为优化变量，对于复杂场景的鲁棒性较好。

在Frenet坐标系中规划的优点是有效地利用了道路结构，实现了更清晰的场景理解。

作者在这里提到了一篇文献[1]，这篇文献的方法是先转换到Frenet坐标系中，并且对参考线进行横向、纵向上的解耦，将规划问题在1D的空间内进行计算，分别得到基于多项式的横纵向的轨迹集，最后将横纵向重新组合并转换到笛卡尔坐标系中进行筛选。（这个思路和Apollo具体实现思路还是比较相似的，不过Apollo是采用优化的方法得到一维的轨迹）

[1] Werling, Moritz, et al. “Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenét Frame.” 2010 IEEE International Conference on Robotics and Automation, 2010, https://doi.org/10.1109/robot.2010.5509799.

作者指出多项式的轨迹难以适用于复杂的场景之中，因此结合了优化方法以及Frenet坐标系的优势，构建了二次规划问题，规划实时性较好。因为在复杂、动态场景中的自动驾驶需要较短的规划周期，他们构建了QP问题进行求解，最后测试时路径优化部分在他们的测试条件下可以跑到15ms左右，参考线生成与平滑20ms左右(不过，参考线由于直接依赖于地图数据，可以离线进行规划，节省在线规划时间)。

III. 问题定义

基于自行车模型建模的，这部分比较基础就不细讲了。

不过Apollo在$(x,y,\theta )$的基础上增加了曲率$\kappa$这一个维度。

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

路径规划的问题就是要在复杂、动态的环境中找到一条安全无碰撞、符合车辆动力学约束同时若是载人的，还需拥有一定的成员舒适度。

Apollo总体策略还是考虑了动态障碍物的，不过本文（基于路径规划部分）还是主要考虑静态障碍物。

下图是Apollo基于Frenet坐标系运动规划的一个示意图。这张图说明了apollo将Path Planning的问题解耦成路径规划和速度规划两部分，之后再重新组合转换到原始坐标系中。车辆的状态变量为$(x,y,\theta ,\kappa ,v,a)$应该是考虑了速度规划的部分。

下图很好地说明了路径规划的具体流程：

• 参考线生成
• Map-Frenet坐标系转换
• 路径边界计算与估计
• 路径规划
• Frenet-Map坐标系转换
  

IV. 参考线平滑与生成

一般都参考线只包括位置信息$(x,y)$，但如此一来则严重影响到路径规划的效果与质量。Apollo依据文献[1]指出，至少要保证参考线曲率导数的连续，如此可以减少路径的不平滑度以及跟踪控制时可能产生的超调。

Apollo在他们之前的工作中利用五次螺旋线构造参考线，同时利用非线性规划的方式去解决，但这种方式计算量和计算难度上都较大，实时性难以保证。于是，他们在这次的工作中采用了二次规划的方式进行解决（不过具体目标函数的构造以及二次规划问题是如何建立的在这篇文章中没有提及，只是阐述了他们方案的一种思路）。这篇博文Apollo星火计划学习笔记——参考线平滑算法解析及实现（以U型弯道场景仿真调试为例）有相应的描述。

看图说话，主要是最小化$p_i$到$p_{i-1}$和$p_{i+1}$之间的欧式距离，同时考虑可能的地图误差，允许输入点在一定程度（一定程度用一个boundingbox去约束）上偏离其原始坐标，以在保持线的原始形状的同时实现可能更高的平滑度。此外，为了确保安全，平滑的引导线必须保持在车道边界内。

具体构造大致如下：

$$\begin{array}{rl}& min\sum _{k=1}^{n-2}||2P_k-P_{k-1}+P_{k+1}|{|}_2^2+\\ & \sum _{k=0}^{n-1}||2P_k-P_{k-ref}|{|}_2^2+\\ & \sum _{k=0}^{n-2}||P_{k+1}-P_k|{|}_2^2\\ & s.t.P_k\in B,for:k=0,\dots ,n-1||2P_k-P_{k-1}-P_{k+1}|{|}_2<\frac{d_{ref}^2}{R_{min}}for:k=1,\dots ,n-2\end{array}$$

其中：

• $P_k$是$(x_k,y_k)$
• $P_{k-ref}$是参考线的原始点
• $B$是$P_k$的边界
• $\frac{d_{ref}^2}{R_{min}}$是最大曲率约束

V. 路径边界

路径边界这部分和现在apollo8.0的部分有许多不一样的地方。这里的路径边界生成主要包括两部分：道路选择决策以及路径边界生成。

---

对于道路选择方面，一个简单的方法是使用所有可用的车道作为可驾驶区域。但未考虑不必要的换道行为所带来的危险影响以及在部分区域需要暂时借用相邻或对向车道这种情况。

对此，他们采用了基于交通规则、自车状态、堵塞障碍物信息的决策树去决定使用什么车道。

---

路径边界生成这一步的目标是通过考虑车辆的位置和周围的障碍物，对从上一步到具体边界的可用车道进行精细处理。

以 $\Delta s$为间隔进行离散化。从第一个点$s_0$开始向前搜索，如果$s_i$位置没有障碍物，直接以车道的边界作为路径边界，继续搜索下一个点。如果在$s_i$的位置有静止或低速障碍物，依据自车估计出的$l$以及除去障碍物剩下的可用的空间，对可能的侧向通过方向进行排序。接着从最宽点处搜索后续的点，如果搜索失败，则回溯，搜索其他方向（Apollo8.0的代码暂时没看到有回溯这一操作）。动态障碍物或高速障碍物在速度规划部分考虑。

最后输出是一系列$(l_{min},l_{max})$路径边界点对构成的路径边界。

VI. 路径优化

定义变量

$$\begin{array}{ccccccccc}{l}_0& & l_1& & l_2& & l_{n-2}& & l_{n-1}\\ l_0^{\prime }& \stackrel{\Delta s}{}& l_0^{\prime }& \stackrel{\Delta s}{}& l_2^{\prime }& \dots & l_{n-2}^{\prime }& \stackrel{\Delta s}{}& l_{n-1}^{\prime }\\ l_0^{\prime \prime }& & l_0^{\prime \prime }& & l_2^{\prime \prime }& & l_{n-2}^{\prime \prime }& & l_{n-1}^{\prime \prime }\end{array}$$

目标函数

• 安全无碰撞
• 最小化横向偏差
• 最小化横向移动(速度、加速度)
• 与障碍物有适当的距离(但感觉这种操作可能会使轨迹偏离道路中心线)。
  $$\begin{array}{rl}\mathbf{f}\left(\mathbf{l}(s)\right)& \begin{array}{r}=w_l\ast \int \mathbf{l}(s{)}^{2}ds+w_{l^{\prime }}\ast \int {\mathbf{l}}^{\prime }(s{)}^{2}ds\end{array}\\ & +w_{l^{\prime \prime }}\ast \int {\mathbf{l}}^{\prime \prime }(s{)}^{2}ds+w_{l^{\prime \prime \prime }}\ast \int {\mathbf{l}}^{\prime \prime \prime }(s{)}^{2}ds\\ & \begin{array}{r}+w_{obs}\ast \int (\mathbf{l}(s)-0.5\ast ({\mathbf{l}}_B(s{)}_{min}+{\mathbf{l}}_B(s{)}_{max}){)}^{2}ds\end{array}\end{array}$

PS：用常三阶导数项$l^{\prime \prime \prime }$连接相邻的离散点，其值用相邻的二阶导数差分得到。

设计约束

• 连续性约束
  $$\begin{array}{rl}& l_{i+1}^{\prime \prime }\begin{array}{rl}=l_i^{\prime \prime }+{\int }_0^{\Delta s}{l}_{i\to i+1}^{\prime \prime \prime }ds& =l_i^{\prime \prime }+l_{i\to i+1}^{\prime \prime \prime }\ast \Delta s\end{array}\\ & l_{i+1}^{\prime }=l_i^{\prime }+{\int }_0^{\Delta s}{\mathbf{l}}^{\prime \prime }(s)ds=l_i^{\prime }+l_i^{\prime \prime }\ast \Delta s+\frac{1}{2}\ast l_{i\to i+1}^{\prime \prime \prime }\ast \Delta s^2\\ & l_{i+1}=l_i+{\int }_0^{\Delta s}{\mathbf{l}}^{\prime }(s)ds\\ & =l_i+l_i^{\prime }\ast \Delta s+\frac{1}{2}\ast l_i^{\prime \prime }\ast \Delta s^2+\frac{1}{6}\ast l_{i\to i+1}^{\prime \prime \prime }\ast \Delta s^3\end{array}$$

• 安全性约束
  $l$方向的点需要在边界内。
  $$l(s)\in l_B(s),\forall s\in [0,s_{max}]$$

• 曲率约束
  自车的转角不能超过最大转角。
  $$tan({\alpha }_{max})\ast k_r\ast l-tan({\alpha }_{max})+|k_r|\ast L⩽0$$

其实可看到apollo后续的版本还是在此基础上做了不少改进的。

采用OSQP求解器。

VII. 结论

• 提出了一种基于优化的自动驾驶汽车路径规划方法。该方法将路径规划解耦为两个主要阶段:第一阶段，生成参考线，这是在Frenet框架中进行规划的先决条件;在第二阶段，采用基于分段加加速度的优化（Piecewise-Jerk）。
• 计算效率很高，在普通PC上平均总计算时间为40ms。
• 该方法已在百度阿波罗开放平台发布，并已部署在硬件上进行了多种场景的路测。