规划
规划简介
构建车辆轨迹需要:
- 高精度地图
- 定位
- 预测
规划的步骤
- 路线导航:输入地图数据,输出可行驶路径
路线规划模块:
- 轨迹规划:由一系列点定义,每个点都有一个关联速度和一个指示何时应抵达那个点的时间戳
- 在构建了高水平路线之后,放大至轨迹规划,可帮助避开障碍物,做出微妙的决策
考虑规划的两个背景:
- 目的地
- 运动轨迹水平
路由
规划的三个输入:
- 地图
- 当前位置
- 目的地
世界到图
地图数据被格式化成为“图形”
图形
- 节点:路段/车道段
- 边缘:路段之间的连接
转换成图形的好处:计算机领域中已经有很多在图形中查找路径的快速算法
可以对一个节点运动到另一个节点的成本进行建模
网格世界
A*:一种经典的路径查找处理算法
- 将地图网格化,每一个单元格代表一个节点
- 从一个地方到另一个地方即从一个节点到另一个节点
步骤:
- 确定初始节点和最终节点
- 将初始节点的相邻的8个节点当作候选节点,计算初始节点到候选节点的成本(g值)
- 计算候选节点到目的节点的估计成本/启发式成本(h值)
- (成本估算方式可自定义)
- 每个候选节点的g值加上h值构成总成本f值,最佳候选节点是f值最小的节点
- 到达选定的候选节点后重复上述操作
- 每次选择节点都选择f值最小且尚未访问过的节点
Ap4
路由到轨迹
路由——高等级地图路线,提供总的行进方向
轨迹生成——生成低等级路线,更高精确度,考虑了路线中可能碰到的障碍物问题
3D轨迹
轨迹生成的目的:生成由一系列路径点所定义的轨迹,每个路径点都分配了一个时间戳和速度,让一条曲线与这些路径点拟合,生成轨迹的几何表征。
时间戳与预测模块的输出结合,确保车辆在通过时,路径上的每个点都未被占用。
评估一条轨迹
要求:
- 无障碍物
- 路径点之间的过渡和速度的变化必须平滑
- 路径点对车辆应该实际可行
- 轨迹应合法
成本函数:
在不同的环境可能使用不同的成本函数
Frenet坐标
笛卡尔坐标系的缺点:如果不知道道路在哪,就很难确定车辆已经行驶了多远,也南意确定它是否偏离车道中心
Frenet坐标系描述了汽车相对于道路的位置
- 横坐标d:与纵向线的位移,表示车辆偏移道路中心线的距离
- 纵坐标s:表示沿道路的距离(车道中心线),表示车辆行驶距离
- 在道路的每个点,横轴与纵轴都是垂直的。
路径-速度解耦规划
将轨迹规划分为两步:
- 路径规划:利用成本函数对候选路径进行评估,考虑平滑度、安全性、与车道的偏离等
- 速度规划:与路径点相关的一系列速度,也称“速度曲线”
路径生成与选择
- 将路段分割成单元格
- 每个单元格取一个点并将点连接
- 重复取点连线过程得到多个候选路径
- 利用成本函数评估候选路径,选择成本最低的路径
ST图
用于选择与路径相关的速度曲线
- S:车辆的纵向位移
- T:时间
- 通过曲线的斜率判断速度
速度规划
构建最佳速度曲线
- 将ST图离散为多个单元格
- 单元格之间的速度有所变化但单个单元格里速度不变
- 障碍物被描绘成在一定时间间隔和路径区域内的矩形
- 生成的候选速度曲线需要绕过网格里的障碍物矩形框
- 利用优化引擎选择成本最低的速度曲线作为最终曲线
优化功能可考虑:法律限制、距离限制、汽车的物理限等
优化
路径选择将道路划分为单元格,速度选择将ST图划分为单元格,所以,路径-速度解耦规划很大程度上取决于离散化
二次规划技术:将离散化形成的不平滑曲线转化为平衡轨迹
- 将平滑的非线性曲线与分段式线性段拟合(使用优化包)
- 转换好的速度曲线和路径曲线可以用来构建3D轨迹
Lattice规划
目标:生成三维轨迹—纵向维度、横向维度、时间维度
ST轨迹:纵向维度和时间
SL轨迹:相对于纵向轨迹的横向偏移
Lattice规划的两个步骤:
- 分别建立ST轨迹和SL轨迹
- 合并轨迹
建立步骤:
- 将初始车辆状态投射到SL和ST坐标系中
- 对预选模式中的多个候选最终状态进行采样
- 对每一个候选最终状态描绘轨迹
- 成本函数计算每个轨迹的成本,选择成本最低的路径,确定最终状态
ST轨迹的终止状态
纵轴表示速度,横轴表示时间
三种类型的终止状态:
- 巡航:车辆在完成规划步骤后定速行驶,图上意思指车辆在时间t以s点的速度巡航,最终状态的加速度为0
- 跟随:对位置和时间状态进行采样,目的是在时间t出现在某车辆后面,速度与加速度取决于跟随的车辆,会进行修正
- 停止:对汽车何时何地停下进行采样,速度和加速度被修正为0
SL轨迹的终止状态
前提假设:不管终止状态如何,车辆都该稳定地与车道中心线对齐,朝向也与中心线对齐
只需在一个小区域内对横向终止位置进行采样(采样相邻车道中心线周围的位置)
在SL坐标系中,曲线终止位置的的一阶导数(非横向运动)和二阶导数(非横向加速)都应该为0
Lattice规划的轨迹生成
将ST曲线和SL曲线转化回笛卡尔坐标系,并且通过相同S值合并构建由二维路径点和一维时间戳构成的三维轨迹。