本帖最后由 lingzhiLab 于 2025-2-20 18:42 编辑
#申请原创#
零知ESP8266学习教程在运动姿态检测、机器人平衡控制、VR头戴设备等应用中，MPU6050（三轴加速度计+三轴陀螺仪）是一个常见的姿态传感器。而ESP8266作为一款低功耗Wi-Fi模块，可以实现数据无线传输，将姿态数据上传至服务器或云端，便于实时监测。
然而，MPU6050 没有磁力计,直接使用陀螺仪的角速度积分计算yaw角（航向角）会导致累积漂移。本次实验采用优化后的互补滤波，减少漂移，提高yaw角的计算精度。
一、硬件连接MPU6050模块采用I2C通信连接到零知ESP8266开发板 1.所需材料：零知ESP8266 MPU6050姿态检测传感器 跳线
2.硬件连接示意图：零知ESP8266 | MPU6050 | 3.3V | VCC | GND | GND | SCL | SCL | SDA | SDA |
二、代码实现
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
设置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.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+上位机效果：
本人才疏学浅，有错误或遗漏的部分欢迎大家探讨学习！
|
楼主
标题置顶
标题高亮
