基于Frenet坐标优化轨迹的无人车动作规划方法

  1. Frenet坐标系下规划的优势
  2. Jerk最小化和五次轨迹多项式求解

本文大部分引用了来自Adam的文章:https://blog.csdn.net/adamshan/article/details/80779615 感谢原作 Adam shan

基于Frenet坐标系的动作规划方法是由Moritz Werling提出的。在讨论基于Frenet坐标系的动作规划方法之前,我们首先需要定义什么是最优的动作序列(或轨迹):对于横向控制而言,假定由于车辆因为之前躲避障碍物、变道等其他制动原因而偏离了期望的车道线,那么此时最优的动作序列(或轨迹)是在车辆制动能力的限制下,相对最安全,舒适,简单和高效的轨迹。

同样的,纵向的最优轨迹也可以这么定义:如果车辆此时过快,或者太接近前方车辆,那么就必须做减速,那么具体什么是“舒适、简单而又安全的”减速呢?我们可以使用 Jerk 这个物理量来描述,Jerk即加速度的变化率,也即加加速度,通常来说,过高的加加速度会会引起乘坐者的不适,所以,从乘坐舒适性而言,应当优化Jerk这个量,同时,引入轨迹的制动周期 T 即一个制动的操作时间:
$T = t_1-t_0 \tag{1}$

Frenet坐标系下规划的优势

在Frenet坐标系中,我们使用道路的中心线作为参考线,使用参考线的切线向量$t$和法线向量$n$建立一个坐标系,如下图右所示,这个坐标系即为Frenet坐标系,它以车辆自身为原点,坐标轴相互垂直,分为$s$方向(即沿着参考线的方向,通常被称为纵向,Longitudinal)和$d$方向(即参考线当前的法向,被称为横向,Lateral),相比于笛卡尔坐标系(下图所示),Frenet坐标系明显地简化了问题,因为在公路行驶中,我们总是能够简单的找到车道的参考线(即车道的中心线),那么基于参考线的位置的表示就可以简单的使用纵向距离(即沿着道路方向的距离)和横向距离(即偏离参考线的距离)来描述,同样的,两个方向的速度($s$ 和 $d$)的计算也相对简单。

那么现在我们的动作规划问题中的配置空间就一共有三个维度:$(s,d,t)$ , $t$ 是我们规划出来的每一个动作的时间点,轨迹和路径的本质区别就是轨迹考虑了时间这一维度。Werling的动作规划方法一个很关键的理念就是将动作规划这一高维度的优化问题分割成横向和纵向两个方向上的彼此独立的优化问题,具体来看下面的图:

假设上游模块(行为规划层)要求当前车辆在 $t8$ 越过虚线完成一次变道,即车辆在横向上需要完成一个 $Δd$ 以及纵向上完成一个 Δs的移动,则可以将 s 和 d分别表示为关于 t 的函数: s(t) 和d(t) (上图右图),那么 $d$,$s$ 关于时间 $t$ 的最优轨迹应该选择哪一条呢?通过这种转换,原来的动作规划问题被解耦成两个独立的优化问题,对于横向和纵向的轨迹优化,我们选取损失函数 $C$ ,将使得$C$最小的轨迹作为最终规划的动作序列。而Werling方法中损失函数的定义,则与我们前面提到的加加速度 Jerk 相关。

Jerk最小化和五次轨迹多项式求解

由于我们将轨迹优化问题分割成了 $s$ 和 $d$ 两个方向,所以Jerk最小化可以分别从横向和纵向进行,令 $p$ 为我们考量的配置(即 $s$ 或 $d$),加加速度$J_t$关于配置 $p$ 在时间段 $t_1-t_0$ 内累计的Jerk的表达式为:
$$\\ J_t(p(t)) = \intop\nolimits_{t_0}^{t_1} p(\tau)^2d\tau \tag{2}$$

现在我们的任务是找出能够使得 $J_t(p(t))$ 最小的 $p(t)$ ,Takahashi的文章——《Local path planning and motion control for AGV in positioning》中已经证明,任何Jerk最优化问题中的解都可以使用一个五次多项式来表示
$$\\ p(t) =\alpha_0 + \alpha_1t + \alpha_2t^2 + \alpha_3t^3 + \alpha_4t^4 + \alpha_5t^5 \tag{3}$$

要解这个方程组需要一些初始配置和目标配置,以横向路径规划为例,初始配置为$D_0 =[d_0, \dot{d_0}, \ddot{d_0}]$,即 $t_0$时刻车辆的横向偏移,横向速度和横向加速度为$d_0, \dot{d_0}, \ddot{d_0}$,即可得方程组:

$$\\ d(t_0) =\alpha_{d0} + \alpha_{d1}t_0 + \alpha_{d2}t_0^2 + \alpha_{d3}t_0^3 + \alpha_{d4}t_0^4 + \alpha_{d5}t_0^5 \tag{4}$$ $$\\ \dot{d}(t_0) =\alpha_{d1} + 2\alpha_{d2}t_0 + 3\alpha_{d3}t_0^2 + 4\alpha_{d4}t_0^3 + 5\alpha_{d5}t_0^4 \tag{5}$$ $$\\ \ddot{d}(t_0) =2\alpha_{d2} + 6\alpha_{d3}t_0 + 12\alpha_{d4}t_0^2 + 20\alpha_{d5}t_0^3 \tag{6}$$

为了区分横向和纵向,我们使用 $\alpha_{di}$和

$\alpha_{si}$来分别表示 d 和 s 方向的多项式系数,同理,根据横向的目标配置$D_1 =[d_1, \dot{d_1}, \ddot{d_1}]$可得方程组:

$$\\ d(t_1) =\alpha_{d0} + \alpha_{d1}t_1 + \alpha_{d2}t_1^2 + \alpha_{d3}t_1^3 + \alpha_{d4}t_1^4 + \alpha_{d5}t_1^5 \tag{7}$$ $$\\ \dot{d}(t_1) =\alpha_{d1} + 2\alpha_{d2}t_1 + 3\alpha_{d3}t_1^2 + 4\alpha_{d4}t_1^3 + 5\alpha_{d5}t_1^4 \tag{8}$$ $$\\ \ddot{d}(t_1) =2\alpha_{d2} + 6\alpha_{d3}t_1 + 12\alpha_{d4}t_1^2 + 20\alpha_{d5}t_1^3 \tag{9}$$

我们通过令

$t_0=0$

来简化这个六元方程组的求解,可直接求得 $\alpha_{d0}$,

$\alpha_{d1}$ 和

$\alpha_{d2}$ 为:

$$\\ \alpha_{d0} = d(t0) \tag{10}$$ $$\\ \alpha_{d1} = \dot{d}(t_0) \tag{11}$$ $$\\ \alpha_{d2} = \ddot{d}(t_0) \tag{12}$$

$T=t_1 − t_0$

剩余的三个系数 $\alpha_{d3}$,

$\alpha_{d4}$,

$\alpha_{d5}$ ,可通过解如下矩阵方程得到:

$$\\ \begin{bmatrix}T^3&T^4&T^5\\3T^2&4T^3&5T^4\\6T&12T^2&20T^3\end{bmatrix} \times \begin{bmatrix}\alpha_{d3}\\\alpha_{d4}\\\alpha_{d5}\end{bmatrix} = \begin{bmatrix}d(t_1) - (d(t_0) + \dot{d}(t_0)T + \frac{1}{2}\ddot{d}(t_0)T^2 )\\\dot{d}(t_1) - (\dot{d}(t_0) + \ddot{d}(t_0)T)\\\ddot{d}(t_1) - \ddot{d}(t_0)\end{bmatrix} \tag{13}$$

该方程的解可以通过Python的Numpy中的 np.linalg.solve简单求得。至此,我们在给定任意的初始配置 $D_0 =[d_0, \dot{d_0}, \ddot{d_0}]$ ,目标配置 $D_1 =[d_1, \dot{d_1}, \ddot{d_1}]$ 以及制动时间 T 的情况下,可以求得对应的 d 方向关于时间 t的五次多项式的系数,同理,可以使用相同的方法来求解纵向(即 s方向)的五次多项式系数。那么问题来了,我们如何去确定最优的轨迹呢? Werling方法的思路是通过一组目标配置来求得轨迹的备选集合,然后在备选集合中基于Jerk最小化的原则选择最优轨迹 ,我们仍然以 d 方向的优化轨迹为例讲解。

我们可以取如下目标配置集合来计算出一组备选的多项式集合:

$$\\ [d_1, \dot{d_1}, \ddot{d_1}, T]_{ij} = [d_i, 0, 0, T_j] \tag{14}$$

对于优化问题而言,我们实际上希望车辆最终沿着参考线(道路中心线)平行的方向行驶,所以我们令 $\dot{d_i} = \ddot{d_i} = 0$,那么目标配置只涉及 $d_i$ 和 $T_j$

两个变量的组合,而这两个变量在无人驾驶的应用场景中实际上是受限的,我们可以通过定义
$($d_{min}$, $d_{max}$)$

$($T_{min}, T_{max}$)$
来约束目标配置的取值范围,通过 $\Delta d$ 和 $\Delta T$来限制采样密度,从而在每一个制动周期获得一个有限的备选轨迹集合,如下图所示:

要在备选集合中选择最优轨迹(即上图中的绿色轨迹),我们需要设计损失函数,对于不同的场景,损失函数也不相同,以横向轨迹为例,在较高速度的情况下,损失函数为:

$$\\ C_d = k_jJ_t(d(t)) + k_tT + k_dd_1^2 \tag{15}$$

该损失函数包含三个惩罚项:

其中 $k_j$,$k_t$ 和 $k_d$ 是这三个惩罚项的系数,它们的比值大小决定了我们的损失函数更加注重哪一个方面的优化,由此我们可以算出所有备选轨迹的损失,取损失最小的备选轨迹作为我们最终的横向轨迹。

值得注意的是,以上的损失函数仅适用于相对高速度的场景,在极端低速的情况下,车辆的制动能力是不完整的,我们不再将d表示为关于时间t的五次多项式,损失函数也会略有不同,但是这种基于有限采样轨迹,通过优化损失函数搜索最优轨迹的方法仍然是一样的。

讨论完横向的轨迹优化问题,我们再来看看纵向的轨迹优化,在不同的场景下纵向轨迹的优化的损失函数也各不相同,Werling方法中将纵向轨迹的优化场景大致分成如下三类:

在本文中我们详细了解车速保持场景下的纵向轨迹优化,在高速公路等应用场景中,目标配置中并不需要考虑目标位置(即 $s_1$ ),所以在该场景下,目标配置仍然是 $(s_0, \dot{s_0}, \ddot{s_0})$ ,目标配置变成了 $(\dot{s_1}, \ddot{s_1})$ ,损失函数为:
$$\\ C_s = k_jJ_t(s(t)) + k_tT + k_{\dot{s}}(\dot{s_1} - \dot{s_c})^2 \tag{16}$$
其中 $\dot{s_c}$ 是我们想要保持的纵向速度,第三个惩罚项的引入实际上是为了让目标配置中的纵向速度尽可能接近设定速度,该情景下的目标配置集为:

$$\\ [\dot{s_1}, \ddot{s_1}, T]_{ij} = [[\dot{s_c} + \Delta\dot{s_i}], 0, T_j] \tag{17}$$

即优化过程中的可变参数为 $\Delta\dot{s_i}$ 和 $T_j$ ,同样,也可以通过设置 $\Delta T$ 和 $\Delta\Delta\dot{s_i}$ 来设置轨迹采样的密度,从而获得一个有限的纵向轨迹集合:

其中,绿线即为纵向最优轨迹。以上我们分别讨论了横向和纵向的最优轨迹搜索方法,在应用中,我们将两个方向的损失函数合并为一个,即:

$$\\ C_{total} = k_{lat}C_d + k_{lon}C_s \tag{18}$$

这样,我们就可以通过最小化 $C_{total}$ 得到优化轨迹集合(我们不能得到“最优”的轨迹多项式参数,还可以得到“次优”,“次次优”轨迹等等)。

避免碰撞(Collision Avoiding)

显然,我们上面的轨迹优化损失函数中并没有包含关于障碍物躲避的相关惩罚,并且我们的损失函数中也没有包含最大速度,最大加速度和最大曲率等制动限制,也就是说我们的优化轨迹集合并没有考虑障碍物规避和制动限制因素,不将障碍物避免加入到损失函数中的一个重要的原因在于碰撞惩罚项的引入将代入大量需要人工调整的参数(即权重),是的损失函数的设计变得复杂 ,Werling方法将这些因素的考量独立出来,在完成优化轨迹以后进行。具体来说,我们会在完成所有备选轨迹的损失计算以后进行一次轨迹检查,过滤掉不符合制动限制的,可能碰撞障碍物的轨迹,检查内容包括:

通常来说,障碍物规避又和目标行为预测等有关联,本身即使一个复杂的课题,高级自动驾驶系统通常具备对目标行为的预测能力,从而确定轨迹是否会发生事故。

Appendix :Paper ‘Optimal Trajectory Generation for Dynamic Street Scenarios in a Frene´t Frame’

A quintic polynomial through the same points and the same time interval will always lead to a smaller cost.

如果你有不同看法?
script>