【花雕学编程】Arduino BLDC 之使用陀螺仪进行姿态监测

《Arduino 手册（思路与案例）》栏目介绍：
在电子制作与智能控制的应用领域，本栏目涵盖了丰富的内容，包括但不限于以下主题：Arduino BLDC、Arduino CNC、Arduino E-Ink、Arduino ESP32 SPP、Arduino FreeRTOS、Arduino FOC、Arduino GRBL、Arduino HTTP、Arduino HUB75、Arduino IoT Cloud、Arduino JSON、Arduino LCD、Arduino OLED、Arduino LVGL、Arduino PID、Arduino TFT，以及Arduino智能家居、智慧交通、月球基地、智慧校园和智慧农业等多个方面与领域。不仅探讨了这些技术的基础知识和应用领域，还提供了众多具体的参考案例，帮助读者更好地理解和运用Arduino平台进行创新项目。目前，本栏目已有近4000篇相关博客，旨在为广大电子爱好者和开发者提供全面的学习资源与实践指导。通过这些丰富的案例和思路，读者可以获取灵感，推动自己的创作与开发进程。
https://blog.youkuaiyun.com/weixin_41659040/category_12422453.html

Arduino BLDC之使用陀螺仪进行姿态监测
一、主要特点

1. 实时姿态监测：
   • 陀螺仪能够实时测量物体的角速度，通过积分计算，可以快速获得物体的姿态信息（如俯仰角、滚转角和偏航角），适合动态环境中对姿态的监测。
2. 高精度：
   • 陀螺仪通常具有较高的测量精度，可以在较小的角度变化下提供精准的数据，确保姿态控制的可靠性。
3. 小型化和轻量化：
   • 现代陀螺仪（如MEMS陀螺仪）体积小、重量轻，非常适合嵌入到电动平衡车、无人机等移动设备中。
4. 抗干扰能力：
   • 陀螺仪对环境的干扰（如光照、磁场变化）相对不敏感，能够在复杂环境中保持良好的工作性能。
5. 多传感器融合：
   • 陀螺仪通常与加速度计、磁力计等传感器进行组合使用，通过传感器融合技术（如卡尔曼滤波）进一步提高姿态估计的精度和稳定性。
   二、应用场景
6. 无人机：
   • 在无人机中，陀螺仪用于实时监测飞行姿态，确保稳定飞行和精准的姿态控制。
7. 电动平衡车：
   • 在电动平衡车中，陀螺仪用于检测车辆的倾斜角度，帮助实现动态平衡和稳定控制。
8. 机器人：
   • 在移动机器人中，陀螺仪用于监测机器人的姿态，以实现高效的导航和路径跟踪。
9. 虚拟现实（VR）设备：
   • 在VR头戴设备中，陀螺仪用于检测用户的头部运动，提供沉浸式的体验。
10. 汽车稳定控制系统：
    • 在汽车中，陀螺仪可用于监测车辆的动态姿态，支持电子稳定程序（ESP）等安全系统的实现。
    三、注意事项
11. 漂移问题：
    • 陀螺仪在长期使用中可能会出现漂移现象，导致姿态估计误差累积。需要通过定期校准或与其他传感器融合来减小漂移影响。
12. 传感器选择：
    • 不同类型的陀螺仪性能差异较大，选择合适的陀螺仪应根据具体应用需求（如精度、响应速度、成本等）进行评估。
13. 环境影响：
    • 虽然陀螺仪对环境干扰较不敏感，但在极端温度或高振动环境下，可能会影响其性能。需要考虑到使用环境的适应性。
14. 数据处理：
    • 陀螺仪输出的数据需要经过适当的滤波和处理，以消除噪声和误差，确保姿态监测的精确性。
15. 系统集成：
    • 在实际应用中，陀螺仪需要与其他传感器（如加速度计）进行有效集成，开发者需熟悉传感器融合算法，以获得最佳的姿态估计。

1、基本陀螺仪读取数据

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

void setup() {
  Serial.begin(9600); // 初始化串口
  Wire.begin(); // 初始化I2C
  mpu.initialize(); // 初始化MPU6050
}

void loop() {
  // 读取陀螺仪数据
  int16_t gyroX, gyroY, gyroZ;
  mpu.getRotation(&gyroX, &gyroY, &gyroZ);

  // 输出陀螺仪数据
  Serial.print("Gyro X: "); Serial.print(gyroX);
  Serial.print(" | Gyro Y: "); Serial.print(gyroY);
  Serial.print(" | Gyro Z: "); Serial.println(gyroZ);

  delay(500); // 延时以便于读取
}

要点解读：
基本数据读取：通过MPU6050陀螺仪获取三个轴的旋转速度，适合初学者理解陀螺仪的基本用法。
实时输出：每次循环读取并输出陀螺仪数据，便于监控和调试。
简单易懂：代码结构清晰，便于扩展和修改。

2、陀螺仪数据滤波与姿态估计

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;
double angleX = 0, angleY = 0; // 姿态角度
unsigned long lastTime = 0;

void setup() {
  Serial.begin(9600); // 初始化串口
  Wire.begin(); // 初始化I2C
  mpu.initialize(); // 初始化MPU6050
  lastTime = millis();
}

void loop() {
  // 读取陀螺仪数据
  int16_t gyroX, gyroY, gyroZ;
  mpu.getRotation(&gyroX, &gyroY, &gyroZ);

  // 计算时间间隔
  unsigned long currentTime = millis();
  double dt = (currentTime - lastTime) / 1000.0; // 转换为秒
  lastTime = currentTime;

  // 简单积分计算姿态
  angleX += gyroX * dt; // 计算X轴角度
  angleY += gyroY * dt; // 计算Y轴角度

  // 输出姿态角度
  Serial.print("Angle X: "); Serial.print(angleX);
  Serial.print(" | Angle Y: "); Serial.println(angleY);

  delay(500); // 延时以便于读取
}

要点解读：
姿态估计：通过积分陀螺仪的旋转速度计算出姿态角度，适合对姿态监测有初步需求的应用。
动态更新：实时更新姿态角度，便于后续应用，如平衡控制。
简单滤波逻辑：基础的姿态计算方法，适合学习和实验。

3、陀螺仪与加速度计融合姿态监测

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;
double angleX = 0, angleY = 0;
double accX, accY, accZ;

void setup() {
  Serial.begin(9600); // 初始化串口
  Wire.begin(); // 初始化I2C
  mpu.initialize(); // 初始化MPU6050
}

void loop() {
  // 读取陀螺仪数据
  int16_t gyroX, gyroY, gyroZ;
  mpu.getRotation(&gyroX, &gyroY, &gyroZ);

  // 读取加速度计数据
  mpu.getAcceleration(&accX, &accY, &accZ);

  // 计算当前姿态角度
  double accelAngleX = atan2(accY, accZ) * 180 / PI; // 计算X轴姿态
  double accelAngleY = atan2(accX, accZ) * 180 / PI; // 计算Y轴姿态

  // 融合陀螺仪与加速度计数据
  angleX = 0.98 * (angleX + gyroX * 0.01) + 0.02 * accelAngleX; // 融合计算
  angleY = 0.98 * (angleY + gyroY * 0.01) + 0.02 * accelAngleY;

  // 输出姿态角度
  Serial.print("Angle X: "); Serial.print(angleX);
  Serial.print(" | Angle Y: "); Serial.println(angleY);

  delay(500); // 延时以便于读取
}

要点解读：
数据融合：结合陀螺仪与加速度计的数据，采用加权平均的方法实现更精确的姿态估计，适合对精度要求较高的应用。
实时计算：通过实时读取并计算姿态角度，增强了系统的稳定性和响应速度。
适合复杂应用：适合需要高精度姿态监测的设备，如无人机和机器人。

4、基于MPU6050的无人机姿态监测与电机控制

#include <Wire.h>
#include <MPU6050.h>
#include <Servo.h>
 
MPU6050 mpu;
Servo motor1, motor2, motor3, motor4; // 四个电机
 
float pitch, roll, yaw; // 欧拉角
float kp = 0.5, ki = 0.01, kd = 0.1; // PID参数
float error_prev = 0, integral = 0; // PID变量
 
void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();
  motor1.attach(9); motor2.attach(10); motor3.attach(11); motor4.attach(12);
}
 
void loop() {
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
 
  // 互补滤波计算欧拉角（简化版）
  float acc_pitch = atan2(-ay, az) * 180 / PI;
  float acc_roll = atan2(ax, az) * 180 / PI;
  float gyro_pitch = gx / 131.0; // 陀螺仪角速度（°/s）
  float gyro_roll = gy / 131.0;
  pitch = 0.98 * (pitch + gyro_pitch * 0.01) + 0.02 * acc_pitch; // 10ms采样周期
  roll = 0.98 * (roll + gyro_roll * 0.01) + 0.02 * acc_roll;
 
  // PID控制（以俯仰为例）
  float error = 0 - pitch; // 目标角度为0°
  integral += error * 0.01;
  float derivative = (error - error_prev) / 0.01;
  float output = kp * error + ki * integral + kd * derivative;
  error_prev = error;
 
  // 电机调速（对角电机反向调整）
  int base_speed = 1000; // 基础转速（PWM值）
  motor1.writeMicroseconds(base_speed + output); // 前左
  motor3.writeMicroseconds(base_speed - output); // 后右
  motor2.writeMicroseconds(base_speed); // 前右（简化模型）
  motor4.writeMicroseconds(base_speed); // 后左
 
  delay(10);
}

要点解读：
传感器融合：结合加速度计（静态稳定）和陀螺仪（动态响应）数据，通过互补滤波降低噪声。
PID控制：通过比例-积分-微分算法计算电机调整量，实现快速响应且无超调。
电机控制：对角电机反向调整以抵消倾斜力矩，维持无人机平衡。

5、机器人平衡车姿态监测与自平衡

#include <Wire.h>
#include <MPU6050.h>
#include <SimpleKalmanFilter.h> // 卡尔曼滤波库
 
MPU6050 mpu;
SimpleKalmanFilter pitch_filter(1, 1, 0.1); // 卡尔曼滤波参数（状态估计、测量噪声、过程噪声）
 
float target_angle = 0.0; // 目标角度（直立）
float current_angle = 0.0;
float motor_power = 0;
 
void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();
  pinMode(5, OUTPUT); // BLDC电机PWM引脚
  pinMode(6, OUTPUT);
}
 
void loop() {
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
 
  // 卡尔曼滤波计算俯仰角
  float acc_angle = atan2(-ay, az) * 180 / PI;
  float gyro_rate = gx / 131.0;
  current_angle = pitch_filter.updateEstimate(acc_angle + gyro_rate * 0.01);
 
  // PD控制（简化版，省略积分项）
  float error = target_angle - current_angle;
  float derivative = -gyro_rate; // 直接使用陀螺仪角速度作为微分项
  motor_power = 2.0 * error + 0.5 * derivative; // PD参数需根据实际调试
 
  // 电机控制（H桥驱动需额外电路）
  analogWrite(5, constrain(100 + motor_power, 0, 255)); // 左电机
  analogWrite(6, constrain(100 - motor_power, 0, 255)); // 右电机（反向）
 
  delay(10);
}

要点解读：

卡尔曼滤波：相比互补滤波，卡尔曼滤波能更优地处理动态噪声，适合高精度平衡控制。
PD控制：省略积分项（I）以避免电机长时间运行导致的积分饱和问题。
电机驱动：需配合H桥电路实现电机正反转，案例中简化为PWM差分控制。

6、机械臂关节姿态监测与闭环控制

#include <Wire.h>
#include <MPU6050.h>
#include <PID_v1.h> // PID库
 
MPU6050 mpu;
double setpoint = 45.0; // 目标角度（45度）
double input, output;
PID myPID(&input, &output, &setpoint, 2.0, 0.5, 1.0, DIRECT); // PID参数（Kp, Ki, Kd）
 
void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(-255, 255); // 限制电机PWM输出范围
}
 
void loop() {
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
 
  // 计算当前角度（简化版，仅用加速度计）
  input = atan2(-ay, az) * 180 / PI;
 
  // PID计算
  myPID.Compute();
 
  // 电机控制（需配合编码器实现闭环）
  if (output > 0) {
    analogWrite(9, output); // 正转
    digitalWrite(8, HIGH); // 方向引脚（需根据电机驱动模块调整）
  } else {
    analogWrite(9, -output); // 反转
    digitalWrite(8, LOW);
  }
 
  delay(10);
}

要点解读：
PID库应用：使用现成库简化PID算法实现，需根据实际响应调整参数。
简化角度计算：案例中仅用加速度计计算角度，实际应用需融合陀螺仪数据以提高动态精度。
闭环控制：需配合编码器反馈电机实际位置，形成双闭环（角度+位置）控制。

注意，以上案例只是为了拓展思路，仅供参考。它们可能有错误、不适用或者无法编译。您的硬件平台、使用场景和Arduino版本可能影响使用方法的选择。实际编程时，您要根据自己的硬件配置、使用场景和具体需求进行调整，并多次实际测试。您还要正确连接硬件，了解所用传感器和设备的规范和特性。涉及硬件操作的代码，您要在使用前确认引脚和电平等参数的正确性和安全性。