之前有写过MPC的控制算法，主要介绍的也是理论部分，在实际使用过程中发现C++没有高效的优化方法，类似python中的cvxpy的库，所以想绕过去，研究一下LQR控制算法。
1. PID控制系统
这里并不是要介绍PID系统，而是为了方便理解LQR，首先我们看PID系统中的P调节方法：
假设我们现在状态是x0，我们有状态方程 : 
（u为控制矩阵）
再假设有一个反馈控制器： 
这里很重要，可以认为是当前的控制量是通过当前的状态量计算出来的。
通过这套方法，我们就能得到一个稳定的系统 ： 
当然这是基本的理论，再进一步，我们就会思考，通过这些控制量作为自变量，再设计一个代价函数，来优化这些控制量？
2. LQR控制算法
讲到这里就很自然引出LQR了，首先的问题代价函数是什么？这里我直接给出：
其中x为状态量，u为控制量，Q为状态权重矩阵，R为控制权重矩阵。
特别的x和u中取值有正有负，所以需要平方和最小，在矩阵中没有平方，这里我们采用转置乘以本身的做法模拟矩阵的平方，如x^T*x 。这里状态量x和控制量u都是多维向量，上式计算的结果是一个标量。
其实我们可以把
看作是
的多维扩展表达式，这里我们需要Q为半正定，就是希望Q能起到a≥0的效果，R为正定矩阵就是希望矩阵R能够起到a>0的效果。
一般的我们认为状态量x为：横向偏差、横向变化率、角度偏差、角度变化率。Q为我们提前标定的对角矩阵，标定值对应以上不同维度的权重，也可以为非对角矩阵，考虑不同维度之间的相互关系。
控制向量u为：前轮转角、加速度。同样的R也为提前标定的对角矩阵。也可以考虑相互关系。
我的理解是Q11选取较大的值，会让x1很快到0；另外一方面，加大R的值，会使得对应的控制量减小，控制器执行更少的动作，意味着系统的状态衰减将变慢。所以要综合看具体的实际应用场景来调节，鱼和熊掌不可兼得。建议在不同场景下采用不同的参数。
3. 公式求解
有了上面的优化目标后，就是如何求解最优的控制量了。下面我们先进行一些转换：
1. 将u=−Kx代入代价函数后： 
，u=−Kx即我们认为当前的控制量可以通过当前的状态量计算出来。如果我们能计算出-K就很简单了，可以直接套出控制量。
2. 假设纯在一个常量矩阵P使得， 
，这里就直接这么假设，接着往下看。
3. 把上式代入得到： 
4. 把2中的方程微分展开：
5. 状态变量x的微分用式
表示： 
整理得到：
通过矩阵得知上式要想有解只有中间部分为零，即：
6. 把
代入上式：
整理得到： 
7. 这里我们再令 
这里需要思考一下，为什么可以这样假设，凑出结果，//Todo
代入：
整理得到： 
，得到这一步就很简单了，式中A、B、Q、R都是已知量，很容易就能计算出P的结果。
8. 计算出P再通过就可以计算出K，也就是达到了我们一开始的目标，轻松得到当前的控制量。
4. MPC与LQR比较
首先，LQR的研究对象是现代控制理论中的状态空间方程给出的线性系统，而MPC的研究对象可以是线性系统，也可以是非线性系统。不过现在很多的做法都是将非线性系统线性化，然后进行相关计算，具体要根据自己的工程情况来确定哪种方式比较好，比如之前做MPC的时候，线控车底层速度控制接口就是加速度，那就没必要根据IMU再套嵌个一层PID。
其次，既然是优化问题，那就离不开目标函数的设计，LQR的目标函数在上面已经有描述，MPC的目标函数，多数都是多个优化目标乘以不同权重然后求和的方式。虽然方式不同，不过都是对达到控制目标的代价累计。
最后，工作时域上的不同，LQR的计算针对同一工作时域，在一个控制周期内，LQR只计算一次，并将此次计算出的最优解下发给控制器即可；而MPC是滚动优化的，计算未来一段时间内，每个采样周期都会经过计算，得出一组控制序列，但是只将第一个控制值下发给控制器。