智能驾驶入门6——规划

规划简介

构建车辆轨迹需要：

• 高精度地图
• 定位
• 预测

规划的步骤

• 路线导航：输入地图数据，输出可行驶路径

路线规划模块：

• 轨迹规划：由一系列点定义，每个点都有一个关联速度和一个指示何时应抵达那个点的时间戳
• 在构建了高水平路线之后，放大至轨迹规划，可帮助避开障碍物，做出微妙的决策

考虑规划的两个背景：

• 目的地
• 运动轨迹水平

路由

规划的三个输入：

• 地图
• 当前位置
• 目的地

世界到图

地图数据被格式化成为“图形”

图形

• 节点：路段/车道段
• 边缘：路段之间的连接

转换成图形的好处：计算机领域中已经有很多在图形中查找路径的快速算法

可以对一个节点运动到另一个节点的成本进行建模

网格世界

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值合并构建由二维路径点和一维时间戳构成的三维轨迹。

综述