自动驾驶预测与决策规划（二）

文档维护：Arvin

网页部署：Arvin

▶

写在前面：本文内容是作者在深蓝学院自动驾驶预测与决策规划学习时的笔记，作者按照自己的理解进行了记录，如果有错误的地方还请执政。如涉侵权，请联系删除。

路径与轨迹规划（笔记）

基于搜索的路径规划

Hybrid A*算法

• 节点的拓展基于车辆运动学模型
• 代价的计算基于栅格栏

代价计算

代价包括节点遍历代价和两个启发函数，即$F(n)=g(n)+h_1(n)+h_2(n)$，其中

• $g(n)$主要考虑路径长度、运动学约束、方向变换成本;
• $h_1(n)$只考虑车辆的运动学约束而不考虑障碍物;
• $h_2(n)$只考虑障碍物信息而不考虑车辆的运动学约束;

基于采样的路径规划

Frenet坐标系

Werling M , Ziegler J , Kammel S , et al. Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame[C]// Robotics and Automation (ICRA), 2010 IEEE International Conference on. IEEE, 2010.

大多数道路行车场景中的道路走势是笔直的，但弯曲道路仍不少见。道路的弯曲形态多种多样，难以找到统一的曲线类型描述弯曲道路。车辆不应与道路边缘线相撞，这一类碰撞躲避约束的有效建模依赖对于道路布局的精细描述。如何统一简洁地描述道路边缘走势对于轨迹规划问题的求解效率有着重要影响。与非结构化泊车场景相比，结构化道路场景中存在指引线，车辆行驶轨迹不会大幅度偏离指引线，因此可以考虑以指引线的存在为契机简化规划任务的建模复杂度——构建Frenet坐标系可实现此理念。

Cartesian转换成Frenet

$$\begin{cases}s=&s_r\\s=&\frac{v_xcos(\theta_x-\theta_r)}{1-k_rl}\\\ddot{s}=&\frac{a_xcos(\theta_x-\theta_r)-\dot{s}^2\left[l^{\prime}\left(k_x\frac{1-k_rl}{cos(\theta_x-\theta_r)}-k_r\right)-(k_r^{\prime}l+k_rl^{\prime})\right]}{1-k_rl}\\l=&sign((y_x-y_r)cos(\theta_r)-(x_x-x_r)sin(\theta_r))\sqrt{(x_x-x_r)^2+(y_x-y_r)^2}\\l^{\prime\prime}=&(1-k_rl)tan(\theta_x-\theta_r)\\l^{\prime\prime}=&-({k_r}^{\prime}l+{k_r}l^{\prime})tan(\theta_x-\theta_r)+\frac{(1-k_rl)}{cos^2(\theta_x-\theta_r)}(\frac{1-k_rl}{cos(\theta_x-\theta_r)}k_x-k_r)\end{cases}$$

Frenet转换成Cartesian

$$\begin{cases}x_x&=&x_r-lsin(\theta_r)\\y_x&=&y_r+lcos(\theta_r)\\\theta_x&=&arctan(\frac{l^{\prime}}{1-k_rl})+\theta_r\in[-\pi,\pi]\\v_x&=&\sqrt{[s(1-k_rl)]^2+(sl^{\prime})^2}\\a_x&=&\ddot{s}\frac{1-k_rl}{cos(\theta_x-\theta_r)}+\frac{\dot{s}^2}{cos(\theta_x-\theta_r)}[l^{\prime}(k_x\frac{1-k_rl}{cos(\theta_x-\theta_r)}-k_r)-(k_r^{\prime}l+k_r^{\prime\prime})]\\k_x&=&((l^{\prime\prime}+(k_r^{\prime}l+k_rl^{\prime})tan(\theta_x-\theta_r))\frac{\cos^2(\theta_x-\theta_r)}{1-k_rl}+k_r)\frac{cos(\theta_x-\theta_r)}{1-k_rl}\end{cases}$$

推导过程：

贝尔曼最优性

上图分别是过拟合与欠拟合得轨迹规划，根据贝尔曼最优性原理:

对于每个$t=0,1,2,...$

$$V(t,k_{t})=max_{c_{t}}[f(t,k_{t},c_{t})+V(t+1,g(t,k_{t},c_{t}))]$$

在每个规划步骤中，都遵循先前计算出的轨迹的剩余部分，提供时间一致性。

最优控制：

问题定义为

给定$t_0$时刻的起始状态$P_{0}=[p_{0},\dot{p_{0}},\ddot{p_{0}}]$，和$t_1=t_0+T$时刻的终止状态$P_{1}=[p_{1},\dot{p_{1}},\ddot{p_{1}}]$

假设求解目标是，最小化jerk平方的积分：

$$DJ_t(p(t)):=\int_{t_0}^{t_1}\ddot{p}^2(\tau)d\tau$$

该最优控制问题的最优解是五次多项式。

定理：给定$t_0$时刻的起始状态$P_{0}=[p_{0},\dot{p_{0}},\ddot{p_{0}}]$，和$t_1=t_0+T$时刻的终止状态$P_1$中的$\dot{p_{1}},\ddot{p_{1}}$， 给定任意函数$g$和$h$，且$k_{j},k_{t},k_{p}>0$，最小化代价函数$C=k_{j}J_{t}+k_{t}g(T)+k_{p}h(p_{1})$ 的解是五次多项式问题。

预测器与规划器（作业）

代码地址

预测器

根据上一章，预测器可以分为定速度预测、定曲率预测等等。

定速度预测

def cv_trajectory(
            object: Agent, duration: float, sample_time: float
        ) -> PredictedTrajectory:
            x, y = object.center.array
            vx, vy = object.velocity._x, object.velocity._y
            waypoints = []
            for time in np.arange(sample_time, sample_time + duration, sample_time):
                future_time = object.metadata.timestamp_us + time * 1e6
                future_position = [x + vx * time, y + vy * time]
                waypoint = Waypoint(
                    time_point=TimePoint(future_time),
                    oriented_box=OrientedBox(
                        StateSE2(
                            future_position[0],
                            future_position[1],
                            object.center.heading,
                        ),
                        object.box.length,
                        object.box.width,
                        object.box.height,
                    ),  # 示例尺寸
                    velocity=None,
                )
                waypoints.append(waypoint)

            predicted_trajectory = PredictedTrajectory(
                probability=1.0, waypoints=waypoints
            )

            return predicted_trajectory

定曲率预测

def ct_trajectory(
            object: Agent, duration: float, sample_time: float
        ) -> PredictedTrajectory:
            x, y = object.center.array
            vx, vy = object.velocity._x, object.velocity._y
            speed = np.sqrt(vx**2 + vy**2)
            yaw, yaw_rate = object.center.heading, object.angular_velocity
            waypoints = []
            for time in np.arange(sample_time, sample_time + duration, sample_time):
                x += sample_time * speed * np.cos(yaw)
                y += sample_time * speed * np.sin(yaw)
                future_time = object.metadata.timestamp_us + time * 1e6
                future_position = [x, y]
                yaw += yaw_rate * sample_time
                waypoint = Waypoint(
                    time_point=TimePoint(future_time),
                    oriented_box=OrientedBox(
                        StateSE2(future_position[0], future_position[1], yaw),
                        object.box.length,
                        object.box.width,
                        object.box.height,
                    ),  # 示例尺寸
                    velocity=None,
                )
                waypoints.append(waypoint)

            predicted_trajectory = PredictedTrajectory(
                probability=1.0, waypoints=waypoints
            )

            return predicted_trajectory

规划器

基于采样的路径规划（解耦）

高速状态下横向运动相比于纵向运动相对幅度非常小，近似横纵向解耦，横纵向可独立计算

横向

初始状态：$D_0=[d_0,\dot{d_0},\ddot{d_0}]$

终止状态：$D_{1}=[d_{1},\dot{d}_{1}=0,\ddot{d}_{1}=0]$

采样时间：$T$

因此可以计算出横向轨迹的五次多项式：$D(t)=k_0+k_1*t+k_2*t^2+k_3*t^3+k_4*t^4+k_5*t^5$

求解过程：

$$\begin{gathered} d(t_0)=&k_0+k_1*t_{0}+k_2*t_{0}^2+k_3*t_{0}^3+k_4*t_{0}^4+k_5*t_{0}^5&=d_0 \\ d_{v}(t_{0})=&k_1+2k_2*t_{0}+3k_3*t_{0}^2+4k_4*t_{0}^3+5k_5*t_{0}^4&=\dot{d_0} \\ d_{a}(t_{0})=&2k_2+6k_3*t_{0}+12k_4*t_{0}^2+20k_5*t_{0}^3&=\ddot{d_0} \\ d(t_1)=&k_0+k_1*t_{1}+k_2*t_{1}^2+k_3*t_{1}^3+k_4*t_{1}^4+k_5*t_{1}^5&=d_1 \\ d_{v}(t_{1})=&k_1+2k_2*t_{1}+3k_3*t_{1}^2+4k_4*t_{1}^3+5k_5*t_{1}^4&=\dot{d}_{1} \\ d_a(t_1)=&2k_2+6k_3*t_{1}+12k_4*t_{1}^2+20k_5*t_{1}^3&=\ddot{d}_{1} \end{gathered}$$

代价函数：$C=k_{j}J_{t}+k_{t}g(T)+k_{p}h(p_{1})\to C_{d}=k_{j}J_{t}(d(t))+k_{t}T+k_{d}d_{1}^{2}$

纵向

初始状态：$S_0=[s_0,\dot{s_0},\ddot{s_0}]$

由不同的$\Delta s_{i}$和$T_j$生成终止轨迹集合：$[s_{1},\dot{s}_{1},\ddot{s}_{1},T]_{ij}=[[s_{\mathrm{target}}(T_{j})+\Delta s_{i}],\dot{s}_{\mathrm{target}}(T_{j}),\ddot{s}_{\mathrm{target}}(T_{j}),T_{j}]$

代价函数： $C_t=k_jJ_t+k_tT+k_s[s_1-s_d]^2$

总结

路径规划步骤如下：

• frenet坐标系下，初始状态已知，采样轨迹末状态，横纵向分别采用五次多项式进行曲线拟合，遍历横纵向曲线，将横纵向组合成候选轨迹簇。

• 转换成笛卡尔坐标系下的车身状态、路径。

• 检查约束和碰撞，纵向速度/加速度/加速度率(jerk)是否超出允许范围，曲率是否超出允许范围，横向加速度/加速度率(jerk)是否超出允许范围。

• 根据代价函数，选出最优路径。

import math
import copy
import logging
from typing import List, Type, Optional, Tuple

import sympy as sp
import numpy as np
import numpy.typing as npt

from nuplan.common.actor_state.oriented_box import in_collision
from nuplan.common.actor_state.state_representation import StateVector2D, TimePoint
from nuplan.common.actor_state.vehicle_parameters import get_pacifica_parameters
from nuplan.planning.simulation.controller.motion_model.kinematic_bicycle import (
    KinematicBicycleModel,
)
from nuplan.planning.simulation.observation.observation_type import (
    DetectionsTracks,
    Observation,
)
from nuplan.planning.simulation.planner.abstract_planner import (
    AbstractPlanner,
    PlannerInitialization,
    PlannerInput,
)
from nuplan.planning.simulation.trajectory.abstract_trajectory import AbstractTrajectory
from nuplan.common.maps.abstract_map import AbstractMap
from nuplan.planning.simulation.planner.project2.bfs_router import BFSRouter
from nuplan.planning.simulation.planner.project2.reference_line_provider import (
    ReferenceLineProvider,
)
from nuplan.planning.simulation.planner.project2.simple_predictor import SimplePredictor
from nuplan.planning.simulation.planner.project2.abstract_predictor import (
    AbstractPredictor,
)
from nuplan.planning.simulation.planner.project2.frame_transform import cartesian2frenet

from nuplan.planning.simulation.planner.project2.merge_path_speed import (
    transform_path_planning,
    cal_dynamic_state,
    cal_pose,
)
from nuplan.common.actor_state.ego_state import DynamicCarState, EgoState
from nuplan.planning.simulation.trajectory.interpolated_trajectory import (
    InterpolatedTrajectory,
)
from nuplan.common.actor_state.state_representation import StateSE2, StateVector2D
from nuplan.common.actor_state.agent import Agent
from nuplan.common.actor_state.tracked_objects import TrackedObject, TrackedObjects

logger = logging.getLogger(__name__)

MAX_SPEED = 50.0 / 3.6  # maximum speed [m/s]
MAX_ACCEL = 2.0  # maximum acceleration [m/ss]
MAX_KAPPA = 1.0  # maximum curvature [1/m]
MAX_ROAD_WIDTH = 7.0  # maximum road width [m]
D_ROAD_W = 1.0  # road width sampling length [m]
DT = 0.2  # time tick [s]
MAX_T = 5.0  # max prediction time [m]
MIN_T = 4.0  # min prediction time [m]
TARGET_SPEED = 30.0 / 3.6  # target speed [m/s]
D_T_S = 5.0 / 3.6  # target speed sampling length [m/s]
N_S_SAMPLE = 1  # sampling number of target speed

# cost weights
K_J = 0.1
K_T = 0.1
K_D = 1.0
K_LAT = 1.0
K_LON = 1.0


# 定义轨迹类
class FrenetPath:
    def __init__(self):
        self.t = []
        self.d = []
        self.d_d = []
        self.d_dd = []
        self.d_ddd = []
        self.s = []
        self.s_d = []
        self.s_dd = []
        self.s_ddd = []
        self.d_cost = 0.0
        self.s_cost = 0.0
        self.cost = 0.0

        self.idx2s = []
        self.x = []
        self.y = []
        self.heading = []
        self.kappa = []


# 五次多项式轨迹
class QuinticPolynomial:
    def __init__(self, strat_state, end_state):
        t_0, x_0, dx_0, ddx_0 = strat_state
        t_1, x_1, dx_1, ddx_1 = end_state
        A = np.array(
            [
                [1, t_0, t_0**2, t_0**3, t_0**4, t_0**5],
                [0, 1, 2 * t_0, 3 * t_0**2, 4 * t_0**3, 5 * t_0**4],
                [0, 0, 2, 6 * t_0, 12 * t_0**2, 20 * t_0**3],
                [1, t_1, t_1**2, t_1**3, t_1**4, t_1**5],
                [0, 1, 2 * t_1, 3 * t_1**2, 4 * t_1**3, 5 * t_1**4],
                [0, 0, 2, 6 * t_1, 12 * t_1**2, 20 * t_1**3],
            ]
        )
        b = np.array([x_0, dx_0, ddx_0, x_1, dx_1, ddx_1])
        self.K = np.linalg.solve(A, b)
        self.k_0, self.k_1, self.k_2, self.k_3, self.k_4, self.k_5 = self.K

    def calc_points(self, t):
        return (
            self.k_0
            + self.k_1 * t
            + self.k_2 * t**2
            + self.k_3 * t**3
            + self.k_4 * t**4
            + self.k_5 * t**5
        )

    def calc_first_derivative(self, t):
        return (
            self.k_1
            + 2 * self.k_2 * t
            + 3 * self.k_3 * t**2
            + 4 * self.k_4 * t**3
            + 5 * self.k_5 * t**4
        )

    def calc_second_derivative(self, t):
        return (
            2 * self.k_2
            + 6 * self.k_3 * t
            + 12 * self.k_4 * t**2
            + 20 * self.k_5 * t**3
        )

    def calc_third_derivative(self, t):
        return 6 * self.k_3 + 24 * self.k_4 * t + 60 * self.k_5 * t**2


# 巡航模式（速度保持）下，轨迹可为4次多项式
class QuarticPolynomial:
    def __init__(self, strat_state, end_state):
        # calc coefficient of quartic polynomial

        t_0, x_0, dx_0, ddx_0 = strat_state
        t_1, dx_1, ddx_1 = end_state
        A = np.array(
            [
                [1, t_0, t_0**2, t_0**3, t_0**4],
                [0, 1, 2 * t_0, 3 * t_0**2, 4 * t_0**3],
                [0, 0, 2, 6 * t_0, 12 * t_0**2],
                [0, 1, 2 * t_1, 3 * t_1**2, 4 * t_1**3],
                [0, 0, 2, 6 * t_1, 12 * t_1**2],
            ]
        )
        b = np.array([x_0, dx_0, ddx_0, dx_1, ddx_1])
        self.K = np.linalg.solve(A, b)
        self.k_0, self.k_1, self.k_2, self.k_3, self.k_4 = self.K

    def calc_points(self, t):
        return self.k_0 + self.k_1 * t + self.k_2 * t**2 + self.k_3 * t**3

    def calc_first_derivative(self, t):
        return self.k_1 + 2 * self.k_2 * t + 3 * self.k_3 * t**2

    def calc_second_derivative(self, t):
        return 2 * self.k_2 + 6 * self.k_3 * t

    def calc_third_derivative(self, t):
        return 6 * self.k_3


# 多项式轨迹生成
def calc_frenet_paths(d_start_state: list, s_start_state: list) -> list:
    frenet_paths = []

    # 生成不同的横向距离目标
    for di in range(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):
        for Ti in np.arange(MIN_T, MAX_T, DT):
            fp = FrenetPath()

            d_end_state = [Ti, di, 0, 0]
            lat = QuinticPolynomial(d_start_state, d_end_state)
            fp.t = [t for t in np.arange(0.0, Ti, DT)]
            fp.d = [lat.calc_points(t) for t in fp.t]
            fp.d_d = [lat.calc_first_derivative(t) for t in fp.t]
            fp.d_dd = [lat.calc_second_derivative(t) for t in fp.t]
            fp.d_ddd = [lat.calc_third_derivative(t) for t in fp.t]

            # Longitudinal motion planning (Velocity keeping)
            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)
                s_end_state = [Ti, tv, 0]
                lon = QuarticPolynomial(s_start_state, s_end_state)

                tfp.s = [lon.calc_points(t) for t in tfp.t]
                tfp.s_d = [lon.calc_first_derivative(t) for t in tfp.t]
                tfp.s_dd = [lon.calc_second_derivative(t) for t in tfp.t]
                tfp.s_ddd = [lon.calc_third_derivative(t) for t in tfp.t]

                Jd = sum(np.power(tfp.d_ddd, 2))
                Js = sum(np.power(tfp.s_ddd, 2))

                ds = (TARGET_SPEED - tfp.s_d[-1]) ** 2

                tfp.d_cost = K_J * Jd + K_T * Ti + K_D * tfp.d[-1] ** 2
                tfp.s_cost = K_J * Js + K_T * Ti + K_D * ds
                tfp.cost = K_LAT * tfp.d_cost + K_LON * tfp.s_cost

                frenet_paths.append(tfp)

        return frenet_paths


# 转换成全局坐标
def calc_global_paths(
    fplist: FrenetPath, ref_line: ReferenceLineProvider
) -> FrenetPath:
    for fp in fplist:
        # calc global positions
        fp.idx2s, fp.x, fp.y, fp.heading, fp.kappa = transform_path_planning(
            fp.s, fp.d, fp.d_d, fp.d_dd, ref_line
        )

    return fplist


# 约束检查
def check_paths(fplist: FrenetPath) -> bool:
    ok_ind = []
    for i, _ in enumerate(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_KAPPA for c in fplist[i].kappa]):
            continue

        ok_ind.append(i)

    return [fplist[i] for i in ok_ind]


def path_planning(
    ego_state: EgoState, reference_path_provider: ReferenceLineProvider
) -> FrenetPath:
    (
        s_set,
        l_set,
        s_dot_set,
        l_dot_set,
        dl_set,
        l_dot2_set,
        s_dot2_set,
        ddl_set,
    ) = cartesian2frenet(
        [ego_state.car_footprint.center.x],
        [ego_state.car_footprint.center.y],
        [ego_state.dynamic_car_state.center_velocity_2d.x],
        [ego_state.dynamic_car_state.center_velocity_2d.y],
        [ego_state.dynamic_car_state.center_acceleration_2d.x],
        [ego_state.dynamic_car_state.center_acceleration_2d.y],
        reference_path_provider._x_of_reference_line,
        reference_path_provider._y_of_reference_line,
        reference_path_provider._heading_of_reference_line,
        reference_path_provider._kappa_of_reference_line,
        reference_path_provider._s_of_reference_line,
    )
    d_start_state = [
        ego_state.time_point.time_us / 1e6,
        l_set[0],
        l_dot_set[0],
        l_dot2_set[0],
    ]
    s_start_state = [
        ego_state.time_point.time_us / 1e6,
        s_set[0],
        s_dot_set[0],
        s_dot2_set[0],
    ]
    fplist = calc_frenet_paths(d_start_state, s_start_state)
    fplist = calc_global_paths(fplist, reference_path_provider)
    fplist = check_paths(fplist)

    # mini_cost path
    min_cost = float("inf")
    best_path = None
    for fp in fplist:
        if min_cost >= fp.cost:
            min_cost = fp.cost
            best_path = fp

    return best_path


class MyPlanner(AbstractPlanner):
    """
    Planner going straight.
    """

    def __init__(
        self,
        horizon_seconds: float,
        sampling_time: float,
        max_velocity: float = 5.0,
    ):
        """
        Constructor for SimplePlanner.
        :param horizon_seconds: [s] time horizon being run.
        :param sampling_time: [s] sampling timestep.
        :param max_velocity: [m/s] ego max velocity.
        """
        self.horizon_time = TimePoint(int(horizon_seconds * 1e6))
        self.sampling_time = TimePoint(int(sampling_time * 1e6))
        self.max_velocity = max_velocity

        self._router: Optional[BFSRouter] = None
        self._predictor: AbstractPredictor = None
        self._reference_path_provider: Optional[ReferenceLineProvider] = None
        self._routing_complete = False

    def initialize(self, initialization: PlannerInitialization) -> None:
        """Inherited, see superclass."""
        self._router = BFSRouter(initialization.map_api)
        self._router._initialize_route_plan(initialization.route_roadblock_ids)

    def name(self) -> str:
        """Inherited, see superclass."""
        return self.__class__.__name__

    def observation_type(self) -> Type[Observation]:
        """Inherited, see superclass."""
        return DetectionsTracks  # type: ignore

    def compute_planner_trajectory(
        self, current_input: PlannerInput
    ) -> AbstractTrajectory:
        """
        Implement a trajectory that goes straight.
        Inherited, see superclass.
        """

        # 1. Routing
        ego_state, observations = current_input.history.current_state
        if not self._routing_complete:
            self._router._initialize_ego_path(ego_state, self.max_velocity)
            self._routing_complete = True

        # 2. Generate reference line
        self._reference_path_provider = ReferenceLineProvider(self._router)
        self._reference_path_provider._reference_line_generate(ego_state)

        # 3. Objects prediction
        self._predictor = SimplePredictor(
            ego_state, observations, self.horizon_time.time_s, self.sampling_time.time_s
        )
        objects = self._predictor.predict()

        # 4. Planning
        trajectory: List[EgoState] = self.planning(
            ego_state,
            self._reference_path_provider,
            objects,
            self.horizon_time,
            self.sampling_time,
            self.max_velocity,
        )

        return InterpolatedTrajectory(trajectory)

    # TODO: 2. Please implement your own trajectory planning.
    def planning(
        self,
        ego_state: EgoState,
        reference_path_provider: ReferenceLineProvider,
        object: List[TrackedObjects],
        horizon_time: TimePoint,
        sampling_time: TimePoint,
        max_velocity: float,
    ) -> List[EgoState]:
        s
        trajectory = []

        # 1.Path planning
        optimal_path = path_planning(ego_state, reference_path_provider)

        (
            optimal_path_t,
            optimal_path_l,
            optimal_path_dl,
            optimal_path_ddl,
            optimal_path_s,
            optimal_path_ds,
            optimal_path_dds,
            optimal_path_idx2s,
            optimal_path_x,
            optimal_path_y,
            optimal_path_heading,
            optimal_path_kappa,
        ) = (
            optimal_path.t,
            optimal_path.d,
            optimal_path.d_d,
            optimal_path.d_dd,
            optimal_path.s,
            optimal_path.s_d,
            optimal_path.s_dd,
            optimal_path.idx2s,
            optimal_path.x,
            optimal_path.y,
            optimal_path.heading,
            optimal_path.kappa,
        )

        for iter in range(int(horizon_time.time_us / sampling_time.time_us)):
            relative_time = (iter + 1) * sampling_time.time_s
            # 根据relative_time 和 speed planning 计算 velocity accelerate （三次多项式）
            s, velocity, accelerate = cal_dynamic_state(
                relative_time,
                optimal_path_t,
                optimal_path_s,
                optimal_path_ds,
                optimal_path_dds,
            )
            # 根据当前时间下的s 和 路径规划结果 计算 x y heading kappa （线形插值）
            x, y, heading, _ = cal_pose(
                s,
                optimal_path_idx2s,
                optimal_path_x,
                optimal_path_y,
                optimal_path_heading,
                optimal_path_kappa,
            )

            state = EgoState.build_from_rear_axle(
                rear_axle_pose=StateSE2(x, y, heading),
                rear_axle_velocity_2d=StateVector2D(velocity, 0),
                rear_axle_acceleration_2d=StateVector2D(accelerate, 0),
                tire_steering_angle=heading,
                time_point=state.time_point + sampling_time,
                vehicle_parameters=state.car_footprint.vehicle_parameters,
                is_in_auto_mode=True,
                angular_vel=0,
                angular_accel=0,
            )

            trajectory.append(state)

        return trajectory

运行

conda activate nuplan
cd scripts
python run_planner.py

结果