ESP8266+MPU6050 实现运动姿态检测

ESP8266+MPU6050 实现运动姿态检测

运动姿态检测在机器人平衡控制、VR头戴设备等领域有着广泛的应用。本文将介绍如何使用ESP8266和MPU6050实现运动姿态检测，并通过优化的互补滤波算法减少航向角的累积漂移，提高检测精度。

硬件连接

MPU6050模块通过I2C通信连接到ESP8266开发板。

所需材料：

• ESP8266开发板
• MPU6050姿态检测传感器
• 跳线

硬件连接示意图：

代码实现

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

设置ESP8266 I2C端口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.98yaw_integral+0.02yaw进行互补滤波，减少漂移。

// 互补滤波计算姿态角
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+上位机获取姿态解算数据视频