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

文档维护：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 numpy as np
import copy
import math

from typing import List, Type, Optional, Tuple

from nuplan.planning.simulation.planner.project2.frame_transform import cartesian2frenet, local2global_vector
from nuplan.planning.simulation.planner.project2.merge_path_speed import transform_path_planning

from nuplan.planning.simulation.planner.project2.reference_line_provider import ReferenceLineProvider
from nuplan.common.actor_state.tracked_objects import TrackedObject, TrackedObjects
from nuplan.common.actor_state.state_representation import TimePoint
from nuplan.common.actor_state.ego_state import EgoState


MAX_SPEED = 17.0  # maximum speed [m/s]
MAX_ACCEL = 5.0  # maximum acceleration [m/ss]
MAX_KAPPA = 1.0  # maximum curvature [1/m]
D_ROAD_W = 0.3  # road width sampling length [m]
DT = 0.25  # time tick [s]
TARGET_SPEED = 3  # target speed [m/s]
D_T_S = 1.0  # target speed sampling length [m/s]
N_SAMPLE = 2  # 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

WEIGHT_PROGRESS = 1.0
WEIGHT_OFFSET = 5.0
WEIGHT_SMOOTH = 10.0

MAXIMUM_JERK = 1.5
MAXIMUM_PROGRESS = 120
MAXIMUM_OFFSET = 1.5

MAXIMUM_DECELERATION = 5.0
MAXIMUM_ACCELERATION = 5.0


class FrenetPath:
    # 定义轨迹类
    def __init__(self):
        self.t = []
        self.l = []
        self.l_dot = []
        self.l_ddot = []
        self.l_dddot = []
        self.s = []
        self.s_dot = []
        self.s_ddot = []
        self.s_dddot = []
        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:
    # x(t) = k0 + k1*t + k2*t^2 + k3*t^3 + k4*t^4 + k5*t^5
    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
        self.k_0, self.k_1, self.k_2 = x_0, dx_0, 0.5 * ddx_0
        A = np.array(
            [
                [t_1**3, t_1**4, t_1**5],
                [3 * t_1**2, 4 * t_1**3, 5 * t_1**4],
                [6 * t_1, 12 * t_1**2, 20 * t_1**3],
            ]
        )
        b = np.array([x_1 - self.k_1, dx_0, ddx_0, x_1, dx_1, ddx_1])
        b = np.array(
            [
                x_1 - self.k_0 - self.k_1 * t_1 - self.k_2 * t_1**2,
                dx_1 - self.k_1 - 2 * self.k_2 * t_1,
                ddx_1 - 2 * self.k_2,
            ]
        )
        self.time = t_1
        self.K = np.linalg.solve(A, b)
        self.k_3, self.k_4, self.k_5 = self.K

    def get_time(self):
        return self.time

    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:
    # x(t) = k0 + k1*t + k2*t^2 + k3*t^3 + k4*t^4
    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
        self.k_0, self.k_1, self.k_2 = x_0, dx_0, 0.5 * ddx_0
        A = np.array(
            [
                [3 * t_1**2, 4 * t_1**3],
                [6 * t_1, 12 * t_1**2],
            ]
        )
        b = np.array([dx_1 - self.k_1 - 2 * self.k_2 * t_1, ddx_1 - 2 * self.k_2])
        self.time = t_1
        self.K = np.linalg.solve(A, b)
        self.k_3, self.k_4 = self.K

    def get_time(self):
        return self.time

    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

    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

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

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


class LatticePlanner:
    def __init__(
        self,
        ego_state: EgoState,
        reference_path_provider: ReferenceLineProvider,
        object: List[TrackedObjects],
        horizon_time: TimePoint,
        sampling_time: TimePoint,
        max_velocity: float,
    ) -> None:
        self.ego_state = ego_state
        self.reference_path_provider = reference_path_provider
        self.objects = object
        self.horizon_time = horizon_time
        self.sampling_time = sampling_time
        self.max_velocity = max_velocity

        self.fplist = []
        self.best_path = None

    # 横向上：在 d，t 维度采样
    def sample_lateral_end_state(self, d_start_state):
        end_d_candidates = np.array([-0.1, 0.0, 0.1])
        end_t_candidates = np.array([8.0, 9.0, 10.0])

        sampled_states = []
        for t in end_t_candidates:
            for d in end_d_candidates:
                # 基于自车当前位置采样
                state = np.array([t, d, 0.0, 0.0])
                sampled_states.append(state)
        return sampled_states

    # 纵向上：在 v, t 维度采样
    def sample_lon_end_state(self, s_start_state):
        end_states = []
        time_samples = []
        # time_samples.append(0.01)
        for i in np.arange(8, 10, 0.5):
            time_samples.append(i)

        for time in time_samples:
            v_upper = min(s_start_state[2] + MAXIMUM_DECELERATION * 1.0, TARGET_SPEED)  # 间隔时间 = 1.0s
            v_lower = max(s_start_state[2] - MAXIMUM_ACCELERATION * 1.0, 0.0)
            end_states.append([time, v_upper, 0.0])
            end_states.append([time, v_lower, 0.0])
            v_range = v_upper - v_lower
            num_of_mid_points = int(min(4, v_range / 1.0))
            if num_of_mid_points > 0:
                velocity_seg = v_range / (num_of_mid_points + 1)
                for i in range(num_of_mid_points):
                    end_states.append([time, v_lower + velocity_seg * i, 0.0])

        return end_states

    def get_trajectory_cost(self, lon_trajectory: QuarticPolynomial, lat_trajectory: QuinticPolynomial) -> float:
        cost = 0.0
        # 1. longitudinal progress cost
        progress = lon_trajectory.calc_points(lon_trajectory.get_time())
        if progress < MAXIMUM_PROGRESS:
            cost += WEIGHT_PROGRESS * (MAXIMUM_PROGRESS - progress) / MAXIMUM_PROGRESS

        for t in np.arange(0.0, self.horizon_time.time_s, self.sampling_time.time_s):
            # 2. lateral smooth cost
            lateral_jerk = lat_trajectory.calc_third_derivative(t)
            if lateral_jerk > 1.0:
                cost += WEIGHT_SMOOTH * (lateral_jerk / MAXIMUM_JERK)

            # 3. offset cost
            lateral_offset = lat_trajectory.calc_points(t)
            if lateral_offset > 0.5:
                cost += WEIGHT_OFFSET * (lateral_offset - 0.5) / MAXIMUM_OFFSET

            # 4. TODO(wanghao): collision cost
        return cost

    def is_valid_lon_trajectory(self, lon_trajectory: QuarticPolynomial) -> bool:
        t = 0.0
        while t < lon_trajectory.get_time():
            velocity = lon_trajectory.calc_first_derivative(t)
            accleration = lon_trajectory.calc_second_derivative(t)
            if velocity > 10.0 and velocity < 0.0:
                return False

            if accleration > 5.0 and accleration < -5.0:
                return False

            t += 0.1
        return True

    # 多项式轨迹生成
    def calc_frenet_paths(self) -> list:
        cos_h = math.cos(self.ego_state.car_footprint.oriented_box.center.heading)
        sin_h = math.sin(self.ego_state.car_footprint.oriented_box.center.heading)

        init_cartesian_state = np.array(
            [
                self.ego_state.car_footprint.oriented_box.center.x,
                self.ego_state.car_footprint.oriented_box.center.y,
                self.ego_state.dynamic_car_state.rear_axle_velocity_2d.magnitude() * cos_h,
                self.ego_state.dynamic_car_state.rear_axle_velocity_2d.magnitude() * sin_h,
                self.ego_state.dynamic_car_state.rear_axle_acceleration_2d.magnitude() * cos_h,
                self.ego_state.dynamic_car_state.rear_axle_acceleration_2d.magnitude() * sin_h,
            ]
        )

        reference_line = np.array(
            [
                self.reference_path_provider._x_of_reference_line,
                self.reference_path_provider._y_of_reference_line,
                self.reference_path_provider._heading_of_reference_line,
                self.reference_path_provider._kappa_of_reference_line,
                self.reference_path_provider._s_of_reference_line,
            ]
        )

        (
            s_set,
            l_set,
            s_dot_set,
            l_dot_set,
            dl_set,
            l_dot2_set,
            s_dot2_set,
            ddl_set,
        ) = cartesian2frenet(
            [init_cartesian_state[0]],
            [init_cartesian_state[1]],
            [init_cartesian_state[2]],
            [init_cartesian_state[3]],
            [init_cartesian_state[4]],
            [init_cartesian_state[5]],
            reference_line[0],
            reference_line[1],
            reference_line[2],
            reference_line[3],
            reference_line[4],
        )

        d_start_state = [
            0,
            l_set[0],
            dl_set[0],
            ddl_set[0],
        ]
        s_start_state = [
            0,
            s_set[0],
            s_dot_set[0],
            s_dot2_set[0],
        ]
        d_end_states = self.sample_lateral_end_state(d_start_state)
        s_end_states = self.sample_lon_end_state(s_start_state)

        for d_end_state in d_end_states:
            for s_end_state in s_end_states:
                fp = FrenetPath()
                lat = QuinticPolynomial(d_start_state, d_end_state)
                lon = QuarticPolynomial(s_start_state, s_end_state)
                fp.t = [
                    t
                    for t in np.arange(
                        0.0, self.horizon_time.time_s + 2 * self.sampling_time.time_s, self.sampling_time.time_s
                    )
                ]
                fp.l = [lat.calc_points(t) for t in fp.t]
                fp.l_dot = [lat.calc_first_derivative(t) for t in fp.t]
                fp.l_ddot = [lat.calc_second_derivative(t) for t in fp.t]
                fp.l_dddot = [lat.calc_third_derivative(t) for t in fp.t]
                fp.s = [lon.calc_points(t) for t in fp.t]
                fp.s_dot = [lon.calc_first_derivative(t) for t in fp.t]
                fp.s_ddot = [lon.calc_second_derivative(t) for t in fp.t]
                fp.s_dddot = [lon.calc_third_derivative(t) for t in fp.t]

                fp.cost = self.get_trajectory_cost(lon_trajectory=lon, lat_trajectory=lat)

                self.fplist.append(fp)

        return self.fplist

    # 转换成全局坐标
    def calc_global_paths(self) -> list:
        for fp in self.fplist:
            # calc global positions
            fp.idx2s, fp.x, fp.y, fp.heading, fp.kappa = transform_path_planning(
                fp.s, fp.l, fp.l_dot, fp.l_ddot, self.reference_path_provider
            )

        return self.fplist

    # 约束检查
    def check_paths(self) -> list:
        ok_ind = []
        for i, _ in enumerate(self.fplist):
            if any([v > MAX_SPEED for v in self.fplist[i].s_dot]):
                continue
            elif any([abs(a) > MAX_ACCEL for a in self.fplist[i].s_ddot]):
                continue
            # elif any([abs(c) > MAX_KAPPA for c in self.fplist[i].kappa]):
            #     continue

            ok_ind.append(i)

        return [self.fplist[i] for i in ok_ind]

    def get_optimal_path(self) -> FrenetPath:
        # sample paths
        fplist = self.calc_frenet_paths()
        # transform to global frame
        fplist = self.calc_global_paths()
        # check constraints
        fplist = self.check_paths()
        # mini_cost path
        min_cost = float("inf")
        for fp in fplist:
            if min_cost >= fp.cost:
                min_cost = fp.cost
                self.best_path = fp

        return self.best_path

运行

conda activate nuplan
cd scripts
python run_planner.py

结果