تعادل خودکار با PID و IMU: آموزش پایه برای ربات‌های متوازن و UGV (فارسی)

BOM

اتصال MPU-6050 به آردوینو

کد کامل: خواندن زاویه با فیلتر مکمل

#include <Wire.h>
#include <MPU6050.h>   // کتابخانه: Jeff Rowberg's MPU6050

MPU6050 mpu;

// ===== PID پارامترها =====
float Kp = 30.0;   // تناسبی — واکنش فوری
float Ki = 0.5;    // انتگرالی — خطای ماندگار
float Kd = 1.2;    // مشتقی — کاهش نوسان

float setpoint = 0.0;   // زاویه هدف (عمود = صفر درجه)
float input, output;
float errSum = 0, lastErr = 0;
unsigned long lastTime;

// ===== فیلتر مکمل =====
float angle = 0.0;
const float alpha = 0.98;  // ۹۸٪ ژیرو + ۲٪ شتاب‌سنج

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) Serial.println("MPU-6050 خطا!");
  lastTime = millis();
}

void loop() {
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // زاویه از شتاب‌سنج (درجه)
  float accelAngle = atan2(ay, az) * 180.0 / PI;

  // نرخ چرخش از ژیرو (درجه/ثانیه)
  float gyroRate = gx / 131.0;

  // فیلتر مکمل
  unsigned long now = millis();
  float dt = (now - lastTime) / 1000.0;
  lastTime = now;
  angle = alpha * (angle + gyroRate * dt) + (1 - alpha) * accelAngle;

  // ===== محاسبه PID =====
  float err = setpoint - angle;
  errSum  += err * dt;
  errSum   = constrain(errSum, -50, 50);   // Anti-windup
  float dErr = (err - lastErr) / dt;
  output = Kp*err + Ki*errSum + Kd*dErr;
  output = constrain(output, -255, 255);
  lastErr = err;

  Serial.print("Angle: "); Serial.print(angle, 2);
  Serial.print("  PID out: "); Serial.println(output, 1);
  delay(10);
}

راهنمای تنظیم Kp/Ki/Kd

روش Ziegler–Nichols ساده: ابتدا Ki=0, Kd=0 بگذار. Kp را آرام بالا ببر تا ربات شروع به نوسان با دامنه ثابت کند. آن را Ku بنام. سپس: Kp = 0.6×Ku، Ki = 1.2×Ku/Tu، Kd = 0.075×Ku×Tu (Tu = دوره تناوب نوسان).

چک‌لیست

• کتابخانه MPU-6050 Jeff Rowberg — GitHub
• آپارات: آموزش فارسی PID آردوینو