最优控制理论 四、线性二次型最优控制和LQR

在前面两节最优控制理论 二、哈密尔顿函数法，我们利用Hamilton函数法讨论了终端等式约束的非线性控制系统的最优控制，它所解决的是
$$\begin{gathered} \dot{\mathbf{x}}=f[\mathbf{x}(t),\mathbf{u}(t),t];\mathbf{x}\left(t_o\right)={\mathbf{x}}_0{t}_o\le t\le t_f \\ \underset{u(t)}{\min }J=\phi \left[\mathbf{x}\left(t_f\right),t_f\right]+{\int }_{t_o}^{t_f}L[\mathbf{x}(t),\mathbf{u}(t),t]\text{d}t \\ \psi (\mathbf{x}(t_f),t_f)=0 \end{gathered}$$

这样的问题。
我们下面来讨论，更特殊一些的，连续线性系统、线性反馈、二次型性能指标、无约束问题的最优控制。事实上，前面两章的方法由于解析解不易推导及BVP问题难以直接求解，所以基本只能用来进行离线的轨迹规划；而LQR方法简单到调用一个命令就可以给出反馈增益矩阵，能在工程应用中实现实时在线控制。

线性时变系统的二次型最优控制

对于这样一个性能指标为二次型函数的线性时变系统：
$$\begin{gathered} \dot{\mathbf{x}}=A(t)\mathbf{x}+B(t)\mathbf{u},\mathbf{x}(0)={\mathbf{x}}_0\ne 00\le t\le t_f \\ \underset{\mathbf{u}(t)}{\min }J=\frac{1}{2}{\mathbf{x}}^{\mathrm{T}}(t_f){\mathbf{S}}_f\mathbf{x}(t_f)+\frac{1}{2}{\int }_0^{t_f}\left({\mathbf{x}}^{\mathrm{T}}\mathbf{Q}(t)\mathbf{x}+{\mathbf{u}}^{\mathrm{T}}\mathbf{R}(t)\mathbf{u}\right)\text{d}t\text{\hspace{1em}(问题1)} \end{gathered}$$

其中${\mathbf{x}}_0,t_f$给定，$\mathbf{x}\in {\mathbb{R}}^n,\mathbf{u}(t)\in {\mathbb{R}}^m,S=S^{\mathrm{T}}\succcurlyeq 0,Q=Q^{\mathrm{T}}\succcurlyeq 0,R=R^{\mathrm{T}}\succ 0$，（正定对称矩阵）状态方程中的$A(t),B(t)$、以及性能指标中的$Q(t),R(t)$均为时变矩阵。
终端控制器问题（LQ Terminal Controller）的要求是，求出一个控制器使得从不为0的初始状态，经过一段时间后转移到接近零状态$\mathbf{x}(t_f)\approx 0\ne 0$。显然若终端状态彻底为0，则性能指标就没有存在第一项的必要了。

Riccati方程

问题的一阶必要条件按照Hamilton函数法建立问题描述：

• 构建Hamiltonian
  $$H=\frac{1}{2}\left({\mathbf{x}}^{\mathrm{T}}\mathbf{Q}\mathbf{x}+{\mathbf{u}}^{\mathrm{T}}\mathbf{R}\mathbf{u}\right)+{\lambda }^{\mathrm{T}}(\mathbf{A}\mathbf{x}+\mathbf{B}\mathbf{u})$$
• Euler方程
  $$\begin{gathered} \dot{\mathbf{x}}=A\mathbf{x}+B\mathbf{u} \\ \dot{\lambda }=-H_{\mathrm{x}}=-\mathbf{Q}\mathbf{x}-{\mathbf{A}}^{\mathrm{T}}\mathbf{\lambda } \end{gathered}$$
• 控制方程
  $$\frac{\partial H}{\partial \mathbf{u}}=\mathbf{R}\mathbf{u}+{\mathbf{B}}^{\mathrm{T}}\lambda =0⟹\mathbf{u}=-{\mathbf{R}}^{-1}{\mathbf{B}}^{\mathrm{T}}\lambda$$
• 边界条件
  $$\begin{gathered} \mathbf{x}(0)={\mathbf{x}}_0 \\ \lambda (t_f)=\frac{\partial \phi }{\partial {\mathbf{x}}^{\mathrm{T}}}⟹\lambda (t_f)=S_f\mathbf{x}(t_f) \end{gathered}$$

假设协态变量始终和状态变量成线性关系
$$\lambda (t)=S(t)x(t)\text{\hspace{1em}(1)}$$

其中$S(t)$未知。这个假设的合理性可以考虑Hamilton系统状态转移矩阵的线性性：
$$\left[\begin{array}{c}x(t_f)\\ \lambda (t_f)\end{array}\right]=\Phi (t_f;t)\left[\begin{array}{c}x(t)\\ \lambda (t)\end{array}\right]=\left[\begin{array}{c}{\Phi }_{11}{\Phi }_{12}\\ {\Phi }_{21}{\Phi }_{22}\end{array}\right]\left[\begin{array}{c}x(t)\\ \lambda (t)\end{array}\right]$$

$$\begin{gathered} {\Phi }_{21}x+{\Phi }_{22}\lambda =\lambda (t_f)=S_f\mathbf{x}(t_f)=S_f[{\Phi }_{11}x+{\Phi }_{12}\lambda ] \\ \Downarrow \\ \lambda (t)=[{\Phi }_{22}-S_f{\Phi }_{12}{]}^{-1}[S_f{\Phi }_{11}-{\Phi }_{21}]\mathbf{x}(t)\equiv S(t)\mathbf{x}(t) \end{gathered}$$

由上述假设，可得到最优的控制器是反馈控制器，且它的形式为
$$u^{\ast }(t)=-R^{-1}{B}^{\mathrm{T}}S(t)\mathbf{x}(t)=-K(t)\mathbf{x}(t)\text{\hspace{1em}(2)}$$
式中的$K(t)$为未知的反馈增益矩阵。把控制代入状态方程和协态变量，构成两点边值问题为
$$\begin{array}{rl}\dot{\mathbf{x}}& =A\mathbf{x}-B{R}^{-1}{B}^{\mathrm{T}}S(t)\mathbf{x}=\left(A-B{R}^{-1}{B}^{\mathrm{T}}S(t)\right)\mathbf{x}\\ \dot{\lambda }& =\dot{S}\mathbf{x}+S(t)\dot{\mathbf{x}}=\dot{S}(t)\mathbf{x}+S(t)\left(A-B{R}^{-1}{B}^{\mathrm{T}}S(t)\right)\mathbf{x}\\ & =-Q\mathbf{x}-A^{\mathrm{T}}S(t)\mathbf{x}\end{array}\text{\hspace{1em}(3)}$$

将最优控制$(3)$代入状态方程，即可得到闭环系统的动态方程：

$$\left[\begin{array}{c}\dot{\mathbf{x}}\\ \dot{\lambda }\end{array}\right]=\left[\begin{array}{cc}A& -{BR}^{-1}{B}^{\mathrm{T}}\\ -Q& -A^{\mathrm{T}}\end{array}\right]\left[\begin{array}{c}\mathbf{x}\\ \lambda \end{array}\right]\text{\hspace{1em}(3*)}$$

这个TPBVP问题的未知变量数2n，初始状态提供n个边界条件，终端时刻$\lambda (t_f)=S\mathbf{x}(t_f)$提供n个横截条件。
公式$(3)$中$S(t)$是一个与时间相关的函数矩阵，它在终端时刻的值就是性能指标中我们设定的值。考虑公式$(3)$最后一个等式关系，可以求解出$S(t)$：
$$-\dot{S}(t)=A^{\mathrm{T}}S+SA-SB{R}^{-1}{B}^{\mathrm{T}}S+Q,S(t_f)=S_f\text{\hspace{1em}(4)}$$
这个公式$(4)$称为矩阵黎卡提Riccati方程，可以证明$S(t)$是半正定对称矩阵。已知$S(t_f)$，按照时间逆向积分，可以得到$S(t)$，最终得到$S(t_0)$；然后考虑到$\lambda (t_0)=S(t_0){\mathbf{x}}_0$可以得到问题$(3\ast )$的初值，然后再正向积分一次即可得到所有的结果。

非对角阵的性能指标

如果遇到下面这种形式的性能指标
$$\begin{gathered} J=\frac{1}{2}{\mathbf{x}}^{\mathrm{T}}(t_f){\mathbf{S}}_f\mathbf{x}(t_f)+\frac{1}{2}{\int }_0^{t_f}\left(x^{T}Qx+u^{T}Ru+2x^{T}Nu\right)\text{d}t \\ =\frac{1}{2}{\mathbf{x}}^{\mathrm{T}}(t_f){\mathbf{S}}_f\mathbf{x}(t_f)+\frac{1}{2}{\int }_{t_0}^{t_f}{\left[\begin{array}{c}X\\ U\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{cc}Q& N\\ N^{\mathrm{T}}& R\end{array}\right]\left[\begin{array}{c}X\\ U\end{array}\right]\mathrm{d}t\text{\hspace{1em}(问题1*)} \end{gathered}$$

其中$Q\succcurlyeq 0,R\succ 0\text{ 且 }Q-N{R}^{-1}{N}^{\mathrm{T}}\succcurlyeq 0$，这个情况可以通过配方法转换成对角阵的形式[2]：

$$\begin{array}{rl}& {\left[\begin{array}{c}X\\ U\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{cc}Q& N\\ N^{\mathrm{T}}& R\end{array}\right]\left[\begin{array}{c}X\\ U\end{array}\right]\\ & =X^{\mathrm{T}}QX+2X^{\mathrm{T}}NU+U^{\mathrm{T}}RU\\ & =X^{\mathrm{T}}QX+U^{\mathrm{T}}RU+X^{\mathrm{T}}NU+U^{\mathrm{T}}{N}^{\mathrm{T}}X+X^{\mathrm{T}}N{R}^{-1}{N}^{\mathrm{T}}X-X^{\mathrm{T}}N{R}^{-1}{N}^{\mathrm{T}}X\\ & =X^{\mathrm{T}}\left(Q-N{R}^{-1}{N}^{\mathrm{T}}\right)X+{\left(U+R^{-1}{N}^{\mathrm{T}}X\right)}^{\mathrm{T}}R\left(U+R^{-1}{N}^{\mathrm{T}}X\right)\end{array}$$

令$$\{\begin{array}{l}\tilde{Q}=Q-Q_1{R}^{-1}{N}^{\mathrm{T}}\\ \tilde{U}=U+R^{-1}{N}^{\mathrm{T}}X\end{array}\text{\hspace{1em}(5)}$$

可把原系统化成新的形式
$$\dot{x}=Ax+B(\tilde{u}-R^{-1}{N}^{\mathrm{T}}x)=(A-B{R}^{-1}{N}^{\mathrm{T}})x+B\tilde{u}$$

这个新系统可以套用前面所述的方法，$(问题1\ast )$等价于求解$(问题1)$：
$$J=\frac{1}{2}{\mathbf{x}}^{\mathrm{T}}(t_f){\mathbf{S}}_f\mathbf{x}(t_f)+\frac{1}{2}{\int }_0^{t_f}\left(x^T\tilde{Q}x+{\tilde{u}}^{T}R\tilde{u}\right)\text{d}t$$
总结
把非线性系统在标称状态附近线性化，就可得到线性时变系统。在这个意义上，上面讨论的问题很宽泛。此外，问题还在于只能采用数值计算方法，要计算$\mathbf{x}(t),\lambda (t),S(t_0)$，以及反馈控制采用的是时变增益$K(t)$。在工程上使用更多的是线性定常系统、无穷时域的相关问题。也就是我们下面要讨论的LQR问题。

$t_f=\infty$的线性二次型调节器

线性系统（定常或时变）、二次型指标的状态调节器问题一般称为线性二次型调节器（Linear Quadratic Regulator，LQR），其任务通常为

• 有限时间内，把状态调节到接近0
• 无穷时域，使状态在干扰下保持在0附近，以上两种都是状态调节器问题
• 无穷时域，使状态保持在参考轨迹$x_{ref}(t)$附近，等价于使跟踪误差$\delta x=x-x_{ref}$维持在0附近，也就是状态跟踪器问题
• 输出调节器和输出跟踪器问题，都是增加了一项观测方程$\mathbf{y}=C\mathbf{x}$，控制的对象是$y(t)$

无限时域、线性定常系统的LQR问题是$(\text{问题}1)$的特殊形式，其中时变矩阵为常值矩阵$A(t)=A,B(t)=B,Q(t)=Q,R(t)=R$，且$S_f=0$，即
$$\begin{gathered} \dot{\mathbf{x}}=A\mathbf{x}+B\mathbf{u},\mathbf{x}(0)={\mathbf{x}}_{0}0\le t<\infty \\ \underset{u(t)=-Kx(t)}{\min }J=\frac{1}{2}{\int }_{t_0}^{\infty }\left[x(t{)}^{\mathrm{T}}Qx(t)+u(t{)}^{\mathrm{T}}Ru(t)\right]\text{d}t\text{\hspace{1em}(问题2)} \end{gathered}$$

采用线性反馈控制器. 控制回路如下图：

要满足线性反馈，即${\mathbf{u}}^{\ast }(t)=-K\mathbf{x}(t)$，考虑最优控制${\mathbf{u}}^{\ast }(t)=-{\mathbf{R}}^{-1}{\mathbf{B}}^{\mathrm{T}}\lambda (t)$，可以导出协态变量与状态变量成线性关系：$\lambda (t)=S^{\ast }\mathbf{x}(t)$，即公式$(1)$中的$S(t)=S^{\ast }⟹\dot{S}(t)=0$。考虑到矩阵Riccati方程的终端条件$S(t_f)=S_f=0$，可得稳态状态的Riccati方程：
$$0=A^{\mathrm{T}}S+SA-SB{R}^{-1}{B}^{\mathrm{T}}S+Q\text{\hspace{1em}(6)}$$

方程$(6)$中唯一需要求解的矩阵是$S=S^{\mathrm{T}}\succcurlyeq 0$。需要注意的是，虽然${\lim }_{t_f\to \infty }S(t_f)=0,\dot{S}(t)=0$，但似乎不能直接假设$S=0$。对这个矩阵的求解方法，可以参考文献[3]中的描述。知道它，就可以得到我们所关心的增益矩阵K：
$$K={\mathbf{R}}^{-1}{\mathbf{B}}^{\mathrm{T}}S\text{\hspace{1em}(7)}$$

此时闭环系统的状态方程为：
$$\dot{\mathbf{x}}=A\mathbf{x}+B\mathbf{u}=(A-B{R}^{-1}{B}^{\mathrm{T}}S)\mathbf{x}\text{\hspace{1em}(8)}$$
调用MATLAB中的函数$\text{[K,S,e] = LQR(A,B,Q,R,N)}$，我们的任务只有设置合适的权重$Q,R$，对于这一点，目前还只能靠经验。Q越大，相当于越强调状态迅速归零；对于轨迹跟踪控制，则跟踪误差越小。这种情况对控制的需求较大，控制带宽和饱和幅值较小的执行机构可能无法做出有效的响应。R越大，越强调最小能量消耗，即控制最少，此时状态误差可能在较大范围内抖动。这两者之间是一个协调关系。进一步的介绍可以参看知乎文章 - 21. LQR控制器— 线性二次型调节器 Linear Quadratic Regulator。

渐近稳定性证明

由以下两个常用的定理：
定理1：闭环系统$\dot{\mathbf{x}}=A_c\mathbf{x}$渐进稳定等价于对$\forall Q=Q^{\mathrm{T}}\succ 0$，Lyapunov方程$A_c^{\mathrm{T}}P+P{A}_c=-Q$存在唯一对称正定解$P$。
定理2：闭环系统$A_c$渐进稳定的充分条件是：存在正定的Lyapunov函数$V(\mathbf{x})$，沿闭环系统的状态方程满足$\frac{\mathrm{d}V}{\mathrm{d}t}<0$。

最优线性反馈的闭环系统满足公式$(8)$，闭环状态矩阵$A_c=A-B{R}^{-1}{B}^{\mathrm{T}}S$，则Lyapunov方程要求解正定矩阵P：
$$A_c^{\mathrm{T}}P+P{A}_c\prec 0$$

把Riccati方程两边同时减去$2SB{R}^{-1}{B}^{\mathrm{T}}S=2K^{\mathrm{T}}RK$，可得
$$\begin{array}{rl}{A}^{\mathrm{T}}S+SA+Q& =SB{R}^{-1}{B}^{\mathrm{T}}S\\ (A-B{R}^{-1}{B}^{\mathrm{T}}S{)}^{\mathrm{T}}S+S(A-B{R}^{-1}{B}^{\mathrm{T}}S)& =-SB{R}^{-1}{B}^{\mathrm{T}}S-Q\\ A_c^{\mathrm{T}}S+S{A}_c& <-Q\prec 0\end{array}$$

则闭环系统渐进稳定。

最优性证明

Riccati方程$(4)$和$(6)$是按照最优控制的必要条件推导得到的，可以证明它是唯一的充分条件。证明这一点可以运用连续系统最优性的充要条件，即求解连续系统动态规划法的$\text{Hamilton-Jacobi-Bellman}$方程可以证明，请参考文献[3]。

参考文献

[1]. Wikipedia En不能用。。。Linear-quadratic regulator-The free Dictionary
[2]. 邢继祥. 最优控制应用基础. 第四章 线性系统二次型最优控制[M]. 科学出版社, 2003.
[3]. Bryson A E , Ho Y C ,Applied optimal control : optimization, estimation, and control. Ch.5 Linear system with quadratic criteria: linear feedback[J]. IEEE Transactions on Systems Man & Cybernetics, 1975
[4]. 知乎专栏 - 21. LQR控制器— 线性二次型调节器 Linear Quadratic Regulator
[5]. 知乎专栏 - 最优控制理论（七）LQR伺服跟踪控制器设计