无人驾驶运动学模型——线性时变模型预测控制的公式推导

最近在学习龚建伟《无人驾驶车辆模型预测控制》里第三章的线性时变模型mpc控制，书里的公式没有推导的细节，而且理论公式跟附的代码对不上，估计公式都是抄现代控制理论的。网上找的好几个帖子的推导也跟书里代码对不上，光看公式不结合代码还是比较难完全理解mpc控制。我总结了下代码，给一个简单版的、能跟书上代码对的上的推导。
参考的资料有：线性时变模型预测控制的思路推演过程、运动学模型线性化、龚建伟《无人驾驶车辆模型预测控制》

运动学模型的线性化和离散化

运动学模型的推导参考《无人驾驶车辆模型预测控制》。
话不多说上手稿

几点说明：
1.下标${\ast }_f{\ast }_r$表示前轮front和后轮rear，同时${\ast }_r$也表示参考轨迹点上的量。
2.参考轨迹点附近泰勒展开要求无人车距离参考点比较近，实际常常会出现距离很远的情况，这种情况虽然误差很大，但只要算法还能够让车靠近参考点就不影响整个控制过程。
3.线性化离散化其实就是要把运动学模型写成公式10这种矩阵乘法的形式，然后在预测阶段我们就能利用这个公式快速计算预测轨迹了。
4.为了跟书上代码保持一致，这里的符号与网上的帖子会有些不一样。

预测和优化