欢迎加入我的Apollo专属课程,本课程将利用本人无人驾驶学习经验,并结合Apollo开发者社区丰富资源,为大家提供学习无人驾驶知识的平台。现在学习课程达到50%进度即可领取Apollo精美周边,快来参加吧!!!
课程链接
参与课程及领取礼品方式可参考此链接
六篇文章带你解析Apollo控制模块 - Part 3:车辆动力学模型
知识点
- 理论基础:二自由度动力学模型
- 轮胎侧偏刚度模型
- 车辆与参考路径误差模型
摘要
动力学模型分析是整个无人车控制模块的基础,同时也是动力学模型是否准确也直接影响控制模块的精度。Apollo横向控制LQR和MPC算法中均需要汽车动力学模型进行控制量计算,所以我对这部分资料进行了整理记录。
本文主要参考了《汽车学》和《车辆动力学控制》两本书籍。首先介绍了车辆的轮胎坐标系模型,明确参考系的正方向,描述侧偏力、侧偏角以及侧偏刚度之间的关系;之后,计算推导车辆二自由度模型中横向加速度、横摆角加速度与轮胎转角之间得状态空间方程;最后,由二自由度模型计算车辆与参考路径的横向误差以及航向角误差,推导出其状态空间方程。
动力学模型只是为控制模块打基础,之后会继续更新Apollo控制模块的内容。
一、预备知识
参考书籍:
《高等学校教材:汽车学》
《车辆动力学控制(美)拉贾马尼著》
需要资源可私信我
二、轮胎侧偏刚度模型
如图所示,车辆从左至右行驶。轮胎坐标系定义为:沿着行驶方向为
x
x
x轴正方向,左侧垂直于行驶方向为
y
y
y轴正方向。
图中 F f y F_{fy} Ffy为侧偏力,即地面对轮胎的作用力,根据坐标系可知方向为正;车体通过车轴对轮胎的作用力与侧偏力方向相反。轮胎受上述两个力的作用会产生负的侧偏角,具体侧偏角分析过程可参考《汽车学》第一章第二节。
侧偏力公式 为:
F
=
C
α
F=C \alpha
F=Cα
其中F为侧偏力,C为侧偏刚度系数,
α
\alpha
α为侧偏角。
根据轮胎坐标系有关符号定义可知,正的侧偏力产生负的侧偏角,因此侧偏刚度为负值。不同坐标系的定义可能不同,Apollo中侧偏刚度系数为正值,相应的状态方程也会有所改变,状态方程一般只会有正负号得差别,大家看到相应公式注意一下就行。
汽车正常行驶时,侧向加速度不超过0.4g,侧偏角不超过4°~5°,可以近似认为侧偏角与侧偏力呈线性关系。轮胎的侧偏特性曲线在
α
=
0
°
\alpha=0°
α=0°处的斜率称为侧偏刚度,单位为N/rad 。
小型轿车轮胎的侧偏刚度在(-28000 ~ -80000)N/rad范围内。侧偏刚度是决定操纵稳定性的重要轮胎参数。
三、车辆二自由度模型公式推导
参数名称 | 符号 |
---|---|
质量 | m m m |
横向速度 | v v v |
纵向速度 | u u u |
横摆角(航向角) | φ \varphi φ |
横摆角速度 | φ ˙ \dot{\varphi} φ˙ |
z轴转动惯量 | I z I_{z} Iz |
前轮转角 | δ \delta δ |
前/后轮胎沿x轴的力 | F x f / F x r F_{xf} / F_{xr} Fxf/Fxr |
前/后轮胎沿y轴的力 | F y f / F y r F_{yf} / F_{yr} Fyf/Fyr |
前后轮胎侧偏角 | α f / α r \alpha_{f} / \alpha_{r} αf/αr |
前后轮胎侧偏刚度 | C f / C r C_{f} / C_{r} Cf/Cr |
前后轴到质心距离 | L f / L r L_{f} / L_{r} Lf/Lr |
轴距 | L |
质心侧偏角 | β \beta β |
将车辆简化为线性二自由度的自行车模型。不考虑方向盘等转向系统的影响,忽略方向盘的转动与前轮转角的关系,直接以前轮转角作为模型的输入;不考虑地面重构单元运动过程中悬架系统的影响,忽略z方向的运动以及俯仰运动和侧倾运动。
假定特定条件下沿x轴的纵向速度不变,据此车辆只有沿y轴的侧向运动和绕z轴横摆运动两个自由度,并且地面重构单元的轮胎的侧偏刚度在线性范围内;忽略驱动力对于轮胎侧偏特性的影响以及空气阻力,并且忽略轮胎回正力矩。将车辆坐标系的原点设在质心处,因为将质心设为原点那么车辆的转动惯量等质量分布参数对于质心坐标系来说是个常值,此时将车辆的绝对加速度与绝对角加速度以及外力与外力矩沿质心坐标系的轴线分解,就可以列出汽车运动微分方程。
二自由度汽车在前轮转角为
δ
\delta
δ时,受到的外力和外力矩沿y轴的分量为:
∑
F
Y
=
m
a
y
=
F
y
f
cos
δ
+
F
y
r
∑
M
Z
=
I
z
φ
¨
=
L
f
F
y
f
cos
δ
−
L
r
F
y
r
(1)
\begin{align} \sum F_{Y} =& ma_{y} &=& F_{yf}\cos \delta +F_{yr} \\ \sum M_{Z} =& I_{z}\ddot{\varphi } &=&L_{f} F_{yf}\cos \delta - L_{r} F_{yr} \end{align} \tag{1}
∑FY=∑MZ=mayIzφ¨==Fyfcosδ+FyrLfFyfcosδ−LrFyr(1)
当 δ \delta δ很小时,侧偏力公式 F y f = C f α f 、 F y r = C r α r F_{yf}=C_{f} \alpha_{f} 、F_{yr}=C_{r} \alpha_{r} Fyf=Cfαf、Fyr=Crαr代入式(1)动力学公式
∑ F Y = m a y = C f α f + C r α r ∑ M Z = I z φ ¨ = L f C f α f − L r C r α r (2) \begin{align} \sum F_{Y}=& ma_{y} &=& C_{f} \alpha_{f} +C_{r} \alpha_{r}\\ \sum M_{Z}=& I_{z}\ddot{\varphi } &=&L_{f} C_{f} \alpha_{f} - L_{r}C_{r} \alpha_{r} \end{align} \tag{2} ∑FY=∑MZ=mayIzφ¨==Cfαf+CrαrLfCfαf−LrCrαr(2)
由图1所示,汽车前、后轴中点的速度为
u
1
u_{1}
u1 、
u
2
u_{2}
u2 ,汽车质心的侧偏角为
β
\beta
β,
β
=
v
/
u
\beta= v/u
β=v/u。
ξ
\xi
ξ是
u
1
u_{1}
u1与
x
x
x轴的夹角,其值为:
ξ
=
v
+
L
f
φ
˙
u
=
β
+
L
f
φ
˙
u
\xi =\frac{v+L_{f}\dot{\varphi}}{u} =\beta +\frac{L_{f}\dot{\varphi}}{u}
ξ=uv+Lfφ˙=β+uLfφ˙
根据坐标系的规定,前后轮侧偏角为:
C
f
=
−
(
δ
−
ξ
)
=
β
+
L
f
φ
˙
u
−
δ
C
r
=
v
−
L
r
φ
˙
u
=
β
−
L
r
φ
˙
u
(3)
\begin{align} C_{f}&= -(\delta -\xi ) = \beta +\frac{L_{f}\dot{\varphi}}{u}-\delta \\ C_{r}&=\frac{v-L_{r}\dot{\varphi}}{u}=\beta -\frac{L_{r}\dot{\varphi}}{u} \end{align} \tag{3}
CfCr=−(δ−ξ)=β+uLfφ˙−δ=uv−Lrφ˙=β−uLrφ˙(3)
接下来利用固结于汽车上的车辆坐标系分析车辆横向加速度
a
y
a_{y}
ay与横向速度
v
v
v、纵向速度
u
u
u和横摆角速度
φ
˙
\dot{\varphi}
φ˙之间的关系。如下图所示,
O
y
O_{y}
Oy和
O
x
O_{x}
Ox为车辆坐标系的纵轴和横轴,质心速度于
t
t
t时刻在
O
x
O_{x}
Ox轴的分量为
u
u
u,在
O
y
O_{y}
Oy轴上分量为
v
v
v,车辆在水平路面上的行驶运动均可分解为质心的平移运动和绕质心的转动。
在
t
+
∆
t
t+∆t
t+∆t时刻,车辆坐标系中质心速度的大小与方向均发生变化,而车辆坐标系的纵轴与横轴的方向也发生变化。在计算质心处的加速度时,以
t
t
t时刻的车辆坐标系作为参考坐标系,此时车辆沿
O
y
O_{y}
Oy轴速度分量变化为:
(
v
+
Δ
v
)
cos
Δ
θ
−
v
+
(
u
+
Δ
u
)
sin
Δ
θ
=
v
cos
Δ
θ
+
Δ
v
cos
Δ
θ
−
v
+
u
sin
Δ
θ
+
Δ
u
sin
Δ
θ
\begin{array}{l} (v+\Delta v) \cos \Delta \theta-v+(u+\Delta u) \sin \Delta \theta \\ =v \cos \Delta \theta+\Delta v \cos \Delta \theta-v+u \sin \Delta \theta+\Delta u \sin \Delta \theta \end{array}
(v+Δv)cosΔθ−v+(u+Δu)sinΔθ=vcosΔθ+ΔvcosΔθ−v+usinΔθ+ΔusinΔθ
考虑到
Δ
θ
\Delta \theta
Δθ很小,则
sin
Δ
θ
≈
Δ
θ
,
cos
Δ
θ
≈
1
\sin \Delta\theta \approx \Delta\theta,\cos\Delta \theta \approx 1
sinΔθ≈Δθ,cosΔθ≈1,并略去二阶小量,上式变成:
Δ
v
+
u
Δ
θ
\Delta v+u\Delta \theta
Δv+uΔθ
把上式除以∆t并取极限,可以得到质心加速度在
t
t
t时刻车辆坐标系的
y
y
y轴上的分量为:
a y = d v d t + u d θ d t = v ˙ + u φ ˙ (4) \begin{align} a_{y} = \frac{dv}{dt}+u \frac{d\theta }{dt}=\dot{v} +u\dot{\varphi } \end{align} \tag{4} ay=dtdv+udtdθ=v˙+uφ˙(4)
将式(3)、式(4)代入式(2)得到二自由度汽车运动微分方程式:
v
u
(
C
f
+
C
r
)
+
1
u
(
L
f
C
f
−
L
r
C
r
)
φ
˙
−
C
f
δ
=
m
(
v
˙
+
u
φ
˙
)
v
u
(
L
f
C
f
−
L
r
C
r
)
+
1
u
(
L
f
2
C
f
+
L
r
2
C
r
)
φ
˙
−
L
f
C
f
δ
=
I
z
φ
¨
(5)
\begin{align} \frac{v}{u}(C_{f}+C_{r}) +\frac{1}{u} (L_{f} C_{f}-L_{r}C_{r})\dot{\varphi } -C_{f}\delta =m(\dot{v}+u\dot{\varphi } ) \\ \frac{v}{u}(L_{f} C_{f}-L_{r}C_{r})+\frac{1}{u} (L_{f}^{2} C_{f}+L_{r}^{2} C_{r})\dot{\varphi }-L_{f}C_{f}\delta=I_{z}\ddot{\varphi } \end{align} \tag{5}
uv(Cf+Cr)+u1(LfCf−LrCr)φ˙−Cfδ=m(v˙+uφ˙)uv(LfCf−LrCr)+u1(Lf2Cf+Lr2Cr)φ˙−LfCfδ=Izφ¨(5)
将
β
=
v
/
u
\beta= v/u
β=v/u代入式(5),并将其中
y
¨
=
a
y
=
v
˙
+
u
φ
˙
,
y
˙
=
v
\ddot{y}=a_{y}=\dot{v}+u\dot{\varphi} , \dot{y}=v
y¨=ay=v˙+uφ˙,y˙=v进行符号替代,可得:
注意,这里设置
y
¨
=
a
y
=
v
˙
+
u
φ
˙
\ddot{y}=a_{y}=\dot{v}+u\dot{\varphi}
y¨=ay=v˙+uφ˙,官方版本是
y
¨
=
v
˙
\ddot{y}=\dot{v}
y¨=v˙,所以推导出的状态空间方程有一项不一样
y
¨
=
C
f
+
C
r
m
u
y
˙
+
L
f
C
f
−
L
r
C
r
m
u
φ
˙
−
C
f
m
δ
φ
¨
=
L
f
C
f
−
L
r
C
r
I
z
u
y
˙
+
L
f
2
C
f
+
L
r
2
C
r
I
z
u
φ
˙
−
L
f
C
f
I
z
δ
(6)
\begin{align} \ddot{y}&=\frac{C_{f}+C_{r}}{mu} \dot{y} +\frac{ L_{f} C_{f}-L_{r}C_{r}}{mu}\dot{\varphi } -\frac{ C_{f}}{m}\delta \\ \ddot{\varphi }&= \frac{L_{f} C_{f}-L_{r}C_{r}}{I_{z}u} \dot{y} +\frac{L_{f}^{2} C_{f}+L_{r}^{2} C_{r}}{I_{z}u} \dot{\varphi }-\frac{L_{f}C_{f}}{ I_{z}}\delta \end{align} \tag{6}
y¨φ¨=muCf+Cry˙+muLfCf−LrCrφ˙−mCfδ=IzuLfCf−LrCry˙+IzuLf2Cf+Lr2Crφ˙−IzLfCfδ(6)
将式(6)整理,可得二自由度模型状态空间方程,即横向加速度、横摆角加速度与轮胎转角之间的关系:
d
d
t
[
y
y
˙
φ
φ
˙
]
=
[
0
1
0
0
0
C
f
+
C
r
m
u
0
L
f
∗
C
f
−
L
r
∗
C
r
m
u
0
0
0
1
0
L
f
∗
C
f
−
L
r
∗
C
r
I
z
u
0
L
f
2
∗
C
f
+
L
r
2
∗
C
r
I
z
u
]
[
y
y
˙
φ
φ
˙
]
+
[
0
−
C
f
m
0
−
L
f
∗
C
f
I
z
]
δ
(7)
\begin{align} \frac{d}{dt} \begin{bmatrix} y \\ \dot{y} \\ \varphi \\ \dot{\varphi } \end{bmatrix}= \begin{bmatrix} 0 & 1 & 0 & 0\\ 0 & \frac{C_{f}+ C_{r}}{m u} & 0 & \frac{L_{f}*C_{f}-L_{r}*C_{r} }{m u}\\ 0 & 0 & 0 & 1\\ 0 & \frac{L_{f}*C_{f}-L_{r}*C_{r} }{I_{z} u} & 0 & \frac{L_{f}^{2} *C_{f}+L_{r}^{2}*C_{r}}{I_{z}u} \end{bmatrix}\begin{bmatrix} y \\ \dot{y} \\ \varphi \\ \dot{\varphi } \end{bmatrix}+\begin{bmatrix} 0 \\ -\frac{C_{f}}{m} \\ 0 \\ -\frac{L_{f}*C_{f}}{I_{z}} \end{bmatrix} \delta \end{align} \tag{7}
dtd
yy˙φφ˙
=
00001muCf+Cr0IzuLf∗Cf−Lr∗Cr00000muLf∗Cf−Lr∗Cr1IzuLf2∗Cf+Lr2∗Cr
yy˙φφ˙
+
0−mCf0−IzLf∗Cf
δ(7)
四、参考路径误差模型
参考书籍:车辆动力学控制(美)拉贾马尼著(推导过程中书中式(2.43)有误,注意甄别)
定义
e
1
e_{1}
e1为横向误差,车辆质心距车道中心线的距离,
e
2
e_{2}
e2为航向角误差,车辆转弯半径为R。此路径点对应的期望横摆角速度和期望横向加速度可以由纵向速度和转弯半径求得:
φ
˙
d
e
s
=
u
R
a
y
d
e
s
=
u
2
R
=
u
φ
˙
d
e
s
(8)
\begin{align} \dot{\varphi} _{des}&=\frac{u}{R} \\ a_{ydes}&=\frac{u^{2} }{R}= u \dot{\varphi} _{des} \end{align} \tag{8}
φ˙desaydes=Ru=Ru2=uφ˙des(8)
定义
e
1
¨
=
a
y
−
a
y
d
e
s
=
(
v
˙
+
u
φ
˙
)
−
u
2
R
=
v
˙
+
u
(
φ
˙
−
φ
˙
d
e
s
)
e
2
=
φ
−
φ
d
e
s
(9)
\begin{align} \ddot{e_{1}}&=a_{y}-a_{ydes}=(\dot{v}+u\dot{\varphi })-\frac{u^{2} }{R} = \dot{v}+u(\dot{\varphi }-\dot{\varphi} _{des}) \\ e_{2}&=\varphi -\varphi _{des} \end{align} \tag{9}
e1¨e2=ay−aydes=(v˙+uφ˙)−Ru2=v˙+u(φ˙−φ˙des)=φ−φdes(9)
假设纵向速度
u
u
u为常数
e
1
=
v
+
u
(
φ
−
φ
d
e
s
)
(10)
\begin{align} e_{1}&=v+u(\varphi -\varphi _{des}) \end{align} \tag{10}
e1=v+u(φ−φdes)(10)
将式(8)(9)(10)代入式(5),消去变量
v
v
v、
φ
˙
\dot{\varphi}
φ˙,得:
m ( e 1 ¨ + a y d e s ) = C f + C r u ( e 1 ˙ − u e 2 ) + L f C f − L r C r u ( e 2 ˙ + φ ˙ d e s ) − C f δ I z ( e 2 ¨ + φ ¨ d e s ) = L f C f − L r C r u ( e 1 ˙ − u e 2 ) + L f 2 C f + L r 2 C r u ( e 2 ˙ + φ ˙ d e s ) − L f C f δ m(\ddot{e_{1}}+a_{ydes} )=\frac{C_{f}+C_{r}}{u} (\dot{e_{1}}-ue_{2} )+\frac{L_{f} C_{f}-L_{r}C_{r}}{u} (\dot{e_{2}}+\dot{\varphi} _{des} )-C_{f}\delta \\ I_{z}(\ddot{e_{2}} +\ddot{\varphi}_{des} )=\frac{L_{f} C_{f}-L_{r}C_{r}}{u}(\dot{e_{1}}-ue_{2})+\frac{L_{f}^{2} C_{f}+L_{r}^{2} C_{r}}{u}(\dot{e_{2}}+\dot{\varphi} _{des} )-L_{f}C_{f}\delta m(e1¨+aydes)=uCf+Cr(e1˙−ue2)+uLfCf−LrCr(e2˙+φ˙des)−CfδIz(e2¨+φ¨des)=uLfCf−LrCr(e1˙−ue2)+uLf2Cf+Lr2Cr(e2˙+φ˙des)−LfCfδ
化简上式得:
e
1
¨
=
e
1
˙
C
f
+
C
r
m
u
−
e
2
C
f
+
C
r
u
+
e
2
˙
L
f
C
f
−
L
r
C
r
m
u
+
φ
˙
d
e
s
(
L
f
C
f
−
L
r
C
r
m
u
−
u
)
−
δ
C
f
m
e
2
¨
=
e
1
˙
L
f
C
f
−
L
r
C
r
I
z
u
−
e
2
L
f
C
f
−
L
r
C
r
I
z
+
e
2
˙
L
f
2
C
f
+
L
r
2
C
r
I
z
u
+
φ
˙
d
e
s
L
f
2
C
f
+
L
r
2
C
r
I
z
u
−
δ
L
f
C
f
I
z
−
φ
¨
d
e
s
(11)
\begin{align} \ddot{e_{1}}=&\dot{e_{1}}\frac{C_{f}+C_{r}}{mu} -e_{2}\frac{C_{f}+C_{r}}{u} \\ &+\dot{e_{2}}\frac{L_{f} C_{f}-L_{r}C_{r}}{mu}\\ &+\dot{\varphi}_{des}(\frac{L_{f} C_{f}-L_{r}C_{r}}{mu} -u) -\delta\frac{ C_{f}}{m}\\ \ddot{e_{2}} =&\dot{e_{1}}\frac{L_{f} C_{f}-L_{r}C_{r}}{I_{z}u}-e_{2}\frac{L_{f} C_{f}-L_{r}C_{r}}{I_{z}}\\ &+\dot{e_{2}}\frac{L_{f}^{2} C_{f}+L_{r}^{2} C_{r}}{I_{z}u}+\dot{\varphi} _{des}\frac{L_{f}^{2} C_{f}+L_{r}^{2} C_{r}}{I_{z}u}\\ &-\delta\frac{L_{f}C_{f}}{I_{z}}-\ddot{\varphi}_{des} \end{align} \tag{11}
e1¨=e2¨=e1˙muCf+Cr−e2uCf+Cr+e2˙muLfCf−LrCr+φ˙des(muLfCf−LrCr−u)−δmCfe1˙IzuLfCf−LrCr−e2IzLfCf−LrCr+e2˙IzuLf2Cf+Lr2Cr+φ˙desIzuLf2Cf+Lr2Cr−δIzLfCf−φ¨des(11)
在实际计算中,忽略
φ
¨
d
e
s
\ddot{\varphi}_{des}
φ¨des项,由此可得路径跟踪误差模型状态方程为式(12):
d
d
t
[
e
1
e
1
˙
e
2
e
2
˙
]
=
[
0
1
0
0
0
C
f
+
C
r
m
u
−
C
f
+
C
r
u
L
f
C
f
−
L
r
C
r
m
u
0
0
0
1
0
L
f
C
f
−
L
r
C
r
I
z
u
−
L
f
C
f
−
L
r
C
r
I
z
L
f
2
C
f
+
L
r
2
C
r
I
z
u
]
[
e
1
e
1
˙
e
2
e
2
˙
]
+
[
0
L
f
C
f
−
L
r
C
r
m
u
−
u
0
L
f
2
C
f
+
L
r
2
C
r
I
z
u
]
φ
˙
d
e
s
+
[
0
−
C
f
m
0
−
L
f
C
f
I
z
]
δ
\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{C_{f}+C_{r}}{mu} & -\frac{C_{f}+C_{r}}{u} & \frac{L_{f} C_{f}-L_{r}C_{r}}{mu}\\ 0 & 0 & 0 &1 \\ 0 & \frac{L_{f} C_{f}-L_{r}C_{r}}{I_{z}u} & -\frac{L_{f} C_{f}-L_{r}C_{r}}{I_{z}} & \frac{L_{f}^{2} C_{f}+L_{r}^{2} C_{r}}{I_{z}u} \end{bmatrix} \begin{bmatrix} e_{1} \\ \dot{e_{1}} \\ e_{2} \\ \dot{e_{2}} \end{bmatrix}+ \begin{bmatrix} 0 \\ \frac{L_{f} C_{f}-L_{r}C_{r}}{mu}-u \\ 0\\ \frac{L_{f}^{2} C_{f}+L_{r}^{2} C_{r}}{I_{z}u} \end{bmatrix}\dot{\varphi} _{des}+ \begin{bmatrix} 0 \\ -\frac{ C_{f}}{m} \\ 0 \\ -\frac{L_{f}C_{f}}{I_{z}} \end{bmatrix}\delta
dtd
e1e1˙e2e2˙
=
00001muCf+Cr0IzuLfCf−LrCr0−uCf+Cr0−IzLfCf−LrCr0muLfCf−LrCr1IzuLf2Cf+Lr2Cr
e1e1˙e2e2˙
+
0muLfCf−LrCr−u0IzuLf2Cf+Lr2Cr
φ˙des+
0−mCf0−IzLfCf
δ
相对路面误差动力学模型公式总结如下:
参数名称 | 符号 | |
---|---|---|
状态空间向量 | x x x | x = [ e 1 e 1 ˙ e 2 e 2 ˙ ] T x=[e_{1}\quad \dot{e_{1}} \quad e_{2} \quad \dot{e_{2}}]^{T} x=[e1e1˙e2e2˙]T |
x ˙ = A x + B φ ˙ d e s + C δ \dot{x}=Ax+B\dot{\varphi} _{des}+C\delta x˙=Ax+Bφ˙des+Cδ | ||
横向误差 | e 1 e_{1} e1 | |
航向角误差 | e 2 e_{2} e2 | |
前轮转向角 | δ \delta δ | |
路径参考点理论横摆角速度 | φ ˙ d e s \dot{\varphi} _{des} φ˙des |