[0057] 所以，可将Vd简化为
[0058] Vd= VxA θ + Δ Vy
[0059] 将上式除以Δ t，应取当Δ t趋于0时的极限，便得到了车辆质心加速度在Y轴上 的分量，即为车辆行驶过程中的侧向加速度
[0061] 因此，可得出
[0062] Vj, -·=αν -K -COr
[0063] 其中，横摆角速度。
[0064] 将得到的!^带入到處的表达式中，得到第一质心侧偏角的微分值矣
[0066] 对上式进行最小二乘法估计，能够较准确得到第一质心侧偏角的微分值為。
[0067] 作为一种优选的，本发明采用一种带遗忘因子的最小二乘法对上式进行估计，首 先将上式整理成为最小二乘法参数估计的标准形式：
[0069] 式中，卵)=A , < (0 =匕，Y⑴为测量值输出，Y (t) = ay_ ω r · Vx，
[0070] 采用带遗忘因子的最小二乘法对公式6中的Θ (t)进行估计，算法如下：
CN 105151047 A 说明书 5/9 页
[0074] 算法中I是单位矩阵；ε (t)是先验误差；K (t)是卡尔曼增益；P (t)是协方差矩 阵；λ是遗忘因子，λ的值越小，历史数据在新的参数估计周期中的权值就越低。
[0075] 通过使用带遗忘因子的最小二乘法进行参数估计，可得到第一质心侧偏角的微分 值處，再通过使用微处理器对處求积分，即可得到车辆实时的第一质心侧偏角的值，即：
[0077] Δ t是控制器的采样周期，一般采用5ms或者5ms的倍数。
[0078] 由上式可知，在使用传感器测量得到汽车行驶过程中的侧向加速度ay、横摆角速 度'和纵向速度Vx这三个参数后，就可以直接得到第一质心侧偏角的微分值處，再将爲 进行积分后就得出了第一质心侧偏角βρ
[0079] 步骤三S130 :汽车在行驶过程中，当车辆侧向加速度大于第二加速度&2时，采用 基于三自由度非线性汽车模型滤波算法进行第二质心侧偏角估计。第二加速度的值可能因 不同的车型而变化。作为一种优选的，a2= 0. 45g。
[0080] 三自由度车辆模型如图3所示，车辆的运动方程为：
[0083] 公式中，&和K f分别为车辆的后轮侧偏刚度和前轮侧偏刚度；a和b则分别是前 轴和后轴到质心的距离；m为车辆质量；Iz为绕z轴的转动惯量、ω 1^为横摆角速度、β 心侧偏角、S前轮转角（由方向盘转角传感器测量得到方向盘转角，再将方向盘转角值代 入传动比公式求得前轮转角。）、VX车辆纵向车速、a JP a y分别为汽车纵向加速度和车辆侧 向加速度，可由车辆加速度传感器测得。
[0084] 采用非线性滤波算法能够估计出第二质心侧偏角的值，通常可采用容积卡尔曼滤 波算法、容积卡尔曼滤波算法。
[0085] 作为一种优选的，本发明采用容积卡尔曼滤波算法，对第二质心侧偏角进行估计。
[0086] 整理为均方根嵌入式容积卡尔曼滤波器方程结构的标准形式：
[0087] X (t) = f (x (t), u (t), w (t))
[0088] z (t) = h (x (t)，u (t)，v (t))
[0089] 式中状态向量为x(t) = [cor，β，νχ]τ;量测向量为z(t) = [ay];控制向量为u(t) =[S，aJT;w(t)为过程噪声；v(t)为量测噪声；w(t)和v(t)满足均值为0,误差方差阵分 别为Q和R的正态分布。
[0090] 容积卡尔曼滤波算法分成三部分，分别是初始化、预报步和滤波步。根据三自由 度车辆模型中的状态方程，利用k时刻的状态量估计值和控制量Uk，计算状态量预报 值％猶，即预报步；利用％k+Ι时刻的控制量uk+1通过量测方程计算量测量预报值 电獅.1:，并对％&^:进行滤波并得到k+Ι时刻估计值即滤波步。
[0091] 初始化：在车辆行驶过程中，〇^和Vx可由加速度传感器中的侧向加速度测量模 块和测速模块测量得到初值，而第二质心侧偏角β Jlj设置为〇,则运行周期k = 1时刻 车辆模型的状态向量初值确定为.此时将车辆状态估计的误差方差初值设置为Plll = diag(10 10 10 10 10 10)
[0092] 预报：
[0093] (1)由k-Ι时刻估计结果得到k时刻车辆状态量的估计值
和估计误差方差阵 P k I k。由于使用的是容积卡尔曼滤波估计算法，所以需要生成车辆行驶状态量的Cu b a t u r e 点，因此对Pklk进行Cholesky分解，即可得到在k时刻估计误差方差阵的平方根矩阵
[0094] (2)根据球面-径向规则对车辆状态量估计值生成一组等权值的Cubature点， 即在车辆状态量估计值周围以估计误差方差阵的平方根矩阵形成一组均匀分布的点。根 据球面-径向规则生成的Cubature点的个数是状态维数的二倍，三自由度车辆模型的 状态变量个数为3,则每个状态量的Cubature点个数为6,即.
i = 1，2,…6,式中X1, k|k是在k时刻状态量的第i个Cubature点；ξ i为矩阵
I的第i列，η为状态量维数。
[0095] (3)利用离散化的车辆动态方程对车辆状态量的每一个Cubature点进行变换，得 到车辆状态量所有的Cubature点的预报值：
T;F为离散化形式的车辆模型的动态方程。
[0096] (4)根据球面-径向规则求状态量的预报值。对所有的车辆状态向量Cubature点 的与报纸进行加权求和。每个Cubature点的权值均为l/m，m = 2n，则Cubature点权值为 1/6。车辆状态向量预报值为
[0097] (5)得到车辆状态向量的预报值后，还需要计算预报误差协方差阵Pk+1|k:即
[0098] 式中：QkS k时刻车辆状态向量预报误差方差阵。
[00"] 滤波：
[0100] (1)得到车辆模型状态向量的预报值和预报误差方差阵后，就需要利用量测量对 状态量预报值进行滤波，进而得到车辆状态量的估计值。该过程需要利用量测方程对车辆 状态向量的预报值进行变换，进而得到状态估计量量测的预报值，并计算量测量的预报误 差方差阵。因此，对状态量预报误差协方差阵Pk+1|k进行Cholesky分解，得到状态量预报误 差方差阵的平方根矩阵Sk+1|k，即
[0101] (2)利用球面-径向规则在车辆状态量的预报值周围按照状态量预报误差方差平 方根矩阵生成一组等权值的点，即Cubature点Xi,k+1|k:
[0102] (3)通过动态状态估计模型中的量测方程对每个状态量预报值Cubature点进行 变换，得到量测量 z (t) = [ay]预报值的 Cubature 点 ZiMllkiZiMllk= H(Xi, k+1|k，uk)，式中 H为量测方程。
[0103] (4)对所有量测量预报值的Cubature点进行加权求和，进而得到量测量预报 值。根据球面-径向规则，每个Cubature点的权值为1/6,则量测量的预报值島i+yk:
[0104] (5)计算量测量的预报误差方差阵Pzz,k+1|k:
式中Rk+1为k+1 时刻量测误差方差阵。
[0106] (6)计算车辆状态量预报值和量测量预报值之间的互协方差矩阵Pxz, k+1|k:
[0107] (7)计算卡尔曼滤波增益
[0108] (8)利用k+Ι时刻量测量的量测值与量测量预报值之间的偏差，即新息，通过卡 尔曼滤波增益对车辆状态量预报值进行滤波，最终得到k+Ι时刻状态量估计值
式中zk+