1. General Planning Problem
典型的Motion Planning问题定义如下：
其中：
是State，
是Control Input；
k表示Time Step，N是Preview Horizon，J是Cost Function；
是Final Stage Cost，
是k时刻的Cost；
式1a)是非线性的车辆动力学方程；
式1b)是Initial State；
1c)和1d)是State和Control的非线性(Non-linear)和非凸(Non-Convex)约束。
SQP算法
SQP是一种解决非线性(Nonlinear)和非凸(Non-Convex)优化问题的通用方法，但是计算效率不高。
ILQR算法
标准的ILQR定义如下：
ILQR可以高效的解决非线性系统(Nonlinear System)的优化控制问题(Predictive Optimal Control Problem)，但它的缺点是不能处理复杂的约束，比如碰撞检测约束、加速度约束、交通规则约束、限速约束等等。
CILQR算法
用标准的ILQR方法解决General Motion Planning问题，需要解决两个问题：
- General Motion Planning的State和Control是带约束的；
- General Motion Planning的Cost是非二次型的；
这就是CILQR要解决的问题。
2. Constrained Iterative LQR
CILQR将Motion Planning的Constraints和Cost转换成二次型的Cost形式，然后对其应用ILQR算法。
Cost Function Quadratization
如果Cost Function
不是Quadratic的形式，那么通过在
处进行二阶泰勒展开将其转换为二次型形式。
Constraint Function Linearization
如果Constraint Function
是非线性的，那么通过在
处进行一阶泰勒展开将其转换为线性形式。
Barrier Function Shaping
linearized Constraint Function转换为Barrier Function，将带约束的问题转换为非约束问题。并且指数形式的Barrier Function二阶可微，导数也很容易求解。
Constraint Function Quadratization
最后将Shaped Constraint Function转化为二次型的形式。
Barrier Function的一阶偏导：
Barrier Function的二阶偏导：
3、CILQR的应用
这里将具体的自动驾驶的运动规划问题定义如下，它是General Motion Planning问题的子问题。
其中：
式13a)是非线性的系统动力学方程(Dynamic Equation)；
式13c)是碰撞避免(Collision Avoidance)约束，其中C是Whole Space，
是障碍物j是k时刻的占用空间。Collision Avoidance Constraints通常是Non-convex Inequality Constraint。
3.1 Vehicle Model
车辆的运动状态
， 其中，
和
是Vehicle Position，v是Vehicle Velocity，
是Yaw Angle。
车辆动力学方程：
其中：
是前轮方向角。
动力学方程的离散表达
假设
，
时刻的车辆状态为
，k时刻的控制输入为
。
可以最终推导出k 1时刻的车辆状态：
3.2 Objective Function
目标函数如下：
Acceleration
用于惩罚较大的加速度，保障乘车体验。
Yaw Rate
的Cost项用于惩罚Vehicle Direction的快速改变和横向加速度变化导致安全风险和驾驶体验。
Offset to Reference
的Cost惩罚主车偏移参考线的行为。
假设参考线的点序列：
，常用的做法是直接计算对应点的距离。
这种方法有一些缺陷。比如在下图的场景中，主车在一条弯曲的道路上，需要停止线A前停下来，行驶参考线
是道路的中心线。
在这种场景下，由于采用对应参考点的距离作为cost，参考点
、
会让优化轨迹偏离道路中心线，与预期的效果背离。
理想的Cost是惩罚主车偏移参考轨迹线的行为，而不是偏离对应参考点的行为。
参考中心线的一个可选项是道路中心线，但道路中心线不是连续可微，因此有不收敛的风险。
最终这里选择三次多项式S作为参考轨迹，规划轨迹到三次多项式的距离平方作为
。
Velocity Difference
的Cost让主车保持相对高的通行速度，从而确定比较好的通行效率。
这些Cost函数都是二次函数，不需要额外处理。
3.3 Constraints
Acceleration Constraint
是车辆引擎能够提供的最大加速度，
是车辆刹车能够提供的最大减速度。
Yaw Rate Constraint
Yaw Rate与方向盘的转角(Steering Wheel Limit)限制有关。
其中，
是Largest Yaw Rate，可以通过将largest steering wheel angle代入
计算得到。
Boundary Constraint
Boundary Constraint的场景很多。路口停车场景下，十字路口的停止线是一种Boundary Constrain。
在Car Following场景下，Front Vehicle可以看做一个移动的Boundary Constraint。
上面几个约束都是线性的，需要采用第2节CILQR的Barrier Function Shaping和Constraint Function Quadratization将其转化为二次形的形式。
Obstacle Avoidance Constraint
如下图所示，将道路上的障碍物按照椭圆建模，椭圆的长轴
，短轴
，其中，l是Vehicle Length，w是Vehicle Width，v是Vehicle Velocity，
是车辆前方的安全距离，
是safety margin。
确定长短轴之后，就可以确定障碍物的椭圆方程：
在椭圆范围之外的区域就可以认为是安全的，因此该障碍物的Safe Constraint的定义就是：
假设主车的位置是
，与障碍物的Safety Margin是r。主车相对于该障碍物的Safety Boundary如下图的红线所示。
红线区域不方便用解析几何表达，因此用一个长轴为a r，短轴为b r的椭圆来近似表达红线区域。
最终得到主车与障碍物的碰撞检测约束：
这个约束是非线性的，需要按照第2节CILQR的Constraint Function Linearization、Barrier Function Shaping和Constraint Function Quadratization将其转化为二次形的形式。
最后，不能简单把主车抽象为一个点，可以使用两个Circle来表达主车，两个Circle的圆心分别是主车前轴和后轴的中心。如下图所示。
假设车辆后轴的中心坐标为
，那么前轴的中心坐标为
。
将这两个坐标带入椭圆方程，就可以判断主车是否会与障碍物发生碰撞。
通过CILQR的处理，将目标函数和约束条件都转化为符合ILQR的形式，最优利用ILQR算法得到最终的控制序列
和轨迹序列
效果演示
带约束的ILQR的在仿真中的一些测试场景。
静态障碍物避障场景
在变道跟车场景中，主车先加速变道，然后减速跟车。
变道跟车场景
在混合场景中，主车先加速超过前方慢速行驶的车辆，然后减速停车，等待对向车辆汇车。
混合场景
参考资料
1、Jianyu Chen, Wei Zhan, and Masayoshi Tomizuka. Constrained iterative lqr for on-road autonomous driving motion planning. In 2017 IEEE 20th International Conference on Intelligent Transportation Systems (ITSC), pages 1–7. IEEE, 2017.
2、https://github.com/pparmesh/Constrained_ILQR/tree/master
