Öğren

Arduino üzerinde kendinden dengelenen bir robot

İleri

Ya da tüm dersleri gör.

  • 526parça tezgahta
  • 46kart şu an çalışıyor
  • 1.00xgerçek zamanlı, her kartta
Arduino Uno: kendinden dengelenen robotcanlı0.000 s 0.00x
Düzenleyicide açmak için tıkla
Balance bot örneği: tamamlayıcı filtresinden geçen bir MPU6050 ve üç kademe üzerinden bir PID. Ok butonlarıyla it.

Kendinden dengelenen bir robot, iki tekerlek üzerinde, ters duran bir sarkaç gibi durur ve yalnızca tekerleklerini sürekli kendi altına sürdüğü için ayakta kalır. Gürültülü bir sensörü iyi bir açıya çevirmeyi ve ona saniyede yüzlerce kez müdahale eden bir PID denetleyiciyi tek bir yapıya koyduğu için klasik bir Arduino projesidir.

Yukarıdaki devre, kendi MPU6050'si ve TB6612 motor sürücüsüyle iki tekerlekli bir robot çalıştıran bir Uno'dur. Çalışırken üç kademe PID kazançlarını ayarlar ve bir OLED açıyı ve kazançları gösterir. Çalıştır'a bas: robot bir an için dik tutulur, sonra bırakılır ve kendini dengeler. Sonra robot üzerindeki ok butonlarıyla onu it ve her kazancın ne yaptığını görmek için kademeleri çevir.

Neye ihtiyacın var?

  • Bir Arduino Uno
  • Dişli DC motorlu iki tekerlekli robot şasesi
  • Bir MPU6050 ivmeölçer ve jiroskop
  • Bir TB6612FNG motor sürücüsü
  • Üç 10k potansiyometre
  • Bir SSD1306 OLED, 128x64, I2C (isteğe bağlı)
  • Motorlar için bir batarya paketi

Kablolama

Parça ve pinGittiği yerNot
Robot SCL ve SDAPin 12 ve 2Robotun MPU6050'si 0x68 adresinde, kendi yazılım I2C'si üzerinde
AIN1, AIN2, PWMAPin 10, 9 ve 6Sol motor
BIN1, BIN2, PWMBPin 5, 4 ve 3Sağ motor
STBY5 VSürücü güç aldığı sürece çalışır
Kp, Ki ve Kd düğmeleriA0, A1 ve A2Uçları 5 V ve GND'ye
OLED SCL ve SDAA5 ve A4Açı ve kazançlar

Adım 1: eğimi hesapla

MPU6050 iki sensöre sahiptir ve hiçbiri tek başına yeterli değildir. İvmeölçer aşağı yönün hangisi olduğunu bilir, bu yüzden atan2(-ax, az) yerçekiminden eğimi verir. Ama tekerleklerin gövdeye verdiği her itişmeyi de hisseder, yani robot hareket ederken gürültülü ve hatalıdır. Jiroskop gövdenin ne kadar hızlı döndüğünü, pürüzsüz bir şekilde ölçer ama bir açı elde etmek için bu hızı toplayarak ilerletmen gerekir ve küçük hatalar da birikir, bu yüzden açı kayar.

Tamamlayıcı filtre her birinin en iyisini alır: angle = 0.98 * (angle + rate * dt) + 0.02 * accelAngle. An be an jiroskop açıyı taşır, ivmeölçer de onu her adımda %2 ile yumuşakça gerçeğe çeker. Tek satır koddur ve çoğu denge robotunun kullandığı şey budur.

Adım 2: açı üzerinde bir PID

Her 10 ms'de kod, gövdenin dik konumdan ne kadar saptığını hesaplar ve düzeltmek için tekerlekleri sürer. Çıktı, toplanan üç terimdir. P (oransal) eğim çarpı Kp'dir: daha çok eğil, daha sert sür. D (türev) jiroskop hızı çarpı Kd'dir: gövdenin hızlı savrulmasına karşı koyar ve bu da aşırı gitmeyi önler. I (integral) zamanla eğimi toplayıp Ki ile çarpar: bir motorun sadece bir hızı korumak için ihtiyaç duyduğu sabit itişi sağlar.

Toplam -255 ile 255 arasına sıkıştırılır, işareti yönü seçer (AIN1 ve AIN2), büyüklüğü ise PWMA ve PWMB üzerindeki analogWrite'a gider. Öne eğildiğinde tekerlekler gövdenin altına geri dönmek için ileri sürülür.

Üç kademe Kp'yi 0 ile 80, Ki'yi 0 ile 600 ve Kd'yi 0 ile 2 arasında ayarlar. Her kademe ortadayken (40, 300 ve 1) robot dengede kalır: birinci saniyede bırak, ayakta kalır ve Çalıştır'a bastıktan yaklaşık dört saniye sonra seri monitör "Balanced for 3 s" bildirir. Okla ittiğinde biraz sonra "Caught it: upright again" der.

Denge robotu döngüsünün kalbi, kısaltılmış hali
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();
}

Her kazanç ne yapar: dene

Ki'yi tamamen aşağı çevir. İntegral olmadan robot, bir motorun hızda ihtiyaç duyduğu itişi tutamaz, bu yüzden sürüklenir, yetişmek için gittikçe daha çok eğilir ve düşer: seri monitör "FALLEN" der. Bu, I teriminin yerini hak edişidir.

Kd'yi tamamen aşağı çevir, sonra robot üzerindeki ayağa kalkma düğmesine bas. Sönümleme olmadan her savrulma öncekini aşar: düşene kadar gittikçe daha sert sallanır. Kd tekrar yukarıdayken aynı itiş bir iki savrulmada söner.

Kp, bir eğime ne kadar sert karşı koyacağını belirler. Çok düşükse tembel ve sallantılıdır; çok yüksekse titrer. İnsanlar gerçek bir robotu şu sırayla ayarlar: neredeyse ayakta durup titremeye başlayana kadar Kp, titreme durana kadar Kd, sonra sürüklenme durana kadar Ki.

Tutuldu, bırakıldı, düştü

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.

45 dereceyi geçince düşmüştür. Motorlar frenlenir, integral sıfırlanır ve robot üzerinde yaklaşık 80 derecede yerde yatar. Düğmeyle ayağa kaldır ve yine tutulur, sonraki deneme için hazırdır.

Model, her milisaniyede hesaplanan, tekerlekleri ileri sürerken gövdeyi geri iten motorlara sahip gerçek bir tekerlekli sarkaçtır. Dışarıda bıraktıkları ise zeminin tümsekleri, ucuz dişli kutularındaki boşluklar ve gerilen bir bataryadır; gerçek bir robotu ayarlamayı zorlaştıran şeyler bunlardır.

Sık yapılan hatalar

Tekerleklerin ters yönde dönmesi. Robot eğildiği yönden uzaklaşırsa anında düşer. Bir motorun kablolarını yer değiştir ya da çıktının işaretini ters çevir.

Yanlış işaretli açı, çünkü MPU6050 farklı bir yönde monte edilmiştir. Motorları hiç çalıştırmadan önce elle öne eğ ve açının pozitife gittiğini kontrol et.

Sadece ivmeölçere güvenmek. Tekerleklerin itişmelerini eğim olarak okur. Jiroskopla harmanla.

TB6612 üzerindeki STBY'nin boş bırakılması. Pull-down'a sahiptir, yani sürücü kapalı kalır ve motorlar hiç dönmez. Yukarıdaki devrede STBY'yi çek, robot bırakılır bırakılmaz düşer.

Yavaş bir döngü. Her döngüde seri monitöre yazdırmak ya da tüm OLED'i bir kerede çizmek, PID'nin ihtiyaç duyduğu zamanı çalar. Kod saniyede dört kez yazdırır ve her çağrıda OLED'in bir satırını çizer.

İntegral şişmesi. Sınır olmadan I terimi robot tutulurken ya da sıkışırken büyümeye devam eder. Kod onu sınırlar ve düşüşte sıfırlar.

Bu devreyi düzenleyicide aç. Arduino Uno: kendi kendini dengeleyen bir robot, bu sayfanın üstünde çalıştığı haliyle kablolandı. Bir parçayı ya da değeri değiştir ve yeniden çalıştır.

Devreyi aç

Sorular

Kendi kendini dengeleyen bir robot hangi sensöre ihtiyaç duyar?

MPU6050 gibi bir ivmeölçer ve jiroskop birlikte. İvmeölçer yerçekiminden açıyı, jiroskop ne kadar hızlı döndüğünü verir ve tamamlayıcı ya da Kalman filtresi bunları harmanlar.

Denge robotunda PID'yi nasıl ayarlarım?

Ki ve Kd ile sıfırla başla. Kp'yi neredeyse ayakta durup titremeye başlayana kadar yükselt, Kd'yi titreme durana kadar yükselt, sonra sürüklenmeyi durdurmak için biraz Ki ekle.

Denge robotu için hangi motor sürücüsü en iyisidir?

TB6612FNG küçük dişli motorlar için iyi bir seçimdir: verimlidir ve hızlı anahtarlar. L298N çalışır ama daha çok gücü ısı olarak boşa harcar.

Denge robotum neden sürüklenip düşüyor?

Genellikle integralin çok az olması, yani hızda ihtiyaç duyduğu itişi tutamaması ya da tam dik olmayan bir denge noktası. Biraz Ki ekle ya da sıfır açıyı ayarla.

Bunu gerçekten kur

Düzenleyiciyi aç, bir değeri değiştir ve sayının onunla birlikte değiştiğini izle. Kurulacak bir şey yok, hesaba da gerek yok.