自动驾驶之车辆运动学与动力学模型

1. 车辆运动学模型：

车辆转向运动模型如图$2.1$所示。

在惯性坐标系$OXY$下,

• $\left(X_r,Y_r\right)$和$\left(X_f,Y_f\right)$分別为车辆后轴和前轴轴心的坐标;
• $\phi$为车体的横摆角(航向角);
• ${\delta }_{\mathrm{f}}$为前轮偏角;
• $v_{\mathrm{r}}$为车辆后轴中心速度;
• $v_{\mathrm{f}}$为车辆前轴中心速度;
• $l$为轴距(注意,变量下标$f$代表 front—前,$r$代表rear—后,下同);

图2.2原图：

便于理解：

图2.2所示为车辆转向过程示意图,$R$为后轮转向半径,$P$为车辆的瞬时转动中心,$M$为车辆后轴轴心,$N$为前轴轴心。此处假设转向过程中车辆质心侧偏角保持不变,即车辆瞬时转向半径与道路曲率半径相同。

在后轴行驶轴心$\left(X_r,Y_r\right)$处，速度为：
$$v_r={\dot{X}}_r\cos \phi +{\dot{Y}}_r\sin \phi \text{\hspace{1em}(1)}$$
前后轴的运动学约束为：
$$\{\begin{array}{l}{\dot{X}}_f\sin \left(\phi +{\delta }_f\right)-{\dot{Y}}_f\cos \left(\phi +{\delta }_f\right)=0\\ {\dot{X}}_r\sin \phi -{\dot{Y}}_r\cos \phi =0\end{array}\text{\hspace{1em}(2)}$$
由式(1)和式(2)联合可得：
$$\{\begin{array}{l}{\dot{X}}_r=v_r\cos \phi \\ {\dot{Y}}_r=v_r\sin \phi \end{array}\text{\hspace{1em}(3)}$$

式3推导：将式1等式两边同时乘以$\cos \phi$，然后将式2代入可得式3

根据前后轮的几何关系可得：
$$\{\begin{array}{l}{X}_{\mathrm{f}}=X_r+l\cos \phi \\ Y_{\mathrm{f}}=Y_r+l\sin \phi \end{array}\text{\hspace{1em}(4)}$$
根据式2，式3和式4可得车辆横摆角速度$\omega$为：
$$\omega =\frac{v_r}{l}\tan {\delta }_f\text{\hspace{1em}(5)}$$

式5推导：对式4左右求导：
$$\begin{array}{l}{\dot{X}}_f={\dot{X}}_r+l(-\sin \phi )\cdot \dot{\phi }=V_r\cos \phi +l(-\sin \phi )\cdot \dot{\phi }\\ {\dot{Y}}_f={\dot{Y}}_r+l\cos \phi \cdot \dot{\phi }=V_r\cdot \sin \phi +l\cos \phi \cdot \dot{\phi }\end{array}$$
所以：
$$\begin{array}{rl}& \left(V_r\cos \phi -l\sin \phi \cdot \dot{\phi }\right)\sin \left(\phi +{\delta }_f\right)-\left(V_r\cdot \sin \phi +l\cos \phi \cdot \dot{\phi }\right)\cdot \cos (\phi +{\delta }_f)=0\\ & \begin{array}{r}{V}_r\cos \phi \sin (\phi +{\delta }_f)-V_r\sin \phi (\cos \phi +{\delta }_f)-l\sin \phi \cdot \dot{\phi }\sin (\phi +{\delta }_f)-l\cos \phi \cdot \dot{\phi }\cos (\phi +{\delta }_f)=0\end{array}\end{array}$$
积化和差公式可得：
$$\begin{array}{rl}& V_r\cdot \sin {\delta }_f=l\cdot \dot{\phi }\cdot \cos {\delta }_f\\ & V_r\cdot \tan {\delta }_f=l\cdot \dot{\phi }\\ & \frac{V_x}{l}\cdot \tan {\delta }_f=\omega \end{array}$$

同时由和车速可得到转向半径和前轮偏角：
$$\{\begin{array}{l}R=v_{\mathrm{r}}/\omega \\ {\delta }_f=\arctan (l/R)\end{array}\text{\hspace{1em}(6)}$$
由式(3)和式(5)可得到车辆运动学模型为:
$$\left[\begin{array}{c}{\dot{X}}_{\mathrm{r}}\\ {\dot{Y}}_{\mathrm{r}}\\ \dot{\phi }\end{array}\right]=\left[\begin{array}{c}\cos \phi \\ \sin \phi \\ \tan {\delta }_f/l\end{array}\right]v_{\mathrm{r}}\text{\hspace{1em}(7}$$