参考：路径规划与轨迹跟踪系列算法学习_第9讲_车辆运动学和动力学模型

1 车辆运动学模型和动力学模型概述

要控制车辆的运动，首先要对车辆的运动建立数字化模型，模型建立的越准确，对车辆运动的描述越准确，对车辆的跟踪控制的效果就越好。除了真实反映车辆特性外，建立的模型也应该尽可能的简单易用。车辆模型一般分为运动学和动力学模型。

1.1 车辆运动学模型

车辆运动学模型(Kinematic Model)把车辆完全视为刚体，主要考虑车辆的位姿$(x,y,\theta )$、速度$v$、前轮转角$\delta$等的关系，不考虑任何力的影响

1.2 车辆动力学模型

车辆动力学模型(Dynamic Model)则需要考虑车辆和地面之间的力的影响，包括轮胎侧偏现象等。

在建立两种模型时，思路都是通过建立状态空间方程，以便于输入控制量得到理想的状态值；并且一般都建立为基于误差（位置误差、航向误差等）的状态空间方程。

2 车辆运动学模型

车辆运动学模型常采用自行车模型(Bicycle Model)， 基于如下假设：

• 不考虑车辆在垂直方向(Z轴方向)的运动，即假设车辆的运动是一个二维平面上的运动。
• 假设车辆左右侧轮胎在任意时刻都拥有相同的转向角度和转速；这样车辆的左右两个轮胎的运动可以合并为一个轮胎来描述。
• 假设车辆行驶速度变化缓慢，忽略前后轴载荷的转移。
• 假设车身和悬架系统都是刚性系统。
• 假设车辆的运动和转向是由前轮驱动(front−wheel−only)的。

车辆运动学方程：
$$\{\begin{array}{l}\dot{x}=v_x=v\cos \phi \\ \dot{y}=v_y=v\sin \phi \\ \dot{\phi }=\frac{v\tan \delta }{l}\end{array}$$
=>
$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\phi }\end{array}\right]=\left[\begin{array}{c}v\cos \phi \\ v\sin \phi \\ \frac{v\tan \delta }{l}\end{array}\right]=\left[\begin{array}{l}{f}_1\\ f_2\\ f_3\end{array}\right]$$

选取状态量为$\mathbf{\chi }=[x,y,\phi {]}^T$，控制量为$\mathbf{u}=[v,\delta {]}^T$，则对于参考轨迹的任意一个参考点，用$r$表示，上式改为：
$$\dot{\mathbf{\chi }}=f(\mathbf{\chi },\mathbf{u})\Rightarrow {\dot{\mathbf{\chi }}}_r=f\left({\mathbf{\chi }}_r,{\mathbf{u}}_r\right)$$
其中，${\mathbf{\chi }}_{\mathbf{r}}=[x_r,y_r,{\phi }_r{]}^T$，${\mathbf{u}}_{\mathbf{r}}=[v_r,{\delta }_r{]}^T$。对上式在参考点采用泰勒级数展开，并忽略高阶项：
$$\dot{\chi }=f\left({\mathbf{\chi }}_r,{\mathbf{u}}_r\right)+\frac{\partial f(\mathbf{\chi },\mathbf{u})}{\partial \mathbf{\chi }}\left(\mathbf{\chi }-{\mathbf{\chi }}_r\right)+\frac{\partial f(\mathbf{\chi },\mathbf{u})}{\partial \mathbf{u}}\left(\mathbf{u}-{\mathbf{u}}_r\right)$$
求雅可比矩阵：
$$\begin{array}{l}\frac{\partial f(\chi ,\mathbf{u})}{\partial \chi }=\left[\begin{array}{lll}\frac{\partial f_1}{\partial x}& \frac{\partial f_1}{\partial y}& \frac{\partial f_1}{\partial \phi }\\ \frac{\partial f_2}{\partial x}& \frac{\partial f_2}{\partial y}& \frac{\partial f_2}{\partial \phi }\\ \frac{\partial f_3}{\partial x}& \frac{\partial f_3}{\partial y}& \frac{\partial f_3}{\partial \phi }\end{array}\right]=\left[\begin{array}{ccc}0& 0& -v_r\sin {\phi }_r\\ 0& 0& v_r\cos {\phi }_r\\ 0& 0& 0\end{array}\right]\\ \frac{\partial f(\mathbf{\chi },\mathbf{u})}{\partial \mathbf{u}}=\left[\begin{array}{ll}\frac{\partial f_1}{\partial v}& \frac{\partial f_1}{\partial \delta }\\ \frac{\partial f_2}{\partial v}& \frac{\partial f_2}{\partial \delta }\\ \frac{\partial f_3}{\partial v}& \frac{\partial f_3}{\partial \delta }\end{array}\right]=\left[\begin{array}{cc}\cos {\phi }_r& 0\\ \sin {\phi }_r& 0\\ \frac{\tan {\phi }_r}{l}& \frac{v_r}{l{\cos }^2{\delta }_r}\end{array}\right]\end{array}$$
则状态量误差的变化量$\dot{\tilde{\chi }}$表示为：
$$\dot{\tilde{\chi }}=\left[\begin{array}{c}\dot{x}-{\dot{x}}_r\\ \dot{y}-{\dot{y}}_r\\ \dot{\phi }-{\dot{\phi }}_r\end{array}\right]=\left[\begin{array}{ccc}0& 0& -v_r\sin {\phi }_r\\ 0& 0& v_r\cos {\phi }_r\\ 0& 0& 0\end{array}\right]\left[\begin{array}{l}x-x_r\\ y-y_r\\ \phi -{\phi }_r\end{array}\right]+\left[\begin{array}{cc}\cos {\phi }_r& 0\\ \sin {\phi }_r& 0\\ \frac{\tan {\phi }_r}{l}& \frac{v_r}{l{\cos }^2{\delta }_r}\end{array}\right]\left[\begin{array}{l}v-v_r\\ \delta -{\delta }_r\end{array}\right]\Rightarrow \dot{\tilde{\chi }}=A\tilde{\chi }+B\tilde{\mathbf{u}}$$
上式表明状态误差量可以构成线性状态空间。对上式进行前向欧拉离散化，得到:
$$\dot{\tilde{\chi }}=\frac{\tilde{\chi }(k+1)-\tilde{\chi }(k)}{T}=A\tilde{\chi }+B\tilde{\mathbf{u}}$$
综上得：
$$\tilde{\chi }(k+1)=(TA+E)\tilde{\chi }(k)+TB\tilde{\mathbf{u}}(k)=\left[\begin{array}{ccc}1& 0& -T{v}_r\sin {\phi }_r\\ 0& 1& T{v}_r\cos {\phi }_r\\ 0& 0& 1\end{array}\right]\tilde{\chi }(k)+\left[\begin{array}{cc}T\cos {\phi }_r& 0\\ T\sin {\phi }_r& 0\\ T\frac{\tan {\phi }_r}{l}& T\frac{v_r}{l{\cos }^2{\delta }_r}\end{array}\right]\tilde{\mathbf{u}}(k)=\tilde{A}\tilde{\chi }(k)+\tilde{B}\tilde{\mathbf{u}}(k)$$

3 车辆动力学模型

车辆动力学模型根据自由度可以划分为：

• 二自由度模型：仅包括车辆侧向与横摆两个自由度。
• 七自由度模型:包括车身纵向位移、横向位移和横摆角速度与四个车轮的回转运动。
• 十一自由度模型:包括车辆纵向运动、车辆侧向运动、整车横摆、车身的俯仰、四个车轮的转动以及前轮转角。

根据受力方向又分为横向动力学和纵向动力学，一般两者解耦之后研究：

• 纵向上，通过控制轮胎转速实现速度跟踪；
• 横向上，通过控制前轮转角实现路径跟踪。

在转向小角度及轮胎动力学的基础上，研究车辆线性二自由度动力学模型。

详细推导

车身𝑦轴方向应用牛顿第二定律可得：
$$m{a}_y=F_{yf}+F_{yr}$$
其中$a_y$是车辆质心处的横向加速度，$F_{yf},F_{yr}$为地面给前轮胎和后轮施加的横向力。

横向加速度$ay$由两部分组成：车辆沿车身𝑦轴横向运动产生的加速度$\ddot{y}$，车身横摆运动产生的向心加速度$v_x\dot{\phi }$，因此$m\left(\ddot{y}+v_x\dot{\phi }\right)=F_{yf}+F_{yr}$，然后车辆绕𝑧轴的转矩平衡方程为$I_z\ddot{\phi }=F_{yf}{l}_f-F_{yr}{l}_r$

对于轮胎受到的两个横向力，考虑到小侧偏角的假设，则地面施加给轮胎的横向力(侧偏力)与轮胎侧偏角成线性关系(侧偏角是车轮速度方向与车轮纵轴的夹角)：前轮侧偏角为：${\alpha }_f=\delta -{\theta }_{vf}$，其中${\theta }_{vf}$是前轮胎速度方向与车身纵轴的夹角，$\delta$为前轮转向角。

后轮的侧偏角为：${\alpha }_r=-{\theta }_{vr}$，因此前轮和后轮的横向轮胎力为:
$$\{\begin{array}{l}{F}_{yf}=2C_{\alpha f}\left(\delta -{\theta }_{vf}\right)\\ F_{yr}=-2C_{\alpha r}{\theta }_{vr}\end{array}$$
将车辆质心到车轮这一部分视为刚体，则根据刚体运动学有：
$$\{\begin{array}{l}\tan {\theta }_{vf}=\frac{v_y+\dot{\phi }{l}_f}{v_x}\\ \tan {\theta }_{vr}=\frac{v_y-\dot{\phi }{l}_r}{v_x}\end{array}\Rightarrow \{\begin{array}{l}{\theta }_{vf}=\frac{v_y+\dot{\phi }{l}_f}{v_x}\\ {\theta }_{vr}=\frac{v_y-\dot{\phi }{l}_r}{v_x}\end{array}$$

以横向位置、横向位置变化率、横摆角、横摆角变化率作为状态量
横向位置变化率：
$$\begin{array}{l}\frac{d\dot{y}}{dt}=\ddot{y}=\frac{F_{yf}+F_{yr}}{m}-v_x\dot{\phi }=\frac{2C_{\alpha f}\left(\delta -{\theta }_{vf}\right)-2C_{\alpha r}{\theta }_{vr}}{m}-v_x\dot{\phi }=\frac{2C_{\alpha f}\delta }{m}-\frac{2C_{\alpha f}{\theta }_{vf}+2C_{\alpha r}{\theta }_{vr}}{m}-v_x\dot{\phi }\\ =-\frac{2C_{\alpha f}\left(\frac{v_y+\dot{\phi }{l}_f}{v_x}\right)+2C_{\alpha r}\left(\frac{v_y-\dot{\phi }{l}_r}{v_x}\right)}{m}-v_x\dot{\phi }+\frac{2C_{\alpha f}\delta }{m}=-\frac{\left(2C_{\alpha f}+2C_{\alpha r}\right)v_y+\left(2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r\right)\dot{\phi }}{m{v}_x}-v_x\dot{\phi }+\frac{2C_{\alpha f}\delta }{m}\\ =-\frac{2C_{\alpha f}+2C_{\alpha r}}{m{v}_x}\dot{y}+\left(-\frac{2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r}{m{v}_x}-v_x\right)\dot{\phi }+\frac{2C_{\alpha f}}{m}\delta \end{array}$$
横摆角变化率：
$$\begin{array}{l}\frac{d\dot{\phi }}{dt}=\ddot{\phi }=\frac{F_{yf}{l}_f-F_{yr}{l}_r}{I_z}=\frac{2C_{\alpha f}\left(\delta -{\theta }_{vf}\right)l_f-2C_{\alpha r}{\theta }_{vr}{l}_r}{I_z}=\frac{2C_{\alpha r}{l}_r{\theta }_{vr}-2C_{\alpha f}{l}_f{\theta }_{vf}}{I_z}+\frac{2C_{\alpha f}{l}_f\delta }{I_z}\\ =\frac{2C_{\alpha r}{l}_r\left(\frac{v_y-\dot{\phi }{l}_r}{v_x}\right)-2C_{\alpha f}{l}_f\left(\frac{v_y+\dot{\phi }{l}_f}{v_x}\right)}{I_z}+\frac{2C_{\alpha f}{l}_f\delta }{I_z}=\frac{\left(2C_{\alpha r}{l}_r-2C_{\alpha f}{l}_f\right)v_y-\left(2C_{\alpha r}{l}_r^2+2C_{\alpha f}{l}_f^2\right)\dot{\phi }}{I_z{v}_x}+\frac{2C_{\alpha f}{l}_f\delta }{I_z}\\ =\frac{2C_{\alpha r}{l}_r-2C_{\alpha f}{l}_f}{I_z{v}_x}\dot{y}+\left(-\frac{2C_{\alpha r}{l}_r^2+2C_{\alpha f}{l}_f^2}{I_z{v}_x}\right)\dot{\phi }+\frac{2C_{\alpha f}{l}_f}{I_z}\delta \end{array}$$
车辆横向动力学模型状态空间方程
$$\frac{d}{dt}\left[\begin{array}{c}y\\ \dot{y}\\ \phi \\ \dot{\phi }\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& -\frac{2C_{\alpha f}+2C_{\alpha r}}{m{v}_x}& 0& -v_x-\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{m{v}_x}\\ 0& 0& 0& 1\\ 0& \frac{2C_{\alpha r}{l}_r-2C_{\alpha f}{l}_f}{I_z{v}_x}& 0& -\frac{2l_f^2{C}_{\alpha f}+2l_r^2{C}_{\alpha r}}{I_z{v}_x}\end{array}\right]\left[\begin{array}{c}y\\ \dot{y}\\ \phi \\ \dot{\phi }\end{array}\right]+\left[\begin{array}{c}0\\ \frac{2C_{\alpha f}}{m}\\ 0\\ \frac{2l_f{C}_{\alpha f}}{I_z}\end{array}\right]$$

根据系统状态方程之后就可以分析出在给定的前轮转角输入下，车辆的横向位移、横向速度、横摆角以及横摆角速度的响应，但是横向跟踪控制的目的是为了减小跟踪偏差，需要的状态方程是能够分析在给定的前轮转角下车辆跟踪偏差的响应。

• 车辆期望横摆角速度和横向加速度分别为:
  $$\{\begin{array}{l}{\dot{\phi }}_{des}=\frac{v_x}{R}\\ a_{y,des}=\frac{v_x^2}{R}=v_x{\dot{\phi }}_{des}\end{array}$$
• 横摆角偏差和横摆角偏差变化率为:
  $$\{\begin{array}{l}\tilde{\phi }=\phi -{\phi }_{des}\\ \dot{\tilde{\phi }}=\dot{\phi }-{\dot{\phi }}_{des}\end{array}$$
• 横向加速度偏差：
  $$\begin{array}{l}{\tilde{a}}_y=a_y-a_{y,des}=\left(\ddot{y}+v_x\dot{\phi }\right)-v_x{\dot{\phi }}_{\text{des }}\\ =\ddot{y}+v_x\left(\dot{\phi }-{\dot{\phi }}_{des}\right)=\ddot{y}+v_x\dot{\tilde{\phi }}\end{array}$$
• 对上式积分，即为横向速度偏差${\tilde{v}}_y$，或称为横向位置偏差变化率$\tilde{\dot{y}}$
  $${\tilde{v}}_y=\dot{\tilde{y}}=\dot{y}+v_x\left(\phi -{\phi }_{\text{des }}\right)=\dot{y}+v_x\tilde{\phi }$$

以横向位置误差、横向位置误差变化率、横摆角误差、横摆角误差变化率作为状态量

• 横向位置误差变化率：
  $$\begin{array}{l}\frac{d\dot{\tilde{y}}}{dt}={\tilde{a}}_y=\ddot{y}+v_x\left(\dot{\phi }-{\dot{\phi }}_{des}\right)=-\frac{\left(2C_{\alpha f}+2C_{\alpha r}\right)}{m{v}_x}\dot{y}+\left[-\frac{\left(2C_{\alpha f}{l}_f-2C_{\alpha x}{l}_r\right)}{m{v}_x}-v_x\right]\dot{\phi }+\frac{2C_{\alpha f}}{m}\delta +v_x\left(\dot{\phi }-{\dot{\phi }}_{des}\right)\\ =-\frac{2C_{\alpha f}+2C_{\alpha r}}{m{v}_x}\left(\dot{\tilde{y}}-v_x\tilde{\phi }\right)+\left(-\frac{2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r}{m{v}_x}\right)\left(\dot{\tilde{\phi }}+{\dot{\phi }}_{des}\right)+\frac{2C_{\alpha f}}{m}\delta -v_x{\dot{\phi }}_{\text{des }}\\ =-\frac{2C_{\alpha f}+2C_{\alpha r}}{m{v}_x}\dot{\tilde{y}}+\frac{2C_{\alpha f}+2C_{\alpha r}}{m}\tilde{\phi }-\frac{2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r}{m{v}_x}\dot{\tilde{\phi }}+\frac{2C_{\alpha f}}{m}\delta -\left(\frac{2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r}{m{v}_x}+v_x\right){\dot{\phi }}_{des}\end{array}$$
• 横摆角误差变化率：
  $$\begin{array}{l}\frac{d\dot{\tilde{\phi }}}{dt}=\ddot{\phi }-{\ddot{\phi }}_{des}=\frac{2C_{\alpha r}{l}_r-2C_{\alpha f}{l}_f}{I_z{v}_x}\dot{y}+\left(-\frac{2C_{\alpha l}{r}_r^2+2C_{\alpha f}{l}_f^2}{I_z{v}_x}\right)\dot{\phi }+\frac{2C_{\alpha f}{l}_f}{I_z}\delta -{\ddot{\phi }}_{\text{des }}\\ =\frac{2C_{\alpha r}{l}_r-2C_{\alpha f}{l}_f}{I_z{v}_x}\left(\dot{\tilde{y}}-v_x\tilde{\phi }\right)+\left(-\frac{2C_{\alpha r}{l}_r^2+2C_{\alpha f}{l}_f^2}{I_z{v}_x}\right)\left(\dot{\tilde{\phi }}+{\dot{\phi }}_{des}\right)+\frac{2C_{\alpha f}{l}_f}{I_z}\delta -{\ddot{\phi }}_{des}\\ =\frac{2C_{\alpha r}{l}_r-2C_{\alpha f}{l}_f}{I_z{v}_x}\dot{\tilde{y}}-\frac{2C_{\alpha r}{l}_r-2C_{\alpha f}{l}_f}{I_z}\tilde{\phi }-\frac{2C_{\alpha r}{l}_r^2+2C_{\alpha f}{l}_f^2}{I_z{v}_x}\dot{\tilde{\phi }}+\frac{2C_{\alpha f}{l}_f}{I_z}\delta -\frac{2C_{\alpha r}{l}_r^2+2C_{\alpha f}{l}_f^2}{I_z{v}_x}{\dot{\phi }}_{des}-{\ddot{\phi }}_{des}\end{array}$$
  车辆横向动力学模型跟踪误差状态空间方程

忽略${\dot{\phi }}_{\text{des }},{\ddot{\phi }}_{\text{des }}$，改写误差状态方程

$$\frac{d}{dt}\left[\begin{array}{c}\tilde{y}\\ \dot{\tilde{y}}\\ \tilde{\phi }\\ \dot{\tilde{\phi }}\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& -\frac{2C_{\alpha f}+2C_{\alpha r}}{m{v}_x}& \frac{2C_{\alpha f}+2C_{\alpha r}}{m{v}_x}& -\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{m{v}_x}\\ 0& 0& 0& 1\\ 0& \frac{2C_{\alpha r}{l}_r-2C_{\alpha f}{l}_f}{I_z{v}_x}& -\frac{2C_{\alpha r}{l}_r-2C_{\alpha f}{l}_f}{I_z{v}_x}& -\frac{2l_f^2{C}_{\alpha f}+2l_r^2{C}_{\alpha r}}{I_z{v}_x}\end{array}\right]\left[\begin{array}{c}\tilde{y}\\ \dot{\tilde{y}}\\ \tilde{\phi }\\ \dot{\tilde{\phi }}\end{array}\right]+\left[\begin{array}{c}0\\ \frac{2C_{\alpha f}}{m}\\ 0\\ \frac{2l_f{C}_{\alpha f}}{I_z}\end{array}\right]\delta$$