LQR控制算法推导以及简单分析

首先，这篇文章是看了几个大神的博客后，自己抄录以及整理的内容，其中有些自己的想法，但是原理部分基本都是学习大神们的，在此先说明一下。

1 全状态反馈控制系统

在介绍LQR之前，首先，先回顾一下现在控制理论中的基本的控制器 —————— 全状态反馈控制，上图

假设有一个线性系统用状态向量表示：
$$\{\begin{array}{ll}\dot{x}=Ax+Bu& \\ y=Cx+Du& \end{array}\text{\hspace{1em}(1)}$$
其中，$x(t)\in R^n$，$u(t)\in R^m$，初始条件是$x(0)$。
在此，我们需要设计一个状态反馈控制器
$$u=-Kx\text{\hspace{1em}(2)}$$
使上述控制器能达到期望的稳定性能，将式(2)带入系统状态方程(1)中，有
$$\dot{x}=(A-BK)x=A_{c}x\text{\hspace{1em}(3)}$$
设定系统中的各个状态量都可知，式(1)所示的开环系统，传递函数的极点就是系统矩阵A的特征值。现在变换成了式(2)的闭环形式，通过配置反馈矩阵$K$，可以使得闭环系统达到所期望的系统状态。(注意，这种控制器设计与矩阵C、D没什么关系)
SO，来了一个新的问题，极点在什么样的位置会使得系统的性能较好呢？并且，当系统变量很多的时候，就算设计极点已经达到了最优，矩阵K的计算如何计算呢？
因此，LQR提供了如下思路。

2 LQR

LQR的目标就是找到一组控制量$u_0,u_1,...$，使得同时有$x_0,x_1,...$足够小(系统达到稳定状态)，$u_0,u_1,...$足够小(控制量尽量小的变化)，选取代价函数为
$$J=\frac{1}{2}{\int }_0^{\infty }{x}^{T}Qx+u^{T}Rudt\text{\hspace{1em}(4)}$$
其中，Q、R就是需要设计的半正定矩阵和正定矩阵。
看上式(4)是不是线代中的二次型(线代真的是一门重要的学科，工程中大量都是线代的运算)，而且还是那种只有平方项的二次型，这样就成了最小二乘法的问题。代价函数$J$需要达到最小值，那么在$t$趋近于无穷时，状态向量$x(t)$肯定趋近于0，即是达到了系统稳态；同理，$t$趋近于无穷时，控制向量$u(t)$也会趋近于0，意味着，随着时间的推移，需要对系统施加的控制量会越来越小，意味着使用最小的控制量使得系统达到了最终控制目标。

下面来聊聊Q、R值的选取，一般来说，选取Q、R矩阵的时候，为了方便观察各个系统状态量而选取对角阵，增加Q的一个值，意味着这个值作用的系统状态量，将以更快的速度衰减到0，这时候，举个栗子还是很必要的，比如，$Q_{11}$选取较大的值，会让$x_1$很快的衰减到0；另外一方面，加大$R$的值，会使得对应的控制量减小，控制器执行更少的动作，意味着系统的状态衰减将变慢。所以，$Q、R$的选取，要综合看具体的实际应用场景来调节，俗话说，鱼和熊掌不可兼得，这个矛盾就像做的轨迹跟踪MPC中的设置不同的权重一样，期望车辆后轴心cte值越小，那么到了转弯处必须要很大幅度的刹车，以及打方向盘，速度以及yaw都会大幅度变化，而如果期望执行器动作越小，那么就无法保证cte的准确度一样。

好了，上面介绍了一些参数的意义问题，下面重点就是公式的推导了，在不同大神那看到了不同的推导方式，下面就把自己比较欣赏的一种写出来吧，虽说过程也很重要，但是工程嘛，只要结果对，效果好，选个自己看起来舒服的方式就好了，开始推导过程

1. 将$u=-Kx$代入代价函数后，有
   $$J=\frac{1}{2}{\int }_0^{\infty }{x}^T(Q+K^{T}RK)xdt\text{\hspace{1em}(5)}$$
2. 假设纯在一个常量矩阵$P$使得，
   $$\frac{d}{dt}(x^{T}Px)=-x^T(Q+K^{T}RK)x\text{\hspace{1em}(6)}$$
3. 把式(6)代入(5)后，有
   $$J=-\frac{1}{2}{\int }_0^{\infty }{x}^T(P)xdt=\frac{1}{2}{x}^T(0)Px(0)\text{\hspace{1em}(7)}$$
   式(7)的意思就是，t趋近于无穷时，系统状态向量x(t)趋近于0，这样就直接结算出了积分方程。
4. 把式(6)左边微分展开，
   $${\dot{x}}^{T}Px+x^{T}P\dot{x}+x^{T}Qx+x^T{K}^{T}RKx=0$$
   状态变量x的微分用式(3)表示，
   $$x^T{A}_c^{T}Px+x^{T}P{A}_{c}x+x^{T}Qx+x^T{K}^{T}RKx=0$$
   整理后，有
   $$x^T(A_c^{T}P+P{A}_c+Q+K^{T}RK)x=0\text{\hspace{1em}(8)}$$
   这样，就又回到了二次型的问题，如果式(8)要有解，那么括号里面的部分必须等于0.
   $$A_c^{T}P+P{A}_c+Q+K^{T}RK=0\text{\hspace{1em}(9)}$$
   把$A_c=A-BK$代入式(9)
   $$(A-BK{)}^{T}P+P(A-BK)+Q+K^{T}RK=0\text{\hspace{1em}(10)}$$
   $$A^T+PA+Q+K^{T}RK-K^T{B}^{T}P-PBK=0\text{\hspace{1em}(11)}$$
5. 式(11)还是一个关于$K$的二次型，这样会导致计算量太复杂，so只要这个等式成立就好，那么这里令$K=R^{-1}{B}^{T}P$，然后式(11)，可以化为
   $$A^T+PA+Q+K^{T}R(R^{-1}{B}^{T}P)-K^T{B}^{T}P-PB(R^{-1}{B}^{T}P)=0$$
   $$A^T+PA+Q-PB{R}^{-1}{B}^{T}P=0\text{\hspace{1em}(12)}$$
   式(12)中，$A,B,Q,R$都是已知量，那么通过式(12)可以求解出$P$，式(12)就是著名的Riccati方程。

总结

上面，从理论以及公式推导两个方面，介绍了LQR，现在从头看一下LQR的思路：

• 选择参数矩阵Q,R
• 求解Riccati方程得到矩阵P
• 根据P计算$K=R^{-1}{B}^{T}P$
• 计算控制量$u=-Kx$

MPC与LQR比较

MPC和LQR两种控制方式有很多的相似之处，但是也有很多不相同的地方，

• 首先，LQR的研究对象是现代控制理论中的状态空间方程给出的线性系统，而MPC的研究对象可以是线性系统，也可以是非线性系统。不过现在很多的做法都是将非线性系统线性化，然后进行相关计算，具体要根据自己的工程情况来确定哪种方式比较好，比如之前做MPC的时候，线控车底层速度控制接口就是加速度，那就没必要根据IMU再套嵌个一层PID。
• 其次，既然是优化问题，那就离不开目标函数的设计，LQR的目标函数在上面已经有描述，MPC的目标函数，多数都是多个优化目标乘以不同权重然后求和的方式。虽然方式不同，不过都是对达到控制目标的代价累计。
• 最后，工作时域上的不同，LQR的计算针对同一工作时域，在一个控制周期内，LQR只计算一次，并将此次计算出的最优解下发给控制器即可；而MPC是滚动优化的，计算未来一段时间内，每个采样周期都会经过计算，得出一组控制序列，但是只将第一个控制值下发给控制器。

Reference：

1. F.L. Lewis .<< Linear Quadratic Regulator (LQR) State Feedback Design >>
2. https://blog.csdn.net/u013914471/article/details/84324754?depth_1-utm_source=distribute.pc_relevant.none-task&utm_source=distribute.pc_relevant.none-task
3. https://zhuanlan.zhihu.com/p/72605138
4. https://blog.csdn.net/heyijia0327/article/details/39270597