基于运动学的横向控制2

        谈到控制就离不开模型预测MPC，直观一点的意思就是在当下这个时刻我其实可以预测以后每个时刻的状态，从而决定未来到底给多少控制。我们可以定义二个函数，一个是预测的未来状态和期望的未来状态的距离，另一个是输入变化量的模长，显然，我们希望优化关于未来输入的损失函数，使得这两项最小，即预测状态最贴近期望状态，并且输入不会变化地太剧烈。与PID控制不同，可以明显看出MPC计算输入的过程，本质上是一个优化问题，既然是优化问题，那么就可以加上约束，这是PID做不到的。

车辆的运动模型上文已经给出，。一个标准的空间状态方程可以定义为 , X代表状态矩阵，U代表控制矩阵，我们对车辆的速度V以及车轮转角感兴趣，所以这个空间状态方程可以入下图所写：

 很明显发现这个非线性，根本找不到适合的A,B矩阵来构建空间状态方程。非线性的线性化往往要用泰勒公式去展开，假设在（Xr,Ur）已知，那么在这个点关于控制量和状态量的偏导就可以求出来 ：

 那么两个式子相减，其实就可以得到一个新的线性化的空间状态方程：

 这个就是基于运动学的空间状态方程，那么拿到这个我们应该怎么处理呢，采用牛顿法进行矩阵离散化处理，就可以得到K+1时刻的方程：

 都这里完成一半目标，下面要讨论最初的问题，希望用有限的能量去让系统更快的趋近于稳定，所以最好让空间状态方程变成 增量控制的形式。重新定义空间状态输入

 

暂时到这里，推不动了。