CN102175463B - 一种基于改进卡尔曼滤波的汽车路试制动性能检测方法 - Google Patents

Abstract

本发明公开了一种基于改进卡尔曼滤波的汽车路试制动性能检测方法，本方法在借鉴导航领域机动载体的“当前”统计模型的基础上，建立汽车制动过程的系统运动模型，根据卡尔曼滤波理论，以单频载波相位单点GPS接收机输出的速度和方位角作为系统观测量，通过改进的卡尔曼滤波递推算法高频率、高精度地推算出汽车制动过程的平面运动坐标和速度，进而计算确定汽车制动距离和平均减速度MFDD。

Description

一种基于改进卡尔曼滤波的汽车路试制动性能检测方法

技术领域

本发明涉及汽车进行道路制动试验时运动参数和距离的测量，尤其涉及一种基于改进卡尔曼滤波的汽车路试制动性能检测方法，其目的在于确定汽车制动过程中的速度、运动轨迹(坐标)和距离等参数，以便评价汽车的制动性能，属于汽车道路试验与测试技术领域。

背景技术

随着交通及汽车工业的快速发展，全世界高速公路里程数及汽车保有量在迅速增加，道路交通事故逐步成为当今社会的一个突出问题，对汽车的安全性能进行及时检测和评价成为保证道路交通安全的必要手段，而制动性能又是汽车安全性能中最重要的一种安全性能。为此，西方发达国家制定了许多汽车制动性能检测与评价标准，以保证汽车的制动安全性。我国也出台了许多相关的国家标准：GB 7258-2004《机动车运行安全技术条件》、GB/T 12676-1999《汽车制动系统结构、性能和试验方法》以及GB/T 13594-2003《机动车和挂车防抱制动性能和试验方法》等国标都对汽车的制动性能检验和评价做了明确的规定。目前，对汽车制动性能检验的方法主要有路试检验方法和台试检验方法两种。其中，路试制动性能检验方法更符合实际情况，其检验结果更容易被各方接受，当对台试制动性能检验结果有质疑时就只能通过路试方法检验制动性能。

在汽车的路试制动性能检验中，主要有两种评价方法：采用制动距离检验行车制动性能和采用充分发出的平均减速度(MFDD)检验行车制动性能。无论那种方法都需要实时测量汽车制动时的汽车速度和距离。目前，国内外汽车试验与测试领域对汽车制动过程的速度或距离的测量主要有以下几种方式：喷迹法、速度计(最典型为光电五轮仪)测速法以及高精度双频载波相位差分GPS测速测轨迹法等。喷迹法是最传统的汽车运动轨迹测量方法，即在汽车的适当部位安装喷嘴，利用压缩空气或水泵将储水罐内染过色的水喷到地面上，从而形成制动时的汽车行车轨迹，利用轨迹通过皮尺即可量出汽车制动距离。喷迹法形象直观，但测量周期长、自动化程度低、效率低且雨天无法测试。光电五轮仪测速法就是利用安装在汽车上的光电五轮仪来实时测量汽车整个制动过程中的速度，通过速度对时间的积分可推算出汽车制动距离。光电五轮仪测速法具有较高的测试精度和自动化程度，但安装麻烦，并且在湿滑路面上无法进行测试。高精度双频载波相位差分GPS测速测轨迹法是利用一套高精度双频载波相位差分GPS系统(包括基站和移动站，基站安置在试验区域附近某固定位置，移动站安装在被测汽车上)来测量汽车制动过程中的运动轨迹和速度，根据轨迹可解算出汽车制动距离。高精度双频载波相位差分GPS测速测轨迹法具有精度高、测试时间短、在无遮挡情况下可全天候测量(夜晚、阴雨天及湿滑路面等不影响测量)等优点，但实时测量的输出频率低(最高20Hz)、成本高(目前一套大概在30万RMB左右)而难于推广。

发明内容

本发明的目的是克服现有技术的不足，提出一种基于改进卡尔曼滤波的汽车路试制动性能检测方法，该方法成本低、输出频率高、环境适应力强，在无遮挡情况下可适应湿滑路面、夜晚及阴雨天等复杂测量工况，且能够满足汽车制动测试精度要求。

本发明采用的技术方案如下：一种基于改进卡尔曼滤波的汽车路试制动性能检测方法，其特征在于：本方法在借鉴导航领域机动载体的“当前”统计模型的基础上，建立汽车制动过程的系统运动模型，根据卡尔曼滤波理论，以单频载波相位单点GPS接收机输出的速度和方位角作为系统观测量，通过改进的卡尔曼滤波递推算法高频率、高精度地推算出汽车制动过程的平面运动坐标和速度，进而计算确定汽车制动距离和平均减速度MFDD；

1)建立汽车制动过程的运动模型

汽车制动性能的主要评价指标包括制动距离、制动稳定性和充分发出的平均减速度MFDD，制动距离是指汽车加速到规定的某一初速度后急踩制动，从脚接触制动踏板时起至汽车停住时止汽车驶过的距离；制动稳定性是指制动过程中汽车的任何部位不允许超出规定宽度的试验通道的边缘线；充分发出的平均减速度MFDD的定义为：

$\mathrm{MFDD}=\frac{V_{\mathrm{bg}}^2-V_{\mathrm{ed}}^2}{25.92(S_e-S_b)}---\left(1\right)$

式中：MFDD——充分发出的平均减速度，单位为米每平方秒(m/s²)；

VO——试验车制动初速度，单位为千米每小时(km/h)；

V_bg——0.8VO，试验车速，单位为千米每小时(km/h)；

V_ed——0.1Vo，试验车速，单位为千米每小时(km/h)；

S_b——试验车速从VO到V_bg之间车辆行驶的距离，单位为米(m)；

S_e——试验车速从VO到V_ed之间车辆行驶的距离，单位为米(m)。

根据上述指标的含义，在得到包括汽车制动过程中的速度、平面坐标和距离在内的运动参数后，推算出汽车的制动距离和MFDD，并根据制动轨迹判断汽车的制动稳定性；

对于汽车制动过程，取系统状态变量为X＝[p_e v_e a_e p_n v_n a_n]^T，其中，P_e、v_e、a_e分别为汽车东向位置分量、速度分量和加速度分量，p_n、v_n、a_n分别为汽车的北向位置分量、速度分量和加速度分量，根据导航领域机动载体的“当前”统计模型，系统状态方程表示为：

$\stackrel{\·}{X}=A\·X+U+W---\left(2\right)$

式中，A表示状态阵，U表示输入阵，W表示系统输入噪声向量，且

其中，

是均值为0、方差为

的高斯白噪声，

是均值为0、方差为

的高斯白噪声，

分别为汽车东向和北向机动加速度的相关时间常数，取

对于汽车运动，它们取值范围在1s～20s，s表示时间单位秒，

分别为汽车“当前”东向和北向机动加速度分量的均值；

2)建立汽车运动的卡尔曼滤波模型

为推算汽车制动过程的系统各状态变量X＝[p_e v_e a_e p_n v_n a_n]^T，通过建立相应的卡尔曼滤波模型来实现，卡尔曼滤波器是以最小均方差为准则的最优状态估计滤波器，它不需要储存过去的测量值，只根据当前的观测值和前一时刻的估计值，利用计算机进行递推计算，便可实现对实时信号的估计，根据卡尔曼滤波理论，汽车运动的卡尔曼滤波模型除包括系统状态方程公式(2)外，还应包括系统观测方程，本发明选择单频载波相位单点GPS作为汽车运动的测量传感器，输出的速度和方位角作为系统观测量，则系统的观测方程可表示为

Z(t)＝h[t，X(t)]+V(t)    (3)

式(3)中，Z为观测向量，h为观测方程，t表示时间，V表示观测噪声向量，且

其中，v_{gps_gnd}为GPS接收机测量输出的对地平面速度，v_gnd为汽车的真实对地平面速度且满足β_gps表示GPS接收机输出的汽车运动方向相对于正北方向的方位角，β表示汽车的真实方位角，n_v为GPS接收机的速度观测噪声且n_v是均值为0、方差为

的高斯白噪声，n_β表示GPS接收机的方位角观测噪声且n_β是均值为0、方差为

的高斯白噪声，方位角β与东向速度v_e和北向速度v_n满足如下关系：

对系统状态方程(2)和观测方程(3)进行离散化处理，离散化后的卡尔曼滤波方程为：

式中，k表示离散化时刻；状态转移阵、系统输入阵和测量阵分别为

$U(k-1)=\left[\begin{array}{c}0\\ 0\\ \frac{T\·{\stackrel{\‾}{a}}_e(k-1)}{{\mathrm{\τ}}_{a_e}}\\ 0\\ 0\\ \frac{T\·{\stackrel{\‾}{a}}_n(k-1)}{{\mathrm{\τ}}_{a_n}}\end{array}\right],$

$h[k,X\left(k\right)]=\left[\begin{array}{c}\sqrt{{v_e}^2\left(k\right)+{v_n}^2\left(k\right)}\\ \mathrm{\β}\left(k\right)\end{array}\right],$ T表示离散的周期，

和

分别取为

及

即

和

分别表示a_e(k-1)和a_n(k-1)的滤波计算值，且

W与V是互不相关的零均值系统白噪声和观测白噪声向量；W对应的系统噪声协方差阵Q(k)为：

$Q\left(k\right)=\left[\begin{array}{cc}\frac{2{{\mathrm{\σ}}_{a_e}}^2}{{\mathrm{\τ}}_{a_e}}{Q}_e\left(k\right)& 0_{3\×3}\\ 0_{3\×3}& \frac{2{{\mathrm{\σ}}_{a_n}}^2}{{\mathrm{\τ}}_{a_n}}{Q}_n\left(k\right)\end{array}\right]$ 且 $Q_e\left(k\right)=Q_n\left(k\right)\≈\left[\begin{array}{ccc}{T}^5/20& T^4/8& T^3/6\\ T^4/8& T^3/3& T^2/2\\ T^3/6& T^2/2& T\end{array}\right],$ 其中，东向加速度方差的确定方法为：

${{\mathrm{\&sigma;}}_{a_e}}^2=\left\{\begin{array}{cc}(\frac{4}{\mathrm{\&pi;}}-1){[a_{\max }-{\hat{a}}_e\left(k\right)]}^2& {\hat{a}}_e\left(k\right)>0\\ (\frac{4}{\mathrm{\&pi;}}-1){[a_{-\max }+{\hat{a}}_e\left(k\right)]}^2& {\hat{a}}_e\left(k\right)<0\end{array}\right.,$

表示a_e(k)的滤波计算值，类似地，北向加速度方差

的确定方法为：

${{\mathrm{\&sigma;}}_{a_n}}^2=\left\{\begin{array}{cc}(\frac{4}{\mathrm{\&pi;}}-1){[a_{\max }-{\hat{a}}_n\left(k\right)]}^2& {\hat{a}}_n\left(k\right)>0\\ (\frac{4}{\mathrm{\&pi;}}-1){[a_{-\max }+{\hat{a}}_n\left(k\right)]}^2& {\hat{a}}_n\left(k\right)<0\end{array}\right.,$

表示a_n(k)的滤波计算值，a_max、a_-max分别表示已知的汽车加速度正上限绝对值和负下限绝对值，取a_max＝a_-max且取值范围为10m/s²～20m/s²；V对应的测量噪声协方差矩阵

其中，方差σ_v ²和σ_β ²可根据单频载波相位单点GPS接收机的速度和方位角测量噪声的统计特性确定；

式(4)中的观测方程为非线性方程，在应用卡尔曼滤波计算时，需先进行线性化处理，将观测方程在

附近按泰勒级数展开，

表示X的滤波计算值，保留一阶微量、忽略高阶微量后得

$Z\left(k\right)\&ap;h[k,\hat{X}(k,k-1)]+\frac{\&PartialD;h[k,\hat{X}(k,k-1)]}{\&PartialD;{\hat{X}}^T(k,k-1)}[X\left(k\right)-\hat{X}(k,k-1)]+V\left(k\right)$

令 $H\left(k\right)=\frac{\&PartialD;h[k,\hat{X}(k,k-1)]}{\&PartialD;{\hat{X}}^T(k,k-1)},$ 则 $H\left(k\right)=\left[\begin{array}{cccccc}0& H_{12}& 0& 0& H_{15}& 0\\ 0& H_{22}& 0& 0& H_{25}& 0\end{array}\right],$ 其中

$H_{12}=\frac{{\hat{v}}_e(k,k-1)}{\sqrt{{{\hat{v}}_e}^2(k,k-1)+{{\hat{v}}_n}^2(k,k-1)}},$ $H_{15}=\frac{{\hat{v}}_n(k,k-1)}{\sqrt{{{\hat{v}}_e}^2(k,k-1)+{{\hat{v}}_n}^2(k,k-1)}},$

$H_{22}=\frac{{\hat{v}}_n(k,k-1)}{{{\hat{v}}_e}^2(k,k-1)+{{\hat{v}}_n}^2(k,k-1)},$ $H_{25}=\frac{-{\hat{v}}_e(k,k-1)}{{{\hat{v}}_e}^2(k,k-1)+{{\hat{v}}_n}^2(k,k-1)},$

和分别表示v_e和v_n的滤波计算值；需指出的是，制动结束时，汽车速度为0，滤波计算值

与

可能均为0，若滤波递推过程还没结束，为保证算法的正确性，

与

取值不能为0，取为0.00001m/s，且满足

β_end为制动临近结束时的汽车方位角；

3)通过改进的卡尔曼滤波递推算法，推算出汽车制动过程的平面运动坐标和速度，进而计算确定汽车制动距离和平均减速度MFDD

对于式(4)所描述的系统状态方程和测量方程，运用卡尔曼滤波理论，建立下面的标准滤波递推过程，该递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新：

时间更新：

状态一步预测方程

一步预测误差方差阵

测量更新：

滤波增益矩阵K(k)＝P(k，k-1)·H^T(k)·[H(k)P(k，k-1)H^T(k)+R(k)]^-1

状态估计 $\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-h[k,\hat{X}(k,k-1)\left]\right]$

估计误差方差阵P(k)＝[I-K(k)·H(k)]·P(k，k-1)

标准卡尔曼滤波递推过程的周期与观测向量的数据更新周期一致，为保证系统具有较高的数据输出频率，对标准滤波过程进行改进，即测量更新的周期仍与GPS的输出周期一致，而时间更新的周期减小且满足测量更新周期是时间更新周期的整数倍；

对于每个离散时刻k＝1，2，...，k_end，k_end表示制动终止时刻，按递增顺序通过下面的改进卡尔曼滤波算法依次递推计算：

①若正整数k∈[1，k_end]且

m为正整数，T_gps表示GPS数据的更新周期，滤波算法只进行时间更新：

并取

P(k)＝P(k，k-1)；

②否则，即满足

m为正整数，滤波算法先进行时间更新，再采用标量化处理(scalar measurement processing)方法进行测量更新：

时间更新：

测量更新：

令P₁＝P(k，k-1)，

将

H(k)、Z(k)和R(k)阵分块，即

$h[k,\hat{X}(k,k-1)]=\left[\begin{array}{c}{h}_{r\_1}\\ h_{r\_2}\end{array}\right],H\left(k\right)=\left[\begin{array}{c}{H}_{r\_1}\\ H_{r\_2}\end{array}\right]$

$Z\left(k\right)=\left[\begin{array}{c}{Z}_1\\ Z_2\end{array}\right],R\left(k\right)=\left[\begin{array}{cc}{R}_1& 0\\ 0& R_2\end{array}\right]$

对于i从1到2，进行2次递推计算：

$K_i=\frac{P_i\&CenterDot;H_{r\_i}^T}{H_{r\_i}{P}_i{H}_{r\_i}^T+R_i}$

${\hat{X}}_{i+1}={\hat{X}}_i+K_i(Z_i-h_{r\_i})$

P_i+1＝(I-K_i·H_{r_i})·P_i

最终可得P(k)＝P₃， $\hat{X}\left(k\right)={\hat{X}}_3;$

经过上述滤波递推计算后，确定出汽车在每个离散时刻k＝1，2，...，k_start，...，k_end的运动参数

即确定了汽车的平面运动坐标

和对地绝对速度

随时刻k的变化情况，利用这些参数，进而就可计算出汽车的制动距离：

$S=\sqrt{{[{\hat{p}}_e\left(k_{\mathrm{end}}\right)-{\hat{p}}_e\left(k_{\mathrm{start}}\right)]}^2+{[{\hat{p}}_n\left(k_{\mathrm{end}}\right)-{\hat{p}}_n\left(k_{\mathrm{start}}\right)]}^2}$

式中，k_start和k_end分别表示制动起始时刻和终止时刻，和

分别为制动起始时刻的东向位置坐标和北向位置坐标，

和

分别表示制动终止时刻的东向位置坐标和北向位置坐标，再利用公式(1)，可以确定汽车的制动性能指标MFDD，通过在汽车上安装制动踏板力传感器，将踏板力传感器检测到的踏板力急剧变化时刻作为制动起始时刻k_start。

在进行滤波递推之前，需要确定系统状态向量和估计误差方差阵的初值

与P(0)；对于

取初始位置

即汽车路试开始时的位置作为坐标原点；初始速度

与

可根据汽车路试开始时的GPS测量的速度和方位角来确定，为保证滤波算法的稳定性，本发明中汽车道路试验开始时刻不能选择在速度为0的时刻，但可以是紧随其后的速度虽小但并不为0的时刻；初始加速度

与

可根据汽车初始运行情况来确定，在初始阶段使汽车较为平稳地运行，取

P(0)为一对角阵，对角线上各元素的取值是相应变量的滤波估计精度值平方的1～4倍。

本发明的优点及显著效果：

(1)本发明针对汽车路试制动性能检验而提出，能够满足汽车路试制动性能检验中对制动距离和MFDD的测量要求。

(2)可适应制动初速度在25～65Km/h之间(也是国标要求的汽车制动初速度范围)的汽车直线制动性能测试要求，且测量精度高，制动距离测量精度可达0.2m～0.3m，速度精度≤0.1m/s(RMS)。

(3)输出频率高，平面位置坐标、速度和距离等运动参数输出频率可达100Hz；环境适应力强，在无遮挡情况下可在湿滑路面、夜晚及阴雨天等复杂条件下完成与干燥路面上同等精度的汽车制动性能检验。

(4)滤波观测传感器为单频载波相位单点GPS接收机，系统成本低，改进的滤波测量算法稳定可靠，测试过程简便易行、效率高。

附图说明

图1是GPS接收机输出的方位角的含义与取值范围的示意图；

图2是某次汽车路试制动性能试验的汽车运动坐标/轨迹曲线图；

图3是图2的局部放大图(将要制动结束时的汽车运动轨迹图)；

图4是整个试验过程的汽车对地速度曲线图；

图5是整个试验过程的汽车东向位置分量p_e和北向位置分量p_n相对于时间的变化曲线图。

具体实施方式

汽车制动性能直接关系汽车的行车安全性，是汽车安全性能中最重要的一种性能。为此，国家制定了许多标准来检验汽车的制动性能，如GB 7258-2004《机动车运行安全技术条件》、GB/T 12676-1999《汽车制动系统结构、性能和试验方法》以及GB/T 13594-2003《机动车和挂车防抱制动性能和试验方法》等国标都对汽车的制动性能检验和评价做了明确的规定。据此，可以看出汽车路试制动性能检验主要有以下几个特点：

(1)汽车制动试验路面平整、坡度小。汽车道路制动性能检验应在平坦(坡度通常不大于1％)的路面上进行，因此可将汽车制动过程中的运动参数简化到二维平面内。通常，汽车道路制动性能检验是在汽车试验场内专用的开阔、平整路面上进行的。

(2)汽车制动试验过程持续时间不长。当将被测汽车加速到规定的初速度后，踩下制动踏板产生制动到汽车停止的时间跨度通常在几秒之内。

(3)汽车制动性能的主要评价指标有制动距离、制动稳定性和充分发出的平均减速度(MFDD)。制动距离是指汽车加速到规定的某一初速度后急踩制动，从脚接触制动踏板时起至汽车停住时止汽车驶过的距离。制动稳定性是指制动过程中汽车的任何部位不允许超出规定宽度的试验通道的边缘线。充分发出的平均减速度MFDD的定义为

$\mathrm{MFDD}=\frac{V_{\mathrm{bg}}^2-V_{\mathrm{ed}}^2}{25.92(S_e-S_b)}---\left(1\right)$

式(1)中：MFDD——充分发出的平均减速度，单位为米每平方秒(m/s²)；

VO——试验车制动初速度，单位为千米每小时(km/h)；

V_bg——0.8VO，试验车速，单位为千米每小时(km/h)；

V_ed——0.1VO，试验车速，单位为千米每小时(km/h)；

S_b——试验车速从VO到V_bg之间车辆行驶的距离，单位为米(m)；

S_e——试验车速从VO到V_ed之间车辆行驶的距离，单位为米(m)。

根据上述指标的含义，可以看出只要能够实时高频、高精度地得到汽车制动过程中的速度、平面轨迹(坐标)和距离等运动参数，就可以推算出汽车的制动距离、MFDD，并可以根据制动轨迹判断汽车的制动稳定性。

针对汽车路试制动性能检验的上述特点，本发明采用低成本的单频载波相位(L1)单点GPS接收机作为测量传感器，提出了一种基于改进卡尔曼滤波的汽车直线制动运动参数(主要指平面运动坐标和速度)与距离测量方法。该方法在借鉴导航领域机动载体的“当前”统计模型的基础上，建立了汽车制动过程的系统运动模型，再根据卡尔曼滤波理论，以单频载波相位单点GPS接收机输出的速度和方位角作为系统观测量，通过改进的卡尔曼滤波递推算法高频率、高精度地推算出汽车直线制动过程的平面运动坐标(轨迹)和速度，进而再计算确定汽车的有关制动参数，如制动距离和MFDD等。本发明的具体思路如下：

在导航领域，对运动载体的动态模型已先后提出了CA模型、Singer模型和“当前”统计模型等。其中，作匀速或匀加速直线运动的动载体适合采用CA模型；作等速和等加速范围的运动载体适宜采用Singer模型；但是许多载体的运动往往并不属于等速或等加速范围的运动(如汽车的运动)，而是遵循当以某一加速度运动时，下一时刻的加速度取值是有限的，即只能在“当前”加速度的某一领域内变化，这种情况适宜采用“当前”统计模型。通过对比几种模型，可以看出，“当前”统计模型的加速度特性更符合汽车的实际运行情况，更能准确地描述汽车的运动过程，因此本发明将借鉴导航领域的动载体“当前”统计模型来建立汽车制动过程的动态模型，即系统状态方程。

对于汽车制动过程，取系统状态变量为X＝[p_e v_e a_e p_n v_n a_n]^T，其中，p_e、v_e、a_e分别为汽车东向位置分量、速度分量和加速度分量，p_n、v_n、a_n分别为汽车的北向位置分量、速度分量和加速度分量，上角标^T表示向量或矩阵的转置(本发明中凡标有上角标^T的均表示对相应的向量或矩阵进行转置，而一般式子中出现的变量T表示离散的周期)。根据“当前”统计模型，系统状态方程可表示

$\stackrel{\&CenterDot;}{X}=A\&CenterDot;X+U+W---\left(2\right)$

式(2)中，A表示状态阵，U表示输入阵，W表示系统输入噪声向量，且

其中，

是均值为0、方差为

的高斯白噪声，

是均值为0、方差为

的高斯白噪声，分别为汽车东向和北向机动加速度的相关时间常数(通常取对于汽车运动，它们取值范围在1s～20s，s表示时间单位秒)，

分别为汽车“当前”东向和北向机动加速度分量的均值。

为推算汽车制动过程的系统各状态变量X＝[p_e v_e a_e p_n v_n a_n]^T，可通过建立相应的卡尔曼滤波模型来实现。卡尔曼滤波器是以最小均方差为准则的最优状态估计滤波器，它不需要储存过去的测量值，只根据当前的观测值和前一时刻的估计值，利用计算机进行递推计算，便可实现对实时信号的估计，具有数据存储量小、算法简便的特点。

根据卡尔曼滤波理论，汽车运动的卡尔曼滤波模型除包括系统状态方程(即式(2))外，还应包括系统观测方程。近年来，全球定位系统(Global Positioning System，GPS)得到了快速发展，相关技术日趋成熟。本发明将根据汽车制动试验的性能测试要求与特点，选择适当的低成本GPS作为系统观测传感器。

GPS技术的突出特点是能够在无遮挡情况下为全球用户实时、全天候地提供三维位置、三维速度和时间信息，并且没有累积误差，因此，GPS受到了越来越多广泛的重视和应用。当然，GPS也存在一些不足，如使用时周围不要有遮挡否则会影响精度，另外GPS信号的实时输出频率较低(最高20Hz)。针对不同精度的使用场合，GPS已形成了不同的解决方案：

(1)对于高端用户，可利用差分GPS技术来消除卫星钟差、轨道误差、大气传播误差等大部分误差以获取较高的定位精度，如双频载波相位差分GPS技术已可以达到厘米级的动态测量精度。但差分GPS至少需要两台接收机，使用和维护不方便，且成本高(如一套完整的双频载波相位差分GPS目前的价格在30万RMB左右)。

(2)对于中低端用户，可采用码相位或单频载波相位(L1)单点GPS接收机。这类接收机定位精度低，但应用时只需要一台，使用简便，成本低。对于单频载波相位GPS接收机还具有这样的特点：虽然定位精度低(通常在5m-10m CEP)，但速度测量精度高，可达0.05m/s。如加拿大Novatel公司的单频载波相位Superstar II接收机的位置测量精度为5m(CEP)，速度测量精度为0.05m/s(RMS)，输出位置、速度和时间信息的频率最高为5Hz，目前价格为几千元RMB。

根据GPS技术的发展现状，本发明选择单频载波相位单点GPS作为汽车运动的测量传感器，以该类接收机输出的速度和方位角作为系统观测量，则系统的观测方程可表示为

Z(t)＝h[t，X(t)]+V(t)    (3)

式(3)中，Z为观测向量，h为观测方程，t表示时间，V表示观测噪声向量，且

其中，v_{gps_gnd}为GPS接收机测量输出的对地平面速度，v_gnd为汽车的真实对地平面速度且满足

β_gps表示GPS接收机输出的汽车运动方向相对于正北方向的方位角，β表示汽车的真实方位角，通常GPS方位角的具体含义如图1所示，即：

若汽车行进方向如p1，则相应的方位角属于区间(0，π/2)，

若汽车行进方向如p2，则相应的方位角属于区间(-π/2，0)，

若汽车行进方向如p3，则相应的方位角属于区间(-π，-π/2)，

若汽车行进方向如p4，则相应的方位角属于区间(π/2，π)；

n_v为GPS接收机的速度观测噪声且n_v是均值为0、方差为

的高斯白噪声，n_β表示GPS接收机的方位角观测噪声且n_β是均值为0、方差为

的高斯白噪声。根据图1，可以看出方位角β与东向速度v_e和北向速度v_n满足如下关系

观测方程(3)的观测量中并没有包括单频载波相位GPS接收机的位置输出量，其原因在于这类接收机的定位精度差，引入位置观测量势必影响滤波器的估计精度。

在实际的卡尔曼滤波递推过程中，需要采用离散化的卡尔曼滤波模型。为此，对系统状态方程(2)和观测方程(3)先进行了离散化处理，离散化后的卡尔曼滤波方程为：

式中，k表示离散化时刻；状态转移阵、系统输入阵和测量阵分别为

$U(k-1)=\left[\begin{array}{c}0\\ 0\\ \frac{T\&CenterDot;{\stackrel{\&OverBar;}{a}}_e(k-1)}{{\mathrm{\&tau;}}_{a_e}}\\ 0\\ 0\\ \frac{T\&CenterDot;{\stackrel{\&OverBar;}{a}}_n(k-1)}{{\mathrm{\&tau;}}_{a_n}}\end{array}\right],$

$h[k,X\left(k\right)]=\left[\begin{array}{c}\sqrt{{v_e}^2\left(k\right)+{v_n}^2\left(k\right)}\\ \mathrm{\&beta;}\left(k\right)\end{array}\right],$ T表示离散的周期，且

W与V是互不相关的零均值系统白噪声和观测白噪声向量；W对应的系统噪声协方差阵Q(k)为

$Q\left(k\right)=\left[\begin{array}{cc}\frac{2{{\mathrm{\&sigma;}}_{a_e}}^2}{{\mathrm{\&tau;}}_{a_e}}{Q}_e\left(k\right)& 0_{3\&times;3}\\ 0_{3\&times;3}& \frac{2{{\mathrm{\&sigma;}}_{a_n}}^2}{{\mathrm{\&tau;}}_{a_n}}{Q}_n\left(k\right)\end{array}\right]$ 且 $Q_e\left(k\right)=Q_n\left(k\right)\&ap;\left[\begin{array}{ccc}{T}^5/20& T^4/8& T^3/6\\ T^4/8& T^3/3& T^2/2\\ T^3/6& T^2/2& T\end{array}\right],$ 其中，东向加速度方差的确定方法为

${{\mathrm{\&sigma;}}_{a_e}}^2=\left\{\begin{array}{cc}(\frac{4}{\mathrm{\&pi;}}-1){[a_{\max }-{\hat{a}}_e\left(k\right)]}^2& {\hat{a}}_e\left(k\right)>0\\ (\frac{4}{\mathrm{\&pi;}}-1){[a_{-\max }+{\hat{a}}_e\left(k\right)]}^2& {\hat{a}}_e\left(k\right)<0\end{array}\right.,$

表示a_e(k)的滤波计算值(本发明中带有上标符号^的变量均表示该变量的滤波计算值，即滤波估计值)，类似地，北向加速度方差

的确定方法为

${{\mathrm{\&sigma;}}_{a_n}}^2=\left\{\begin{array}{cc}(\frac{4}{\mathrm{\&pi;}}-1){[a_{\max }-{\hat{a}}_n\left(k\right)]}^2& {\hat{a}}_n\left(k\right)>0\\ (\frac{4}{\mathrm{\&pi;}}-1){[a_{-\max }+{\hat{a}}_n\left(k\right)]}^2& {\hat{a}}_n\left(k\right)<0\end{array}\right.,$

a_max、a_-max分别表示已知的汽车加速度正上限绝对值和负下限绝对值，针对本发明的应用场合，可取a_max＝a_-max且取值范围为10m/s²～20m/s²；V对应的测量噪声协方差矩阵

其中，方差σ_v ²和σ_β ²可根据GPS的速度和方位角测量噪声的统计特性来确定，如对于前面所述的单频载波相位Superstar II GPS接收机可取σ_v＝0.05m/s和σ_β＝0.08rad。

式(4)中的观测方程为非线性方程，在应用卡尔曼滤波计算时，需先进行线性化处理，将观测方程在

附近按泰勒级数展开，保留一阶微量、忽略高阶微量后得

$Z\left(k\right)\&ap;h[k,\hat{X}(k,k-1)]+\frac{\&PartialD;h[k,\hat{X}(k,k-1)]}{\&PartialD;{\hat{X}}^T(k,k-1)}[X\left(k\right)-\hat{X}(k,k-1)]+V\left(k\right)$

令 $H\left(k\right)=\frac{\&PartialD;h[k,\hat{X}(k,k-1)]}{\&PartialD;{\hat{X}}^T(k,k-1)},$ 则 $H\left(k\right)=\left[\begin{array}{cccccc}0& H_{12}& 0& 0& H_{15}& 0\\ 0& H_{22}& 0& 0& H_{25}& 0\end{array}\right],$ 其中

$H_{12}=\frac{{\hat{v}}_e(k,k-1)}{\sqrt{{{\hat{v}}_e}^2(k,k-1)+{{\hat{v}}_n}^2(k,k-1)}},$ $H_{15}=\frac{{\hat{v}}_n(k,k-1)}{\sqrt{{{\hat{v}}_e}^2(k,k-1)+{{\hat{v}}_n}^2(k,k-1)}},$

$H_{22}=\frac{{\hat{v}}_n(k,k-1)}{{{\hat{v}}_e}^2(k,k-1)+{{\hat{v}}_n}^2(k,k-1)},$ $H_{25}=\frac{-{\hat{v}}_e(k,k-1)}{{{\hat{v}}_e}^2(k,k-1)+{{\hat{v}}_n}^2(k,k-1)}.$ 需指出的是，制动结束时，汽车速度为0，滤波计算值

与

可能均为0，若滤波递推过程还没结束，为保证算法的正确性，

与取值不能为0，可取为尽量小的值(如取为0.00001m/s)且满足

β_end为制动临近结束时的汽车方位角(大概值即可)。

对于式(4)所描述的系统状态方程和测量方程，可运用卡尔曼滤波理论，建立下面的标准滤波递推过程(该递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新)：

时间更新：

状态一步预测方程

一步预测误差方差阵

测量更新：

滤波增益矩阵K(k)＝P(k，k-1)·H^T(k)·[H(k)P(k，k-1)H^T(k)+R(k)]^-1

状态估计 $\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-h[k,\hat{X}(k,k-1)\left]\right]$

估计误差方差阵P(k)＝[I-K(k)·H(k)]·P(k，k-1)

标准卡尔曼滤波递推过程的周期与观测向量的数据更新周期一致。由于本发明中的观测向量(GPS接收机输出的速度和方位角)的数据更新周期长，即频率较低，因此若采用上述标准卡尔曼滤波，势必导致系统的输出频率低。为保证系统具有较高的数据输出频率，对标准滤波过程进行改进，即测量更新的周期仍与GPS的输出周期一致，而时间更新的周期减小且满足测量更新周期是时间更新周期的整数倍(该整数的典型取值为20、10、5、1等)。

若GPS数据的更新周期为T_gps(典型值如0.2s、0.1s、0.5s、1s等)，则时间更新的周期T(即离散的周期，本发明常用的典型值如10ms、20ms、50ms等)应满足n为正整数且典型取值为20、10、5、1等。经改进后的递推算法，即对于k·T采样时刻的滤波递推算法(k为正整数)可描述为：

①若k＝1，2，...且

(m为正整数)，滤波算法只进行时间更新

并取

P(k)＝P(k，k-1)；

②否则，即满足

(m为正整数)，滤波算法不仅进行时间更新，而且进行测量更新

K(k)＝P(k，k-1)·H^T(k)·[H(k)P(k，k-1)H^T(k)+R(k)]^-1、

$\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-h[k,\hat{X}(k,k-1)\left]\right],$

P(k)＝[I-K(k)·H(k)]·P(k，k-1)。

对于上述改进后的滤波递推算法，需要说明的是：

(1)上述算法保证了系统能够达到较高的运动参数输出频率(例如若T_gps＝0.2s，T取为0.01s，则系统输出频率可达100Hz；当然，GPS的输出频率越高，并且T_gps/T越接近于1，则系统的测量精度越高)，输出的运动参数包括汽车平面运动坐标(即轨迹，由p_e和p_n确定)、汽车东向和北向速度以及对地绝对速度等。进而利用这些参数，可确定出汽车的制动距离和MFDD等汽车制动性能评价参数。如对于制动距离，可通过下式计算确定

$S=\sqrt{{[{\hat{p}}_e\left(k_{\mathrm{end}}\right)-{\hat{p}}_e\left(k_{\mathrm{start}}\right)]}^2+{[{\hat{p}}_n\left(k_{\mathrm{end}}\right)-{\hat{p}}_n\left(k_{\mathrm{start}}\right)]}^2}$

式中，k_start和k_end分别表示制动起始时刻和终止时刻，

和

分别为制动起始时刻的东向位置坐标和北向位置坐标，

和

分别表示制动终止时刻的东向位置坐标和北向位置坐标。通常，通过在汽车上安装制动踏板力传感器，将踏板力传感器检测到的踏板力急剧变化时刻作为制动起始时刻k_start。

(2)在递推过程中，需要确定输入阵U(k-1)中的“当前”东向和北向机动加速度分量的均值

与

确定方法之一是在汽车上安装两个加速度计，这两个加速度计分别沿汽车车体的纵轴和横轴并保持在水平面内，这样就可以测出汽车沿纵轴的加速度分量a_x和沿横轴的加速度分量a_y，进而可计算确定

与

其中α表示但该方法在实际处理中难于操作，一是α难以准确确定，二是因为安装双轴加速度计时很难保证被安装在水平面上，即使微小的倾斜也会引入较大的重力加速度分量误差，由于该误差很难被补偿掉而会导致较大的系统误差。考虑到这些因素，本发明将采用另外一种确定方法：根据汽车运动加速度变化的“当前”特点，在每次递推计算时，取

本发明采用的这种方法无需加速度计，不仅可以克服上述安装加速度计方法的不足，而且成本低、便于处理。

(3)在进行滤波递推之前，需要确定系统状态向量和估计误差方差阵的初值

与P(0)。对于

本发明的确定方法为：为便于处理，取初始位置

即汽车路试开始时的位置作为坐标原点；初始速度

与

可根据汽车路试开始时的GPS测量的速度和方位角来确定(为保证滤波算法的稳定性，本发明中汽车道路试验开始时刻不能选择在速度为0的时刻，但可以是紧随其后的速度虽小但并不为0的时刻)；初始加速度

与可根据汽车初始运行情况来确定，为处理方便，可在初始阶段使汽车较为平稳地运行，这样可取

初始估计误差方差阵P(0)为一对角阵，对角线上各元素的取值可取为是相应变量的滤波估计精度值平方的1～4倍。

(4)上述经过改进的卡尔曼滤波递推过程在测量更新过程中(即计算k(k)时)存在着矩阵的求逆运算。矩阵求逆时，计算量大且容易造成数值计算的不稳定。对此，在测量更新时本发明不直接采用矩阵求逆的方法，而采用标量化处理(scalar measurementprocessing)方法。具体而言，时间更新过程可按照上述滤波过程进行，而测量更新按以下递推算法进行：

令P₁＝P(k，k-1)，

将

H(k)、Z(k)和R(k)阵分块，即

$h[k,\hat{X}(k,k-1)]=\left[\begin{array}{c}{h}_{r\_1}\\ h_{r\_2}\end{array}\right],H\left(k\right)=\left[\begin{array}{c}{H}_{r\_1}\\ H_{r\_2}\end{array}\right],Z\left(k\right)=\left[\begin{array}{c}{Z}_1\\ Z_2\end{array}\right],R\left(k\right)=\left[\begin{array}{cc}{R}_1& 0\\ 0& R_2\end{array}\right]$

对于i从1到2，进行2次递推计算：

$K_i=\frac{P_i\&CenterDot;H_{r\_i}^T}{H_{r\_i}{P}_i{H}_{r\_i}^T+R_i}$

${\hat{X}}_{i+1}={\hat{X}}_i+K_i(Z_i-h_{r\_i})$

$P_{i+1}=(I-K_i\&CenterDot;H_{r\_i})\&CenterDot;P_i$

最终可得P(k)＝P₃， $\hat{X}\left(k\right)={\hat{X}}_3.$

经过上述滤波递推计算后，可确定出汽车在每个离散时刻k＝1，2，...，k_start，...，k_end的运动参数

即确定了汽车的平面运动坐标

和对地绝对速度 随时刻k的变化情况，利用这些参数，进而就可计算出汽车的制动距离：

$S=\sqrt{{[{\hat{p}}_e\left(k_{\mathrm{end}}\right)-{\hat{p}}_e\left(k_{\mathrm{start}}\right)]}^2+{[{\hat{p}}_n\left(k_{\mathrm{end}}\right)-{\hat{p}}_n\left(k_{\mathrm{start}}\right)]}^2}$

式中，k_start和k_end分别表示制动起始时刻和终止时刻，

和

分别为制动起始时刻的东向位置坐标和北向位置坐标，

和

分别表示制动终止时刻的东向位置坐标和北向位置坐标。当然，根据这些参数随时间的变化情况，再利用公式(1)，还可以确定汽车的制动性能指标MFDD。

为检验本发明提出的汽车制动运动参数与距离测量方法的实际效果，进行了实车试验。试验基本情况说明如下：

试验目的：检验本发明提出的汽车制动运动参数与距离测量方法的性能。

试验系统组成：试验系统由硬件设备以及软件采集与处理程序组成。硬件设备主要包括：PC104嵌入式计算机(奔腾800MHz CPU、256M内存、40G硬盘、AD采集通道和多路串口通道)、Novatel公司的单频载波相位GPS接收机Superstar II、踏板力传感器、赛欧实验车、皮尺、固定支架与车载电源逆变器等。其中，Superstar II接收机通过串口提供测量数据，其位置测量精度为5m(CEP)、速度测量精度为0.05m/s(RMS)，输出位置、速度、方位角和时间信息的频率最高为5Hz。试验前，将Superstar II接收机天线置于车顶中心位置附近，并通过串口将GPS接收机与PC104相连，踏板力传感器固定在汽车制动踏板上并将其输出的模拟信号连至PC104的AD采集通道；试验过程中，由PC104实时同步采集并存储GPS接收机的数据和踏板力传感器的数据。

软件采集程序采用多线程技术开发，以保证能够实时、同步的采集与存储汽车路试制动性能检验过程中的GPS测量数据和踏板力传感器数据。软件处理程序是按照本发明提出的汽车制动运动参数与距离测量方法开发的，主要对已采集到的汽车制动试验数据进行事后的分析与处理，以便评价汽车的制动性能。

试验设置：试验时，Superstar II接收机设置成位置、速度、方位角和时间信息输出方式，频率设置在最高的5Hz，即T_GPS＝0.2s；踏板力信号的AD采样频率设为200Hz。试验中，汽车运行至某一规定的制动初速度后踩制动踏板，此刻踏板力传感器信号将产生急剧变化，踏板力信号急剧变化的时刻就作为制动起始时刻k_start。试验结束后的滤波处理算法的离散周期T取为0.01s，即系统运动参数输出频率为100Hz。

试验路面与环境：在总装备部定远汽车试验场、南京江宁开发区等地的平坦路面上进行了多次跑车制动试验；试验路面平整、符合国标要求，路面既有干燥硬路面又有湿滑路面，路面周围开阔无遮挡。

试验结果：试验表明，本发明提出的汽车制动运动参数与距离测量方法具有良好的性能，能够满足汽车路试制动性能检验中对制动距离和MFDD的测量要求，主要指标和性能如下：①可适应制动初速度在25～65Km/h之间(也是国标要求的汽车制动初速度范围)的汽车直线制动性能测试要求，且测量精度高，制动距离测量精度可达0.2m～0.3m，速度精度≤0.1m/s(RMS)；②输出频率高，平面位置坐标、速度和距离等运动参数输出频率可达100Hz；③环境适应力强，在无遮挡情况下可在湿滑路面、夜晚及阴雨天等复杂条件下完成与干燥路面上同等精度的汽车制动性能检验；④采用单频载波相位GPS接收机作为滤波观测传感器，成本低、测试周期短、效率高。

为说明本发明的实际效果，下面就给出某次汽车路试制动性能检验的试验结果，试验结果曲线如图2～图5所示。为比较与检验本发明提出的测量方法的效果，需要用皮尺测量汽车在整个试验过程走过的距离以及汽车制动后行驶的距离。为此，在汽车行驶起点设白色标记，制动结束后在汽车停靠点也画出白色标记，另外在试验道路将要开始制动的位置设置一条白色带子，当汽车以某一制动初速度撞上该白带时立即踩制动踏板力以便产生制动。试验结束后，用皮尺测量汽车行驶起点和结束点的白色标记之间的距离，就是汽车整个试验过程走过的距离；用皮尺测量白色带子和汽车行驶结束点之间的距离，就是汽车的制动距离。

滤波计算的初始条件

P(0)＝diag[(0.2m)²(0.1m/s)²(0.5m/s²)²(0.2m)²(0.1m/s)²(0.5m/s²)²]，另外计算是取

a_max＝a_-max＝15m/s²。为保证滤波算法的稳定，汽车行驶起始时刻并不选在速度为0的时刻，而是选在紧随其后的速度虽小但并不为0的时刻(速度很小由此引入的距离误差很小可忽略不计)，因此，中的两个初始速度分量并不为0，它们的大小可根据此刻的GPS输出来确定。根据本发明提出的滤波算法递推的有关计算结果曲线如图2～图5所示。图2中的密集点线是整个试验过程(包括起动、运行至某一初速度后再制动)的汽车运动坐标/轨迹曲线，由于数据输出频率高，点线很密集。图3是图2的局部放大图，是试验临近结束时(即将要制动结束)的汽车运动轨迹图，图中的黑实点就表示滤波计算出的离散运动轨迹。图4是整个试验过程的汽车对地速度曲线，图中的竖直虚线表示制动起始时刻，它对应着制动踏板力传感器信号的急剧变化时刻，利用此时刻的坐标，可确定出汽车的制动初速度为28.42km/h。图5是整个试验过程的汽车东向位置分量p_e和北向位置分量p_n相对于时间的变化曲线，图中的竖直虚线对应着制动起始时刻。

根据本发明提出的测量方法计算出的汽车行驶起点和结束点之间的距离为42.37m(即整个试验过程的汽车运动距离)，相应的皮尺测量结果为42.58m，二者相差0.21m；根据本发明提出的测量方法计算出的汽车制动距离S为7.44m，相应的皮尺测量结果为7.57m，二者相差0.13m。

Claims (2)

1.一种基于改进卡尔曼滤波的汽车路试制动性能检测方法，其特征在于：本方法在借鉴导航领域机动载体的“当前”统计模型的基础上，建立汽车制动过程的系统运动模型，根据卡尔曼滤波理论，以单频载波相位单点GPS接收机输出的速度和方位角作为系统观测量，通过改进的卡尔曼滤波递推算法高频率、高精度地推算出汽车制动过程的平面运动坐标和速度，进而计算确定汽车制动距离和平均减速度MFDD；

1)建立汽车制动过程的系统运动模型

汽车制动性能的主要评价指标包括制动距离、制动稳定性和充分发出的平均减速度MFDD，制动距离是指汽车加速到规定的某一初速度后急踩制动，从脚接触制动踏板时起至汽车停住时止汽车驶过的距离；制动稳定性是指制动过程中汽车的任何部位不允许超出规定宽度的试验通道的边缘线；充分发出的平均减速度MFDD的定义为：

$\mathrm{MFDD}=\frac{V_{\mathrm{bg}}^2-V_{\mathrm{ed}}^2}{25.92(S_e-S_b)}---\left(1\right)$

式中：MFDD——充分发出的平均减速度，单位为米每平方秒(m/s²)；

Vo——试验车制动初速度，单位为千米每小时(km/h)；

V_bg——0.8Vo，试验车速，单位为千米每小时(km/h)；

V_ed——0.1Vo，试验车速，单位为千米每小时(km/h)；

S_b——试验车速从Vo到V_bg之间车辆行驶的距离，单位为米(m)；

S_e——试验车速从Vo到V_ed之间车辆行驶的距离，单位为米(m)；

根据上述指标的含义，在得到包括汽车制动过程中的速度、平面运动坐标和距离在内的运动参数后，推算出汽车的制动距离和MFDD，并根据制动轨迹判断汽车的制动稳定性；

对于汽车制动过程，取系统状态变量为X＝[p_e v_e a_e p_n v_n a_n]^T，其中，p_e、v_e、a_e分别为汽车东向位置分量、速度分量和加速度分量，p_n、v_n、a_n分别为汽车的北向位置分量、速度分量和加速度分量，根据导航领域机动载体的“当前”统计模型，系统状态方程表示为：

$\stackrel{\&CenterDot;}{X}=A\&CenterDot;X+U+W---\left(2\right)$

式中，A表示状态阵，U表示输入阵，W表示系统输入噪声向量，且

$A=\left[\begin{array}{cccccc}0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& -1/{\mathrm{\&tau;}}_{a_e}& 0& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& -1/{\mathrm{\&tau;}}_{a_n}\end{array}\right],$ $U=\left[\begin{array}{c}0\\ 0\\ {\stackrel{\&OverBar;}{a}}_e/{\mathrm{\&tau;}}_{a_e}\\ 0\\ 0\\ {\stackrel{\&OverBar;}{a}}_n/{\mathrm{\&tau;}}_{a_n}\end{array}\right],$ $W=\left[\begin{array}{c}0\\ 0\\ w_{a_e}\\ 0\\ 0\\ w_{a_n}\end{array}\right],$ 其中，

是均值为0、方差为

的高斯白噪声，

是均值为0、方差为

的高斯白噪声，

分别为汽车东向和北向机动加速度的相关时间常数，取

对于汽车运动，它们取值范围在1s～20s，s表示时间单位秒，分别为汽车“当前”东向和北向机动加速度分量的均值；

2)建立汽车运动的卡尔曼滤波模型

为推算汽车制动过程的系统状态变量X＝[p_e v_e a_e p_n v_n a_n]^T，通过建立相应的卡尔曼滤波模型来实现，卡尔曼滤波器是以最小均方差为准则的最优状态估计滤波器，它不需要储存过去的测量值，只根据当前的观测值和前一时刻的估计值，利用计算机进行递推计算，便可实现对实时信号的估计，根据卡尔曼滤波理论，汽车运动的卡尔曼滤波模型除包括系统状态方程公式(2)外，还应包括系统观测方程，选择单频载波相位单点GPS接收机作为汽车运动的测量传感器，输出的速度和方位角作为系统观测量，则系统的观测方程表示为

Z(t)＝h[t，X(t)]+V(t)            (3)

式(3)中，Z为观测向量，h为观测方程，t表示时间，V表示观测噪声向量，且 $Z=\left[\begin{array}{c}{v}_{\mathrm{gps}\_\mathrm{gnd}}\\ {\mathrm{\&beta;}}_{\mathrm{gps}}\end{array}\right],$ $h[t,X\left(t\right)]=\left[\begin{array}{c}{v}_{\mathrm{gnd}}\\ \mathrm{\&beta;}\end{array}\right],$ $V=\left[\begin{array}{c}{n}_v\\ n_{\mathrm{\&beta;}}\end{array}\right],$ 其中，v_{gps_gnd}为GPS接收机测量输出的对地平面速度，v_gnd为汽车的真实对地平面速度且满足

β_gps表示GPS接收机输出的汽车运动方向相对于正北方向的方位角，β表示汽车的真实方位角，n_v为GPS接收机的速度观测噪声且n_v是均值为0、方差为

的高斯白噪声，n_β表示GPS接收机的方位角观测噪声且n_β是均值为0、方差为

的高斯白噪声，方位角β与东向速度v_e和北向速度v_n满足如下关系：

对系统状态方程(2)和系统观测方程(3)进行离散化处理，离散化后的卡尔曼滤波方程为：

式中，k表示离散化时刻；状态转移阵、系统输入阵和测量阵分别为

$U(k-1)=\left[\begin{array}{c}0\\ 0\\ \frac{T\&CenterDot;{\stackrel{\&OverBar;}{a}}_e(k-1)}{{\mathrm{\&tau;}}_{a_e}}\\ 0\\ 0\\ \frac{T\&CenterDot;{\stackrel{\&OverBar;}{a}}_n(k-1)}{{\mathrm{\&tau;}}_{a_n}}\end{array}\right],$

$h[k,X\left(k\right)]=\left[\begin{array}{c}\sqrt{{v_e}^2\left(k\right)+{v_n}^2\left(k\right)}\\ \mathrm{\&beta;}\left(k\right)\end{array}\right],$ T表示离散的周期，

和

分别取为

及

即 ${\stackrel{\&OverBar;}{a}}_e(k-1)={\hat{a}}_e(k-1),$ ${\stackrel{\&OverBar;}{a}}_n(k-1)={\hat{a}}_n(k-1),$

和

分别表示a_e(k-1)和a_n(k-1)的滤波计算值，且

W与V是互不相关的零均值系统白噪声和观测白噪声向量；W对应的系统噪声协方差阵Q(k)为：

$Q\left(k\right)=\left[\begin{array}{cc}\frac{{{2\mathrm{\&sigma;}}_{a_e}}^2}{{\mathrm{\&tau;}}_{a_e}}{Q}_e\left(k\right)& 0_{3\&times;3}\\ 0_{3\&times;3}& \frac{2{{\mathrm{\&sigma;}}_{a_n}}^2}{{\mathrm{\&tau;}}_{a_n}}\end{array},Q_n\left(k\right)\right]$ 且 $Q_e\left(k\right)=Q_n\left(k\right)\&ap;\left[\begin{array}{ccc}{T}^5/20& T^4/8& T^3/6\\ T^4/8& T^3/3& T^2/2\\ T^3/6& T^2/2& T\end{array}\right],$ 其中，东向加速度方差

的确定方法为：

${{\mathrm{\&alpha;}}_{a_e}}^2=\left\{\begin{array}{cc}(\frac{4}{\mathrm{\&pi;}}-1){[a_{\max }-{\hat{a}}_e\left(k\right)]}^2& {\hat{a}}_e\left(k\right)>0\\ (\frac{4}{\mathrm{\&pi;}}-1){[a_{-\max }+{\hat{a}}_e\left(k\right)]}^2& {\hat{a}}_e\left(k\right)<0\end{array}\right.,$

表示a_e(k)的滤波计算值，类似地，北向加速度方差的确定方法为：

${{\mathrm{\&alpha;}}_{a_n}}^2=\left\{\begin{array}{cc}(\frac{4}{\mathrm{\&pi;}}-1){[a_{\max }-{\hat{a}}_n\left(k\right)]}^2& {\hat{a}}_n\left(k\right)>0\\ (\frac{4}{\mathrm{\&pi;}}-1){[a_{-\max }+{\hat{a}}_n\left(k\right)]}^2& {\hat{a}}_n\left(k\right)<0\end{array}\right.,$

表示a_n(k)的滤波计算值，a_max、a_-max分别表示已知的汽车加速度正上限绝对值和负下限绝对值，取a_max＝a_-max且取值范围为10m/s²～20m/s²；V对应的测量噪声协方差矩阵 $R\left(k\right)=\left[\begin{array}{cc}{{\mathrm{\&sigma;}}_v}^2& 0\\ 0& {{\mathrm{\&sigma;}}_{\mathrm{\&beta;}}}^2\end{array}\right],$ 其中，方差σ_v ²和σ_β ²根据单频载波相位单点GPS接收机的速度和方位角测量噪声的统计特性确定；

式(4)中的系统观测方程为非线性方程，在应用卡尔曼滤波计算时，需先进行线性化处理，将系统观测方程在

附近按泰勒级数展开，

表示X的滤波计算值，保留一阶微量、忽略高阶微量后得

$Z\left(k\right)\&ap;h[k,\hat{X}(k,k-1)]+\frac{\&PartialD;h[k,\hat{X}(k,k-1)]}{\&PartialD;{\hat{X}}^T(k,k-1)}[X\left(k\right)-\hat{X}(k,k-1)]+V\left(k\right)$

令 $H\left(k\right)=\frac{\&PartialD;h[k,\hat{X}(k,k-1)]}{\&PartialD;{\hat{X}}^T(k,k-1)},$ 则 $H\left(k\right)=\left[\begin{array}{cccccc}0& H_{12}& 0& 0& H_{15}& 0\\ 0& H_{22}& 0& 0& H_{25}& 0\end{array}\right],$ 其中

$H_{12}=\frac{{\hat{v}}_e(k,k-1)}{\sqrt{{{\hat{v}}_e}^2(k,k-1)+{{\hat{v}}_n}^2(k,k-1)}},$ $H_{15}=\frac{{\hat{v}}_n(k,k-1)}{\sqrt{{{\hat{v}}_e}^2(k,k-1)+{{\hat{v}}_n}^2(k,k-1)}},$

$H_{22}=\frac{{\hat{v}}_n(k,k-1)}{{{\hat{v}}_e}^2(k,k-1)+{{\hat{v}}_n}^2(k,k-1)},$ $H_{25}=\frac{-{\hat{v}}_e(k,k-1)}{{{\hat{v}}_e}^2(k,k-1)+{{\hat{v}}_n}^2(k,k-1)},$

和分别表示v_e和v_n的滤波计算值；需指出的是，制动结束时，汽车速度为0，滤波计算值

与

可能均为0，若滤波递推过程还没结束，为保证算法的正确性，

与

取值不能为0，取为0.00001m/s，且满足

β_end为制动临近结束时的汽车方位角；

3)通过改进的卡尔曼滤波递推算法，推算出汽车制动过程的平面运动坐标和速度，进而计算确定汽车制动距离和平均减速度MFDD

对于式(4)所描述的系统状态方程和系统观测方程，运用卡尔曼滤波理论，建立下面的标准卡尔曼滤波递推过程，该递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新：

时间更新：

状态一步预测方程

一步预测误差方差阵

测量更新：

滤波增益矩阵K(k)＝P(k，k-1)·H^T(k)·[H(k)P(k，k-1)H^T(k)+R(k)]^-1

状态估计 $\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-h[k,\hat{X}(k,k-1)\left]\right]$

估计误差方差阵P(k)＝[I-K(k)·H(k)]·P(k，k-1)

标准卡尔曼滤波递推过程的周期与观测向量的数据更新周期一致，为保证系统具有较高的数据输出频率，对标准卡尔曼滤波递推过程进行改进，即测量更新的周期仍与GPS接收机的输出周期一致，而时间更新的周期减小且满足测量更新周期是时间更新周期的整数倍；

对于每个离散化时刻k＝1，2，...，k_end，k_end表示制动终止时刻，按递增顺序通过下面的改进卡尔曼滤波算法依次递推计算：

①若正整数k∈[1，k_end]且

m为正整数，T_gps表示GPS接收机数据的更新

周期，滤波算法只进行时间更新：

并取

P(k)＝P(k，k-1)；

②否则，即满足

m为正整数，滤波算法先进行时间更新，再采用标量化处理(scalar measurement processing)方法进行测量更新：

时间更新：

测量更新：

令P₁＝P(k，k-1)，

将

H(k)、Z(k)和R(k)阵分块，即

$h[k,\hat{X}(k,k-1)]=\left[\begin{array}{c}{h}_{r\_1}\\ h_{r\_2}\end{array}\right],$ $H\left(k\right)=\left[\begin{array}{c}{H}_{r\_1}\\ H_{r\_2}\end{array}\right]$

$Z\left(k\right)=\left[\begin{array}{c}{Z}_1\\ Z_2\end{array}\right],$ $R\left(k\right)=\left[\begin{array}{cc}{R}_1& 0\\ 0& R_2\end{array}\right]$

对于i从1到2，进行2次递推计算：

$K_i=\frac{P_i\&CenterDot;H_{r\_i}^T}{H_{r\_i}{P}_i{H}_{r\_i}^T+R_i}$

${\hat{X}}_{i+1}={\hat{X}}_i+K_i(Z_i-h_{r\_i})$

P_i+1＝(I-K_i·H_{r_i})·P_i

最终得P(k)＝P₃，

经过上述滤波递推计算后，确定出汽车在每个离散化时刻k＝1，2，...，k_start，...，k_end的运动参数 $\hat{X}\left(k\right)={\left[\begin{array}{cccccc}{\hat{p}}_e\left(k\right)& {\hat{v}}_e\left(k\right)& {\hat{a}}_e\left(k\right)& {\hat{p}}_n\left(k\right)& {\hat{v}}_n\left(k\right)& {\hat{a}}_n\left(k\right)\end{array}\right]}^T,$ 即确定了汽车的平面运动坐标和对地绝对速度

随时刻k的变化情况，利用这些参数，进而就计算出汽车的制动距离：

$S=\sqrt{{[{\hat{p}}_e\left(k_{\mathrm{end}}\right)-{\hat{p}}_e\left(k_{\mathrm{start}}\right)]}^2+{[{\hat{p}}_n\left(k_{\mathrm{end}}\right)-{\hat{p}}_n\left(k_{\mathrm{start}}\right)]}^2}$

式中，k_start和k_end分别表示制动起始时刻和终止时刻，

和

分别为制动起始时刻的东向位置坐标和北向位置坐标，和

分别表示制动终止时刻的东向位置坐标和北向位置坐标，再利用公式(1)，确定汽车的制动性能指标MFDD，通过在汽车上安装制动踏板力传感器，将踏板力传感器检测到的踏板力急剧变化时刻作为制动起始时刻k_start。

2.根据权利要求1所述的基于改进卡尔曼滤波的汽车路试制动性能检测方法，其特征在于：在进行滤波递推之前，需要确定系统状态变量和估计误差方差阵的初值

与P(0)；对于

取初始位置即汽车路试开始时的位置作为坐标原点；初始速度

与根据汽车路试开始时的GPS接收机测量的速度和方位角来确定，为保证滤波算法的稳定性，本方法中汽车路试开始时刻不能选择在速度为0的时刻，而是紧随其后的速度虽小但并不为0的时刻；初始加速度

与根据汽车初始运行情况来确定，在初始阶段使汽车较为平稳地运行，取P(0)为一对角阵，对角线上各元素的取值是相应变量的滤波估计精度值平方的1～4倍。

Images