基于卡尔曼滤波与四轮轮速估计的车速算法验证

基于卡尔曼滤波的车速估计，基于四轮轮速估计车速，利用实车测试数据进行验证，该算法应用在实际工程项目中。 背景: 在自动驾驶运动控制或者底盘控制算法中，需要知道真实车速，以进行控制器的计算。 但实际车辆并无车速传感器，需要进行车速估计。 可从CAN上获取轮速等信息，进行估算。 优势: 采用四轮轮速估计纵向车速，轮速在车辆上容易获取，从底盘CAN即可读取; 采用运动学模型，实际应用无需关注复杂动力学模型及参数; 利用估计出的车速进行定位，精度较高; 软件环境: MATLAB/Simulink（2018以上版本） 算法输入:四轮轮速 算法输出:估计出的车速

轮胎碾过减速带时突然的转速波动，工程师老张盯着屏幕上的轮速曲线直皱眉——这要直接拿来当车速用，控制算法还不得抽风？在苏州某自动驾驶公司的车间里，这种车速估计的难题几乎每天都会上演。今天我们就来聊聊他们团队实战验证过的解决方案：基于四轮轮速的卡尔曼滤波车速估计。

轮子会骗人

直接从CAN总线拿到的轮速信号就像个调皮的孩子：过弯时内外侧轮速差能差出30%，急刹车时轮子转速甚至可能归零（此时车速可还没归零）。直接取四个轮速的平均值？实测发现遇到湿滑路面照样翻车。这倒逼团队必须用更聪明的方式处理原始信号。

来看这段预处理代码：

function v_wheel = preprocess_wheel_speed(fl, fr, rl, rr)
    % 剔除异常跳变（CAN丢帧补偿）
    wheel_speeds = medfilt1([fl, fr, rl, rr], 3);
    
    % 前轮加权系数更大（实测前轮信号更稳定）
    v_wheel = 0.4*mean(wheel_speeds(1:2)) + 0.3*mean(wheel_speeds(3:4));
    
    % 低于5km/h时启用特殊处理（应对轮速脉冲噪声）
    if v_wheel < 1.39 % m/s
        v_wheel = kalman_low_speed_correction(v_wheel);
    end
end

滤波窗口选3帧不是理论计算而是实车标定的结果——太大会导致响应延迟，太小又滤不干净噪声。前轮权重系数来自二十多组不同路况的数据回灌测试，这种工程细节论文里可不会写。

卡尔曼的魔法

核心算法在Simulink里搭了个变参数模型。这里有个反直觉的设计：状态变量不是车速而是车速变化率。因为实测发现这样更适应急加减速工况。

看预测阶段的处理：

% 状态转移矩阵（动态调整系数）
function F = get_F(dt, a_prev)
    % 急加速时增大过程噪声协方差
    if abs(a_prev) > 2.5
        F = [1 dt; 0 1]*1.2;
    else
        F = [1 dt; 0 1];
    end
end

这个动态调整的F矩阵是项目组的独门秘方。他们发现固定参数的卡尔曼滤波在车辆紧急制动时会有约0.3m/s的滞后，而动态调整后滞后缩短到0.1s以内——对自动驾驶的跟车场景来说，这0.2秒的进步可能就是追尾与否的区别。

实车验证的彩蛋

在苏州阳澄湖边的封闭测试场，算法经历了魔鬼测试：让车辆以40km/h压过搓板路，此时轮速信号会出现10Hz的剧烈振荡。传统移动平均算法估计的车速波动达±8km/h，而卡尔曼滤波版本控制在±2km/h以内。

不过老张偷偷告诉我，最难的其实是雨后的环氧地坪车库。地面附着力突变导致轮速信号像心电图，团队为此专门增加了滑移率补偿模块：

% 滑移率计算模块
slip_ratio = (wheel_speed - vehicle_speed) / max(vehicle_speed, 0.1);
if slip_ratio > 0.15
    % 进入滑移状态时切换观测模型
    set_param('KF_Model/Observer', 'sw', '1');
else
    set_param('KF_Model/Observer', 'sw', '0'); 
end

这种条件判断在理论模型里看起来很粗暴，但实测就是管用。毕竟工程落地不是写论文，可靠性和实时性才是王道。

不是结尾的结尾

现在这套算法已经跑在上千辆量产车上，但老张的团队还在持续迭代。最近他们在尝试融合IMU数据，不过用他的话说："别迷信多传感器融合，有时候把单信号用到极致才是工程智慧。" 下次你的车在湿滑路面稳稳巡航时，说不定正用着这个看似简单却暗藏玄机的轮速滤波算法呢。