车辆稳定性控制系统能够提高汽车在极限情况下的操纵稳定性，而质心侧偏角尤其重要。目前高度集成化的传感器已经可以轻松测得车辆行驶过程中的横摆角速度和侧向加速度，但是质心侧偏角则无法直接测量，因此对于质心侧偏角而言必须通过估算来得到。
对于质心侧偏角的估算国内外学者也有进行不同程度的探索，但是大多采用的是积分算法、卡尔曼滤波算法等等，但是在工程实际应用中却很少。
质心侧偏角通常定义为侧向车速与纵向车速的比值：
β = arctan ( v y v x ) ≈ v y v x (1) \beta =\arctan (\frac{{{v}_{y}}}{{{v}_{x}}})\approx \frac{{{v}_{y}}}{{{v}_{x}}}\tag{1} β=arctan(vxvy)≈vxvy(1) 其中， v x {{v}_{x}} vx表示车辆纵向速度， v y {{v}_{y}} vy为车辆侧向速度。
本博文结合卡尔曼滤波算法设计质心侧偏角观测器。
卡尔曼滤波算法
卡尔曼滤波是一种最优估算方法，其算法具有递推性。对于车辆控制系统而言，由于在车辆的行驶过程中，会受到各种非确定性因素的干扰，因此车用的传感器所测得的信号都夹带有随机的噪声，所以为了保证信号的准确性，需要使用卡尔曼滤波对信号进行处理。估算问题是指通过对这一系列带有观测噪声和干扰信号的实际观测数据的处理，从中得到所需要的各种参量的估算值的过程。而在车辆动力学系统控制系统中，通过状态估算的方法得到车辆的各种状态参数的求解问题，称为车辆动力学估算问题。
卡尔曼滤波是以最小均方差误差为最佳估计准则，假设线性离散系统状态方程和观测方程如下所示：
x
(
k
+
1
)
=
A
(
k
)
x
(
k
)
+
w
(
k
)
(2)
x(k+1)=A(k)x(k)+w(k)\tag{2}
x(k+1)=A(k)x(k)+w(k)(2)
y
(
k
)
=
H
(
k
)
x
(
k
)
+
v
(
k
)
(3)
y(k)=H(k)x(k)+v(k)\tag{3}
y(k)=H(k)x(k)+v(k)(3)其中，系统的随机噪声协方差矩阵为
Q
Q
Q，观测噪声协方差矩阵为
R
R
R，
x
(
k
+
1
)
x(k+1)
x(k+1)是系统n维状态向量，
A
(
k
)
A(k)
A(k)为系统状态转移矩阵，
y
(
k
)
y(k)
y(k)是系统的m维观测向量，
w
(
k
)
w(k)
w(k)是系统的随机噪声，
v
(
k
)
v(k)
v(k)是系统的m维观测噪声，
H
(
k
)
H(k)
H(k)为观测矩阵。假设
w
(
k
)
,
v
(
k
)
w(k),v(k)
w(k),v(k)为相互独立正态分布的白噪声。
卡尔曼滤波器主要包括预测和校正两个过程。总结卡尔曼滤波算法的基本步骤如下：
1.状态一步预测：
x
^
(
k
)
−
=
A
(
k
)
x
^
(
k
−
1
)
(4)
\hat{x}{{(k)}^{-}}=A(k)\hat{x}(k-1) \tag{4}
x^(k)−=A(k)x^(k−1)(4)
2.状态估算计算：
x
^
(
k
)
=
x
^
(
k
)
−
+
K
(
k
)
[
y
(
k
)
−
H
(
k
)
x
^
(
k
)
−
]
(5)
\hat{x}(k)=\hat{x}{{(k)}^{-}}+K(k)[y(k)-H(k)\hat{x}{{(k)}^{-}}] \tag{5}
x^(k)=x^(k)−+K(k)[y(k)−H(k)x^(k)−](5)
3滤波增益矩阵：
K
(
k
)
=
P
(
k
)
−
H
T
(
k
)
[
H
(
k
)
P
(
k
)
−
H
T
(
k
)
+
R
]
−
1
(6)
K(k)=P{{(k)}^{-}}{{H}^{T}}(k){{[H(k)P{{(k)}^{-}}{{H}^{T}}(k)+R]}^{-1}}\tag{6}
K(k)=P(k)−HT(k)[H(k)P(k)−HT(k)+R]−1(6)
4.一步预测误差方差阵：
P
(
k
)
−
=
A
(
k
)
P
(
k
)
A
T
(
k
)
+
Q
(7)
P{{(k)}^{-}}=A(k)P(k){{A}^{T}}(k)+Q\tag{7}
P(k)−=A(k)P(k)AT(k)+Q(7)
5.估算误差方差阵：
P
(
k
)
=
[
1
−
K
(
k
)
H
(
k
)
]
P
(
k
)
−
(8)
P(k)=[1-K(k)H(k)]P{{(k)}^{-}}\tag{8}
P(k)=[1−K(k)H(k)]P(k)−(8)
上述即随机线性离散系统卡尔曼滤波的基本方程，只要给定初值，就可以根据观测值
y
k
{{y}_{k}}
yk递推计算得到状态值的估算。
基于卡尔曼滤波算法的质心侧偏角观测器
线性二自由度的汽车模型如图1所示，运动微分方程为：
m
a
y
=
F
y
f
cos
δ
+
F
y
r
(9)
m{{a}_{y}}={{F}_{yf}}\cos \delta +{{F}_{yr}}\tag{9}
may=Fyfcosδ+Fyr(9)
I
z
w
˙
r
=
a
F
y
f
cos
δ
−
b
F
y
f
(10)
{{I}_{z}}{{\dot{w}}_{r}}=a{{F}_{yf}}\cos \delta -b{{F}_{yf}}\tag{10}
Izw˙r=aFyfcosδ−bFyf(10)
( C f + C r ) β + 1 v x ( l f C f − l r C r ) − C f δ = m ( v ˙ y + v x w r ) (11) ({{C}_{f}}+{{C}_{r}})\beta +\frac{1}{{{v}_{x}}}({{l}_{f}}{{C}_{f}}-{{l}_{r}}{{C}_{r}})-{{C}_{f}}\delta =m({{\dot{v}}_{y}}+{{v}_{x}}{{w}_{r}})\tag{11} (Cf+Cr)β+vx1(lfCf−lrCr)−Cfδ=m(v˙y+vxwr)(11)
(
l
f
C
f
−
l
r
C
r
)
β
+
1
v
x
(
l
f
2
C
f
+
l
r
2
C
r
)
w
r
−
l
f
C
f
δ
=
I
z
w
˙
r
(12)
({{l}_{f}}{{C}_{f}}-{{l}_{r}}{{C}_{r}})\beta +\frac{1}{{{v}_{x}}}(l_{f}^{2}{{C}_{f}}+l_{r}^{2}{{C}_{r}}){{w}_{r}}-{{l}_{f}}{{C}_{f}}\delta ={{I}_{z}}{{\dot{w}}_{r}}\tag{12}
(lfCf−lrCr)β+vx1(lf2Cf+lr2Cr)wr−lfCfδ=Izw˙r(12)
将二自由度的汽车运动微分方程进行离散处理得到13和14：
β
(
k
+
1
)
=
(
1
+
Δ
t
C
f
+
C
r
m
v
x
(
k
)
)
β
(
k
)
+
Δ
t
(
C
r
l
r
−
C
f
l
f
m
v
x
2
(
k
)
−
1
)
w
r
(
k
)
+
Δ
t
C
f
m
v
x
(
k
)
δ
f
(
k
)
(13)
\beta (k+1)=(1+\Delta t\frac{{{C}_{f}}+{{C}_{r}}}{m{{v}_{x}}(k)})\beta (k)+\Delta t(\frac{{{C}_{r}}{{l}_{r}}-{{C}_{f}}{{l}_{f}}}{m{{v}_{x}}^{2}(k)}-1){{w}_{r}}(k)+\Delta t\frac{{{C}_{f}}}{m{{v}_{x}}(k)}{{\delta }_{f}}(k)\tag{13}
β(k+1)=(1+Δtmvx(k)Cf+Cr)β(k)+Δt(mvx2(k)Crlr−Cflf−1)wr(k)+Δtmvx(k)Cfδf(k)(13)
w
r
(
k
+
1
)
=
(
1
−
Δ
t
C
f
l
f
2
+
C
r
l
r
2
I
Z
v
x
(
k
)
)
w
r
(
k
)
+
Δ
t
C
r
l
r
−
C
f
l
f
I
Z
β
(
k
)
+
Δ
t
l
f
C
f
I
Z
δ
f
(
k
)
(14)
{{w}_{r}}(k+1)=(1-\Delta t\frac{{{C}_{f}}l_{f}^{2}+{{C}_{r}}l_{r}^{2}}{{{I}_{Z}}{{v}_{x}}(k)}){{w}_{r}}(k)+\Delta t\frac{{{C}_{r}}{{l}_{r}}-{{C}_{f}}{{l}_{f}}}{{{I}_{Z}}}\beta (k)+\Delta t\frac{{{l}_{f}}{{C}_{f}}}{{{I}_{Z}}}{{\delta }_{f}}(k)\tag{14}
wr(k+1)=(1−ΔtIZvx(k)Cflf2+Crlr2)wr(k)+ΔtIZCrlr−Cflfβ(k)+ΔtIZlfCfδf(k)(14)
为了估算质心侧偏角需要建立卡尔曼滤波的方程，因此将上述建立的离散二自由度车辆的运动微分方程转换成如下式15和式16：
X
(
k
)
=
A
X
(
k
−
1
)
+
B
δ
(
k
)
+
W
(
k
)
(15)
X(k)=AX(k-1)+B\delta (k)+W(k)\tag{15}
X(k)=AX(k−1)+Bδ(k)+W(k)(15)
Y
^
(
k
)
=
C
X
(
k
)
+
D
δ
(
k
)
+
V
(
k
)
(16)
\hat{Y}(k)=CX(k)+D\delta (k)+V(k)\tag{16}
Y^(k)=CX(k)+Dδ(k)+V(k)(16)
式中，
X
(
k
)
=
[
β
(
k
)
w
r
(
k
)
]
X(k)=\left[ \begin{matrix} \beta (k) \\ {{w}_{r}}(k) \\ \end{matrix} \right]
X(k)=[β(k)wr(k)]，
A
=
[
1
+
Δ
t
C
f
+
C
r
m
v
x
(
k
)
Δ
t
(
C
r
l
r
−
C
f
l
f
m
v
x
2
(
k
)
−
1
)
Δ
t
C
r
l
r
−
C
f
l
f
I
Z
1
−
Δ
t
C
f
l
f
2
+
C
r
l
r
2
I
Z
v
x
(
k
)
]
A=\left[ \begin{matrix} & 1+\Delta t\frac{{{C}_{f}}+{{C}_{r}}}{m{{v}_{x}}(k)}\mathop{{}}^{{}}\Delta t(\frac{{{C}_{r}}{{l}_{r}}-{{C}_{f}}{{l}_{f}}}{mv_{x}^{2}(k)}-1) \\ & \Delta t\frac{{{C}_{r}}{{l}_{r}}-{{C}_{f}}{{l}_{f}}}{{{I}_{Z}}}\mathop{{}}^{{}}1-\Delta t\frac{{{C}_{f}}l_{f}^{2}+{{C}_{r}}l_{r}^{2}}{{{I}_{Z}}{{v}_{x}}(k)} \\ \end{matrix} \right]
A=[1+Δtmvx(k)Cf+CrΔt(mvx2(k)Crlr−Cflf−1)ΔtIZCrlr−Cflf1−ΔtIZvx(k)Cflf2+Crlr2]，
B = [ Δ t C f m v x ( k ) Δ t l f C f I Z ] B=\left[ \begin{matrix} & \Delta t\frac{{{C}_{f}}}{m{{v}_{x}}(k)} \\ & \Delta t\frac{{{l}_{f}}{{C}_{f}}}{{{I}_{Z}}} \\ \end{matrix} \right] B=[Δtmvx(k)CfΔtIZlfCf]， W ( k ) = [ w 1 ( k ) w 2 ( k ) ] W(k)=\left[ \begin{matrix} & {{w}_{1}}(k) \\ & {{w}_{2}}(k) \\ \end{matrix} \right] W(k)=[w1(k)w2(k)]， Y ^ ( k ) = [ a ^ y ( k ) w ^ r ( k ) ] \hat{Y}(k)=\left[ \begin{matrix} & {{{\hat{a}}}_{y}}(k) \\ & {{{\hat{w}}}_{r}}(k) \\ \end{matrix} \right] Y^(k)=[a^y(k)w^r(k)]，
C = [ − C f + C r m C r l r − C f l f m 0 1 ] C=\left[ \begin{matrix} & -\frac{{{C}_{f}}+{{C}_{r}}}{m}\overset{{}}{\mathop {}}\,\frac{{{C}_{r}}{{l}_{r}}-{{C}_{f}}{{l}_{f}}}{m} \\ & \overset{{}}{\mathop {}}\,\overset{{}}{\mathop {}}\,\overset{{}}{\mathop {}}\,0\overset{{}}{\mathop {}}\,\overset{{}}{\mathop {}}\,\overset{{}}{\mathop {}}\,\overset{{}}{\mathop {}}\,\overset{{}}{\mathop {}}\,\overset{{}}{\mathop {}}\,1 \\ \end{matrix} \right] C=[−mCf+CrmCrlr−Cflf01]， D = [ C f m 0 ] D=\left[ \begin{matrix} & \frac{{{C}_{f}}}{m} \\ & \overset{{}}{\mathop {}}\,0 \\ \end{matrix} \right] D=[mCf0]， V ( k ) = [ v 1 ( k ) v 2 ( k ) ] V(k)=\left[ \begin{matrix} & {{v}_{1}}(k) \\ & {{v}_{2}}(k) \\ \end{matrix} \right] V(k)=[v1(k)v2(k)]。
将上述式子套入卡尔曼滤波器即可估算汽车质心侧偏角。
2280
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
