Understand More on the MP Difficulty

优化问题的三个方面：目标函数、约束条件和求解方法。
约束限制分为硬约束和软约束，例如交通规则属于硬约束。

EM规划框架

eg.如图
当前情况：在蓝线和红线交点处发现前方有车辆行驶缓慢，可能要进行换道处理。
EM规划框架：对换道和继续在本车道行驶分别规划出一条轨迹，只有换道之后的轨迹要比本车道的轨迹好的情况下才换道。

Reference Line Decider：决定哪个道比较好的模块，其中中间并行计算模块是由Path Speed Iterative 的方式实现的。

优化决策问题本身是一个 3D optimization 问题，其中包含了三个维度，需要生成 SLT，需要知道在什么时间出现在什么位置，然后生成轨迹，轨迹需要做到避开静态和动态障碍物，以及安全到达目的地。

三维空间的优化方法（复杂问题）：

• 离散化处理：从Configuration space出发，然后离散化生成一些三维空间的轨迹，再寻找一个最优解。
• Expectation Maximization（期望最大化）：降维处理，先在一个维度上进行优化，然后在优化的基础上再对其它维度进行优化，并持续迭代以获得局部最优解。

EM迭代优化Path-Speed思想：

• 首先，生成一条最优路径，在最优路径的基础上生成 Optimal Speed Profile 。
• 在下一个迭代周期，在优化后的速度（Optimal Speed Profile ）的基础上，进一步优化路径，依次类推，不断重复迭代直到收敛到最优解。
• 根据EM优化分了四步走，两步E step，两步M step
  • E step：根据隐含数据的假设值，给出当前的参数的极大似然估计。
  • M step：重新给出未知变量的期望估计，应用于缺失值。
• 缺点：不一定能收敛到全局最优解。

优化问题的关键步骤：

非线性优化问题

对于非线性优化问题，通常都是分两步走，一是动态规划，先找一个粗略解，然后再是二次规划，从粗略解出发，找出一个最优解。

如何动态规划 Path ？
答：先确定主车的位置，然后往前排撒若干点，对动态障碍物和静止障碍物处理方式一致，再基于撒点网络得到一个代价最低的路径，这时候的路径不够平滑。动态规划不要求是凸问题。

然后利用二次规划方法生成一条平滑的轨迹。

对速度的优化方法：

要保证帧与帧之间以及轨迹的连续性。

对于逆行的处理，首先根据当前 Speed Profile 去估计当前逆行障碍物的位置，然后再修正 Path，根据修正之后的 Path 再来处理 Speed，例如需要减速。减速之后，估计需要重新改变路径，依此类推，直到得到理想的规划轨迹。

算法提速：
投影与离散化：QP计算热启动，平均耗时将为3ms。
QP hotstart：这个周期算出来的最优取向可能与下一个周期的差不多。但是要保证整个的决策是连续的。

reinforcement learning and data driven approaches

决策问题通常用 POMDP 加上一些机器学习的技术来解决。
解决好规划问题，需要把两个方面做好：

• 基于规则的方法：基于规则的方法可以对遇到的新案例，很快给出解决方案。在这个基础上，对问题形成一定的认识，把问题抽象成更加通用的问题，定义目标函数来进一步优化问题（进行Data Driven）。
• 数据闭环（Data Driven）：数据驱动是在基于规则的闭环里面的小闭环。数据驱动的方法是通过大量的案例统计分析，得到模型，使得遇到类似问题的时候，不需要过多的考虑，直接套用数据驱动的模型获得结果。他是基于经验的方法，只不过这些经验是模型通过大量的样本数据学习得到的。

Handling uncertainty with model

在一个RL（reinforcement learning）系统里，它的思想是寻找一个action（or 轨迹），在这个action可以优化reward function。在给出reward function的情况，建立当前状态到action的一个映射。即对做过的题进行总结，建立好model就知道处理某类题应该是什么套路。

POMDP：在了解周围无人车在运动过程中要了解周围障碍物的意图，但这个意图不是100%确定的，要把它当作一个hidden state，观察得到的现象是它目前的一个运动轨迹，通过轨迹来判断他的意图。

端到端模仿学习问题：所需的数据量极大。不够聪明。