Freestyle Robot samobalansujący

[S0wa]

Cześć. Chciałbym przedstawić wam mój projekt robota samobalansującego. Jest to mój najbardziej zaawansowany jak dotąd projekt, który pochłonął najwięcej pracy. Pomysł na zbudowanie takiego robota zrodził się w mojej głowie w wakacje po obejrzeniu filmiku przedstawiającego podobną konstrukcję. Pracę nad nim rozpocząłem jeszcze w wakacje, a z przerwami zakończyłem w lutym. Działanie robota jest podobne do dostępnych na rynku Hoverboardów, czyli robot ma za zadanie poruszać się w taki sposób, aby utrzymać się w pionie, oczywiście bez podpierania. 

Konstrukcję robota zaprojektowałem sam i wykonałem z pręta gwintowanego i plexi. Uznałem że taka konstrukcja zapewni robotowi wymaganą sztywność i niską masę, jednocześnie nie rujnując budżetu. Najpierw wykonałem jej model w Fusion 360, a później przystąpiłem do pracy. Roboty z tym trochę było, bo obróbka plexi do łatwych nie należy. Sporo czasu zajęło przycięcie prostokątów, a jeszcze więcej wywiercenie otworów, które musiały być idealnie rozmieszczone, aby później dało się przełożyć przez nie pręt gwintowany.

 

Po zbudowaniu ramy przyszła pora na elektronikę. Ponieważ robot nie zapewnia dużo miejsca, a jednocześnie środek ciężkości powinien być możliwie wysoko, dla ułatwienia balansu, musiałem się postarać ścisnąć wszystko jak to tylko możliwe. Baterię zasilającą robota (ogniwe Li-Po odzyskane z powerbanka o pojemności 3000 mAh) umieściłem na najwyższej półce. Tam również trafiła przetwornica step-up podnosząca napięcie do 7,4 V. Wiem że lepiej byłoby dać dwie baterie połączone szeregowo, ale w momencie budowania budżet na to nie pozwolił. Szybko okazało się że jedna przetwornica to za mało - Arduino restartowało się przy każdym uruchomieniu silnika, nawet jeśli do układu przyłączyłem bardzo duży kondensator. Dodałem więc drugą - teraz silniki były zasilanie niezależnie od elektroniki sterującej. Pozwoliło to stanąć robotowi po raz pierwszy, jednak wciąż jedna przetwornica zasilająca silniki ograniczała ich moc. Dodałem drugą, co nieco poprawiło sytuację, jednak wciąż nieco ogranicza silniki. Póki co szukam lepszego rozwiązania.

 

Schodząc na niższą półkę. Tam trafiła płytka ze sterownikiem i druga z mostkiem H. Pierwszą płytkę lutowałem sam na płytce uniwersalnej - zajęło to kilka godzin i mnóstwo cyny, ale satysfakcja była nie do opisania, zwłaszcza że wszystkie połączenia wykonałem cyną. Po spędzeniu kolejnej godziny na pozbywaniu się zwarć układ był gotowy do testów 🙂

Robot aby ustalić swoją pozycję korzysta z modułu żyroskopu i akcelerometru MPU6050 oraz wbudowanych w silniki enkoderów. Dane z tych czujników trafiają do Arduino Nano które poprzez układ L298N steruje dwoma silnikami. Dzięki akcelerometrowi i żyroskopowi robot zna swój kąt przechylenia. W pierwszych wersjach kodu szybko okazało się, że to nie wystarczająco, ponieważ o ile robot stał, to cały czas przesuwał się w jedną stronę. Wynika to z tego że konstrukcja nie jest idealnie wyważona, a robot nie zna swojej pozycji, jedynie kąt nachylenia, więc jeżeli ustawiony na sztywno kąt odbiegał od kąta balansu, robot "dryfował". Aby temu zapobiec dodałem drugie sprzężenie zwrotne, tym razem oparte o enkodery przy silnikach. Enkodery zliczają ilość impulsów które silniki wykonały w każdą ze stron, a oprogramowanie dąży, aby ich różnica była równa 0. 

Pierwsze próby nie były zbyt udane. Kiedy doprowadziłem kod do stanu w którym robot chociaż próbował się utrzymać, udawało mu się to przez maksymalnie kilkanaście sekund. Jednak wraz z poprawianiem kodu robot stał dłużej i dłużej.

 Obecnie już się nie wywala, jednak mostek H po dłuższym staniu się przegrzewa, więc czas pracy ograniczony jest do kilku minut. Mimo to robot robi duże wrażenie, chociaż z pewnością nie jest idealny. Zastosowałem tanie chińskie silniki, i jak się okazało, jeden obraca się szybciej niż drugi, więc robot obraca się w jedną stronę. Do tego jak wspomniałem przegrzewa się, a silnikom brakuje momentu, więc robot się trzęsie zamiast stać idealnie prosto. Są to jednak problemy które mam w planach poprawić. Planuję dodać również możliwość sterowania po BT (już nawet na płytce jest miejsce na moduł HC-06), ale na razie nie miałem czasu. 

Oto filmik prezentujący działanie robota:

 

Edytowano Marzec 26, 2019 przez S0wa

[Treker (Damian Szymański)]

@S0wa, właśnie zaakceptowałem opis. Dziękuję za przedstawienie ciekawego projektu, zachęcam do prezentowania kolejnych DIY oraz aktywności na naszym forum 😉

[bigthomas]

Witam,

Ciekawy projekt, chciałbym spróbować skonstruować podobnego robota. Chętnie zobaczył bym kod źródłowy dla tego robota.

[S0wa]

#include <Wire.h>

///////////////////////MPU Data/////////////////////////
long rawAccelX, rawAccelY, rawAccelZ; // raw data from accel
float accelX, accelY, accelZ; //calculated data from Accel

long rawGyroX, rawGyroY, rawGyroZ; //raw data from gyro
float gyroX, gyroY, gyroZ; //calculated data from Gyro 
///////////////////////MPU Data//////////////////////////

//////////////////////Time and angle/////////////////////////
float elapsedTime, timePrev, time;
float totalAngle; //Real, filtered angle from MPU
float desiredAngle=-2; //Desired angle of robot 
//////////////////////Time and angle/////////////////////////

//////////////////////PID/////////////////////////////////// 
float pid, error, prevError;
float pidP;
float pidI;
float pidD;

double kp=70; //<do ustawienia>>
double ki=1.2; //<do ustawienia>>
double kd=0.7; //<do ustawienia>>
//////////////////////PID///////////////////////////////////

//////////////////////PIDA//////////////////////////////////
int pidA, errorA, prevErrorA;
int desiredStepsA=0;   //
float pidPA;
float pidIA;
float pidDA;

double kpA=0.001; //<do ustawienia>>
double kiA=0.0000015; //<do ustawienia>>
double kdA=0.0015; //<do ustawienia>>
//////////////////////PIDA//////////////////////////////////


//////////////////////Motors////////////////////////////////
int in1=8; 
int in2=7;
int in3=5;
int in4=4;

int enA=9;
int enB=6;

int enc1=3;
int enc2=2;
int enc3=12;
int enc4=11;

int stepsA=0;
int stepsB=0;

int speedA;
int speedB;
//////////////////////Motors////////////////////////////////

/////////////////////Other/////////////////////////
float rad_to_deg = 180/3.141592654;
/////////////////////Other/////////////////////////


void setup() {
  pinMode(in1, INPUT);
  pinMode(in2, INPUT);
  pinMode(in3, INPUT);
  pinMode(in4, INPUT);
  pinMode(enc1, INPUT);
  pinMode(enc2, INPUT);
  pinMode(enc3, INPUT);
  pinMode(enc4, INPUT);
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  Wire.begin();
  
  setupMPU(); //setups MPU

  attachInterrupt(digitalPinToInterrupt(enc1), steps1, CHANGE);
  attachInterrupt(digitalPinToInterrupt(enc2), steps2, CHANGE);

  time = millis();
}

void loop() {
  timePrev = time;
  time = millis();
  elapsedTime = (time - timePrev) / 1000;
  
  readAccelData(); //reads data from accelerometer
  readGyroData(); //reads data from gyroscope
  calculateRealAngle(); //calculates real angle
  calculatePIDA();
  calculatePID(); //calculates PID value
  action();
}

void setupMPU() {
  Wire.beginTransmission(0b1101000); //Begins transmision with MPU
  Wire.write(0x6B);  //
  Wire.write(0b00000000); //Turns sleep mode off
  Wire.endTransmission();
  Wire.beginTransmission(0b1101000);
  Wire.write(0x1B); //
  Wire.write(0x00000000); //Sets Gyro scale to +/- 250 deg/sec <do ustawienia>
  Wire.write(0x1C); //
  Wire.write(0x00000000); //Sets Accel scale to +/- 2g <do ustawienia>
  Wire.endTransmission();
}

void readAccelData() {
  Wire.beginTransmission(0b1101000);
  Wire.write(0x3B);
  Wire.endTransmission();
  Wire.requestFrom(0b1101000, 6); //Requests Accel registers
  while (Wire.available()<6);
  rawAccelX = Wire.read()<<8|Wire.read();
  rawAccelY = Wire.read()<<8|Wire.read();
  rawAccelZ = Wire.read()<<8|Wire.read();

    //Process Accel data to sth usefull
  accelX = rawAccelX / 16384.0; //<- depends on Accel scale
  accelY = rawAccelY / 16384.0; //<- depends on Accel scale
  accelZ = rawAccelZ / 16384.0; //<- depends on Accel scale
}

void readGyroData() {
  Wire.beginTransmission(0b1101000);
  Wire.write(0x43);
  Wire.endTransmission();
  Wire.requestFrom(0b1101000, 6); //Requests Gyro registers
  while (Wire.available()<6);
  rawGyroX = Wire.read()<<8|Wire.read();
  rawGyroY = Wire.read()<<8|Wire.read();
  rawGyroZ = Wire.read()<<8|Wire.read();

    //Process Gyro data to sth usefull
  gyroX = rawGyroX / 131.0; //<- depends on Gyro scale
  gyroY = rawGyroY / 131.0; //<- depends on Gyro scale
  gyroZ = rawGyroZ / 131.0; //<- depends on Gyro scale
}

void calculateRealAngle() {
  float accelAngle = atan(accelY/sqrt(pow(accelX,2) + pow(accelZ,2)))*rad_to_deg; //Calculates Accel angle in X axis
  totalAngle = 0.98 *(totalAngle + gyroX * elapsedTime) + 0.02*accelAngle; //Real filtered angle in X axis
  
}

void calculatePID() {
  error = totalAngle - desiredAngle+pidA;
  pidP = kp*error;
  pidI = pidI+(ki*error);
  pidD = kd*((error-prevError)/elapsedTime);
  pid = pidP+pidI+pidD;
  prevError = error;
}

void action() {

  speedA = constrain(abs(pid), 0, 255);
  speedB = constrain(abs(pid), 0, 255);
  
  if (pid >0) {   
    moveForward();
  }
  else if (pid <0){ 
    moveBackward();
  }
  
}

void steps1(){
  if (digitalRead(enc1) == HIGH) {
    if (digitalRead(enc3) == LOW) {
      stepsA++;
    } else {
      stepsA--;
    }
  } else {
    if (digitalRead(enc3) == LOW) {
      stepsA--;
    } else {
      stepsA++;
    }
  }
}
void steps2(){
  if (digitalRead(enc2) == HIGH) {
    if (digitalRead(enc4) == LOW) {
      stepsB--;
    } else {
      stepsB++;
    }
  } else {
    if (digitalRead(enc4) == LOW) {
      stepsB++;
    } else {
      stepsB--;
    }
  }
}

void moveForward(){ //moves motors forward
  digitalWrite (in1, HIGH);
  digitalWrite (in2, LOW);
  digitalWrite (in3, HIGH);
  digitalWrite (in4, LOW);

  analogWrite(enA, speedA);
  analogWrite(enB, speedB);
}

void moveBackward(){ //moves motors backward
  digitalWrite (in1, LOW);
  digitalWrite (in2, HIGH);
  digitalWrite (in3, LOW);
  digitalWrite (in4, HIGH);

  analogWrite(enA, speedA);
  analogWrite(enB, speedB);
}

void calculatePIDA() {    //compensates position drift
  errorA = stepsA - desiredStepsA;
  pidPA = kpA*errorA;
  pidIA = pidIA+(kiA*errorA);
  pidDA = kdA*((errorA-prevErrorA)/elapsedTime);
  pidA = pidPA+pidIA+pidDA;
  prevErrorA = errorA;
}

Proszę bardzo. Z pewnością nie jest to kod, którym można się chwalić, ale mniej lub bardziej działa. Starałem się go komentować, ale na pewnym etapie prac wymknęło się to spod kontroli. Na wszystkie pytania postaram się odpowiedzieć 🙂 

[Polecacz 101]

Produkcja i montaż PCB - wybierz sprawdzone PCBWay!
   • Darmowe płytki dla studentów i projektów non-profit
   • Tylko 5$ za 10 prototypów PCB w 24 godziny
   • Usługa projektowania PCB na zlecenie
   • Montaż PCB od 30$ + bezpłatna dostawa i szablony
   • Darmowe narzędzie do podglądu plików Gerber
Zobacz również » Film z fabryki PCBWay

[adusn86]

Po przeskalowaniu tego projektu o 200%-300% widzę zastosowanie dla osób niepełnosprawnych poruszających się na wózku. Bądź osób element rehabilitacji czy wsparcia w problemie z chodzeniem, czy po amputacji nóg/nogi.

Świetny DIY.

[S0wa]

@adusn86 Dzięki 😉 Projekt nie powstawał z myślą poprawy świata, bardziej służył mi nauce nowych rzeczy, a przy okazji załapał się jako projekt szkolny. Segwaye i hoverboardy działają na tej samej zasadzie, więc ktoś już to powiększył, ja tylko zmniejszyłem aby zmieścić się w budżecie 🙂. Są to jednak głównie zabawki (chociaż segwaye znalazły zastosowanie np. w branży ochroniarskiej), a mój robot bardziej obrazuje zasadę działania żyroskopu. W ten sam sposób sterowane są np. drony, tylko że w robocie mamy kontrolę jednej osi, a w dronach dwóch.  

[adusn86]

Dokładnie, Segwaye znalazły zastosowanie, tylko w dalszym ciągu uważam że ceny tej technologi - żyroskopu w mobilnych pojazdach (bez uwzględnienia kosztów ogniw) są przeszacowane. Powinni "klepać" protezy nóg nogi na potęgę. 

 

[ethanak]

z ciekawości: po co w protezie nogi żyroskop? poważnie pytam...

[S0wa]

@adusn86 Wszystko co ma efekt "wow" kosztuje. Żyroskop do mojego robota kosztował z 5 zł, ale podejrzewam że w Segwayach są dokładniejsze. Cały mój robot nie jest zbyt dokładny co widać po tym jak się trzęsie, choć to pewnie głównie zasługa silników. Są to drogie pojazdy, ale jak się doda wszystko razem to koszt części niski nie jest, a że ich główne zastosowanie jest w specjalistycznej branży to i marżę można dodać sobie większą.

@ethanak Zgaduję że dzięki temu taka noga będzie się sama naturalnie zginać w kolanie, tak jak to robi ludzka noga. Nogi mam swoje i nigdy nie miałem do czynienia z protezami, ale tak zgaduję. Drugą opcją która przychodzi mi do głowy to balansowanie tej nogi. Zauważ że jak stoisz, pomimo że się nie ruszasz to mięśnie w nodze cały czas pracują aby utrzymać cię w pionie, a szczególnie palce u stopy. Z tego co słyszałem, osobom które straciły palce u nóg ciężej jest utrzymać stabilność. Bardzo dobrze to widać na robocie, który pomimo że ma za zadanie stać prosto, cały czas musi korygować swoją pozycję i silniki cały czas pracują, w przeciwnym wypadku od razu się wywala. Osoby z protezą nogi często chodzą o kulach, chociaż teoretycznie ilość kończyn jest taka sama. Sztuczna noga nie zapewnia jednak tego balansu który zapewnia prawdziwa. 

To wszystko moje domysł, poczekajmy aż Adusn się wypowie 🙂 

[ethanak]

To wszystko zależy od protezy.

Jeśli masz protezę poniżej kolana to po prostu się musisz przyzwyczaić do stopy.

Jeśli masz nad kolanem (i jakiś napęd tego kolana) to ważniejszy jest kąt zgięcia (zresztą mózg nie dostaje danych o bezwzględnym położeniu kończyny, ale właśnie o kątach).

To że ludzie z protezami chodzą o kulach to raczej normalne - żadna proteza nie zapewni dokładnego wyczucia położenia, a kula pomaga w wielu przypadkach w utrzymaniu równowagi (ewentualnie po porostu w tym, żeby nie polecieć na ryj jak się potkniesz). A jeśli masz sztywną protezę powyżej kolana, to i tak kula jest potrzebna choćby po to, aby zejść ze schodów bo sztywna proteza tego nie umożliwia.

Sam mam taką trzecią nogę (cukrzyca, dna i tak dalej) i potrzebuję jej wtedy kiedy tracę równowagę... a przecież obie nogi mam swoje...

[S0wa]

@ethanak To ciekawe co piszesz. Trzeciej nogi potrzebujesz kiedy tracisz kontrolę nad swoimi nogami, jak zgaduję. W takiej sytuacji nie ma to większego znaczenia czy to prawdziwa noga, czy sztuczna, jeżeli kontrola nad nią jest ograniczona. Stanie prosto też nie jest takie proste jak się wydaje. Wymaga ciągłej korekcji swojej pozycji, co nasz organizm opanował bardzo dobrze, ponieważ punkt styku z podłożem jest relatywnie mały w porównaniu do wysokości ciała. Jeżeli postawimy coś podobnych rozmiarów do człowiek o takiej samej powierzchni styku, będzie to łatwo przewrócić, a wystarczy że nie będzie stało prosto i już się wywali. Człowiek może o wiele lepiej utrzymać stabilność, nawet w trudnych warunkach, kosztem ciągłej korekcji swojej pozycji. Jeżeli możliwość tej korekcji z jakiegoś powodu traci to się wywala. 

Tym samym podejrzewam że rozbudowanie protezy to jakiś system utrzymywania pozycji oparty na różnych czujnikach może umożliwić poruszanie się na takich protezach bez kul. Mówię tu oczywiście o protezach powyżej kolana ze zginanym kolanem i kostką. Wydaje mi się że już istnieją takie protezy, ale podejrzewam że są strasznie drogie.

[wmaker]

Dnia 30.03.2019 o 21:28, S0wa napisał:

@ethanak To ciekawe co piszesz. Trzeciej nogi potrzebujesz kiedy tracisz kontrolę nad swoimi nogami, jak zgaduję. W takiej sytuacji nie ma to większego znaczenia czy to prawdziwa noga, czy sztuczna, jeżeli kontrola nad nią jest ograniczona. Stanie prosto też nie jest takie proste jak się wydaje. Wymaga ciągłej korekcji swojej pozycji, co nasz organizm opanował bardzo dobrze, ponieważ punkt styku z podłożem jest relatywnie mały w porównaniu do wysokości ciała. Jeżeli postawimy coś podobnych rozmiarów do człowiek o takiej samej powierzchni styku, będzie to łatwo przewrócić, a wystarczy że nie będzie stało prosto i już się wywali. Człowiek może o wiele lepiej utrzymać stabilność, nawet w trudnych warunkach, kosztem ciągłej korekcji swojej pozycji. Jeżeli możliwość tej korekcji z jakiegoś powodu traci to się wywala. 

Tym samym podejrzewam że rozbudowanie protezy to jakiś system utrzymywania pozycji oparty na różnych czujnikach może umożliwić poruszanie się na takich protezach bez kul. Mówię tu oczywiście o protezach powyżej kolana ze zginanym kolanem i kostką. Wydaje mi się że już istnieją takie protezy, ale podejrzewam że są strasznie drogie.

https://www.nibib.nih.gov/news-events/newsroom/powered-robotic-legs-leaping-toward-future

 

[ethanak]

Przepraszam że wcześniej nie odpowiedziałem, ale po prostu gdzieś mi post zginął 😞

@S0waPisałeś o trudnościach w utrzymaniu równowagi w przypadku amputacji palców. To akurat jest dość proste - bez palców można spokojnie chodzić, palce po prostu działają jak tensometr. Zauważ: jeśli zbudujesz maszynę której zadaniem będzie stanie prosto i wyposażysz ją w tensometr mierzący siłę nacisku przed punktem podparcia (taką "piętą") będzie w stanie utrzymać równowagę odpowiednio balansując ciałem i kontrolując siłę naciskającą na ów tensometr. Za duża - przechylamy się za mocno do przodu; za mała - do tyłu. I dalej nie ma w nodze żadnych żyroskopów, akcelerometrów i tym podobnych - coś takiego masz w uchu, ale od nogi to trochę daleko 🙂 Zresztą - przy nierównym terenie, gdzie stopa ma stanąć nie prostopadle do wektora grawitacji, ale równolegle do przechylonego podłoża, żadne żyroskopy nie pomogą. Poza tym przyspieszenia działające na nogę w czasie chodzenia (nie wspominając o biegu) uczynią wskazania owego żyroskopu bezwartościowymi.

Natomiast przy wózku akcelerometr/żyroskop są konieczne; wózek ma utrzymać pozycję poziomą siedziska niezależnie od podwozia, inaczej po prostu się przewróci (zauważ: środek ciężkości wózka położony jest stosunkowo wysoko). Przyspieszenia są niewielkie, prędkości też. Tu jest większe pole do popisu - również dla konstrukcji balansujących. Zaprojektuj porządny wózek jeżdżący po schodach i pochylniach czy wjeżdżający skosem na krawężnik, to Cię inwalidzi ozłocą 🙂