2024年电子设计竞赛自动驾驶小车交叉行驶原理(陀螺仪)实战解析与避坑指南

1次阅读

共计 1993 个字符，预计需要花费 5 分钟才能阅读完成。

在电子设计竞赛的自动驾驶小车项目中，交叉行驶是最考验控制精度的环节之一。很多队伍都会遇到这样的问题：明明直线跑得挺稳，一到十字路口转弯时，小车要么转过了头，要么转得不够，最终偏离预定路线。这背后的罪魁祸首往往是航向角的累积误差。

角度积分漂移：陀螺仪通过测量角速度积分得到角度，但哪怕微小的零偏误差，经过积分也会被不断放大
动态干扰：转弯时的加速度变化会影响加速度计的倾角测量，导致互补滤波失效
比赛失分点：规则要求小车必须严格按路线行驶，1cm 的偏差可能就导致任务失败

常见的三类姿态传感器对比：

传感器	优点	缺点
陀螺仪	动态响应快，不受磁场干扰	存在零漂，需要定期校准
加速度计	静态精度高	动态环境下噪声大
磁力计	提供绝对航向参考	易受电磁干扰

选择依据：
1. 交叉行驶场景需要快速响应方向变化（陀螺仪优势）
2. 比赛现场电磁环境复杂（磁力计易受干扰）
3. 小车频繁加减速（加速度计噪声大）

基本公式（离散时间系统）：

预测阶段：x_k = A·x_{k-1} + B·u_k
P_k = A·P_{k-1}·A^T + Q

更新阶段：K = P_k·H^T·(H·P_k·H^T + R)^-1
x_k = x_k + K·(z_k - H·x_k)
P_k = (I - K·H)·P_k

工程实现要点：

状态量(x)：选择角度和角速度偏差（β）
控制输入(u)：陀螺仪原始角速度读数
观测矩阵(H)：[1 0]（仅能观测到角度）
过程噪声(Q)：根据陀螺仪特性设置（通常 1e-6~1e-5）
观测噪声(R)：取决于加速度计精度（通常 0.01~0.1）

// 传感器初始化
void IMU_Init() {
  // MPU6050 初始化（陀螺仪 + 加速度计）Wire.begin();
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);  // PWR_MGMT_1 寄存器
  Wire.write(0);     // 解除休眠状态
  Wire.endTransmission(true);

  // 卡尔曼滤波器初始化
  float Q_angle = 0.001;  // 过程噪声协方差
  float Q_bias = 0.003;   // 零偏过程噪声
  float R_measure = 0.03; // 测量噪声协方差
  kalman.init(Q_angle, Q_bias, R_measure);
}

// 主循环数据处理
void loop() {
  // 读取陀螺仪数据（单位：度 / 秒）Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x43);  // 陀螺仪数据寄存器
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 6, true);
  float gyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;  

  // 读取加速度计计算倾角（单位：度）float accelAngle = atan2(accelY, accelX) * 180/PI;

  // 卡尔曼滤波
  float dt = (millis() - lastTime) / 1000.0;
  float kalAngle = kalman.update(accelAngle, gyroZ, dt);

  // 更新控制指令
  controlSteering(kalAngle);
  lastTime = millis();}