智能驾驶进阶5——规划技术

基础知识概述

1. 规划本质是一个搜索问题（寻找目标函数），比如给一个函数，找一个最优解。
2. 无人车的搜索问题是给一个当前状态，求接下去运动的最优解。在机器学习中可以被定义为一个寻找映射的过程。
3. 目标函数定义无人车运动的优良性。
4. 运动规划分为三个领域
   （1）robotic fields：RRT、A*、D*lite、Lattice Planning
   （2）控制理论：MPC、LQR
   （3）AI：reinforcement learning（通过搜索算法找出一个运动可以优化运动功能）、end to end imitation learning（直接寻找方式）
5. 把运动规划问题转化为路径寻找问题，简化问题。
6. 运动规划难点：
   （1）处理动态障碍物
   （2）融入交通规则
   （3）实时计算（面对突发情况）
   （4）保证无人车按照规划路径行驶（使用一个动态模型）
7. 运动规划的两部分信息：
   （1）动态信息。包括感知模块获得的信息和从定位模块获得的信息。
   （2）静态信息。主要通过高精地图获得。
8. 车辆状态、交通灯信息、障碍物以及障碍物预测的轨迹、路由、高精度地图都是规划模块所获得的信息。轨迹不仅仅是要怎么走，而是随着时间这条路应该怎么走。要知道速度才知道随时间位置在哪里，要保证速度、路径规划都是平滑的。
9. non-information search——A*——D*——加入运动学模型——加入动力学模型——得出一个trajectory（技术轨道，一串点怎么随着时间运动）

路由选择
BFS广度优先搜索、DFS深度优先搜索，这两种搜索算法都是无信息搜索，我们不知道目的地与出发点之间的信息。

A算法、D算法

无人车只能部分观察环境，不能看到较远处的障碍物。A要求全知环境，对目的地和出发点的信息了解全面，在导航运用较多。无人车的这种部分已知特性适合运用增量搜索（incremental search）D。可以通过当前的观察得到的障碍对原先全局的路由规划做一个修正，但很难达到全局最优解。
二次规划技术使之更平滑

运动规划要求安全平稳到达终点，是三维空间中的规划问题。XY表示位置，还有时间维度。还需考虑车的转向、转向角、速度、加速度，三维空间还需提升。

规划方法

1. A* D*还是不太能处理无人车要求的技术，因为这种规划从质点模型出发，即把运动轨迹当成一个点的运动轨迹，无人车是一个有体积的物体，可能会发生碰撞。
   configuration space（构形空间）对无人车驾驶影响较大。
   geometric complexity（几何复杂度）研究边界框和边界框之间怎么样相交，一个多边形和一个多边形之间怎样探测出一条路径

2. 道路限制：
   （1）静态/动态障碍物
   （2）方向盘转动曲率（车本身的物理限制）
   （3）最小长度。规划不是只关注最小路径长度，还要捕获它的平滑性

3. 如何判断一条线是不是平滑的：看这条曲线的曲率是否连续、曲率是否可导、一阶/二阶导数是否连续

4. 当解决连续空间的运动规划问题时，可以先离散化简化问题。把空间离散化成网格。

5. 如何离散化空间：
   （1）roadmap：把空间能走的离散成一些点和连接点的路径。
   i. visibility graph已知环境后把障碍物多边形化，地点标记点，可推知一点到一点的最短路径一定是贴着障碍物边走的。——>优化的最优解可能不是在中间，很可能贴着边界走，必须要满足边界。
   （2）cell decomposition（网格解构）也是将空间离散化
   （3）potential field 直接处理

6. PRM一种抽象连续空间的方法，空间随机撒点，在障碍物上的点被抹去，剩下的点都是可行进的点。

RRT

1. 是PRM的一种增量搜索方法。
2. 通过部分观察的环境上撒点，选择最近点，在最近点再散播点（若遇到障碍物，不能连接，则重新散播），找到最近，以此类推。
3. 问题：没有曲率，曲线不平滑。解决方法：增量搜索是有方向的，可以用平滑的线去代替折线连接；仍有局限性，随机散播点会使得运动曲线弯弯绕绕，不符合常规驾驶路线。

Lattice

1. 原始Lattice方案指数加点，计算难度大。改进后，先撒点再连线，点量减少，避免重复计算，简化运算。
2. 动态规划本质是解动力系统，把之前做过的东西记录下来。

Lattice in Frenet Frame

1. SL坐标系中撒点得到参考路径，选择最优解，时间维度上也要离散化。
2. 时间维度使得动态规划变得困难，因为不同的速度使得可能出现的动态障碍物不同

function optimization

1. 抽象曲线变成一个函数，优化函数。
2. 二次规划以非常快的速度找到最优解，更进一步讲是凸优化空间，凸空间里有唯一的最优解。
3. 无人车问题不是一个凸问题，要使之变成一个凸问题。

polynomial（多项式）：分成path和speed两个模块分别进行处理，即降维优化问题。
先优化再平滑

Lattice search通过规划方式把它降维成一个多项式。
搜索是在三维空间中的搜索，要考虑他的形状，构形空间里要包含他的朝向以及边界框等。车是刚体
优化框架里包含直接寻找优化还有一种path speed iterative，即降维解决子问题，用不断迭代的方式寻找最优解。
二次优化

运动规划关于环境的变化

要求理解：
（1）如何从质点模型到车辆模型
（2）理解汽车控制方面的需求
（3）如何生成平滑的曲线
（4）如何通过世界坐标系下的XY坐标变成道路上的SL曲线坐标
（5）曲线坐标系中障碍物的投影
（6）为什么要在规则道路下进行规划，规则道路带来的好处

汽车模型的建立

bicycle model

bicycle model的分析：
（1）前轮与后轮是沿着同一个转向中心转动的。
（2）bicycle model中转向半径的倒数是关于转向半径的一个函数，后轮的转向半径和前轮的转向半径是不一样的。车装差速计就是为了这个原因。

描述前轮和后轮差异的方式：

假设刚体的后轴中心按照平滑轨迹运动，存在一个对应的曲率，那么就可以知道转向的时候的角度。

如何避免障碍物

Frenet Frame

1. 在规则道路上考虑SL坐标系
2. XY<——SL
   

（1）先获得曲线坐标系。曲线坐标系上的每一个s都对应一个（x_r,y_r），即定位一个s点可知其xy坐标。要确认横向偏移后的坐标，还需要参考线本身的朝向（即切线方向），已知L是与S垂直的，所以可以在切点做一条垂直线，得到theta以及法向L方向平移的距离。
（2）如果运动轨迹一直和参考曲线坐标系s的方向平行（沿着车道线或者偏移固定量），则存在上图公式，实际上是基本不存在的。
（3）SL坐标对XY坐标是唯一对应的，但XY坐标对SL坐标不是唯一对应的（比如圆情况），当曲线曲率的最小值大于某一个值，该值倒数范围内都是唯一对应的。
（4）计算机记录曲线的时候是记录连续的点，保证了点与点之间很近。

障碍物问题
（1）如何判断两个障碍物之间有相交：对障碍物本身抽象为一个凸的polynomial，为简化计算用边界框研究相交问题。
（2）超平面分割定理，如果两个边界框不相交一定可以找到一条直线将两个边界框分开；如果不相交，一定能找到一条沿着某一条边的直线把两个边界框分割。
（3）先把几个障碍物用AABB框框住，投影到到一维。

平滑曲线：
（1）XY和曲率是有联系的，不能单独改变曲率使之平滑，要根据动力学模型得知其中的联系。
（2）先考虑导数空间里的平滑性

龙格现象：高阶的多项式不能用于平滑，抖动太大。
Bezier spline：一种传统的数据方案。先规划后平滑
如何设计：先把地图上的线抽象成控制点，生成的光滑曲线经过控制点时能经过box

曲率的波动现象是不好现象。
既要满足约束条件，又要足够平滑。
spline 2D
假设曲线被分为n截曲线段，满足每个曲线段x坐标、y坐标都是多项式