Apollo学习笔记（8）车辆动力学模型

原文链接：https://blog.csdn.net/u013914471/article/details/83018664
大佬那边写的其实更详细

车辆动力学模型

动力学主要研究作用于物体的力与物体运动的关系，车辆动力学模型一般用于分析车辆的平顺性和车辆操纵的稳定性。对于车来说，研究车辆动力学，主要是研究车辆轮胎及其相关部件的受力情况。

正常情况下，车辆上的作用力沿着三个不同的轴分布：

1. 纵轴上的力包括驱动力和制动力，以及滚动阻力和拖拽阻力作滚摆运动(roll)；

2. 横轴上的力包括转向力、离心力和侧风力，汽车绕横轴作俯仰运动(pitch)；

3. 立轴上的力包括车辆上下振荡施加的力，汽车绕立轴作偏摆或转向运动(yaw)；

$$图1车辆受力模型$$

而在单车模型假设的前提下，再作如下假设1即可简单搭建车辆的动力学模型：

1. 只考虑纯侧偏轮胎特性，忽略轮胎力的纵横向耦合关系；
2. 用单车模型来描述车辆的运动，不考虑载荷的左右转移；
3. 忽略横纵向空气动力学。

$$图2车辆单车模型受力状态$$
如图2所示，$oxyz$为固定于车身的车辆坐标系，$OXY$为固定于地面的惯性坐标系。单车模型的车辆具有2个自由度：绕$z$轴的横摆运动，和沿$x$轴的纵向运动。
纵向指沿物体前进方向，横向(或侧向)指垂直纵向方向。

横向运动：出自横向的风力，以及曲线行驶时的离心力等。
纵向运动：受总驱动阻力、加速、减速等的影响。总驱动阻力由滚动阻力、拖拽阻力和坡度阻力等构成。
滑移角(slip−angle)(slip−angle)：轮胎方向和轮胎速度方向的夹角。滑移角的产生主要是由于车轮所受合力方向并非朝向车轮行进方向，但车轮的偏移角通常较小。

图2中各符号定义：

符号	定义
$F_{lf},F_{lr}$	前、后轮胎受到的纵向力
$F_{cf},F_{cr}$	前、后轮胎受到的侧向力
$F_{xf},F_{xr}$	前、后轮胎受到的x方向力
$F_{yf},F_{yr}$	前、后轮胎受到的y方向力
a,${\ell }_f$	前悬长度
b,${\ell }_r$	后悬长度
${\delta }_f$,$\delta$	前轮偏角
${\delta }_r$	后轮偏角
$a_f$	前轮偏移角

分别沿着$x$,$y$,$z$轴做相关分析：
在x轴上：
$$m{a}_x=F_{xf}+F_{xr}\text{\hspace{1em}(1)}$$
在y轴上：
$$m{a}_y=F_{yf}+F_{yr}\text{\hspace{1em}(2)}$$
在z轴上：
$$I_z\ddot{\phi }=a{F}_{yf}-b{F}_{yr}\text{\hspace{1em}(3)}$$
式中，m为车辆质量，$\phi$为车辆航向(yaw)，$I_z$为车辆绕z轴的转动惯量。
x轴方向的运动暂时不用考虑，车子翻滚不属于正常行驶范围。

横向动力学

$$图3横向动力学$$
y轴方向加速度$a_y$由两部分构成：沿着y轴方向的加速度$\ddot{y}$，垂直于y轴方向的向心加速度$V_x\dot{\phi }$
$$a_y=\ddot{y}+V_x\dot{\phi }$$
则式(2)可变为：
$$m(\ddot{y}+V_x\dot{\phi })=F_{yf}+F_{yr}\text{\hspace{1em}(4)}$$

由于轮胎受到的横向压力，轮胎会有一个很小的滑移角，如图4所示

$$图4轮胎滑移角$$
前轮滑移角
$$a_f=\delta -{\theta }_{Vf}\text{\hspace{1em}(5)}$$
其中，${\theta }_{Vf}$为前轮速度方向，$\delta$为前轮转角。

后轮滑移角
$$a_r=-{\theta }_{Vr}\text{\hspace{1em}(6)}$$
其中，${\theta }_{Vr}$为前轮速度方向。

则前轮所受到的横向力为
$$F_{yf}=2C_{af}(\delta -{\theta }_{Vf})\text{\hspace{1em}(7)}$$
后轮所受到的横向力为
$$F_{yr}=2C_{ar}(-{\theta }_{Vr})\text{\hspace{1em}(8)}$$
其中，$C_{af}$​、$C_{ar}$分别为前后轮的侧偏刚度(cornering stiffness)，由于车辆前后各两个轮，所以受力要乘以2。
结合图4，可以有
$$\tan ({\theta }_{Vf})=\frac{\dot{y}+a\dot{\phi }}{V_x}\text{\hspace{1em}(9)}$$
$$\tan ({\theta }_{Vr})=\frac{\dot{y}-b\dot{\phi }}{V_x}\text{\hspace{1em}(10)}$$
式(9)，(10)可以近似转换为：
$${\theta }_{Vf}=\frac{\dot{y}+a\dot{\phi }}{V_x}\text{\hspace{1em}(11)}$$
$${\theta }_{Vr}=\frac{\dot{y}-b\dot{\phi }}{V_x}\text{\hspace{1em}(12)}$$
将式(5)，(6)，(11)，(12)带入式(2)，(3)可以得到车辆动力学模型
$$\frac{d}{dt}\left[\begin{array}{c}y\\ \dot{y}\\ \phi \\ \dot{\phi }\end{array}\right]=\left[\begin{array}{ccccccc}0& & 1& & 0& & 0\\ 0& & -\frac{2C_{af}+2C_{ar}}{m{V}_x}& & 0& & -V_x-\frac{2C_{af}{\ell }_f-2C_{ar}{\ell }_r}{m{V}_x}\\ 0& & 0& & 0& & 1\\ 0& & -\frac{2C_{af}{\ell }_f-2C_{ar}{\ell }_r}{I_z{V}_x}& & 0& & -\frac{2C_{af}{\ell }_f^2+2C_{ar}{\ell }_r^2}{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_{af}}{m}\\ 0\\ \frac{2C_{af}{\ell }_f}{I_z}\end{array}\right]\delta \text{\hspace{1em}(13)}$$
式中，${\ell }_f$，${\ell }_r$分别为车辆的前悬距离，后悬距离，也就是前文中的a，b。

方向盘控制模型

横向控制主要通过控制轮胎转角实现，而对于驾驶员来说，可直接操控的是方向盘角度，因此在搭建车辆动力学模型时，可以以相对于道路的方向和距离误差为状态变量的动力学模型。
如上文所述，$e_1$为横向误差，车辆质心到规划轨迹最近投影点的距离，$e_2$为航向误差，车辆纵向速度为$V_x$，车辆转弯半径为$R$，结合上文有：
车辆转过期望角度所需角速度
$$\dot{\phi }=V_x/R\text{\hspace{1em}(14)}$$
所需横向加速度
$$a_{ydes}=V_x^2/R=V_x{\phi }_{des}\text{\hspace{1em}(15)}$$
则横向加速度误差
$${\ddot{e}}_1=a_y-a_{ydes}=(\ddot{y}+V_x\dot{\phi })-\frac{V_x^2}{R}=\ddot{y}+V_x(\dot{\phi }-{\dot{\phi }}_{des})\text{\hspace{1em}(16)}$$
横向速度误差为
$${\dot{e}}_1=V_y-V_{ydes}=\dot{y}+V_x(\phi -{\phi }_{des})\text{\hspace{1em}(17)}$$
航向误差
$$e_2=\phi -{\phi }_{des}\text{\hspace{1em}(18)}$$
将式(17)，(18)带入式(3)，(4)后，有
$$\begin{gathered} \frac{d}{dt}\left[\begin{array}{c}{e}_1\\ {\dot{e}}_1\\ e_2\\ {\dot{e}}_2\end{array}\right]=\left[\begin{array}{ccccccc}0& & 1& & 0& & 0\\ 0& & -\frac{2C_{af}+2C_{ar}}{m{V}_x}& & \frac{2C_{af}+2C_{ar}}{m}& & -\frac{2C_{af}{\ell }_f-2C_{ar}{\ell }_r}{m{V}_x}\\ 0& & 0& & 0& & 1\\ 0& & -\frac{2C_{af}{\ell }_f-2C_{ar}{\ell }_r}{I_z{V}_x}& & \frac{2C_{af}{\ell }_f-2C_{ar}{\ell }_r}{I_z}& & -\frac{2C_{af}{\ell }_f^2+2C_{ar}{\ell }_r^2}{I_z{V}_x}\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_{af}}{m}\\ 0\\ \frac{2C_{af}{\ell }_f}{I_z}\end{array}\right]\delta +\left[\begin{array}{c}0\\ -V_x-\frac{2C_{af}{\ell }_f-2C_{ar}{\ell }_r}{m{V}_x}\\ 0\\ -\frac{2C_{af}{\ell }_f^2+2C_{ar}{\ell }_r^2}{I_z{V}_x}\end{array}\right]{\dot{\phi }}_{des}\text{\hspace{1em}(19)} \end{gathered}$$
综上，车辆动力学模型在Apollo中主要使用的为横向动力学模型，且有以下假设前提，

• 侧偏角角度很小
• 车辆匀速行驶
• 不考虑环境因素

再根据上述公式推导，得到横向动力学连续模型转换为可用的近似离散化模型
$$x(k+1)=Ax+Bu+C\text{\hspace{1em}(20)}$$
式中，
$$\begin{gathered} A=\left[\begin{array}{ccccccc}0& & 1& & 0& & 0\\ 0& & -\frac{2C_{af}+2C_{ar}}{m{V}_x}& & \frac{2C_{af}+2C_{ar}}{m}& & -\frac{2C_{af}{\ell }_f-2C_{ar}{\ell }_r}{m{V}_x}\\ 0& & 0& & 0& & 1\\ 0& & -\frac{2C_{af}{\ell }_f-2C_{ar}{\ell }_r}{I_z{V}_x}& & \frac{2C_{af}{\ell }_f-2C_{ar}{\ell }_r}{I_z}& & -\frac{2C_{af}{\ell }_f^2+2C_{ar}{\ell }_r^2}{I_z{V}_x}\end{array}\right]， \\ B=\left[\begin{array}{c}0\\ \frac{2C_{af}}{m}\\ 0\\ \frac{2C_{af}{\ell }_f}{I_z}\end{array}\right]， \\ C=\left[\begin{array}{c}0\\ -V_x-\frac{2C_{af}{\ell }_f-2C_{ar}{\ell }_r}{m{V}_x}\\ 0\\ -\frac{2C_{af}{\ell }_f^2+2C_{ar}{\ell }_r^2}{I_z{V}_x}\end{array}\right]{\dot{\phi }}_{des} \end{gathered}$$
Apollo中的LQR控制就是基于此模型的，具体的LQR控制在另外一篇文章中有详细描述。

前馈控制

系统加入反馈后，发现还有一个问题，就是相关的项不一定收敛于0，为了解决这个问题，于是就再加入了一个前馈项来进行补偿。
$${\delta }_{ff}=L/R+K_v{a}_y-k_{(2,0)}[\frac{{\ell }_r}{R}-\frac{{\ell }_f}{2C_{ar}}\frac{m{V}_x^2}{R\ell }]\text{\hspace{1em}(21)}$$
式中，
$$K_v=\frac{{\ell }_{r}m}{2C_{af}({\ell }_f+{\ell }_r)}-\frac{{\ell }_{f}m}{2C_{ar}({\ell }_f+{\ell }_r)}$$
$$a_y=V_x^2/R$$。
注意：$k_{(2,0)}$是LQR中求解出来的K矩阵。后续会对Apollo中的各个控制算法做详细的记录和说明。