自动驾驶：LQR、ILQR和DDP原理、公式推导以及代码演示（七、CILQR约束条件下的ILQR求解）

（七）CILQR约束条件下的ILQR求解

CILQR（(Constrained Iterative Linear Quadratic Regulator)） 是为了在 iLQR 基础上扩展处理控制输入和状态约束的问题。在这种情况下，系统不仅要优化控制输入以最小化代价函数，还需要满足各种物理约束，例如控制输入的限制（如力矩或速度的上下限）、状态的约束（如位置、速度的范围），甚至是复杂的非线性约束条件。

要在 iLQR 中处理约束，通常使用多种方法，包括：

投影法（Projection Method）
惩罚函数法（Penalty Method）
障碍函数法（Barrier Method）
拉格朗日乘子法（Lagrangian Method）
QP 求解方法（Quadratic Programming for Constraints）

下面我们详细介绍这些方法在 iLQR 中的应用以及如何通过它们处理约束。

1. 投影法（Projection Method）

投影法 是一种简单的方法，主要用于处理控制输入或状态的上下限边界约束。它通过在每次迭代后将违反约束的控制输入或状态投影到可行域内，保证最终结果满足约束。

过程：

迭代更新控制输入：在优化的过程中，根据 iLQR 更新控制输入 $\mathbf{u}_k$ 和状态 $\mathbf{x}_k$ 。
投影回可行域：如果控制输入或状态超出给定的限制，则将其投影回到约束范围内：
$\mathbf{u}_k = \max(\mathbf{u}_{\min}, \min(\mathbf{u}_{\max}, \mathbf{u}_k))$

$\mathbf{x}_k = \max(\mathbf{x}_{\min}, \min(\mathbf{x}_{\max}, \mathbf{x}_k))$

继续迭代：在投影后继续下一步的前向和后向传播，直到控制输入和状态满足收敛条件。

优缺点：

优点：方法简单，易于实现；适合处理简单的线性约束（如上下限）。
缺点：对于复杂的非线性约束不适用，且强制投影可能会对算法收敛性和解的质量产生负面影响。

2. 惩罚函数法（Penalty Method）

惩罚函数法 是通过在代价函数中增加一个罚项，用于惩罚违反约束的行为，从而迫使解满足约束条件。罚项的大小取决于违反约束的程度，随着违反约束的增大，罚项的代价增加。

过程：

定义罚项：引入违反控制输入或状态约束的罚项，例如对于控制输入 uk\mathbf{u}_kuk 的上下限约束，可以定义一个惩罚项：
$P(\mathbf{u}_k) = \rho \left( \max(0, \mathbf{u}_k - \mathbf{u}_{\max})^2 + \max(0, \mathbf{u}_{\min} - \mathbf{u}_k)^2 \right)$
其中 $\rho$ 是惩罚参数，用来调节约束的严格性。
更新代价函数：将惩罚项加入到原始代价函数中：
$J_{\text{total}} = J_{\text{original}} + \sum_k P(\mathbf{u}_k)$
优化：通过 iLQR 的前向传播和后向传播最小化这个新的代价函数 $J_{\text{total}}$ ，随着惩罚参数 $\rho$ 的增大，解会更严格地满足约束条件。

优缺点：

优点：可以处理复杂的非线性约束，能够逐渐逼近约束条件。
缺点：需要调整惩罚参数 ρ\rhoρ，如果惩罚项选择不当，可能导致收敛缓慢或约束不精确。

3. 障碍函数法（Barrier Method）

障碍函数法 是一种特殊的惩罚函数法，通过引入障碍函数来处理约束条件。当控制输入或状态接近约束边界时，障碍函数的值会趋向无穷大，从而限制解靠近或超过约束边界。

过程：

定义障碍函数：常用的障碍函数是对数障碍函数，例如对于控制输入的上下限约束：
$B(\mathbf{u}_k) = - \sum_{i} \ln(\mathbf{u}_{\max} - \mathbf{u}_k) - \sum_{i} \ln(\mathbf{u}_k - \mathbf{u}_{\min})$
更新代价函数：将障碍函数加入到代价函数中：
$J_{\text{total}} = J_{\text{original}} + \alpha \sum_k B(\mathbf{u}_k)$
其中 $\alpha$ 是障碍函数的权重，控制障碍的强度。
优化：通过 iLQR 最小化这个新的代价函数，使得解接近约束边界时，代价函数趋向无穷大，限制解超出可行域。

优缺点：

优点：能较好地处理边界约束，保证解在可行域内。
缺点：数值稳定性差，当解接近边界时，障碍函数趋向无穷大可能导致数值不稳定。

4. 拉格朗日乘子法（Lagrangian Method）

拉格朗日乘子法 通过引入拉格朗日乘子，将约束条件与目标函数相结合，转化为一个对偶优化问题。这种方法特别适用于等式约束问题。

过程：

定义拉格朗日函数：假设约束为 h(xk,uk)=0h(\mathbf{x}_k, \mathbf{u}_k) = 0h(xk,uk)=0，可以构造拉格朗日函数：
$\mathcal{L}(\mathbf{x}_k, \mathbf{u}_k, \lambda_k) = l(\mathbf{x}_k, \mathbf{u}_k) + \lambda_k h(\mathbf{x}_k, \mathbf{u}_k)$
其中 $\lambda_k$ 是拉格朗日乘子。
优化问题：通过优化拉格朗日函数，使得原目标函数最优的同时满足约束条件。
更新拉格朗日乘子：迭代更新拉格朗日乘子 λk\lambda_kλk，逐步逼近约束条件。

优缺点：

优点：适合处理等式约束问题，尤其在严格的等式约束问题中具有良好的性能。
缺点：处理不等式约束较复杂，优化过程中计算量较大。

5. QP 求解方法（Quadratic Programming for Constraints）

QP 求解方法 是通过将 iLQR 的每次迭代转化为一个带有约束的 二次规划问题（Quadratic Programming, QP）。通过引入二次规划求解器，处理线性或二次型约束问题。

过程：

线性化和二次化：在每次迭代中，将非线性系统线性化，将代价函数二次化。对于每个时间步，代价函数可以近似为：
$J(\mathbf{x}_k, \mathbf{u}_k) \approx \frac{1}{2} \left[ \mathbf{x}_k^T \mathbf{Q}_k \mathbf{x}_k + \mathbf{u}_k^T \mathbf{R}_k \mathbf{u}_k \right]$
引入约束：将控制输入或状态的约束引入到二次规划问题中，例如：
$\mathbf{u}_{\min} \leq \mathbf{u}_k \leq \mathbf{u}_{\max}, \quad \mathbf{x}_{\min} \leq \mathbf{x}_k \leq \mathbf{x}_{\max}$
求解 QP 问题：使用二次规划求解器（如 OSQP）来求解带有约束的二次规划问题，得到满足约束的最优解。

优缺点：

优点：可以精确处理线性或二次型约束问题，适用于更复杂的约束场景。
缺点：计算复杂度较高，每次迭代需要求解一个 QP 问题，适用于小规模系统或计算资源较充足的情况。

6. AL-iLQR求解方法（Augmented Lagrangian ILQR）

AL-iLQR通过引入 增广拉格朗日法（Augmented Lagrangian Method），将原始问题的约束条件通过拉格朗日乘子和惩罚项结合到代价函数中，逐步逼近最优解。

对于带有等式和不等式约束的最优控制问题，引入 增广拉格朗日函数：

$\mathcal{L}_A(\mathbf{x}, \mathbf{u}, \lambda, \rho) = l(\mathbf{x}, \mathbf{u}) + \sum_{i} \lambda_i h_i(\mathbf{x}, \mathbf{u}) + \frac{\rho}{2} \sum_{i} \left( h_i(\mathbf{x}, \mathbf{u}) \right)^2$