MPU6886 与 Arduino Mega 2560：转向角计算和漂移问题

Question

我正在开展一个项目，需要使用连接到 Arduino Mega 2560 的 MPU6886 传感器来计算车辆的转向角。我面临的问题是转向角随着每次完整旋转而漂移。问题详细如下：

第一次旋转（360度）后，角度读数正确。 第二次旋转后，角度读数显示 360 + 4 度。 第三次旋转后，显示360+12度，零角偏移12度。 随着每次旋转，漂移都会增加。 这是我正在使用的代码：

#include <Wire.h>
#include <Ethernet.h>

// MPU6886 I2C address
#define MPU6886_ADDR 0x68
#define MPU6886_GYRO_XOUT_H 0x43

// Ethernet setup
byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED }; // Replace with your Ethernet shield MAC address
IPAddress ip(169, 254, 5, 2); // Replace with your desired IP address
EthernetServer server(80);

// Global variables
float totalAngle = 0.0;
unsigned long previousTime = 0;
float gyroZ_zero = 0.0;
const float stationaryThreshold = 0.05; // Threshold to detect if the sensor is stationary
unsigned long ethernetInterval = 1000; // Interval in milliseconds to send data over Ethernet
unsigned long previousEthernetTime = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin(); // Initialize I2C communication

  // Initialize MPU6886
  Wire.beginTransmission(MPU6886_ADDR);
  Wire.write(0x6B); // Power management register
  Wire.write(0x00); // Wake up MPU6886
  if (Wire.endTransmission() != 0) {
    Serial.println("Failed to initialize MPU6886!");
    while (1);
  } else {
    Serial.println("MPU6886 initialized successfully.");
  }

  // Calibrate the gyro to zero
  calibrateGyro();

  Ethernet.begin(mac, ip); // Initialize Ethernet with MAC and IP
  server.begin(); // Start Ethernet server
  Serial.print("Server is at ");
  Serial.println(Ethernet.localIP());
  previousTime = millis(); // Initialize time tracking
}

void calibrateGyro() {
  int numReadings = 100;
  long sum = 0;

  for (int i = 0; i < numReadings; i++) {
    int16_t gyroZ;
    readGyroData(&gyroZ);
    sum += gyroZ;
    delay(10);
  }

  gyroZ_zero = sum / (float)numReadings;
  Serial.print("Gyro zero point: ");
  Serial.println(gyroZ_zero);

  // Reset the total angle to 0 after calibration
  totalAngle = 0.0;
}

void readGyroData(int16_t *gyroZ) {
  Wire.beginTransmission(MPU6886_ADDR);
  Wire.write(MPU6886_GYRO_XOUT_H);
  if (Wire.endTransmission(false) != 0) {
    Serial.println("Failed to request gyro data!");
    return;
  }

  Wire.requestFrom(MPU6886_ADDR, 2); // Request 2 bytes of data
  if (Wire.available() == 2) {
    *gyroZ = Wire.read() << 8 | Wire.read(); // Combine high byte and low byte
  } else {
    Serial.println("Failed to read gyro data!");
  }
}

void loop() {
  unsigned long currentTime = millis();
  float deltaTime = (currentTime - previousTime) / 1000.0;
  previousTime = currentTime;

  int16_t gyroZ;
  readGyroData(&gyroZ);

  // Convert gyroZ to degrees per second using sensor sensitivity (assuming ±250 °/s range)
  float gyroZ_degPerSec = (gyroZ - gyroZ_zero) / 131.0;

  // If the gyroZ rate is below the stationary threshold, set it to zero
  if (abs(gyroZ_degPerSec) < stationaryThreshold) {
    gyroZ_degPerSec = 0.0;
  }

  // Calculate angle change using gyroZ rate and time interval
  float angleChange = gyroZ_degPerSec * deltaTime;

  // Update total angle by adding the angle change
  totalAngle += angleChange;

  // Normalize totalAngle to 0-360 degrees range
  while (totalAngle >= 360.0) {
    totalAngle -= 360.0;
  }
  while (totalAngle < 0.0) {
    totalAngle += 360.0;
  }

  // Print totalAngle to monitor the progression
  Serial.print("Total Angle:");
  Serial.print(totalAngle);
  Serial.print("\tGYRO_Z values:");
  Serial.println(gyroZ);

  // Check if it's time to send data over Ethernet
  if (currentTime - previousEthernetTime >= ethernetInterval) {
    previousEthernetTime = currentTime;

    // Check for Ethernet client
    EthernetClient client = server.available();
    if (client) {
      if (client.connected()) {
        // Send the total angle value with a specific format


        client.print("Encoder 1 Angle: ");
        client.print(totalAngle);
        client.println(" degrees, Steering Angle: Right");

        // Print connection status
        Serial.println("Ethernet connection is live.");
      } else {
        Serial.println("Ethernet connection is not live.");
      }
    }
  }

  delay(10); // Adjust delay as needed (to match sensor read rate or for serial stability)
}

是否有推荐的方法根据 MPU6886 传感器数据使用偏航、俯仰和滚动来计算转向角？任何有关这方面的指导将不胜感激。

我希望每次完整旋转后角度都能正确重置，一次旋转后显示 360 度，第二次旋转后显示 360 度，依此类推，没有任何漂移。

Answer 1

根据他们的数据表，您用于陀螺仪校准的神奇“131.0”数字只是一个“典型”值（没有给出误差线或容差），并且在临时数据表中，这是我在快速互联网搜索。数据表中也有一个用于示例代码的 Github 链接。如果您能找到的话，值得寻找更新的、临时性的数据表。

代码中需要更改的行是：

// Convert gyroZ to degrees per second using sensor sensitivity (assuming ±250 °/s range)
float gyroZ_degPerSec = (gyroZ - gyroZ_zero) / gyroZ_sensitiviy;

尝试根据您的数据以

gyroZ_sensitivity = 132.5

作为起始猜测。

我认为您需要将传感器精确旋转 360 度来校准传感器，以确定每个传感器（以及每个轴）的真实值。然后保存这些新的实际校准数字，这样您就不必每次开机时都进行校准。公平地说，360 度 +/-1% 的误差其实一点也不坏。许多电子元件具有类似的工作公差。