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

背景

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

约束优化问题

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

$\begin{aligned} \min_{x_{0:N},u_{0:N-1}} \quad & \ell_N(x_N)+\sum_{k=0}^{N-1}\ell_k(x_k,u_k)\\ s.t. \quad & x_{k+1}=f(x_k,u_k),\quad k=0,\cdots,N-1\\ & g_k(x_k,u_k)\leq 0,\\ & h_k(x_k,u_k)=0, \end{aligned} \tag{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{aligned} \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))^Tc_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))^Tc_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{aligned} \tag{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{cases} 0, & \text{if $c_{k_i}<0$ $\land$ $\lambda_{k_i} = 0$},i\in\mathcal{I}_k\\ \mu_{k_i}, & \text{otherwise}, \end{cases} \tag{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{aligned} \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))^Tc_N(x_N)\\ \hat J_k(x_k)|_{\lambda,\mu}&=\min_{u_k}\left[\mathcal{L}_k(x_k,u_k,\lambda_k,\mu_k)+\hat{J}_{k+1}(f(x_k,u_k))|_{\lambda,\mu}\right]\\&=\min_{u_k}\tilde{J}_k(x_k,u_k)|_{\lambda,\mu}, \end{aligned} \tag{4}$

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

$\begin{aligned} \delta \hat J_k(x_k)|_{\lambda,\mu}&\approx p_k^T\delta x_k+\frac{1}{2}\delta x_k^TP_k\delta x_k\\ \end{aligned} \tag{5}$

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

$\begin{aligned} p_N&=\nabla_x\mathcal{L}_N(x_N,\lambda_N,\mu_N)\\ &=(\ell_N)_x+(c_N)^T_x(\lambda+I_{\mu_N}c_N)\\ P_N&=\nabla_{xx}\mathcal{L}_N(x_N,\lambda_N,\mu_N)\\ &=(\ell_N)_{xx}+(c_N)^T_{x}I_{\mu_N}(c_N)_x\\ \end{aligned} \tag{6}$

其中：

$\begin{aligned} (\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{aligned} \tag{7}$

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

$\begin{aligned} \delta \tilde{J}_i(x_i,u_i)&=\frac{1}{2} \begin{bmatrix} \delta x_i \\ \delta u_i \end{bmatrix}^T \begin{bmatrix} Q_{x_i^2} & Q_{x_iu_i}\\ Q_{u_ix_i} & Q_{u_i^2} \end{bmatrix} \begin{bmatrix} \delta x_i \\ \delta u_i \end{bmatrix}+ \begin{bmatrix} Q_{x_i} \\ Q_{u_i} \end{bmatrix}^T \begin{bmatrix} \delta x_i \\ \delta u_i \end{bmatrix}\\ \end{aligned} \tag{8}$

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

$\begin{aligned} Q_{x_i}&=\ell_{x_i}+p_{i+1}^TA_i+(c_i)^T_{x_i}(\lambda_i+I_{\mu_i}c_i)\\ Q_{u_i}&=\ell_{u_i}+p_{i+1}^TB_i+(c_i)^T_{u_i}(\lambda_i+I_{\mu_i}c_i)\\ Q_{x_i^2}&=\ell_{x_ix_i}+A_i^TP_{i+1}A_i+(c_i)^T_{x_i}I_{\mu_i}(c_i)_{x_i}\\ Q_{x_iu_i}&=\ell_{x_iu_i}+A_i^TP_{i+1}B_i+(c_i)^T_{x_i}I_{\mu_i}(c_i)_{u_i}\\ Q_{u_ix_i}&=\ell_{u_ix_i}+B_i^TP_{i+1}A_i+(c_i)^T_{u_i}I_{\mu_i}(c_i)_{x_i}\\ Q_{u_i^2}&=\ell_{u_iu_i}+B_i^TP_{i+1}B_i+(c_i)^T_{u_i}I_{\mu_i}(c_i)_{u_i}\\ \end{aligned} \tag{9}$
其中：

$\begin{aligned} A_i&=\frac{\partial f}{\partial x}|_{x_i,u_i}\\ B_i&=\frac{\partial f}{\partial u}|_{x_i,u_i}\\ \end{aligned} \tag{10}$

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

$\begin{aligned} \delta \hat u_i &= -Q_{u_i^2}^{-1}(Q_{u_i}+Q_{u_ix_i}^T\delta x_i)\\ &\triangleq K_i\delta x_i + d_i\\ K_i &= -Q_{u_i^2}^{-1}Q_{u_ix_i}^T\\ d_i &= -Q_{u_i^2}^{-1}Q_{u_i}\\ p_i&=Q_{x_i}+K_i^TQ_{u_i^2}d_i+Q_{x_iu_i}d_i+K_i^TQ_u\\ P_i&=Q_{x_i^2}+K_i^TQ_{u_i^2}K_i+Q_{x_iu_i}K_i+K_i^TQ_{u_ix_i}\\ \Delta \hat J_i&=\frac{1}{2}d_i^TQ_{u_i^2}d_i+Q_{u_i}^Td_i\\ \end{aligned} \tag{11}$

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

方法1：

$\begin{aligned} \overline{Q}_{u_i^2} &= Q_{u_i^2}+\rho I\\ &= \ell_{u_i^2}+B_i^TP_{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_ix_i}^T\\ \\ \Delta \hat J_i &= \frac{1}{2}d_i^TQ_{u_i^2}d_i+Q_{u_i}^Td_i\\ p_i &= Q_{x_i}+K_i^TQ_{u_i^2}d_i+Q_{x_iu_i}d_i+K_i^TQ_{u_i}\\ P_i &= Q_{x_i^2}+K_i^TQ_{u_i^2}K_i+Q_{x_iu_i}K_i+K_i^TQ_{u_ix_i}\\ \end{aligned} \tag{12}$

方法2：

$\begin{aligned} \overline{Q}_{u_i^2} &= \ell_{u_i^2}+B_i^T(P_{i+1}+\rho I)B_i\\ \overline{Q}_{u_ix_i} &= \ell_{u_ix_i}+B_i^T(P_{i+1}+\rho I)A_i\\ \\ d_i &= -\overline{Q}_{u_i^2}^{-1}Q_{u_i}\\ K_i &= -\overline{Q}_{u_i^2}^{-1}Q_{u_ix_i}^T\\ \\ \Delta \hat J_i &= \frac{1}{2}d_i^TQ_{u_i^2}d_i+Q_{u_i}^Td_i\\ p_i &= Q_{x_i}+K_i^TQ_{u_i^2}d_i+Q_{x_iu_i}d_i+K_i^TQ_{u_i}\\ P_i &= Q_{x_i^2}+K_i^TQ_{u_i^2}K_i+Q_{x_iu_i}K_i+K_i^TQ_{u_ix_i}\\ \end{aligned} \tag{13}$

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

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

$\begin{aligned} \lambda_{k_i}^+&=\begin{cases} \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{cases}\\ \end{aligned} \tag{14}$

$\begin{aligned} \mu_{k_i}^+&=\phi\mu_{k_i} \end{aligned} \tag{15}$

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

AL-iLQR算法整体流程

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

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

未经允许，禁止转载。

最后编辑于：2023.10.15 15:40:59

人面猴
序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
沈念sama阅读 213,928评论 6赞 493
死咒
序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
沈念sama阅读 91,192评论 3赞 387
救了他两次的神仙让他今天三更去死
文/潘晓璐我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
开封第一讲书人阅读 159,468评论 0赞 349
道士缉凶录：失踪的卖姜人
文/不坏的土叔我叫张陵，是天一观的道长。经常有香客问我，道长，这世上最难降的妖魔是什么？我笑而不...
开封第一讲书人阅读 57,186评论 1赞 286
港岛之恋（遗憾婚礼）
正文为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
茶点故事阅读 66,295评论 6赞 386
恶毒庶女顶嫁案：这布局不是一般人想出来的
文/花漫我一把揭开白布。她就那样静静地躺着，像睡着了一般。火红的嫁衣衬着肌肤如雪。梳的纹丝不乱的头发上，一...
开封第一讲书人阅读 50,374评论 1赞 292
城市分裂传说
那天，我揣着相机与录音，去河边找鬼。笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。我是一名探鬼主播，决...
沈念sama阅读 39,403评论 3赞 412
双鸳鸯连环套：你想象不到人心有多黑
文/苍兰香墨我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
开封第一讲书人阅读 38,186评论 0赞 269
万荣杀人案实录
序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
沈念sama阅读 44,610评论 1赞 306
护林员之死
正文独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋以下内容为张勋视角年9月15日...
茶点故事阅读 36,906评论 2赞 328
白月光启示录
正文我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
茶点故事阅读 39,075评论 1赞 341
活死人
序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
沈念sama阅读 34,755评论 4赞 337
日本核电站爆炸内幕
正文年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
茶点故事阅读 40,393评论 3赞 320
男人毒药：我在死后第九天来索命
文/蒙蒙一、第九天我趴在偏房一处隐蔽的房顶上张望。院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
开封第一讲书人阅读 31,079评论 0赞 21
一桩弑父案，背后竟有这般阴谋
文/苍兰香墨我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。一阵脚步声响...
开封第一讲书人阅读 32,313评论 1赞 267
情欲美人皮
我被黑心中介骗来泰国打工，没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。一个月前我还...
沈念sama阅读 46,934评论 2赞 365
代替公主和亲
正文我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
茶点故事阅读 43,963评论 2赞 351