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

• 前言
• 车辆动力学模型
• • 横向动力学
  • • 方向盘控制模型
• 总结
• 补充 2018.11.27

前言

接上一篇：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中各符号定义：

根据牛顿第二定律，分别沿$x$轴、$y$轴和$z$轴作受力分析：
在$x$轴方向上：
(1)max=Fxf+Fxrma_x = F_{xf} + F_{xr} \tag{1}max​=Fxf​+Fxr​(1)
在$y$轴方向上：
(2)may=Fyf+Fyrma_y = F_{yf} + F_{yr} \tag{2}may​=Fyf​+Fyr​(2)
在$z$轴方向上：

(3)Izφ¨=aFyf−bFyrI_z\ddot{\varphi} = aF_{yf} – bF_{yr} \tag{3}Iz​φ¨​=aFyf​−bFyr​(3)
其中，$m$为整车质量，IzI_zIz​为车辆绕$z$轴转动的转动惯量。$x$轴方向的运动（绕纵轴的滚动运动）可暂不用考虑。

横向动力学

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

图3 横向动力学

图片来源：Vehicle Dynamics and Control

$y$轴方向加速度aya_yay​由两部分构成：$y$轴方向的位移相关的加速度y¨\ddot{y}y¨​和向心加速度Vxφ˙V_x\dot{\varphi}Vx​φ˙​
ay=y¨+Vxφ˙a_y=\ddot{y} + V_x\dot{\varphi}ay​=y¨​+Vx​φ˙​
则公式2可变为：

(4)m(y¨+Vxφ˙)=Fyf+Fyrm(\ddot{y} + V_x\dot{\varphi})= F_{yf} + F_{yr} \tag{4}m(y¨​+Vx​φ˙​)=Fyf​+Fyr​(4)

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

图4 轮胎滑移角

图片来源：Vehicle Dynamics and Control

前轮滑移角
(5)αf=δ−θVf\alpha_f = \delta – \theta_{Vf} \tag{5}αf​=δ−θVf​(5)
其中，θVf\theta_{Vf}θVf​为前轮速度方向，$\delta$为前轮转角。
后轮滑移角
(6)αr=−θVr\alpha_r = – \theta_{Vr} \tag{6}αr​=−θVr​(6)
其中，θVr\theta_{Vr}θVr​为后轮速度方向。
则前轮所受的横向力为
(7)Fyf=2Cαf(δ−θVf)F_{yf} = 2C_{\alpha f}(\delta – \theta_{Vf} ) \tag{7}Fyf​=2Cαf​(δ−θVf​)(7)
后轮所受的横向力为
(8)Fyr=2Cαr(−θVr)F_{yr} = 2C_{\alpha r}(- \theta_{Vr} ) \tag{8}Fyr​=2Cαr​(−θVr​)(8)
其中，CαfC_{\alpha f}Cαf​、CαrC_{\alpha r}Cαr​分别为前后轮的$侧偏刚度(corneringstiffness)$，由于车辆前后各两个轮，所以受力要乘以2。

结合图4，θVf\theta_{Vf}θVf​、θVr\theta_{Vr}θVr​可用下式计算：
(9)tan⁡(θVf)=Vy+ℓfφ˙Vx\tan(\theta_{Vf}) = \frac{V_y+\ell_f\dot{\varphi}}{V_x} \tag{9}tan(θVf​)=Vx​Vy​+ℓf​φ˙​​(9)
(10)tan⁡(θVr)=Vy−ℓrφ˙Vx\tan(\theta_{Vr}) = \frac{V_y-\ell_r\dot{\varphi}}{V_x} \tag{10}tan(θVr​)=Vx​Vy​−ℓr​φ˙​​(10)
其中，ℓf\ell_fℓf​为前悬长度，ℓr\ell_rℓr​为后悬长度。
又y˙=Vy\dot{y}=V_yy˙​=Vy​,则公式9、公式10可近似转换为：
(11)θVf=y˙+ℓfφ˙Vx\theta_{Vf}=\frac{\dot{y}+\ell_f\dot{\varphi}}{V_x} \tag{11}θVf​=Vx​y˙​+ℓf​φ˙​​(11)
(12)θVr=y˙−ℓrφ˙Vx\theta_{Vr}=\frac{\dot{y} – \ell_r\dot{\varphi}}{V_x} \tag{12}θVr​=Vx​y˙​−ℓr​φ˙​​(12)
将公式5、公式6、公式11、公式12代入公式2、公式3中可得动力学模型：
(13)ddt[yy˙φφ˙]=[01000−2Caf+2CarmVx0−Vx−2Cafℓf−2CarℓrmVx00010−2Cafℓf−2CarℓrIzVx0−2Cafℓf2+2Carℓr2IzVx][yy˙φφ˙]+[02Cafm02ℓfCafIz]δ\frac{d}{dt} \begin{bmatrix} y \\ \dot{y} \\ \varphi \\ \dot{\varphi} \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & -\frac{2C_{af}+2C_{ar}}{mV_x} & 0 & -V_x-\frac{2C_{af}\ell_f-2C_{ar}\ell_r}{mV_x} \\ 0 & 0 & 0 & 1 \\ 0 & -\frac{2C_{af}\ell_f-2C_{ar}\ell_r}{I_zV_x} & 0 & -\frac{2C_{af}\ell_f^2+2C_{ar}\ell_r^2}{I_zV_x} \end{bmatrix}\begin{bmatrix} y \\ \dot{y} \\ \varphi \\ \dot{\varphi} \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{2C_{af}}{m} \\ 0 \\ \frac{2\ell_fC_{af}}{I_z} \end{bmatrix} \delta \tag{13} dtd​⎣⎢⎢⎡​yy˙​φφ˙​​⎦⎥⎥⎤​=⎣⎢⎢⎢⎡​0000​1−mVx​2Caf​+2Car​​0−Iz​Vx​2Caf​ℓf​−2Car​ℓr​​​0000​0−Vx​−mVx​2Caf​ℓf​−2Car​ℓr​​1−Iz​Vx​2Caf​ℓf2​+2Car​ℓr2​​​⎦⎥⎥⎥⎤​⎣⎢⎢⎡​yy˙​φφ˙​​⎦⎥⎥⎤​+⎣⎢⎢⎡​0m2Caf​​0Iz​2ℓf​Caf​​​⎦⎥⎥⎤​δ(13)

方向盘控制模型

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

假设，e1e_1e1​为横向误差，车辆质心距车道中心线的距离，e2e_2e2​为航向误差，车辆纵向速度为VxV_xVx​，车辆转弯半径为$R$，结合图1、图2、图3，则：
车身转过期望角度所需转角速度
(15)φ˙des=VxR\dot{\varphi}_{des}=\frac{V_x}{R} \tag{15}φ˙​des​=RVx​​(15)
所需横向加速度
(16)aydes=Vx2R=Vxφ˙desa_{y_{des}}=\frac{V_x^2}{R} = V_x\dot{\varphi}_{des} \tag{16}aydes​​=RVx2​​=Vx​φ˙​des​(16)
则横向加速度误差
(17)e1¨=ay−aydes=(y¨+Vxφ˙)−Vx2R=y¨+Vx(φ˙−φ˙des)\ddot{e_1}=a_y-a_{y_{des}}=(\ddot{y}+V_x\dot{\varphi})- \frac{V_x^2}{R}=\ddot{y}+V_x(\dot{\varphi}-\dot{\varphi}_{des})\tag{17}e1​¨​=ay​−aydes​​=(y¨​+Vx​φ˙​)−RVx2​​=y¨​+Vx​(φ˙​−φ˙​des​)(17)
横行速度误差为
(18)e1˙=y˙+Vx(φ−φdes)\dot{e_1}=\dot{y}+V_x(\varphi-\varphi_{des})\tag{18}e1​˙​=y˙​+Vx​(φ−φdes​)(18)
航向误差
(19)e2=φ−φdese_2=\varphi-\varphi_{des} \tag{19}e2​=φ−φdes​(19)

将公式18、公式19带入公式3、公式4可得：
(20)m(e¨1+Vxφ˙des)=e˙1[−2CαfVx−2CαrVx]+e2[2Cαf+2Cαr]+e˙2[−2CαfℓfVx+2CαrℓrVx]+φ˙des[−2CαfℓfVx+2CαrℓrVx]+2Cαfδm(\ddot{e}_1+V_x\dot{\varphi}_{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{\varphi}_{des}[-\frac{2C_{\alpha f}\ell_f}{V_x}+\frac{2C_{\alpha r}\ell_r}{V_x}]+2C_{\alpha f}\delta \tag{20}m(e¨1​+Vx​φ˙​des​)=e˙1​[−Vx​2Cαf​​−Vx​2Cαr​​]+e2​[2Cαf​+2Cαr​]+e˙2​[−Vx​2Cαf​ℓf​​+Vx​2Cαr​ℓr​​]+φ˙​des​[−Vx​2Cαf​ℓf​​+Vx​2Cαr​ℓr​​]+2Cαf​δ(20)

(21)Ize¨2=2Cαfℓfδ+e˙1[−2CαfℓfVx+2CαrℓrVx]+e2[2Cαfℓf−2Cαrℓr]+e˙2[−2Cαfℓf2Vx−2Cαrℓr2Vx]−Izφ¨des+φ˙[−2Cαfℓf2Vx−2Cαrℓr2Vx]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{\varphi}_{des}+\dot{\varphi}[-\frac{2C_{\alpha f}\ell_f^2}{V_x}-\frac{2C_{\alpha r}\ell_r^2}{V_x}] \tag{21}Iz​e¨2​=2Cαf​ℓf​δ+e˙1​[−Vx​2Cαf​ℓf​​+Vx​2Cαr​ℓr​​]+e2​[2Cαf​ℓf​−2Cαr​ℓr​]+e˙2​[−Vx​2Cαf​ℓf2​​−Vx​2Cαr​ℓr2​​]−Iz​φ¨​des​+φ˙​[−Vx​2Cαf​ℓf2​​−Vx​2Cαr​ℓr2​​](21)
公式20可能与书中第2章公式2.34略有出入，个人认为是书本编者笔误，感兴趣的可以自己推一下。

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

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

分别对应e1e_1e1​、e˙1\dot{e}_1e˙1​、e2e_2e2​、e˙2\dot{e}_2e˙2​
综上，可得方向盘控制的动力学模型：
(24)ddt[e1e˙1e2e˙2]=[01000−2Caf+2CarmVx2Caf+2Carm−2Cafℓf+2CarℓrmVx00010−2Cafℓf−2CarℓrIzVx2Cafℓf−2CarℓrIz−2Cafℓf2+2Carℓr2IzVx][e1e˙1e2e˙2]+[02Cαfm02CαfℓfIz]δ+[0−2Cafℓf−2CarℓrmVx−Vx0−2Cafℓf2+2Carℓr2IzVx]φ˙des\frac{d}{dt} \begin{bmatrix} e_1 \\ \dot{e}_1 \\ e_2 \\ \dot{e}_2 \end{bmatrix}= \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & -\frac{2C_{af}+2C_{ar}}{mV_x} & \frac{2C_{af}+2C_{ar}}{m} & \frac{-2C_{af}\ell_f+2C_{ar}\ell_r}{mV_x}\\ 0 & 0 & 0 & 1 \\ 0 & -\frac{2C_{af}\ell_f-2C_{ar}\ell_r}{I_zV_x} & \frac{2C_{af}\ell_f-2C_{ar}\ell_r}{I_z} & -\frac{2C_{af}\ell_f^2+2C_{ar}\ell_r^2}{I_zV_x} \end{bmatrix} \begin{bmatrix} e_1 \\ \dot{e}_1 \\ e_2 \\ \dot{e}_2 \end{bmatrix} \\ +\begin{bmatrix} 0 \\ \frac{2C_{\alpha f}}{m} \\ 0 \\ \frac{2C_{\alpha f}\ell_f}{I_z} \end{bmatrix}\delta+ \begin{bmatrix} 0 \\ -\frac{2C_{af}\ell_f-2C_{ar}\ell_r}{mV_x} -V_x\\ 0 \\ -\frac{2C_{af}\ell_f^2+2C_{ar}\ell_r^2}{I_zV_x} \end{bmatrix}\dot{\varphi}_{des} \tag{24} dtd​⎣⎢⎢⎡​e1​e˙1​e2​e˙2​​⎦⎥⎥⎤​=⎣⎢⎢⎢⎡​0000​1−mVx​2Caf​+2Car​​0−Iz​Vx​2Caf​ℓf​−2Car​ℓr​​​0m2Caf​+2Car​​0Iz​2Caf​ℓf​−2Car​ℓr​​​0mVx​−2Caf​ℓf​+2Car​ℓr​​1−Iz​Vx​2Caf​ℓf2​+2Car​ℓr2​​​⎦⎥⎥⎥⎤​⎣⎢⎢⎡​e1​e˙1​e2​e˙2​​⎦⎥⎥⎤​+⎣⎢⎢⎡​0m2Cαf​​0Iz​2Cαf​ℓf​​​⎦⎥⎥⎤​δ+⎣⎢⎢⎢⎡​0−mVx​2Caf​ℓf​−2Car​ℓr​​−Vx​0−Iz​Vx​2Caf​ℓf2​+2Car​ℓr2​​​⎦⎥⎥⎥⎤​φ˙​des​(24)

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

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 testdebug->set_lateral_error_rate(linear_v * sin_delta_theta);

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

图5 横向误差示意图

则横向误差的表达式为：
(25){e1=dy∗cos⁡θdes−dx∗sin⁡θdese1˙=Vx∗sin⁡Δθ=Vx∗sin⁡e2e2=θ−θdese2˙=θ˙−θ˙des\begin{cases} e_1=dy*\cos{\theta_{des}}-dx*\sin{\theta_{des}} \\ \dot{e_1}=V_x*\sin{\Delta\theta} =V_x*\sin{e_2} \\ e_2=\theta-\theta_{des}\\ \dot{e_2}=\dot{\theta}-\dot{\theta}_{des} \end{cases} \tag{25} ⎩⎪⎪⎪⎨⎪⎪⎪⎧​e1​=dy∗cosθdes​−dx∗sinθdes​e1​˙​=Vx​∗sinΔθ=Vx​∗sine2​e2​=θ−θdes​e2​˙​=θ˙−θ˙des​​(25)
其中，e1e_1e1​为横向误差($latteral\_error$)，e1˙\dot{e_1}e1​˙​为横向误差率($latteral\_error\_rate$)，e2e_2e2​为航向误差($heading\_error$)，e2˙\dot{e_2}e2​˙​为航向误差率($heading\_error\_rate$)；θ˙\dot{\theta}θ˙为车辆转角速度，可由车身传感器测得；θ˙des\dot{\theta}_{des}θ˙des​为期望车辆转角速度，由规划参数获得：
θ˙des=Vdes∗kdes\dot{\theta}_{des}=V_{des}*k_{des}θ˙des​=Vdes​∗kdes​
其中，VdesV_{des}Vdes​为期望车速，kdesk_{des}kdes​期望道路曲率，具体可见apollo中对路径规划参数结构体的定义planning.proto。

总结

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

补充 2018.11.27

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