接上一篇：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代码学习(五)—横纵向控制