CN101655504B - 一种机动车辆自适应巡航系统的车速估计方法 - Google Patents

Abstract

一种机动车辆自适应巡航系统的车速估计方法，该方法以Kalman滤波算法为核心，利用轮速传感器(1)和纵向加速度传感器(2)的量测信号，通过对系统噪声及在制动状态下量测方程的修正，改进了算法对加速度信号静态偏差变化的跟踪能力，减小了以轮速计算车速量测值的误差，该方法具有不依赖大量实车试验、计算量小、可靠性高的特点。

Description

一种机动车辆自适应巡航系统的车速估计方法

技术领域：

本发明涉及一种车速估计方法，特别涉及用于机动车辆自适应巡航控制系统的车速估计方法。

背景技术：

本发明为一种车速(也称为“参考车速”)估计方法，主要用于道路行驶机动车辆(称为“主车”)的自适应巡航控制系统(Adaptive Cruise Control，ACC)，该系统利用一个车距传感器探测行驶前方是否存在以低于主车车速行驶的另一个机动车辆或其它障碍物(称为“目标对象”)，当存在目标对象时，依据一定规则确定其中对主车安全行驶最具危险的目标对象(称为“主目标”)，并控制主车与其保持安全距离(称为“安全车距”)行驶，当不存在目标对象时，控制主车按照驾驶员设定的期望车速(称为“设定车速”)行驶。

有别于传统巡航控制，自适应巡航控制系统能够控制主车与可能出现的目标对象之间保持安全车距行驶，安全车距的确定原则称为“安全策略”。主车的行驶车速是决定自适应巡航系统安全策略的主要因素之一，准确掌握车速信息是系统运用安全策略确定正确安全车距、保证行车安全的前提，这对于存在近距离目标对象的情况尤为重要。迄今已有多项关于自适应巡航控制系统的公开专利，如US2004/0064240、US2005/0240334、US2007/0233353、CN101326074A。但在这些专利中，均未涉及准确获取车速/加速度信息的技术手段。现有技术一般利用ABS轮速传感器测取的车轮转速信号计算行驶车速，这种方法的缺点是轮速信号包含较大的噪声成分，并且在制动过程中，车轮的滑移会导致计算结果与真值产生较大的偏差，严重影响其使用价值。

发明内容：

本发明的目的在于解决现有自适应巡航系统由于无法准确掌握车速信息所导致的系统性能难以保障的问题，提供一种两轮驱动机动车辆的车速估计方法。

本发明的特征在于利用机动车辆自适应巡航控制系统的纵向加速度传感器和轮速传感器的量测信号，以Kalman滤波算法为基础，通过估计系统噪声特征改进了算法对加速度信号静态偏差变化的跟踪能力，通过在制动状态下对量测方程的修正，减小了以轮速计算车速量测值的误差。试验表明，本发明具有不依赖大量实车试验、计算量小、可靠性高的特点。

本发明的基本原理说明如下：

A基本估计算法

理论上，在非制动工况下，非驱动轮轮速与车轮有效半径的乘积可以作为参考车速，但直接利用非驱动轮轮速计算参考车速会将轮速量测噪声带入估计结果，影响使用效果。当汽车制动时，通过加速度信号积分可以求取参考车速，但一方面，积分初值并非在任意时刻均可准确获取，另一方面，由于环境温度、道路坡度、车身俯仰运动等因素的影响，加速度传感器采集的加速度值存在静态偏差，从而极易导致很大的累计误差。

利用Kalman滤波算法能够有效地解决信号噪声问题，因此，本发明借助Kalman滤波算法估计主车的车速和加速度。

以主车车速、加速度信号偏差为状态变量，并视加速度信号偏差具有缓变特征，可以建立如下离散形式的状态空间表达式：

$\left[\begin{array}{c}{v}_r(k+1)\\ a_0(k+1)\end{array}\right]=\left[\begin{array}{cc}1& -\mathrm{dT}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{v}_r\left(k\right)\\ a_0\left(k\right)\end{array}\right]+\left[\begin{array}{c}\mathrm{dT}\\ 0\end{array}\right]a_m\left(k\right)+\left[\begin{array}{c}{w}_1\left(k\right)\\ w_2\left(k\right)\end{array}\right]---\left(1\right)$

$v_m\left(k\right)=\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{c}{v}_r\left(k\right)\\ a_0\left(k\right)\end{array}\right]+w_o\left(k\right)---\left(2\right)$

式中，v_r，车速；a₀，加速度信号静态偏差；dT，采样周期；a_m，加速度信号量测值；v_m，车速量测值；w₁，w₂，系统噪声；w_o，量测噪声。式(1)为系统方程，式(2)为量测方程。对其应用Kalman滤波算法即可实时估计状态变量，即车速和加速度信号静态偏差。a_m由加速度传感器直接测取，v_m由两非驱动轮轮速的均值依下式计算而得：

v_m＝(ω₁+ω_r)r_w/2

式中，ω₁、ω_r分别为轮速传感器测取的左右非驱动轮转速，r_w为非驱动轮半径。

B算法修正一

通常，Kalman滤波中的系统噪声和量测噪声均被视为零均值的白噪声。式(1)中的状态方程a₀(k+1)＝a₀(k)+w₂(k)表明，系统噪声w₂的均值表征了加速度信号静态偏差的变化情况。当加速度信号静态偏差以较大的速率波动时，w₂的零均值处理方式会影响滤波算法对加速度信号静态偏差估计值的跟踪能力，从而影响车速估计精度。

在汽车行驶过程中，环境温度相对稳定，因此加速度信号静态偏差发生波动的主要原因是汽车加减速、道路坡度所产生的俯仰运动导致了加速度传感器方位的变化。故而可以认为：a₀的变化率与加速度变化率存在一定的量化关系。

对于加速度信号伴随很强的噪声成分的情况，直接微分求取冲击度会引入很大的误差，因此，本发明先利用低通滤波器、通过控制通频带对加速度信号进行滤波处理。滤波器可以是依任意方法设计的数字滤波器，其阶次不应过高，须满足车载应用的要求，另外还须满足如下要求：对滤波前后加速度信号进行积分计算车速，除在加速度急剧变化的个别区段以外，滤波前和滤波后的两个车速变量的差值维持基本不变。

设所设计的低通滤波器为

$H\left(z\right)=\frac{\underset{i=0}{\overset{M}{\mathrm{\Σ}}}{b}_i{z}^{-i}}{1-\underset{j=0}{\overset{N}{\mathrm{\Σ}}}{c}_j{z}^{-j}}---\left(3\right)$

则由下式计算滤波后的加速度值

$a_f\left(k\right)=\underset{i=0}{\overset{M}{\mathrm{\Σ}}}{b}_i{a}_m(k-i)+\underset{j=0}{\overset{N}{\mathrm{\Σ}}}{c}_j{a}_f(k-j)---\left(4\right)$

其中，k＝1，2，3，...，表示离散过程的各采样时刻；a_f，滤波后的加速度值；b_i，c_j，滤波器系数。

操纵车辆反复进行持续加速、持续减速的直线行驶，记录加速度、非驱动轮轮速信号，并由两非驱动轮轮速均值计算车速量测值。分别利用前面的基本Kalman滤波算法、低通滤波器对多组行驶试验数据进行处理，求取加速度信号静态偏差估计值和滤波后的加速度值。在加速度滤波信号中找到加速度变化开始点、加速度变化结束点，求取加速度变化开始点之前一段时段内的加速度均值、加速度偏差估计均值；求取加速度变化结束点之后一段时段内的加速度均值a₂；在加速度偏差估计值中找到加速度变化开始点之后、加速度偏差估计值开始趋于稳定的点，求取该点之后一段时段内的加速度偏差估计均值。所述的三个时段的选取原则是：在该区段内，加速度值或加速度偏差估计值基本恒定。由下式计算加速度静态偏差估计值变化率与加速度变化率的比例关系κ：

$\mathrm{\κ}=\frac{{\stackrel{\‾}{a}}_{\mathrm{0,2}}-{\stackrel{\‾}{a}}_{\mathrm{0,1}}}{{\stackrel{\‾}{a}}_2-{\stackrel{\‾}{a}}_1}---\left(5\right)$

对多组行驶数据进行同样的分析，得到多个加速度静态偏差估计值变化率与加速度变化率的比例关系，对其求平均得到均值为κ。则，在Kalman滤波迭代的每一步，即可由下式计算系统噪声w₂的均值。

${\widehat{\stackrel{\‾}{w}}}_2\left(k\right)=\stackrel{\‾}{\mathrm{\κ}}(a_f\left(k\right)-a_f(k-1))---\left(6\right)$

式中，

w₂均值的估计值。

C算法修正二

在汽车制动过程中，由于车轮滑移的存在，以非驱动轮轮速计算的车速量测值与车速真值存在偏差，成为导致Kalman滤波估计误差的原因之一。因此，本发明在制动工况下利用非驱动轮的滑移率对量测方程式(2)进行修正。滑移率的定义式为

$s=\frac{v_r-v_m}{v_r}---\left(7\right)$

式中，v_r，车速；v_m，由车轮轮速计算的车速量测值。由上式易得滑移率、车速量测值与参考车速之间的关系，但实车应用的问题是，无法实时获取当前采样时刻的滑移率。在小采样周期的前提下，以上一时刻的滑移率代替，故修正后的观测方程可表示为：

$v_m\left(k\right)=\left[\begin{array}{cc}1-s(k-1)& 0\end{array}\right]\left[\begin{array}{c}{v}_r\left(k\right)\\ a_0\left(k\right)\end{array}\right]+w_v\left(k\right)---\left(8\right)$

由式(1)和式(8)组成新的状态空间模型，并由式(6)实时估计系统噪声w₂的均值，应用Kalman迭代算法即可实时获得车速估计值和加速度信号静态偏差估计值

本发明的具体步骤如下：

1)首先进行主车多组直线行驶加减速试验，记录纵向加速度传感器、轮速传感器测得的车身加速度和左右非驱动轮轮速信号。利用所得试验数据：

①设计低通滤波器去除加速度信号中的高频噪声：

如果试验数据中车身加速度传感器信号噪声成分过高，则设计低通滤波器去除加速度信号中的高频噪声。滤波器可以是依任意方法设计的数字滤波器，但应满足以下要求：阶次不应过高，须满足车载应用的要求；用低通滤波器对滤波前和滤波后的加速度信号进行积分可分别求得车速计算值，除在加速度急剧变化的个别区段以外，滤波前和滤波后的两个车速变量的差值维持基本不变。设所设计的低通滤波器为：

$H\left(z\right)=\frac{\underset{i=0}{\overset{M}{\mathrm{\Σ}}}{b}_i{z}^{-i}}{1-\underset{j=0}{\overset{N}{\mathrm{\Σ}}}{c}_j{z}^{-j}}$

则由下式计算滤波后的加速度值：

$a_f\left(k\right)=\underset{i=0}{\overset{M}{\mathrm{\Σ}}}{b}_i{a}_m(k-i)+\underset{j=0}{\overset{N}{\mathrm{\Σ}}}{c}_j{a}_f(k-j)$

以上两式中，k＝1，2，3，...，表示离散过程的各采样时刻；a_m，纵向加速度传感器量测信号；a_f，滤波后的加速度值；b_i，c_j为滤波器系数。

对于信噪比高的加速度传感器信号，可略去本步骤，即无需设计低通滤波器，此时，a_f(k)＝a_m(k)。

②对于如下的离散时间过程，进行Kalman滤波迭代，求取加速度量测信号的静态偏差：

状态方程x(k)＝Ax(k-1)+Bu(k-1)+w_s(k-1)

量测方程z(k)＝Cx(k)+w_o(k)

其中，k＝1，2，3，...，表示离散过程的各采样时刻；

状态向量x＝[v_ra₀]′，v_r为车速，a₀为加速度量测信号的静态偏差；

控制输入u＝a_m，a_m为加速度信号量测值；

量测输出z＝v_m，v_m＝(ω₁+ω_r)r_w/2，ω₁和ω_r分别为轮速传感器测得的左右非驱动轮转速，r_w为车轮半径；

状态转移矩阵 $A=\left[\begin{array}{cc}1& -\mathrm{dT}\\ 0& 1\end{array}\right],$ dT为采样周期；

输入矩阵 $B=\left[\begin{array}{c}\mathrm{dT}\\ 0\end{array}\right];$

观测矩阵C＝[10]；

w_s与w_o分别为过程噪声向量与量测噪声向量；

以

表示状态估计值，具体滤波过程如下：

·初始化，设置状态估计的Kalman滤波迭代初值：

状态初值 $\hat{x}\left(0\right)=\left\{\begin{array}{c}{\hat{v}}_r\left(0\right)\\ {\hat{a}}_0\left(0\right)\end{array}\right\}=\left\{\begin{array}{c}({\mathrm{\ω}}_1\left(0\right)+{\mathrm{\ω}}_r\left(0\right))r_w/2\\ 0\end{array}\right\}$

输入初值u(0)＝a_m(0)

·在每一个采样时刻k，进行以下迭代计算，即：

z(k)＝(ω₁(k)+ω_r(k))r_w/2

K(k)＝(AP(k-1)A^T+Q)C^T(C(AP(k-1)A^T+Q)C^T+R)^-1

P(k)＝(I-K(k)C)(AP(k-1)A^T+Q)

$\hat{x}\left(k\right)=\left\{\begin{array}{c}{\hat{v}}_r\left(k\right)\\ {\hat{a}}_0\left(k\right)\end{array}\right\}=A\hat{x}(k-1)+\mathrm{Bu}(k-1)+K\left(k\right)(z\left(k\right)-C(A\hat{x}(k-1)+\mathrm{Bu}(k-1)))$

u(k)＝a_m(k)

以上各式中，K为Kalman增益，P为误差协方差，Q为过程噪声协方差矩阵，R为量测噪声协方差矩阵。

通过Kalman滤波迭代，求得各采样时刻的加速度量测信号静态偏差估计值

③利用上述过程求得的各次试验的加速度值a_f和加速度量测信号静态偏差估计值的时间历程，求加速度静态偏差估计值变化率与加速度变化率的比例系数，具体步骤如下：

·取a_f时间历程中加速度由某一较为平稳的数值变化至另一较为平稳数值的区段；

·取各区段起点之前、a_f基本不变的一段数据并求其均值，记为a₁ ⁱ，其中，i表示区段序号；

·取各区段终点之后、a_f基本不变的一段数据并求其均值，记为a₂ ⁱ；

·对于加速度量测信号静态偏差估计值

在以上各区段起始时刻之前、基本不变的一段数据求均值，记为a_0，1 ⁱ：

·在以上各区段起点时刻之后，加速度量测信号静态偏差估计值

也会发生变化、并逐渐过渡至新的稳定值，对

再次趋于稳定的一段数据求均值，记为a_0，2 ⁱ；

·计算加速度静态偏差估计值变化率与加速度变化率的比例系数：

$\stackrel{\‾}{\mathrm{\κ}}=\frac{1}{L}\underset{i}{\mathrm{\Σ}}\frac{{\stackrel{\‾}{a}}_{0,2}^i-{\stackrel{\‾}{a}}_{\mathrm{0,1}}^i}{{\stackrel{\‾}{a}}_2^i-{\stackrel{\‾}{a}}_1^i}$

其中，κ，加速度静态偏差估计值变化率与加速度变化率的比例系数；L，总区段数。

2)在车载实时应用时进行如下迭代计算，计算车速估计值

·初始化，设置状态估计的Kalman滤波迭代初值：

状态初值 $\hat{x}\left(0\right)=\left\{\begin{array}{c}{\hat{v}}_r\left(0\right)\\ {\hat{a}}_0\left(0\right)\end{array}\right\}=\left\{\begin{array}{c}({\mathrm{\ω}}_1\left(0\right)+{\mathrm{\ω}}_r\left(0\right))r_w/2\\ 0\end{array}\right\}$

系统噪声均值初值 ${\widehat{\stackrel{\‾}{w}}}_s\left(0\right)=\left[\begin{array}{c}0\\ 0\end{array}\right]$

非驱动轮滑移率初值s(0)＝0

输入初值u(0)＝a_m(0)

·在每一个采样时刻k，对上述离散时间过程进行以下的迭代计算，即

z(k)＝(ω₁(k)+ω_r(k))r_w/2

K(k)＝(AP(k-1)A^T+Q)C^T(C(AP(k-1)A^T+Q)C^T+R)^-1

P(k)＝(I-K(k)C)(AP(k-1)A^T+Q)

$\hat{x}\left(k\right)=\left\{\begin{array}{c}{\hat{v}}_r\left(k\right)\\ {\hat{a}}_0\left(k\right)\end{array}\right\}=A\hat{x}(k-1)+\mathrm{Bu}(k-1)+K\left(k\right)(z\left(k\right)-C(A\hat{x}(k-1)+\mathrm{Bu}(k-1){+\widehat{\stackrel{\‾}{w}}}_s(k-1)))+{\widehat{\stackrel{\‾}{w}}}_s(k-1)$

$a_f\left(k\right)=\underset{i=0}{\overset{M}{\mathrm{\Σ}}}{b}_i{a}_m(k-i)+\underset{j=0}{\overset{N}{\mathrm{\Σ}}}{c}_j{a}_f(k-j)$

${\widehat{\stackrel{\‾}{w}}}_s\left(k\right)=\left[\begin{array}{c}0\\ \stackrel{\‾}{\mathrm{\κ}}(a_f\left(k\right)-a_f(k-1))\end{array}\right],$

为系统噪声均值的估计值

s为非驱动轮滑移率

u(k)＝a_m(k)

通过以上迭代，在每一个采样时刻k可得车速估计值

附图说明：

图1本发明所适用自适应巡航系统的一个具体实施例，图中：1纵向加速度传感器，2轮速传感器，3制动传感器，4车距传感器，5控制面板，6控制器，7执行机构，8参考车速估计模块，9安全策略计算模块，10车辆动力学控制模块；

图2本发明所适用自适应巡航系统具体实施例的控制器6的工作流程图；

图3确定加速度静态偏差估计值变化率与加速度变化率比例系数κ的辅助说明。

具体实施方式：

以下结合附图和具体实施方式进一步说明本发明。

图1所示为本发明所适用的自适应巡航系统的一个具体实施例，该系统包括纵向加速度传感器1，轮速传感器2，制动传感器3，车距传感器4，控制面板5，控制器6，执行机构7，其中，控制器6由参考车速估计模块8，安全策略计算模块9及车辆动力学控制模块10组成。纵向加速度传感器1测量车辆的纵向加速度，轮速传感器2测量非驱动轮转速，制动传感器3用于判断主车是否处于制动状态。车距传感器4探测在主车前方一定范围内的运动或静止对象，如车辆、行人、障碍物等，并探测各对象与主车之间的距离、相对方位角及相对运动速度。控制器6根据车距传感器4探测到的目标对象信息、主车自身的运动及状态信息产生控制命令，控制主车的发动机控制装置和制动控制装置。控制面板5提供系统交互界面。执行机构7为发动机控制装置和制动控制装置，执行控制器的车辆动力学控制模块10发出的指令，通过控制发动机及制动器控制主车的车速。所述各传感器测取对应的信号并通过CAN通信或其它车载通信方式将其发送至控制器。控制器6和控制面板5、执行机构7通过CAN通信或其它车载通信方式进行信号传输。

自适应巡航系统的工作由控制器6控制。控制器6的功能可分为三个模块：参考车速估计模块8、安全策略计算模块9及车辆动力学控制模块10。

参考车速估计模块8利用本发明的方法计算车速估计值

为安全策略计算9及车辆动力学控制10提供准确的主车车速信息。

安全策略计算模块9是完成以下功能的一整套规则、逻辑或计算方法：利用来自车距传感器4的各目标对象与主车之间的距离、相对方位角及相对运动信息，分析判断并确定主目标，并利用主目标与主车之间的距离及相对运动信息，结合控制面板5的参数设定情况，计算应与主目标保持的安全车距d_s，这里，主车的行驶车速是决定安全车距的一个主要因素；依据安全车距d_s、主目标与主车之间的距离及相对运动信息、主车自身的运动及状态信息，以实现安全车距d_s为目标，计算主车的期望车速v_d或期望加速度a_d；当不存在主目标时，以控制面板5的设定车速v_s计算主车的期望车速v_d或期望加速度a_d。

车辆动力学控制模块10基于车辆动力学及其控制原理，产生控制信号，发送至执行机构7，如发动机控制装置、制动控制装置，从而控制主车实现安全策略计算模块所确定的期望车速v_d或期望加速度a_d。

图2为控制器6的工作流程，如图2所示：

第一步S1：分析由车距传感器4探测到的目标对象信息，确定主目标；

第二步S2：如果存在主目标，则结合驾驶员在控制面板5设定的参数，计算安全车距d_s；否则，读取控制面板5的设定车速v_s；

第三步S3：采集加速度传感器1、轮速传感器2、制动传感器3的信号，利用本发明的车速估计算法，计算车速估计值

第四步S4：利用车速估计值

确定能够实现第二步S2所计算安全车距d_s或设定车速v_s的主车车辆动力学控制目标，这个控制目标表示为主车的期望加速度a_d或期望车速v_d；

第五步S5：基于车辆动力学及其控制原理，确定能够实现第四步S4计算得到的期望加速度a_d或期望车速v_d所需的对执行机构7，即发动机控制装置和制动控制装置的控制指令，并将指令发送至对应执行机构。

本发明的核心是控制器6中参考车速估计模块8所涉及的参考车速估计方法，具体实施步骤如下：

1)首先进行主车多组直线行驶加减速试验，记录纵向加速度传感器1、轮速传感器2测得的车身加速度和左右非驱动轮轮速信号。利用所得试验数据设计低通滤波器去除加速度信号中的高频噪声，并求加速度静态偏差估计值变化率与加速度变化率的比例系数κ，具体步骤为：

①设计低通滤波器去除加速度信号中的高频噪声：

如果试验数据中车身加速度传感器信号噪声成分过高，则设计低通滤波器去除加速度信号中的高频噪声。滤波器可以是依任意方法设计的数字滤波器，但应满足以下要求：阶次不应过高，须满足车载应用的要求；用低通滤波器对滤波前后加速度信号进行积分计算车速，除在加速度急剧变化的个别区段以外，二者之间的差值维持基本不变。设所设计的低通滤波器为：

$H\left(z\right)=\frac{\underset{i=0}{\overset{M}{\mathrm{\Σ}}}{b}_i{z}^{-i}}{1-\underset{j=0}{\overset{N}{\mathrm{\Σ}}}{c}_j{z}^{-j}}$

则由下式计算滤波后的加速度值：

$a_f\left(k\right)=\underset{i=0}{\overset{M}{\mathrm{\Σ}}}{b}_i{a}_m(k-i)+\underset{j=0}{\overset{N}{\mathrm{\Σ}}}{c}_j{a}_f(k-j)$

以上两式中，k＝1，2，3，...，表示离散过程的各采样时刻；a_m，纵向加速度传感器量测信号；a_f，滤波后的加速度值；b_i，c_j为滤波器系数。

对于信噪比高的加速度传感器信号，无需设计低通滤波器，此时，a_f(k)＝a_m(k)。

②对于如下的离散时间过程，进行Kalman滤波迭代，求取加速度量测信号的静态偏差：

状态方程x(k)＝Ax(k-1)+Bu(k-1)+w_s(k-1)

量测方程z(k)＝Cx(k)+w_o(k)

其中，k＝1，2，3，...，表示离散过程的各采样时刻；

状态向量x＝[v_ra₀]′，v_r为车速，a₀为加速度量测信号的静态偏差；

控制输入u＝a_m，a_m为加速度信号量测值；

量测输出z＝v_m，v_m＝(ω₁+ω_r)r_w/2，ω₁和ω_r分别为轮速传感器测得的左右非驱动轮转速，r_w为车轮半径；

状态转移矩阵 $A=\left[\begin{array}{cc}1& -\mathrm{dT}\\ 0& 1\end{array}\right],$ dT为采样周期；

输入矩阵 $B=\left[\begin{array}{c}\mathrm{dT}\\ 0\end{array}\right];$

观测矩阵 $C=\left[\begin{array}{cc}1& 0\end{array}\right];$

w_s与w_o分别为过程噪声向量与量测噪声向量；

以

表示状态估计值，具体滤波过程如下：

·初始化，设置状态估计的Kalman滤波迭代初值：

状态初值 $\hat{x}\left(0\right)=\left\{\begin{array}{c}{\hat{v}}_r\left(0\right)\\ {\hat{a}}_0\left(0\right)\end{array}\right\}=\left\{\begin{array}{c}({\mathrm{\ω}}_1\left(0\right)+{\mathrm{\ω}}_r\left(0\right))r_w/2\\ 0\end{array}\right\}$

输入初值u(0)＝a_m(0)

·在每一个采样时刻k，进行以下迭代计算，即：

z(k)＝(ω₁(k)+ω_r(k))r_w/2

K(k)＝(AP(k-1)A^T+Q)C^T(C(AP(k-1)A^T+Q)C^T+R)^-1

P(k)＝(I-K(k)C)(AP(k-1)A^T+Q)

$\hat{x}\left(k\right)=\left\{\begin{array}{c}{\hat{v}}_r\left(k\right)\\ {\hat{a}}_0\left(k\right)\end{array}\right\}=A\hat{x}(k-1)+\mathrm{Bu}(k-1)+K\left(k\right)(z\left(k\right)-C(A\hat{x}(k-1)+\mathrm{Bu}(k-1)))$

u(k)＝a_m(k)

以上各式中，K为Kalman增益，P为误差协方差，Q为过程噪声协方差矩阵，R为量测噪声协方差矩阵。

通过Kalman滤波迭代，求得各采样时刻的加速度量测信号静态偏差估计值

①求加速度静态偏差估计值变化率与加速度变化率的比例系数：

图3为确定加速度静态偏差估计值变化率与加速度变化率比例系数的一个具体实施例。

图3分别绘出了主车在一次减速过程中，滤波后的加速度值a_f和加速度静态偏差估计值的时间历程。图中，S表示加速度发生变化的开始点，t1为S点之前加速度较为恒定的一段时间；E1表示加速度变化的结束点，t2为E1点之后加速度较为恒定的一段时间；E2为加速度发生变化后，加速度偏差估计值也发生变化并开始趋于恒定的点，t3为加速度偏差估计值基本维持恒定的一段时间。

·求取S点之前t1时段内的加速度均值、加速度偏差估计均值，分别记为a₁ ¹和a_{0， 1} ¹；

·求取E1点之后t2时段内的加速度滤波均值，记为a₂ ¹；

·求取E2点之后t3时段内的加速度偏差估计均值，记为a_0，2 ¹；

·由下式求本实施例中加速度静态偏差估计值变化率与加速度变化率的比例系数κ₁：

${\mathrm{\κ}}_1=\frac{{\stackrel{\‾}{a}}_{\mathrm{0,2}}^1-{\stackrel{\‾}{a}}_{\mathrm{0,1}}^1}{{\stackrel{\‾}{a}}_2^1-{\stackrel{\‾}{a}}_1^1}$

·对各次试验数据处理后获得的加速度值a_f和加速度量测信号静态偏差估计值的时间历程，进行与本实施例同样的分析，得到多个加速度静态偏差估计值变化率与加速度变化率的比例关系κ_i，i＝1，2，3，...，L，由下式计算加速度静态偏差估计值变化率与加速度变化率的比例系数

$\stackrel{\‾}{\mathrm{\κ}}=\frac{1}{L}\underset{i}{\mathrm{\Σ}}{\mathrm{\κ}}_i$

②在控制器6的参考车速估计模块中采用如下迭代计算方法，实时计算参考车速估计值

·初始化，设置状态估计的Kalman滤波迭代初值：

状态初值 $\hat{x}\left(0\right)=\left\{\begin{array}{c}{\hat{v}}_r\left(0\right)\\ {\hat{a}}_0\left(0\right)\end{array}\right\}=\left\{\begin{array}{c}({\mathrm{\ω}}_1\left(0\right)+{\mathrm{\ω}}_r\left(0\right))r_w/2\\ 0\end{array}\right\}$

系统噪声均值初值 ${\widehat{\stackrel{\‾}{w}}}_s\left(0\right)=\left[\begin{array}{c}0\\ 0\end{array}\right]$

非驱动轮滑移率初值s(0)＝0

输入初值u(0)＝a_m(0)

·在每一个采样时刻k，对上述离散时间过程进行以下的迭代计算，即

z(k)＝(ω₁(k)+ω_r(k))r_w/2

K(k)＝(AP(k-1)A^T+Q)C^T(C(AP(k-1)A^T+Q)C^T+R)^-1

P(k)＝(I-K(k)C)(AP(k-1)A^T+Q)

$\hat{x}\left(k\right)=\left\{\begin{array}{c}{\hat{v}}_r\left(k\right)\\ {\hat{a}}_0\left(k\right)\end{array}\right\}=A\hat{x}(k-1)+\mathrm{Bu}(k-1)+K\left(k\right)(z\left(k\right)-C(A\hat{x}(k-1)+\mathrm{Bu}(k-1){+\widehat{\stackrel{\‾}{w}}}_s(k-1)))+{\widehat{\stackrel{\‾}{w}}}_s(k-1)$

$a_f\left(k\right)=\underset{i=0}{\overset{M}{\mathrm{\Σ}}}{b}_i{a}_m(k-i)+\underset{j=0}{\overset{N}{\mathrm{\Σ}}}{c}_j{a}_f(k-j)$

${\widehat{\stackrel{\‾}{w}}}_s\left(k\right)=\left[\begin{array}{c}0\\ \stackrel{\‾}{\mathrm{\κ}}(a_f\left(k\right)-a_f(k-1))\end{array}\right],$

为系统噪声均值的估计值

s为非驱动轮滑移率

u(k)＝a_m(k)

通过以上迭代，在每一个采样时刻k可得车速估计值

Claims (1)

1.一种机动车辆自适应巡航系统的车速估计方法，其特征在于利用纵向加速度传感器(1)和轮速传感器(2)的量测信号，通过Kalman滤波迭代，求得各采样时刻的加速度量测信号静态偏差估计值

及在每一个采样时刻k的车速估计值

所述的车速估计方法的步骤为：

(1)首先进行主车多组直线行驶加减速试验，记录纵向加速度传感器(1)和轮速传感器(2)测得的车身加速度和左右非驱动轮轮速信号；利用所得试验数据求加速度静态偏差估计值变化率与加速度变化率的比例系数，具体步骤为：

①设计低通滤波器去除加速度信号中的高频噪声：

如试验数据中车身加速度传感器信号噪声成分过高，则设计低通滤波器去除加速度信号中的高频噪声；设所设计的低通滤波器为：

$H\left(z\right)=\frac{\underset{i=0}{\overset{M}{\mathrm{\Σ}}}{b}_i{z}^{-i}}{1-\underset{j=0}{\overset{N}{\mathrm{\Σ}}}{c}_j{z}^{-j}}$

则由下式计算滤波后的加速度值：

$a_f\left(k\right)=\underset{i=0}{\overset{M}{\mathrm{\Σ}}}{b}_i{a}_m(k-i)+\underset{j=0}{\overset{N}{\mathrm{\Σ}}}{c}_j{a}_f(k-j)$

以上两式中，k＝1，2，3，...，表示离散过程的各采样时刻；a_m，纵向加速度传感器量测信号；a_f，滤波后的加速度值；b_i，c_j为滤波器系数；

对于信噪比高的加速度传感器信号，无需设计低通滤波器，此时，a_f(k)＝a_m(k)；

②对于如下的离散时间过程，进行Kalman滤波迭代，求取加速度量测信号的静态偏差：

状态方程x(k)＝Ax(k-1)+Bu(k-1)+w_s(k-1)

量测方程z(k)＝Cx(k)+w_o(k)

其中，k＝1，2，3，...，表示离散过程的各采样时刻；

状态向量x＝[v_r a₀]′，v_r为车速，a₀为加速度量测信号的静态偏差；

控制输入u＝a_m，a_m为加速度信号量测值；

量测输出z＝v_m，v_m＝(ω₁+ω_r)r_w/2，ω₁和ω_r分别为轮速传感器(2)测得的左右非驱动轮转速，r_w为车轮半径；

状态转移矩阵

dT为采样周期；

输入矩阵 $B=\left[\begin{array}{c}\mathrm{dT}\\ 0\end{array}\right];$

观测矩阵C＝[10]；

w_s与w_o分别为过程噪声向量与量测噪声向量；

以表示状态估计值，具体滤波过程如下：

·初始化，设置状态估计的Kalman滤波迭代初值：

状态初值 $\hat{x}\left(0\right)=\left\{\begin{array}{c}{\hat{v}}_r\left(0\right)\\ {\hat{a}}_0\left(0\right)\end{array}\right\}=\left\{\begin{array}{c}({\mathrm{\ω}}_1\left(0\right)+{\mathrm{\ω}}_r\left(0\right))r_w/2\\ 0\end{array}\right\}$

输入初值u(0)＝a_m(0)

·在每一个采样时刻k，进行以下迭代计算，即：

z(k)＝(ω₁(k)+ω_r(k))r_w/2

K(k)＝(AP(k-1)A^T+Q)C^T(C(AP(k-1)A^T+Q)C^T+R)^-1

P(k)＝(I-K(k)C)(AP(k-1)A^T+Q)

$\hat{x}\left(k\right)=\left\{\begin{array}{c}{\hat{v}}_r\left(k\right)\\ {\hat{a}}_0\left(k\right)\end{array}\right\}=A\hat{x}(k-1)+\mathrm{Bu}(k-1)+K\left(k\right)(z\left(k\right)-C(A\hat{x}(k-1)+\mathrm{Bu}(k-1)))$

u(k)＝a_m(k)

以上各式中，K为Kalman增益，P为误差协方差，Q为过程噪声协方差矩阵，R为量测噪声协方差矩阵；

通过Kalman滤波迭代，求得各采样时刻的加速度量测信号静态偏差估计值

③利用上述①、②求得的加速度值a_f和加速度量测信号静态偏差估计值

的时间历程，求加速度静态偏差估计值变化率与加速度变化率的比例系数，具体步骤如下：

·取加速度值a_f时间历程中加速度由某一较为平稳的数值变化至另一较为平稳数值的区段；

·取各区段起点之前加速度值a_f基本不变的一段数据，并求其均值，记为

其中，i表示区段序号；

·取各区段终点之后加速度值a_f基本不变的一段数据并求其均值，记为

·对于加速度量测信号静态偏差估计值

在所述各区段起始时刻之前，加速度量测信号静态偏差估计值

基本不变的一段数据求均值，记为

·在所述各区段起点时刻之后，加速度量测信号静态偏差估计值

发生变化，并逐渐过渡至新的稳定值，对加速度量测信号静态偏差估计值

再次趋于稳定的一段数据求均值，记为

·按以下公式计算加速度静态偏差估计值变化率与加速度变化率的比例系数：

$\stackrel{\‾}{\mathrm{\κ}}=\frac{1}{L}\underset{i}{\mathrm{\Σ}}\frac{{\stackrel{\‾}{a}}_{\mathrm{0,2}}^i-{\stackrel{\‾}{a}}_{\mathrm{0,1}}^i}{{\stackrel{\‾}{a}}_2^i-{\stackrel{\‾}{a}}_1^i}$

其中，

加速度静态偏差估计值变化率与加速度变化率的比例系数；L，总区段数；

(2)在车载实时应用时进行如下迭代计算，计算车速估计值

·初始化，设置状态估计的Kalman滤波迭代初值：

状态初值 $\hat{x}\left(0\right)=\left\{\begin{array}{c}{\hat{v}}_r\left(0\right)\\ {\hat{a}}_0\left(0\right)\end{array}\right\}=\left\{\begin{array}{c}({\mathrm{\ω}}_1\left(0\right)+{\mathrm{\ω}}_r\left(0\right))r_w/2\\ 0\end{array}\right\}$

系统噪声均值初值 ${\widehat{\stackrel{\‾}{w}}}_s\left(0\right)=\left[\begin{array}{c}0\\ 0\end{array}\right]$

非驱动轮滑移率初值s(0)＝0

输入初值u(0)＝a_m(0)

·在每一个采样时刻k，对上述离散时间过程进行以下的迭代计算，即

z(k)＝(ω₁(k)+ω_r(k))r_w/2

K(k)＝(AP(k-1)A^T+Q)C^T(C(AP(k-1)A^T+Q)C^T+R)^-1

P(k)＝(I-K(k)C)(AP(k-1)A^T+Q)

$\hat{x}\left(k\right)=\left\{\begin{array}{c}{\hat{v}}_r\left(k\right)\\ {\hat{a}}_0\left(k\right)\end{array}\right\}=A\hat{x}(k-1)+\mathrm{Bu}(k-1)+K\left(k\right)(z\left(k\right)-C(A\hat{x}(k-1)+\mathrm{Bu}(k-1)+{\widehat{\stackrel{\‾}{w}}}_s(k-1)))+{\widehat{\stackrel{\‾}{w}}}_s(k-1)$

$a_f\left(k\right)=\underset{i=0}{\overset{M}{\mathrm{\Σ}}}{b}_i{a}_m(k-i)+\underset{j=0}{\overset{N}{\mathrm{\Σ}}}{c}_j{a}_f(k-j)$

${\widehat{\stackrel{\‾}{w}}}_s\left(k\right)=\left[\begin{array}{c}0\\ \stackrel{\‾}{\mathrm{\κ}}(a_f\left(k\right)-a_f(k-1))\end{array}\right],$

为系统噪声均值的估计值

s为非驱动轮滑移率

u(k)＝a_m(k)

通过以上迭代，即求得在每一个采样时刻k的车速估计值

Images