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

eefocus_4087784

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

1. ​
   
2. #include "MPU6050.h"
   
3. 
   
4. MPU6050 accelgyro;
   
5. 
   
6. int16_t ax, ay, az;
   
7. int16_t gx, gy, gz;
   
8. 
   
9. float nax, nay, naz;
   
10. float ngx, ngy, ngz;
    
11. 
    
12. float roll, pitch, yaw;
    
13. float yaw_integral = 0.0f;  // 累积 yaw 角
    
14. unsigned long previousTime = 0;  // 记录上一帧的时间
    
15. 
    
16. // 校准值
    
17. int16_t ax_offset = 0, ay_offset = 0, az_offset = 0;
    
18. int16_t gx_offset = 0, gy_offset = 0, gz_offset = 0;
    
19. 
    
20. #define LED_PIN LED_BUILTIN

复制代码

2.初始化MPU6050

设置ESP8266 I2C端口SDA、SCL
初始化MPU6050并进行连接测试
校准传感器，减少偏差
设置50Hz采样率和±2000°/s陀螺仪量程

1. ​
   
2. void setup() {
   
3.     Serial.begin(9600);
   
4. 
   
5.     // MPU6050 初始化
   
6.     Serial.println("Initializing I2C devices...");
   
7.     accelgyro.initialize();
   
8. 
   
9.     // 检测 MPU6050 是否连接成功
   
10.     Serial.println("Testing device connections...");
    
11.     if (accelgyro.testConnection()) {
    
12.         Serial.println("MPU6050 connection successful");
    
13.     } else {
    
14.         Serial.println("MPU6050 connection failed");
    
15.     }
    
16. 
    
17.     // 传感器校准
    
18.     calibrateSensors();
    
19. 
    
20.     // 设置 MPU6050 的采样率和陀螺仪的量程
    
21.     accelgyro.setRate(50);  // 采样率 50Hz
    
22.     accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);  // 陀螺仪量程 ±2000°/s
    
23. 
    
24.     // LED 指示灯
    
25.     pinMode(LED_PIN, OUTPUT);
    
26. 
    
27.     // 记录起始时间
    
28.     previousTime = millis();
    
29. }

复制代码

3.获取MPU6050数据

获取加速度计&陀螺仪原始数据
减去偏移量，提高数据精度
归一化数据，提高计算稳定性
调用complementaryFilter()计算姿态角
串口打印姿态角数据

1. ​
   
2. void loop() {
   
3.     // 获取原始数据
   
4.     accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
   
5. 
   
6.     // 减去偏移量
   
7.     ax -= ax_offset;
   
8.     ay -= ay_offset;
   
9.     az -= az_offset;
   
10.     gx -= gx_offset;
    
11.     gy -= gy_offset;
    
12.     gz -= gz_offset;
    
13. 
    
14.     // 读取归一化数据
    
15.     accelgyro.readNormalizeAccel(&nax, &nay, &naz);
    
16.     accelgyro.readNormalizeGyro(&ngx, &ngy, &ngz);
    
17. 
    
18.     // 计算姿态角
    
19.     complementaryFilter();
    
20. 
    
21.     // 打印姿态角
    
22.     Serial.print("Roll: ");
    
23.     Serial.print(roll);
    
24.     Serial.print(" Pitch: ");
    
25.     Serial.print(pitch);
    
26.     Serial.print(" Yaw: ");
    
27.     Serial.println(yaw);
    
28. 
    
29.     delay(10);
    
30. }

复制代码

4.传感器校准

采集100组数据，计算平均值作为偏移量
过滤MPU6050启动时的零偏误差
减少环境噪声对传感器的影响

1. ​
   
2. // 传感器校准
   
3. void calibrateSensors() {
   
4.     int num_readings = 100;
   
5.     for (int i = 0; i < num_readings; i++) {
   
6.         accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
   
7.         ax_offset += ax;
   
8.         ay_offset += ay;
   
9.         az_offset += az;
   
10.         gx_offset += gx;
    
11.         gy_offset += gy;
    
12.         gz_offset += gz;
    
13.         delay(50);
    
14.     }
    
15.     ax_offset /= num_readings;
    
16.     ay_offset /= num_readings;
    
17.     az_offset /= num_readings;
    
18.     gx_offset /= num_readings;
    
19.     gy_offset /= num_readings;
    
20.     gz_offset /= num_readings;
    
21. }

复制代码

5.互补滤波计算姿态角

计算dt（时间间隔），用于陀螺仪积分计算 yaw

roll和 pitch 采用加速度计计算：
yaw采用 陀螺仪积分计算并限制范围 [-180, 180]
yaw=0.98*yaw_integral+0.02*yaw进行互补滤波，减少漂移

1. ​
   
2. // 互补滤波计算姿态角
   
3. void complementaryFilter() {
   
4.     // 计算时间间隔 dt（单位：秒）
   
5.     unsigned long currentTime = millis();
   
6.     float dt = (currentTime - previousTime) / 1000.0;  // ms 转换为 s
   
7.     previousTime = currentTime;
   
8. 
   
9.     // 计算 Roll 和 Pitch
   
10.     roll = atan2(nay, naz) * 180 / M_PI;
    
11.     pitch = atan2(-nax, sqrt(nay * nay + naz * naz)) * 180 / M_PI;
    
12. 
    
13.     // 陀螺仪角速度转换
    
14.     float gyroYawRate = ngz;  // 直接使用归一化后的 ngz（角速度 deg/s）
    
15. 
    
16.     // 计算 Yaw (积分计算)
    
17.     yaw_integral += gyroYawRate * dt;  // 积分计算 yaw
    
18.     yaw_integral = fmod(yaw_integral + 180, 360) - 180;  // 限制 yaw 在 [-180, 180] 之间
    
19. 
    
20.     // 互补滤波减少漂移影响
    
21.     yaw = 0.98 * yaw_integral + 0.02 * yaw;  // 0.98 和 0.02 为滤波系数
    
22. }

复制代码

6.完整代码

1. ​
   
2. #include "MPU6050.h"
   
3. 
   
4. MPU6050 accelgyro;
   
5. 
   
6. int16_t ax, ay, az;
   
7. int16_t gx, gy, gz;
   
8. 
   
9. float nax, nay, naz;
   
10. float ngx, ngy, ngz;
    
11. 
    
12. float roll, pitch, yaw;
    
13. float yaw_integral = 0.0f;  // 累积 yaw 角
    
14. unsigned long previousTime = 0;  // 记录上一帧的时间
    
15. 
    
16. // 校准值
    
17. int16_t ax_offset = 0, ay_offset = 0, az_offset = 0;
    
18. int16_t gx_offset = 0, gy_offset = 0, gz_offset = 0;
    
19. 
    
20. #define LED_PIN LED_BUILTIN
    
21. 
    
22. void setup() {
    
23.     Serial.begin(9600);
    
24. 
    
25.     // MPU6050 初始化
    
26.     Serial.println("Initializing I2C devices...");
    
27.     accelgyro.initialize();
    
28. 
    
29.     // 检测 MPU6050 是否连接成功
    
30.     Serial.println("Testing device connections...");
    
31.     if (accelgyro.testConnection()) {
    
32.         Serial.println("MPU6050 connection successful");
    
33.     } else {
    
34.         Serial.println("MPU6050 connection failed");
    
35.     }
    
36. 
    
37.     // 传感器校准
    
38.     calibrateSensors();
    
39. 
    
40.     // 设置 MPU6050 的采样率和陀螺仪的量程
    
41.     accelgyro.setRate(50);  // 采样率 50Hz
    
42.     accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);  // 陀螺仪量程 ±2000°/s
    
43. 
    
44.     // LED 指示灯
    
45.     pinMode(LED_PIN, OUTPUT);
    
46. 
    
47.     // 记录起始时间
    
48.     previousTime = millis();
    
49. }
    
50. 
    
51. void loop() {
    
52.     // 获取原始数据
    
53.     accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    
54. 
    
55.     // 减去偏移量
    
56.     ax -= ax_offset;
    
57.     ay -= ay_offset;
    
58.     az -= az_offset;
    
59.     gx -= gx_offset;
    
60.     gy -= gy_offset;
    
61.     gz -= gz_offset;
    
62. 
    
63.     // 读取归一化数据
    
64.     accelgyro.readNormalizeAccel(&nax, &nay, &naz);
    
65.     accelgyro.readNormalizeGyro(&ngx, &ngy, &ngz);
    
66. 
    
67.     // 计算姿态角
    
68.     complementaryFilter();
    
69. 
    
70.     // 打印姿态角
    
71.     Serial.print("Roll: ");
    
72.     Serial.print(roll);
    
73.     Serial.print(" Pitch: ");
    
74.     Serial.print(pitch);
    
75.     Serial.print(" Yaw: ");
    
76.     Serial.println(yaw);
    
77. 
    
78.     delay(10);
    
79. }
    
80. 
    
81. // 传感器校准
    
82. void calibrateSensors() {
    
83.     int num_readings = 100;
    
84.     for (int i = 0; i < num_readings; i++) {
    
85.         accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    
86.         ax_offset += ax;
    
87.         ay_offset += ay;
    
88.         az_offset += az;
    
89.         gx_offset += gx;
    
90.         gy_offset += gy;
    
91.         gz_offset += gz;
    
92.         delay(50);
    
93.     }
    
94.     ax_offset /= num_readings;
    
95.     ay_offset /= num_readings;
    
96.     az_offset /= num_readings;
    
97.     gx_offset /= num_readings;
    
98.     gy_offset /= num_readings;
    
99.     gz_offset /= num_readings;
    
100. }
     
101. 
     
102. // 互补滤波计算姿态角
     
103. void complementaryFilter() {
     
104.     // 计算时间间隔 dt（单位：秒）
     
105.     unsigned long currentTime = millis();
     
106.     float dt = (currentTime - previousTime) / 1000.0;  // ms 转换为 s
     
107.     previousTime = currentTime;
     
108. 
     
109.     // 计算 Roll 和 Pitch
     
110.     roll = atan2(nay, naz) * 180 / M_PI;
     
111.     pitch = atan2(-nax, sqrt(nay * nay + naz * naz)) * 180 / M_PI;
     
112. 
     
113.     // 陀螺仪角速度转换
     
114.     float gyroYawRate = ngz;  // 直接使用归一化后的 ngz（角速度 deg/s）
     
115. 
     
116.     // 计算 Yaw (积分计算)
     
117.     yaw_integral += gyroYawRate * dt;  // 积分计算 yaw
     
118.     yaw_integral = fmod(yaw_integral + 180, 360) - 180;  // 限制 yaw 在 [-180, 180] 之间
     
119. 
     
120.     // 互补滤波减少漂移影响
     
121.     yaw = 0.98 * yaw_integral + 0.02 * yaw;  // 0.98 和 0.02 为滤波系数
     
122. }

复制代码

   将上述代码移植到零知开源平台，选择连接的端口编译并上传到零知ESP8266。

三、实验结果

   点击零知开源平台调试按钮，打开零知开源平台的串口监视器，设置波特率为9600，观察串口打印测量到的MPU6050姿态角。

​

使用vofa+上位机效果：

VOFA+上位机获取MPU6050运动姿态

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