零知ESP8266的I2C通信

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

然而，MPU6050沒有磁力計,直接使用陀螺儀的角速度積分計算yaw角（航向角）會導(dǎo)致累積漂移。本次實(shí)驗(yàn)采用優(yōu)化后的互補(bǔ)濾波，減少漂移，提高yaw角的計算精度。

一、硬件連接

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

1.所需材料：

零知ESP8266

MPU6050姿態(tài)檢測傳感器

跳線

2.硬件連接示意圖：

二、代碼實(shí)現(xiàn)

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;  // 記錄上一幀的時間

// 校準(zhǔn)值
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

設(shè)置ESP8266I2C端口SDA、SCL
初始化MPU6050并進(jìn)行連接測試
校準(zhǔn)傳感器，減少偏差
設(shè)置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");
    }

    // 傳感器校準(zhǔn)
    calibrateSensors();

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

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

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

3.獲取MPU6050數(shù)據(jù)

獲取加速度計&陀螺儀原始數(shù)據(jù)
減去偏移量，提高數(shù)據(jù)精度
歸一化數(shù)據(jù)，提高計算穩(wěn)定性
調(diào)用complementaryFilter()計算姿態(tài)角
串口打印姿態(tài)角數(shù)據(jù)

void loop() {
    // 獲取原始數(shù)據(jù)
    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;

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

    // 計算姿態(tài)角
    complementaryFilter();

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

    delay(10);
}

4.傳感器校準(zhǔn)

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

// 傳感器校準(zhǔn)
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.互補(bǔ)濾波計算姿態(tài)角

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

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

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

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

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

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

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

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;  // 記錄上一幀的時間

// 校準(zhǔn)值
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");
    }

    // 傳感器校準(zhǔn)
    calibrateSensors();

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

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

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

void loop() {
    // 獲取原始數(shù)據(jù)
    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;

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

    // 計算姿態(tài)角
    complementaryFilter();

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

    delay(10);
}

// 傳感器校準(zhǔn)
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;
}

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

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

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

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

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

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

三、實(shí)驗(yàn)結(jié)果

點(diǎn)擊零知開源平臺調(diào)試按鈕，打開零知開源平臺的串口監(jiān)視器，設(shè)置波特率為9600，觀察串口打印測量到的MPU6050姿態(tài)角。

使用vofa+上位機(jī)效果：

VOFA+上位機(jī)獲取MPU6050運(yùn)動姿態(tài)

vofa+上位機(jī)獲取姿態(tài)解算數(shù)據(jù)視頻：

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

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

審核編輯 黃宇