Iterative LQR (iLQR)

✅ 本章定位：理解 iLQR 如何求解非线性轨迹优化问题，掌握其核心思想、算法流程和应用场景。

一、从 LQR 到 iLQR

LQR 的局限性

LQR 问题有闭式解，但仅适用于：

问题：人形角色动画是非线性系统：

• 关节旋转涉及三角函数
• 动力学方程包含科里奥利力、离心力等非线性项
• 接触约束是非线性的

iLQR 的核心思想

关键洞察：虽然 LQR 只能处理线性系统，但可以在当前轨迹附近局部线性化，然后用 LQR 求解。

非线性轨迹优化问题
    ↓
在当前轨迹 (x̄, ū) 附近线性化
    ↓
δx_{t+1} = A·δx_t + B·δu_t  （线性化动力学）
    ↓
J ≈ δx^T Q δx + δu^T R δu  （二次近似）
    ↓
LQR 问题（有闭式解）
    ↓
求解得到最优控制 δu* = -K·δx
    ↓
更新轨迹：x ← x̄ + δx, u ← ū + δu
    ↓
重复线性化→求解→更新，直到收敛

这就是 iLQR（Iterative LQR）的核心思想！

二、每轮迭代中做了什么

iLQR 的每一轮迭代，本质上就是在当前轨迹处构造一个 LQR 子问题并求解。构造过程涉及三个操作（详细推导见 LQR）：

关键区别：LQR 里 A、B、Q、R 是问题本身给定的，而 iLQR 里它们每轮都在变——因为线性化点 \((\bar{x}, \bar{u})\) 随着迭代在移动。

修正量 vs 全量

LQR 直接输出最优控制 \(u^* = -Kx\)。iLQR 输出的是修正量：

$$u_{new} = \bar{u} + \alpha \cdot \delta u^* = \bar{u} + \alpha \cdot (-K\delta x)$$

其中 \(\alpha\) 是线搜索步长。直觉上：每次只敢走一小步，避免跳出局部近似的有效范围。

三、iLQR 算法

完整算法流程

算法：iLQR 轨迹优化

输入：
    - 非线性动力学 f(x, u)
    - 目标函数 J(x, u)
    - 初始状态 x₀
    - 初始控制猜测 ū₀:T

输出：
    - 最优状态轨迹 x*₀:T
    - 最优控制轨迹 u*₀:T

步骤：
1. 初始化：设定初始轨迹 (x̄, ū)
2. Repeat until convergence:
   
   (a) 前向传递（Forward Pass）
       - 用当前控制 ū 仿真得到状态轨迹 x̄
       - 计算当前总代价 J
   
   (b) 线性化/二次化
       - 计算雅可比矩阵：Aₜ = ∂f/∂x, Bₜ = ∂f/∂u
       - 计算 Hessian 矩阵：Qₜ = ∂²J/∂x², Rₜ = ∂²J/∂u²
   
   (c) 逆向传递（Backward Pass）
       - 初始化：P_T = Q_T
       - 对 t = T-1, ..., 0:
         · 计算 Kₜ = (Rₜ + BₜᵀPₜ₊₁Bₜ)⁻¹ BₜᵀPₜ₊₁Aₜ
         · 计算 Pₜ = Qₜ + KₜᵀRₜKₜ + (Aₜ - BₜKₜ)ᵀPₜ₊₁(Aₜ - BₜKₜ)
         · （可选）计算 feedforward 项 kₜ
   
   (d) 更新控制
       - 计算新控制：u_new = ū + α·δu* （α 为线搜索步长）
       - δu* = -K·δx + k （LQR 解）
   
   (e) 收敛检查
       - 如果 |J_new - J| < ε，则收敛

前向传递 vs 逆向传递

算法可视化

迭代 1: 初始轨迹（可能不满足动力学）
    ↓
┌─────────────────────────────┐
│  前向传递：仿真得到 x̄        │
│  线性化：计算 A, B           │
│  逆向传递：计算 K            │
│  更新：u_new = u + δu*      │
└─────────────────────────────┘
    ↓
迭代 2: 改进的轨迹
    ↓
┌─────────────────────────────┐
│  重复...                    │
└─────────────────────────────┘
    ↓
迭代 N: 收敛到最优轨迹

四、iLQR 与 DDP 的对比

一阶 vs 二阶方法

数学差异

iLQR（一阶动力学）： $$ f(x,u) \approx f(\bar{x},\bar{u}) + \nabla f(\bar{x},\bar{u}) \begin{bmatrix} x-\bar{x} \ u-\bar{u} \end{bmatrix} $$

DDP（二阶动力学）： $$ f(x,u) \approx f(\bar{x},\bar{u}) + \nabla f(\bar{x},\bar{u}) \begin{bmatrix} x-\bar{x} \ u-\bar{u} \end{bmatrix} + \frac{1}{2} \begin{bmatrix} x-\bar{x} \ u-\bar{u} \end{bmatrix}^T \nabla^2 f(\bar{x},\bar{u}) \begin{bmatrix} x-\bar{x} \ u-\bar{u} \end{bmatrix} $$

方法选择

五、iLQR 在轨迹优化中的定位

按分类维度

与其他方法对比

方法选择建议

更多方法对比请参见：方法对比与分类。

问题类型 → 推荐方法
─────────────────────────
线性系统 → LQR
平滑非线性 → iLQR/DDP
接触频繁/不可微 → CMA-ES
实时控制 → MPC/SAMCON
需要泛化 → DeepMimic/AMP
离线制作动作 → iLQR/DDP/CMA-ES

六、工程实践

应用实例

实际考虑

1. 初始猜测的重要性

• iLQR 是局部优化方法，可能陷入局部最优
• 好的初始猜测（如动捕数据）能显著提高结果质量

2. 线搜索（Line Search）

• 更新轨迹时需要控制步长 α
• 常用：Armijo 条件、回溯线搜索

3. 正则化

• 在 Hessian 矩阵上加正则项防止奇异
• \(R \leftarrow R + \lambda I\)

4. 终止条件

• 代价变化小于阈值：\(|J_{new} - J| < \epsilon\)
• 达到最大迭代次数

七、与 DeepMimic/AMP 的关系

关系：

• iLQR 轨迹优化结果可作为 RL 的参考轨迹
• RL 可学习模仿 iLQR 生成的行为

八、关键要点总结

核心思想

1. 迭代线性化：在当前位置附近将非线性问题近似为 LQR 问题
2. 前后向传递：前向仿真轨迹，逆向求解最优策略
3. 闭式解：每轮迭代都有 LQR 闭式解，但需要多次迭代收敛

算法流程

初始化轨迹 → 前向传递 → 线性化 → 逆向传递 → 更新轨迹 → 检查收敛

优缺点

适用场景

• ✅ 平滑的非线性轨迹优化问题
• ✅ 有参考轨迹的跟踪问题
• ✅ 需要精确控制的场景
• ❌ 接触频繁、不可微的问题（用 CMA-ES）
• ❌ 实时控制（用 MPC/SAMCON）

九、深入学习

前置知识

• LQR - 线性二次调节器
• 轨迹优化的数学描述 - 问题形式化

相关方法

• DDP - 二阶方法
• CMA-ES - 无梯度优化
• MPC - 在线优化

参考资料

• Muico et al 2011 - Composite Control of Physically Simulated Characters
• Al Borno et al. 2013 - Trajectory Optimization for Full-Body Movements with Complex Contacts

本文出自 CaterpillarStudyGroup，转载请注明出处。 https://caterpillarstudygroup.github.io/GAMES105_mdbook/