自动驾驶控制算法——车辆动力学模型

车辆动力学模型（2自由度模型）

考虑车辆 y 方向和绕 z 轴的旋转，可以得到车辆2自由度模型，如下图：

$$m{a}_y=F_{yf}+F_{yr}\text{\hspace{1em}(2.1)}$$

$$I_z\ddot{\psi }=l_f{F}_{yf}-l_r{F}_{yr}\text{\hspace{1em}(2.2)}$$

经验公式可知，轮胎侧向力和速度偏角成正比，前后各两个轮胎，故有（根据轮胎坐标系有关符号定义可知，正的侧偏力产生负的侧偏角，因此侧偏刚度为负值。不同坐标系的定义可能不同，Apollo中侧偏刚度系数为正值，相应的状态方程也会有所改变，因此，这里与apollo保持一致，侧偏刚度采用正值）：

$$\begin{gathered} F_{yf}=2C_{\alpha f}(\delta -{\theta }_{vf}) \\ {F}_{yr}=2C_{\alpha r}(-{\theta }_{vr})\text{\hspace{1em}(2.3)} \end{gathered}$$

图四

再参考图四可得:

$$\begin{gathered} \tan ({\theta }_{vf})=\frac{V_y+l_f\dot{\psi }}{V_x} \\ \tan ({\theta }_{vr})=\frac{V_y-l_r\dot{\psi }}{V_x}\text{\hspace{1em}(2.4)} \end{gathered}$$

在小角度转向假设下，可以将上述公式转换为：

$$\begin{gathered} {\theta }_{vf}=\frac{V_y+l_f\dot{\psi }}{V_x} \\ {\theta }_{vr}=\frac{V_y-l_r\dot{\psi }}{V_x}\text{\hspace{1em}(2.5)} \end{gathered}$$

上述公式中：

• $F_{fy}$ 和 $F_{fr}$ – 前轮横向力和后轮横向力
• $C_{\alpha f}$ 和 $C_{\alpha r}$ – 前轮侧偏刚度和后轮侧偏刚度（采用正值）
• ${\theta }_{vf}$ 和 ${\theta }_{vr}$ – 前轮速度偏角和后轮速度偏角
• $a_y$ – 侧向加速度（等于 y 方向加速度和向心加速度之和，即 $a_y=\ddot{y}+V_x\dot{\psi }$）
• $V_y=\dot{y}$ – 车辆y方向速度
• m – 车辆质量
• $I_z$ – 绕Z轴转动惯量

将式$(2.3,2.4,2.5)$带入公式$(2.1)$:

$$m{a}_y=m(\ddot{y}+V_x\dot{\psi })=F_{yf}+F_{yr}=2C_{\alpha f}({\delta }_f-{\theta }_{vf})+2C_{\alpha r}(-{\theta }_{vr})=2C_{\alpha f}({\delta }_f-\frac{V_y+l_f\dot{\psi }}{V_x})+2C_{\alpha r}(-\frac{V_y-l_r\dot{\psi }}{V_x})\text{\hspace{1em}(2.6)}$$

整理得到横向加速度公式：

$$\ddot{y}=-\frac{2C_{\alpha f}+2C_{\alpha r}}{m{V}_x}\dot{y}+(-V_x-\frac{2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r}{m{V}_x})\dot{\psi }+\frac{2C_{\alpha f}}{m}{\delta }_f\text{\hspace{1em}(2.7)}$$

将式$(2.3,2.4,2.5)$带入公式$(2.2)$:

$$I_z\ddot{\psi }=l_{f}2C_{\alpha f}({\delta }_f-{\theta }_{vf})-l_{r}2C_{\alpha r}(-{\theta }_{vr})=l_{f}2C_{\alpha f}({\delta }_f-\frac{V_y+l_f\dot{\psi }}{V_x})-l_{r}2C_{\alpha r}(-\frac{V_y-l_r\dot{\psi }}{V_x})\text{\hspace{1em}(2.8)}$$

整理得到横摆角加速度公式：

$$\ddot{\psi }=-(\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{V_x{I}_z})\dot{y}+(-\frac{2l_f^2{C}_{\alpha f}+2l_r^2{C}_{\alpha r}}{V_x{I}_z})\dot{\psi }+\frac{2l_f{C}_{\alpha f}}{I_z}{\delta }_f\text{\hspace{1em}(2.9)}$$

将式$2.7$和$2.9$写成矩阵形式，得到车辆动力学模型（与apollo保持一致，侧偏刚度采用正值）：

$$\frac{d}{dt}\left\{\begin{array}{c}y\\ \dot{y}\\ \psi \\ \dot{\psi }\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{2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r}{m{V}_x}\\ 0& 0& 0& 1\\ 0& -\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{V_x{I}_z}& 0& -\frac{2l_f^2{C}_{\alpha f}+2l_r^2{C}_{\alpha r}}{V_x{I}_z}\end{array}\right]\left\{\begin{array}{c}y\\ \dot{y}\\ \psi \\ \dot{\psi }\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 }_f\text{\hspace{1em}(2.10)}$$

基于车辆动力学的道路-车辆误差模型

要实现自动驾驶车辆的横向控制系统，首先要建立车辆质心相对于车道中心线的位置和航向误差模型，示意图如下所示：

其中：

• $e_1$ – 横向位置偏差（横向位置偏差）
• $\dot{e_1}$ – 横向偏差变化率（横向速度偏差）
• $\ddot{e_1}$ – 横向偏差变化率的变化率（横向加速度偏差）
• $e_2$ – 航向角偏差
• $\dot{e_2}$ – 航向角偏差变化率
• $\ddot{e_2}$ – 航向角偏差变化率的变化率
• $V_x$ – 车辆沿轴线方向速度
• $\varphi$ – 道路横向坡度
• $\dot{{\psi }_{des}}$ – （路径参考点理论横摆角速度）车辆参考航向变化率，$\frac{V_x}{R}$
• $\ddot{{\psi }_{des}}$ – （路径参考点理论横摆角加速度）车辆参考航向变化率的导数，$\frac{V_x^2}{R}=V_x\dot{{\psi }_{des}}$

首先根据定义容易得到如下公式：

$$e_2=\psi -{\psi }_{des}$$

$$\dot{e_2}=\dot{\psi }-\dot{{\psi }_{des}}$$

$$\ddot{e_2}=\ddot{\psi }-\ddot{{\psi }_{des}}$$

根据定义可以得到：

$$\begin{array}{rl}\ddot{e_1}& =a_y-a_{ydes}\\ & =(\ddot{y}+\dot{\psi }{V}_x)-\dot{{\psi }_{des}}{V}_x\\ & =\ddot{y}+V_x(\dot{\psi }-\dot{{\psi }_{des}})\\ & =\ddot{y}+V_x\dot{e_2}\end{array}$$

假设纵向速度$V_x$不变，可以进一步推导出：
$$\dot{e_1}=\dot{y}+V_x{e}_2$$

将公式转换形式为：

$$\dot{y}=\dot{e_1}-V_x{e}_2$$

$$\ddot{y}=\ddot{e_1}-V_x\dot{e_2}$$

$$\dot{\psi }=\dot{e_2}+\dot{{\psi }_{des}}$$

$$\ddot{\psi }=\ddot{e_2}+\ddot{{\psi }_{des}}$$

整理公式可以得到横向位置误差变化率的导数：

$$\ddot{e_1}=-\frac{2C_{\alpha f}+2C_{\alpha r}}{m{V}_x}\dot{e_1}+\frac{2C_{\alpha f}+2C_{\alpha r}}{m{V}_x}{V}_x{e}_2-\frac{2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r}{m{V}_x}\dot{e_2}-(V_x+\frac{2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r}{m{V}_x})\dot{{\psi }_{des}}+\frac{2C_{\alpha f}}{m}\delta \text{\hspace{1em}(2.11)}$$

整理公式可以得到航向误差变化率的导数：
$$\ddot{e_2}=-(\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{V_x{I}_z})\dot{e_1}+\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{V_x{I}_z}{V}_x{e}_2+(-\frac{2l_f^2{C}_{\alpha f}+2l_r^2{C}_{\alpha r}}{V_x{I}_z})\dot{e_2}-\frac{2l_f^2{C}_{\alpha f}+2l_r^2{C}_{\alpha r}}{V_x{I}_z}\dot{{\psi }_{des}}-\ddot{{\psi }_{des}}+\frac{2l_f{C}_{\alpha f}}{I_z}\delta \text{\hspace{1em}(2.12)}$$

取$X=[e_1,\dot{e_1},e_2,\dot{e_2}{]}^T$为误差状态变量，则道路-车辆误差状态方程可写成（与apollo保持一致，侧偏刚度采用正值）：
$$\begin{gathered} \dot{X}=AX+BU+B_1\dot{{\psi }_{des}}+B_2\ddot{{\psi }_{des}}+C \\ \frac{d}{dt}\left[\begin{array}{c}{e}_1\\ \dot{e_1}\\ e_2\\ \dot{e_2}\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}& -\frac{2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r}{m{V}_x}\\ 0& 0& 0& 1\\ 0& -\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{V_x{I}_z}& \frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z}& -\frac{2l_f^2{C}_{\alpha f}+2l_r^2{C}_{\alpha r}}{V_x{I}_z}\end{array}\right]\left[\begin{array}{c}{e}_1\\ \dot{e_1}\\ e_2\\ \dot{e_2}\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 +\left[\begin{array}{c}0\\ -V_x-\frac{2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r}{m{V}_x}\\ 0\\ -\frac{2l_f^2{C}_{\alpha f}+2l_r^2{C}_{\alpha r}}{V_x{I}_z}\end{array}\right]\dot{{\psi }_{des}}+\left[\begin{array}{c}0\\ 0\\ 0\\ -1\end{array}\right]\ddot{{\psi }_{des}}+\left[\begin{array}{c}0\\ g\\ 0\\ 0\end{array}\right]\sin \varphi \text{\hspace{1em}(2.13)} \end{gathered}$$

其中：
$$A=\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}& -\frac{2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r}{m{V}_x}\\ 0& 0& 0& 1\\ 0& -\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{V_x{I}_z}& \frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z}& -\frac{2l_f^2{C}_{\alpha f}+2l_r^2{C}_{\alpha r}}{V_x{I}_z}\end{array}\right]，B=\left[\begin{array}{c}0\\ \frac{2C_{\alpha f}}{m}\\ 0\\ \frac{2l_f{C}_{\alpha f}}{I_z}\end{array}\right]$$

$$B_1=\left[\begin{array}{c}0\\ -V_x-\frac{2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r}{m{V}_x}\\ 0\\ -\frac{2l_f^2{C}_{\alpha f}+2l_r^2{C}_{\alpha r}}{V_x{I}_z}\end{array}\right],B_2=\left[\begin{array}{c}0\\ 0\\ 0\\ -1\end{array}\right],C=\left[\begin{array}{c}0\\ g\\ 0\\ 0\end{array}\right]\sin \varphi ,U=\delta$$

这里仅仅考虑了横向的动力学关系，如果需要同时考虑到纵向动力学，且忽略最后带 $\ddot{{\psi }_{des}}$项和横坡度$C$项，可以将上式进一步扩展为：
$$\frac{d}{dt}\left[\begin{array}{c}{e}_1\\ \dot{e_1}\\ e_2\\ \dot{e_2}\\ e_3\\ \dot{e_3}\end{array}\right]=\left[\begin{array}{cccccc}0& 1& 0& 0& 0& 0\\ 0& -\frac{2C_{\alpha f}+2C_{\alpha r}}{m{V}_x}& \frac{2C_{\alpha f}+2C_{\alpha r}}{m}& -\frac{2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r}{m{V}_x}& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& -\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{V_x{I}_z}& \frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z}& -\frac{2l_f^2{C}_{\alpha f}+2l_r^2{C}_{\alpha r}}{V_x{I}_z}& 0& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{e}_1\\ \dot{e_1}\\ e_2\\ \dot{e_2}\\ e_3\\ \dot{e_3}\end{array}\right]+\left[\begin{array}{cc}0& 0\\ \frac{2C_{\alpha f}}{m}& 0\\ 0& 0\\ \frac{2l_f{C}_{\alpha f}}{I_z}& 0\\ 0& 0\\ 0& -1\end{array}\right]\left[\begin{array}{c}\delta \\ a\end{array}\right]+\left[\begin{array}{c}0\\ -V_x-\frac{2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r}{m{V}_x}\\ 0\\ -\frac{2l_f^2{C}_{\alpha f}+2l_r^2{C}_{\alpha r}}{V_x{I}_z}\\ 0\\ 0\end{array}\right]\dot{{\psi }_{des}}\text{\hspace{1em}(2.14)}$$

其中:

• $e_3$ – 纵向位置偏差
• $\dot{e_3}$ – 纵向速度偏差（纵向位置偏差变化率）
• $\ddot{e_3}$ – 纵向加速度偏差（纵向速度偏差变化率）
• $a$ – 加速度补偿量

动力学误差模型离散化

连续系统的离散化方法比较多，例如正向/反向差分变化法，双线性变换法，脉冲响应不变法，阶跃响应不变法等。
上述部分基于车辆横向误差和航向误差的动力学模型建立后，需要对模型进行离散化，这里对系统矩阵A进行中点欧拉离散化，对B和C矩阵进行正向差分变换，可以得到离散模型。下面对下列连续状态空间方程进行离散化：
连续状态空间方程(忽略最后带 $\ddot{{\psi }_{des}}$项和横坡度$C$项)：
$$\dot{X}=AX+BU+B_1\dot{{\psi }_{des}}$$

系统矩阵 A 双线性变换离散后为：
$$A_d=(I-\frac{T}{2}A{)}^{-1}(I+\frac{T}{2}A)$$
$$B_d=B\ast T$$
$$B_{1d}=B_1\ast T$$

则离散后的模型为:
$$X(k+1)=A_{d}X(k)+B_{d}U(k)+B_{1d}\ast \dot{{\psi }_{des}}(k)$$
注：

• $I$ 为单位矩阵
• $T$ 为采样周期
• 忽略最后一项 $\ddot{{\psi }_{des}}$
• $U(k)=[\delta (k),a(k){]}^T$