请求补全基于MPU6050的车辆速度检测Python代码

要通过MPU-6050计算车辆速度，核心是提取线性加速度并积分，但需要先处理重力干扰和传感器零偏问题。以下是补全后的代码，包含校准、加速度滤波、速度积分逻辑：

完整代码 #

import time

# 假设read_raw_data函数和寄存器常量已提前定义
ACCEL_XOUT_H = 0x3B
ACCEL_YOUT_H = 0x3D
ACCEL_ZOUT_H = 0x3F
GYRO_XOUT_H = 0x43
GYRO_YOUT_H = 0x45
GYRO_ZOUT_H = 0x47

# 加速度计校准偏移量（静态校准后自动更新）
accel_x_offset = 0.0
accel_y_offset = 0.0
accel_z_offset = 0.0

def calibrate_accel(samples=1000):
    """静态校准加速度计，计算零偏值"""
    global accel_x_offset, accel_y_offset, accel_z_offset
    x_sum, y_sum, z_sum = 0.0, 0.0, 0.0
    print("正在校准加速度计，请保持传感器静止...")
    for _ in range(samples):
        acc_x = read_raw_data(ACCEL_XOUT_H)
        acc_y = read_raw_data(ACCEL_YOUT_H)
        acc_z = read_raw_data(ACCEL_ZOUT_H)
        x_sum += acc_x / 16384.0
        y_sum += acc_y / 16384.0
        z_sum += acc_z / 16384.0
        time.sleep(0.001)
    # 静止时Z轴加速度应为1g（重力），因此偏移量为均值减去对应基准值
    accel_x_offset = x_sum / samples
    accel_y_offset = y_sum / samples
    accel_z_offset = z_sum / samples - 1.0
    print(f"校准完成：X偏移={accel_x_offset:.4f}, Y偏移={accel_y_offset:.4f}, Z偏移={accel_z_offset:.4f}")

def stateCondition():
    # 先执行加速度计校准
    calibrate_accel()
    
    velocity = 0.0  # 初始速度（单位：m/s）
    prev_time = time.time()
    alpha = 0.2  # 低通滤波系数，越小滤波效果越强
    filtered_accel = 0.0  # 滤波后加速度缓存
    
    while True:
        current_time = time.time()
        dt = current_time - prev_time
        prev_time = current_time
        
        # 读取原始加速度和陀螺仪数据
        acc_x = read_raw_data(ACCEL_XOUT_H)
        acc_y = read_raw_data(ACCEL_YOUT_H)
        acc_z = read_raw_data(ACCEL_ZOUT_H)
        
        # 转换为g单位并去除校准偏移
        Ax = (acc_x / 16384.0) - accel_x_offset
        Ay = (acc_y / 16384.0) - accel_y_offset
        Az = (acc_z / 16384.0) - accel_z_offset
        
        # 提取前进方向线性加速度（假设车辆前进对应Y轴，需根据实际安装方向调整）
        # 水平行驶时重力仅作用于Z轴，因此Ay即为前进方向的线性加速度（单位：g）
        linear_accel = Ay
        
        # 低通滤波降低噪声影响
        filtered_accel = alpha * linear_accel + (1 - alpha) * filtered_accel
        
        # 转换为国际单位制加速度（1g = 9.8m/s²）
        accel_mps2 = filtered_accel * 9.8
        
        # 积分计算速度（v = v₀ + a*dt）
        velocity += accel_mps2 * dt
        
        # 转换为km/h（1m/s = 3.6km/h）
        velocity_kmh = velocity * 3.6
        
        # 限制速度非负（若需支持倒车可移除该逻辑）
        velocity_kmh = max(0.0, velocity_kmh)
        
        print(f"当前速度：{velocity_kmh:.2f} km/h")
        time.sleep(0.01)  # 控制采样频率，避免数据过载

关键注意事项 #

• 校准必须执行：传感器静止时的零偏误差会严重影响速度计算，务必在启动前完成校准。
• 轴对应调整：代码默认前进方向为Y轴，实际使用时需根据传感器安装角度修改对应的加速度轴（如改为Ax）。
• 漂移问题：纯加速度积分会累积误差，长时间运行后速度值会偏离真实值，建议结合GPS或轮速传感器进行修正。
• 滤波系数调整：alpha系数可根据实际噪声情况调整，平衡噪声抑制和速度响应速度。

内容的提问来源于stack exchange，提问作者Abhisek Pandey