Vodenje invertiranega sferičnega nihala z robotom tipa SCARA
Žiga PETRIČ, Igor ŠKRJANC
Izvleček: Prispevek raziskuje možnosti modeliranja in simulacije sistemov na PLC krmilniku proizvajalca Allen-Bradley in pripadajoči programski opremi. Za nalogo smo izbrali robot tipa SCARA, ki ima na prijemalo pritrjeno invertirano sferično nihalo (to je nihalo, ki je prosto gibljivo v dveh prostostnih stopnjah). Naloga robota je, da zaniha nihalo v navpično lego in jo vzdržuje, medtem pa se lahko giblje v delovnem območju in izvaja druge operacije (npr. pobiranje in odlaganje predmetov). Delo vključuje izdelavo matematičnega modela nihala, algoritem za zanihanje nihala v navpično lego, regulator za vodenje nihala, simulacijo in 3D vizualizacijo celotnega sistema. Narejen je tudi grafični vmesnik (HMI) za upravljanje s poskusom in nastavljanje različnih parametrov.
Ključne besede: Invertirano nihalo, sferično nihalo, SCARA robot, napredno vodenje, simulacija, 3D izris, Allen-Bradley
■ 1 Uvod
Cilj dela je preučiti, uporabiti in ovrednotiti nekatere napredne funkcije, ki jih ponuja oprema proizvajalca Rockwell Automation (Allen-Bradley), vendar so pri realizaciji praktičnih projektov večinoma neizkoriščene. Te funkcije so krmilnik gibanja (motion control), inverzna in direktna kinema-tika za različne tipe robotov, napredno vodenje, možnosti simulacije in modeliranja sistemov. Nastala naj bi demonstracijska in učna platforma, uporabna za izobraževalne namene in za prikaz zmožnosti krmilnega sistema potencialnim uporabnikom.
Fizična naprava je za takšen namen nepraktična, ker ni prenosljiva, poleg tega pa potrebujemo delovno mesto za več udeležencev šolanja ali predstavitve hkrati.
Demonstracijska naprava naj bo torej modelirana in simulirana, vendar do te mere, da so rezultati hitro in z minimalnimi posegi prenosljivi na realno, fizično napravo. V tem smislu je izbra-
Žiga Petrič, univ. dipl. inž., Tehna, d. o. o., Ljubljana Prof. dr. Igor Škrjanc, univ. dipl. inž., Univerza v Ljubljani, Fakulteta za elektrotehniko
na platforma za simulacijo progra-mabilni krmilnik (proizvajalca Allen-Bradley, družina CompactLogix), za vizualizacijo pa smo izbrali program RSTestStand (Rockwell Automation).
Kot delovno nalogo smo izbrali invertirano nihalo na vozičku, ki je klasičen referenčni sistem za preizkušanje različnih sistemov vodenja (slika 1.) [1].
Slika 1. Shema invertiranega nihala na vozičku
■ 2 Invertirano nihalo kot eksperimentalna platforma
Praktično je tak sistem navadno izdelan kot laboratorijska naprava za izobraževalne namene, ki jo sestavlja:
•	linearno vodilo (dolžine 1 do 2 m);
•	voziček s pritrjenim nihalom (0,2 do 1 m);
•	DC elektromotor, ki žene voziček direktno ali preko jermena;
•	napetostno krmiljen ojačevalnik (driver) za motor;
•	dva optična linijska dajalnika (en-koder) za merjenje kota odmika nihala in pomika vozička, z vezjem za pretvorbo merjene veličine v analogno (napetost) ali digitalno vrednost;
•	regulator, ki je lahko analogno vezje z operacijskimi ojačevalniki, programabilni (mikro)krmilnik ali PC programska oprema (npr. Matlab paket) z ustreznimi I/O vmesniki.
Splošno shemo naprave prikazuje slika 2 [10].
Vhodne veličine regulatorja so želeni in dejanski položaj vozička ter dejanski položaj nihala. Želeni položaj oz. kot nihala je 0 (navpična lega); če je ta različen od 0, se mora sistem pospešeno gibati v pozitivno ali negativno smer, to gibanje pa ima mehanske omejitve. Izhodna veličina regulatorja je napetost Um, ojačana v krmilni tok motorja Im. Ta generira moment M in preko jermenskega ali zobniškega prenosa silo F na voziček. Navadno se sma-
Slika 2. Eksperimentalna naprava z invertiranim nihalom
tra, da so U , I
'	m' m
visne veličine: F = fci ■ M = fc^
M in F linearno od-


(1)
Regulator, ki vzdržuje navpično lego nihala, je lahko različnih izvedb; v literaturi lahko najdemo klasično rešitev z regulatorjem stanj (State Space Controller), PID regulator, uporabo mehke logike (Fuzzy Logic), nevronske mreže (Neuron Network Controller) in druge različne sisteme.
Alternativna oblika te naprave je »vozilo«, ki je v mirovanju nestabilno, vendar lahko lovi ravnotežje z vožnjo naprej in nazaj. Praktična izvedba je npr. Segway, pri katerem je "nihalo" voznik in krmilo, "voziček" pa sta dve kolesi, ki sta gnani neodvisno. Naprave takšnega tipa za izobraževalne namene lahko najdemo tudi na osnovi Fischertechnic in LEGO Mindstorms sestavljank.
■ 3 Dodatne zahteve
Problem invertiranega nihala [7] je torej že mnogokrat teoretično
Slika 3. Sferično nihalo Ventil 21 /2015/ 3
in praktično obdelan, zato smo ga razširili z dodatnimi zahtevami:
a.	Nihalo je sferično nihalo, naj ima dve prostostni stopnji (slika 3.).
b.	V tem primeru je potreben sistem, ki lahko premika pivot nihala v smereh x in y. Analitično je najpreprostejša uporaba kartezičnega sistema, katerega prostostni stopnji gibanja sta x in y (lahko tudi z). Zdi se, da je regulator za takšen sistem preprosta razširitev že prikazane naprave (slika 2.) na dve neodvisni koordinati gibanja. Kot zahtevnejšo rešitev smo izbrali industrijski robot tipa SCARA, ki ravno tako omogoča gibanje v ravnini x-y, pri čemer pogoni za posamezne osi gibanja niso neodvisni. Kratica SCARA pomeni Selective Compliance Articulated Robot Arm in je pogosta oblika
robota za preprostejša opravila (npr. "Pick-and-place").
c.	Sistem mora imeti možnost, mehansko in v smislu krmiljenja, da zaniha nihalo iz spodnje (stabilne) lege v zgornjo (nestabilno), kjer vodenje prevzame regulator. Za ta namen se zdi SCARA robot primerna konstrukcija, ker minimalno ovira prosto gibanje nihala. [3]
d.	Realiziranih naj bo več tipov regulatorjev, med katerimi lahko preklapljamo (v realnem času) in primerjamo delovanje.
e.	Sistem bo sicer simuliran, vendar bo grafično prikazan v realnem času v 3D izrisu. Realiziran bo operaterski vmesnik za upravljanje s poskusno napravo.
■ 4 Izvedba
Idejno izvedbo naprave prikazuje slika 4.
Poskusna naprava je torej robot tipa SCARA (a). Na mesto za vpetje orodja je pritrjen tog nosilec (b), ter nanj nihalo (c) preko krogličnega zgloba (universal joint) (d). Nosilec omogoča, da se nihalo lahko prosto giblje v vsaj eni ravnini za polni kot 360° in pri tem ne zadane v druge
Slika 4. Idejna shema SCARA robota s sferičnim nihalom na prijemalu
Slika 5. 3D vizualizacija robota s sferičnim nihalom v programu RSTestStand
dele robota. Prostostne stopnje robota so označene kot A1, A2, Z in U [14]. A1 in A2 določata položaj roke v x-y ravnini, os Z ustreza kartezični koordinati z, os U pa opravlja vrtenje orodja okoli osi z. V pravilnem položaju roke robota je možno zanihati nihalo v navpično lego. Lego nihala določata kota ax in ay, ki ju dobimo s projekcijo nihala na ravnino x-z in y-z.
Idejno shemo smo nato realizirali kot 3D vizualizacijo v programu RSTestStand (slika 5.).
RSTestStand je namenjen interaktivnemu razvoju in preizkušanju krmilnega (PLC) programa na na-
mizju - vhodne in izhodne veličine PLC zamenja s simuliranim procesom. Za to nalogo smo iz množice funkcij uporabili samo možnost 3D izrisa, virtualni operaterski vmesnik
in generator šuma. Kvaliteta grafike 3D vizualizacije sicer zaostaja za sodobnimi sistemi, ki ponujajo 3D izris, je pa izdelava in animacija objektov dokaj preprosta in intuitivna.
Model robota in nihala je sestavljen iz osnovnih geometrijskih oblik (kvader, valj, krogla) v 3D editorju programa RSTestStand. Posamezni gradniki so hierarhično gnezdeni in povezani v skupine.
Objekti ali skupine objektov imajo možnost povezave atributov rotacije, translacije in velikosti (po koordinatah x, y ali z) s spremenljivkami, katerih vir je krmilnik ali interni modeli oz. generatorji RSTestStand okolja. Tako lahko animiramo sklepe robota (A1, A2, Z, U), oba kota nihala (ax, ay) in dolžino nihala (/^).
RSTestStand uporablja za povezavo s krmilnikom (ali drugimi naprava-
Slika 6. HMI vmesnik v programu RSTestStand
Slika 7. Bločna shema simulacije na PLC platformi
mi) standard OPC, kot fizično vodilo pa je uporabljen protokol Ethernet/ IP (industrijski ethernet).
Operaterski vmesnik (HMI) za upravljanje s poskusom prikazuje slika 6.
Programski del simulacije je v celoti realiziran na platformi PLC krmilnika CompactLogix in vključuje ročno in avtomatsko vodenje robota (inverzna kinematika za tip SCARA), model nihala, regulator (dve varianti), algoritem za zanihanje nihala in pomožne programe. Bločna shema je prikazana na sliki 7., posamezne sklope pa bomo podrobneje opisali v nadaljevanju.
■ 5 Simulacija nihala
Simulacija nihala povezuje položaj pivota nihala x, yp in zp (ki je funkcija položajev, h itrosti in pospeškov osi A1, A2, Z in U) s položajem kotov ax in ay [11].
Za realizacijo regulatorja zadostuje model dveh nesklopljenih nihal, ki sta projecirani na ravnini x-z in y-z, želeli pa smo razviti kompleten model.
Razvoj simulacije naj bi potekal takole:
a.	razvoj matematičnega modela nihala z diferencialnimi enačbami (Lagrangeova funkcija);
b.	pretvorbaenačbv eksplicitnoobli-ko, kjer je najvišji odvod odvisne spremenljivke enak (nelinearni) funkciji nižjih odvodov, torej:

(2)
Slika 8. Izpeljava rešitve za invertirano nihalo
m _ točkasta masa, g _ gravitacija.
Lagrangeova funkcija sistema je razlika med kinetično (T) in potencialno energijo (V) sistema (mehansko trenje, upor in izgube v tej izpeljavi zanemarimo) [8]:

(3)
kjer je q posplošena koordinata; c. numerično reševanje diferencialnih enačb z Eulerjevo metodo, pri kateri najprej izračunamo vrednost najvišjih odvodov z enačbo (2), nato s (približno) integracijo izračunamo vse nižje odvode, jih vstavimo nazaj v izraz (2) in to zanko ponavljamo.
Spremenljivke, ki jih potrebujemo za opis nihala, so:
x, y, z _ kartezični koordinatni sistem,
9 ^ sferična kota nihala, l _ dolžina nihala,

(6)
Po izpeljavi dobimo diferencialne enačbe za sferično nihalo:
e = (cp^sni(e')cos(e') + ssinCö)/;^)	(4)
^ = -2cpecos(ß')/sin(ß')	(5)
Vidimo, da izraz (5) predstavlja težavo za reševanje z Eulerjevo numerično metodo [15], ker vsebuje singularnost v točki sin (9) = 0, torej v ravnovesni (9 = 0) in navpični legi (9 = n). Opisana metoda reševanja je zato tu neuporabna.
Po več poskusih smo razvili alternativno metodo, kjer problem rešujemo v x-y-z koordinatnem sistemu kot gibanje masnega delca m po površini sfere z radijem l^, (slika 8.)
a.	V nekem trenutku ima masa m položaj P0 (x,y,z), ki je na površini sfere z radijem l, in hitrost V, ki je pravokotna na normalo sfere oz. P0.
b.	Pod vplivom sile F(x,y,z) naredi masa m v času dt pot S(x,y,z) do novega položaja P1(x,y,z).
c.	P^ ni več na površini sfere, zato izvedemo korekcijo:
(6) pod predpostavko, da gre P^, ko gre dt ^ 0. V našem primeru je dt enak periodi izvajanja programa, stremimo torej k čim hitrejšemu izvajanju programa ter računsko enostavnejši rešitvi.
d.	Nov trenutni položaj P0 postane enak Pc.
e.	Zanko ponavljamo s periodo dt. (Tak model ne upošteva rotacije nihala okoli lastne osi.)
Vhodi v sistem so pospeški ax, ay in az. Dobimo jih z dvojnim odvajanjem položaja pivota nihala. Izhod sistema je položaj mase nihala Px0, Py0, Pz0, ki je nato pretvorjen v kota ax in ay.
■ 6 Strategija za zanihanje
Strategija za zanihanje temelji na
naslednjih izhodiščih:
•	Smer zanihanja je v ravnini y-z, nekako na sredini dosega robota v koordinati x, kjer ima ta največji doseg v smeri y.
•	Temu ustreza tudi izbrani tip robotske roke in izvedba pritrdilnega mehanizma (kot ay, ki predstavlja odmik v ravnini y-z, nima omejitve gibanja, medtem ko je kot ax (v ravnini x-z).
•	Pri vzbujanju se sme nihalo gibati samo v izbrani ravnini, sicer pride do kroženja.
•	Energijo za vzbujanje je potrebno dovajati v intervalih, ki so enaki
naravni frekvenci sistema (ki pa ni konstantna) [4]. • Postopek je uspešen, ko privede nihalo v bližino navpične lege pri dovolj majhni krožni hitrosti, oba parametra sta nastavljiva v programski proceduri in sta bila ugotovljena s poskušanjem. Vgrajen evaluator nato preklopi fazo zani-hanja v regulacijo.
Rezultat vidimo na sliki 9, prikazan je dosežen kot a^,, ki je v spodnji legi enak 0 in se v zgornji navpični legi približa -n. Celotno trajanje procesa zanihanja je 15 sekund.
■ 7 Regulator
Invertirano nihalo je klasična platforma za preizkušanje različnih regulatorjev. V literaturi so opisani uspešni primeri vodenja s PID, regulatorjem stanj (State Space Controller), mehko logiko (Fuzzy Logic) [9], adaptivnimi nevronskimi mrežami (ANN) in drugim.
Najpogostejši regulator je regulator stanj, načrtan po metodi LQR. Ta tip regulatorja je bil prvi, ki smo ga uporabili na razvitem modelu.
Izhodišče za načrtovanje regulatorja je predpostavka, da sferično nihalo v okolici navpične (in ravnovesne) lege (a^, ay ^ 0) lahko aproksimiramo z dvema nesklo-pljenima (neodvisnima) nihaloma - projekcijama na ravnini x-z in y-z.
Si tr, cos(ffj,) « 1, d^.
Si o (8)
Dobimo izraz (9), ki je primeren za pretvorbo v matrično obliko - prostor stanj (10):

(9)
C =
Slika 9. Časovni diagram zanihanja od mirovne lege do vklopa regulatorja (območje t = 0 ^ 30 s, a = -4 ^ 4 rad)
10 0 0 .0010
D = o
(10)
Načrtovanje regulatorja torej lahko opravimo na osnovi modela za nihalo z eno prostostno stopnjo (7), kot je to v primeru klasičnega poskusa z vozičkom.
äy = ■ sin a^ + y ■ cos a^
Uporabljene spremenljivke in njihove privzete vrednosti so: a^, ay ^ kot nihala; g ^ gravitacijski pospešek (9,8 m/s2); x, y, z ^ kartezični koordinatni sistem; f ^ konstanta viskoznega trenja (0,02 Nm/s); dolžina nihala (0,6 m); m . masa nihala (0,3 kg).
Sistem lahko lineariziramo s pre-tpostavko (8):
Odprtozančni sistem je inherentno nestabilen, brez ustreznega zunanjega vzbujanja bo nihalo padlo iz navpične v spodnjo ravnovesno lego.
Regulator stanj dobimo, če sistem zaključimo z (negativno) povratno zanko preko ojačanja K (slika 10.) [6] [12] [13].
Vhod sistema, u je sedaj povezan z vektorjem stanj X preko matrike ojačanj K:

(11)
Paramatre vektorja K lahko dobimo s pomočjo paketa Matlab in vgrajene funkcije lqr [5]. Dobljen rezultat je k1 = -1,00, k2 = -1,91, k3 = -27,70 in k4 = -6,85.
Odziv regulatorja na stopničasto vzbujanje (0,3 m) prikazuje slika 11.
Slika 10. Shema zaprtozančnega sistema v obliki prostora stanj
Slika 11. Odziv LQR regulatorja (območje t = 0 ^ 10 s, kot a (zeleno) = -0,02 ^ 0,01 rad, položaj y (modro) = -0,50 ^ 1,00 m)
Slika 12. Regulator v delovanju
Regulator vklopimo, ko algoritem za zanihanje uspešno privede nihalo v bližino navpične lege pri majhni kotni hitrosti. Regulator prevzame gibanje robota v ravnini x-y in s tem vzdržuje navpično lego nihala (slika 12.). V model nihala pripeljemo tudi generator šuma (slika 7.), ki simulira različne motnje v sistemu. Referenčni položaj
pivota nihala v ravnini x-y seveda lahko spreminjamo. Neodvisno od regulatorja lahko vodimo osi Z (koordinatna os z) in U (vrtenje orodja), kar predstavlja motnjo za sistem.
Izveden je tudi PID regulator, ki je po pričakovanju manj stabilen od regulatorja stanj, saj ima kot vhodni
Slika 13. Robot in virtualno delovno okolje Ventil 21 /2015/ 3
veličini na voljo samo dve spremenljivki, X in ax (oz. y in a).
■ 8 Sekundarna opravila
Robot se lahko, medtem ko vzdržuje navpično lego nihala, giblje v delovnem območju in izvaja sekundarne operacije (Nullspace Motion) [2], npr. pobiranje in odlaganje predmetov (pick-and-place). V ta namen smo v okolju RSTestStand dodali virtualno delovno površino z nekaj predmeti, ki jih lahko robot z orodjem prime, prenese in odloži (slika 13.).
Natančnost odlaganja je seveda neposredno odvisna od motnje, ki jo nihalo vnaša v gibanje prijemala.
■ 9 Zaključki
Opisano platformo smo v podjetju uspešno uporabili v demonstracijske in učne namene. Vsi rezultati, matematične izpeljave, programska koda in drugo so prosto dostopni pri avtorju prispevka.
Platforma je odprta za množico nadaljnjih poskusov in upamo, da jih bomo v sodelovanju s Fakulteto in bodočimi študenti tudi uresničili.
Literatura
[1]	Control Tutorials for MATLAB and Simulink (CTMS). http://ctms. engin.umich.edu/CTMS/index. php?aux=Home. [Dostopano: 12. 8. 2014].
[2]	G. Schreiber, C. Ott in G. Hirzinger, Interactive Redundant Robotics: Control of the Inverted Pendulum with Nullspace Motion. Wessling: German Aerospace Center -DLR, Institute for Robotics and Mechatronics, b. l.
[3]	B. Sprenger, L. Kucera in S. Mourad, Balancing of an Inverted Pendulum with a SCARA Robot. Zurich: Swiss Federal Institute of Technology Zurich (ETHZ), Institute of Robotics.
[4]	K. J. Astrom in K. Furuta, "Swinging up a Pendulum by Energy Control", Automatica, vol. 36, 2000.
[5]	Control System Design. Dosegljivo: http://csd.newca-stle.edu.au/control/simulations/ pendulum.html. [Dostopano: 12. 8. 2014].
[6]	J. P. Hespanha, Undergraduate Lecture Notes on LQG/LQR controller design, April 1, 2007.
[7]	Wikipedia, The Free Encyclopedia, »Inverted_pendulum«. Dosegljivo: http://en.wikipedia.org/wiki/ Inverted_pendulum. [Dostopano: 12. 8. 2014].
[8]	Wikipedia, The Free Encyclopedia, »Euler-Lagrange equation«. Dosegljivo: http://en.wikipedia.
org/wiki/Euler_lagrange. [Dostopano: 12. 8. 2014].
[9]	Y. Beceriklia in K. Celik, Fuzzy control of inverted pendulum and concept of stability using Java application. Mathematical and Computer Modelling, 46, str. 24-37, 2007.
[10]	Googol Technology,"Inverted Pendulum Experimental Manual suitable for GLIP series," 2nd edition, 2006.
[11]	B. Zupančič, Simulacija dinamičnih sistemov. Ljubljana: Fakulteta za elektrotehniko in računalništvo, 1995.
[12]	B. Zupančič, Zvezni regulacijski sistemi (2. del, 2. popravljena in dopolnjena izdaja), Ljubljana: Fakulteta za elektrotehniko in računalništvo, 1995.
[13]	I. Škrjanc, Regulacije I, Ljubljana: Fakulteta za elektrotehniko in računalništvo, 2006.
[14]	T. Bajd, Osnove robotike (7. popravljena in dopolnjena izdaja). Ljubljana: Fakulteta za elektrotehniko, 2006.
[15]	B. Jurčič in B. Orel, Numerične metode. Ljubljana: Fakulteta za elektrotehniko in računalništvo, 2004.
Control of Inverted Pendulum with SCARA type Robot
Abstract: This article considers the possibilities of system simulation and modelling using a PLC (producer Allen-Bradley) and the complementing software packages. The resulting platform is intended for user training and commercial demonstration purposes. The selected control task involves a SCARA type robot with a spherical (2 DOF) inverted pendulum, pivoting freely on its effector point. The task of the robot is to swing up the pendulum and maintain an upright position while being able to move inside its working envelope and execute secondary tasks. The research includes the development of spherical pendulum governing equations, swing-up and control procedures, simulation and 3D visualization of the complete system. A graphical interface enables the user to control the experiment and set different variables.
Keywords: Inverted pendulum, spherical pendulum, SCARA robot, advanced control, simulation, 3D render, Allen-Bradley
T E H n P
Avtomatizacija
Rešitve za mehatroniko, avtomatizacijo in informatiko
Radtwen Automatfoi*

Authorized Distributor
Jfltn-Äfj^Upf. AiMiim-jr
#
w
m
info@tehna,si www.tehna.si Tehnološki park 19 - Ljubljana
Novo v programu:
Osvetljene tipke 22,5 mm na osnovi piezo tehnologije Stopnja zaščite 1P69K. material nerjaveče jeklo 316 Primemi so za uporabo v živilski in farmacevtski industriji {pranje s čistili pod pritiskom) So brez vdolbin^ kjer se lahko nabirajo kontaminanti Standardni izrez 22,5 mm, napetost 24VDC Gravirani napisi po naročilu