Balancující robot

Pochlubte se svým robotem ostatním!
JuraS.
Příspěvky: 318
Registrován: 28 led 2015, 16:06

Balancující robot

Příspěvek od JuraS. »

Abych nepsal do různých otázek tak založím toto téma.
Můj robot by měl umět balancovat na dvou kolech, umět používat kameru a pomocí opencv řídit směr a další věci.
K tomu mám doma :
Kostru z překližkových desek které jsou provrtané v rozích kudy vede šroubovice pomocí které se nastaví kde přesně bude jednotlivé patro.
Patra budou 4 : První je pro motory a gyro (zespodu). Z vrchní strany budou akumulátory. Druhé patro zespodu bude mít elektrické rozvody, tj. stabilizaci napětí a rozvody včetně napájení aku a vypínačů. na horní straně pak bude CPU a celé řízení. Třetí patro je prozatím uvažováno jako stabilizační (aby celá kostra líp seděla) ale v plánu je i ruka na zvednutí robota. Čtvrté patro je prozatím obsazeno jen z vrchu kde je pan/tild a řízení serv.
Jako motory jsou použity bipolární krokové motory NEMA17 které jsou v držácích. Mezi nimi je umístněn MPU-6050.
Akumulátory budou teprve rozhodnuty. Buď v páru 2x 3článek LiPo a nebo 12V gelový akumulátor.
Převodníků elektřiny bude více. pro CPU slouží 5V/3A 2xUSB. Pro zbytek elektroniky pak 5A konvertor napětí nebo případně tento. Rozvody jsou zapojené do čokoládek a využity jsou klasické vypínače.
Jako CPU je využito Raspberry PI B+ které s OpenCV se bude starat o "mozek". Zároveň bude sloužit jako komunikační cesta svou wifi. Na RPi pak bude štít který přidá funkcionalitu arduina které bude řídit balanc a motory. Pro motory pak bude použit štít který bude řídit motory pomocí dvou driverů A4988. Pro jejich chlazení připravuji pasivní chladiče z můstků základní desky počítače. neměli by se tak přehřívat.
Pan/tild čili krk a hlavu řídí dvě 9gr serva zapojená do 16 kanálového řadiče pro řízení modelářských serv které cestou I2C budou řídit otáčení hlavou která bude sestavena z desky tacpronu s dvěmi kukadly z HC-SR04 a mezi nimi bude čočka kamery.

V současnosti si hraji s motory připojenými na arduino uno a jeho cnc shield se dvěmi A4988. Včera pošta našla a dodala kameru ale se základním tj.10 cm ribbon kabelem. To jsem nevychytal takže čekám na 50cm dlouhý kabel. Radši delší než kratší. Mám kódy pro čtení hodnot z MPU-6050 včetně výpočtu kalmanova filtru. Mám i knihovnu včetně kódu pro řízení PID procesu, ale zatím se v tom nějak nevyznám tzn. že nechápu jaké hodnoty mám do knihovny PID poslat a co mi z toho vyleze. Motory se pomocí stepper knihovny řídí jednoduchými příkazy které zahrnují jednoduché parametry : akcelerace, maximální rychlost, o kolik kroků, jakou rychlostí. Zatím co jsem našel vzorové řešení tak ty byly pro DC motory kdy PID proces musel stále dorovnávat stylem dopředu/dozadu, přidej/uber balanc robota. S kamerou jsem si včera trochu hrál, dnes si budu zkoušet steamování videa do počítače tj. abych viděl to co vidí robot. Potřebuji totiž raspberry s kamerou přemístit někam kde je víc světla a ne roh kde mám počítač. A tam mu naštelovat čočku. Případně najít kde se zapíná nějaký autofokus. Nicméně tato kamera umí vidět i na hodně daleko. Jakmile tohle rozchodím tak si budu zkoušet napři facedetect. A pak detekování předmětů. Rozeznávání barev a detekování. Kódy v pythonu mám, chyběla jen kamera.
Až bude umět robot stát, otáčet se a jezdit tak mu přidám možnost umět jezdit podle čáry. K tmu buď použiju opencv (ale to by bylo matematicky náročné) a nebo použiju 4 kanálový detektor čáry. K sledování úbytku v baterii pak bude použit měřič napětí na základě kterého pak spočítám kolik toho zbývá v baterce. Podle maximálních proudů pak rozhodnu jestli bude v robotovi stačit 12V gelovka (mám 4,5Ah, 7Ah) a nebo jestli bude třeba použít lipolku. V případě gelovek by pak mohl robot sám jezdit do nabíjecí stanice (měl by dvě aku.K tomu bych, ale potřeboval dost přesný dálkoměr.
Ten štít pro raspberry nemají už 3tí týden na skladu, už jsem si tam i stěžoval. Slíbili mi, že jakmile se objeví na skladě tak první hned půjde ke mě. Štít pro motory je na cestě do Evropy. Doufám, že se na poště neztratí. Neměl by, platil jsem u něj poštovné. Ale s českou poštou nemám dobré zkušenosti.
Uživatelský avatar
Rosparovac
Příspěvky: 26
Registrován: 30 dub 2014, 19:17

Re: Balancující robot

Příspěvek od Rosparovac »

Co dodat...
Hodně štěstí!!!
Náš tým RTOstrava
"Nikdy se nehádejte s blbcem, protože se tím snížíte na jeho úroveň a tam Vás ubije svými zkušenostmi..."
JuraS.
Příspěvky: 318
Registrován: 28 led 2015, 16:06

Re: Balancující robot

Příspěvek od JuraS. »

Díky. Včera jsem si chvíli hrál s kamerou. Streamování videa do počítače cestou vlc je super, jen musím zjistit ve kterém parametru nastavení jsem dal cca 2 sec prodlevu tj. to co se mi zobrazí na počítači se stalo před 2 mi sekundami. Kde se ten obraz toulá nevím. Pak jsem si hrál s opencv. Podařilo se mi rozchodit aby opencv hledalo lidský xicht a snažilo se za ním otáčet pan/tild. To jsem zatím (kvůli krátkému kabelu ke kameře) netestoval. Výsledek jsem promítal na počítači cestou vnc serveru. To bylo líné jak slimák. Tak se teď pokusím najít odpověď zda by se nenašla možnost kdy by se výsledek opencv vysílal do sítě a já to na počítači chytal do vlc. Příští týden bude dost času. Tak ho podělím mezi opencv a WOT. Ke kameře jsem objednal dva zářiče pro noční vidění. K čemu mi to bude zatím nevím ale za tu cenu ? Proč bych to nepřidal když to tam patří, že ?
Odpovědi jsem našel tady. Na propojení výstupů z gyra a akcelometru a spojení s PID procesem jsem našel a mám několik řešení. Vzorec na PID je "jednoduchý" a je dokonce i vzorové řešení části kódu v C pro arduino které ten vzorec převede do arduina. Jen mi ještě nesecvaklo proč tam jsou u každého řešení tři potenciometry co něco nastavují a k čemu tam vlastně souží. Jak to pochopím tak si to udělám po svým. Četl jsem popis PID procesu kde se počítalo s výškou předmětu, průměrem kola, sílou motoru, akcelarací motoru, těžištěm předmětu (to bych chtěl vědět jak spočítat bod těžiště robota co postavím) a další veličiny aby ten výpočet byl do nejvíc podrobný.
Stížnost u číňanů zabrala. Obdržel jsem informaci, že se jim na skladu objevil shield pro raspberry a hned mi ho zabalili a připravili k odeslání. Tak jen doufám, že to nezůstane viset měsíc na celnici a nebo že se to neztratí na poště. To by mě už fakt jeblo.
Uživatelský avatar
jrt
Příspěvky: 1921
Registrován: 17 úno 2013, 17:13

Re: Balancující robot

Příspěvek od jrt »

Jen mi ještě nesecvaklo proč tam jsou u každého řešení tři potenciometry co něco nastavují a k čemu tam vlastně souží.
Ty nastavují Proporcionální, Integrační a Derivační část regulátoru.
Jak to pochopím tak si to udělám po svým.
Odvaha ti rozhodně nechybí... Tak směle do díla!
jrt
JuraS.
Příspěvky: 318
Registrován: 28 led 2015, 16:06

Re: Balancující robot

Příspěvek od JuraS. »

Tohle jsem četl několikrát ale nechápu to. Nakloněná rovina. Vím kde je střed roviny a vím o kolik je nakloněna v úhlu na každou stranu v prostoru. Uvažujme, že rovina se naklání ve 3 směrech kdy X je směr dopředu a dozadu, Y je náklon vpravo a vlevo a Z je stoupání či klesání. Pro rovinného robota co nemá ambice létat stačí pro počátek jen osa X a Y. ještě mi vypadl směr kdy se nemění X ani Y ale rovina se otáčí kolem Z. No nic. Mám kód kdy výsledkem jsou hodnoty vyčtené z gyra a akcelerometru v čisté podobě a i s použitím kalmanova filtru (pro odstranění "chvění")

Kód: Vybrat vše

#if 0 // Set to 1 to activate
  Serial.print(accX); Serial.print("\t");
  Serial.print(accY); Serial.print("\t");
  Serial.print(accZ); Serial.print("\t");

  Serial.print(gyroX); Serial.print("\t");
  Serial.print(gyroY); Serial.print("\t");
  Serial.print(gyroZ); Serial.print("\t");

  Serial.print("\t");
#endif

  Serial.print(roll); Serial.print("\t");
  Serial.print(gyroXangle); Serial.print("\t");
  Serial.print(compAngleX); Serial.print("\t");
  Serial.print(kalAngleX); Serial.print("\t");

  Serial.print("\t");

  Serial.print(pitch); Serial.print("\t");
  Serial.print(gyroYangle); Serial.print("\t");
  Serial.print(compAngleY); Serial.print("\t");
  Serial.print(kalAngleY); Serial.print("\t");
a mám kód pro PID proces.

Kód: Vybrat vše

#include <PID_v1.h>

//Define Variables we'll be connecting to
double Setpoint, Input, Output;
int inputPin=0, outputPin=3;

//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);
Nerozumím tomu co mám za hodnoty poslat do PID procesu aby ve výsledku byla hodnota pravý motor 150 mikrokroků vpřed a levý motor 25 mikrokroků vzad. Mám to všechno vytištěné, čtu si to cestou z práce a do práce ale zatím se mi to v hlavě neseplo. Mám i řešení kdy je vzorec PID součástí kodu a není jako knihovna. Tohle řešení bych bral raději. Já raději dám všechno do samostatných funkcí a ty pak spojuju. I v setupu raději volám funkci kde je odděleně nějaká samostaná část než to prasit všechno dohromady.

Vzhledem k tomu jak je robot koncipován se bude startovat následovně. Bude postavený na hranulu ze dřeva či polystyrenu tak, že na něm bude posazený na spodní hranu motorů tak aby kola byly nad terénem čili "ve vzduchu". Zapnu jedním vypínačem raspberry. Nevím ještě jak je propojené s tím shieldem co přidá funkcionalitu arduina tj. zda se štít aktivuje až po naběhnutí RPi a nebo zda hned při startu. Koncipuji to tak aby se rovnou po startu RPi spustil kod pro řízení robota co bude v pythonu tj. řízení opencv, komunikace s počítačem, přenos obrazu z kamery. Řízení směru pohybu. RPi se nespouští moc rychle, holt počítač. Nevím jestli už v tu dobu nebude spuštěno arduino abych mohl vypínačem pustit 12V do štítu pro řízení motorů. každopádně tam dám nějaké diody a pípátko aby mi hlásilo jednotlivé stavy. Počítám že až všechno naběhne a slinkuje se arduino s Rpi tak se provede kalibrace k de fakto rovině. Kola by neměla být víš než milimetr, dva nad zemí takže odchalka by měla být minimální případně ji rovnou zadám do kalibrace.. Po kalibraci gyra a akcelometru by se takhle měl kalibrovat i PID proces a konečným výsledkem sdělujícím připravenost bude že se obě kola otočí o jednu otáčku dopředu a dozadu. Pak robota sundám z podstavce a měl by už sám fungovat. takže nějaké potenciometry na kalibraci nepotřebuji.

http://waset.org/publications/15128/the ... ol-schemes
AlesH
Příspěvky: 323
Registrován: 25 úno 2013, 09:18

Re: Balancující robot

Příspěvek od AlesH »

Ta tvoje ukázka kódu pro PID není celá, takže konkrétně odpovědět nemohu. Zkusím to ale obecně (to by mohlo pomoci pochopení).

PID regulátor se obecně snaží dosáhnout, aby nějaká "měřená hodnota" odpovídala "požadované hodnotě". Dosahuje toho tím, že porovnává "naměřenou hodnotu" s "požadovanou" a podle odchylky (velikosti a rychlosti změn) nastavuje svoji výstupní hodnotu, která musí ovládat nějaký "silový člen" (např. motor pohánějící kolo), schopný ovlivnit "měřenou hodnotu".

Kalmanův filtr obecně nedělá nic jiného, než že z dostupných informací složí "měřenou hodnotu", která co nejlépe odpovídá skutečnosti. Tato "měřená hodnota" pak může sloužit jako jeden vstup pro PID regulátor (viz. výše). Druhým "vstupem" PID regulátoru je "požadovaná hodnota".

V případě balancujícího robota budeš zřejmě potřebovat víc PID regulátorů a víc měřených hodnot. Dělá se to ale tak, že pro každou jednu hodnotu (měřenou/požadovanou) je v programu jeden pokud možno nezávislý PID regulátor. Pro balancujícího robota je nejdůležitější "se udržet svisle", takže "měřenou hodnotou" musí být "náklon směrem dopředu/dozadu" (pitch) a "požadovanou hodnotou" bude 0 (nula). Výstup z PID regulátoru musí dokázat nastavovat směr a rychlost otáčení kol (obou současně a obou stejně). To by mělo jít ve tvém případě nastavováním parametru driveru motorů pro "rychlost" (se znaménky + a -). Druhý PID regulátor budeš muset mít použit pro řízení směru (azimutu) robota (yaw). Protože ale výstup tohoto regulátoru vede ke stejným motorům, budou se muset výsledky z obou PID regulátorů nějak vhodně "sloučit" aby se správně nastavovala rychlost každého kola zvlášť. To zatím ještě nevím přesně jak.

Je to alespoň trochu srozumitelné?
JuraS.
Příspěvky: 318
Registrován: 28 led 2015, 16:06

Re: Balancující robot

Příspěvek od JuraS. »

Jo parádně. PID kód je odsud a odsud respektive odsud. MPU6050 s kalmanovým filtrem pak odsud.
Chápu že proces balancu je samostatný a pak pohyb je další proces co se musí s tím balancem poprat respektive táhnout za jeden provaz. Takže nejprve se chci postavit, pak se umět dívat a reagovat a pak teprve uděláme malý krůček dopředu. Viděl sjem pokusy s balancujícími roboty kdy jel vzhůru po nakloněné desce tj že i šikmo kdy jedno kolo bylo výš než druhé.
JuraS.
Příspěvky: 318
Registrován: 28 led 2015, 16:06

Re: Balancující robot

Příspěvek od JuraS. »

AlesH píše: V případě balancujícího robota budeš zřejmě potřebovat víc PID regulátorů a víc měřených hodnot. Dělá se to ale tak, že pro každou jednu hodnotu (měřenou/požadovanou) je v programu jeden pokud možno nezávislý PID regulátor. Pro balancujícího robota je nejdůležitější "se udržet svisle", takže "měřenou hodnotou" musí být "náklon směrem dopředu/dozadu" (pitch) a "požadovanou hodnotou" bude 0 (nula). Výstup z PID regulátoru musí dokázat nastavovat směr a rychlost otáčení kol (obou současně a obou stejně). To by mělo jít ve tvém případě nastavováním parametru driveru motorů pro "rychlost" (se znaménky + a -). Druhý PID regulátor budeš muset mít použit pro řízení směru (azimutu) robota (yaw). Protože ale výstup tohoto regulátoru vede ke stejným motorům, budou se muset výsledky z obou PID regulátorů nějak vhodně "sloučit" aby se správně nastavovala rychlost každého kola zvlášť. To zatím ještě nevím přesně jak.
Pokud to správně chápu pak z MPU mi poleze pitch" a "kalAngleY" což je hodnota Input pro PID. Jako Output pak bude požadovaná hodnota 0 co pak je ale Setpoint a další hodnoty ? Počítám že výsledkem by mělo být číslo vyjadřující úhel o který se musí nakloněná rovina naklonit aby se dostala do roviny. Musím se ještě zamyslet jak motoru říct aby se posunul o přesný úhel, určitě to umí knihovna jen nevím z hlavy jak.
Otázkou je. Musí být PID kontroler pro každé kolo zvlášť ?
grrr. oficiální, udržovaná knihovna pro krokové motory nevím proč nepočítá s tím, že by někdo potřeboval natočit hřídel do přesného úhlu. Existuje customstepper knihovna co to umí ale ta zamrzla v čase tj. v roce 2012. Vyzkouším ji ale nevím jestli se raději napustím do toho že budu vypočítávat úhel sám.
AlesH
Příspěvky: 323
Registrován: 25 úno 2013, 09:18

Re: Balancující robot

Příspěvek od AlesH »

No, pro balancování ti stačí brát z filtru jen hodnotu "kalAngleY", což by měla být nejlepší dostupná okamžitá hodnota pro "pitch" (úhel náklonu dopředu/dozadu). Ten "kalAngleY" bude Input pro PID. Setpoint bude žádaná hodnota (v našem případě 0). Output by v našem případě měl odpovídat požadované rychlosti otáčení kol (obou současně). Nepotřebuješ řídit absolutní úhel pootočení kol. Stačí ta rychlost dopředu a dozadu.

Původně jsem myslel, že ta tebou odkazovaná PID knihovna možná nebude použitelná (přímo), ale po bližším prozkoumání ji použít lze takto:

Kód: Vybrat vše

#include <PID_v1.h>
double Setpoint, Input, Output, kalAngleY, rychlost_toceni_motoru;
double Kp=2, Ki=5, Kd=1;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

void setup()
{
  Input = 0; // predpokladame, ze robot stoji rovne
  Setpoint = 0; // toto je nase zadana hodnota naklonu
  myPID.SetMode(AUTOMATIC);
}

void loop()
{
  // kalAngleY = kalman.compute(); // nevim jak presne se Kalmanuv filtr vola v te knihovne pro MPU-6050
  Input = kalAngleY; // pred touto radkou musi byt volani Kalmanova filtru !!!
  myPID.Compute();
  rychlost_toceni_motoru = Output - 128; // hruby prevod z PWM vystupu na hodnotu se znamenkem
}
Ale ani obecně není PID vzorec složitý takže si ho klidně můžeme napsat sami. Jeden z nejlepších popisů PID v češtině jsem našel na http://robotika.cz/guide/control/cs . PID vzorec, upravený pro naše podmínky je pak tento (bez ošetření mezních hodnot):

Kód: Vybrat vše

e = zadany_naklon - kalAngleY;
sum_e += e;
rychlost_toceni_motoru = P * e + I * sum_e - D * (kalAngleY - predchozi_kalAngleY);
// P, I, D jsou konstanty, které je třeba určit odhadem a testováním
Také velmi dobrý popis Kalmanova filtru jsem našel na stejném serveru - http://robotika.cz/guide/filtering/cs .
Naposledy upravil(a) AlesH dne 26 čer 2015, 20:39, celkem upraveno 1 x.
JuraS.
Příspěvky: 318
Registrován: 28 led 2015, 16:06

Re: Balancující robot

Příspěvek od JuraS. »

Díky. vytisknu si to v práci a kouknu na to. Díky moc.
Zadal jsem zatím tu první možnost do kódu. Bez arduina, motorů i MPU6050 to prošlo kontrolou. Na cestě mám jednak shield arduina pro RPi a i jeho štít s dvěma A4988. Nicméně doma to zapojím zkušebně a dám vypisovat hodnoty a uvidím co z toho poleze. Nicméně pořádně si začnu hrát až budou potřebné součástky.
Uvědomil jsem si, že bude lepší když udělám samostatně PID proces pro levé a samostatně pro pravé kolo. Aby se mohl otáčet na místě. Třeba. Ty konstanty P, I D jsou co ? co určují ? výšku, hmotnost, počet prdů když sním cibulačku ? Co to je za hodnoty ?
Vzal bych vzorec odsud kde je pochopitelně napsaný a napsal ho 2x tj. pro pravé kolo a pro levé zvlášť. Kód jsem zatím upravil tak, že jsem proměnné dal samostatně, všechno co bylo v loopu jsem vzal a vytvořil samostatnou funkci co vyplivne ve výsledku hodnoty gyra a akcelerometru a to buď čisté po matematickém výpočtu a po kalmanovu filtru. samostatná funkce pak bude počítat PID pro každý motor. Chci to udělat tak aby se ta funkce dala rozšířit o proměnné co budou do PID procesu vstupovat když se budu chtít otočit a nebo když budu chtít jet dopředu. nevím co vyleze za číslo jako "Output" z toho PID procesu. Uvítal bych hodnotu úhlu.Krokové motory umí stát rovně oproti DC motorům byť s převodovkou.
Naposledy upravil(a) JuraS. dne 26 čer 2015, 12:28, celkem upraveno 3 x.
Odpovědět