Constrained Iterative LQR 自动驾驶中使用的经典控制算法

Motion planning 运动规划在自动驾驶领域是一个比较有挑战的部分。它既要接受来自上层的行为理解和决策的输出，也要考虑一个包含道路结构和感知所检测到的所有障碍物状态的动态世界模型。最终生成一个满足安全性和可行性约束并且具有理想驾驶体验的轨迹。

通常，motion planners中对collision avoidance的约束的表示比较复杂。因为为了处理具有高度动态的驾驶交互场景流，需要在时空域中生成轨迹。 例如需要变道和超车场景。在较长视界的时空域内生成运动时，计算量往往难以处理，导致自动驾驶汽车无法实时响应突发事件。

因此，自动驾驶的motion planner 需要满足以下条件：

1. 在时空域中产生一个持续时间较长的运动规划
2. 考虑复杂的避免避障约束 和 车辆的运动学和动力学模型
3. 能够实时计算，及时处理突发情况。要求ms级（一般要求10ms~100ms）

本文利用线性二次型调节器( Linear Quadratic Regulator LQR)中的解析表达式，设计了一种计算速度快的运动规划器来解决复杂的规划问题。
针对具有非二次目标和非线性模型的无约束问题，动态规划(dynamic programming DP)可以有效地求解迭代LQR (ILQR)
针对自动驾驶汽车motion planning中的约束问题，本文提出了约束迭代