接上一篇: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所示, 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) (slip−angle):轮胎方向和轮胎速度方向的夹角。滑移角的产生主要是由于车轮所受合力方向并非朝向车轮行进方向,但车轮的偏移角通常较小。
图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φ¨=aFyf−bFyr(3)
其中, m m m为整车质量, I z I_z Iz为车辆绕 z z z轴转动的转动惯量。 x x x轴方向的运动(绕纵轴的滚动运动)可暂不用考虑。
可结合Vehicle Dynamics and Control2第2章和第13章进行研究。
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所示
前轮滑移角
(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)=VxVy−ℓrφ˙(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} dtd⎣⎢⎢⎡yy˙φφ˙⎦⎥⎥⎤=⎣⎢⎢⎢⎡00001−mVx2Caf+2Car0−IzVx2Cafℓf−2Carℓr00000−Vx−mVx2Cafℓf−2Carℓr1−IzVx2Cafℓf2+2Carℓr2⎦⎥⎥⎥⎤⎣⎢⎢⎡yy˙φφ˙⎦⎥⎥⎤+⎣⎢⎢⎡0m2Caf0Iz2ℓfCaf⎦⎥⎥⎤δ(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¨=ay−aydes=(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αf−Vx2Cαr]+e2[2Cαf+2Cαr]+e˙2[−Vx2Cαfℓf+Vx2Cαrℓr]+φ˙des[−Vx2Cαfℓf+Vx2Cαrℓr]+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αfℓfδ+e˙1[−Vx2Cαfℓf+Vx2Cαrℓr]+e2[2Cαfℓf−2Cαrℓr]+e˙2[−Vx2Cαfℓf2−Vx2Cαrℓr2]−Izφ¨des+φ˙[−Vx2Cαfℓf2−Vx2Cαrℓr2](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中横向控制中,系统的状态变量有四个:
分别对应 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} dtd⎣⎢⎢⎡e1e˙1e2e˙2⎦⎥⎥⎤=⎣⎢⎢⎢⎡00001−mVx2Caf+2Car0−IzVx2Cafℓf−2Carℓr0m2Caf+2Car0Iz2Cafℓf−2Carℓr0mVx−2Cafℓf+2Carℓr1−IzVx2Cafℓf2+2Carℓr2⎦⎥⎥⎥⎤⎣⎢⎢⎡e1e˙1e2e˙2⎦⎥⎥⎤+⎣⎢⎢⎡0m2Cαf0Iz2Cαfℓf⎦⎥⎥⎤δ+⎣⎢⎢⎢⎡0−mVx2Cafℓf−2Carℓr−Vx0−IzVx2Cafℓ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 test
debug->set_lateral_error_rate(linear_v * sin_delta_theta);
如图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=dy∗cosθdes−dx∗sinθdese1˙=Vx∗sinΔθ=Vx∗sine2e2=θ−θ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=Vdes∗kdes
其中, V d e s V_{des} Vdes为期望车速, k d e s k_{des} kdes期望道路曲率,具体可见apollo中对路径规划参数结构体的定义planning.proto。
至此,车辆运动学和动力学模型搭建完毕,如有偏颇或错误的地方望指正。
接下来会具体分析控制模块中横向控制用于求取最优控制解的LQR算法,及MPC模型预测控制算法。
对于横向误差计算不理解的请参考另一篇文章中的补充内容:Apollo代码学习(五)—横纵向控制
龚建伟, 姜岩, 徐威. 无人驾驶车辆模型预测控制[M]. 北京理工大学出版社, 2014. ↩︎
Rajamani R. Vehicle Dynamics and Control[M]. Springer Science, 2006. ↩︎