视频字幕
iLQR算法是迭代线性二次调节器的简称，是一种高效的非线性最优控制算法。它通过迭代地对非线性系统进行线性化，并在每次迭代中求解线性二次调节问题来逼近原始的非线性最优控制问题。算法的核心思想是将复杂的非线性优化问题分解为一系列相对简单的线性二次问题，通过迭代求解获得最优控制序列。
车辆动力学建模是轨迹跟踪问题的基础。我们定义车辆的状态变量包括位置坐标x和y、航向角θ以及速度v。控制输入包括前轮转向角δ和纵向加速度a。基于自行车模型，我们可以建立非线性状态方程，描述车辆在二维平面上的运动规律。轨迹跟踪的目标是使实际轨迹尽可能接近参考轨迹。
成本函数设计是iLQR算法的关键环节。总成本函数由跟踪误差项和控制代价项组成。跟踪误差项衡量实际轨迹与参考轨迹的偏差，包括位置误差、航向误差和速度误差。控制代价项约束控制输入的幅度，防止过度控制。权重矩阵Q和R用于平衡跟踪精度和控制平滑性，较大的Q值强调跟踪精度，较大的R值强调控制平滑性。
iLQR算法的执行过程包含四个主要步骤。首先是前向传播，从初始控制序列开始，通过动力学方程计算状态轨迹并评估成本。然后是后向传播，计算成本函数的梯度和Hessian矩阵，求解Riccati方程得到控制更新方向。第三步是线性化过程，计算雅可比矩阵并对成本函数进行二次近似。最后通过线搜索确定合适的步长，更新控制序列。这个过程不断迭代直到收敛，成本函数值逐渐下降。
通过S型曲线跟踪实例来演示iLQR算法的实际应用。我们设置车辆从原点出发，跟踪一条S型参考轨迹。算法从一个简单的直线初始猜测开始，通过迭代优化逐步改进轨迹。可以看到，随着迭代次数增加，优化轨迹越来越接近参考轨迹，同时成本函数值不断下降。经过12次迭代后，算法收敛到最优解，实现了高精度的轨迹跟踪。