最速轨迹规划算法

        在智驾行驶过程中，车辆遇到的很多场景都是“非凸”的，例如超车时可以选择从左边超车也可以选择从右边超车，但是不能直行超车。又比如存在并排车辆时的变道，可以加速超过后变道，也可以减速避让后变道，但是不能直接变道。在这些场景下，常用的处理方式先确定多个（所有）可行的行驶策略（将非凸空间划分成多个凸空间），然后对每个凸空间进行评估，选择最好的一个，交给后续的规控模块处理。这即是决策模块最重要的作用，不合理的决策或决策反复是造成不舒适体感的重要原因。

        比较凸空间的好坏，即比较凸空间中最好的行驶轨迹的好坏，所以一般的做法在每个凸空间中使用优化的方法得到一条最优轨迹，最后优中选优，得到最好的凸空间，即得到左右策略。使用优化方法获取最优轨迹虽然能保证轨迹质量，但是这个过程是一个很耗时的过程，难以在要求实时性的智驾系统评估多个策略。所以好的评价算法不仅要求准确全面，更要求低耗时。

        本文介绍一种简单的满足约束的轨迹生成算法，主要来源这篇论文[1]，以满足如下状态方程的运动学系统为例，控制量为j，状态量为p，v，a；且给定初始状态和目标状态，以及相关变量的约束；

        基本思想类似Bang-bang控制，控制量的取值为{umax，0，umin}组成的序列，时刻或以最大输入量将状态量拉到边界值，或以0输入将状态量保持在边界值，所以得到的轨迹为在满足约束的条件下时间最短的轨迹；

        算法过程很简单，轨迹几乎以解析的形式给出，计算量极小，非常适用于对计算资源要求非常严苛的决策模块。具体的算法步骤在论文中有详细描述，下面给出python代码以及运行结果；

C++版本以上传资源栏

#! /usr/bin/python3
import matplotlib.pyplot as plt
import math

class ThirdOrderState:
    def __init__(self, p=0, v=0, a=0, j=0):
        self.p = p
        self.v = v
        self.a = a
        self.j = j

class SecondOrderLimits:
    def __init__(self, a_max, a_min, j_max, j_min):
        self.a_max = a_max
        self.a_min = a_min
        self.j_max = j_max
        self.j_min = j_min

class SecondOrderParameters:
    def __init__(self, t=[0, 0, 0], j=[0, 0, 0]):
        self.t = t
        self.j = j

def KinematicModel(start_state, jerk, dt):
    p = (
        start_state.p
        + start_state.v * dt
        + start_state.a * dt * dt * 0.5
        + jerk * dt * dt * dt / 6.0
    )
    v = start_state.v + start_state.a * dt + jerk * dt * dt * 0.5
    a = start_state.a + jerk * dt
    j = jerk
    return ThirdOrderState(p, v, a, j)

class SecondOrderMinTimeCurve:
    def __init__(self, start_state, v_d, limits):
        self.start_state = start_state
        self.parameters = self.solve_velocity(start_state, v_d, limits)

    @staticmethod
    def solve_velocity(start_state, v_d, limits):
        kEpsilon = 1e-7

        a_max = limits.a_max
        a_min = limits.a_min
        j_max = max(limits.j_max, kEpsilon)
        j_min = min(limits.j_min, -kEpsilon)
        v0 = start_state.v
        a0 = start_state.a

        ve = 0
        if a0 >= 0:
            ve = v0 + a0 * math.fabs(a0 / j_min) / 2.0
        else:
            ve = v0 + a0 * math.fabs(a0 / j_max) / 2.0

        ac = 0
        da = 0
        if v_d - ve > kEpsilon:
            da = 1
            ac = a_max
        elif ve - v_d > kEpsilon:
            da = -1
            ac = a_min

        t1 = 0
        j1 = 0
        if ac - a0 >= 0:
            t1 = (ac - a0) / j_max
            j1 = j_max
        else:
            t1 = (ac - a0) / j_min
            j1 = j_min

        v1 = v0 + a0 * t1 + t1 * t1 * j1 / 2.0

        t3 = 0.0
        j3 = 0.0
        if -ac >= 0.0:
            t3 = -ac / j_max
            j3 = j_max
        else:
            t3 = -ac / j_min
            j3 = j_min

        v3 = ac * t3 + t3 * t3 * j3 / 2.0
        v2 = v_d - v1 - v3

        t2 = 0.0
        if da == 0:
            t2 = 0.0
        else:
            t2 = v2 / ac

        if t2 < 0.0:
            if da == 1:
                an = math.sqrt(
                    (2.0 * (v_d - v0) + a0 * a0 / j_max) / (1 / j_max - 1 / j_min)
                )
                t1 = (an - a0) / j_max
                t2 = 0.0
                t3 = -an / j_min
            elif da == -1:
                an = -math.sqrt(
                    (2.0 * (v_d - v0) + a0 * a0 / j_min) / (1 / j_min - 1 / j_max)
                )
                t1 = (an - a0) / j_min
                t2 = 0.0
                t3 = -an / j_max
        t = [t1, t1 + t2, t1 + t2 + t3]
        j = [j1, 0, j3]
        return SecondOrderParameters(t, j)

    @staticmethod
    def get_state(start_state, parameters, ts):
        if ts <= parameters.t[0]:
            return KinematicModel(start_state, parameters.j[0], ts)

        if ts <= parameters.t[1]:
            state1 = KinematicModel(start_state, parameters.j[0], parameters.t[0])
            return KinematicModel(state1, parameters.j[1], ts - parameters.t[0])

        state1 = KinematicModel(start_state, parameters.j[0], parameters.t[0])
        state2 = KinematicModel(
            state1, parameters.j[1], parameters.t[1] - parameters.t[0]
        )
        if ts < parameters.t[2]:
            return KinematicModel(state2, parameters.j[2], ts - parameters.t[1])

        state3 = KinematicModel(
            state2, parameters.j[2], parameters.t[2] - parameters.t[1]
        )
        state3.p += state3.v * (ts - parameters.t[2])
        state3.a = 0.0
        state3.j = 0.0
        return state3

class ThirdOrderLimits:
    def __init__(self, v_max, v_min, a_max, a_min, j_max, j_min):
        self.v_max = v_max
        self.v_min = v_min
        self.a_max = a_max
        self.a_min = a_min
        self.j_max = j_max
        self.j_min = j_min

class ThirdOrderParameters:
    def __init__(self, P_a, P_b, t_pb, v_c, t_c):
        self.P_a = P_a
        self.P_b = P_b
        self.t_pb = t_pb
        self.v_c = v_c
        self.t_c = t_c

class ThirdOrderMinTimeCurve:
    def __init__(self, start_state, p_f, v_f, limits):
        self.start_state = start_state
        self.parameters = self.solve_position(start_state, p_f, v_f, limits)

    @staticmethod
    def solve_position(start_state, p_f, v_f, limits):
        kMaxN = 100
        kEpsilon = 1e-3

        P = SecondOrderMinTimeCurve.solve_velocity(start_state, v_f, limits)
        state_sp = SecondOrderMinTimeCurve.get_state(start_state, P, P.t[2])

        d_p = 0
        if p_f - state_sp.p > kEpsilon:
            d_p = 1
        elif state_sp.p - p_f > kEpsilon:
            d_p = -1

        v_c = 0.0
        if d_p == 1:
            v_c = max(limits.v_max, kEpsilon)
        elif d_p == -1:
            v_c = min(limits.v_min, -kEpsilon)
        else:
            P_a = P
            P_b = SecondOrderParameters()
            t_pb = P.t3
            v_c = v_c
            t_c = 0.0
            outputs = ThirdOrderParameters(P_a, P_b, t_pb, v_c, t_c)
            return outputs

        P_a = SecondOrderMinTimeCurve.solve_velocity(start_state, v_c, limits)
        state_fa = SecondOrderMinTimeCurve.get_state(start_state, P_a, P_a.t[2])
        P_b = SecondOrderMinTimeCurve.solve_velocity(state_fa, v_f, limits)
        state_fb = SecondOrderMinTimeCurve.get_state(state_fa, P_b, P_b.t[2])

        p_fb = state_fb.p
        t_c = 0.0
        t_pb = 0.0
        if (p_fb - p_f) * d_p <= 0:
            t_c = (p_f - p_fb) / v_c
            t_pb = P_a.t[2]
        else:
            t_c = 0.0
            t_H = P_a.t[2]
            t_L = 0.0
            for i in range(kMaxN):
                t_pb = (t_H + t_L) / 2.0
                state_pb = SecondOrderMinTimeCurve.get_state(start_state, P_a, t_pb)
                P_b = SecondOrderMinTimeCurve.solve_velocity(state_pb, v_f, limits)
                state_fb = SecondOrderMinTimeCurve.get_state(state_pb, P_b, P_b.t[2])

                if (state_fb.p - p_f) * d_p < 0:
                    t_L = t_pb
                else:
                    t_H = t_pb

                if math.fabs(state_fb.p - p_f) < kEpsilon:
                    break

        return ThirdOrderParameters(P_a, P_b, t_pb, v_c, t_c)

    @staticmethod
    def get_state(start_state, parameters, t_s):
        kEpsilon = 1e-3

        state_pb = SecondOrderMinTimeCurve.get_state(
            start_state, parameters.P_a, parameters.t_pb
        )

        state = ThirdOrderState()
        if t_s < parameters.t_pb:
            state = SecondOrderMinTimeCurve.get_state(start_state, parameters.P_a, t_s)
        elif t_s >= parameters.t_pb and t_s < parameters.t_pb + parameters.t_c:
            cruise_state = ThirdOrderState()
            cruise_state.p = state_pb.p + parameters.v_c * (t_s - parameters.t_pb)
            cruise_state.v = parameters.v_c
            cruise_state.a = 0.0
            state = cruise_state
        else:
            cruise_state = ThirdOrderState()
            cruise_state.p = state_pb.p + parameters.v_c * parameters.t_c
            cruise_state.v = parameters.v_c
            cruise_state.a = 0.0
            if math.fabs(parameters.t_c) > kEpsilon:
                state = SecondOrderMinTimeCurve.get_state(
                    cruise_state,
                    parameters.P_b,
                    t_s - (parameters.t_c + parameters.t_pb),
                )
            else:
                state = SecondOrderMinTimeCurve.get_state(
                    state_pb, parameters.P_b, t_s - (parameters.t_c + parameters.t_pb)
                )
        return state

    def plot(self):
        N = 100
        t = 0
        max_time = self.parameters.t_pb + self.parameters.t_c + self.parameters.P_b.t[2]
        delta_t = max_time / N
        lst_t = []
        lst_j = []
        lst_a = []
        lst_v = []
        lst_p = []
        while t <= max_time:
            state = self.get_state(self.start_state, self.parameters, t)
            lst_j.append(state.j)
            lst_a.append(state.a)
            lst_v.append(state.v)
            lst_p.append(state.p)
            lst_t.append(t)

            t = t + delta_t

        plt.figure(figsize=(8, 8))
        plt.subplot(4, 1, 1)
        plt.plot(lst_t, lst_p)
        plt.ylabel("p")

        plt.subplot(4, 1, 2)
        plt.plot(lst_t, lst_v)
        plt.ylabel("v")

        plt.subplot(4, 1, 3)
        plt.plot(lst_t, lst_a)
        plt.ylabel("a")

        plt.subplot(4, 1, 4)
        plt.plot(lst_t, lst_j)
        plt.ylabel("j")

        plt.show()

def main():
    start_state = ThirdOrderState(0, 5, -1.5, 0)
    limits = ThirdOrderLimits(15, 0, 2, -2, 2, -2)
    third_curve = ThirdOrderMinTimeCurve(start_state, 150, 6, limits)
    third_curve.plot()

if __name__ == "__main__":
    main()

运行结果如下：

[1] Lai et al. Safe navigation of quadrotors with jerk limited trajectory. Front Inform Technol Electron Eng 2019 20(1):107-119