基于cilqr的纵向跟车功能

在控制领域，LQR（线性-二次调节器）是少见的能在理论上得到解析解的模型，被誉为控制皇冠上的明珠。但传统的lqr不能直接应用于非线性系统和处理约束条件，而非线性和约束在现实系统中又是无处不在的，为此发展出了cilqr(Constrained Iterative LQR)[1]；

本文根据车辆的运动学模型，基于cilqr算法来实现ACC跟车功能；

lqr简介

ilqr的基本知识和求解过程可参考[2][3]；

求解标准lqr的步骤如下，主要分为两步，后向递归和前向递归，迭代次数均为T，T为轨迹步长。

设状态变量个数为m，控制变量个数为n，则上式中主要变量的维数为：

ilqr的求解步骤如下：

状态方程

目标函数

约束条件及其处理

主要对状态变量和控制变量的约束

使用如下障碍函数将约束作为惩罚项合并到目标函数中，从而化成标准的无约束lqr形式；

指数障碍函数

对数障碍函数

ilqr求解

综合后的目标函数如下：

差分处理转为增量形式

令

将状态方程和目标函数在(xt,ut)处分别进行一阶Taylor和二阶Taylor展开，化成标准lqr格式

仿真结果

前车（橙线）先匀速行驶，后恒加速度刹停，自车（蓝线）初始为静止，所以先加速追上前车，稳定跟车后跟随前车刹停；

绿线为巡航车速（速度约束），加速度上限为2mpss（加速度约束）；

分析与总结

初始值的影响

定速巡航模式，巡航速度为8.0，轨迹长度30个cycle；

自车初始状态x=[0.0,7.5,0.0]，控制jerk u=[4.0]，运行10步；

x=[0.0,7.5,0.0]，u=[-0.2]

上述对比结果表明，可行的初始解能显著降低收敛所需的迭代次数，降低运行时间；

使用指数障碍函数将硬约束转化为软约束，所以规划的轨迹有时不完全满足约束；

调试过程中发现，给的初解轻微违反约束，lqr迭代几次可以收敛，但是如果违反太多，就会发散，导致轨迹完全不可用；

算法复杂度分析

1. 标准lqr的一步运行只涉及十几次的小矩阵（维度为m+n）的乘法和一次矩阵求逆，具体复杂度为，非常高效；

2. ilqr的主要时间瓶颈在于最外层的收敛步骤，这个和初始值的选取密切相关，好的初始值能大大降低运行时间；

3. 一般地，cost函数需要考虑每个变量，即权重系数的个数为(m+n)，约束主要是对变量的上下界进行约束，所以约束个数为2(m+n)，相应的约束权重的数量为2(m+n)*a，a为障碍函数中的参数个数，总的需要调试的参数数量为(2a+1)(m+n)；

不同障碍函数的影响

下图是使用对数障碍函数仿真的结果

指数障碍函数有2个参数，对数障碍函数有1个参数；

指数障碍函数允许轨迹超过bound，使用对数障碍函数一般不会发生这种情况；

实际调参过程中，对数障碍函数对初始值和参数更敏感，很容易出现发散的情况；

[1] Jianyu Chen, Wei Zhan, and Masayoshi Tomizuka. Autonomous driving motion planning with constrained iterative lqr. IEEE Transactions on Intelligent Vehicles, 4(2):244–254, 2019

[2] https://www.imgeorgiev.com/2023-02-01-ddp/

[3] https://zhuanlan.zhihu.com/p/101129909