Uvod Filip Parag i Milomir Stefanoviæ · 21 R 0 R M (, ) (,), y ... polju koji stvaraju kalemovi,...

Filip Parag i Milomir Stefanoviæ

Modeliranje, simulacija iimplementacijasamobalansirajuæeg robota

Cilj ovog rada je modeliranje, simulacija i iz-rada samobalansirajuæeg robota. Samobalansi-rajuæi robot je mehanièki sistem za kretanje poravnim površinama sa kojima je u kontaktupreko para toèkova na jednoj osovini. Osnovnafunkcionalnost ovakvog robota je samostalnoodr�avanje centra mase iznad ose rotacije. Pro-blem stabilizacije posmatran je kao problem sta-bilizacije inverznog fizièkog klatna, dok je kaopogonski motor korišæen DC motor. U simula-ciji, ugao otklona robota od pravca gravitacije,meren je na èetiri razlièita naèina: pomoæu�iroskopa, pomoæu akcelerometra, pomoæu �iro-skopa i akcelerometra zajedno u kombinaciji sakomplementarnim filterom i pomoæu �iroskopa iakcelerometra zajedno u kombinaciji sa Kalma-novim filterom. U hardverskoj implementacijikorišæeni su �iroskop i akcelerometar u kombi-naciji sa komplemetarnim filterom. U simulacijisu korišæene dve vrste stabilizatora: PID i LQR,dok je u hardveru implementiran samo PID reg-ulator. Uporeðene su performanse stabilizacijesa ovim stabilizatorima u odnosu na naèin esti-macije ugla. U simulaciji je dobijeno da je sta-bilizacija nemoguæa ukoliko se koristi samo�iroskop, dok je stabilizacija u ostalim sluèaje-vima moguæa. Najbolje performanse su dobijeneako se ugao otklona meri pomoæu �iroskopa iakcelerometra zajedno u kombinaciji sa Kalma-novim filterom. U hardveru nije bilo moguæeizvršiti potpunu stabilizaciju sistema zbog ve-likog slobodnog hoda motora, jer bi se robotstalno izvodio iz ravnote�nog polo�aja.

UvodSamobalansirajuæi robot je primer stabiliza-

cije dinamièkog sistema. Ovaj sistem se èestonaziva i problem stabilizacije inverznog klatna.Ovakvi roboti veæ su pravljeni za komercijalnuupotrebu, a neki od njih su: Segway, Emiew inBot. Takoðe, neki od njih, kao što je Segway,mogu se koristiti i za prevoz èoveka. Cilj ovograda je da se modelira, simulira i implementirasamobalansirajuæi robot.

Modeliranje i simulacija slu�e kako bi seevaluirale performanse i osobine regulatora ifiltera merenja sa senzora. Simulacija robotazahteva postojanje dinamièkog modela ovogrobota, odnosno model koji predstavlja fizièkeosobine robota. Na levoj strani slike 1 je pred-stavljen metod izrade simulacije. Kako bi robotmogao da se stabilizuje, potrebno je prvo izmeritiugao pod kojim je otklonjen, a nakon toga pri-meniti silu u vidu okretanja toèkova koja se opiredaljem naginjanju u stranu.

U ovom radu, za dobijanje ugla korišæeni suakcelerometar i �iroskop samostalno, kao i ukombinaciji upotrebom komplementarnog iKalmanovog filtra. Pred poèetak simulacije, ro-bot se postavlja da stoji pod odreðenim uglom, anakon toga se simulacija pokreæe. Tokom si-mulacije, jedan od dva poreðena regulatora –PID i LQR, je korišæen da se robot stabilizuje.

Za izradu hardverskog robota modelirani sudelovi šasije, koji se zatim štampaju na 3D štam-paèu. Nakon toga se sastavlja robot i implemen-tira se PID kontroler na mikrokontroleru (desnastrana slike 1). Koeficijenti PID stabilizatora sedobijaju na dva naèina – iz simulacije i Cigler--Nikols metodom.

ZBORNIK RADOVA 2017 PRIMENJENA FIZIKA I ELEKTRONIKA • 241

Filip Parag (1999), Suboticaučenik 3. razreda Gimnazije „JovanJovanović Zmaj” u Novom Sadu

Milomir Stefanović (1999), Kragujevac,učenik 3. razreda Prvekragujevačke gimnazije

MENTOR: Vuk Vukomanoviæ, studentElektrotehnickog fakulteta Univerziteta uBeogradu

242 • PETNIÈKE SVESKE 76 DEO I

Da bi se dobio ugao otklona pomoæu �iro-skopa mora se integraliti ugaona brzina koja seoèitava u svakom trenutku, a sa tom ugaonombrzinom integrali se i njena greška, što æe za po-sledicu veoma brzo imati netaènu vrednost te seizvodi prva hipoteza da stabilizacija samo po-moæu �iroskopa nije moguæa. Koristeæi samoakcelerometar stabilizacija je moguæa, što pred-stavlja drugu hipotezu. Pošto se oèekuje da se

korišæenjem akcelerometra i �iroskopa zajednopomoæu komplemetarnog filtera, kao i Kalma-novog filtera, dobija još precizniji podatak o uglu(Krsmanoviæ i Polovina 2010), komplemetarnifilter i Kalmanov filter bi trebali da daju manjaodstupanja od realne vrednosti u odnosu samo naakcelerometar, što je treæa hipoteza. Pošto jeKalmanov filter matematièki definisan kaooptimalni filter po datom kriterijumu, èetvrta

Slika 1. Blok šema sistema zastabilizaciju robota

Figure 1. Block diagram of therobot stabilization system

hipoteza se odnosi na to da æe odstupanja kodKalmanovog filtera biti manja nego kod ko-mplementarnog filtera. Što se tièe samih stabili-zatora, LQR je takoðe, kao i Kalmanov filter,definisan kao optimalni stabilizator te se oèekujeda æe preskok kao i period stabilizacije sa ovimstabilizatorom biti manji nego sa PID regula-torom. U hardveru bi trebalo da se doðe do iden-tiènih zakljuèaka kao i u simulaciji, što je šestahipoteza.

Modeliranje sistema

Dinamika robota

Kako bismo ispitali dinamiku sistemamoramo definisati koordinate delova robota kojeæemo posmatrati kao relevantne (slika 2). To sukoordinate centra mase (xc, zc) i centra toèka x,zatim ugao izmeðu horizontale i prave koja spajacentar mase i centar toèka (�) i ugaoni pomerajtoèka (�). Parametri koje æemo koristiti za opi-sivanje gore pomenutih parametara su: masa telarobota m, masa toèka mw, polupreènik toèka R,rastojanje izmeðu centra mase i centra toèka L,ugaoni momeat �, moment inercije tela I i mo-ment inercije toèka Iw.

Koristeæi Lagran�ov metod za opisivanjedinamike sistema mo�emo izraziti x, xc, zc, �x, �xc i�zc kao:

x R� �,x R L

c� �� sin ,

z R Lc� � cos �,

� �x R� �, (1)� �

� cosx R Lc� �� , (2)�

� sinz Lc� � � �. (3)

Dalje, mo�emo definisati potencijalnu ener-giju (èija je vrednost 0 u vertikalnom polo�aju) ikinetièku energiju kao:

E mg R L mg R Lp � � � � �( cos ) ( )�

� �mgL(cos )� 1 ,

Em x I mx mz I

kw w c c� � � � ��

�2 2 2 2 2

2 2 2 2 2� �

.(4)

Ubacivši jednaèine (1),(2) i (3) u jednaèinu(4) dobija se:

�E I m R mRk w w� � � �122 2 2

� ( )�

� �2 2mRL I mLcos � � � ( )�� .Suma kinetièke i potencijalne energije pred-

stavlja ukupnu energiju, tj. mehanièku energijudok razlika ove dve fizièke velièine daje Lagran-�evu energiju koja se definiše kao razlika kine-tièke i potencijalne energije, te je Lagran�evaenergija �:

�� 122 2 2

� ( )� I m R mRw w

� � � �2 12mRL I mL mgLcos � � � ( ) (cos )�� .Sada mo�emo izvesti jednaèine dinamike za

� i �.1. � :

��

� � ��

( ) � � cos� � � �I m R mR mRLw w2 2

�� 0,

ddt

I m R mRw w��

��

��

( ) ��

��

2 2

mRL mRL�� cos sin �� 2 .


Slika 2. Fizièki model samobalansirajuæeg robota

Figure 2. Physical model of a self-balancing robotwith starting position marked


2. � :

��

��

cos � ( ) �� mRL I mL2 ,

��

�� mRL mgLsin � � sin ,

ddt

I mL��

��

��

( ) ��

��

�

��

2

� � �mRL mRLcos �� sin�� .

Velièine� i� su ukupni ugaoni momenti kojiutièu na � i �.

Ovo se u matriènom obliku mo�e zaspisatikao:

I m m R mR

mRL mLw w� �

��

��

��( ) cos

cos

��

��

2

21�

��

� ��

��

��

��

00 0

0mRLmgL

sin � �� sin

��

��

.

Sada je potrebno da ukupne momente iz-razimo preko poznatih parametara. Ukupnimomenat dobijamo superpozicijom obrtnogmomenta i momenata statièkog i dinamièkogtrenja:

� � � � � �� M M M Ms d s d0 0 (��) � ,

� � � �� M M Ms s0 0 (��) ,

gde je �s

koeficijent statièkog trenja, a �d

– di-namièkog.

Ove relacije se mogu zapisati i matrièno i tokao:

��

� � ��

��

��

��

��

��

11 0

M s d s

s s

�

�

.

Linearizacija. Kako bi mogli dalje da reša-vamo ovaj sistem moramo da uvedemo nekeaproksimacije koje æe da va�e za ovaj sistem uopsegu za koji kreiramo kontroler pa æemo reæida je � jako mali ugao pa æe da va�i da je �� 0,�� 0, cos� �1 a sin � �� . Pa onda va�i sledeæe:

I m m R mRL

mRL mLw w� �

��

��

��( )

��

��

2

21��

��

��

��

��

��

� ��

� ��

s d s

s s mgL

�

�

0

��M0 .

Neka matrice sa koeficijentima se redom zo-vu A, B, C i D, tada je:

A B C DM��

��

�

�

��

��

��

�� 0 .

Ovo je matrièna jednaèina koja opisuje ceosistem.

Raèunanje centra mase. Ako celog robotapredstavimo kao sistem od dva tela: šipke du�ineL1 i mase m1 – telo robota i cilindra du�ine L2 imase m2 glava robota, onda se centar mase nalazina rastojanju:

LL L L m

m m� �

��

�2 1 2 1

1 23 2,

te je ukupan moment inercije robota:

I mL

L m L� ��

�� 1

12

2

2 22

21

12.

State-space model sistema. Neka je:

P A C� � �1 , Q A B� � �1 i R A D� � �1 ,

tada va�i da je:

dd t P P Q Q

��

�

�

( , ) ( , ) ( , ) ( ,

�

�

��

�

�

��

0 0 1 00 0 0 11 1 1 2 1 1 1 22 1 2 2 2 1 2 2

)( , ) ( , ) ( , ) ( , )

�

�P P Q Q

�

�

��

�

�

��

�

�

��

�

�

��

��

�

�

�

��

�

�

��

001 12 1

0R

R

M( , )( , )

,

yR

� ��

��

�

�

��

�

�

��

0 0 00 1 0 0

��

�

�

.

U skraæenom obliku ovo mo�emo zapisatikao:

ddt

E F Mx x� � � 0 ,

y G� x.

Dinamika motora

Kako vidimo iz opisa dinamike robota, kaoulaz u sistem potrebno je dati ugaoni momenat

( )M0 kako bi se izraèunao nagibni ugao robota utom trenutku. Pošto nije moguæe da mikrokon-troleru „ka�emo” da motor pravi toliki moment,moramo da izraèunamo kako ugaoni momenatzavisi od napona koji mo�emo da saopštimomikrokontroleru, pa zato moramo da izraèunamozavisnost ugaone brzine motora (ugaona brzinaje direktno srazmerna ugaonom momentu(M Iw0 � � ) od napona koji se dovodi na motor.

Motor proizvodi momenat M0 ako se na nje-ga dovodi napon V0 , taj momenat je direktnosramzmeran struji i koja prolazi kroz motor pava�i:

M k i0 � � . (5)

Pošto se rotor motora okreæe u magnetnompolju koji stvaraju kalemovi, u motoru jedno-smerne struje nastaje kontra elektromotorna sila,koja je direktno srazmerna ugaonoj brziniokretanja rotora motora i koeficijenta samoin-dukcije, pa va�i:

V ki i� ��. (6)

Motor æemo modelirati kao rednu vezu otpo-rnika i induktora, te se mo�e napisati II Krihofovzakon za to kolo kao:

� � � �Ri L it

dd

0.(7)

U jednaèini kretanja motora trenje osovinemotora se aproksimira kao linearna funkcijaugaone brzine motora. Njutnov zakon kretanjagovori da je suma svih momenata koji deluju naosovinu motora direktno srazmerna inercijalnomoptereæenju namotaja motora Ir što se mo�e pred-staviti kao:

M M I Im r� � � � � 0 �� , (8)gde je M

0moment koji motor a I

mmoment iner-

cije motora.Ubacivanjem jednaèina 6 i 5 u jednaèine 7 i 8

dobija se:

dd

i

t

R

Li

k

L Li� � �� ,

dd� �t

k

Ii

k

I

I

Ir

e

r

m

r

� � � .

Za potrebe modeliranja motora u ovom slu-èaju moguæe je induktivnost, kao i silu trenja,zanemariti, te je onda jednaèina koja opisuje radovog motora:

��

� ��kk

I R

k I R

I Ri

r

m

r

.(9)

U jednaèini 9 ulaz u sistem predstavlja napon�, a izlaz ugaona brzina � .

Senzor

Inicijalni problem pri konstruisanju robota jeda se odredi ugao pod kojim je on nagnut; to semo�e uraditi pomoæu akcelerometra, �iroskopaili oba senzora zajedno.

Akcelerometar. Akcelerometar je elektro-mehanièki senzor koji se koristi za merenje ubr-zanja. On meri ubrzanje u zavisnosti od sila kojedeluju na njega. Te sile mogu biti statièke i di-namièke. Primer statièke sile je gravitaciona sila,a dinamièke mogu biti prouzrokovane kretanjemrobota ili samim vibracijama akceletometra.Prostim trigonometrijskim transformacijama odubrzanja koje akcelerometar meri dobija se ugaopod kojim se nalazi robot. Problem kod ovogsenzora je sto æe male vibracije akcelerometraprouzrokovati grešku merenja ovakvog ugla alise ova greška mo�e otkloniti primenom filteraniskih uèestalosti kod implementacije sa kom-plementarnim filterom ili pomoæu Kalmanovogfiltera. Ovaj filter propušta samo niske frekven-cije dok one više frekvencije odstranjuje. Na ovajnaèin se smanjuje uticaj vibracija akcelerometra isamim tim su preciznija oèitavanja.

�iroskop. �iroskop je senzor koji se koristiza merenje ugaone brzine. Ugao za koji je robototklonjen dobija se numerièkom integracijomizmerene ugaone brzine. Zbog same integracijekod �iroskopa javlja se problem akumuliranjagreške i offseta. Na primer, dešava se da �iroskopza nultu vrednost ugaone brzine pokazuje nekunenultu vrednost, jer senzor ima offset, a ovajproblem utièe na akumulaciju greške numeriè-kom integracijom. Ovaj problem mo�e se deli-mièno rešiti primenom filtera visokih uèestalostikod implemetacije sa komplementarnim filteromili Kalmanovim filterom. Ovaj filter radi sup-rotnom logikom od prethodno pomenutog filtera,tj. on propušta samo visoke frekvencije dok oneniske odstranjuje. Na ovaj naèin posti�u se pre-ciznija merenja sa �iroskopa.

Modeliranje senzora

Kako mikroelektromehanièki senzori(MEMS) korišæeniu ovom projektu nisu idealni,



za verodostojnu simulaciju je potrebno mo-delirati senzore sa njihovim fizièkim osobinama.

Karakteristike greške

Stalna greška. Stalna greška je greška kojaje prisutna sve vreme i raste linearno. Ova greškase mo�e izraèunati merenjem proseène vrednostimerenja u du�em vemenskom intervalu kada nasenzor ne deluju spoljašnje sile. Gravitacionusilu je nemoguæe izbeæi, pa je zato taèna orijen-tacija senzora veoma va�na za njeno oduzimanjeod signala.

Termo-mehanièki šum. Termo-mehanièkainterferencija se javlja kao nuspojava termo--elektriènog šuma koji osciluje na mnogo veæojfrekvenciji od frekvencije odabiranja senzora;posledica je da se u merenju javlja šum koji sejavlja u vidu sluèajne šetnje.

Treptajni šum. Zbog treperenje u elektro-nièkim delovima, treptajni šum se javlja na ni-skim frekvencijama spektra izmerenog signala uvidu belog šuma.

Temperaturni efekti. Ukoliko se tempera-tura menja tokom rada senzora, dolazi do širenjai skupljanja komponenti, što direktno utièe nafluktuacije greške senzora. zavisnost greške odtemperature se razlikuje kod senzora i uglavnomnije linearna.

Ova vrsta greške je zanemarena u simulacijijer sistem nije predviðen da se kreæe u prostorupromenljive temperature.

Projektovanje upravljanja

Filtri

U praksi se radi dobijanja preciznijih rezul-tata koriste razlièite vrste filtera za kombinacijuizmerenih vrednosti u cilju dobijanja što preciz-nijeg �eljenog podatka – u našem sluèaju uglaotklona.

Komplemetarni filter. Komplementarnifiltar (Krsmanoviæ i Polovina 2010) je vrsta filtrakoja kombinuje merenja više senzora u unapredodreðenim proporcijama. Prvo se raèunaju ug-lovi koji daju samo akcelerometar i �iroskop.Trenutni ugao akcelerometra je:

�accelaccelmerenje� arccos

mg.

Dok je trenutni ugao �iroskopa:

� �gyro gyro gyromerenje d( ) ( ( ) ( ))t t t t� � �1 .

Kada se izraèuna potrebno je podatke saakcelerometra filtrirati filterom niskih uèesta-losti, odnosno podatke sa �iroskopa filteromvisokih uèestalosti.

Niskofrekventni filtar. Upotreba ovog filtraima za cilj da propusti samo niskofrekventnepromene signala sa senzora na izlaz filtera, a daone visokofrekventne ukloni. Na primer, ukolikoje ugao koji oèitava akcelerometar nula i nagloskoèi na deset, filer bi trebao da reaguje na sle-deæi naèin: izlaz filtra ne pokazuje odmah ugaood deset stepeni, veæ se taj ugao postepeno pove-æava, a ako se objekat du�e vremena zadr�i u tompolo�aju, na izlazu filtra æe posle nekog vremenabiti deset stepeni. Samo dugotrajne promenesignala se propagiraju na izlaz filtra. Nedostatakovog filtra je kašnjenje koje se javlja zbog poste-pene, a ne skokovite promene izlaza.

Jednaèina niskofrekventnog filtera je:

� � �( ) ( ) ( )t k k t� � � �1 1 1 ,

gde je � ugao, koji se dobija na izlazu niskofrek-ventnog filtra, � ugao koji se dobija sa akcelero-metra i koji se dovodi na ulaz niskofrekventnogfiltra i k konstanta niskofrekventnog filtra.

Visokofrekventni filter. Ovaj filter radi pot-puno suprotno od niskofrekventnog. Propuštavisokofrekventne promene na izlaz, dok one ni-skofrekventne eliminiše. Uloga visokofrek-ventnog filtra je da otkloni proces akumulacijegreške usled integracije, odnosno da minimizujeovaj efekat. Jednaèina ovog filtera je:

� �( ) ( ( ) ( ))t k t t t� � � �1 1gyro gyromerenje d ,

gde je k koeficijent visokofrekventnog filtera, �izlaz visokofreventnog filtera a sa „merenje” jeoznaèeno oèitavanje ugla sa �iroskopa.

Nakon što se dobiju uglovi koji daju akcele-rometar (�accel) i �iroskop (�gyro) ponaosob, dobi-jene vrednosti se saberu i podele sa 2 i takodobijeni broj (�) je izlaz komplementarnog filte-ra:

��

��accel gyro2

.

Kalmanov filter. Za razliku od komplemen-tarnog filtera, Kalmanov filter za estimaciju uglaotklona koristi i dinamièki model sistema.

Kalmanov filter ili linearno-kvadratni esti-mator ima veoma široku primenu u industrijivozila zbog moguænosti da daje veoma precizne

estimacije ukoliko je dinamièki model verodo-stojan fizièkom sistemu.

Algoritam filtesra je podeljen u dva povezanaprocesa. U procesu predviðanja se procenjujetrenutno stanje na osnovu dinamièkog modelapredstavljenog matricom promene stanja A,kontrolnom matricom koja predstavlja promenestanja unete pokretanjem aktuatora i kontrolnimvektorom

�

un koji predstavlja trenutni signal aktu-atora. Predviðeno trenutno stanje

�

x p je:� � �

x Ax Bup n n� ��1 ,

gde je�

xn�1 stanje sistema dobijeno u prethodnoj

iteraciji algoritma. Poèetno stanje sistema, od-nosno

�

x0

u prvoj iteraciji, je oèekivano stanje sis-tema pre pokretanja.

Pored estimacije stanja, u prvom procesu seprocenjuje i kovarijansa predviðanja, odnosnomera greške predviðenog stanja. Kovarijansapredviðanja Pp je:

P AP A Qp n� ��1T ,

gde je Pn�1 kovarijansa iz prethodne iteracije, a Q

je procenjena greška kovarijanse procesa, odno-sno greška u korelaciji izmeðu zadatog signala ipokretanja aktuatora. Ova greška predstavljaproklizavanje toèkova ili prazan hod u prenosumomenta impulsa od motora do toèkova.

Drugi proces algoritma uvodi merenja sa sen-zora. U prvom koraku se raèuna inovacija ~y kojapredstavlja razliku predviðenog stanja i stanjadobijenog oèitavanjem senzora. Izmerene vred-nosti sa senzora predstavljaju vektor merenja

�

zn ,a opservaciona matrica H preslikava vektor sta-nja u vektor merenja. Inovacija je predstavljenakao:

~y z H xn p� ��

.

Slièno prvom procesu, raèuna se kovarijansainovacije S koja predstavlja stvarnu grešku uodnosu na predviðanje:

S HP H Rp� �T ,

gde je R procenjena greška kovarijanse merenja.Kalmanovo pojaèanje je relativno pojaèanje

merenja i predviðenog trenutnog stanja i mo�e sepodešavati za dobijanje �eljenog uèinka. Sa vi-sokim pojaèanjem, filtar dodeljuje veæu pou-zdanost skorašnim merenjima, a nisko pojaèanjeprvenstvo daje dinamièkom modelu. U eks-tremnim sluèajevima, pojaèanje koje je blizu je-dinice æe imati veoma nagle promene u stanjima,

dok æe pojaèanje blizu nule imati mali šum, ali æestanje biti veoma inertno. Kalmanovo pojaèanjese raèuna kao:

K P H Sp��T 1 .

U poslednjem koraku se raèuna krajnja pro-cena trenutnog stanja xn :

� �

x x Kyn p� �~

i a�urira se kovarijansa predviðanja:

P I KH Pn p� �( ) ,

gde je I matrica identiteta.U ovom projeku su korišæena dva senzora,

akcelerometar i �iroskop, pa je vektor merenja:

�

zn �

�

�

��

�

�

��

��

�

accel

accel

gyro

gyro

�

�

.

Kontrolni vektor je�

u Vn � motor i predstavljanapon signala motora.

Opservaciona matrica je:

H �

�

�

��

�

�

��

1 0 0 00 1 0 01 0 0 00 1 0 0

.

Procenjena greška kovarijanse procesa i pro-cenjena greška kovarijanse merenja su:

Q �

�

�

��

�

�

��

0 0 0 00 0 0 00 0 0 00 0 0 0

,

R �

�

�

��

�

�

��

0 1 0 0 00 0 1 0 00 0 0 1 00 0 0 0 1

..

..

.

Poèetna vrednost matrice kovarijanse predvi-ðanja je:

P �

�

�

��

�

�

��

1 0 0 00 1 0 00 0 1 00 0 0 1

.



Stabilizatori

PID. U trenutku kada robota izvedemo izravnote�nog polo�aja, on æe pasti na stranu nakoju je otklonjen, osim ako neka sila ne poène dadeluje na robota u suprotnom smeru kako bismanjila ugao otklona. Sila koja robota vraæa uravnote�ni polo�aj je inercijalna sila izazvanaradom motora, tj njegovim okretanjem toèkova.Zatim je potrebno da se na neki naèin izraèunakako brzina koja treba da se preda motoru zavisiod ugla otkolna. Jedan od naèina je da se koristiPID kontroler. PID kontroler se sastoji od tri po-sebna kontrolera: P (proporcijalni), I (inte-gracioni) i D (derviacioni, odnosno izvodni)kontroler. Svaki od kontrolera prima grešku, tj.odstupanje trenutne vrednosti od �eljene vred-nosti, a na izlazu daje napon koji se šalje na mo-tor kako bi se zatvorila povratna sprega. Šemarada PID kontrolera prikazana je na slici 3.

Jednaèina P kontrolera je: V KP� �error

Jednaèina D kontrolera je: V KtD

� � d errord

Jednaèina I kontrolera je: V KI� � errorSamim tim jednaèina PID kontrolera je:

V K Kt

KP D I� � � � � � error d errord error

Koeficijenti KP , KD i KI mogu da se dobiju naviše naèina. Dva najpoznatija naèina jesu posma-tranje root locusa sistema i Cigler-Niklsonovmetod. U ovom radu posmatran je root locusasistema i dobijeno je da je KP = 15.3, KI = 0.7 iKD = 13.

LQR. Teorija optimalne kontrole bavi se mo-deliranjem kontrolera tako da oni matematièki

ispunjavaju idealne uslove za stabilizaciju jed-nog dinamièkog modela po datim instrukcijama.Sistem koji je opisan diferencijalnim jedna-èinama prvog reda i kod koga je funkcija greškekvadratna funkcija naziva se LQ problemom.Jedno od najpoznatijih rešenja ovog problemapredstavlja LQR regulatorom, koji zatvara pov-ratnu spregu po stanjima. Podešavanje regulatorakoji regulišu neku mašinu ili proces zarad posti-zanja �eljene vrednosti vrši se tako da se mini-mizuje funkcija greške. Ona se mo�e razlièitoraèunati od sluèaja do sluèaja. Ako pokušavamoda dostignemo temperaturu od 100 K, atemperatura je 90 K, greška æe biti 10 K, a akopokušavamo da vidimo kako jedna prava fitujeskup nekih taèaka, grešku æemo raèunati kaosumu kvadrata odstupanja taèke od fitovaneprave. U našem sluèaju greška æe biti ugao podkojim je robot nagnut. Prednost LQR je to što sepodešavaju parametri kontrolera, a ne samkontroler, kao što je sluèaju sa PIDom. ParametriLQR kontrolera su matrice Q i R na osnovu kojihse raèuna gain koji se naziva još i vektor K i ondase povratna sprega zatvara po formuli:

u Kx� �

gde je x vektor stanja sistema.U ovom radu je:

Q �

�

�

��

�

�

��

1 0 0 00 1 0 00 0 1 00 0 0 1

.

Hardverska konfiguracijarobota

Sa slike 2 jasno se vidi da centar mase morabiti na veæoj visini od motora, ali ne veæoj od10 cm, da masa robota bude manja od 0.5 kg i daotklonjeni ugao ne bude preveliki da bi va�ilaaproksimacija (ne veæi od 6 deg) (Ding et al.2012) jer je onda moment sile zemljine te�etoliko veliki da ga inercijalna sila, koju motoriizazivaju ubrzanim kretanjem u smeru u kom jerobot otklonjen, ne mo�e stabilizovati. Ako saoznaèimo minimalni moment sile koji motorimoraju „savladati” onda va�i:

M mg L� � � �sin � 0.05 Nm.

Slika 3. Zatvoreni sistem upravljanja sa PIDkontrolerom

Figure 3. Closed-loop control system with PIDcontroler

Motori. Pošto imamo jedan motor, on trebada stvori moment sile od najmanje 0.05 Nm atakoðe je potrebno je da ugaona brzina motorabude najmanje 400 obrtaja po minutu.

H most. H most je elektronsko kolo kojeomoguæuje kontrolu smera struje kroz elektro-motor ili kroz neki drugi potrošaè. Promenomsmera struje kroz armaturu jednosmernog mo-tora menja se smer rotacije njegove osovine. Hmost šematski mo�emo predstaviti kao na slici 4.Zatvaranjem dijagonalnog para 1-4 a otvaranjemdigitalnog para 2-3 izaziva kretanje motora su-protno od smera kazaljke na satu, a otvaranjemdigitalnog para 1-4 a zatvaranjem para 2-3 smerkretanja motora je u smeru kazaljke na satu.

IMU. Kako bismo imali podatak o ugaonompolo�aju robota ovaj ugao je potrebno izmeriti, uovom sluèaju pomoæu inercijalno-meganièkejedinice (IMU). IMU na sebi ima 2 senzora:akcelerometar i �iroskop. Akcelerometar merite�inu, a �iroskop ugaonu brzinu po 3 ose. Fi-zièkim transformacijama ovih fizièkih velièina

dobija se podatak o uglu otklona, što æe biti obja-šnjeno kasnije. Komunikacija sa mikrokontro-lerom vrši se preko UART protokola. IMU senalazi na vrhu robota kako bi se postigla najboljarezolucija za merenje nagibnog ugla robota.

Mikrokontroler. Za oèitavanje podataka sasenzora i kontrole brzine motora potrebno jekoristiti, mikrokontroler, mali programabilniraèunar. Mikrokontroler koji je korišæen u ovomsluèaju je Arduino Mega. Takoðe za ovaj mikro-kontroler mogu biti vezani i još neki LCD ekrani,prekidaèi, LED diode kao indikatori promene, asve u zavisnosti od trenutne potrebe. U ovomsluèaju postavljene su 3 potenciometra kako bi sepodešavali koeficijenti PID stabilizatora.

3D model robota

Šasija robota je modelirana na raèunaru, uprogramu SektchUp Pro, kako bi robot bio štoviše simetrièan. Svaka nesimetriènost po osinormalnoj na osu otklona dovodi do dodatnih od-stupanja od inverznog klatna, što znaèi da robotte�iti ka padanju u jednu stranu. Sve kompone-nte, ukljuèujuæi i DC motor, se svojim centrimamase nalaze u osi osovine toèkova (slika 5).

Fizièka ogranièenja štampaèa. Kako je ko-rišæeni 3D štampaè ogranièen po horizontalnim


Slika 4. H-most

Figure 4. H-bridge

Slika 5. Boèni prikaz robota

Slika 5. Side view of the robot


osama na 20 cm i po vertikalnoj osi na 18 cm,potrebno je bilo izraditi sve horizontalne potpo-rne ploèe iz dva dela. Vrsta materijala korišæenaza izradu šasije je polilaktièna kiselina, poznatijakao PLA. Svi delovi šasije su napravljeni tako dase meðusobno prièvrste pomoæu M3 šrafova èijije preènih oko 2.9 mm.

Delovi šasije. Na slici 6 se mogu videti mo-deli svih delova šasije. Ispunjenost PLA fila-mentom je kod dr�aèa bila 15%, a kod potpornihploèa 10%. Spoljašnjost je sadr�ala tri slojafilamenta. Nakon sklapanja, šasija te�i oko polakilograma i mo�e se videti na slici 7.

Toèkovi i aktuatori. Jednosmerni motor,koji se koristi kao aktuator, je povezan na toèko-ve preko menjaèke kutije. Na ovaj naèin gubi sena trenutaènosti i brzine prenosa, ali se dobija naslabijim trzajima osovine i manjoj potrebnojsnazi motora. Toèkovi su èvrsto povezani metal-nom osovinom, a šasija motora je povezana saosovinom pomoæu dva kuglièna le�aja.

Napajanje. Na robotu se nalaze dve baterije:jedna baterija na 15 V sa moguænošæu dosta-vljanja visokih struja i druga baterija tipa PP3 na9 V. Aktuator je indirektno povezan na mikro-kontroler preko h-mosta i koristi bateriju od 15 Vza napajanje. Mikrokontroler je preko DC-DCkonvertora povezan na bateriju od 9 V na ulazninaponski pin.

Rezultati

U ovom radu ispitivano je kako preskok ipeiod stabilizacije zavise od primenjenog regu-latora, kao i kako vrsta senzora utièe na stabi-lizaciju. Za svaki od regulatora prikazano je poèetiri odskoèna odziva i to:

1. Odskoèni odziv kada se za podatak o uglukoristi samo �iroskop

Slika 6. Gornji prikaz štampanih delova šasije

Figure 6.Top view of all chassis parts

Slika 7. 3D prikaz sklopljenog robota

Figure 7. 3D render of the assembled robot

2. Odskoèni odziv kada se za podatak o uglukoristi samo akcelerometar

3. Odskoèni odziv kada se za podatak o uglukoriste �iroskop i akcelerometar zajednosa komplementarnim filterom

4. Odskoèni odziv kada se za podatak o uglukoriste �iroskop i akcelerometar zajednosa Kalmanovim filterom

Simulacija

Na slikama 8 i 9 prikazani su odskoèni odzivisistema kada se koristi samo �iroskop. Mo�e se

primetiti da �iroskop jako brzo integrali greškumerenja što za rezultat ima da æe �iroskop poslejedne sekunde pokazivati vrednosti preko ! tesamim tim stabilizacija sistema u ovom sluèajunije moguæa bez obzira na vrstu regulatora.

Na slikama 10 i 11 prikazani su odskoèni od-zivi sistema kada se koristi samo akcelerometar.Mo�e se primetiti da u ovom sluèaju signal imaneku konstantnu grešku u odnosu na taènu vred-nost, kao i da je taèna vrednost zašumljena višenego u sluèajevima kada se koristi neki od filtera,što je u skladu sa hipotezom. Konstantna greškase mo�e ukloniti kalibrisanjem akcelerometra pa


Slika 8. Zavisnost nagibarobota od vremena sa PIDregulatorom i �iroskopom usimulaciji

Figure 8. Robot deflection asa function of time using PIDcontroller and gyroscopeinside the simulationDashed line – theoreticalFull line – experimental

Slika 9. Zavisnost nagibarobota od vremena sa LQRregulatorom i �iroskopom usimulaciji

Figure 9. Robot deflection asa function of time using LQRcontroller and gyroscopeinside the simulationDashed line – theoreticalFull line – experimental


se ovaj efekat mo�e zanemariti. Takoðe, prime-æuje se da do stabilizacije dolazi, tako da je i tupotvrðena hipoteza. Takoðe, primeæuje se da ješum dosta veæi koristeæi LQR nego PID, što jelogièno jer se trenutni ugao mnogo br�e menjakada se koristi PID nego LQR (treptajni šum), alizato kada se koristi PID preskok postoji, dok kodLQR preskok ne postoji.

Na slikama 12 i 13 prikazani su odskoèni od-zivi sistema kada se koriste i akcelerometar i�iroskop sa komplementarnim filterom. Prime-æuje se da se u odnosu na �iroskop greška ne

integrali, a da je u odnosu na akcelerometarstalna greška manja, kao i šum. Takoðe, kao i usluèaju sa akcelerometrom, šum je veæi kada sekoristi LQR nego PID zbog treptajnog šuma.Šum u ovom sluèaju je manji nego kada se koristisamo akcelerometar. Takoðe, do stabilizacije do-lazi bez obzira na vrstu regulatora.

Na slikama 14 i 15 prikazani su odskoèni od-zivi sistema kada se koriste i akcelerometar i �i-roskop sa Kalmanovim filterom. Primeæuje se dase u odnosu na �iroskop greška ne integrali a daje u odnosu na akcelerometar stalna greška

Slika 10. Zavisnost nagibarobota od vremena sa PIDregulatorom iakcelerometrom u simulaciji

Figure 10. Robot deflectionas a function of time usingPID controller andaccelerometer inside thesimulationDashed line – theoreticalFull line – experimental

Slika 11. Zavisnost nagibarobota od vremena sa LQRregulatorom iakcelerometrom u simulaciji

Figure 11. Robot deflectionas a function of time usingLQR controller andaccelerometer inside thesimulationDashed line – theoreticalFull line – experimental

manja, kao i šum što je bio sluèaj i kod komple-mentarnog filtera, ali je kod Kalmanovog filteraovaj šum još manje izra�en. Do stabilizacije do-lazi sa najmanjim šumom od posmatrana èetirisluèaja bez obzira na vrstu regulatora, što je uskladu sa hipotezom. Takoðe i u ovom sluèajuzbog treptajnog šuma kod LQR regulatora je šumveæi nego kod PID regulatora.

Što se tièe performansi regulatora meðu-sobno, period stabilizacije je slièan kod oba,samo što preskok kod LQR ne postoji, dok kodPID regulatora preskok iznosi preko 10%, štoznaèi da se LQR se pokazao kao bolji.

Hardver

Za razliku od imelementacije u simulaciji, uhardveru je implementiran samo sistem sa PIDregulatorom gde se za podatak o uglu koriste obasenzora ukombinovana sa komplementarnimfilterom. Kao problem u ovom sistemu pojaviose preveliki slobodan hod motora koji je kori-šæen, što za posledicu ima da æe sam slobodanhod motora da izvede robota iz ravnote�nog po-lo�aja æime æe stabilizaciju onemoguæiti ili za-poèeti ponovo proces stabilizacije, što se mo�e


Slika 13. Zavisnost nagibarobota od vremena sa LQRregulatorom ikomplementarnim filtrom usimulaciji

Figure 13. Robot deflectionas a function of time usingLQR controller andcomplementary filter insidethe simulationDashed line – theoretical

Full line – experimental

Slika 12. Zavisnost nagibarobota od vremena sa PIDregulatorom ikomplementarnim filtrom usimulaciji

Figure 12. Robot deflectionas a function of time usingPID controller andcomplementary filter insidethe simulationDashed line – theoreticalFull line – experimental


primetiti na slici 16 gde se problem dešava u de-vetoj sekundi. Iz navedenog razloga dat je jedinotako dobijeni rezultat.

S obzirom na opisane rezultate zakljuèuje seda je stabilizacija pomoæu �iroskopa nemoguæabez obzira na vrstu stabilizatora, èime je potvr-ðena prva hipoteza. Takoðe primeæeno je da jestabilizacija samo pomoæu akcelerometra mogu-æa, ali da su je odstupanje oèitane od oèekivanevrednosti veæa nego kada se koriste komple-mentatni filter i Kalmanov filter, èime su potrvr-ðene druga i treæa hipoteza. Kalmanov filter jedao manja odstupanja izmerenih vrednosti uglaod realnih vrednosti u odnosu na komplemen-

tarni filter, èime je èetvrta hipoteza potvrðena.Uporedivši performanse PID i LQR stabilizatoraprimeæeno je da je preskok minimalan kod LQR,ali da se periodi stabilizacije ne razlikuju bitno,èime je peta hipoteza delimièno potvrðena. Šestahipoteza nije potvrðena jer nije sve opisanoimplmentirano i na hardveru. Kako bi ona moglada se razmatra dalji rad na projektu treba usred-srediti na to da se promeni DC motor koji jekorišæen zbog velikog slobodnog hoda motora.Takoðe, dalji rad mo�e da ide i tom smeru da seposmartaju osobine stabilizatora kada se centarmase robota spušta, tj kada se robot optereæuje.

Slika 15. Zavisnost nagibarobota od vremena sa LQRregulatorom i Kalmanovimfiltrom u simulaciji

Figure 15. Robot deflectionas a function of time usingLQR controller and Kalmanfilter inside the simulationDashed line – theoreticalFull line – experimental

Slika 14. Zavisnost nagibarobota od vremena sa PIDregulatorom i Kalmanovimfiltrom u simulaciji

Figure 14. Robot deflectionas a function of time usingPID controller and Kalmanfilter inside the simulationDashed line – theoreticalFull line – experimental

Zahvalnost. Zahvaljujemo se Marku Ska-kunu i Nemanji Filipoviæu za pomoæ u izradihardvera, kao i Danilu Ðokiæu za pomoæ priimplementaciji Kalmanovog filtera.

LiteraturaDing Y., Gafford J., Kunio M. 2012. Modeling,simulation and fabrication of a balancing robot.Advanced System Dynamics and Control, 151: 1.

Friedland B. 2005. Control system design: anintroduction to state-space methods. DoverPublications

Huang S. 2010. Extended Kalman Filter. ARCCentre of Excellence for Autonomous Systems(CAS), Faculty of Engineering and InformationTechnology University of Technology Sydney,Australia 2010

Krsmanoviæ S., Polovina V. 2011. Balansirajuæirobot. Petnièke sveske, 69: 123.

Filip Parag and Milomir Stefanoviæ

Modeling, Simulation and HardwareImplementation of a Self-balancingRobot

The aim of this paper is the modeling, simula-tion and implementation of a self-balancing ro-bot. The problem of a self-balancing robot isconsidered as a problem of inverted pendulumstabilization, while a DC motor is used as thedrive motor.

The deflection angle indicates the angle bywhich the robot is inclined to a side, in otherwords, by how many degrees the y-axis of the ro-bot is rotated from the absolute y-axis, so that thecenter of wheels is exactly at the section of theseaxises.

In the simulation, the deflection angle is mea-sured in four ways: using a gyroscope, an accel-erometer, both gyroscope and accelerometer incombination using a complementary filter. andboth gyroscope and accelerometer in combina-tion using Kalman filter. These sensors areplaced on the apex of the robot in order to maxi-mize the amplitude of measured values. Twotypes of stabilization controllers are used: PIDand LQR. Respective performances of these reg-ulators are compared relative to the way that theangle is obtained.


Slika 16. Zavisnost nagibarobota od vremena sa PIDregulatorom ikomplementarnim filtrom

Figure 16. Robot deflectionas a function of time usingPID controller and Kalmanfilter


Results of the simulation in which only a gy-roscope is used indicate that this sensor accumu-lates measuring error exceptionally fast, whichresults in a value over !after just one second. Be-cause of this, stabilization is not possible regard-less of the regulator.

In the case of measuring using an accelerom-eter, a constant measuring error is observed andthe presence of noise is more apparent than whena filter is used. Furthermore, the noise’s ampli-tude is greater with the LQR regulator as a conse-quence of flickering noise, which is expectedbecause the current deflection angle changes at aquicker rate using PID. Contrary to LQR, stabili-zation using PID regulator has overshoot.

When a complementary filter is used, the gy-roscope error accumulation is not observed andboth constant error and noise of the accelerome-ter are less pronounced. Similarly to the previouscase, noise is more apparent with LQR, butoverall noise is lower.

When the Kalman filter is used, properties ofthe error and noise are similar to those from thecomplementary filter, except that the noise is of alesser amplitude. Regardless of the regulatortype, stabilization is achieved with the lowest

noise of all four cases. Noise is again lower whenLQR regulator is used due to the flickering noise.

All simulation hypotheses set during the ini-tial problem analysis and modeling are con-firmed: the robot can successfully stabilizeregardless of regulator type, except when mea-surements are only acquired from a gyroscope.

Unlike in the simulation, the PID regulator isonly implemented in hardware where the mea-surements represent output of a complementaryfilter of both the gyroscope and the accelerome-ter. A major setback in the hardware systememerged as significant free movement in transferof the motor torque to the wheel axle; as a conse-quence, the robot can be displaced from balanceand disable any further stabilization. The systemis successfully put into stable oscillatory move-ment, but the stabilization is not possible and thehypothesis is not confirmed.

To review the plausibility of this hypothesis,further work should focus on removing the freemovement in mechanical transfer. Also, stabi-lizer performance should be observed when thecenter of the robot’s mass is shifted downwards,in other words, when additional weight is addedto its base.

Uvod Filip Parag i Milomir Stefanoviæ · 21 R 0 R M (, ) (,), y ... polju koji stvaraju kalemovi,...

Documents

Transcript of Uvod Filip Parag i Milomir Stefanoviæ · 21 R 0 R M (, ) (,), y ... polju koji stvaraju kalemovi,...