连续系统的LQR变分法推导

优化控制问题

考虑下述优化问题：
$\begin{aligned} J=h(x(t_f),t_f)+\int_{t_0}^{t_f}g(x(t),u(t),t)dt\\ \text{subject to}\quad&\dot{x}(t)=a(x(t),u(t),t)\\ &t_0,x(t_0)&\quad\text{fixed}\\ &t_f&\quad\text{free}\\ &x(t_f)&\quad\text{free or fixed} \end{aligned} \tag{1}$
其中 $\dot{x}(t)=a(x(t),u(t),t)$ 可以被看作一种约束，因此我们定义拉格朗日乘子 $p(t)$ ，同时对原有的代价函数 $J$ 进行增广，得到如下增广代价函数：
$J_a=h(x(t_f),t_f)+\int_{t_0}^{t_f}[g(x(t),u(t),t)+p(t)^T\{a(x(t),u(t),t)-\dot{x}(t)\}]dt \tag{2}$
对 $J_a$ 取变分（Variation），得到如下表达式：
$\delta J_a=h_{x_f}\delta x_f+h_{t_f}\delta t_f+\int_{t_0}^{t_f}[g_x\delta x+g_u\delta u+(a-\dot{x})^T\delta p(t)+p^T(t){a_x\delta x+a_u\delta u-\delta\dot{x}}]dt+[g+p^T(a-\dot{x})](t_f)\delta t_f \tag{3}$
其中：
$\begin{aligned} x_f&=x(t_f)\\ \dot x_f&=\dot x(t_f)\\ u_f&=u(t_f)\\ p_{f}&=p(t_f)\\ h_{x_f} &= \frac{\partial h(x,t)}{\partial x}(x_f,t_f)\\ h_{tf} &= \frac{\partial h(x,t)}{\partial t}(x_f,t_f)\\ g_x&=\frac{\partial g(x,u,t)}{\partial x}\\ g_u&=\frac{\partial g(x,u,t)}{\partial u}\\ [g+p^T(a-\dot x)](t_f)&=g(x_f,u_f,t_f)+p^T_f(a(x_f,u_f,t_f)-\dot x_f) \end{aligned} \tag{4}$
下面我们定义哈密顿量（Hamiltonian）：
$H(x,u,p,t)=g(x(t),u(t),t)+p^T(t)a(x(t),u(t),t) \tag{5}$
将定义的哈密顿量代入到公式（3）的变分中可以得到：
$\begin{aligned} \delta J_a&=h_{x_f}\delta x_f+[h_{t_f}+g+p^T(a-\dot x)](t_f)\delta t\\ &+\int_{t_0}^{t_f}[H_x\delta x+H_u\delta u+(a-\dot x)^T\delta p(t)\underbrace{-p^T(t)\delta\dot x}_{(6.1)}]dt \end{aligned} \tag{6}$
我们需要将变分表达式中的变分项进行合并，（6.1）中的 $\delta\dot x$ 可以通过分部积分法（Integragint by Parts）将导数从变分项中移出，具体操作如下：
$\begin{aligned} -\int_{t_0}^{t_f}p^T(t)\delta\dot xdt&=-\int_{t_0}^{t_f}p^T(t)d\delta x\\ &=-p^T\delta x\bigg|_{t_0}^{t_f}+\int_{t_0}^{t_f}(\frac{dp(t)}{dt})^T\delta xdt\\ &=-p^T(t_f)\delta x(t_f)+\int_{t_0}^{t_f}\dot{p}^T\delta xdt\\ &=-p^T(t_f)(\underbrace{\delta x_f-\dot x(t_f)\delta t_f}_{7.1})+\int_{t_0}^{t_f}\dot{p}^T\delta xdt \end{aligned} \tag{7}$
上式中关于分部积分法使用到了公式 $\int udv=uv-\int vdu$ ，还有一个需要注意的点是公式（7）中的（7.1）部分，这个地方看起来那么直观，不过我们只要搞清楚 $\delta x(t_f)$ 和 $\delta x_f$ 的定义，那么这个等式关系就很容易得到了。其中 $\delta x(t_f)$ 表示的函数 $x(t)$ 在时间点 $t_f$ 处的变分（我这个说法可能在数学上并不研究，只是为了方便理解），注意！在这个定义中 $t_f$ 是固定的，该变分只由函数 $x(t)$ 本身的变化决定；而 $\delta x_f$ 的定义则是 $x$ 最终状态的变分，根据定义，该变分由函数 $x(t)$ 变化和终止时间 $t_f$ 的变化共同决定。因此可以认为终末状态 $x_f$ 的变化在小量范围内，约等于函数 $x(t)$ 的变化在时间点 $t_f$ 引起的变化，叠加上由于时间 $t_f$ 本身的变化 $\delta t_f$ 累积上 $t_f$ 处 $x(t)$ 导数 $\dot x(t_f)$ （时间乘以变化率等于时间引起的变化量）引起的变化，故而有如下关系：
$\begin{aligned} \delta x_f=\delta x(t_f) + \dot x(t_f)\delta t_f \end{aligned} \tag{8}$
下面，我们可以将公式（7）的结果代入到公式（6），可以得到：
$\begin{aligned} \delta J_a&=h_{x_f}\delta x_f+[h_{t_f}+g+p^T(a-\dot x)](t_f)\delta t\\ &+\int_{t_0}^{t_f}[H_x\delta x+H_u\delta u+(a-\dot x)^T\delta p(t)]dt-\int_{t_0}^{t_f}p^T(t)\delta\dot xdt\\ &=h_{x_f}\delta x_f+[h_{t_f}+g+p^T(a-\dot x)](t_f)\delta t\\ &+\int_{t_0}^{t_f}[H_x\delta x+H_u\delta u+(a-\dot x)^T\delta p(t)]dt\\ &-p^T(t_f)(\delta x_f-\dot x(t_f)\delta t_f)+\int_{t_0}^{t_f}\dot p^T(t)\delta xdt\\ &=\underbrace{(h_{x_f}-p^T(t_f))}_{9.1}\delta x_f+\underbrace{[h_{t_f}+g+p^Ta](t_f)}_{9.2}\delta t\\ &+\int_{t_0}^{t_f}[\underbrace{(H_x+\dot p^T)}_{9.3}\delta x+\underbrace{H_u}_{9.4}\delta u+\underbrace{(a-\dot x)^T}_{9.5}\delta p(t)]dt \end{aligned} \tag{9}$
综上所述，使得 $\delta J_a=0$ 在 $t \in [t_0,t_f]$ 成立的必要条件是上述（9.1-9.5）都等于0（其中在 $t_f$ 是固定的（fixed）的时候（9.2）不需要满足），如下：
$\begin{aligned} \dot x&=a(x,u,t)\\ \dot p&=-H_x^T\\ H_u&=0 \end{aligned} \tag{10}$
以下是边界条件和约束：

当 $t_f$ 是自由的，需满足：
$[h_{t_f}+g+p^Ta](t_f)=h_{t_f}+H(t_f)=0 \tag{11}$
需满足边界条：
$x(t_0)=x_0\tag{12}$
当 $x(t_f)$ 固定时，需满足：
$x(t_f)=x_f\tag{13}$
当 $x(t_f)$ 自由时，需满足：
$p^T(t_f)=h_{x_f}\tag{14}$

我们把公式（10）中的第二项进一步展开，会有一些额外的发现：
$\begin{aligned} \dot{p}=-H_x^T=-\left(\frac{\partial H}{\partial x}\right)^T&=-\left(\frac{\partial(g+p^Ta)}{\partial x}\right)^T\\ &=-\left(\frac{\partial a}{\partial x}\right)^Tp-\left(\frac{\partial g}{\partial x}\right) \end{aligned} \tag{15}$
有上述方程我们可以发现如果把 $p$ 也当作一种状态，那上述方程就可以当作它的状态状态转移方程，由状态 $p$ 构成的系统是一个线性系统。
除此之外我们还很容易发现如下关系：
$\begin{aligned} \dot{x}=a(x,u,t)=-\left(\frac{\partial H}{\partial p}\right)^T \end{aligned} \tag{16}$
由此，我们可以发现 $x$ 和 $p$ 具有某种对称性，他们的维度是一样的，又都可以当作状态变量，同时又拥有各自的状态转移矩阵，因此，一般我们把 $p$ 称作协态（Costate），公式（15）又被称作协态方程（Costate Equation）,或者被称作辅助方程（Auxiliary Equation）、伴随方程（Adjoin Equation）、影响方程（Influence Equation）或乘数方程（Multiplier Equation）。

LQR的变分法推导

我们考虑确定性的线性二次型调节器（Deterministic Quadratic Regulator），被控对象如下：
$\begin{aligned} &\dot x(t)=A(t)x(t)+B_u(t)u(t),\quad x(t_0)=x_0\\ &z(t)=C_z(t)x(t) \end{aligned} \tag{17}$
代价函数如下：
$\begin{aligned} J_{LQR}&=\frac{1}{2}\int_{t_0}^{t_f}[z^T(t)R_{zz}z(t)+u^T(t)R_{uu}u(t)]dt+x^T(t_f)P_{t_f}x(t_f)\\ \end{aligned} \tag{18}$
上述表达式满足如下条件：

$P_{t_f}\geq0$ ， $R_{zz}(t)>0$ 和 $R_{uu}(t)>0$
我们定义 $R_{xx}=C_z^TR_{zz}C_z\geq0$
$A(t)$ 是时间连续函数
$B_u(t)$ ， $C_z(t)$ ， $R_{zz}(t)$ ， $R_{uu}(t)$ 是时间上的分段连续函数，并且有界。

根据上述定义，我们可以将问题描述为：寻找 $u(t),\forall t \in[t_0,t_f]$ 使得代价函数 $J_{LQR}$ 最小。
为了优化代价函数，我们使用拉格朗日乘子法对代价函数进行增广，我们定义增广之后的代价函数为 $J_{ALQR}$ ：
$\begin{aligned} J_{ALQR}&=\int_{t_0}^{t_f}\left\{\frac{1}{2}[z^T(t)R_{zz}z(t)+u^T(t)R_{uu}u(t)]+p^T(t)(Ax(t)+B_uu(t))\right\}dt+x^T(t_f)P_{t_f}x(t_f)\\ \end{aligned} \tag{19}$
根据公式（5）的定义，我们可以得到哈密顿量：
$\begin{aligned} H&=\frac{1}{2}[z^T(t)R_{zz}z(t)+u^T(t)R_{uu}u(t)]+p^T(t)(Ax(t)+B_uu(t))\\ \end{aligned} \tag{20}$
根据公式（10）中的必要条件，可以得到：
$\dot x(t)=\frac{\partial H^T}{\partial p}=Ax(t)+B_uu(t),\quad x(t_0)=x_0\tag{21}$
$\dot p(t)=-\frac{\partial H^T}{\partial x}=-R_{xx}x(t)-A^Tp(t),\quad p(t_f)=P_{t_f}x(t_f)\tag{22}$
$\frac{\partial H}{\partial u}=0 \Rightarrow R_{uu}u+B_u^Tp(t)=0\tag{23}$
根据公式（23）可以得到最优控制率 $\hat{u}$ 的必要条件是：
$\hat{u}=-R_{uu}^{-1}B_u^Tp(t)\tag{24}$
为了使其成为充分必要条件，还需要满足 $\frac{\partial^2H}{\partial u^2}\geq 0$ 。
显然有：
$\begin{aligned} \frac{\partial^2H}{\partial u^2}=R_{uu}\geq0 \end{aligned} \tag{25}$
因此可以得知公式（24）是充分且必要的。
现在我们可以联系在《连续系统的LQR推导》这篇文章中的讨论，会发现 $p(t)$ 扮演着和 $\hat{J}_x(x(t),t)^T$ 同样的角色，其中最主要的区别在于我们不需要去大胆假设 $\hat{J}(x(t),t)$ 的解是一个二次型了！接下来我们可以证明这一点。
显然有：
$\begin{aligned} \dot x(t)&=-Ax(t)+B_u\hat u(t)=Ax(t)-B_uR_{uu}^{-1}B_u^Tp(t)\\ \dot p(t)&=-R_{xx}x(t)-A^Tp(t)=-C_z^TR_{zz}C_zx(t)-A^Tp(t)\\ \end{aligned} \tag{26}$
综上我们可以得到如下关系：
$\begin{bmatrix} \dot x(t)\\ \dot p(t)\\ \end{bmatrix}= \underbrace{ \begin{bmatrix} A & -B_uR_{uu}^{-1}B_u^T\\ -C_z^TR_{zz}C_z & -A^T\\ \end{bmatrix} }_{H} \begin{bmatrix} x(t)\\ p(t)\\ \end{bmatrix} \tag{27}$
我们将上述 $H$ 矩阵称为哈密顿矩阵（Hamiltonian Matrix）。我们发现， $x$ 和 $p$ 的状态转移矩阵存在耦合，同时，对于 $x$ 我们只知道它的初始状态 $x_0$ ，而对于 $p$ 我们只知道它的终末状态 $p(t_f)=P_{t_f}x(t_f)$ 。这是一个难以解决并且比较典型的两点边值问题（Two-Point Boundary Problem）。
根据公式（27），可以发现它是一个线性常微分方程组。根据相关理论，如果已知一个在时间 $t_0$ 的状态 $[x(t_0),p(t_0)]^T$ ，其任意时间点状态 $[x(t),p(t)]^T$ 满足如下关系：
$\begin{bmatrix} x(t)\\ p(t)\\ \end{bmatrix}=F(t, t_0) \begin{bmatrix} x(t_0)\\ p(t_0)\\ \end{bmatrix}= \begin{bmatrix} F_{11}(t,t_0)&F_{12}(t,t_0)\\ F_{21}(t,t_0)&F_{22}(t,t_0)\\ \end{bmatrix} \begin{bmatrix} x(t_0)\\ p(t_0)\\ \end{bmatrix} \tag{28}$
上式证明比较复杂，在此不打算着墨太多（主要是我不会），不过大概可以确定的是在给定初始状态 $t_0$ 时， $F(t,t_0)$ 只与时间 $t$ 有关，这一点可以将公式（28）代入到公式（27）中，可以得到：
$\dot F(t, t_0)=F(t, t_0)\begin{bmatrix} x(t_0)\\ p(t_0)\\ \end{bmatrix} \tag{29}$
根据相关理论可以得到 $F(t,t_0)$ 的解的形式如下：
$F(t,t_0)=\mathcal{T}e^{\int_{t_0}^{t}A(\tau)d\tau}$
其中时间排序算符 $\mathcal{T}$ 用来保证矩阵 $A(\tau)$ 按照时间顺序正确地作用。
根据公式（28），考虑任意时间 $t$ 和终末时间 $t_f$ ，有：
$\begin{bmatrix} x(t)\\ p(t)\\ \end{bmatrix}=F(t, t_f) \begin{bmatrix} x(t_f)\\ p(t_f)\\ \end{bmatrix}= \begin{bmatrix} F_{11}(t,t_f)&F_{12}(t,t_f)\\ F_{21}(t,t_f)&F_{22}(t,t_f)\\ \end{bmatrix} \begin{bmatrix} x(t_f)\\ p(t_f)\\ \end{bmatrix} \tag{30}$
于是很容易得到如下关系：
$\begin{aligned} x(t)&=F_{11}(t,t_f)x(t_f)+F_{12}(t,t_f)p(t_f)\\ &=[F_{11}(t,t_f)+F_{12}(t,t_f)P_{t_f}]x(t_f)\\ p(t)&=F_{21}(t,t_f)x(t_f)+F_{22}(t,t_f)p(t_f)\\ &=[F_{21}(t,t_f)+F_{22}(t,t_f)P_{t_f}]x(t_f) \end{aligned} \tag{31}$
于是有：
$\begin{aligned} p(t)&=[F_{21}(t,t_f)+F_{22}(t,t_f)P_{t_f}][F_{11}(t,t_f)+F_{12}(t,t_f)P_{t_f}]^{-1}x(t)\\ &\triangleq P(t)x(t) \end{aligned} \tag{32}$
现在我们得了 $p(t)=P(t)x(t)$ 了，下面我们看一看 $P(t)$ 是如何变化的，根据公式（32）我们有：
$\dot p(t)=\dot P(t)x(t)+P(t)\dot x(t) \tag{33}$
根据公式（33）、（22）、（21）、（24）、（32）有：
$\begin{aligned} -\dot P(t)x(t)&=-\dot p(t)+P(t)\dot x(t)\\ &=C_z^TR_{zz}C_zx(t)+A^Tp(t)+P(t)(Ax(t)-B_uR_{uu}^{-1}B_u^Tp(t))\\ &=(C_z^TR_{zz}C_z+P(t)A)x(t)+(A^T-P(t)B_uR_{uu}^{-1}B_u^T)p(t)\\ &=(C_z^TR_{zz}C_z+P(t)A)x(t)+(A^T-P(t)B_uR_{uu}^{-1}B_u^T)P(t)x(t)\\ &=[A^TP(t)+P(t)A+C_z^TR_{zz}C_z-P(t)B_uR_{uu}^{-1}B_u^TP(t)]x(t) \end{aligned} \tag{34}$
于是我们得到了连续时间代数黎卡提方程（continuous time algebraic Riccati equation-CARE）：
$-\dot P(t)=A^TP(t)+P(t)A+C_z^TR_{zz}C_z-P(t)B_uR_{uu}^{-1}B_u^TP(t) \tag{35}$
我们可以从终末时间点 $t_f$ 的 $P(t_f)=P_{t_f}$ 反向求解最优的 $P(t)$ ，同时可以据此得到最优控制输入：
$\hat{u}=-R_{uu}^{-1}B_u^Tp(t)=-R_{uu}^{-1}B_u^TP(t)x(t)\triangleq-K(t)x(t) \tag{36}$

推导完毕。

未经授权，禁止转载

最后编辑于：2023.09.30 22:25:00

人面猴
序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
沈念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

连续系统的LQR变分法推导

优化控制问题

LQR的变分法推导

推荐阅读更多精彩内容