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

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

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

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

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

预测和优化

${\xi }_{k+1}、{\xi }_{k+2}...$就是预测的状态量。从a b到矩阵A B的计算过程有点小窍门，最好能自己推到一遍。

几点说明：
1.优化目标函数的意义就是预测的轨迹点要跟参考轨迹点的差要尽量小、预测的控制量跟参考轨迹点处的控制量的差要尽量小。
2.看完公式推导后最好对照着代码再理解一遍。

最后是matlab代码中变量跟公式的对应关系。
$x\_piao(k,:)={\xi }_k={\tilde{X}}_k=X_k-X_r$ 表示状态量和参考轨迹点上状态量的差
$u\_piao(k,:)={\tilde{u}}_k=u_k-u_r$ 表示控制量和参考轨迹点上控制量的差。

网上很多推导都牵扯到$\Delta u$，还把状态量和控制量进行组合，其实这是把问题复杂化了。直接参照代码推导，不考虑松弛因子，整个推导还是比较好理解的。