离散LQR理论推导

离散LQR

工程中用的大部分控制分为两种，无模型和基于模型的。无模型的主要是PID，基于模型的主要是LQR和MPC。PID的思想很直观不涉及推导，而LQR和MPC设计到一些具体数学公式的推导。这篇博客记录一下LQR的原理和推导过程，之后学到MPC了再详细展开记录。

首先限定讨论的范围，LQR（Linear Quadratic Regulation，线性二次调节器），针对的是线性时不变系统，本质上是一种状态反馈控制，最终目的是求反馈增益$K$。此外，工程中应用中计算机控制是离散的，因此本篇博客讨论离散系统的LQR。

基于模型的控制，首先需要建立被控对象的模型。这里我们假设模型已经建立，状态矩阵和输入矩阵分别为$A$和$B$，系统离散状态空间方程为：
$$x(k+1)=Ax(k)+Bu(k)$$

LQR的思想是建立一个代价函数$J$，找到某个控制律$u$，使得代价函数最小，这里建立代价函数为：
$$J_{0\to N}=\frac{1}{2}{x}_N^{T}S{x}_N+\frac{1}{2}\sum _{k=0}^{N-1}\left(x_k^{T}Q{x}_k+u_k^{T}R{u}_k\right)$$
式中，$N$为终端时刻，$S$表示终端权重矩阵，即最终时刻系统状态的权重，$Q$和$R$分别表示权重矩阵，这两个矩阵正定，赋予系统状态$x$和系统输入$u$中元素不同的权重，这些代价都是自行定义的参数。这个代价函数表示系统从初始$0$时刻到最终$N$时刻，状态和输入的加权平方和。

公式推导

先给出最终推导得到的形式：
$$\begin{array}{rl}& P_k=Q+A^T{P}_{k-1}A-A^{T}PB(B^T{P}_{k-1}B+R{)}^{-1}{B}^T{P}_{k-1}A\\ & K_{N-k}=(B^T{P}_{k-1}B+R{)}^{-1}{B}^T{P}_{k-1}A\\ & u_{N-k}^{\ast }=-K_{N-k}{x}_{N-k}\end{array}$$
式中，$k$表示从$N$时刻开始往前的步数，$K$表示反馈增益，$P$表示一个对称正定阵，$u^{\ast }$表示最优控制输入。

在推导的过程当中，我们需要时刻牢记推导的目标是找到最优的控制输入$u$，使代价函数$J$最小。这里的推导主要参考了b站DR_CAN的视频中反向迭代求解的方法。求解思路就是从最终时刻往前推，每一个迭代步骤的计算都需要用到上一步算出来的结果。

计算终端N时刻代价

首先讨论$N$到$N$时刻，代价函数可以表示为：
$$J_{N\to N}=\frac{1}{2}{x}_N^{T}S{x}_N+\frac{1}{2}\sum _{k=N}^{N-1}\left(x_k^{T}Q{x}_k+u_k^{T}R{u}_k\right)$$

终端时刻，加号后面的一项为$0$，因为只有终端一个状态，那么其实终端时刻的代价与控制输入无关。这里我们令$S=P_0$，方便后续说明，终端代价为：
$$J_{N\to N}^{\ast }=\frac{1}{2}{x}_N^T{P}_0{x}_N$$

计算N-1时刻最优代价

接下来讨论$N-1$到$N$时刻，代价函数为：
$$J_{N-1\to N}=\frac{1}{2}{x}_N^T{P}_0{x}_N+\frac{1}{2}\left(x_{N-1}^{T}Q{x}_{N-1}+u_{N-1}^{T}R{u}_{N-1}\right)$$

根据状态空间方程，其中$x_N=A{x}_{N-1}+B{u}_{N-1}$。

为了找到$u_{N-1}^{\ast }$使$J_{N-1\to N}$最小，首先求$J$关于$u$的偏导为$0$的极值点，然后求二阶偏导大于0验证该极值点为极小值，即进行这两步：
$$\frac{\partial J_{N-1\to N}}{\partial u_{N-1}}=0,\frac{{\partial }^2{J}_{N-1\to N}}{\partial u_{N-1}^2}>0$$

这里需要用到矩阵求导和链式求导法则，（矩阵求导可以参考这篇博客，矩阵求导的公式为：
$$\frac{\partial Ax}{\partial x}=A^T,\frac{\partial x^{T}Px}{\partial x}=Px$$
$$\frac{\partial \frac{1}{2}{x}_N^T{P}_0{X}_N}{\partial u_{N-1}}=\frac{1}{2}\frac{\partial x_N}{\partial u_{N-1}}\ast \frac{\partial x_N^T{P}_0{x}_N}{\partial x_N}=B^T{P}_0{x}_N$$
求偏导过程为：
$$\frac{\partial \frac{1}{2}\left(x_{N-1}^{T}Q{x}_{N-1}+u_{N-1}^{T}R{u}_{N-1}\right)}{\partial u_{N-1}}=R{u}_{N-1}$$

$$\begin{array}{rl}\frac{\partial J_{N-1\to N}}{\partial u_{N-1}}& =B^T{P}_0\left(A{x}_{N-1}+B{u}_{N-1}\right)+R{u}_{N-1}\\ & =B^T{P}_{0}A{x}_{N-1}+(B^T{P}_{0}B+R)u_{N-1}\end{array}$$

令$\frac{\partial J_{N-1\to N}}{\partial u_{N-1}}=0$，可以找到的极值点。
$$u_{N-1}^{\ast }=-(B^T{P}_{0}B+R{)}^{-1}{B}^T{P}_{0}A{x}_{N-1}$$
接下来求二阶偏导验证此时的极值点为极小值（求二阶偏导验证>0）。
$$\frac{{\partial }^2{J}_{N-1\to N}}{\partial u_{N-1}^2}=\frac{\partial \frac{\partial J_{N-1\to N}}{\partial u_{N-1}}}{\partial u_{N-1}}=(B^T{P}_{0}B+R^T)$$
其中，二次型$B^T{P}_{0}B>0$，权重矩阵$R>0$，因此$\frac{{\partial }^2{J}_{N-1\to N}}{\partial u_{N-1}^2}>0$，上面求的极值点为极小值点。

若令其中的$(B^T{P}_{0}B+R{)}^{-1}{B}^T=K_{N-1}$，那么$u_{N-1}^{\ast }$可以写为：
$$u_{N-1}^{\ast }=-K_{N-1}{x}_{N-1}$$
此时的输入量已经是一个状态反馈$u=-Kx$的形式。接下来把把$u_{N-1}^{\ast }$代入$J_{N-1\to N}$，可得代价函数的最优形式：
$$J_{N-1\to N}^{\ast }=\frac{1}{2}{x}_{N-1}^T\left[(A-B{K}_{N-1}{)}^T{P}_0(A-B{K}_{N-1})+K_{N-1}^{T}R{K}_{N-1}+Q\right]x_{N-1}$$

这个二次型中间的矩阵变量其实都已经通过上面步骤推导得到了，那么可以令$P_1=(A-B{K}_{N-1}{)}^T{P}_0(A-B{K}_{N-1})+K_{N-1}^{T}R{K}_{N-1}+Q$。那么$N-1$到$N$时刻的代价函数可以写为：
$$J_{N-1\to N}^{\ast }=\frac{1}{2}{x}_{N-1}^T{P}_1{x}_{N-1}$$
此时与$N\to N$时刻的代价函数形式一致：
$$J_{N\to N}^{\ast }=\frac{1}{2}{x}_N^T{P}_0{x}_N$$

那么由此可以迭代推导，$(N-2)\to N$时刻的代价函数的最优形式为：
$$J_{N-2\to N}^{\ast }=\frac{1}{2}{x}_{N-2}^T{P}_2{x}_{N-2}$$

归纳递推

那么$(N-k)\to N$时刻的代价函数最优形式为：
$$J_{N-k\to N}^{\ast }=\frac{1}{2}{x}_{N-k}^T{P}_k{x}_{N-k}$$

其中
$$\begin{array}{rl}& P_k=(A-B{K}_{N-k}{)}^T{P}_{k-1}(A-B{K}_{N-k})+K_{N-k}^{T}R{K}_{N-k}+Q\\ & K_{N-k}=(B^T{P}_{k-1}B+R{)}^{-1}{B}^T{P}_{k-1}A\end{array}$$
由于$P$和$R$都是对称正定，因此$((B^T{P}_{k-1}B+R{)}^{-1}{)}^T=(B^T{P}_{k-1}B+R{)}^{-1}$。把上面的$K_{N-k}$代入到$P_k$中可以得到$P_k$的迭代形式：
$$P_k=Q+A^T{P}_{k-1}A-A^T{P}_{k-1}B(B^T{P}_{k-1}B+R{)}^{-1}{B}^T{P}_{k-1}A$$

至此，LQR公式推导完毕。

应用demo

LQR实现轨迹跟踪的demo可以参考这篇博客。