基础知识概述
- 规划本质是一个搜索问题（寻找目标函数），比如给一个函数，找一个最优解。
- 无人车的搜索问题是给一个当前状态，求接下去运动的最优解。在机器学习中可以被定义为一个寻找映射的过程。
- 目标函数定义无人车运动的优良性。
- 运动规划分为三个领域
（1）robotic fields：RRT、A*、D*lite、Lattice Planning
（2）控制理论：MPC、LQR
（3）AI：reinforcement learning（通过搜索算法找出一个运动可以优化运动功能）、end to end imitation learning（直接寻找方式） - 把运动规划问题转化为路径寻找问题，简化问题。
- 运动规划难点：
（1）处理动态障碍物
（2）融入交通规则
（3）实时计算（面对突发情况）
（4）保证无人车按照规划路径行驶（使用一个动态模型） - 运动规划的两部分信息：
（1）动态信息。包括感知模块获得的信息和从定位模块获得的信息。
（2）静态信息。主要通过高精地图获得。 - 车辆状态、交通灯信息、障碍物以及障碍物预测的轨迹、路由、高精度地图都是规划模块所获得的信息。轨迹不仅仅是要怎么走，而是随着时间这条路应该怎么走。要知道速度才知道随时间位置在哪里，要保证速度、路径规划都是平滑的。
- non-information search——A*——D*——加入运动学模型——加入动力学模型——得出一个trajectory（技术轨道，一串点怎么随着时间运动）
路由选择
BFS广度优先搜索、DFS深度优先搜索，这两种搜索算法都是无信息搜索，我们不知道目的地与出发点之间的信息。
A算法、D算法
无人车只能部分观察环境，不能看到较远处的障碍物。A要求全知环境，对目的地和出发点的信息了解全面，在导航运用较多。无人车的这种部分已知特性适合运用增量搜索（incremental search）D。可以通过当前的观察得到的障碍对原先全局的路由规划做一个修正，但很难达到全局最优解。
二次规划技术使之更平滑
运动规划要求安全平稳到达终点，是三维空间中的规划问题。XY表示位置，还有时间维度。还需考虑车的转向、转向角、速度、加速度，三维空间还需提升。
规划方法
-
A* D*还是不太能处理无人车要求的技术，因为这种规划从质点模型出发，即把运动轨迹当成一个点的运动轨迹，无人车是一个有体积的物体，可能会发生碰撞。
configuration space（构形空间）对无人车驾驶影响较大。
geometric complexity（几何复杂度）研究边界框和边界框之间怎么样相交，一个多边形和一个多边形之间怎样探测出一条路径 -
道路限制：
（1）静态/动态障碍物
（2）方向盘转动曲率（车本身的物理限制）
（3）最小长度。规划不是只关注最小路径长度，还要捕获它的平滑性 -
如何判断一条线是不是平滑的：看这条曲线的曲率是否连续、曲率是否可导、一阶/二阶导数是否连续
-
当解决连续空间的运动规划问题时，可以先离散化简化问题。把空间离散化成网格。
-
如何离散化空间：
（1）roadmap：把空间能走的离散成一些点和连接点的路径。
i. visibility graph已知环境后把障碍物多边形化，地点标记点，可推知一点到一点的最短路径一定是贴着障碍物边走的。——>优化的最优解可能不是在中间，很可能贴着边界走，必须要满足边界。
（2）cell decomposition（网格解构）也是将空间离散化
（3）potential field 直接处理 -
PRM一种抽象连续空间的方法，空间随机撒点，在障碍物上的点被抹去，剩下的点都是可行进的点。
RRT
- 是PRM的一种增量搜索方法。
- 通过部分观察的环境上撒点，选择最近点，在最近点再散播点（若遇到障碍物，不能连接，则重新散播），找到最近，以此类推。
- 问题：没有曲率，曲线不平滑。解决方法：增量搜索是有方向的，可以用平滑的线去代替折线连接；仍有局限性，随机散播点会使得运动曲线弯弯绕绕，不符合常规驾驶路线。
Lattice
- 原始Lattice方案指数加点，计算难度大。改进后，先撒点再连线，点量减少，避免重复计算，简化运算。
- 动态规划本质是解动力系统，把之前做过的东西记录下来。
Lattice in Frenet Frame
- SL坐标系中撒点得到参考路径，选择最优解，时间维度上也要离散化。
- 时间维度使得动态规划变得困难，因为不同的速度使得可能出现的动态障碍物不同
function optimization
- 抽象曲线变成一个函数，优化函数。
- 二次规划以非常快的速度找到最优解，更进一步讲是凸优化空间，凸空间里有唯一的最优解。
- 无人车问题不是一个凸问题，要使之变成一个凸问题。
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
- 在规则道路上考虑SL坐标系
- 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坐标都是多项式