车辆运动学和动力学模型

参考：路径规划与轨迹跟踪系列算法学习_第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 {\delta }_r}{l}& \frac{v_r}{l{\cos }^2{\delta }_r}\end{array}\right]\end{array}$$
则状态量误差的变化量$\dot{\tilde{\chi }}$表示为：
χ ~ ˙ = [ x ˙ − x ˙ r y ˙ − y ˙ r φ ˙ − φ ˙ r ] = [ 0 0 − v r sin ⁡ φ r 0 0 v r cos ⁡ φ r 0 0 0 ] [ x − x r y − y r φ − φ r ] + [ cos ⁡ φ r 0 sin ⁡ φ r 0 tan ⁡ φ r l v r l cos ⁡ 2 δ r ] [ v − v r δ − δ r ] ⇒ χ ~ ˙ = A χ ~ + B u ~ \dot{\tilde{\chi}}=\left[\begin{array}{c} \dot{x}-\dot{x}_{r} \\ \dot{y}-\dot{y}_{r} \\ \dot{\varphi}-\dot{\varphi}_{r} \end{array}\right]=\left[\begin{array}{ccc} 0 & 0 & -v_{r} \sin \varphi_{r} \\ 0 & 0 & v_{r} \cos \varphi_{r} \\ 0 & 0 & 0 \end{array}\right]\left[\begin{array}{l} x-x_{r} \\ y-y_{r} \\ \varphi-\varphi_{r} \end{array}\right]+\left[\begin{array}{cc} \cos \varphi_{r} & 0 \\ \sin \varphi_{r} & 0 \\ \frac{\tan \varphi_{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}} χ~​˙​= ​x˙−x˙r​y˙​−y˙​r​φ˙​−φ˙​r​​ ​= ​000​000​−vr​sinφr​vr​cosφr​0​ ​ ​x−xr​y−yr​φ−φr​​