2023-10-15-增广拉格朗日方法在iLQR算法中的应用

增广拉格朗日方法在iLQR算法中的应用

背景

在之前的文章《iLQR算法公式推导》中，我们推导了iLQR算法在无约束情况下的迭代公式，但是在实际应用中，我们往往需要考虑约束条件。本文将介绍如何使用增广拉格朗日法Augmented Lagrangian Methods来处理约束条件。

约束优化问题

具有约束的优化问题具有如下形式：

$$\begin{array}{rl}\underset{x_{0:N},u_{0:N-1}}{\min }& {\ell }_N(x_N)+\sum _{k=0}^{N-1}{\ell }_k(x_k,u_k)\\ s.t.& x_{k+1}=f(x_k,u_k),k=0,\cdots ,N-1\\ & g_k(x_k,u_k)\le 0,\\ & h_k(x_k,u_k)=0,\end{array}\text{\hspace{1em}(1)}$$

其中$k$是时间步，$x_k\in {\mathbb{R}}^{n_x}$是状态，$u_k\in {\mathbb{R}}^{n_u}$是控制量，$f$是状态转移函数，${\ell }_f$是终端损失函数，${\ell }_k$是中间损失函数，$g_k$是不等式约束，$h_k$是等式约束。

增广拉格朗日法

将约束优化问题转化为增广拉格朗日函数的形式：

$$\begin{array}{rl}\mathcal{L}(x_{0:N},u_{0:N-1},{\lambda }_{0:N},{\mu }_{0:N-1})& ={\ell }_N(x_N)+({\lambda }_N+\frac{1}{2}{I}_{{\mu }_N}{c}_N(x_N){)}^T{c}_N(x_N)\\ & +\sum _{k=0}^{N-1}\left[{\ell }_k(x_k,u_k)+({\lambda }_k+\frac{1}{2}{I}_{{\mu }_k}{c}_k(x_k,u_k){)}^T{c}_k(x_k,u_k)\right]\\ & ={\mathcal{L}}_N(x_N,{\lambda }_N,{\mu }_N)+\sum _{k=0}^{N-1}{\mathcal{L}}_k(x_k,u_k,{\lambda }_k,{\mu }_k),\end{array}\text{\hspace{1em}(2)}$$

其中${\lambda }_k\in {\mathbb{R}}^{p_k}$是拉格朗日乘子，${\mu }_k\in {\mathbb{R}}^{p_k}$是惩罚系数，$c_k=(g_k,h_k)\in {\mathbb{R}}^{p_k}$是不等式约束和等式约束的组合，相应的，不等式约束的序号和等式约束的序号的集合分别是${\mathcal{I}}_k$和${\mathcal{E}}_k$，$I_{{\mu }_k}$是对角矩阵，它的定义如下：

$$I_{{\mu }_k,ii}=\{\begin{array}{ll}0,& \text{if cki<0 ∧ λki=0},i\in {\mathcal{I}}_k\\ {\mu }_{k_i},& \text{otherwise},\end{array}\text{\hspace{1em}(3)}$$

其中$k_i$表示第$k$个时间步的第$i$个约束，$c_{k_i}$表示第$k$个时间步的第$i$个约束的值，${\lambda }_{k_i}$表示第$k$个时间步的第$i$个约束的拉格朗日乘子，${\mu }_{k_i}$表示第$k$个时间步的第$i$个约束的惩罚系数。

系统的动力学约束可以通过如下方式显式得处理：使用系统的初始状态$x_0$和控制量$u_{0:N-1}$，通过状态转移函数$f$得到状态序列$x_{1:N}$。

增广拉格朗日法的迭代公式

根据iLQR算法的反向传播过程，我们可以通过固定拉格朗日乘子和惩罚系数来定义cost-to-go函数：

J ^ N ( x N ) ∣ λ , μ = L N ( x N , λ N , μ N ) = ℓ N ( x N ) + ( λ N + 1 2 I μ N c N ( x N ) ) T c N ( x N ) J ^ k ( x k ) ∣ λ , μ = min ⁡ u k [ L k ( x k , u k , λ k , μ k ) + J ^ k + 1 ( f ( x k , u k ) ) ∣ λ , μ ] = min ⁡ u k J ~ k ( x k , u