零知开源——ESP8266+MPU6050 实现运动姿态检测

零知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

本人才疏学浅，有错误或遗漏的部分欢迎大家探讨学习！

审核编辑 黄宇