Belajar

Robot penyeimbang sendiri di sebuah Arduino

Mahir

Atau lihat semua pelajaran.

  • 526komponen di meja kerja
  • 46board sedang berjalan
  • 1.00xwaktu nyata, di semua board
Arduino Uno: robot penyeimbang sendirilive0.000 s 0.00x
Klik untuk membukanya di editor
Contoh Balance bot: MPU6050 melalui filter komplementer dan PID di tiga knob. Dorong dengan tombol panah.

Robot penyeimbang sendiri berdiri di dua roda, seperti pendulum terbalik, dan tetap tegak hanya karena terus menggerakkan roda itu ke bawah titik beratnya. Ini proyek Arduino klasik karena menggabungkan dua ide besar dalam satu build: mengubah sensor yang berisik menjadi sudut yang bagus, dan controller PID yang bertindak atasnya ratusan kali per detik.

Rangkaian di atas adalah Uno yang menjalankan robot dua roda dengan MPU6050 dan driver motor TB6612 sendiri. Tiga knob mengatur gain PID saat berjalan, dan OLED menampilkan sudut serta gain-nya. Tekan Jalankan: robot ditahan tegak sejenak, lalu dilepaskan, dan ia menyeimbangkan kembali. Lalu dorong dengan tombol panah di robot, dan putar knob untuk melihat efek tiap gain.

Yang kamu butuhkan

  • Arduino Uno
  • Chassis robot dua roda dengan motor DC bergigi
  • Akselerometer dan giroskop MPU6050
  • Driver motor TB6612FNG
  • Tiga potensiometer 10k
  • OLED SSD1306, 128x64, I2C (opsional)
  • Baterai untuk motor

Pengkabelan

Komponen dan pinDisambung keCatatan
SCL dan SDA robotPin 12 dan 2MPU6050 robot di 0x68, dengan software I2C sendiri
AIN1, AIN2, PWMAPin 10, 9 dan 6Motor kiri
BIN1, BIN2, PWMBPin 5, 4 dan 3Motor kanan
STBY5 VDriver menyala setiap kali ada daya
Knob Kp, Ki dan KdA0, A1 dan A2Ujung ke 5 V dan GND
SCL dan SDA OLEDA5 dan A4Sudut dan gain

Langkah 1: hitung kemiringannya

MPU6050 punya dua sensor, dan keduanya kurang bagus jika sendirian. Akselerometer tahu arah bawah, jadi atan2(-ax, az) memberi kemiringan dari gravitasi. Tapi ia juga merasakan setiap hentakan roda ke badan, jadi saat robot bergerak bacanya berisik dan salah. Giroskop mengukur seberapa cepat badan berputar, dengan halus, tapi kamu harus menjumlahkan laju itu untuk mendapat sudut, dan error kecil ikut menumpuk, jadi sudutnya bergeser.

Filter komplementer mengambil yang terbaik dari keduanya: angle = 0.98 * (angle + rate * dt) + 0.02 * accelAngle. Dari saat ke saat giroskop menggerakkan sudut, dan akselerometer menariknya perlahan kembali ke kebenaran, 2% setiap langkah. Ini cuma satu baris kode, dan inilah yang dipakai kebanyakan robot penyeimbang.

Langkah 2: PID pada sudut

Setiap 10 ms sketch menghitung seberapa jauh badan miring dari tegak dan menggerakkan roda untuk memperbaikinya. Outputnya adalah tiga suku yang dijumlahkan. P (proportional) adalah kemiringan dikali Kp: makin miring, makin kencang melaju. D (derivatif) adalah laju giroskop dikali Kd: ia menahan badan yang berayun cepat, supaya tidak kelewat. I (integral) menjumlahkan kemiringan seiring waktu dikali Ki: ia memberi dorongan tetap yang dibutuhkan motor hanya untuk menjaga satu kecepatan.

Jumlahnya dibatasi ke -255 sampai 255, dan tandanya menentukan arah (AIN1 dan AIN2) sedangkan besarannya masuk ke analogWrite di PWMA dan PWMB. Saat miring ke depan, roda melaju ke depan untuk kembali ke bawah badan.

Ketiga knob mengatur Kp dari 0 sampai 80, Ki dari 0 sampai 600 dan Kd dari 0 sampai 2. Dengan setiap knob di tengah (40, 300 dan 1) robot seimbang: lepaskan pada detik pertama, ia berdiri, dan sekitar empat detik setelah Jalankan serial monitor melaporkan "Balanced for 3 s". Dorong dengan panah dan ia berkata "Caught it: upright again" sesaat kemudian.

Inti loop robot penyeimbang, dipangkas
const float ALPHA = 0.98;
const float FALLEN = 45.0;
const float I_LIMIT = 150.0;

void loop() {
  unsigned long now = micros();
  if (now - lastUs < PERIOD_US) return;   // every 10 ms
  float dt = (now - lastUs) / 1000000.0;
  lastUs = now;

  Mpu6050Reading r;
  if (!imu.read(r)) return;
  float accelAngle = atan2(-(float)r.ax, (float)r.az) * 57.29578;
  float rate = r.gy / 131.0;
  angle = ALPHA * (angle + rate * dt) + (1 - ALPHA) * accelAngle;

  if (fabs(angle) > FALLEN) {
    fallen = true;
    integral = 0;
    brake();
    Serial.println("FALLEN");
  } else {
    integral += angle * dt;
    if (ki > 0.01) {
      float limit = I_LIMIT / ki;
      if (integral > limit) integral = limit;
      if (integral < -limit) integral = -limit;
    }
    float pid = kp * angle + ki * integral + kd * rate;
    output = (int)constrain(pid, -255, 255);
    drive(output);
  }
  readKnob();
}

Apa gunanya tiap gain: coba saja

Turunkan Ki sampai nol. Tanpa integral, robot tidak bisa menjaga dorongan yang dibutuhkan motor pada kecepatan tinggi, jadi ia merayap menjauh, makin lama makin miring untuk mengejar, dan jatuh: serial monitor bilang "FALLEN". Itulah jasa suku I.

Turunkan Kd sampai nol, lalu tekan tombol berdiri di robot. Tanpa redaman, tiap ayunan melewati yang sebelumnya: ia menggoyang makin keras sampai jatuh. Dengan Kd dinaikkan lagi, dorongan yang sama mereda dalam satu dua ayunan.

Kp mengatur seberapa keras ia melawan kemiringan. Terlalu rendah ia malas dan goyang; terlalu tinggi ia bergetar. Orang menyetel robot sungguhan dengan urutan itu: Kp sampai hampir berdiri dan mulai bergetar, Kd sampai getarannya berhenti, lalu Ki sampai berhenti bergeser.

Ditahan, dilepas, jatuh

A real robot is held upright while it powers up. If the PID ran during that time, the integral would fill up with a lean the wheels cannot fix (your hand is holding it), and the stored push would throw it over the moment you let go. So the sketch keeps the motors braked while the gyro says the body is still, and starts balancing when the rate goes over 1 degree a second: the hand has let go.

Lewat 45 derajat berarti ia jatuh. Motor mengerem, integral direset, dan di robot ia tergeletak di lantai pada sekitar 80 derajat. Dirikan dengan tombol dan ia ditahan lagi, siap untuk percobaan berikutnya.

Modelnya adalah pendulum sungguhan di atas roda, dihitung setiap milidetik, dengan motor yang mendorong badan ke belakang saat menggerakkan roda ke depan. Yang tidak disertakan adalah benjolan lantai, kelonggaran pada gearbox murah dan baterai yang drop, hal-hal yang membuat robot sungguhan lebih sulit disetel.

Kesalahan umum

Roda berputar ke arah yang salah. Kalau robot melaju menjauhi kemiringannya, ia langsung jatuh. Tukar kabel salah satu motor, atau balik tanda outputnya.

Sudut bertanda salah, karena MPU6050 dipasang dengan arah berbeda. Miringkan ke depan dengan tangan dan pastikan sudutnya positif sebelum menjalankan motor.

Bersandar pada akselerometer saja. Ia membaca hentakan roda sebagai kemiringan. Campur dengan giroskop.

STBY di TB6612 tidak disambungkan. Ia punya pull-down, jadi driver tetap mati dan motor tidak pernah berputar. Sambungkan STBY di rangkaian di atas dan robot langsung jatuh begitu dilepas.

Loop yang lambat. Mencetak ke serial monitor setiap loop, atau menggambar seluruh OLED sekaligus, mencuri waktu yang dibutuhkan PID. Sketch mencetak empat kali per detik dan menggambar satu baris OLED tiap pemanggilan.

Integral yang membengkak. Tanpa batas, suku I terus membesar saat robot ditahan atau terjebak. Sketch membatasinya dan me-reset saat jatuh.

Buka rangkaian ini di editor. Arduino Uno: robot penyeimbang mandiri, dirangkai seperti yang berjalan di bagian atas halaman ini. Ubah komponen atau nilai lalu jalankan lagi.

Buka rangkaian ini

Pertanyaan

Sensor apa yang dibutuhkan robot penyeimbang mandiri?

Akselerometer dan giroskop bersama, seperti MPU6050. Akselerometer memberi sudut dari gravitasi, giroskop seberapa cepat rotasinya, dan filter komplementer atau Kalman mencampurnya.

Bagaimana menyetel PID pada robot penyeimbang?

Mulai dengan Ki dan Kd di nol. Naikkan Kp sampai hampir berdiri dan mulai bergetar, naikkan Kd sampai getarannya berhenti, lalu tambahkan sedikit Ki supaya berhenti bergeser.

Driver motor mana yang terbaik untuk robot penyeimbang?

TB6612FNG pilihan bagus untuk motor bergigi kecil: efisien dan berpindah cepat. L298N bisa dipakai tapi membuang lebih banyak daya sebagai panas.

Kenapa robot penyeimbangku bergeser dan jatuh?

Biasanya integral terlalu kecil, jadi tidak bisa menjaga dorongan yang dibutuhkan pada kecepatan tinggi, atau titik seimbangnya belum benar-benar tegak. Tambah sedikit Ki, atau sesuaikan sudut nolnya.

Rakit versi nyatanya

Buka editor, ubah sebuah nilai, dan lihat angkanya ikut berubah. Tidak ada yang perlu dipasang, dan tidak perlu akun.