无人驾驶汽车系统入门（二十一）——基于Frenet优化轨迹的无人车动作规划方法

动作规划动作在无人车规划模块的最底层，它负责根据当前配置和目标配置生成一序列的动作，我们前面讨论的三次样条插值实际上只是一个简单的路径，而非我们最终能够执行的轨迹，本文介绍一种基于Frenet坐标系的优化轨迹动作规划方法，该方法在高速情况下的高级车道保持和无人驾驶都具有很强的实用性，是目前普遍采用的一种动作规划算法。

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

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

$$T = t_{end} - t_{start}$$

为什么使用Frenet坐标系

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

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

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

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

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

由于我们将轨迹优化问题分割成了 $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$$

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

$$p(t) =\alpha_0 + \alpha_1t + \alpha_2t^2 + \alpha_3t^3 + \alpha_4t^4 + \alpha_5t^5$$

要解这个方程组需要一些初始配置和目标配置，以横向路径规划为例，初始配置为 $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$$
$$\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$$
$$\ddot{d}(t_0) =2\alpha_{d2} + 6\alpha_{d3}t_0 + 12\alpha_{d4}t_0^2 + 20\alpha_{d5}t_0^3$$

为了区分横向和纵向，我们使用 $\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$$
$$\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$$
$$\ddot{d}(t_1) =2\alpha_{d2} + 6\alpha_{d3}t_1 + 12\alpha_{d4}t_1^2 + 20\alpha_{d5}t_1^3$$

我们通过令 $t_0 = 0$来简化这个六元方程组的求解，可直接求得 $\alpha_{d0}$ ，$\alpha_{d1}$ 和 $\alpha_{d2}$ 为：

$$\alpha_{d0} = d(t_0)$$
$$\alpha_{d1} = \dot{d}(t_0)$$
$$\alpha_{d2} = \frac{\ddot{d}(t_0)}{2}$$

令 $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}$$

该方程的解可以通过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]$$

对于优化问题而言，我们实际上希望车辆最终沿着参考线（道路中心线）平行的方向行驶，所以我们令 $\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$$

该损失函数包含三个惩罚项：
* $k_jJ_t(d(t))$ ：惩罚Jerk大的备选轨迹；
* $k_tT$ ：制动应当迅速，时间短；
* $k_dd_1^2$ ：目标状态不应偏离道路中心线太远

其中 $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$$

其中 $\dot{s_c}$ 是我们想要保持的纵向速度，第三个惩罚项的引入实际上是为了让目标配置中的纵向速度尽可能接近设定速度，该情景下的目标配置集为：

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

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

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

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

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

事故避免（Collision Avoiding）

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

• s方向上的速度是否超过设定的最大限速
• s方向的加速度是否超过设定的最大加速度
• 轨迹的曲率是否超过最大曲率

• 轨迹是否会引起碰撞（事故）

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

  基于Frenet优化轨迹的无人车动作规划实例

  由于planner的代码篇幅过长，本实例完整代码请见文末链接，在此仅讲解算法核心代码内容。和之前一样，我们仍然使用Python来实现该动作规划算法。

  首先，我们生成要追踪的参考线以及静态障碍物，参考线的生成只要使用了我们上一节提到的立方样条插值，代码如下：

# 路线
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)

生成如下参考路径以及障碍物：

其中红线就是我们的全局路径，蓝点为障碍物。定义一些参数：

# 参数
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

使用基于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

其中，一个重要的类是五次多项式类，其定义如下：

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

这里的五次多项式的系数的求解过程和我们前面的理论讲解是一样的，只不过我们使用Numpy中的 np.linalg.solve(A, b) 方法将矩阵解了出来。最后，我们来看一下障碍物规避是如何实现的：

def check_collision(fp, ob):
    for i in range(len(ob[:, 0])):
        d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)
             for (ix, iy) in zip(fp.x, fp.y)]

        collision = any([di <= ROBOT_RADIUS ** 2 for di in d])

        if collision:
            return False

    return True

由于我们将障碍物规避问题都简化为静态了，所以在这里我们只简单地计算了所有规划点到障碍物的距离，一句距离预计是否会发生碰撞，来看看完整的优化轨迹检查函数：

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的优化轨迹来做无人车的动作规划，通常能够找到有限集的最优解，当最优解无法通过检查是，自会采用“次优解”甚至更加“次优的”解。

最后我们来看一下完整的动作规划效果：

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