Real-Time Trajectory Planning for Autonomous Urban Driving: Framework, Algorithms, and Verifications

Real-Time Trajectory Planning for Autonomous Urban Driving: Framework, Algorithms, and Verifications

Abstract

这篇文章针对的是真实城市道路环境下自动驾驶车辆的实时轨迹规划问题。使用了一个分层的运动规划框架。首先，根据高层的行为决策器和高精度地图，生成一个粗略的参考路径，然后使用共轭梯度非线性优化算法和三次B样条来平滑和插值参考路径；为了能够在跟踪路径的过程中，同时处理动态静态障碍物，轨迹规划在frenet坐标系下被分成了横向和纵向两个单独的部分。

1. INTRODUCTION AND STATE OF THE ART

2. SYSTEM FRAMEWORK

3. SYSTEM ALGORITHMS

这篇文章的话，针对的是有车道线的结构化场景的导航问题。

3.1 Reference Path Smoothing and Interpolation

对于一组给定的的初始参考路径点，通过下列的目标函数进行优化来进行平滑。
$$\mathrm{argmin}\left\{{\omega }_1\sum _{i=1}^{N-1}{\left(\Delta x_{i+1}-\Delta x_i\right)}^2+{\omega }_2\sum _{i=1}^{N-1}{\left(\frac{\Delta {\theta }_i}{|\Delta x_i|}\right)}^2+f\left(x_i\right)\right\}$$

• $\Delta x_i=x_i-x_{i-1}$是在$x_i=\left(x_i,y_i\right)$点处的位移向量；
• $\Delta \theta =\arccos \left(\frac{\Delta x_{i+1}\Delta x_i}{|\Delta x_{i+1}||\Delta x_i|}\right)$ 是在$x_i=\left(x_i,y_i\right)$点处的向量方向变化量；
• $f\left(x_i\right)$ 为一个可导非线性函数，当点$x_i$ 靠近边界时，它的值会变得无穷大，在边界以内的时候，它的值接近0。

完成路径的平滑以后，因为路径点太稀疏，下一步进行路径点之间的插值运算， 插值使用三次B样条进行：

离散控制点为$P_i\left(i=0,1,....n\right)$，分段三次B样条可以写为
C ( u ) = ∑ i = 0 n P i G i , 3 ( u ) = 1 6 [ u 3 u 2 u 1 ] [ − 1 3 − 3 1 3 − 6 3 0 − 3 0 3 0 1 4 1 0 ] [ P i P i + 1 P i + 2 P i + 3 ] u ∈ [ 0 , 1 ] C\left(u\right)=\sum^n_{i=0}P_iG_{i,3}\left(u\right)=\frac{1}{6}

$$\begin{bmatrix}u^3 & u^2 & u & 1 \\ \end{bmatrix}$$ $$\begin{bmatrix} -1 & 3 & -3 & 1 \\ 3 & -6 & 3 & 0 \\ -3 & 0 & 3 & 0 \\ 1 & 4 & 1 & 0 \end{bmatrix}$$ $$\begin{bmatrix} P_i \\ P_{i+1} \\ P_{i+2} \\ P_{i+3} \end{bmatrix}$$ u \in \left[0,1\right] C(u)=i=0∑n​Pi​Gi,3​(u)=61​[u3​u2​u​1​]⎣⎢⎢⎡​−13−31​3−604​−3331​1000​⎦⎥⎥⎤​⎣⎢⎢⎡​Pi​Pi+1​Pi+2​Pi+3​​⎦⎥⎥⎤​u∈[0,1]

其中，$G_{i,3}$ 是B样条的基函数。

3.2 Spatial Path Generation Based on the Curvilinear Coordinate Framework

Frenet 坐标系的优势：可以将车辆的横向和纵向的行为分解开。
经过平滑与插值算法处理后，得到的路径作为基本参考轨迹。

根据微分平坦理论，多项式曲线可以表示从当前状态点到采样终点状态点之间的平滑过度曲线：
$$l\left(s\right)=c_0+c_{1}s+c_2{s}^2+c_3{s}^3$$
为了确定上式中的系数，根据初始边界条件，有：
$$l\left(s_0\right)=l_0,l\left(s_f\right)=l_f$$

$\theta \left(s\right)=\arctan \left(\frac{dl}{ds}\right)$为车辆本体与基础参考轨迹之间的航向偏差。

为了保证生成的路径能够与基础轨迹尽量重合，在预瞄点处航向设置为与基础参考轨迹的切向相同。但是在起点处，车辆的起始航向与基础参考路径的航向可以不同，因此可以得到下列的边界条件：
$$\theta \left(s_0\right)={\theta }_0,\theta \left(s_f\right)=0$$
基于上述的4个边界条件，可以求解得到曲线的多项式系数。

路径规划的采样结果，根据前面提到的4个边界条件，通过变换终点的方式，可以生成一簇备选路径。

在Frenet坐标系中得到路径规划结果后，需要将路径变换回到笛卡尔坐标系中。这里论文里的公式感觉有点问题，计划使用Udacity里面的笛卡尔与Frenet坐标系的转换关系。

3.3 Velocity Profile Geneartion

在速度规划的时候考虑的因素有横向与纵向的加速度约束、速度约束（计及交通规则），
考虑这些约束条件可能会大大减少速度规划的求解空间，并使速度规划者将精力集中在最可能存在最优解的空间上。 因此，在完成空间规划后，基于空间路径规划的结果再执行速度生成器。