基于micro:bit的Maqueen小车偏航角计算与直行控制技术咨询

首先明确：单独使用加速度计无法测量偏航角（yaw）。因为偏航是绕小车竖直轴（你的代码中映射后的Z轴）的旋转，而加速度计只能检测重力的分量，绕竖直轴旋转时重力在各轴的分量不会变化，因此无法通过加速度计直接计算偏航角。以下是可行的解决思路：

1. 利用micro:bit内置磁力计（罗盘）计算偏航角 #

micro:bit内置了磁力计，可以检测地球磁场的方向，绕竖直轴旋转时，磁场在水平平面的分量会发生变化，以此可以计算出偏航角。

实现步骤： #

• 轴映射匹配：和你调整加速度计轴的逻辑一致，因为micro:bit安装后新Z轴对应原Y轴，磁力计的轴也要做对应映射：

  // 假设原磁力计轴：X=水平前后，Y=水平左右，Z=竖直
  // 安装后新Z=原Y，所以磁力计轴映射为：
  int magX = compass.x();       // 新X轴 = 原X轴
  int magZ = compass.y();       // 新Z轴 = 原Y轴（小车竖直轴）
  int magY = compass.z();       // 新Y轴 = 原Z轴
  

• 校准磁力计：必须先校准磁力计以消除环境磁场干扰，micro:bit提供了校准API：

  compass.calibrate(); // 执行时需要让micro:bit在空中画"8"字完成校准
  

• 计算偏航角：利用水平面上的磁场分量（新X和新Z轴）计算偏航角：

  float yaw = atan2(magZ, magX) * 180 / 3.14159; // atan2(竖直轴磁场分量, 水平前后分量)
  

  注：atan2的参数顺序需要根据你的轴映射调整，确保旋转方向与实际小车转向一致。

注意点： #

Maqueen的直流电机运行时会产生电磁干扰，可能影响磁力计精度。可以尝试将micro:bit安装在远离电机的位置，或用铜箔对电机进行简单屏蔽；校准磁力计时最好让电机处于运行状态，以抵消电机的固定干扰。

2. 融合磁力计与加速度计数据（互补滤波） #

单独使用磁力计可能受环境磁场干扰出现跳变，而加速度计可以提供稳定的横滚/俯仰角，两者融合能得到更可靠的姿态数据：

• 用加速度计计算的roll和pitch来修正磁力计的姿态，减少倾斜对磁力计测量的影响；
• 对偏航角做简单的滑动加权平均，降低噪声：

  float last_yaw = 0;
  // 每次计算新的磁力计偏航角mag_yaw后
  float yaw = 0.9 * last_yaw + 0.1 * mag_yaw; // 权重可根据实际调整
  last_yaw = yaw;
  

3. 无磁力计方案：利用电机编码器计算偏航角 #

如果磁力计干扰无法解决，可以借助Maqueen小车自带的电机编码器，通过左右轮的转速差计算偏航角：

• 原理：小车曲线行驶时，左右轮的行驶距离会产生差值，差值除以轮距就是偏航角的变化量，累加后得到总偏航角。
• 实现代码示例：

  const float wheel_diameter = 6.5; // 车轮直径（单位：cm）
  const float wheel_base = 12;      // 轮距（左右轮中心距离，单位：cm）
  long last_left = 0, last_right = 0;
  float current_yaw = 0;
  
  void calculate_yaw() {
      long left_count = maqueen.readEncoder(LEFT);
      long right_count = maqueen.readEncoder(RIGHT);
      
      // 计算左右轮行驶距离
      float left_dist = (left_count - last_left) * 3.14159 * wheel_diameter / 360;
      float right_dist = (right_count - last_right) * 3.14159 * wheel_diameter / 360;
      
      // 计算偏航角变化量（弧度转角度）
      float delta_yaw = (left_dist - right_dist) / wheel_base * 180 / 3.14159;
      current_yaw += delta_yaw;
      
      last_left = left_count;
      last_right = right_count;
  }
  

• 优缺点：无需额外传感器，不受磁场干扰，但存在累计误差（长时间行驶后偏航角会漂移），适合短距离直行控制场景。

4. 直行控制逻辑 #

得到偏航角后，就可以通过PID或简单的比例控制调整左右电机转速，修正方向：

• 设定目标偏航角（比如小车启动时的初始yaw为0）；
• 计算当前yaw与目标yaw的差值error = current_yaw - target_yaw；
• 根据error调整左右电机转速：比如error为正（小车向右偏），则减慢右电机/加快左电机，反之则调整左电机转速。

内容的提问来源于stack exchange，提问作者Giannis Tzagarakis