【转载】基于Frenet优化轨迹的无人车动作规划方法

论文下载 ：https://download.csdn.net/download/enterenvy/11061495

基于Frenet坐标系的动作规划方法由于是由BMW的Moritz Werling提出的，为了简便，我们在后文中也会使用Werling方法简称。在讨论基于Frenet坐标系的动作规划方法之前，我们首先得定义什么是最优的动作序列：对于横向控制而言，假定由于车辆因为之前躲避障碍物或者变道或者其他制动原因而偏离了期望的车道线，那么此时最优的动作序列（或者说轨迹）是在车辆制动能力的限制下，相对最安全，舒适，简单和高效的轨迹。

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

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

那么现在我们的动作规划问题中的配置空间就一共有三个维度：(s,d,t)(s,d,t) , tt 是我们规划出来的每一个动作的时间点，轨迹和路径的本质区别就是轨迹考虑了时间这一维度。

Werling的动作规划方法一个很关键的理念就是将动作规划这一高维度的优化问题分割成横向和纵向两个方向上的彼此独立的优化问题，具体来看下面的图：

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

Jerk最小化和5次轨迹多项式求解
由于我们将轨迹优化问题分割成了 ss 和 dd 两个方向，所以Jerk最小化可以分别从横向和纵向进行，令 pp 为我们考量的配置（即 ss 或 dd），加加速度JtJt关于配置 pp 在时间段 t1−t0t1−t0 内累计的Jerk的表达式为：

Jt(p(t))=∫t1t0p(τ)2dτ
Jt(p(t))=∫t0t1p(τ)2dτ
现在我们的任务是找出能够使得 Jt(p(t))Jt(p(t)) 最小的 p(t)p(t) ，Takahashi的文章——Local path planning and motion control for AGV in positioning中已经证明，任何Jerk最优化问题中的解都可以使用一个5次多项式来表示：

p(t)=α0+α1t+α2t2+α3t3+α4t4+α5t5
p(t)=α0+α1t+α2t2+α3t3+α4t4+α5t5
要解这个方程组需要一些初始配置和目标配置，以横向路径规划为例，初始配置为 D0=[d0,d0˙,d0¨]D0=[d0,d0˙,d0¨] ，即 t0t0 时刻车辆的横向偏移，横向速度和横向加速度为 d0,d0˙,d0¨d0,d0˙,d0¨ ，即可得方程组：

d(t0)=αd0+αd1t0+αd2t20+αd3t30+αd4t40+αd5t50
d(t0)=αd0+αd1t0+αd2t02+αd3t03+αd4t04+αd5t05

d˙(t0)=αd1+2αd2t0+3αd3t20+4αd4t30+5αd5t40
d˙(t0)=αd1+2αd2t0+3αd3t02+4αd4t03+5αd5t04

d¨(t0)=2αd2+6αd3t0+12αd4t20+20αd5t30
d¨(t0)=2αd2+6αd3t0+12αd4t02+20αd5t03
为了区分横向和纵向，我们使用 αdiαdi 和 αsiαsi 来分别表示d和s方向的多项式系数，同理，根据横向的目标配置 D1=[d1,d1˙,d1¨]D1=[d1,d1˙,d1¨] 可得方程组：

d(t1)=αd0+αd1t1+αd2t21+αd3t31+αd4t41+αd5t51
d(t1)=αd0+αd1t1+αd2t12+αd3t13+αd4t14+αd5t15

d˙(t1)=αd1+2αd2t1+3αd3t21+4αd4t31+5αd5t41
d˙(t1)=αd1+2αd2t1+3αd3t12+4αd4t13+5αd5t14

d¨(t1)=2αd2+6αd3t1+12αd4t21+20αd5t31
d¨(t1)=2αd2+6αd3t1+12αd4t12+20αd5t13
我们通过令 t0=0t0=0来简化这个六元方程组的求解，可直接求得 αd0αd0 ，αd1αd1 和 αd2αd2 为：
αd0=d(t0)
αd0=d(t0)

αd1=d˙(t0)
αd1=d˙(t0)

αd2=d¨(t0)2
αd2=d¨(t0)2
令 T=t1−t0T=t1−t0 ，剩余的三个系数 αd3,αd4,αd5αd3,αd4,αd5 ，可通过解如下矩阵方程得到：

⎡⎣⎢T33T26TT44T312T2T55T420T3⎤⎦⎥×⎡⎣⎢αd3αd4αd5⎤⎦⎥=⎡⎣⎢⎢d(t1)−(d(t0)+d˙(t0)T+12d¨(t0)T2)d˙(t1)−(d˙(t0)+d¨(t0)T)d¨(t1)−d¨(t0)⎤⎦⎥⎥
[T3T4T53T24T35T46T12T220T3]×[αd3αd4αd5]=[d(t1)−(d(t0)+d˙(t0)T+12d¨(t0)T2)d˙(t1)−(d˙(t0)+d¨(t0)T)d¨(t1)−d¨(t0)]
该方程的解可以通过Python的Numpy中的 np.linalg.solve 简单求得。至此，我们在给定任意的初始配置 D0=[d0,d0˙,d0¨]D0=[d0,d0˙,d0¨] ，目标配置 D1=[d1,d1˙,d1¨]D1=[d1,d1˙,d1¨] 以及制动时间 TT 的情况下，可以求的对应的 dd 方向关于时间 tt 的五次多项式的系数，同理，可以使用相同的方法来求解纵向（即 ss 方向）的五次多项式系数。那么问题来了，我们如何去确定最优的轨迹呢？ Werling方法的思路是通过一组目标配置来求得轨迹的备选集合，然后在备选集合中基于Jerk最小化的原则选择最优轨迹 ，我们仍然以 dd 方向的优化轨迹为例讲解：

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

[d1,d1˙,d1¨,T]ij=[di,0,0,Tj]
[d1,d1˙,d1¨,T]ij=[di,0,0,Tj]
对于优化问题而言，我们实际上希望车辆最终沿着参考线（道路中心线）平行的方向行驶，所以我们令 di˙=di¨=0di˙=di¨=0 ，那么目标配置只涉及 didi 和 TjTj 两个变量的组合，而这两个变量在无人驾驶的应用场景中实际上是受限的，我们可以通过定义 (dmin,dmax)(dmin,dmax) 和 (Tmin,Tmax)(Tmin,Tmax) 来约束目标配置的取值范围，通过 ΔdΔd 和 ΔTΔT 来限制采样密度，从而在每一个制动周期获得一个有限的备选轨迹集合，如下图所示：

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

Cd=kjJt(d(t))+ktT+kdd21
Cd=kjJt(d(t))+ktT+kdd12
该损失函数包含三个惩罚项：

• kjJt(d(t))kjJt(d(t)) ：惩罚Jerk大的备选轨迹；
• ktTktT ：制动应当迅速，时间短；
• kdd21kdd12 ：目标状态不应偏离道路中心线太远

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

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

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

• 跟车
• 汇流和停车
• 车速保持

在本文中我们详细了解车速保持场景下的纵向轨迹优化，在高速公路等应用场景中，目标配置中并不需要考虑目标位置（即 s1s1 ），所以在该场景下，目标配置仍然是 (s0,s0˙,s0¨)(s0,s0˙,s0¨) ，目标配置变成了 (s1˙,s1¨)(s1˙,s1¨) ，损失函数为：

Cs=kjJt(s(t))+ktT+ks˙(s1˙−sc˙)2
Cs=kjJt(s(t))+ktT+ks˙(s1˙−sc˙)2
其中 sc˙sc˙ 是我们想要保持的纵向速度，第三个惩罚项的引入实际上是为了让目标配置中的纵向速度尽可能接近设定速度，该情景下的目标配置集为：

[s1˙,s1¨,T]ij=[[sc˙+Δsi˙],0,Tj]
[s1˙,s1¨,T]ij=[[sc˙+Δsi˙],0,Tj]
即优化过程中的可变参数为 Δsi˙Δsi˙ 和 TjTj ，同样，也可以通过设置 ΔTΔT 和 ΔΔsi˙ΔΔsi˙ 来设置轨迹采样的密度，从而获得一个有限的纵向轨迹集合：

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

Ctotal=klatCd+klonCs
Ctotal=klatCd+klonCs
这样，我们就可以通过最小化 CtotalCtotal 得到优化轨迹集合（我们不能得到“最优”的轨迹多项式参数，还可以得到“次优”，“次次优”轨迹等等）。

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

s方向上的速度是否超过设定的最大限速
s方向的加速度是否超过设定的最大加速度
轨迹的曲率是否超过最大曲率
轨迹是否会引起碰撞（事故）

通常来说，障碍物规避又和目标行为预测等有关联，本身即使一个复杂的课题，高级自动驾驶系统通常具备对目标行为的预测能力，从而确定轨迹是否会发生事故。在本节中，我们关注的重点是无人车的动作规划，故后面的实例仅涉及静态障碍物的规避和动作规划。

原文地址
https://blog.csdn.net/adamshan/article/details/80779615