参考资料
1. 基本概述
车辆横向控制主要通过控制轮胎转角实现，而对于驾驶员来说，可直接操控的是方向盘角度，因此在搭建
车辆动力学模型
时，可以建立以相对于道路的方向和距离误差为状态变量的动力学模型。
假设——
-
ey
e_y
e
y
:车辆重心距车道中心线的距离； -
eψ
e_{\psi}
e
ψ
:偏航角误差； -
Vx
V_x
V
x
:车辆纵向速度； -
RR
R
：车辆转弯半径，其中转弯半径
RR
R
足够大，以满足
上述章节
的小角度近似假设。
定义——
-
车身转过
期望角度所需要的偏航角速度
为
ψ˙
d
e
s
=
V
x
R
(1)
\tag{1} \dot\psi_{des}=\frac{V_x}{R}
ψ
˙
d
es
=
R
V
x
(
1
)
-
所需的
横向加速度
（即期望的向心加速度）为
ad
e
s
=
V
x
2
R
=
V
x
ψ
˙
d
e
s
(2)
\tag{2} a_{des}=\frac{V_x^2}{R}=V_x\dot\psi_{des}
a
d
es
=
R
V
x
2
=
V
x
ψ
˙
d
es
(
2
)
-
车辆
偏航角误差
为
e
ψ
=
ψ
−
ψ
d
e
s
(3)
\tag{3} e_{\psi}=\psi-\psi_{d e s}
e
ψ
=
ψ
−
ψ
d
es
(
3
)
-
车辆
偏航角速度误差
为
e˙
ψ
=
ψ
˙
−
ψ
˙
des
(4)
\tag{4} \dot{e}_{\psi}=\dot{\psi}-\dot{\psi}_{\text {des }}
e
˙
ψ
=
ψ
˙
−
ψ
˙
des
(
4
)
-
车辆
偏航角加速度误差
为
e¨
ψ
=
ψ
¨
−
ψ
¨
des
(5)
\tag{5} \ddot{e}_{\psi}=\ddot{\psi}-\ddot{\psi}_{\text {des }}
e
¨
ψ
=
ψ
¨
−
ψ
¨
des
(
5
)
-
车辆
横向加速度误差
为
e¨
y
=
a
y
−
a
d
e
s
=
(
y
¨
+
V
x
ψ
˙
)
−
V
x
ψ
˙
des
=
y
¨
+
V
x
(
ψ
˙
−
ψ
˙
d
e
s
)
(6)
\tag{6} \begin{aligned} \ddot{e}_{y} &=a_{y}-a_{d e s} \\ &=\left(\ddot{y}+V_{x} \dot{\psi}\right)-V_{x} \dot{\psi}_{\text {des }} \\ &=\ddot{y}+V_{x}\left(\dot{\psi}-\dot{\psi}_{d e s}\right) \end{aligned}
e
¨
y
=
a
y
−
a
d
es
=
(
y
¨
+
V
x
ψ
˙
)
−
V
x
ψ
˙
des
=
y
¨
+
V
x
(
ψ
˙
−
ψ
˙
d
es
)
(
6
)
其中，
ay
a_y
a
y
由两 种力共同作用产生 : 车辆延
yy
y
轴产生的惯性加速度
y¨
\ddot{y}
y
¨
和车辆绕旋转中心
OO
O
旋转产生的向心加速度
ac
=
V
x
2
R
=
V
x
ψ
˙
∘
a_{c}=\frac{
{V_x}^2}{R}=V_{x} \dot{\psi}_{\circ}
a
c
=
R
V
x
2
=
V
x
ψ
˙
∘
-
车辆
横向速度误差
为当车辆纵向速度
Vx
V_x
V
x
恒定时，
yy
y
轴方向的速度误差可以表示为
e˙
y
=
∫
e
¨
y
d
t
=
y
˙
+
V
x
(
ψ
−
ψ
d
e
s
)
(7)
\tag{7} \dot{e}_{y}=\int \ddot{e}_{y} \mathrm{~d} t=\dot{y}+V_{x}\left(\psi-\psi_{d e s}\right)
e
˙
y
=
∫
e
¨
y
d
t
=
y
˙
+
V
x
(
ψ
−
ψ
d
es
)
(
7
)
当纵向速度
Vx
V_x
V
x
随着时间变化时，对等式(6)积分得
e˙
y
=
∫
e
¨
y
d
t
=
∫
y
¨
+
V
x
(
ψ
˙
−
ψ
˙
des
)
d
t
=
y
˙
+
∫
V
x
(
ψ
˙
−
ψ
˙
des
)
d
t
(8)
\tag{8} \dot{e}_{y}=\int \ddot{e}_{y} \mathrm{~d} t=\int \ddot{y}+V_{x}\left(\dot{\psi}-\dot\psi_{\text {des }}\right) \mathrm{d} t=\dot{y}+\int V_{x}\left(\dot\psi-\dot\psi_{\text {des }}\right) \mathrm{d} t
e
˙
y
=
∫
e
¨
y
d
t
=
∫
y
¨
+
V
x
(
ψ
˙
−
ψ
˙
des
)
d
t
=
y
˙
+
∫
V
x
(
ψ
˙
−
ψ
˙
des
)
d
t
(
8
)
参考资料中的等式(8)为
e˙
y
=
∫
e
¨
y
d
t
=
y
˙
+
∫
V
x
(
ψ
−
ψ
des
)
d
t
\dot{e}_{y}=\int \ddot{e}_{y} \mathrm{~d} t=\dot{y}+\int V_{x}\left(\psi-\psi_{\text {des }}\right) \mathrm{d} t
e
˙
y
=
∫
e
¨
y
d
t
=
y
˙
+
∫
V
x
(
ψ
−
ψ
des
)
d
t
根据一步一步推导看的话
书中
的这个等式应该是有问题的，这里先贴在这，有懂的朋友也可以帮忙解惑一下~~这就使得模型非线性且时变，不利于控制系统的设计。因此这里假设纵向速度
Vx
V_x
V
x
恒定， 即当作一个线性时不变(LTI)模型。如果速度变化，就需要使用线性参变模型(LPV) 替代， 其纵向速度是一个随着时间变化的参数。
2. 误差动力学模型
将等式(6)(7)变换如下：
y
¨
=
e
¨
y
+
V
x
ψ
˙
d
e
s
−
V
x
ψ
˙
y
˙
=
e
˙
y
−
V
x
e
ψ
(9)
\tag{9} \begin{aligned} &\ddot{y}=\ddot{e}_{y}+V_{x} \dot{\psi}_{d e s}-V_{x} \dot{\psi}\\ &\dot{y}=\dot{e}_{y}-V_{x} e_{\psi} \end{aligned}
y
¨
=
e
¨
y
+
V
x
ψ
˙
d
es
−
V
x
ψ
˙
y
˙
=
e
˙
y
−
V
x
e
ψ
(
9
)
根据
车辆动力学模型
中的等式(14)
y
¨
=
−
2
C
α
f
+
2
C
α
r
m
V
x
y
˙
−
(
V
x
+
2
C
α
f
l
f
−
2
C
α
r
l
r
m
V
x
)
ψ
˙
+
2
C
α
f
m
δ
(10)
\tag{10} \ddot{y}=-\frac{2 C_{\alpha f}+2 C_{\alpha r}}{m V_{x}} \dot{y}-\left(V_{x}+\frac{2 C_{\alpha f} l_{f}-2 C_{\alpha r} l_{r}}{m V_{x}}\right) \dot{\psi}+\frac{2 C_{\alpha f}}{m} \delta
y
¨
=
−
m
V
x
2
C
α
f
+
2
C
α
r
y
˙
−
(
V
x
+
m
V
x
2
C
α
f
l
f
−
2
C
α
r
l
r
)
ψ
˙
+
m
2
C
α
f
δ
(
10
)
将等式(4)(9)代入等式(10)得
e
¨
y
+
V
x
ψ
˙
d
e
s
−
V
x
(
e
˙
ψ
+
ψ
˙
des
)
=
−
2
C
α
f
+
2
C
α
r
m
V
x
(
e
˙
y
−
V
x
e
ψ
)
−
(
V
x
+
2
C
α
f
l
f
−
2
C
α
r
l
r
m
V
x
)
(
e
˙
ψ
+
ψ
˙
des
)
+
2
C
α
f
m
δ
(11)
\tag{11} \begin{aligned} \ddot{e}_{y}+V_{x} \dot{\psi}_{d e s}-V_{x} \left(\dot{e}_{\psi}+\dot{\psi}_{\text {des }}\right)&=-\frac{2 C_{\alpha f}+2 C_{\alpha r}}{m V_{x}} \left(\dot{e}_{y}-V_{x} e_{\psi}\right)-\\ &\left(V_{x}+\frac{2 C_{\alpha f} l_{f}-2 C_{\alpha r} l_{r}}{m V_{x}}\right) \left(\dot{e}_{\psi}+\dot{\psi}_{\text {des }}\right)+\frac{2 C_{\alpha f}}{m} \delta \end{aligned}
e
¨
y
+
V
x
ψ
˙
d
es
−
V
x
(
e
˙
ψ
+
ψ
˙
des
)
=
−
m
V
x
2
C
α
f
+
2
C
α
r
(
e
˙
y
−
V
x
e
ψ
)
−
(
V
x
+
m
V
x
2
C
α
f
l
f
−
2
C
α
r
l
r
)
(
e
˙
ψ
+
ψ
˙
des
)
+
m
2
C
α
f
δ
(
11
)
对等式(11)提取
e
¨
y
\ddot{e}_y
e
¨
y
、
e
˙
y
\dot{e}_y
e
˙
y
、
e
y
e_y
e
y
、
e
ψ
e_{\psi}
e
ψ
、
e
˙
ψ
\dot e_{\psi}
e
˙
ψ
、
ψ
˙
d
e
s
\dot{\psi}_{des}
ψ
˙
d
es
和
δ
\delta
δ
项得
e
¨
y
=
−
2
C
α
f
−
2
C
α
r
m
V
x
e
˙
y
+
2
C
α
f
+
2
C
α
r
m
e
ψ
+
−
2
C
α
f
l
f
+
2
C
α
r
l
r
m
V
x
e
˙
ψ
+
(
−
2
C
α
f
l
f
+
2
C
α
r
l
r
m
V
x
−
V
x
)
ψ
˙
des
+
2
C
α
f
m
δ
(12)
\tag{12} \begin{gathered} \ddot{e}_{y}=\frac{-2 C_{\alpha f}-2 C_{\alpha r}}{m V_{x}} \dot{e}_{y}+\frac{2 C_{\alpha f}+2 C_{\alpha r}}{m} e_{\psi}+\frac{-2 C_{\alpha f} l_{f}+2 C_{\alpha r} l_{r}}{m V_{x}} \dot{e}_{\psi} \\ +\left(\frac{-2 C_{\alpha f} l_{f}+2 C_{\alpha r} l_{r}}{m V_{x}}-V_{x}\right) \dot{\psi}_{\text {des }}+\frac{2 C_{\alpha f}}{m} \delta \end{gathered}
e
¨
y
=
m
V
x
−
2
C
α
f
−
2
C
α
r
e
˙
y
+
m
2
C
α
f
+
2
C
α
r
e
ψ
+
m
V
x
−
2
C
α
f
l
f
+
2
C
α
r
l
r
e
˙
ψ
+
(
m
V
x
−
2
C
α
f
l
f
+
2
C
α
r
l
r
−
V
x
)
ψ
˙
des
+
m
2
C
α
f
δ
(
12
)
整理成矩阵形式为
d
d
t
e
˙
y
=
[
0
−
2
C
α
f
+
2
C
α
r
r
m
V
x
2
C
α
f
+
2
C
α
r
m
−
2
C
α
f
l
f
+
2
C
α
r
l
r
m
V
x
]
[
e
y
e
˙
y
e
ψ
e
˙
ψ
]
+
(
−
2
C
α
f
l
f
+
2
C
α
r
l
r
m
V
x
−
V
x
)
ψ
˙
d
e
s
+
2
C
α
f
m
δ
(13)
\tag{13} \frac{\mathrm{d}}{\mathrm{d} t} \dot{e}_{y}=\left[\begin{array}{llll}0 & -\frac{2 C_{\alpha f}+2 C_{\alpha r} r}{m V_{x}} & \frac{2 C_{\alpha f}+2 C_{\alpha r}}{m} & \frac{-2 C_{\alpha f} l_{f}+2 C_{\alpha r} l_{r}}{m V_{x}}\end{array}\right]\left[\begin{array}{c}e_{y} \\ \dot{e}_{y} \\ e_{\psi} \\ \dot{e}_{\psi}\end{array}\right]+\\ \left(\frac{-2 C_{\alpha f} l_{f}+2 C_{\alpha r} l_{r}}{m V_{x}}-V_{x}\right) \dot{\psi}_{d e s}+\frac{2 C_{\alpha f}}{m} \delta
d
t
d
e
˙
y
=
[
0
−
m
V
x
2
C
α
f
+
2
C
α
r
r
m
2
C
α
f
+
2
C
α
r
m
V
x
−
2
C
α
f
l
f
+
2
C
α
r
l
r
]
e
y
e
˙
y
e
ψ
e
˙
ψ
+
(
m
V
x
−
2
C
α
f
l
f
+
2
C
α
r
l
r
−
V
x
)
ψ
˙
d
es
+
m
2
C
α
f
δ
(
13
)
同理可得，根据
车辆动力学模型
中的等式(17)
ψ
¨
=
−
2
l
f
C
α
f
−
2
l
r
C
α
r
I
z
v
x
y
˙
−
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
I
z
v
x
ψ
˙
+
2
l
f
C
α
f
I
z
δ
(14)
\tag{14} \ddot{\psi}=-\frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z} v_{x}} \dot{y}-\frac{2 l_{f}{ }^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} v_{x}} \dot{\psi}+\frac{2 l_{f} C_{\alpha f}}{I_{z}} \delta
ψ
¨
=
−
I
z
v
x
2
l
f
C
α
f
−
2
l
r
C
α
r
y
˙
−
I
z
v
x
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
ψ
˙
+
I
z
2
l
f
C
α
f
δ
(
14
)
将等式(4)(5)(9)代入等式(14)，得
e
¨
ψ
+
ψ
¨
des
=
−
2
l
f
C
α
f
−
2
l
r
C
α
r
I
z
V
x
(
e
˙
y
−
V
x
e
ψ
)
−
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
I
z
V
x
(
e
˙
ψ
+
ψ
˙
des
)
+
2
l
f
C
α
f
I
z
δ
(15)
\tag{15} \begin{aligned} \ddot{e}_{\psi}+\ddot{\psi}_{\text {des }}=-\frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z} V_{x}}\left(\dot{e}_{y}-V_{x} e_{\psi}\right) -\frac{2 l_{f}{ }^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} V_{x}} \left(\dot{e}_{\psi}+\dot{\psi}_{\text {des }}\right)+\frac{2 l_{f} C_{\alpha f}}{I_{z}} \delta \end{aligned}
e
¨
ψ
+
ψ
¨
des
=
−
I
z
V
x
2
l
f
C
α
f
−
2
l
r
C
α
r
(
e
˙
y
−
V
x
e
ψ
)
−
I
z
V
x
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
(
e
˙
ψ
+
ψ
˙
des
)
+
I
z
2
l
f
C
α
f
δ
(
15
)
对等式(15)提取
e
˙
y
\dot{e}_y
e
˙
y
、
e
y
e_y
e
y
、
e
ψ
e_{\psi}
e
ψ
、
e
¨
ψ
\ddot{e}_{\psi}
e
¨
ψ
、
e
˙
ψ
\dot e_{\psi}
e
˙
ψ
、
ψ
˙
d
e
s
\dot{\psi}_{des}
ψ
˙
d
es
和
δ
\delta
δ
项得
e
¨
ψ
=
−
2
l
f
C
α
f
−
2
l
r
C
α
r
I
z
V
x
e
˙
y
+
2
l
f
C
α
f
−
2
l
r
C
α
r
I
z
e
ψ
−
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
I
z
V
x
e
˙
ψ
−
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
I
z
V
x
ψ
˙
d
e
s
+
2
l
f
C
α
f
I
z
δ
−
ψ
¨
d
e
s
(16)
\tag{16} \begin{gathered} \ddot{e}_{\psi}=-\frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z} V_{x}} \dot{e}_{y}+\frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z}} e_{\psi} \\ -\frac{2 l_{f}{ }^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} V_{x}} \dot{e}_{\psi}-\frac{2 l_{f}{ }^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} V_{x}} \dot{\psi}_{d e s}+\frac{2 l_{f} C_{\alpha f}}{I_{z}} \delta-\ddot{\psi}_{d e s} \end{gathered}
e
¨
ψ
=
−
I
z
V
x
2
l
f
C
α
f
−
2
l
r
C
α
r
e
˙
y
+
I
z
2
l
f
C
α
f
−
2
l
r
C
α
r
e
ψ
−
I
z
V
x
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
e
˙
ψ
−
I
z
V
x
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
ψ
˙
d
es
+
I
z
2
l
f
C
α
f
δ
−
ψ
¨
d
es
(
16
)
由于上述假设为线性时不变系统
(
L
T
1
)
(
V
˙
x
=
0
)
(L T 1)\left(\dot{V}_{x}=0\right)
(
L
T
1
)
(
V
˙
x
=
0
)
，故
ψ
¨
d
e
s
=
V
˙
x
R
=
0
\ddot{\psi}_{d e s}=\frac{\dot{V}_{x}}{R}=0
ψ
¨
d
es
=
R
V
˙
x
=
0
，将上述等式整理成矩阵形式得
d
d
t
e
˙
ψ
=
[
0
−
2
l
f
C
α
f
−
2
l
r
C
α
r
I
z
V
x
2
l
f
C
α
f
−
2
l
r
C
α
r
I
z
−
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
I
z
V
x
]
[
e
y
e
˙
y
e
ψ
e
˙
ψ
]
−
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
I
z
V
x
ψ
˙
d
e
s
+
2
l
f
C
α
f
I
z
δ
(17)
\tag{17} \begin{aligned} \frac{\mathrm{d}}{\mathrm{d} t} \dot{e}_{\psi}=&\left[\begin{array}{llll} 0 & -\frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z} V_{x}} & \frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z}} & -\frac{2 l_{f}{ }^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} V_{x}} \end{array}\right]\left[\begin{array}{c} e_{y} \\ \dot{e}_{y} \\ e_{\psi} \\ \dot{e}_{\psi} \end{array}\right] \\ &-\frac{2 l_{f}{ }^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} V_{x}} \dot{\psi}_{d e s}+\frac{2 l_{f} C_{\alpha f}}{I_{z}} \delta \end{aligned}
d
t
d
e
˙
ψ
=
[
0
−
I
z
V
x
2
l
f
C
α
f
−
2
l
r
C
α
r
I
z
2
l
f
C
α
f
−
2
l
r
C
α
r
−
I
z
V
x
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
]
e
y
e
˙
y
e
ψ
e
˙
ψ
−
I
z
V
x
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
ψ
˙
d
es
+
I
z
2
l
f
C
α
f
δ
(
17
)
根据等式(13)和(17)，基于跟踪误差变量的状态空间模型表示为
d
d
t
[
e
y
e
˙
y
e
ψ
e
˙
ψ
]
=
[
0
1
0
0
0
−
2
C
α
f
+
2
C
α
r
m
V
x
2
C
α
f
+
2
C
α
r
m
−
2
C
α
f
l
f
+
2
C
α
r
l
r
m
V
x
0
0
0
1
0
−
2
l
f
C
α
f
−
2
l
r
C
α
r
I
z
V
x
2
l
f
C
α
f
−
2
l
r
C
α
r
I
z
−
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
I
z
V
x
]
[
e
y
e
˙
y
e
ψ
e
˙
ψ
]
+
[
0
2
C
α
f
m
0
2
l
f
C
α
f
I
z
]
δ
+
[
0
−
2
C
α
f
l
f
+
2
C
α
r
l
r
m
V
x
−
V
x
0
−
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
I
z
V
x
]
ψ
˙
d
e
s
(18)
\tag{18} \begin{aligned} \frac{\mathrm{d}}{\mathrm{d} t}\left[\begin{array}{l} e_{y} \\ \dot{e}_{y} \\ e_{\psi} \\ \dot{e}_{\psi} \end{array}\right]=& {\left[\begin{array}{cccc} 0 & 1 & 0 & 0 \\ 0 & -\frac{2 C_{\alpha f}+2 C_{\alpha r}}{m V_{x}} & \frac{2 C_{\alpha f}+2 C_{\alpha r}}{m} & \frac{-2 C_{\alpha f} l_{f}+2 C_{\alpha r} l_{r}}{m V_{x}} \\ 0 & 0 & 0 & 1 \\ 0 & -\frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z} V_{x}} & \frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z}} & -\frac{2 l_{f}^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} V_{x}} \end{array}\right]\left[\begin{array}{l} e_{y} \\ \dot{e}_{y} \\ e_{\psi} \\ \dot{e}_{\psi} \end{array}\right] } \\ &+\left[\begin{array}{c} 0 \\ \frac{2 C_{\alpha f}}{m} \\ 0 \\ \frac{2 l_{f} C_{\alpha f}}{I_{z}} \end{array}\right] \delta+\left[\begin{array}{c} 0\\ \frac{-2 C_{\alpha f} l_{f}+2 C_{\alpha r} l_{r}}{m V_{x}}-V_{x} \\ 0\\ -\frac{2 l_{f}^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} V_{x}} \end{array}\right]\dot\psi_{des} \end{aligned}
d
t
d
e
y
e
˙
y
e
ψ
e
˙
ψ
=
0
0
0
0
1
−
m
V
x
2
C
α
f
+
2
C
α
r
0
−
I
z
V
x
2
l
f
C
α
f
−
2
l
r
C
α
r
0
m
2
C
α
f
+
2
C
α
r
0
I
z
2
l
f
C
α
f
−
2
l
r
C
α
r
0
m
V
x
−
2
C
α
f
l
f
+
2
C
α
r
l
r
1
−
I
z
V
x
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
e
y
e
˙
y
e
ψ
e
˙
ψ
+
0
m
2
C
α
f
0
I
z
2
l
f
C
α
f
δ
+
0
m
V
x
−
2
C
α
f
l
f
+
2
C
α
r
l
r
−
V
x
0
−
I
z
V
x
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
ψ
˙
d
es
(
18
)
写成一般形式，状态空间表达式如下:
X
˙
=
A
X
+
B
1
δ
+
B
2
ψ
˙
des
(19)
\tag{19} \dot{X}=A X+B_{1} \delta+B_{2} \dot{\psi}_{\text {des }}
X
˙
=
A
X
+
B
1
δ
+
B
2
ψ
˙
des
(
19
)
在使用时，我们一般会忽略
B
2
ψ
˙
des
B_{2} \dot{\psi}_{\text {des }}
B
2
ψ
˙
des
， 然后将状态方程离散化，便于实现。即：
X
t
=
X
t
−
1
+
A
X
t
−
1
Δ
t
+
B
1
δ
Δ
t
=
(
I
+
A
Δ
t
)
X
t
−
1
+
B
1
δ
Δ
t
=
A
ˉ
X
t
−
1
+
B
ˉ
δ
(20)
\tag{20} X_t=X_{t-1}+A X_{t-1}\Delta t+B_{1} \delta\Delta t=(I+A\Delta t)X_{t-1}+B_{1} \delta\Delta t = \bar{A}X_{t-1}+\bar{B}\delta
X
t
=
X
t
−
1
+
A
X
t
−
1
Δ
t
+
B
1
δ
Δ
t
=
(
I
+
A
Δ
t
)
X
t
−
1
+
B
1
δ
Δ
t
=
A
ˉ
X
t
−
1
+
B
ˉ
δ
(
20
)
其中
A
ˉ
=
I
+
A
Δ
t
(
I
为单位矩阵
)
B
ˉ
=
B
1
Δ
t
\begin{aligned} \bar{A} &= I+A\Delta t ~~~~~(I为单位矩阵)\\ \bar{B} &= B_{1}\Delta t \end{aligned}
A
ˉ
B
ˉ
=
I
+
A
Δ
t
(
I
为单位矩阵
)
=
B
1
Δ
t
另外，如果考虑路面坡度角(road bank angle)
ϕ
\phi
ϕ
（定义可参考
前文动力学模型
），则等式(18)变为
d
d
t
[
e
y
e
˙
y
e
ψ
e
˙
ψ
]
=
[
0
1
0
0
0
−
2
C
α
f
+
2
C
α
r
m
V
x
2
C
α
f
+
2
C
α
r
m
−
2
C
α
f
l
f
+
2
C
α
r
l
r
m
V
x
0
0
0
1
0
−
2
l
f
C
α
f
−
2
l
r
C
α
r
I
z
V
x
2
l
f
C
α
f
−
2
l
r
C
α
r
I
z
−
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
I
z
V
x
]
[
e
y
e
˙
y
e
ψ
e
˙
ψ
]
+
[
0
2
C
α
f
m
0
2
l
f
C
α
f
I
z
]
δ
+
[
0
−
2
C
α
f
l
f
+
2
C
α
r
l
r
m
V
x
−
V
x
0
−
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
I
z
V
x
]
ψ
˙
d
e
s
+
[
0
g
0
0
]
sin
ϕ
(21)
\tag{21} \begin{aligned} \frac{\mathrm{d}}{\mathrm{d} t}\left[\begin{array}{l} e_{y} \\ \dot{e}_{y} \\ e_{\psi} \\ \dot{e}_{\psi} \end{array}\right]=& {\left[\begin{array}{cccc} 0 & 1 & 0 & 0 \\ 0 & -\frac{2 C_{\alpha f}+2 C_{\alpha r}}{m V_{x}} & \frac{2 C_{\alpha f}+2 C_{\alpha r}}{m} & \frac{-2 C_{\alpha f} l_{f}+2 C_{\alpha r} l_{r}}{m V_{x}} \\ 0 & 0 & 0 & 1 \\ 0 & -\frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z} V_{x}} & \frac{2 l_{f} C_{\alpha f}-2 l_{r} C_{\alpha r}}{I_{z}} & -\frac{2 l_{f}^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} V_{x}} \end{array}\right]\left[\begin{array}{l} e_{y} \\ \dot{e}_{y} \\ e_{\psi} \\ \dot{e}_{\psi} \end{array}\right] } \\ &+\left[\begin{array}{c} 0 \\ \frac{2 C_{\alpha f}}{m} \\ 0 \\ \frac{2 l_{f} C_{\alpha f}}{I_{z}} \end{array}\right] \delta+\left[\begin{array}{c} 0\\ \frac{-2 C_{\alpha f} l_{f}+2 C_{\alpha r} l_{r}}{m V_{x}}-V_{x} \\ 0\\ -\frac{2 l_{f}^{2} C_{\alpha f}+2 l_{r}{ }^{2} C_{\alpha r}}{I_{z} V_{x}} \end{array}\right]\dot\psi_{des}+\left[\begin{array}{c} 0 \\ g \\ 0 \\ 0 \end{array}\right] \sin{\phi} \end{aligned}
d
t
d
e
y
e
˙
y
e
ψ
e
˙
ψ
=
0
0
0
0
1
−
m
V
x
2
C
α
f
+
2
C
α
r
0
−
I
z
V
x
2
l
f
C
α
f
−
2
l
r
C
α
r
0
m
2
C
α
f
+
2
C
α
r
0
I
z
2
l
f
C
α
f
−
2
l
r
C
α
r
0
m
V
x
−
2
C
α
f
l
f
+
2
C
α
r
l
r
1
−
I
z
V
x
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
e
y
e
˙
y
e
ψ
e
˙
ψ
+
0
m
2
C
α
f
0
I
z
2
l
f
C
α
f
δ
+
0
m
V
x
−
2
C
α
f
l
f
+
2
C
α
r
l
r
−
V
x
0
−
I
z
V
x
2
l
f
2
C
α
f
+
2
l
r
2
C
α
r
ψ
˙
d
es
+
0
g
0
0
sin
ϕ
(
21
)
3. python 实现
根据公式(18)，我们可以使用
numpy
实现如下代码：
import numpy as np
class LateralErrorModel:
"""
This model describes the lateral motion of a vehicle and its response to various inputs.
Parameters:
self.m (float): Mass of the vehicle (kg).
self.Vx (float): Longitudinal velocity (self.m/s).
self.C_alpha_f (float): Front tire cornering stiffness (N/rad).
self.C_alpha_r (float): Rear tire cornering stiffness (N/rad).
self.l_f (float): Distance from the vehicle's center of mass to the front axle (self.m).
self.l_r (float): Distance from the vehicle's center of mass to the rear axle (self.m).
self.I_z (float): Moment of inertia about the vertical axis (kg self.m^2).
self.g (float): Acceleration due to gravity (self.m/s^2).
"""
def __init__(self, m, Vx, C_alpha_f, C_alpha_r, l_f, l_r, I_z, g):
self.m = m
self.Vx = Vx
self.C_alpha_f = C_alpha_f
self.C_alpha_r = C_alpha_r
self.l_f = l_f
self.l_r = l_r
self.I_z = I_z
self.g = g
def GenerateStateSpace(self):
A = np.array([[0, 1, 0, 0],
[0, -(2*self.C_alpha_f + 2*self.C_alpha_r) / (self.m * self.Vx), (2*self.C_alpha_f + 2*self.C_alpha_r) / self.m,
(-2*self.C_alpha_f*self.l_f + 2*self.C_alpha_r*self.l_r) / (self.m * self.Vx)],
[0, 0, 0, 1],
[0, -(2*self.l_f*self.C_alpha_f - 2*self.l_r*self.C_alpha_r) / (self.I_z * self.Vx),
(2*self.l_f*self.C_alpha_f - 2*self.l_r*self.C_alpha_r) / self.I_z,
(-2*self.l_f**2*self.C_alpha_f + 2*self.l_r**2*self.C_alpha_r) / (self.I_z * self.Vx)]])
B = np.array([[0],
[2 * self.C_alpha_f / self.m],
[0],
[2 * self.l_f * self.C_alpha_f / self.I_z]])
C = np.array([[0],
[(-2*self.C_alpha_f*self.l_f + 2*self.C_alpha_r*self.l_r) / (self.m * self.Vx) - self.Vx],
[0],
[(-2*self.l_f**2*self.C_alpha_f + 2*self.l_r**2*self.C_alpha_r) / (self.I_z * self.Vx)]])
D = np.array([[0],
[self.g],
[0],
[0]])
return A,B,C,D
def ComputeStateDerivative(self, state, delta, psi_des, phi):
"""
Computes the derivative of the state vector using the lateral error dynamics model.
Parameters:
state (numpy.ndarray): The current state vector [e_y, e_y_dot, e_psi, e_psi_dot].
delta (float): The steering input.
psi_des (float): The desired yaw rate input.
phi (float): The roll angle input.
Returns:
numpy.ndarray: The derivative of the state vector [e_y_dot, e_y_ddot, e_psi_dot, e_psi_ddot].
"""
A,B,C,D=self.GenerateStateSpace()
state_dot = np.dot(A, state) + np.dot(B, delta) + np.dot(C, psi_des) + np.dot(D, np.sin(phi))
return state_dot[:,0]
def DiscreteStateSpace(self,dt):
"""
Returns:
discrete state space
"""
A,B,C,D = self.GenerateStateSpace()
I = np.eye(4)
A_bar = I + A * dt
B_bar = B*dt
return A_bar,B_bar
例如我们使用如下参数：
model = LateralErrorModel(m=1000.0, Vx=20.0, C_alpha_f=10000.0, C_alpha_r=15000.0, l_f=1.5, l_r=1.0, I_z=3000.0, g=9.8)
# Initial conditions and inputs
initial_state = np.array([0.0, 0.0, 0.0, 0.0])
delta_input = 0.1
psi_des_input = 0.0
phi_input = 0.1
dt = 0.1
# Compute the derivative of the state using the model instance
state_derivative = model.ComputeStateDerivative(initial_state, delta_input, psi_des_input, phi_input)
A_bar, B_bar = model.DiscreteStateSpace(dt)
print("State derivative:", state_derivative)
print("A_bar: ", A_bar)
print("B_bar: ", B_bar)
最终得到状态转移：
State derivative: [0. 2.97836748 0. 1. ]
A_bar: [[1. 0.1 0. 0. ]
[0. 0.75 5. 0. ]
[0. 0. 1. 0.1 ]
[0. 0. 0. 0.975]]
B_bar: [[0.]
[2.]
[0.]
[1.]]
完整代码文件可见
github仓库
.