【路径生成+轨迹优化--apollo无人驾驶规划器】Lattice planner算法

系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

---

前言

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！

在项目和平时的学习中，我对机器人/无人驾驶的决策规划模块进行了划分，当然划分的方法有很多，我的划分方式仅供参考
（1）动态障碍物行为预测模块（Behavior prediction）–结合感知和高精度地图信息，估计周围障碍物未来运动状态
（2）执行机构的轨迹规划模块（Trajectory_planning）–执行机构如机器人载体上的机械臂、串行云台等的运动轨迹规划
（3）任务决策模块（Mission_planning）–任务决策模块比较偏业务层了，处理机器人/无人驾驶的各种任务，主要分为三个方面：车底盘航线业务决策（交规、横向换道等等）、执行应用机构业务决策（机械臂、人机交互等等）、不同场景的导航方案切换决策（组合导航、融合导航）
（4）前端路径探索模块（path_finding）–全局路径规划算法难度不算复杂，找到一条可通行的（必须满足）、考虑动力学的（尽可能满足）、可以是稀疏的路径base_waypoints【由于其只考虑了环境几何信息，往往忽略了无人机本身的运动学与动力学模型。因此，其得到的轨迹往往显得比较“突兀”，并不适合直接作为无人机的控制指令】
（5）后端轨迹处理模块（motion_planning）–我主要归纳整理为三个方向：（1）对base_waypoints进行简单处理及生成方向、（2）对base_waypoints轨迹优化方向【一般是二次优化，这里用的较多的事优化方面的知识】、（3）进行对应功能的replan方向（replan之前的预处理、进入replan的条件、停障replan、避障replan、纠偏replan、换道replan、自动泊车replan、穿过狭窄道路replan等等），这部分内容使用的方法比较专
（6）路径跟踪模块（trajectory_following）–这个模块就得针对机器人载体了，如无人驾驶使用得阿克曼模型可以采用几何的pure pursuit纯追踪算法，更好的可以用模型预测控制MPC方法，还有强化学习做的（效果怎样我就没验证过了）；当然也可能事麦克纳姆轮车、差速车PID、还有无人机的三维轨迹跟踪等等
（7）碰撞检测模块
（8）集群多机器人规划模块

当然，这种划分方式是我权衡了原理和功能粗略划分的，在实际产品研发过程中，需要理解了各个算法的功能和定位的基础上融汇贯通，不能生搬硬套，如全段通过hybrid A探索出来的路径与A、RRT*探索出来的路径更平滑，后端轨迹优化的任务就不用这么重了；又如，机器人/无人驾驶项目研发的需求业务还没发展到能响应很多功能阶段，任务决策使用简单的状态机（fsm）就可以对现有任务进行状态转移了

本文先对Lattice planner算法做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

---

提示：以下是本篇文章正文内容

Lattice planner一些基本概念

Frenet坐标

Frenet坐标系描述了汽车相对于道路的位置，s代表沿道路的距离，也称为纵坐标，d表示与纵向线的位移，也被称为横坐标，纵坐标表示在道路中行驶距离，横坐标表示汽车偏离中心线的距离。
.
.

ST图

ST图可以帮助我们设计和选择速度曲线，“ｓ”表示车辆的纵向位移，“t”表示时间，ST图上的曲线是对车辆运动的描述，因为它说明了车辆在不同时间的位置，由于速度是位置变化的速率，，因此可以通过曲线的斜率从ST图上推断速度，斜坡越陡说明在短时间内有更大的移动对应更快的速度
.
.

路径规划–速度规划

在ST图内可以将障碍物绘制为在特定时间段内阻挡道路部分的矩形，速度曲线不得与矩形相交。
路径－速度解耦规划将轨迹规划分为两步：路径规划和速度规划。

路径规划：生成候选曲线，这是车辆可行驶的路径，使用代价函数对每条路径进行评估，该函数包括平滑度、安全性、与车道中心的偏离等等，按代价对路径进行排名，并选择代价最低的路径。

速度规划：确定沿这条路径行进的速度，可以使用优化功能为路径选择受到各种限制约束的良好速度曲线
.
.

ST轨迹的终止状态

根据情况可以将状态分为三组巡航、跟随与停止。
.
.

Lattice规划

通过Frebet坐标可将环境投射到纵轴和横轴上，目标是生成三维轨迹：纵向维度、横向维度和时间维度。整个三维问题可以分解成两个单独的二维问题，即通过分离轨迹的纵向和横向分量来解决，一个二维轨迹是指具有时间戳的纵向轨迹（ST轨迹），另一个二维轨迹相对于纵向轨迹的横向偏移（SL轨迹）。Lattice规划分为两步：先分别建立ST和SL轨迹再将它们合并。为生成纵向和横向的二维轨迹，先将初始车辆投射到ST和SL坐标系中，通过预选模式中的多个候选最终状态进行采样，选择最终车辆状态，对于每个车辆最终状态，须创建一组轨迹，将车辆从初始状态转换为最终状态，并通过代价函数选择代价最低的轨迹
.
.

Lattice轨迹生成

一旦同时拥有了ST和SL轨迹，就需要将它们重新转化为笛卡尔坐标，然后将它们相结合，构建由二维路径点和一维时间戳组成的三维轨迹，ST轨迹是随时间变化的纵向位移，SL纵向轨迹上每个点的横向偏移，两条轨迹都有纵坐标S，可以通过将S值进行匹配来合并轨迹
.
.

一、Lattice planner算法原理–【Search graph】在地图上如何构建一张图（graph）或树（tree）?

1.方法一：通过离散控制空间进行采样的方法【control lattice search】

【正向方法–以车当前控制状态为起点探索车可执行的路径】

（1）算法步骤

0）步骤0：栅格地图的4/8/16连接进行控制状态空间的离散【防盗标记–盒子君hzj】
【控制状态空间的方法】->【图搜索的方法】

1）步骤一：建立机器人的模型【模型用状态方程表示】

其中s是机器人的当前状态（s可以包含x,y及其速度），u是机器人的控制输入

2）步骤二：清楚机器人的初始状态s0

3）步骤三：通过选择多个不同的控制输入量u与合适的控制周期T，【防盗标记–盒子君hzj】可以正向模拟仿真出机器人多条下一步的路径（状态s），最后选择一条符合约束的，代价最低的一条路径
【核心思想：给定多个采样的控制量u【控制量是有限幅的】 -> 模拟规划多条路径后选择一条 -> 正解机器人最终状态】

（2）算法优点

这种规划方法比较容易实现，知道机器人的模型就可以正解出多条轨迹

（3）算法缺点

（1）这种规划方法的控制量输入是随机的，没有到达目标点任务导向性的，【防盗标记–盒子君hzj】发散性的去搜索的，因此规划的效率较低
（2）这种规划方法的规划出来的路径比较集群，不能很好的覆盖到实际的场景，容易出现一条路径不可行是其他候选的路径也不可行的情况

（4）无人车的离散控制空间lattice方法举例

基本思路：
先建立车底盘的动力学模型，再离散化车底盘的控制量(给不同方向的方向盘打角和油门大小)，从而得到车的不同行驶轨迹

无人车的状态假设为：

无人车的控制输入假设为：

得到的无人车的状态方程为：

给定无人机的初始速度为（直往前走）：

通过离散化速度输入，得到再控制周期T的无人机运动轨迹

lattice graph生成图的方法
（1）若某一个时刻对控制量进行9次采样，得到模拟的九条路径（如图所示）
1）非启发式探索的路径

2）启发式探索的路径

（2）若某一个时刻对控制量进行25次采样，得到模拟的25条路径（如图所示）

（5）无人机的离散控制空间lattice方法举例

无人机的状态假设为：

无人机的控制输入假设为：

得到的无人机的状态方程为：

其中
给定无人机的初始速度为：

通过离散化加速度输入，得到再控制周期T的无人机运动轨迹如下：

无人机的运动轨迹可以通过状态转移方程计算得到：（状态转移方程包括了零输入响应和零状态响应两个部分）

2.方法二：通过离散状态空间进行采样的方法【state lattice search】

【反向方法–以环境当前空间状态为起点探索车可执行的路径】

（1）算法步骤

（0）步骤0：PRM随机均匀采样进行状态空间的离散【防盗标记–盒子君hzj】
离散状态空间采样的方法当然也可以不使用PRM，举个例子用来在状态空间采样而已

1）步骤一：建立机器人的模型【模型用状态方程表示】

其中s是机器人的当前状态（s可以包含x,y及其速度），u是机器人的控制输入

2）步骤二：清楚机器人的初始状态s0

3）步骤三：通过先确定机器人目标的状态Sf，再找到一条连接起点和目标点的路径，进而确定控制量u与控制周期T
【核心思想：给定多个采样的机器人最终状态 -> 探索规划多条路径后选择一条 -> 反解控制量u】

3.1）lattice graph生成图的方法

3.2）给定起始状态和最终状态，反解出可行轨迹【较核心，当然反解的方法也不止下面几种】

方法一：Boundary Value Problem（BVP）边界值的方法
【本质是解一个高维的线性方程A=BX–待定系数法求解】【防盗标记–盒子君hzj】
（1）目的：给定起始状x(0)=a 和最终状态x(T)=b，反解出可行轨迹x(t)

（2）步骤
1）假设可行轨迹可以用5阶的多项式函数进行拟合【当然可以用其他函数拟合】

2）给定起始值和最终值的边界条件

3）通过待定系数法求解高维的线性方程B=AX【把C1~C5都解出来即可以得到可行轨迹的表达式】

---

方法二：Optimal Boundary Value Problem (OBVP)最优边界值方法
【本质是通过优化的方法进行轨迹反解–极小值原理（minimun principle）求解】【防盗标记–盒子君hzj】

（1）论文参考
Optimal Rough Terrain Trajectory Generation for Wheeled Mobile Robots, Thomas M. Howard Alonzo Kelly

（2）步骤
1）建模Modelling

2）通过优化的方法反解

其中：

---

3.3）给定可行轨迹，反解控制量u的方法
（1）纯跟踪算法的（pure_puresuit）
（2）MPC的方法

（2）算法优点

这种规划方法有比较好的目标点任务导向的，效率比较高

（3）算法缺点

这种规划方法需要根据起始点和目标点反解轨迹、【防盗标记–盒子君hzj】控制量u和控制周期T，相对控制空间control lattice search的方法来说没有那么容易实现。当然反解的方法也有很多

…
.
.
.

3.control lattice search和state lattice search两种算法的对比

放图就懂了~

论文参考
State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments, Thomas M. Howard, Colin J. Green, and Alonzo Kelly
.
.
.

二、Lattice planner算法原理–【Search tree】在图或树上进行路径搜索？

1.启发式函数的设计

可以借鉴A*等的图搜索的算法法框架来实现~

三、基于Frenet坐标系的lattice算法（lattice in Frenet Frame）

1.把世界坐标系下的xy坐标转换成车坐标系的sl坐标系

sl坐标系转xy坐标系是唯一的，xy坐标系转sl坐标系不是唯一的，但是有约束的时候可以是唯一的
xy坐标系转sl坐标系的转换过程相对复杂，推导不要去推，记住转换结果会用就好

2.Planning in Frenet-serret frame参考链接

https://pythonrobotics.readthedocs.io/en/latest/modules/path_planning.html#optimal-trajectory-in-a-frenet-frame

四、Lattice planner的改进方法

探索路径之前先撒点，再连线，这样可以有效减少参数，简化计算了

五、Lattice planner总结

（1）非结构化场景和局部规划的场景可以用一下
（2）时间和空间离散化后，参数呈指数增长

Lattice planner论文

Lattice planner的autoware视频

https://www.bilibili.com/video/BV1Fy4y187P4?spm_id_from=333.999.0.0