基于卡尔曼滤波器的车速观测器

写在前

汽车的纵向车速的识别是汽车控制系统的一个极其重要的数据，车速识别的精度将直接影响到汽车稳定性控制系统性能的好坏。

目前已经可以买到准确测量车辆的纵向车速的装备，但是这种设备往往体积过大或者是成本过高，并且不适合长期安装在车上，长期使用，有耐用性较差，容易受干扰等等缺点。因此在实际车辆中均采用估算的方式间接得到车速。目前基于车辆的传感器进行纵向车速估算的方法主要有两种：

• 通过测量从动轮的轮速信号，同时采集车辆在行驶过程中的加速度信号来进行测量和换算的方法：
  通过轮速乘以车轮的滚动半径用于间接估算车速或者利用加速度信号与速度信号的积分关系来估算车速，这一类的算法虽然比较简单，但是长时间的积分会由于各种路面等环境噪声的影响导致结果严重偏离真实值，因此该类方法不适合长时间进行车速估算。
• 基于模型间接估计的方法：
  利用轮速信号等等的方法是指基于模型的间接估计就是利用原始信号首先经过数据处理，然后通过车辆模型，最后估计模型的状态信息作为最终的结果，其估计精度的好差也受到很多因素的制约。

本博文介绍一种基于平均轮速信号卡尔曼滤波算法的车速观测器。

卡尔曼滤波算法

卡尔曼滤波是一种最优估算方法，其算法具有递推性。对于车辆控制系统而言，由于在车辆的行驶过程中，会受到各种非确定性因素的干扰，因此车用的传感器所测得的信号都夹带有随机的噪声，所以为了保证信号的准确性，需要使用卡尔曼滤波对信号进行处理。估算问题是指通过对这一系列带有观测噪声和干扰信号的实际观测数据的处理，从中得到所需要的各种参量的估算值的过程。而在车辆动力学系统控制系统中，通过状态估算的方法得到车辆的各种状态参数的求解问题，称为车辆动力学估算问题。

卡尔曼滤波是以最小均方差误差为最佳估计准则，假设线性离散系统状态方程和观测方程如下所示：
$x(k+1)=A(k)x(k)+w(k)\tag{1}$ $y(k)=H(k)x(k)+v(k)\tag{2}$其中，系统的随机噪声协方差矩阵为 $Q$，观测噪声协方差矩阵为 $R$， $x(k+1)$是系统n维状态向量， $A(k)$为系统状态转移矩阵， $y(k)$是系统的m维观测向量， $w(k)$是系统的随机噪声， $v(k)$是系统的m维观测噪声， $H(k)$为观测矩阵。假设 $w(k),v(k)$为相互独立正态分布的白噪声。

卡尔曼滤波器主要包括预测和校正两个过程。总结卡尔曼滤波算法的基本步骤如下：
1.状态一步预测：
$\hat{x}{{(k)}^{-}}=A(k)\hat{x}(k-1) \tag{3}$
2.状态估算计算：
$\hat{x}(k)=\hat{x}{{(k)}^{-}}+K(k)[y(k)-H(k)\hat{x}{{(k)}^{-}}] \tag{4}$
3滤波增益矩阵：
$K(k)=P{{(k)}^{-}}{{H}^{T}}(k){{[H(k)P{{(k)}^{-}}{{H}^{T}}(k)+R]}^{-1}}\tag{5}$
4.一步预测误差方差阵：
$P{{(k)}^{-}}=A(k)P(k){{A}^{T}}(k)+Q\tag{6}$
5.估算误差方差阵：
$P(k)=[1-K(k)H(k)]P{{(k)}^{-}}\tag{7}$
上述即随机线性离散系统卡尔曼滤波的基本方程，只要给定初值，就可以根据观测值 ${{y}_{k}}$递推计算得到状态值的估算。

在汽车行驶过程中，可以将车身速度认为是均匀变化的信号，利用轮速的平均值作为测量值来估算车辆的纵向车速。若是四轮驱动的车辆，则可以采集四轮的轮速信号的平均值。由于车辆在信号的采集过程中，受到各种路面环境噪声的影响，使得采集到的信号往往会带有毛刺和误差，这种毛刺会随着误差的积累影响估算的精度，因此，采用了卡尔曼滤波法对信号进行滤波处理。

基于卡尔曼滤波的车速观测器

依据线性二自由度车辆模型建立卡尔曼滤波器。车身速度估计的系统状态方程与量测方程如下：
$X(k)=AX(k-1)+Ba(k)+W(k)\tag{8}$ $\hat{Y}(k)=CX(k)+V(k)\tag{9}$
其中， $X(k)=\left[ \begin{matrix}v(k) \\ {{a}_{e}}(k) \\ \end{matrix} \right]$，

$A=\left[ \begin{matrix} 1&\mathop{{}}^{{}}\Delta t \\ 0& \mathop{{}}^{{}}1 \\ \end{matrix} \right]$，

$B=\left[ \begin{matrix} \Delta t \\ 0 \\ \end{matrix} \right]$，

$W(k)=\left[ \begin{matrix} {{w}_{1}}(k) \\ {{w}_{2}}(k) \\ \end{matrix} \right]$，

$\hat{Y}(k)=\left[ {{v}_{wheel}} \right]$，

$C=\left[ 1\mathop{{}}^{{}}0 \right]$，

$V(k)=\left[ {{v}_{1}}(k) \right]$。
$\Delta t$为系统的采样时间， $v$为系统估算的车速， ${{a}_{e}}$为加速度信号的偏差， ${{v}_{wheel}}$为系统测量到的轮速信号。假设 ${{w}_{1}},{{w}_{2}},{{v}_{1}}$均为零均值相互独立的高斯分布的随机信号， $Q,R$分别为噪音 $W(k),V(k)$的协方差矩阵。