零知ESP8266的I2C通信

在運動姿態檢測、機器人平衡控制、VR頭戴設備等應用中，MPU6050（三軸加速度計+三軸陀螺儀）是一個常見的姿態傳感器。而ESP8266作為一款低功耗Wi-Fi模塊，可以實現數據無線傳輸，將姿態數據上傳至服務器或云端，便于實時監測。

然而，MPU6050沒有磁力計,直接使用陀螺儀的角速度積分計算yaw角（航向角）會導致累積漂移。本次實驗采用優化后的互補濾波，減少漂移，提高yaw角的計算精度。

一、硬件連接

MPU6050模塊采用I2C通信連接到零知ESP8266開發板

1.所需材料：

零知ESP8266

MPU6050姿態檢測傳感器

跳線

2.硬件連接示意圖：

二、代碼實現

1.頭文件及變量定義

通過MPU6050庫與傳感器交互

使用yaw_integral變量累積航向角
previousTime變量用于計算時間間隔dt

#include "MPU6050.h"

MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;

float nax, nay, naz;
float ngx, ngy, ngz;

float roll, pitch, yaw;
float yaw_integral = 0.0f;  // 累積 yaw 角
unsigned long previousTime = 0;  // 記錄上一幀的時間

// 校準值
int16_t ax_offset = 0, ay_offset = 0, az_offset = 0;
int16_t gx_offset = 0, gy_offset = 0, gz_offset = 0;

#define LED_PIN LED_BUILTIN

2.初始化MPU6050

設置ESP8266I2C端口SDA、SCL
初始化MPU6050并進行連接測試
校準傳感器，減少偏差
設置50Hz采樣率和±2000°/s陀螺儀量程

void setup() {
    Serial.begin(9600);

    // MPU6050 初始化
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();

    // 檢測 MPU6050 是否連接成功
    Serial.println("Testing device connections...");
    if (accelgyro.testConnection()) {
        Serial.println("MPU6050 connection successful");
    } else {
        Serial.println("MPU6050 connection failed");
    }

    // 傳感器校準
    calibrateSensors();

    // 設置 MPU6050 的采樣率和陀螺儀的量程
    accelgyro.setRate(50);  // 采樣率 50Hz
    accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);  // 陀螺儀量程 ±2000°/s

    // LED 指示燈
    pinMode(LED_PIN, OUTPUT);

    // 記錄起始時間
    previousTime = millis();
}

3.獲取MPU6050數據

獲取加速度計&陀螺儀原始數據
減去偏移量，提高數據精度
歸一化數據，提高計算穩定性
調用complementaryFilter()計算姿態角
串口打印姿態角數據

void loop() {
    // 獲取原始數據
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    // 減去偏移量
    ax -= ax_offset;
    ay -= ay_offset;
    az -= az_offset;
    gx -= gx_offset;
    gy -= gy_offset;
    gz -= gz_offset;

    // 讀取歸一化數據
    accelgyro.readNormalizeAccel(&nax, &nay, &naz);
    accelgyro.readNormalizeGyro(&ngx, &ngy, &ngz);

    // 計算姿態角
    complementaryFilter();

    // 打印姿態角
    Serial.print("Roll: ");
    Serial.print(roll);
    Serial.print(" Pitch: ");
    Serial.print(pitch);
    Serial.print(" Yaw: ");
    Serial.println(yaw);

    delay(10);
}

4.傳感器校準

采集100組數據，計算平均值作為偏移量
過濾MPU6050啟動時的零偏誤差
減少環境噪聲對傳感器的影響

// 傳感器校準
void calibrateSensors() {
    int num_readings = 100;
    for (int i = 0; i < num_readings; i++) {
        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        ax_offset += ax;
        ay_offset += ay;
        az_offset += az;
        gx_offset += gx;
        gy_offset += gy;
        gz_offset += gz;
        delay(50);
    }
    ax_offset /= num_readings;
    ay_offset /= num_readings;
    az_offset /= num_readings;
    gx_offset /= num_readings;
    gy_offset /= num_readings;
    gz_offset /= num_readings;
}

5.互補濾波計算姿態角

計算dt（時間間隔），用于陀螺儀積分計算yaw

roll和pitch采用加速度計計算：
yaw采用陀螺儀積分計算并限制范圍[-180,180]
yaw=0.98*yaw_integral+0.02*yaw進行互補濾波，減少漂移

// 互補濾波計算姿態角
void complementaryFilter() {
    // 計算時間間隔 dt（單位：秒）
    unsigned long currentTime = millis();
    float dt = (currentTime - previousTime) / 1000.0;  // ms 轉換為 s
    previousTime = currentTime;

    // 計算 Roll 和 Pitch
    roll = atan2(nay, naz) * 180 / M_PI;
    pitch = atan2(-nax, sqrt(nay * nay + naz * naz)) * 180 / M_PI;

    // 陀螺儀角速度轉換
    float gyroYawRate = ngz;  // 直接使用歸一化后的 ngz（角速度 deg/s）

    // 計算 Yaw (積分計算)
    yaw_integral += gyroYawRate * dt;  // 積分計算 yaw
    yaw_integral = fmod(yaw_integral + 180, 360) - 180;  // 限制 yaw 在 [-180, 180] 之間

    // 互補濾波減少漂移影響
    yaw = 0.98 * yaw_integral + 0.02 * yaw;  // 0.98 和 0.02 為濾波系數
}

6.完整代碼

#include "MPU6050.h"

MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;

float nax, nay, naz;
float ngx, ngy, ngz;

float roll, pitch, yaw;
float yaw_integral = 0.0f;  // 累積 yaw 角
unsigned long previousTime = 0;  // 記錄上一幀的時間

// 校準值
int16_t ax_offset = 0, ay_offset = 0, az_offset = 0;
int16_t gx_offset = 0, gy_offset = 0, gz_offset = 0;

#define LED_PIN LED_BUILTIN

void setup() {
    Serial.begin(9600);

    // MPU6050 初始化
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();

    // 檢測 MPU6050 是否連接成功
    Serial.println("Testing device connections...");
    if (accelgyro.testConnection()) {
        Serial.println("MPU6050 connection successful");
    } else {
        Serial.println("MPU6050 connection failed");
    }

    // 傳感器校準
    calibrateSensors();

    // 設置 MPU6050 的采樣率和陀螺儀的量程
    accelgyro.setRate(50);  // 采樣率 50Hz
    accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);  // 陀螺儀量程 ±2000°/s

    // LED 指示燈
    pinMode(LED_PIN, OUTPUT);

    // 記錄起始時間
    previousTime = millis();
}

void loop() {
    // 獲取原始數據
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    // 減去偏移量
    ax -= ax_offset;
    ay -= ay_offset;
    az -= az_offset;
    gx -= gx_offset;
    gy -= gy_offset;
    gz -= gz_offset;

    // 讀取歸一化數據
    accelgyro.readNormalizeAccel(&nax, &nay, &naz);
    accelgyro.readNormalizeGyro(&ngx, &ngy, &ngz);

    // 計算姿態角
    complementaryFilter();

    // 打印姿態角
    Serial.print("Roll: ");
    Serial.print(roll);
    Serial.print(" Pitch: ");
    Serial.print(pitch);
    Serial.print(" Yaw: ");
    Serial.println(yaw);

    delay(10);
}

// 傳感器校準
void calibrateSensors() {
    int num_readings = 100;
    for (int i = 0; i < num_readings; i++) {
        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        ax_offset += ax;
        ay_offset += ay;
        az_offset += az;
        gx_offset += gx;
        gy_offset += gy;
        gz_offset += gz;
        delay(50);
    }
    ax_offset /= num_readings;
    ay_offset /= num_readings;
    az_offset /= num_readings;
    gx_offset /= num_readings;
    gy_offset /= num_readings;
    gz_offset /= num_readings;
}

// 互補濾波計算姿態角
void complementaryFilter() {
    // 計算時間間隔 dt（單位：秒）
    unsigned long currentTime = millis();
    float dt = (currentTime - previousTime) / 1000.0;  // ms 轉換為 s
    previousTime = currentTime;

    // 計算 Roll 和 Pitch
    roll = atan2(nay, naz) * 180 / M_PI;
    pitch = atan2(-nax, sqrt(nay * nay + naz * naz)) * 180 / M_PI;

    // 陀螺儀角速度轉換
    float gyroYawRate = ngz;  // 直接使用歸一化后的 ngz（角速度 deg/s）

    // 計算 Yaw (積分計算)
    yaw_integral += gyroYawRate * dt;  // 積分計算 yaw
    yaw_integral = fmod(yaw_integral + 180, 360) - 180;  // 限制 yaw 在 [-180, 180] 之間

    // 互補濾波減少漂移影響
    yaw = 0.98 * yaw_integral + 0.02 * yaw;  // 0.98 和 0.02 為濾波系數
}

將上述代碼移植到零知開源平臺，選擇連接的端口編譯并上傳到零知ESP8266。

三、實驗結果

點擊零知開源平臺調試按鈕，打開零知開源平臺的串口監視器，設置波特率為9600，觀察串口打印測量到的MPU6050姿態角。

使用vofa+上位機效果：

VOFA+上位機獲取MPU6050運動姿態

vofa+上位機獲取姿態解算數據視頻：

https://www.bilibili.com/video/BV1HiAPe1Etj?share_source=copy_web

本人才疏學淺，有錯誤或遺漏的部分歡迎大家探討學習！

審核編輯 黃宇