系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

文章目录

系列文章目录
前言
【核心思想】基于Frenet坐标系的无人车轨迹生成并优化的功能
- 功能
- 基于Frenet坐标系方法生成的路径和直接用三五次样条（三五次多项式）生成的路径的不同之处
一、无人驾驶最优轨迹序列的定义
二、基于Frenet坐标系轨迹优化的过程理论
三、基于Frenet坐标系轨迹优化的过程代码实现
四、障碍物规避实现
总结
参考资料

前言

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！

使用A*路径探索算法、三次样条插值路径生成算法得到的路径实际上只是一个简单的路径，而非我们最终能够执行的轨迹，我们还需要进行后端轨迹优化，基于Frenet坐标系的优化轨迹动作规划方法，该方法在高速情况下的高级车道保持和无人驾驶都具有很强的实用性，是目前普遍采用的一种轨迹优化的算法。

本文先对基于Frenet坐标系的无人车轨迹优化方法做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

提示：以下是本篇文章正文内容

【核心思想】基于Frenet坐标系的无人车轨迹生成并优化的功能

功能

给定地图上的起点和目标点，通过mini_jerk或者mini_snap的插值优化问题，求解生成出两点间的轨迹，并保证了两点间的轨迹最优的
.

基于Frenet坐标系方法生成的路径和直接用三五次样条（三五次多项式）生成的路径的不同之处

直接用三次五次样条（三五次多项式）生成的路径不考虑实际场景，没有代价损失函数，因此没有避障等功能
三次样条插值生成轨迹方法，参考我另外一篇博客
https://blog.csdn.net/qq_35635374/article/details/121772237

.
基于Frenet坐标系方法生成的路径是继承三五次样条的方法生成路径方法的，通过尝试给定sd横纵两个方向的目标点，使用三五次样条的方法生成路径一堆候选路径，再通过求解mini_jerk的优化问题和设计不同代价损失函数项，限幅等选择出最优轨迹
在这里插入图片描述

.
.

一、无人驾驶最优轨迹序列的定义

Frenet坐标系分为横向和纵向两个坐标轴，因为讨论最优轨迹序列应该分成横向控制最优轨迹序列、纵向控制最优轨迹序列

1.横向控制最优轨迹序列

对于横向控制而言，假定由于车辆因为之前躲避障碍物或者变道或者其他制动原因而偏离了期望的车道线，那么此时最优的动作序列（或者说轨迹）是在车辆制动能力的限制下，相对最安全，舒适，简单和高效的轨迹。
.
.

2.纵向控制最优轨迹序列

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

3.Frenet坐标系定义

在Frenet坐标系中，我们使用道路的中心线作为参考线，使用参考线的切线向量 tt 和法线向量 nn 建立一个坐标系，如下图的右图所示，这个坐标系即为Frenet坐标系，它以车辆自身为原点，坐标轴相互垂直，分为 ss 方向（即沿着参考线的方向，通常被称为纵向，Longitudinal）和 dd 方向（即参考线当前的法向，被称为横向，Lateral），相比于笛卡尔坐标系（下图的作图），Frenet坐标系明显地简化了问题，因为在公路行驶中，我们总是能够简单的找到道路的参考线（即道路的中心线），那么基于参考线的位置的表示就可以简单的使用纵向距离（即沿着道路方向的距离）和横向距离（即偏离参考线的距离）来描述，同样的，两个方向的速度（s˙s˙ 和 d˙d˙）的计算也相对简单。
在这里插入图片描述
那么现在我们的动作规划问题中的配置空间就一共有三个维度：(s,d,t) , t 是我们规划出来的每一个动作的时间点，轨迹和路径的本质区别就是轨迹考虑了时间这一维度。

.
.

4.为什么要用Frenet坐标系做轨迹优化

答案：将动作规划这一高维度的优化问题分割成横向和纵向两个方向上的彼此独立的优化问题
在这里插入图片描述
假若规划层中的任务层下达变道的任务，要求当前车辆在 t8t8 越过虚线完成一次变道，即车辆在横向上需要完成一个 ΔdΔd 以及纵向上完成一个 ΔsΔs的移动，则可以将 s和 d 分别表示为关于 t 的函数：s(t)和 d(t)（上图右图），那么 d,sd,s 关于时间 tt 的最优轨迹应该选择哪一条呢？通过这种转换原来的动作规划问题被分割成了两个独立的优化问题，对于横向和纵向的轨迹优化，我们选取损失函数 C ，将使得 C 最小的轨迹作为最终规划的动作序列。

简单来说，就是优化一个具有二维信息的轨迹（x,y）,转换成优化两条独立的一维轨迹s(t)和 d(t)

.
.

二、基于Frenet坐标系轨迹优化的过程理论

Werling方法中损失函数的定义使用了加加速度 Jerk 函数。Jerk即加速度的变化率，通常来说，绝对值过高的加加速度会会引起乘坐者的不适，所以，从乘坐舒适性而言，应当优化Jerk这个量

1.步骤一：设计Jerk最优化问题

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

在这里插入图片描述
现在我们的任务是找出能够使得 Jt(p(t)) 最小的 p(t)
怎么理解上面这个优化问题呢？
上述最优问题表达式是在一段短时间段内对加速度的积分，若加速度曲线震荡，加速度的积分在较短的时间段每的积分至就大，我们的目的是通过优化位置p(t)使得加加速度不震荡
.
.

2.步骤二：求解Jerk最优化问题（即求解一个5次多项式的5各系数）

Takahashi的文章——Local path planning and motion control for AGV in positioning中已经证明，任何Jerk最优化问题中的解都可以使用一个5次多项式来表示：
在这里插入图片描述
要解这个方程组需要一些初始配置和目标配置，以横向路径规划为例，初始配置为 D0=[d0,d0˙,d0¨]，即 t0t0 时刻车辆的横向偏移，横向速度和横向加速度为 d0,d0˙,d0¨，即可得方程组：

在这里插入图片描述
同理，根据横向的目标配置 D1=[d1,d1˙,d1¨] 可得方程组：

我们通过令 t0=0来简化这个六元方程组的求解，可直接求得 αd0 ，αd1 和 αd2 为：

令 T=t1−t0 ，剩余的三个系数 αd3,αd4,αd5 ，可通过解如下矩阵方程得到：
在这里插入图片描述
该方程的解可以通过Python的Numpy中的 np.linalg.solve 简单求得。

至此，我们在给定任意的初始配置 D0=[d0,d0˙,d0¨]，目标配置 D1=[d1,d1˙,d1¨]以及制动时间 T 的情况下，可以求的对应的 dd 方向关于时间 t 的五次多项式的系数

为了区分横向和纵向，我们使用 αdi 和 αsi 来分别表示d和s方向的多项式系数，同理，可以使用相同的方法来求解纵向（即 s 方向）的五次多项式系数。

.
.

3.步骤三：确定最优的轨迹

第一步：通过一组目标配置来求得轨迹的备选集合

横向轨迹的备选集合

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

纵向轨迹的备选集合

在不同的场景下纵向轨迹的优化的损失函数也各不相同，Werling方法中将纵向轨迹的优化场景大致分成如下三类：

跟车
汇流和停车
车速保持

在本文中我们详细了解车速保持场景下的纵向轨迹优化，在高速公路等应用场景中，目标配置中并不需要考虑目标位置（即 s1s1 ），所以在该场景下，目标配置仍然是 (s0,s0˙,s0¨)，目标配置变成了 (s1˙,s1¨)，损失函数为：
在这里插入图片描述
其中 sc˙ 是我们想要保持的纵向速度，第三个惩罚项的引入实际上是为了让目标配置中的纵向速度尽可能接近设定速度，该情景下的目标配置集为：

即优化过程中的可变参数为 Δsi˙ 和 Tj ，同样，也可以通过设置 ΔT 和 ΔΔsi˙ 来设置轨迹采样的密度，从而获得一个有限的纵向轨迹集合：
在这里插入图片描述
.
.

第二步：备选集合中基于Jerk最小化的原则，设计代价损失函数来选择最优轨迹

要在备选集合中选择最优轨迹（即上图中的绿色轨迹），我们需要设计损失函数，对于不同的场景，损失函数也不相同

横向轨迹的损失函数设计

在较高速度的情况下，横向轨迹的损失函数为：
在这里插入图片描述
该损失函数包含三个惩罚项：

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

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

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

横向轨迹 + 纵向轨迹的损失函数设计

。以上我们分别讨论了横向和纵向的最优轨迹搜索方法，在应用中，我们将两个方向的损失函数合并为一个，即：
在这里插入图片描述
这样，我们就可以通过最小化 Ctotal 得到优化轨迹集合（我们不能得到“最优”的轨迹多项式参数，还可以得到“次优”，“次次优”轨迹等等）。
.
.

第三步：通过限幅剔除轨迹（事故避免）

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

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

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

三、基于Frenet坐标系轨迹优化的过程代码实现

完整代码链接：
https://download.csdn.net/download/adamshan/10494062

1.首先，我们生成要追踪的参考线以及静态障碍物

# 路线
wx = [0.0, 10.0, 20.5, 30.0, 40.5, 50.0, 60.0]
wy = [0.0, -4.0, 1.0, 6.5, 8.0, 10.0, 6.0]
# 障碍物列表
ob = np.array([[20.0, 10.0],
               [30.0, 6.0],
               [30.0, 5.0],
               [35.0, 7.0],
               [50.0, 12.0]
               ])

tx, ty, tyaw, tc, csp = generate_target_course(wx, wy)

生成如下参考路径以及障碍物,其中红线就是我们的全局路径，蓝点为障碍物
在这里插入图片描述

2.定义一些参数

# 参数
MAX_SPEED = 50.0 / 3.6  # 最大速度 [m/s]
MAX_ACCEL = 2.0  # 最大加速度[m/ss]
MAX_CURVATURE = 1.0  # 最大曲率 [1/m]
MAX_ROAD_WIDTH = 7.0  # 最大道路宽度 [m]
D_ROAD_W = 1.0  # 道路宽度采样间隔 [m]
DT = 0.2  # Delta T [s]
MAXT = 5.0  # 最大预测时间 [s]
MINT = 4.0  # 最小预测时间 [s]
TARGET_SPEED = 30.0 / 3.6  # 目标速度（即纵向的速度保持） [m/s]
D_T_S = 5.0 / 3.6  # 目标速度采样间隔 [m/s]
N_S_SAMPLE = 1  # 目标速度的采样数量
ROBOT_RADIUS = 2.0  # robot radius [m]

# 损失函数权重
KJ = 0.1
KT = 0.1
KD = 1.0
KLAT = 1.0
KLON = 1.0

3.使用基于Frenet的优化轨迹方法生成一系列横向和纵向的轨迹，并且计算每条轨迹对应的损失

def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
    frenet_paths = []

    # 采样，并对每一个目标配置生成轨迹
    for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):

        # 横向动作规划
        for Ti in np.arange(MINT, MAXT, DT):
            fp = Frenet_path()
            # 计算出关于目标配置di，Ti的横向多项式
            lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)

            fp.t = [t for t in np.arange(0.0, Ti, DT)]
            fp.d = [lat_qp.calc_point(t) for t in fp.t]
            fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]
            fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
            fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]

            # 纵向速度规划 (速度保持)
            for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE, TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
                tfp = copy.deepcopy(fp)
                lon_qp = quartic_polynomial(s0, c_speed, 0.0, tv, 0.0, Ti)

                tfp.s = [lon_qp.calc_point(t) for t in fp.t]
                tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
                tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
                tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]

                Jp = sum(np.power(tfp.d_ddd, 2))  # square of jerk
                Js = sum(np.power(tfp.s_ddd, 2))  # square of jerk

                # square of diff from target speed
                ds = (TARGET_SPEED - tfp.s_d[-1]) ** 2
                # 横向的损失函数
                tfp.cd = KJ * Jp + KT * Ti + KD * tfp.d[-1] ** 2
                # 纵向的损失函数
                tfp.cv = KJ * Js + KT * Ti + KD * ds
                # 总的损失函数为d 和 s方向的损失函数乘对应的系数相加
                tfp.cf = KLAT * tfp.cd + KLON * tfp.cv

                frenet_paths.append(tfp)

    return frenet_paths

.
.

4.五次多项式(五次样条曲线求解)类

这里的五次多项式的系数的求解过程和我们前面的理论讲解是一样的，只不过我们使用Numpy中的 np.linalg.solve(A, b) 方法将矩阵解了出来。

class quintic_polynomial:
    def __init__(self, xs, vxs, axs, xe, vxe, axe, T):
        # 计算五次多项式系数
        self.xs = xs
        self.vxs = vxs
        self.axs = axs
        self.xe = xe
        self.vxe = vxe
        self.axe = axe

        self.a0 = xs
        self.a1 = vxs
        self.a2 = axs / 2.0

        A = np.array([[T ** 3, T ** 4, T ** 5],
                      [3 * T ** 2, 4 * T ** 3, 5 * T ** 4],
                      [6 * T, 12 * T ** 2, 20 * T ** 3]])
        b = np.array([xe - self.a0 - self.a1 * T - self.a2 * T ** 2,
                      vxe - self.a1 - 2 * self.a2 * T,
                      axe - 2 * self.a2])
        x = np.linalg.solve(A, b)

        self.a3 = x[0]
        self.a4 = x[1]
        self.a5 = x[2]

    def calc_point(self, t):
        xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
             self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5

        return xt

    def calc_first_derivative(self, t):
        xt = self.a1 + 2 * self.a2 * t + \
             3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4

        return xt

    def calc_second_derivative(self, t):
        xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3

        return xt

    def calc_third_derivative(self, t):
        xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2

        return xt

.
.

四、障碍物规避实现

def check_paths(fplist, ob):
    okind = []
    for i in range(len(fplist)):
        if any([v > MAX_SPEED for v in fplist[i].s_d]):  # 最大速度检查
            continue
        elif any([abs(a) > MAX_ACCEL for a in fplist[i].s_dd]):  # 最大加速度检查
            continue
        elif any([abs(c) > MAX_CURVATURE for c in fplist[i].c]):  # 最大曲率检查
            continue
        elif not check_collision(fplist[i], ob):
            continue

        okind.append(i)

    return [fplist[i] for i in okind]

由此可以看出，最终的优化轨迹的选择并不单纯基于最小损失函数，轨迹检查还会过滤掉一些轨迹，所以使用基于Frenet的优化轨迹来做无人车的动作规划，通常能够找到有限集的最优解，当最优解无法通过检查是，自会采用“次优解”甚至更加“次优的”解。

总结

1、基于Frenet坐标系的无人车轨迹生成并优化的功能：

给定地图上的起点和目标点，通过mini_jerk或者mini_snap的插值优化问题，求解出两点间的轨迹，并保证了两点间的轨迹最优的

2、基于Frenet坐标系的无人车轨迹生成并优化的使用方法

可以把轨迹从笛卡尔坐标系转换到frenet坐标系，完成基于Frenet坐标系的无人车轨迹优化后，把轨迹再转换到笛卡尔坐标系
.

3、基于Frenet坐标系的无人车轨迹生成并优化是继承五次样条（五次多项式曲线）生成路径的方法的

.
.

4、基于Frenet坐标系计算五次样条的方法和基于笛卡尔坐标系计算五次样条的方法异同

基于笛卡尔坐标系计算五次样条的方法，可以看我这篇博客
https://blog.csdn.net/qq_35635374/article/details/121772237
https://blog.csdn.net/qq_35635374/article/details/120732771

基于笛卡尔坐标系计算五次样条的方法的轨迹是是二维的，即横纵坐标是xy-- y(x)
基于Frenet坐标系计算五次样条的方法的轨迹是把一维（把二维轨迹解耦至两个一维轨迹），即s(t)、d(t)

参考资料

https://blog.csdn.net/AdamShan/article/details/80779615

【五次样条路径生成并Frenet坐标系解耦轨迹优化】基于Frenet坐标系的无人车轨迹生成并优化方法