最优化理论与自动驾驶（四）：iLQR原理、公式及代码演示-CSDN博客

本文链接：https://blog.csdn.net/a8598671/article/details/141969181

（二）iLQR原理、公式推导以及代码演示

1. iLQR 原理

iLQR（迭代线性二次调节器）是一种用于求解非线性系统最优控制问题的算法。我们在第一部分已经介绍了LQR相关的内容，iLQR其实就是LQR的迭代版本。它在每个时间步通过一阶泰勒展开线性化系统状态方程，并对成本函数进行二阶近似，然后使用线性二次调节器（LQR）求解该近似问题。通过迭代调整策略，逐步逼近最优策略。iLQR属于微分动态规划（DDP）类算法的简化版本，特别适用于解决状态空间和控制输入较大的问题。

2. 应用场景

iLQR 应用于解决许多复杂的非线性系统优化控制问题，尤其是在机器人控制、轨迹规划、自主驾驶、航天器姿态控制等领域。它通常用于以下场景：

轨迹跟踪：机器人、无人机、车辆的轨迹规划与控制。
姿态控制：如航天器的姿态调整，精确控制其方向与姿态。
动态系统优化：如机械臂在复杂任务中的路径优化。

3. iLQR 问题定义

我们考虑一个离散时间的动态系统，其状态方程为：

$\mathbf{x}_{k+1} = \mathbf{f}(\mathbf{x}_k, \mathbf{u}_k)$

其中， $x_k \in \mathbb{R}^n$ 是系统在时间步k的状态， $u_k \in \mathbb{R}^m$ 是控制输入， $f$ 描述系统的动态方程。我们的目标是找到控制序列 $\{u_0, u_1, \dots, u_{N-1}\}$ 最小化以下成本函数：

$\min_{\{u_0, u_1, \dots, u_{N-1}\}} \sum_{k=0}^{N-1} l(x_k, u_k) + l_f(x_N)$

其中， $l(\mathbf{x}_k, \mathbf{u}_k)$ 是阶段成本函数， $l_f(\mathbf{x}_N)$ 是终端成本。由于LQR算法要求系统的状态方程为线性状态方程，成本函数为二次型，因此iLQR是通过牛顿高斯方法进行迭代逼近最优解。

4. iLQR 公式推导

1. 值函数的定义

为了方便优化和递推，我们引入值函数 $V_k(x_k)$ ，它表示从时刻k开始，给定当前状态 $x_k$ ，剩余时间内的最小代价。其定义如下：

$V_k(x_k) = \min_{\{u_k, \dots, u_{N-1}\}} \sum_{t=k}^{N-1} l(x_t, u_t) + l_f(x_N)$

值函数 $V_k(x_k)$ 依赖于当前状态 $x_k$ ，并反映了从当前状态到终端状态所需支付的最小累积代价。终端时刻的值函数为：

$V_N(x_N) = l_f(x_N)$

2. 值函数的贝尔曼递推方程

在动态规划（Dynamic Programming）中，值函数满足贝尔曼方程。贝尔曼方程是通过递推的方式，将问题从一个长时间的优化问题简化为一步步的局部优化问题。对于k时刻的值函数，我们有：

$V_k(x_k) = \min_{u_k} \left[ l(x_k, u_k) + V_{k+1}(x_{k+1}) \right]$

其中， $x_{k+1} = f(x_k, u_k)$ 。也就是说，值函数 $V_k(x_k)$ 是当前阶段成本 $l(x_k, u_k)$ 和未来值函数 $V_{k+1}(x_{k+1})$ 之和的最小值。

3. 一阶近似与线性化

在iLQR算法中，核心思想是将非线性的动力学方程 $f(x_k, u_k)$ 和成本函数 $l(x_k, u_k)$ 线性化和二次近似化。首先对 $x_{k+1} = f(x_k, u_k)$ 进行一阶泰勒展开，得到其线性化近似：

$x_{k+1} \approx f(x_k^*, u_k^*) + f_x \delta x_k + f_u \delta u_k$

这里， $\delta x_k = x_k - x_k^*$ 和 $\delta u_k = u_k - u_k^*$ 分别是状态和控制输入的偏差， $f_x = \frac{\partial f}{\partial x}$ 和 $f_u = \frac{\partial f}{\partial u}$ 是系统动力学方程的雅可比矩阵，分别表示状态和控制输入对下一时刻状态的影响。