连续系统的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$是自由的，需满足：
- 需满足边界条：
- 当$x(t_f)$固定时，需满足：
- 当$x(t_f)$自由时，需满足：
我们把公式（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}\]推导完毕。
未经授权，禁止转载