【笔记】自动驾驶预测与决策规划_Part3_路径与轨迹规划

0. 前言

本文主要记录课程《自动驾驶预测与决策技术》的学习过程，难免会有很多纰漏，感谢指正。
课程链接：https://www.shenlanxueyuan.com/my/course/700

• 基于搜索的路径规划

• 基于采样的路径规划

• 基于优化的路径规划

1. 基于搜索的路径规划

1.1 A* 算法

A*算法是一种用于路径规划和搜索的启发式算法，基于图搜索的思想，结合了深度优先搜索（DFS）和广度优先搜索（BFS）的优点，可以找到从起点到终点的最优路径。

基本原理

A*算法通过维护一个优先队列来存储当前待探索的节点，每次从中选择一个估计总代价最小的节点进行扩展。每个节点的总代价由两个部分组成：

• g(n): 从起点到当前节点n的实际代价。这是路径上已经发生的开销，也就是从起点出发，经过一系列已探索的节点，到达当前节点n所花费的总代价。g(n)通常根据具体问题中的距离、时间、能量消耗等进行计算。
• h(n): 从节点n到终点的估计代价（通常使用启发式函数计算）

A*算法的核心思想是通过选择最小的f(n)来扩展节点，其中：

$$f(n)=g(n)+h(n)$$
启发式函数

启发式函数h(n)是关键所在，它估计从当前节点到目标节点的代价。常用的启发式函数包括：

• 曼哈顿距离: $h(n)=|x_n-x_{goal}|+|y_n-y_{goal}|$ 适用于网格状地图。
• 欧几里得距离: $h(n)=\sqrt{(x_n-x_{goal}{)}^2+(y_n-y_{goal}{)}^2}$

启发式函数需要满足一致性或可接受性，即h(n)不会高估实际代价。

A*算法的优点在于通过启发式函数指导搜索，能够高效地找到最优路径。它在自动驾驶中的应用包括动态路径规划，避免障碍物，并实时更新路径以应对道路上的变化。

二维格点上进行 A* 算法 缺点：不满足车辆的运动学约束

1.2 Hybrid A* 算法

Hybrid含义：

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

Hybrid A*算法的关键特性

1. 连续空间搜索:
   • 传统A算法通常用于离散网格的搜索，例如二维网格上的路径规划。Hybrid A则在连续空间中进行搜索，允许路径规划的结果在更高的精度上表示车辆的位姿（位置和方向）。
   • 车辆的状态不仅仅包括其在网格上的位置，还包括其方向和速度等动态信息。
2. 车辆动力学约束:
   • Hybrid A*考虑车辆的运动学和动力学约束，例如最小转弯半径、最大转向角等。这些约束使得路径规划的结果不仅是可达的，还必须是车辆能够实际执行的。
   • 在搜索过程中，Hybrid A*会基于这些约束来生成平滑的轨迹，而不仅仅是直线或折线段。

Dubins 曲线： 经典的圆弧直线法

Reeds Shepp 曲线：可以倒车 泊车主流算法。随着无图的自动驾驶，需要基于搜索，Hybrid A* 可以适用该场景。

代价计算：

g(n): 路径长度、运动学约束、道路切合程度、方向突变、压实线、逆行等（过程代价，描述已知的代价）；

最短路径：

h1(n): 只考虑运动学约束，不考虑障碍物。

h2(n): 只考虑障碍物信息，通过格子去拓展，计算到终点的距离。基于经验，或者存图。

或者 leaning based huritical 。

伪代码流程：

2. 基于采样的路径规划

要求路径的一致性

2.1 Frenét Frame方法

缺点：很难约束极限道路场景下的曲率。（被参考线的曲率所欺骗）

Frenet坐标系是基于车辆的运动路径（通常称为参考路径）建立：

1. 切向坐标 $s$: 这是沿着参考路径的距离坐标。它表示在路径上某一点距离起点的弧长，也就是路径上的累计距离。
2. 横向坐标 $l$: 这是垂直于参考路径的距离坐标。它表示车辆当前位置偏离参考路径的横向距离，即从路径到车辆的最短距离。

2.2 Cartesian →Frenét 1D $(x,y)$ —> ( s , l ) (s, l) <