🌀 ESP32 + MPU6050 惯性导航：从获取数据到自平衡机器人

MPU6050  →  ESP32-S3
VCC       →  3.3V（或 5V 也可以）
GND       →  GND
SCL       →  GPIO 1（I2C0_SCL）
SDA       →  GPIO 0（I2C0_SDA）

⚠️ ESP32-S3 默认 I2C 引脚：
  GPIO 0 = SDA
  GPIO 1 = SCL

// Wire.h 由 ESP32 Arduino 核心提供，I2C 通信 #include <Wire.h>const int MPU_ADDR = 0x68; // AD0 接地 => 0x68；接 VCC => 0x69void setup() { Serial.begin(115200); Wire.begin(0, 1); // ESP32-S3: SDA=GPIO0, SCL=GPIO1 Wire.setClock(400000); // 400kHz I2C 高速模式// 唤醒 MPU6050（默认睡眠模式） Wire.beginTransmission(MPU_ADDR); Wire.write(0x6B); // PWR_MGMT_1 寄存器 Wire.write(0x00); // 唤醒，解除睡眠 Wire.endTransmission(); } void loop() { Wire.beginTransmission(MPU_ADDR); Wire.write(0x3B); // ACCEL_XOUT_H — 加速度数据起始寄存器 Wire.endTransmission(); Wire.requestFrom(MPU_ADDR, 14); // 一次读 14 字节if (Wire.available() == 14) { int ax = Wire.read() << 8 | Wire.read(); int ay = Wire.read() << 8 | Wire.read(); int az = Wire.read() << 8 | Wire.read(); int tmp = Wire.read() << 8 | Wire.read(); int gx = Wire.read() << 8 | Wire.read(); int gy = Wire.read() << 8 | Wire.read(); int gz = Wire.read() << 8 | Wire.read(); // 除以 16384 得到 g（加速度计默认 ±2g 量程）// 除以 131 得到 °/s（陀螺仪默认 ±250°/s 量程） Serial.printf("Acc: %7.2f %7.2f %7.2f | Gyr: %7.1f %7.1f %7.1f\n", ax/16384.0, ay/16384.0, az/16384.0, gx/131.0, gy/131.0, gz/131.0); } delay(20); }

// 卡尔曼滤波器 — 单轴（俯仰角） class KalmanFilter { public: KalmanFilter(float q, float r) : Q(q), R(r), angle(0), bias(0), P(1) {} float update(float accAngle, float gyroRate, float dt) { // 预测步骤 angle += (gyroRate - bias) * dt; P += Q * dt; // 更新步骤float S = P + R; float K = P / S; float y = accAngle - angle; angle += K * y; bias += K * bias; P = (1 - K) * P; return angle; } float getAngle() { return angle; } private: float Q, R, angle, bias, P; };

#include <Wire.h> #include <servo.h>#define SERVO_L_PIN 2 #define SERVO_R_PIN 3 #define MPU_ADDR 0x68 #define SERVO_MIN 40 #define SERVO_MAX 115 #define BALANCE_ANG 0 Servo servoL, servoR; KalmanFilter kf(0.001, 0.1); // PID 参数（需根据实际重心机械调参） float Kp = 50, Ki = 0.1, Kd = 10; float integral = 0, prevAngle = 0; void readMPU6050(float& ax, float& ay, float& az, float& gx, float& gy, float& gz) { Wire.beginTransmission(MPU_ADDR); Wire.write(0x3B); Wire.endTransmission(); Wire.requestFrom(MPU_ADDR, 14); if (Wire.available() == 14) { ax = (Wire.read() << 8 | Wire.read()) / 16384.0; ay = (Wire.read() << 8 | Wire.read()) / 16384.0; az = (Wire.read() << 8 | Wire.read()) / 16384.0; Wire.read(); Wire.read(); gx = (Wire.read() << 8 | Wire.read()) / 131.0; gy = (Wire.read() << 8 | Wire.read()) / 131.0; gz = (Wire.read() << 8 | Wire.read()) / 131.0; } } float calcAngle(float ax, float az) { return atan2(ax, az) * 180.0 / 3.1415926; } void setup() { Serial.begin(115200); Wire.begin(0, 1); Wire.setClock(400000); Wire.beginTransmission(MPU_ADDR); Wire.write(0x6B); Wire.write(0x00); Wire.endTransmission(); servoL.attach(SERVO_L_PIN, SERVO_MIN, SERVO_MAX); servoR.attach(SERVO_R_PIN, SERVO_MIN, SERVO_MAX); } void loop() { static unsigned long lastT = 0; float ax, ay, az, gx, gy, gz; readMPU6050(ax, ay, az, gx, gy, gz); float accAngle = calcAngle(ax, az); float dt = (millis() - lastT) / 1000.0; lastT = millis(); float angle = kf.update(accAngle, gy, dt); float error = angle - BALANCE_ANG; integral += error * dt; integral = constrain(integral, -50, 50); float output = Kp * error + Ki * integral + Kd * (error - prevAngle) / dt; prevAngle = error; int pwm = map(constrain(output, -400, 400), -400, 400, SERVO_MIN, SERVO_MAX); servoL.writeMicroseconds(pwm); servoR.writeMicroseconds(pwm); Serial.printf("%.2f,%.2f,%.1f\n", angle, accAngle, output); delay(5); }

void calibrateMPU6050(float& gyroXBias, float& gyroYBias, float& gyroZBias, float& accXBias, float& accYBias, float& accZBias) { const int N = 200; float sumGX = 0, sumGY = 0, sumGZ = 0; float sumAX = 0, sumAY = 0, sumAZ = 0; Serial.println("Calibrating MPU6050 — keep sensor still..."); for (int i = 0; i < N; i++) { float ax, ay, az, gx, gy, gz; readMPU6050(ax, ay, az, gx, gy, gz); sumGX += gx; sumGY += gy; sumGZ += gz; sumAX += ax; sumAY += ay; sumAZ += az; delay(10); } gyroXBias = sumGX / N; gyroYBias = sumGY / N; gyroZBias = sumGZ / N; accXBias = sumAX / N; accYBias = sumAY / N; accZBias = (sumAZ / N) - 1.0; Serial.printf("Calibration done: gyro(%.2f, %.2f, %.2f) acc(%.4f, %.4f, %.4f)\n", gyroXBias, gyroYBias, gyroZBias, accXBias, accYBias, accZBias); }