Autoware的MPC源码解析（三）mpc_follower解析：车辆模型介绍

版权声明：本文为博主原创文章，未经博主允许不得转载。
源码地址：https://gitlab.com/autowarefoundation/autoware.ai/autoware/wikis/Source-Build
如有错误或建议，欢迎提出。

在Autoware的mpc源码中使用了三种车辆模型，其中车辆动力学误差模型可参考此博文https://blog.csdn.net/u013914471/article/details/83018664
因此本文主要对两种运动学模型进行介绍。

• 1车辆运动学自行车误差模型（no delay）
  首先根据自行车运动学模型如下如所示：
  其中$\delta$为车辆转向角，R为转弯半径，L为车辆轴距。则他们的关系式如下式所示：
  $$tan(\delta )=L/R$$

之后定义e为横向误差，$\dot{e}$为横向误差率，th为航向角误差，$\dot{th}$为航向角误差率，v为车速，${\delta }_r$为参考转向角。
则：
$$\dot{e}=v\ast \sin (th)（1）$$$$\dot{th}=v/L\ast \tan (\delta )-v/L\ast \tan ({\delta }_r)（2）$$
其中式（2）可根据小角度理论以及三角函数关系化简成如下形式：
$$\dot{th}=v/L\ast \delta /{\cos }^2({\delta }_r)-v/L\ast {\delta }_r/{\cos }^2({\delta }_r)（3）$$
则根据式（1）和式（3）可得到车辆运动学自行车误差模型如下所示：
$$\left[\begin{array}{c}\dot{e}\\ \dot{th}\end{array}\right]=\left[\begin{array}{cc}0& v\\ 0& 0\end{array}\right]\left[\begin{array}{c}e\\ th\end{array}\right]+\left[\begin{array}{c}0\\ v/L/{\cos }^2({\delta }_r)\end{array}\right]\delta +\left[\begin{array}{c}0\\ -v\ast {\delta }_r/L/{\cos }^2({\delta }_r)\end{array}\right]$$
其中，如果车辆行驶在参考路径上，则${\delta }_r=\delta$。

• 2车辆运动学自行车误差模型（考虑转向角变化率）
  第二种运动学模型在第一种模型的基础上考虑了转向角的变化率。定义t为循环周期，$\dot{\delta }$为转向角变化率，则：
  $$\dot{\delta }=\Delta \delta /t$$
  结合第一种运动学模型可得：
  $$\left[\begin{array}{c}\dot{e}\\ \dot{th}\\ \dot{\delta }\end{array}\right]=\left[\begin{array}{ccc}0& v& 0\\ 0& 0& v/L/{\cos }^2({\delta }_r)\\ 0& 0& -1/t\end{array}\right]\left[\begin{array}{c}e\\ th\\ \delta \end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 1/t\end{array}\right]{\delta }_d+\left[\begin{array}{c}0\\ -v\ast {\delta }_r/L/{\cos }^2({\delta }_r)\\ 0\end{array}\right]$$
  其中，${\delta }_d$为模型的输入——目标转向角度。