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即可简单搭建车辆的动力学模型:

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

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

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

图2中各符号定义:

符号定义
F l f , F l r F_{lf},F_{lr} Flf,Flr前、后轮胎受到的纵向力
F c f , F c r F_{cf},F_{cr} Fcf,Fcr前、后轮胎受到的侧向力
F x f , F x r F_{xf},F_{xr} Fxf,Fxr前、后轮胎受到的 x x x方向的力
F y f , F y r F_{yf},F_{yr} Fyf,Fyr前、后轮胎受到的 y y y方向的力
a a a前悬长度
b b b后悬长度
δ f \delta_f δf前轮偏角
δ r \delta_r δr后轮偏角
α f \alpha_f αf前轮偏移角

根据牛顿第二定律,分别沿 x x x轴、 y y y轴和 z z z轴作受力分析:
x x x轴方向上:
(1) m a x = F x f + F x r ma_x = F_{xf} + F_{xr} \tag{1} max=Fxf+Fxr(1)
y y y轴方向上:
(2) m a y = F y f + F y r ma_y = F_{yf} + F_{yr} \tag{2} may=Fyf+Fyr(2)
z z z轴方向上:

(3) I z φ ¨ = a F y f − b F y r I_z\ddot{\varphi} = aF_{yf} - bF_{yr} \tag{3} Izφ¨=aFyfbFyr(3)
其中, m m m为整车质量, I z I_z Iz为车辆绕 z z z轴转动的转动惯量。 x x x轴方向的运动(绕纵轴的滚动运动)可暂不用考虑。

横向动力学

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

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

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

(4) m ( y ¨ + V x φ ˙ ) = F y f + F y r m(\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 = δ − θ V f \alpha_f = \delta - \theta_{Vf} \tag{5} αf=δθVf(5)
其中, θ V f \theta_{Vf} θVf为前轮速度方向, δ \delta δ为前轮转角。
后轮滑移角
(6) α r = − θ V r \alpha_r = - \theta_{Vr} \tag{6} αr=θVr(6)
其中, θ V r \theta_{Vr} θVr为后轮速度方向。
则前轮所受的横向力为
(7) F y f = 2 C α f ( δ − θ V f ) F_{yf} = 2C_{\alpha f}(\delta - \theta_{Vf} ) \tag{7} Fyf=2Cαf(δθVf)(7)
后轮所受的横向力为
(8) F y r = 2 C α r ( − θ V r ) F_{yr} = 2C_{\alpha r}(- \theta_{Vr} ) \tag{8} Fyr=2Cαr(θVr)(8)
其中, C α f C_{\alpha f} Cαf C α r C_{\alpha r} Cαr分别为前后轮的 侧 偏 刚 度 ( c o r n e r i n g   s t i f f n e s s ) \color{red}侧偏刚度(cornering \ stiffness) (cornering stiffness),由于车辆前后各两个轮,所以受力要乘以2。

结合图4, θ V f \theta_{Vf} θVf θ V r \theta_{Vr} θVr可用下式计算:
(9) tan ⁡ ( θ V f ) = V y + ℓ f φ ˙ V x \tan(\theta_{Vf}) = \frac{V_y+\ell_f\dot{\varphi}}{V_x} \tag{9} tan(θVf)=VxVy+fφ˙(9)
(10) tan ⁡ ( θ V r ) = V y − ℓ r φ ˙ V x \tan(\theta_{Vr}) = \frac{V_y-\ell_r\dot{\varphi}}{V_x} \tag{10} tan(θVr)=VxVyrφ˙(10)
其中, ℓ f \ell_f f为前悬长度, ℓ r \ell_r r为后悬长度。
y ˙ = V y \dot{y}=V_y y˙=Vy,则公式9、公式10可近似转换为:
(11) θ V f = y ˙ + ℓ f φ ˙ V x \theta_{Vf}=\frac{\dot{y}+\ell_f\dot{\varphi}}{V_x} \tag{11} θVf=Vxy˙+fφ˙(11)
(12) θ V r = y ˙ − ℓ r φ ˙ V x \theta_{Vr}=\frac{\dot{y} - \ell_r\dot{\varphi}}{V_x} \tag{12} θVr=Vxy˙rφ˙(12)
将公式5、公式6、公式11、公式12代入公式2、公式3中可得动力学模型:
(13) d d t [ y y ˙ φ φ ˙ ] = [ 0 1 0 0 0 − 2 C a f + 2 C a r m V x 0 − V x − 2 C a f ℓ f − 2 C a r ℓ r m V x 0 0 0 1 0 − 2 C a f ℓ f − 2 C a r ℓ r I z V x 0 − 2 C a f ℓ f 2 + 2 C a r ℓ r 2 I z V x ] [ y y ˙ φ φ ˙ ] + [ 0 2 C a f m 0 2 ℓ f C a f I z ] δ \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} dtdyy˙φφ˙=00001mVx2Caf+2Car0IzVx2Caff2Carr00000VxmVx2Caff2Carr1IzVx2Caff2+2Carr2yy˙φφ˙+0m2Caf0Iz2fCafδ(13)

方向盘控制模型

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

假设, e 1 e_1 e1为横向误差,车辆质心距车道中心线的距离, e 2 e_2 e2为航向误差,车辆纵向速度为 V x V_x Vx,车辆转弯半径为 R R R,结合图1、图2、图3,则:
车身转过期望角度所需转角速度
(15) φ ˙ d e s = V x R \dot{\varphi}_{des}=\frac{V_x}{R} \tag{15} φ˙des=RVx(15)
所需横向加速度
(16) a y d e s = V x 2 R = V x φ ˙ d e s a_{y_{des}}=\frac{V_x^2}{R} = V_x\dot{\varphi}_{des} \tag{16} aydes=RVx2=Vxφ˙des(16)
则横向加速度误差
(17) e 1 ¨ = a y − a y d e s = ( y ¨ + V x φ ˙ ) − V x 2 R = y ¨ + V x ( φ ˙ − φ ˙ d e s ) \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¨=ayaydes=(y¨+Vxφ˙)RVx2=y¨+Vx(φ˙φ˙des)(17)
横行速度误差为
(18) e 1 ˙ = y ˙ + V x ( φ − φ d e s ) \dot{e_1}=\dot{y}+V_x(\varphi-\varphi_{des})\tag{18} e1˙=y˙+Vx(φφdes)(18)
航向误差
(19) e 2 = φ − φ d e s e_2=\varphi-\varphi_{des} \tag{19} e2=φφdes(19)

将公式18、公式19带入公式3公式4可得:
(20) m ( e ¨ 1 + V x φ ˙ d e s ) = e ˙ 1 [ − 2 C α f V x − 2 C α r V x ] + e 2 [ 2 C α f + 2 C α r ] + e ˙ 2 [ − 2 C α f ℓ f V x + 2 C α r ℓ r V x ] + φ ˙ d e s [ − 2 C α f ℓ f V x + 2 C α r ℓ r V x ] + 2 C α 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[Vx2CαfVx2Cαr]+e2[2Cαf+2Cαr]+e˙2[Vx2Cαff+Vx2Cαrr]+φ˙des[Vx2Cαff+Vx2Cαrr]+2Cαfδ(20)

(21) I z e ¨ 2 = 2 C α f ℓ f δ + e ˙ 1 [ − 2 C α f ℓ f V x + 2 C α r ℓ r V x ] + e 2 [ 2 C α f ℓ f − 2 C α r ℓ r ] + e ˙ 2 [ − 2 C α f ℓ f 2 V x − 2 C α r ℓ r 2 V x ] − I z φ ¨ d e s + φ ˙ [ − 2 C α f ℓ f 2 V x − 2 C α r ℓ r 2 V x ] 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} Ize¨2=2Cαffδ+e˙1[Vx2Cαff+Vx2Cαrr]+e2[2Cαff2Cαrr]+e˙2[Vx2Cαff2Vx2Cαrr2]Izφ¨des+φ˙[Vx2Cαff2Vx2Cαrr2](21)
公式20可能与书中第2章公式2.34略有出入,个人认为是书本编者笔误,感兴趣的可以自己推一下。

假设车辆系统的状态空间方程为:
(22) X ˙ = A X + B u \dot{X}=AX+Bu \tag{22} X˙=AX+Bu(22)
(23) Y = C X + D u Y=CX+Du \tag{23} Y=CX+Du(23)
在apollo中横向控制中,系统的状态变量有四个:

  • 横向误差 l a t e r a l _ e r r o r lateral\_error lateral_error
  • 横向误差率 l a t e r a l _ e r r o r _ r a t e lateral\_error\_rate lateral_error_rate
  • 航向误差 h e a d i n g _ e r r o r heading\_error heading_error
  • 航向误差率 h e a d i n g _ e r r o r _ r a t e heading\_error\_rate heading_error_rate

分别对应 e 1 e_1 e1 e ˙ 1 \dot{e}_1 e˙1 e 2 e_2 e2 e ˙ 2 \dot{e}_2 e˙2
综上,可得方向盘控制的动力学模型:
(24) d d t [ e 1 e ˙ 1 e 2 e ˙ 2 ] = [ 0 1 0 0 0 − 2 C a f + 2 C a r m V x 2 C a f + 2 C a r m − 2 C a f ℓ f + 2 C a r ℓ r m V x 0 0 0 1 0 − 2 C a f ℓ f − 2 C a r ℓ r I z V x 2 C a f ℓ f − 2 C a r ℓ r I z − 2 C a f ℓ f 2 + 2 C a r ℓ r 2 I z V x ] [ e 1 e ˙ 1 e 2 e ˙ 2 ] + [ 0 2 C α f m 0 2 C α f ℓ f I z ] δ + [ 0 − 2 C a f ℓ f − 2 C a r ℓ r m V x − V x 0 − 2 C a f ℓ f 2 + 2 C a r ℓ r 2 I z V x ] φ ˙ d e s \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} dtde1e˙1e2e˙2=00001mVx2Caf+2Car0IzVx2Caff2Carr0m2Caf+2Car0Iz2Caff2Carr0mVx2Caff+2Carr1IzVx2Caff2+2Carr2e1e˙1e2e˙2+0m2Cαf0Iz2Cαffδ+0mVx2Caff2CarrVx0IzVx2Caff2+2Carr2φ˙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 test
  debug->set_lateral_error_rate(linear_v * sin_delta_theta);

如图5,为横向误差计算的示意图:

图5 横向误差示意图

则横向误差的表达式为:
(25) { e 1 = d y ∗ cos ⁡ θ d e s − d x ∗ sin ⁡ θ d e s e 1 ˙ = V x ∗ sin ⁡ Δ θ = V x ∗ sin ⁡ e 2 e 2 = θ − θ d e s e 2 ˙ = θ ˙ − θ ˙ d e s \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=dycosθdesdxsinθdese1˙=VxsinΔθ=Vxsine2e2=θθdese2˙=θ˙θ˙des(25)
其中, e 1 e_1 e1为横向误差( l a t t e r a l _ e r r o r latteral\_error latteral_error), e 1 ˙ \dot{e_1} e1˙为横向误差率( l a t t e r a l _ e r r o r _ r a t e latteral\_error\_rate latteral_error_rate), e 2 e_2 e2为航向误差( h e a d i n g _ e r r o r heading\_error heading_error), e 2 ˙ \dot{e_2} e2˙为航向误差率( h e a d i n g _ e r r o r _ r a t e heading\_error\_rate heading_error_rate); θ ˙ \dot{\theta} θ˙为车辆转角速度,可由车身传感器测得; θ ˙ d e s \dot{\theta}_{des} θ˙des为期望车辆转角速度,由规划参数获得:
θ ˙ d e s = V d e s ∗ k d e s \dot{\theta}_{des}=V_{des}*k_{des} θ˙des=Vdeskdes
其中, V d e s V_{des} Vdes为期望车速, k d e s k_{des} kdes期望道路曲率,具体可见apollo中对路径规划参数结构体的定义planning.proto

总结

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

补充 2018.11.27

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


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

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

打赏
文章很值,打赏犒劳作者一下
相关推荐
©️2020 CSDN 皮肤主题: 游动-白 设计师:白松林 返回首页

打赏

follow轻尘

你的鼓励将是我创作的最大动力

¥2 ¥4 ¥6 ¥10 ¥20
输入1-500的整数
余额支付 (余额:-- )
扫码支付
扫码支付:¥2
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者