Motion Planning简介（apollo）_apollo motion planning

目录

What is Motion Planning（决策规划）

如何搜索路径

 自动驾驶决策规划问题剖析

 下文概要​

Motion Planning Approaches

环境变化下的决策规划

百度apollo的开发

 数据驱动方法

---

侵删，取自百度apollo视频课程link。

What is Motion Planning（决策规划）

Motion Planning 本质是搜索问题。是基于当前自车的状态和目标函数找最优路径的过程。

对机器学习方法来说是寻找mapping的过程，state to action 的mapping，end to end。

搜索是寻找action去optimize objective function 的过程。

如何搜索路径

可以通过BFS（Dijkstra算法）， DFS进行搜索，不过效率不高，他们都是Non-informative search。

 A*为代表的方法使用了目标的信息来定义启发函数，属于informative search，效率会高些。

prioritize search目标的时候是通过目前的cost G(n)加上heuristic cost H(n)，就是value function F(n)。 同样对于RL，怎么设计和找到H(n)也是个关键点。

A*算法对于无人车的路径规划有多远？

对于无人车而言，周围是个Partially observed dynamics environment，这种情况下，A*是有局限性的，A*本身是个Global Optimization，要求对整个环境全知，部分了解就会出问题。因此A*现在只应用在Global routing。

对于Partially observed dynamics environment，可以用贪心算法，表现出来就是Incremental search（增量搜索）。

 无人车还要考虑车辆自己的运动模型，让平滑规划的轨迹，使其可以行驶。

 自动驾驶决策规划问题剖析

三维问题：车质点的global X Y坐标和Time，3D轨迹优化问题。

但是从车辆动力学和运动学的角度看维度可能要进一步提高，包括heading、转向角、速度、加速度，使得问题难度会进一步显著提升。

IMU是装在后轴中心，因为这个位置最稳，能方便积分出整车的位移和转角，尤其过隧道没有GPS信号时。

整车信息分为动态信息（含perception、localization、部分底盘信息）和静态信息（主要指HdMap，避免感知花过多时间用于环境理解，L4的决策规划需要100ms一个周期）

 

Navigation Info包括前面很长一段具体（例如400米）的路点信息和辅助信息。下图是规划模块的能获取的所有信息，并利用这些信息做规划模块的工作，即给无人车提供一条好的trajectory，trajectory包含两部分信息：path和speed profile。

 下文概要

motion planner的方向，但是要先基于对问题的理解，再用ML看能不能解决某些棘手的问题。

 

Motion Planning Approaches

本节主要参考论文是：A Review of Motion Planning Techniques for Automated Vehicles

点击link

 structured road 比  unstructured road上的motion planning要难，因为规则多。

各种曲线是我们生成path的building blocks。

前面提到的都是质点模型，但是实际在自动驾驶里面是有体积有姿态的，该如何建立车辆模型，怎么考虑相互碰撞问题。Geometric complexity比如bounding box和bounding box之间怎么看相交，polygon和polygon之间怎么相交。

先看看有什么样的处理方法。

trajectory有各种限制，curvature是转弯半径的倒数，表征方向盘的角度。

dubin path对于无人车来说是不可以的，因为curvature不是连续的。我们给的trajectory要高阶可导，不然意味着急加急减速，急转方向盘等行为。

连续高维度问题复杂，简化为低维度和离散化问题。

最优解往往要么贴着边线要么在中间。这是一种将configuration space离散化的方法。

 configuration space离散化的方法有这三类：

motion planning的一些方法：

 PRM：撒点然后用A*等方法连接

 RRT生成的path在节点处是没有曲率的，车就没法开。点是随机撒的。

MIT做了一个研究来用平滑的线sample，改进了RRT。但是dynamic obstacle怎么办？而且这样扭来扭去也不够好。而且道路是规则的，不能随机散布点，在structure road上没必要这样做。

 这样可以用lattice网格的方式避免歪歪扭扭的问题。而且XY坐标系需要将空间和时间分开离散，依然不利于避开动态障碍物。虽然下图中右边的图已经做到了longitudinal和lateral两个方向的离散化，但是没有temporal的离散化。没有的话就只能对付静态的。

但是规则性道路上撒点也应该是沿着路撒啊，所以应该用frenet frame坐标系，也就是s-l坐标系。

下图是从车辆的运动出发的，沿着道路的纵向运动和横向分解。离散化网格之后寻找到最优的path，将path和speed放在一起研究了，三维空间的优化问题。

将问题分成path和speed两个维度去优化，因为三维处理起来不好处理，就分成两个二维问题来处理，s-l和speed-t（相当于s-t）逐渐优化并不断迭代，直到最后有用但不是最优解。

everthing is a trade-off. every model is wrong, but some are useful.

因为模型在利用观察到的重要信息。

如果想进一步找到更好的解决方案。

能不能用functional的方法，即优化函数l = l(s)。

QP（二次规划）优化速度快，并且可以找到唯一的最优解，但是要求目标函数和search space都是凸的。

QP是牛顿法衍生出来的一种方法，收敛速度是doubl exponential（二次迭代收敛），非常快。

还可以考虑先离散化优化再平滑，用贝塞尔曲线平滑，但是三阶贝塞尔会出现曲线波动的情况。

在平滑的过程中可能撞到障碍物，比如U turn的时候。在平滑的时候还要考虑安全性，插值也会出现龙格现象，点和点之间的误差大。

环境变化下的决策规划

质点到刚体，

 

 后轮转向半径和前轮转向半径不等。

 

X Y坐标系中的一个点可能对应多个s-l坐标系下的点，但是l的绝对值小于在转弯半径内是够用的。 但是U turn的时候要将路旁的点往多个方向映射，因为各个方向都有可能。

 bounding box方框（凸的）。超平米分割定理，如果两个bounding box不想交，那一定存在一条线可以将他们分割开。

 高阶的多项式龙格现象太明显了。

 怎么保证足够平滑？贝塞尔曲线平滑，但是二阶导会波动。

 

 上述方法没有X Y坐标系的旋转不变性。

螺旋曲线本来是在极坐标中定义的。螺旋线更接近开车转方向盘的平滑。

 牛顿法很快。QP更快。

 先用DP撒点得到大概的了解，然后分段用牛顿法或QP。

拉格朗日算子。现在求解基本上所有的QP问题基本上都是从KKT condition出发。

对于有些不符合二次型的问题，但是问题是凸的，可以用局部牛顿法逼近。无人车问题本身是非线性优化问题。这样可以两步走： 

二次规划参考的两本书： Convex optimization  和 Numerical optimization

百度apollo的开发

以下主要介绍百度apollo的planning，前面学了：机器人planning问题中如何和环境进行交互，例如bounding box和bounding box怎样算相交，这是如何处理constraint；学习的平滑性等各方面指标可以用于objective function如何设置；动态规划和二次规划作为优化算法的核心，是解决solver的问题。

实例1：无人车怎样抽象constraint，可以从三个方面去考虑

 reference line decider决定哪条lane更好。中间并行的是通过path-speed iteration实现。

EM = Expectation Maximization,期望最大化。思想是将高维度的问题分解为多个低维度问题，然后迭代优化。

在自动驾驶上的体现是先生成一条optimal path，然后将所有障碍物投影，投影后再生成一条optimal speed profile，然后将speed profile又拿去优化path，不断迭代。

迭代算法本质上是local optimal，是贪心算法，但是够不够用取决于问题本身。

 各种车辆都是有概率分布的，这种EM的思想反映在自动驾驶上是四步走战略，先E step（Expectation ） 和M step（Maximization）

先用DP得到粗略的解

然后用QP优化，用smoothing spline，正好是二次型。QP虽然不能解决所有问题，但是能在他能解决的范围内做到最好。QP得到的多帧最优解之间的差距很小。 

 

rule-based →optimization→Data Driven

 

 imitation的时候不能证明李雅普诺夫稳定性，会有蝴蝶效应。

 数据驱动方法

伯克利的课程，需要有马尔科夫随机过程的基础。

课程包括