运动规划中的优化算法(Numerical Optimization)及其拓展和应用
论文阅读笔记，记录个人理解，若有错误欢迎指出，交流进步。
[1] 参数优化控制基础
A. Kelly and B. Nagy, “Reactive Nonholonomic Trajectory Generation via Parametric Optimal Control,” The International Journal of Robotics Research, vol. 22, no. 7–8, pp. 583–601, Jul. 2003
这篇是参数优化的经典文章，后续的很多文章都是受其启发并进一步扩展应用这个方法。这篇文章笔记已写过一次（https://blog.****.net/Neo11111/article/details/105960645），这里不再详述，只简单记录关键思想：
- 文章将给定两个边界状态，由其生成Path的问题建模为优化问题：在有了车辆运动模型的基础上，将两个边界状态作为约束，将对Path曲率的要求作为目标函数，通过优化算法求解，得到问题的最优解，即满足约束的最佳Path，文章中给的例子是Spiral Curve；
- 在问题满秩时，解唯一。在欠约束时，参数有多的*度可以用来优化，得到最符合目标函数的解；
- 求解优化问题方法为引入拉格朗日乘子，用雅阁比矩阵和Hesee矩阵构建哈密顿方程，使用牛顿迭代法求解；
- 该算法对初值敏感，收敛速度和结果严重依赖初值的质量，文章提到，一般初值用LUT查表，计算过程中进一步丰富，之后也有文章提到可以用机器学习的方式给出初值。
[2] 时间维度的引入
T. M. Howard and A. Kelly, “Optimal Rough Terrain Trajectory Generation for Wheeled Mobile Robots,” The International Journal of Robotics Research, vol. 26, no. 2, pp. 141–166, Feb. 2007
实际上[1]的规划结果仅能称为Path，作为运动规划来说，这个结果是不完整的，因为它是二维平面上的解，缺失了时间维度的信息，而要指定一个运动，除了指定其路径，还要指定速度，即引入时间维度，得到的是一条Trajectory。本篇在[1]的基础上增加了这一维度的信息。
这篇文章在问题建模时没有把变量代换为s，而是直接使用时间t作为变量：
这样，运动模型中的速度就无法消去，在求解问题时就不可避免地需要用到速度信息。这篇文章使用的办法是直接给出速度曲线（Speed Profile），文章提到的曲线模型如下，在给定始末速度、中间速度以及加速度参数的情况下，简单地将Speed Profile如下给出：
在这一维度信息给定的情况下，规划结果即为完整的Trajectory。并且给出了一些简单变体，如在目标函数加上跟障碍物相关的cost，即可让规划结果体现出避障的特性等。
其实这篇文章的重点在于将上述规划算法应用于地形崎岖的环境时改进运动和动态模型技巧，这部分与规划算法无关，此处不记录。
[3] 应用优化算法进行采样
C. Urmson et al., “Autonomous driving in urban environments: Boss and the Urban Challenge,” Journal of Field Robotics, vol. 25, no. 8, pp. 425–466, Aug. 2008,
这篇文章在[2]的基础上，进一步扩展了Speed Profile的模型，如常值、线性、斜坡、梯形等。Local Planner可以根据不同的驾驶场景，选用恰当的模型、赋予合适的参数来生成速度曲线，用来规划Trajectory。
并且，文章提到用优化算法做采样的思想。文章将车辆的形式环境分为Road Navigation和Zone Navigation，在前者即公路场景下，在车辆沿Frenet坐标系station方向的前方，沿lateral方向“撒点”，生成一些采样，使用优化算法对生成当前状态到这些采样的Trajectory（smooth + sharp）。
接着对Trajectory的平滑性、碰撞性、与道路中心线的距离等进行评价，执行最佳的Trajectory。
至于Zone Navigation，如停车场等场景，则应用传统的State Lattice算法。
可以发现，[2]和[3]虽然引入了时间维度，但并未在这维度上做所谓的规划，而是直接拿出Speed Profile。
[4] 三维规划问题降维为两个二维问题
M. Werling, J. Ziegler, S. Kammel, and S. Thrun, “Optimal trajectory generation for dynamic street scenarios in a Frenet Frame,” in 2010 IEEE International Conference on Robotics and Automation, Anchorage, AK, May 2010
这篇文章真正地在“时间维度”上进行了规划。并采用了非常有特点的方法，将SLT空间的三维规划解耦为LT维度的规划和ST维度的规划分开进行，即L向和S向的速度分开规划。这是基于高速条件下可以认为L和S向的运动独立。
并且，单独看LT或ST平面会对末端点有一些 “先验”约束，帮助建模和解决问题。
-
规划目标
S向和L向距离关于时间的五次多项式 -
LT维度
(1) 边界约束：，后两式体现L向运动的特点
(2) 目标函数： 如下式，分别体现曲率约束、时间、距离中心线的偏移 -
ST维度
(1) 边界约束： 在不同场景下，S向边界约束不同，这里不细述
(2) 目标函数： 形式同上，最后一项换成与末端点位置的偏移 -
搜索方法
文章提到，在LT和ST两维度上各自择优，合并为Trajectory进行碰撞检测，若遇障则在两维度上轻微调整，选择次优，再合并，直到找到恰当的Trajectory。为了方便选取次优，文章提到的技巧是，不要直接求优化的最优解，而是分别给出LT和ST的可行集中的一些点带入到目标函数中求cost，这样可产生很多备选方案，方便后续使用。
[5] 降维优化+采样+混合搜索
Wenda Xu, Junqing Wei, J. M. Dolan, Huijing Zhao, and Hongbin Zha, “A real-time motion planner with trajectory optimization for autonomous vehicles,” in 2012 IEEE International Conference on Robotics and Automation, St Paul, MN, USA, May 2012
这篇文章同样将三维规划问题降维为两个二维平面去解决，不同的是[4]解耦为LT和ST，而本文解耦为LS和ST两个维度。即先规划Path再加上时间维度（速度）生成Trajectory。 这种方式避免了处理非全向车辆两个方向上运动不独立的情况。
-
生成Path
(1) 对单个Path的优化建模同[1]，本文补充三次Spiral生成的解在规划周期间的一致性较差，改用五次Spiral生成的解则可在周期间保持较好的一致性。
(2) 采样：构建SL系下的State Lattice -
生成速度曲线
(1) 约束始末端点速度导数为0，考虑速度阈值，生成V-S域的State Lattice： -
完成了两个维度上的采样后，开始进行搜索。本文的解决方案为把两个维度的解混合搜索，其cost为path和speed的总cost，文章给出了详细的评价标准。
-
由于混合搜索的计算量很大，另一篇文章中提到了克服这一点的动态规划的搜索方式，尽可能减少重复计算：可以先认为节点是静态的，不加入速度，从station方向的第一列节点开始，分别生成起点到它们的不同speed profile的轨迹，每个节点需要评估以其为终点的包含speed profile的轨迹，选择最优解将其保留，并认为最优解的cost即为节点cost，且该节点的速度和时间也可唯一确定。这样的搜索结束后，进入到每一个节点的Trajectory是唯一的，其cost、T、V都唯一，故每个节点与起点的最短路是确定的。
详见M. McNaughton, C. Urmson, J. M. Dolan, and J.-W. Lee, “Motion planning for autonomous driving with a conformal spatiotemporal lattice,” in 2011 IEEE International Conference on Robotics and Automation, Shanghai, China, May 2011. -
文章提到，为补偿SL和速度这两个域的离散化搜索带来的损失，需要对结果进行后优化。
其迭代优化方式被用在了Apollo EM Planner中，后者叙述更加详细，故这块放在对EM Planner解读中记录。Apollo的方案把上述的一些经典思想和方法融合了起来，日后有空好好总结一下。