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函数：

$$\begin{array}{rl}{\hat{J}}_N(x_N){|}_{\lambda ,\mu }& ={\mathcal{L}}_N(x_N,{\lambda }_N,{\mu }_N)\\ & ={\ell }_N(x_N)+({\lambda }_N+\frac{1}{2}{I}_{{\mu }_N}{c}_N(x_N){)}^T{c}_N(x_N)\\ {\hat{J}}_k(x_k){|}_{\lambda ,\mu }& =\underset{u_k}{\min }\left[{\mathcal{L}}_k(x_k,u_k,{\lambda }_k,{\mu }_k)+{\hat{J}}_{k+1}(f(x_k,u_k)){|}_{\lambda ,\mu }\right]\\ & =\underset{u_k}{\min }{\tilde{J}}_k(x_k,u_k){|}_{\lambda ,\mu },\end{array}\text{\hspace{1em}(4)}$$

根据上述新的 ${\hat{J}}_k(x_k){|}_{\lambda ,\mu }$ 和 ${\tilde{J}}_k(x_k){|}_{\lambda ,\mu }$ 的定义对其进行泰勒展开：

$$\begin{array}{rl}\delta {\hat{J}}_k(x_k){|}_{\lambda ,\mu }& \approx p_k^T\delta x_k+\frac{1}{2}\delta x_k^T{P}_k\delta x_k\end{array}\text{\hspace{1em}(5)}$$

根据定义，我们可以得到终末（terminal）的最优cost-to-go函数的二阶展开之后的系数：

$$\begin{array}{rl}{p}_N& ={\nabla }_x{\mathcal{L}}_N(x_N,{\lambda }_N,{\mu }_N)\\ & =({\ell }_N{)}_x+(c_N{)}_x^T(\lambda +I_{{\mu }_N}{c}_N)\\ P_N& ={\nabla }_{xx}{\mathcal{L}}_N(x_N,{\lambda }_N,{\mu }_N)\\ & =({\ell }_N{)}_{xx}+(c_N{)}_x^T{I}_{{\mu }_N}(c_N{)}_x\end{array}\text{\hspace{1em}(6)}$$

其中：

$$\begin{array}{rl}({\ell }_N{)}_x& =\frac{\partial {\ell }_N}{\partial x}\\ (c_N{)}_x& =\frac{\partial c_N}{\partial x}\\ ({\ell }_N{)}_{xx}& =\frac{{\partial }^2{\ell }_N}{\partial x^2}\\ (c_N{)}_{xx}& =\frac{{\partial }^2{c}_N}{\partial x^2}\end{array}\text{\hspace{1em}(7)}$$

接下来我们看action-value函数的二阶展开之后的系数，根据iLQR中的推导：

$$\begin{array}{rl}\delta {\tilde{J}}_i(x_i,u_i)& =\frac{1}{2}{\left[\begin{array}{c}\delta x_i\\ \delta u_i\end{array}\right]}^T\left[\begin{array}{cc}{Q}_{x_i^2}& Q_{x_i{u}_i}\\ Q_{u_i{x}_i}& Q_{u_i^2}\end{array}\right]\left[\begin{array}{c}\delta x_i\\ \delta u_i\end{array}\right]+{\left[\begin{array}{c}{Q}_{x_i}\\ Q_{u_i}\end{array}\right]}^T\left[\begin{array}{c}\delta x_i\\ \delta u_i\end{array}\right]\end{array}\text{\hspace{1em}(8)}$$

其中$Q_{x_i}$，$Q_{u_i}$，$Q_{x_i^2}$，$Q_{x_i{u}_i}$，$Q_{u_i{x}_i}$，$Q_{u_i^2}$的形式和iLQR中的有所不同，它们的定义如下：

$$\begin{array}{rl}{Q}_{x_i}& ={\ell }_{x_i}+A_i^T{p}_{i+1}+(c_i{)}_{x_i}^T({\lambda }_i+I_{{\mu }_i}{c}_i)\\ Q_{u_i}& ={\ell }_{u_i}+B_i^T{p}_{i+1}+(c_i{)}_{u_i}^T({\lambda }_i+I_{{\mu }_i}{c}_i)\\ Q_{x_i^2}& ={\ell }_{x_i{x}_i}+A_i^T{P}_{i+1}{A}_i+(c_i{)}_{x_i}^T{I}_{{\mu }_i}(c_i{)}_{x_i}\\ Q_{x_i{u}_i}& ={\ell }_{x_i{u}_i}+A_i^T{P}_{i+1}{B}_i+(c_i{)}_{x_i}^T{I}_{{\mu }_i}(c_i{)}_{u_i}\\ Q_{u_i{x}_i}& ={\ell }_{u_i{x}_i}+B_i^T{P}_{i+1}{A}_i+(c_i{)}_{u_i}^T{I}_{{\mu }_i}(c_i{)}_{x_i}\\ Q_{u_i^2}& ={\ell }_{u_i{u}_i}+B_i^T{P}_{i+1}{B}_i+(c_i{)}_{u_i}^T{I}_{{\mu }_i}(c_i{)}_{u_i}\end{array}\text{\hspace{1em}(9)}$$

其中：

$$\begin{array}{rl}{A}_i& =\frac{\partial f}{\partial x}{|}_{x_i,u_i}\\ B_i& =\frac{\partial f}{\partial u}{|}_{x_i,u_i}\end{array}\text{\hspace{1em}(10)}$$

接下来就和iLQR中的过程一样了，其中反向传播的过程如下：

$$\begin{array}{rl}\delta {\hat{u}}_i& =-Q_{u_i^2}^{-1}(Q_{u_i}+Q_{u_i{x}_i}^T\delta x_i)\\ & \triangleq K_i\delta x_i+d_i\\ K_i& =-Q_{u_i^2}^{-1}{Q}_{u_i{x}_i}^T\\ d_i& =-Q_{u_i^2}^{-1}{Q}_{u_i}\\ p_i& =Q_{x_i}+K_i^T{Q}_{u_i^2}{d}_i+Q_{x_i{u}_i}{d}_i+K_i^T{Q}_u\\ P_i& =Q_{x_i^2}+K_i^T{Q}_{u_i^2}{K}_i+Q_{x_i{u}_i}{K}_i+K_i^T{Q}_{u_i{x}_i}\\ \Delta {\hat{J}}_i& =\frac{1}{2}{d}_i^T{Q}_{u_i^2}{d}_i+Q_{u_i}^T{d}_i\end{array}\text{\hspace{1em}(11)}$$

当然，如果考虑正则化，$\delta {\hat{u}}_i$的计算方式如下：

方法1：

$$\begin{array}{rl}{\overline{Q}}_{u_i^2}& =Q_{u_i^2}+\rho I\\ & ={\ell }_{u_i^2}+B_i^T{P}_{i+1}{B}_i+\rho I\\ & \\ d_i& =-{\overline{Q}}_{u_i^2}^{-1}{Q}_{u_i}\\ K_i& =-{\overline{Q}}_{u_i^2}^{-1}{Q}_{u_i{x}_i}\\ & \\ \Delta {\hat{J}}_i& =\frac{1}{2}{d}_i^T{Q}_{u_i^2}{d}_i+Q_{u_i}^T{d}_i\\ p_i& =Q_{x_i}+K_i^T{Q}_{u_i^2}{d}_i+Q_{x_i{u}_i}{d}_i+K_i^T{Q}_{u_i}\\ P_i& =Q_{x_i^2}+K_i^T{Q}_{u_i^2}{K}_i+Q_{x_i{u}_i}{K}_i+K_i^T{Q}_{u_i{x}_i}\end{array}\text{\hspace{1em}(12)}$$

方法2：

$$\begin{array}{rl}{\overline{Q}}_{u_i^2}& ={\ell }_{u_i^2}+B_i^T(P_{i+1}+\rho I)B_i+(c_i{)}_{u_i}^T{I}_{{\mu }_i}(c_i{)}_{u_i}\\ {\overline{Q}}_{u_i{x}_i}& ={\ell }_{u_i{x}_i}+B_i^T(P_{i+1}+\rho I)A_i+(c_i{)}_{u_i}^T{I}_{{\mu }_i}(c_i{)}_{x_i}\\ & \\ d_i& =-{\overline{Q}}_{u_i^2}^{-1}{Q}_{u_i}\\ K_i& =-{\overline{Q}}_{u_i^2}^{-1}{\overline{Q}}_{u_i{x}_i}\\ & \\ \Delta {\hat{J}}_i& =\frac{1}{2}{d}_i^T{Q}_{u_i^2}{d}_i+Q_{u_i}^T{d}_i\\ p_i& =Q_{x_i}+K_i^T{Q}_{u_i^2}{d}_i+Q_{x_i{u}_i}{d}_i+K_i^T{Q}_{u_i}\\ P_i& =Q_{x_i^2}+K_i^T{Q}_{u_i^2}{K}_i+Q_{x_i{u}_i}{K}_i+K_i^T{Q}_{u_i{x}_i}\end{array}\text{\hspace{1em}(13)}$$

更新增广拉格朗日乘子和惩罚系数

在保持$\lambda$和$\mu$不变的情况下进行完iLQR操作之后（如上），我们可以得到新的控制量序列$\delta {\hat{u}}_{0:N-1}$以及对应的状态序列$\delta {\hat{x}}_{0:N}$，接下来我们在这个基础上更新$\lambda$和$\mu$：

$$\begin{array}{rl}{\lambda }_{k_i}^{+}& =\{\begin{array}{ll}{\lambda }_{k_i}+{\mu }_{k_i}{c}_{k_i}({\hat{x}}_k,{\hat{u}}_k)& i\in {\mathcal{E}}_k\\ \max (0,{\lambda }_{k_i}+{\mu }_{k_i}{c}_{k_i}({\hat{x}}_k,{\hat{u}}_k))& i\in {\mathcal{I}}_k\end{array}\end{array}\text{\hspace{1em}(14)}$$

$$\begin{array}{rl}{\mu }_{k_i}^{+}& =\varphi {\mu }_{k_i}\end{array}\text{\hspace{1em}(15)}$$

其中$\varphi$是一个大于1的常数，${\lambda }_{k_i}^{+}$和${\mu }_{k_i}^{+}$表示第$k$个时间步的第$i$个约束的更新后的拉格朗日乘子和惩罚系数。

AL-iLQR算法整体流程

1. 初始化$\lambda$，$\mu$和$\varphi$；
2. 根据当前的$\lambda$和$\mu$，使用iLQR算法计算$\delta {\hat{u}}_{0:N-1}$和$\delta {\hat{x}}_{0:N}$；
3. 根据$\delta {\hat{u}}_{0:N-1}$和$\delta {\hat{x}}_{0:N}$，更新$u_{0:N-1}$和$x_{0:N}$。
4. 根据$\delta {\hat{u}}_{0:N-1}$和$\delta {\hat{x}}_{0:N}$，更新$\lambda$和$\mu$；
5. 检查$\max (c)>tol$如果是真的，转到步骤2，否则结束。
6. 输出$u_{0:N-1}$和$x_{0:N}$和$\lambda$。

---

至此AL-iLQR算法的推导就完成了。

未经允许，禁止转载。