Apollo代码学习(三)—车辆动力学模型

前言

接上一篇：Apollo代码学习(二)—车辆运动学模型
主要参考资料仍是这三个：
【1】Rajamani R. Vehicle Dynamics and Control[M]. Springer Science, 2006.
【2】龚建伟, 姜岩, 徐威. 无人驾驶车辆模型预测控制[M]. 北京理工大学出版社, 2014.
【3】无人驾驶汽车系统入门（五）——运动学自行车模型和动力学自行车模型

提倡大家支持正版资源，本人提供文档仅限交流学习使用，侵删：
【1】Rajamani R. Vehicle Dynamics and Control[M]. Springer Science, 2006. | CSDN资源
【2】龚建伟, 姜岩, 徐威. 无人驾驶车辆模型预测控制[M]. 北京理工大学出版社, 2014. | CSDN资源

车辆动力学模型

动力学主要研究作用于物体的力与物体运动的关系，车辆动力学模型一般用于分析车辆的平顺性和车辆操纵的稳定性。对于车来说，研究车辆动力学，主要是研究车辆轮胎及其相关部件的受力情况。比如纵向速度控制，通过控制轮胎转速实现；横向航向控制，通过控制轮胎转角实现。

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

1. 纵轴上的力包括驱动力和制动力，以及滚动阻力和拖拽阻力作滚摆运动；
2. 横轴上的力包括转向力、离心力和侧风力，汽车绕横轴作俯仰运动；
3. 立轴上的力包括车辆上下振荡施加的力，汽车绕立轴作偏摆或转向运动

图1 车辆受力模型

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

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

图2 车辆单车模型

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

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

图2中各符号定义：

符号	定义
$F_{lf},F_{lr}$	前、后轮胎受到的纵向力
$F_{cf},F_{cr}$	前、后轮胎受到的侧向力
$F_{xf},F_{xr}$	前、后轮胎受到的$x$方向的力
$F_{yf},F_{yr}$	前、后轮胎受到的$y$方向的力
$a$	前悬长度
$b$	后悬长度
${\delta }_f$	前轮偏角
${\delta }_r$	后轮偏角
${\alpha }_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$为整车质量，$I_z$为车辆绕$z$轴转动的转动惯量。$x$轴方向的运动（绕纵轴的滚动运动）可暂不用考虑。

横向动力学

可结合Vehicle Dynamics and Control²第2章和第13章进行研究。

图3 横向动力学 图片来源：Vehicle Dynamics and Control

$y$轴方向加速度$a_y$由两部分构成：$y$轴方向的位移相关的加速度$\ddot{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)}$$

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

图4 轮胎滑移角 图片来源：Vehicle Dynamics and Control

前轮滑移角
$${\alpha }_f=\delta -{\theta }_{Vf}\text{\hspace{1em}(5)}$$
其中，${\theta }_{Vf}$为前轮速度方向，$\delta$为前轮转角。
后轮滑移角
$${\alpha }_r=-{\theta }_{Vr}\text{\hspace{1em}(6)}$$
其中，${\theta }_{Vr}$为后轮速度方向。
则前轮所受的横向力为
$$F_{yf}=2C_{\alpha f}(\delta -{\theta }_{Vf})\text{\hspace{1em}(7)}$$
后轮所受的横向力为
$$F_{yr}=2C_{\alpha r}(-{\theta }_{Vr})\text{\hspace{1em}(8)}$$
其中，$C_{\alpha f}$、$C_{\alpha r}$分别为前后轮的$侧偏刚度(corneringstiffness)$，由于车辆前后各两个轮，所以受力要乘以2。

结合图4，${\theta }_{Vf}$、${\theta }_{Vr}$可用下式计算：
$$\tan ({\theta }_{Vf})=\frac{V_y+{\ell }_f\dot{\phi }}{V_x}\text{\hspace{1em}(9)}$$
$$\tan ({\theta }_{Vr})=\frac{V_y-{\ell }_r\dot{\phi }}{V_x}\text{\hspace{1em}(10)}$$
其中，${\ell }_f$为前悬长度，${\ell }_r$为后悬长度。
又$\dot{y}=V_y$,则公式9、公式10可近似转换为：
$${\theta }_{Vf}=\frac{\dot{y}+{\ell }_f\dot{\phi }}{V_x}\text{\hspace{1em}(11)}$$
$${\theta }_{Vr}=\frac{\dot{y}-{\ell }_r\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}{cccc}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{2{\ell }_f{C}_{af}}{I_z}\end{array}\right]\delta \text{\hspace{1em}(13)}$$

方向盘控制模型

上面提到，横向控制主要通过控制轮胎转角实现，而对于驾驶员来说，可直接操控的是方向盘角度，因此在搭建车辆动力学模型时，可以以相对于道路的方向和距离误差为状态变量的动力学模型。

假设，$e_1$为横向误差，车辆质心距车道中心线的距离，$e_2$为航向误差，车辆纵向速度为$V_x$，车辆转弯半径为$R$，结合图1、图2、图3，则：
车身转过期望角度所需转角速度
$${\dot{\phi }}_{des}=\frac{V_x}{R}\text{\hspace{1em}(15)}$$
所需横向加速度
$$a_{y_{des}}=\frac{V_x^2}{R}=V_x{\dot{\phi }}_{des}\text{\hspace{1em}(16)}$$
则横向加速度误差
$$\ddot{e_1}=a_y-a_{y_{des}}=(\ddot{y}+V_x\dot{\phi })-\frac{V_x^2}{R}=\ddot{y}+V_x(\dot{\phi }-{\dot{\phi }}_{des})\text{\hspace{1em}(17)}$$
横行速度误差为
$$\dot{e_1}=\dot{y}+V_x(\phi -{\phi }_{des})\text{\hspace{1em}(18)}$$
航向误差
$$e_2=\phi -{\phi }_{des}\text{\hspace{1em}(19)}$$

将公式18、公式19带入公式3、公式4可得：
$$\begin{gathered} m({\ddot{e}}_1+V_x{\dot{\phi }}_{des})={\dot{e}}_1[-\frac{2C_{\alpha f}}{V_x}-\frac{2C_{\alpha r}}{V_x}]+e_2[2C_{\alpha f}+2C_{\alpha r}] \\ +{\dot{e}}_2[-\frac{2C_{\alpha f}{\ell }_f}{V_x}+\frac{2C_{\alpha r}{\ell }_r}{V_x}]+{\dot{\phi }}_{des}[-\frac{2C_{\alpha f}{\ell }_f}{V_x}+\frac{2C_{\alpha r}{\ell }_r}{V_x}]+2C_{\alpha f}\delta \text{\hspace{1em}(20)} \end{gathered}$$

$$\begin{gathered} I_z{\ddot{e}}_2=2C_{\alpha f}{\ell }_f\delta +{\dot{e}}_1[-\frac{2C_{\alpha f}{\ell }_f}{V_x}+\frac{2C_{\alpha r}{\ell }_r}{V_x}]+e_2[2C_{\alpha f}{\ell }_f-2C_{\alpha r}{\ell }_r] \\ +{\dot{e}}_2[-\frac{2C_{\alpha f}{\ell }_f^2}{V_x}-\frac{2C_{\alpha r}{\ell }_r^2}{V_x}]-I_z{\ddot{\phi }}_{des}+\dot{\phi }[-\frac{2C_{\alpha f}{\ell }_f^2}{V_x}-\frac{2C_{\alpha r}{\ell }_r^2}{V_x}]\text{\hspace{1em}(21)} \end{gathered}$$
公式20可能与书中第2章公式2.34略有出入，个人认为是书本编者笔误，感兴趣的可以自己推一下。

假设车辆系统的状态空间方程为：
$$\dot{X}=AX+Bu\text{\hspace{1em}(22)}$$
$$Y=CX+Du\text{\hspace{1em}(23)}$$
在apollo中横向控制中，系统的状态变量有四个：

• 横向误差 $lateral\_error$
• 横向误差率 $lateral\_error\_rate$
• 航向误差 $heading\_error$
• 航向误差率 $heading\_error\_rate$

分别对应$e_1$、${\dot{e}}_1$、$e_2$、${\dot{e}}_2$
综上，可得方向盘控制的动力学模型：
$$\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}{cccc}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_{\alpha f}}{m}\\ 0\\ \frac{2C_{\alpha f}{\ell }_f}{I_z}\end{array}\right]\delta +\left[\begin{array}{c}0\\ -\frac{2C_{af}{\ell }_f-2C_{ar}{\ell }_r}{m{V}_x}-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}(24)} \end{gathered}$$

对于横向误差和横向误差率的计算，根据代码可知：

const double raw_lateral_error =
      cos_matched_theta * dy - sin_matched_theta * dx;
  if (FLAGS_use_navigation_mode) {
    double filtered_lateral_error =
        lateral_error_filter_.Update(raw_lateral_error);
    debug->set_lateral_error(filtered_lateral_error);
  } else {
    debug->set_lateral_error(raw_lateral_error);
  }
  const double delta_theta =
      common::math::NormalizeAngle(theta - target_point.path_point().theta());
  const double sin_delta_theta = std::sin(delta_theta);
  // d_error_dot = linear_v * sin_delta_theta;
  // theta_error = delta_theta
  // TODO(QiL): Code reformat after test
  debug->set_lateral_error_rate(linear_v * sin_delta_theta);

如图5，为横向误差计算的示意图：

图5 横向误差示意图

则横向误差的表达式为：
$$\{\begin{array}{l}{e}_1=dy\ast \cos {\theta }_{des}-dx\ast \sin {\theta }_{des}\\ \dot{e_1}=V_x\ast \sin \Delta \theta =V_x\ast \sin e_2\\ e_2=\theta -{\theta }_{des}\\ \dot{e_2}=\dot{\theta }-{\dot{\theta }}_{des}\end{array}\text{\hspace{1em}(25)}$$
其中，$e_1$为横向误差($latteral\_error$)，$\dot{e_1}$为横向误差率($latteral\_error\_rate$)，$e_2$为航向误差($heading\_error$)，$\dot{e_2}$为航向误差率($heading\_error\_rate$)；$\dot{\theta }$为车辆转角速度，可由车身传感器测得；${\dot{\theta }}_{des}$为期望车辆转角速度，由规划参数获得：
$${\dot{\theta }}_{des}=V_{des}\ast k_{des}$$
其中，$V_{des}$为期望车速，$k_{des}$期望道路曲率，具体可见apollo中对路径规划参数结构体的定义planning.proto。

总结

至此，车辆运动学和动力学模型搭建完毕，如有偏颇或错误的地方望指正。
接下来会具体分析控制模块中横向控制用于求取最优控制解的LQR算法，及MPC模型预测控制算法。

补充 2018.11.27

对于横向误差计算不理解的请参考另一篇文章中的补充内容：Apollo代码学习(五)—横纵向控制

---

1. 龚建伟, 姜岩, 徐威. 无人驾驶车辆模型预测控制[M]. 北京理工大学出版社, 2014. ↩︎

2. Rajamani R. Vehicle Dynamics and Control[M]. Springer Science, 2006. ↩︎