VYSOKÉ UČENÍ TECHNICKÉ V BRNĚBRNO UNIVERSITY OF TECHNOLOGY

FAKULTA ELEKTROTECHNIKY A KOMUNIKAČNÍCHTECHNOLOGIÍÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY

FACULTY OF ELECTRICAL ENGINEERING AND COMMUNICATIONDEPARTMENT OF CONTROL AND INSTRUMENTATION

ŘÍDICÍ SYSTÉM MOBILNÍHO ROBOTU

MOBILE ROBOT CONTROL SYSTEM

BAKALÁŘSKÁ PRÁCEBACHELOR'S THESIS

AUTOR PRÁCE MICHAL KALINAAUTHOR

VEDOUCÍ PRÁCE Ing. ONDŘEJ HYNČICASUPERVISOR

BRNO 2015

VYSOKÉ UČENÍTECHNICKÉ V BRNĚ

Fakulta elektrotechniky a komunikačních technologií

Ústav automatizace a měřicí techniky

Bakalářská prácebakalářský studijní obor

Automatizační a měřicí technika

Student: Michal Kalina ID: 154757Ročník: 3 Akademický rok: 2014/2015

NÁZEV TÉMATU:

Řídicí systém mobilního robotu

POKYNY PRO VYPRACOVÁNÍ:

Navrhněte řídicí aplikaci pro balancující robot a implementujte do řídicí desky na bázi Arduina.1) Seznamte se s řídicí elektronikou, snímači a použitými motory.2) Vytvořte matematický model robotu a navrhněte způsob regulace pro udržení vzpřímené polohy.3) Implementujte ovladače pro snímače, řízení motorů a regulátor.4) Odlaďte a otestujte výsledek, navrhněte vhodné úpravy.

DOPORUČENÁ LITERATURA:

ZBYŠEK VODA a kol. Průvodce světem Arduina [online]. 2015 [cit. 2015-02-04]. Dostupné z:http://arduino.cz/

Termín zadání: 9.2.2015 Termín odevzdání: 25.5.2015

Vedoucí práce: Ing. Ondřej HynčicaKonzultanti bakalářské práce:

doc. Ing. Václav Jirsík, CSc.Předseda oborové rady

UPOZORNĚNÍ:

Autor bakalářské práce nesmí při vytváření bakalářské práce porušit autorská práva třetích osob, zejména nesmízasahovat nedovoleným způsobem do cizích autorských práv osobnostních a musí si být plně vědom následkůporušení ustanovení § 11 a následujících autorského zákona č. 121/2000 Sb., včetně možných trestněprávníchdůsledků vyplývajících z ustanovení části druhé, hlavy VI. díl 4 Trestního zákoníku č.40/2009 Sb.

3

ABSTRAKT

Tato práce se zabývá stavbou dvoukolého mobilního robota a následnou tvorbou

ovladačů a řízení pro jeho senzory, motory a návrh regulátoru. Řízení funguje na

platformě Arduino s procesorem ATmega328. V prvé řadě je v práci provedena rešerše.

Součástí práce je modelová část, kde je vytvořen simulační model soustavy, jeho

linearizace a následný návrh regulátoru. Dále obsahuje popis výroby a stavby těla robota,

popis a funkčnost použitých motorů, popis všech senzorů a snímačů a detailní popis

dodané desky plošných spojů. A nakonec programovou část, kde je rozebráno ovládání

motorů, práce se senzory a funkčnost regulátoru. Výsledkem práce je funkční aplikace

ovládání a řízení dvoukolého nestabilního robota.

KLÍČOVÁ SLOVA

mobilní dvoukolý robot, Arduino, ARM, ATmega328, BLDC motor, gyroskop,

akcelerometr, inverzní kyvadlo, I2C sběrnice, Martinez V3, regulátor, modelování,

simulace, Matlab, Simulink, MPU 6050, Li-Pol akumulátor

4

ABSTRACT

This thesis deals with building of two-wheeled mobile robot and consecutive

creation of drivers and control for sensors, engines and controller design. The control is

based on Arduino platform using ATmega328 processor. Firstly there is in this thesis

recherche. Part of this thesis is modelling one where simulation model of our system is

created, its linearization and then controller design. Further it contains description of the

building robots body, description and functionallity of used engines, description of all

sensors and detailed description of available printed circuit board. In the end there is

software part where the engines control is described, operation of the sensors and

functionallity of the controller. Result of this thesis is function application of controlling

and operating of two-wheeled unstable mobile robot.

KEYWORDS

Mobile two-wheeled robot, Arduino, ARM, ATmega328, BLDC motor,

gyroscope, accelerometer, inverted pendulum, I2C bus, Martinez V3, controller,

modelling, simulation, Matlab, Simulink, MPU 6050, Li-Pol accumulator

5

BIBLIOGRAFICKÁ CITACE

KALINA, M. Řídicí systém mobilního robotu. Brno: Vysoké učení technické v Brně,

Fakulta elektrotechniky a komunikačních technologií, 2015. 55 s. Vedoucí bakalářské

práce Ing. Ondřej Hynčica.

6

PROHLÁŠENÍ

Prohlašuji, že svou bakalářskou práci na téma „Řídicí systém mobilního robotu“

jsem vypracoval samostatně pod vedením vedoucího bakalářské práce a s využitím

odborné literatury a dalších informačních zdrojů, které jsou všechny citovány v práci a

uvedeny v seznamu literatury na konci práce.

Jako autor uvedené bakalářské práce dále prohlašuji, že v souvislosti s vytvořením

této bakalářské práce jsem neporušil autorská práva třetích osob, zejména jsem nezasáhl

nedovoleným způsobem do cizích autorských práv osobnostních a jsem si plně vědom

následků za porušení ustanovení §11 a následujících autorského zákona č. 121/2000 Sb.,

včetně možných trestněprávních důsledků vyplívajících z ustanovení §152 trestního

zákona č. 140/1961 Sb.

V Brně dne 25.5.2015 …………………………..

(podpis autora)

7

PODĚKOVÁNÍ

Děkuji vedoucímu bakalářské práce Ing. Ondřeji Hynčicovi za odbornou pomoc,

kritické připomínky a cenné rady při zpracování mé bakalářské práce. Děkuji také mé

rodině a blízkým přátelům za podporu během celého studia i psaní této bakalářské práce.

V Brně dne 25.5.2015 …………………………..

(podpis autora)

8

OBSAH

1 ÚVOD .......................................................................................................... 11

2 JINÁ ŘEŠENÍ ............................................................................................. 12

2.1 nBot ....................................................................................................... 12

2.2 JOE ........................................................................................................ 12

2.3 LegWay ................................................................................................. 13

2.4 Segway PT ............................................................................................ 14

3 IDENTIFIKACE ........................................................................................ 15

3.1 Identifikace soustavy ............................................................................. 15

3.1.1 Rychlost kyvadla ............................................................................. 16

3.1.2 Lagrangeovy rovnice II. druhu ........................................................ 17

3.2 Linearizace modelu ............................................................................... 23

3.3 Stavový popis ........................................................................................ 24

3.4 Číselné dosazení a zjištění přenosu soustavy ........................................ 25

4 NÁVRH REGULÁTORU .......................................................................... 27

4.1 Vlastnosti základních regulátorů ........................................................... 28

4.1.1 PID regulátor ................................................................................... 28

4.1.2 PSD regulátor .................................................................................. 29

4.2 Návrh samotného regulátoru ................................................................. 30

4.2.1 Základní odezvy soustavy s navrhnutým regulátorem .................... 31

5 STAVBA ROBOTA .................................................................................... 33

5.1 Podvozek ............................................................................................... 33

5.2 Konstrukce ............................................................................................ 34

5.3 Montáž periferií ..................................................................................... 35

5.4 Hotový robot ......................................................................................... 35

6 HARDWARE .............................................................................................. 36

6.1 Deska plošných spojů ............................................................................ 36

6.2 Senzory .................................................................................................. 39

6.2.1 Princip MEMS gyroskopu ............................................................... 39

6.2.2 Princip MEMS akcelerometru ......................................................... 40

6.3 Motory ................................................................................................... 40

6.4 Akumulátor ........................................................................................... 41

6.4.1 Li-Pol akumulátor............................................................................ 41

7 SOFTWARE ............................................................................................... 42

7.1 Platforma Arduino ................................................................................. 42

7.2 Práce s IMU ........................................................................................... 42

7.2.1 Nastavení IMU ................................................................................ 43

7.2.2 Nastavení offsetu IMU .................................................................... 44

9

7.3 I2C sběrnice ........................................................................................... 44

7.4 Řízení motorů ........................................................................................ 45

7.4.1 PWM – Pulsně šířková modulace ................................................... 46

7.5 Regulátor ............................................................................................... 47

8 VYHODNOCENÍ ....................................................................................... 47

9 ZÁVĚR ........................................................................................................ 49

REFERENCE .................................................................................................... 50

SEZNAM POUŽITÝCH ZKRATEK ............................................................. 52

SEZNAM POUŽITÝCH SYMBOLŮ ............................................................. 53

SEZNAM PŘÍLOH ........................................................................................... 55

10

SEZNAM OBRÁZKŮ

Obr. 2.1: nBot [1] ............................................................................................... 12

Obr. 2.2: JOE [2] ................................................................................................ 13

Obr. 2.3: LegWay [3] ......................................................................................... 14

Obr. 2.4: Segway x2 Turf [4] ............................................................................. 15

Obr. 3.1: Model kyvadla [5]............................................................................... 15

Obr. 3.2: Zobrazení vektorů rychlostí [5] .......................................................... 16

Obr. 3.3: Blokové schéma modelu vytvořené v prostředí Matlab Simulink......22

Obr. 4.1: Blokové schéma uzavřeného regulačního obvodu [8] ........................ 27

Obr. 4.2: Odezva na jednotkový skok řízení ...................................................... 31

Obr. 4.3: Odezva na jednotkový skok poruchy .................................................. 32

Obr. 4.4: Odezva na jednotkový skok regulační odchylky ................................ 32

Obr. 4.5: Velikost akčního zásahu ..................................................................... 33

Obr. 5.1: Diferenciální podvozek robota ........................................................... 33

Obr. 5.2: Vyvrtané otvory pro montáž závitových tyčí ..................................... 34

Obr. 5.3: Technický výkres podvozku ............................................................... 34

Obr. 5.4: Kompletní konstrukce robota ............................................................. 35

Obr. 5.5: Finální vzhled robota .......................................................................... 36

Obr. 6.1: Blokové schéma robota....................................................................... 36

Obr. 6.2: Martinez V3 [10] ................................................................................ 37

Obr. 6.3: Schéma zapojení DPS [11] ................................................................. 38

Obr. 6.4: Deska GY-521 s MPU 6050 [10] ....................................................... 39

Obr. 6.5: Princip MEMS gyroskopu [13] .......................................................... 40

Obr. 6.6: Detail mechanického snímače gyroskopu [13] ................................... 40

Obr. 6.7: Princip DC motoru [13] ...................................................................... 41

Obr. 6.8: Princip BLDC motoru [13] ................................................................. 41

Obr. 7.1: Princip fungování I2C sběrnice [19] ................................................... 45

Obr. 7.2: Princip pulsně šířkové modulace [20] ................................................ 46

Obr. 8.1: Odchylka robota od vzpřímenné polohy ............................................. 48

Obr. 8.2: Akční zásah regulátoru ....................................................................... 48

11

1 ÚVOD

Dvoukolý mobilní robot, jehož těžiště se nachází nad osou kol, se chová jako

inverzní kyvadlo, které chápeme jako obecně nestabilní soustavu. Stabilizování takové

soustavy dosáhneme udržením mechanické rovnováhy, to je stav, při kterém je osa těla

robota kolmá na zemské zrychlení.

Jako podvozek pro dvoukolé roboty se využívá podvozek diferenciálního typu.

Základem diferenciálního podvozku jsou dvě nezávisle poháněná kola. Robot je tedy

schopen přímočarého pohybu vpřed a vzad, šikmého pohybu a otáčení se na místě.

Výhodou je dobrá manévrovatelnost a jednoduchá, relativně přesná odometrie.

Nevýhodou bývá špatná odolnost vůči terénu.

Při vykonávání jakéhokoliv pohybu je robot udržován ve stabilní poloze. O to se

stará mikroprocesor s řídícími prvky za pomocí kombinace gyroskopu a akcelerometru.

Mikroprocesor zpracovává data z akcelerometru a gyroskopu a předává je navrhnutému

regulátoru, který následně za pomocí dvou můstků řídí činnost obou motorů respektive

kol.

Pro vytvoření regulátoru je potřeba identifikovat soustavu a následně navrhnout

regulátor. Identifikace soustavy bude provedena pomocí Lagrangeových rovnic II. druhu.

Vzhledem k fyzikální podstatě problému jsou hlavními požadavky na regulaci především

rychlost regulace a velikost překmitu.

Úloha řízení a ovládání diferenciálních podvozku respektive dvoukolého

mobilního robota je častým tématem nejen na akademické úrovni, ale i na úrovni

komerční a také mezi amatéry. Důvodem velké oblíbenosti této úlohy je využití širokého

spektra oborů z oblasti automatizace. A to od zpracovávání dat ze snímačů přes regulaci

s využitím mikroprocesorů, návrh desky plošných spojů a výkonových prvků až po

samotné řízení motorů. Může obsahovat i bezdrátové ovládání s využitím PC a

počítačového programu.

Tato práce se bude týkat řídicího systému mobilního robotu. Cílem práce je stavba

robota, instalace dostupným senzorů, motorů a DPS. Výběr vhodného napájecího prvku

a celkové oživení všech častí. Dále budou vytvořeny ovladače pro čtení senzorů, regulaci

a řízení motorů.

Hlavním cílem je docílit stavu, kdy robot bude schopný samostatně stát ve

vzpřímené poloze bez vnějšího působení.

12

2 JINÁ ŘEŠENÍ

V následující kapitole bude představeno několik již existujících řešení dvoukolého

mobilního robota. Zaměříme se hlavně na použitou řídící elektroniku, senzoriku a řídící

algoritmus.

2.1 nBot

Autorem tohoto návrhu je David P. Anderson, profesor na Southern Methodist

University v Texasu v USA a také hobby-robotik.

nBot prošel za více jak deset let svojí existence mnoha proměnami. Na počátku

šlo o trojkolého robota, který měl stabilizovat inverzní kyvadlo. V druhé fázi se již jednalo

o dvoukolého robota. V této fázi byly baterie umístěny ve spodní části robota, což

ztěžovalo stabilizaci, a proto ve třetí fázi tvorby došlo k přesunu baterií do horní části.

V současné době robot využívá dva 24voltové stejnosměrné motory, robot tedy dosahuje

vysokého momentu a vyšších otáček.

Měření náklonu zajišťuje kompaktní inerciální měřící jednotka (IMU), která je

tvořena akcelerometrem a gyroskopem. Vyhodnocení získaných hodnot je řešeno

Wienerovým filtrem, který je implementován přímo v IMU.

Řízení je obstaráno regulátorem, jehož vstupy jsou náklon robota, natočení kol a

jejich derivace (tedy rychlosti). Pohybu se docílí změnou žádané hodnoty regulátoru.

Robot tedy musí být v pohybu, aby byl stále vyvážen. Otáčení robota na místě se dosáhne

přidáním napětí na jeden motor a ubrání stejné hodnoty na motoru druhém. [1]

Obr. 2.1: nBot [1]

2.2 JOE

JOE je dalším dvoukolým autonomním robotem, vyvinutý čtyřmi autory z

univerzity Swiss Federal Institute of Technology Lausanne ve Švýcarsku. Patří mezi

jednoho z prvních vytvořených dvoukolých robotů vůbec.

13

Cílem tohoto projektu bylo vytvořit dvoukolého robota, který by byl schopný

uvést jednoho člověka na dvou souosých kolech. Kvůli obavám o bezpečnost byl nakonec

vytvořený zmenšený model převážející pouze závaží.

Robot je poháněn dvěma stejnosměrnými motory připojenými na planetární

převodovky. Inkrementální enkodér, připojený na obou motorech, předává řídicí jednotce

informaci o poloze (úhlu natočení) motoru. Zjištění náklonu robota zajišťují akcelerometr

a gyroskop.

Řízení obstarává procesor z rodiny Sharc s plovoucí desetinnou čárkou,

programové hradlové pole (FPGA) od firmy XILINK, čtyři 10bitové čislicovo-analogové

převodníky (DAC) a 14 12bitových analogovo-číslicových převodníků (ADC). [2]

Obr. 2.2: JOE [2]

2.3 LegWay

Autorem tohoto dvoukolého robota postaveného ze stavebnice LEGO

Mindstorms, je Steve Hassenplug, amatérský robotik, který má na svědomí mnoho

podobných projektů.

Pro ovládání robota na dálku je použit standartní dálkový ovladač z dílny LEGO,

robot je tedy schopen pohybu vpřed, vzad a otáčení se na místě.

Program pro řízení robota byl napsán v brickOS, což je operační systém pro

stavebnici LEGO Mindstorms. V první verzi robota byl použit standartní akcelerometr

pro udržení stability, ale autor nebyl schopen tuto variantu zprovoznit a tak přešel na

variantu se dvěma elektro-optickými proximitními snímači. Jejich princip je založen na

14

infračervených proximitních snímačích, ty využívají viditelné světlo, které vysílají a

vyhodnocují množství odraženého světla. Díky tomu je robot schopen se orientovat i

v prostoru. [3]

Obr. 2.3: LegWay [3]

2.4 Segway PT

Segway Personal Transporter je asi nejznámější aplikace dvoukolého robota. Jeho

autorem je americký podnikatel a vynálezce Dean Kamen. Na trh byl uveden v roce 2001

firmou Segway Inc.

Je poháněn dvěma elektromotory napájenými Ni-MH nebo Li-ion akumulátory.

Díky tomu je schopen dosáhnout rychlosti až 20 km/h.

Stabilitu zajišťuje soustava pěti gyroskopů řízená procesorem, který vyhodnocuje

polohu základny a na základě těchto informací ovládá stabilizační elektromotory tak, aby

byla plošina stále v rovině.

Ovládání zajišťuje řidič a to náklonem dopředu pro jízdu vpřed, náklonem dozadu

pro jízdu vzad a náklonem řídítek doleva či doprava pro otáčení.

Segway byl zkonstruován tak, že je možné s ním jezdit prakticky v každém terénu

i uvnitř budov, bez problému zvládne i schody. [4]

15

Obr. 2.4: Segway x2 Turf [4]

3 IDENTIFIKACE

3.1 Identifikace soustavy

Pro identifikaci soustavy bylo použito Lagrangeových rovnic II. druhu, podle

kterých lze sestavit pohybové rovnice soustavy za použití tzv. zobecněných souřadnic.

Při sestavování rovnic se vycházelo z Obr. 3.1:

Obr. 3.1: Model kyvadla [5]

16

kde:

𝜑 natočení kyvadla [rad ]

𝑣21 obvodová rychlost kyvadla kolem kola [m/s ]

𝛿 natočení kola [rad ]

�̇� translační rychlost kola [m/s ]

𝑚1 hmotnost kola (podvozku) [kg ]

𝑚2 hmotnost závaží (kyvadla) [kg ]

𝑀1 moment závaží (kyvadla) [Nm ]

𝑀2 moment kola [Nm ]

𝐽 moment setrvačnosti kola [kg∙m2 ]

𝐿 délka kyvadla (vzdálenost kola od závaží) [m ]

3.1.1 Rychlost kyvadla

Pro zjednodušení výpočtu byla brána v úvahu jízda modelu po rovině a závaží

bylo reprezentováno hmotným bodem v umístěným v těžišti závaží. Následně byla určena

rychlost tohoto hmotného bodu 𝑣2. Ta se skládá z translační rychlosti �̇� a obvodové

rychlosti 𝑣21 kola (Obr. 3.2).

Obr. 3.2: Zobrazení vektorů rychlostí [5]

Vyjádření rychlosti 𝑣2 v osách x a y:

𝑣2𝑥 = �̇� + 𝑣21 𝑐𝑜𝑠 𝜑 (3.1) [5]

𝑣21 = 𝐿�̇� (3.2) [5]

𝑣2𝑥 = �̇� + 𝐿�̇� 𝑐𝑜𝑠 𝜑 (3.3) [5]

𝑣2𝑦 = 𝑣21 𝑠𝑖𝑛 𝜑 (3.4) [5]

17

𝑣2𝑦 = 𝐿�̇� 𝑠𝑖𝑛 𝜑 (3.5) [5]

𝑣2𝑥 = �̇� + 𝑣21 𝑐𝑜𝑠 𝜑 (3.6) [5]

Výsledná rychlost 𝑣2:

𝑣22 = 𝑣2𝑥

2 + 𝑣2𝑦2 (3.7) [5]

𝑣22 = �̇�2 + 2�̇�𝐿�̇� 𝑐𝑜𝑠 𝜑 + 𝐿2�̇�2 𝑐𝑜𝑠2 𝜑 + 𝐿2�̇�2 𝑠𝑖𝑛2 𝜑 (3.8) [5]

Rovnice 3.8 byla upravena:

𝑣22 = �̇�2 + 2�̇�𝐿�̇� 𝑐𝑜𝑠 𝜑 + 𝐿2�̇�2 (𝑐𝑜𝑠2 𝜑 + 𝑠𝑖𝑛2 𝜑) (3.9) [5]

kde:

𝑐𝑜𝑠2 𝜑 + 𝑠𝑖𝑛2 𝜑 = 1 (3.10) [5]

pak:

𝑣22 = �̇�2 + 2�̇�𝐿�̇� 𝑐𝑜𝑠 𝜑 + 𝐿2�̇�2 (3.11) [5]

3.1.2 Lagrangeovy rovnice II. druhu

Obecný zápis Lagrangeových rovnic II. druhu:

𝑑

𝑑𝑡(𝛿𝐸𝑘

𝛿𝑞) −

𝛿𝐸𝑘

𝛿𝑞+

𝛿𝐸𝑝

𝛿𝑞=

𝛿𝐴

𝛿𝑞 (3.12) [5]

kde:

𝐸𝑘 kinetická energie

𝐸𝑝 potenciální energie

𝐴 práce soustavy

𝑞 zobecněná souřadnice

Výpočet kinetické energie:

𝐸𝑘 = 𝐸𝑘_𝑧á𝑣𝑎ží + 𝐸𝑘_𝑘𝑜𝑙𝑜 (3.13) [5]

𝐸𝑘_𝑧á𝑣𝑎ží =1

2𝑚2𝑣2

2 (3.14) [5]

18

𝐸𝑘_𝑘𝑜𝑙𝑜 =1

2𝑚1�̇�

2 +1

2𝐽�̇�2 (3.15) [5]

Dosazením rovnic 3.13, 3.14 a 3.9 do rovnice 3.12 získáme vztah:

𝐸𝑘 =1

2𝑚2(�̇�

2 + 2�̇�𝐿�̇� 𝑐𝑜𝑠 𝜑 + 𝐿2�̇�2) +1

2𝑚1�̇�

2 +1

2𝐽�̇�2 (3.16) [5]

Rychlost otáčení kola:

�̇� =�̇�

𝑟 (3.17) [5]

Potom lze vztah 3.15 zapsat ve tvaru:

𝐸𝑘 =1

2𝑚2�̇�

2 + 𝑚2�̇�𝐿�̇� 𝑐𝑜𝑠 𝜑 +1

2𝑚2𝐿

2�̇�2 +1

2𝑚1�̇�

2 +1

2𝐽 (

�̇�

𝑟)2

(3.18)

[5]

Parciální derivace podle zobecněných souřadnic 𝑞 = 𝑥, �̇� = �̇�, 𝑞 = 𝜑, �̇� = �̇�:

𝛿𝐸𝑘

𝛿𝑥= 0 (3.19) [5]

𝛿𝐸𝑘

𝛿�̇�= 𝑚2�̇� + 𝑚2𝐿�̇� 𝑐𝑜𝑠 𝜑 + 𝑚1�̇� +

𝐽

𝑟2�̇� (3.20) [5]

𝑑

𝑑𝑡(𝛿𝐸𝑘

𝛿�̇�) = 𝑚2�̈� + 𝑚2𝐿�̈� 𝑐𝑜𝑠 𝜑 − 𝑚2𝐿𝜑2̇ 𝑠𝑖𝑛 𝜑 + 𝑚1�̈� +

𝐽

𝑟2�̈� (3.21) [5]

𝛿𝐸𝑘

𝛿𝜑= −𝑚2�̇�𝐿�̇� 𝑠𝑖𝑛 𝜑 (3.22) [5]

𝛿𝐸𝑘

𝛿�̇�= 𝑚2�̇�𝐿 𝑐𝑜𝑠 𝜑 + 𝑚2𝐿

2�̇� (3.23) [5]

𝑑

𝑑𝑡(𝛿𝐸𝑘

𝛿�̇�) = 𝑚2�̈�𝐿 𝑐𝑜𝑠 𝜑 + 𝑚2𝐿

2�̈� − 𝑚2�̇�𝐿�̇� 𝑠𝑖𝑛 𝜑 (3.24) [5]

Výpočet potenciální energie:

19

𝐸𝑝 = 𝑚2𝑔ℎ (3.25) [5]

kde 𝑔 představuje gravitační zrychlení Země.

Za nulovou hladinu potenciální energie považujeme osu otáčení kola, díky čemuž

dokážeme z Obr. 3.1: Model kyvadla [5]Obr. 3.1 určit vzdálenost ℎ mezi touto hladinou

a těžištěm závaží.

ℎ = 𝐿 𝑐𝑜𝑠 𝜑 (3.26) [5]

Pak je potenciální energie:

𝐸𝑝 = 𝑚2𝑔𝐿 𝑐𝑜𝑠 𝜑 (3.27) [5]

Parciální derivace potenciální energie podle zobecněných souřadnic 𝑞 = 𝑥 a

𝑞 = 𝜑:

𝛿𝐸𝑝

𝛿𝑥= 0 (3.28) [5]

𝛿𝐸𝑝

𝛿𝜑= −𝑚2𝑔𝐿 𝑠𝑖𝑛 𝜑 (3.29) [5]

Výpočet práce A:

𝐴 = 𝑀1𝛿 + 𝑀2𝜑 (3.30) [5]

Předpokládáme-li, že při rovnovážném stavu platí:

𝑀1 = 𝑀2 (3.31) [5]

pak:

𝐴 = 𝑀(𝛿 + 𝜑) (3.32) [5]

kde:

𝛿 =𝑥

𝑟 (3.33) [5]

Dosazením rovnice 3.32 do rovnice 3.31 získáme práci A:

𝐴 = 𝑀 (𝑥

𝑟+ 𝜑) (3.34) [5]

Parciální derivace práce A podle zobecněných souřadnic 𝑞 = 𝑥 a 𝑞 = 𝜑:

20

𝛿𝐴

𝛿𝑥=

𝑀

𝑟 (3.35) [5]

𝛿𝐴

𝛿𝜑= 𝑀 (3.36) [5]

Dosazením vybraných parciálních derivací (rovnice 3.19, 3.21, 3.28 a 3.35) do

rovnice 3.12 získáme Lagrangeovu rovnici II. druhu pro zobecněné souřadnice 𝑞 = 𝑥 a

�̇� = �̇�:

𝑚2�̈� + 𝑚2𝐿�̈� 𝑐𝑜𝑠 𝜑 − 𝑚2𝐿𝜑2̇ 𝑠𝑖𝑛 𝜑 + 𝑚1�̈� +𝐽

𝑟2�̈� − 0 + 0 =

𝑀

𝑟

(3.37)

[5]

Po úpravě získáme:

�̈� + 𝐿�̈� 𝑐𝑜𝑠 𝜑 − 𝐿𝜑2̇ 𝑠𝑖𝑛 𝜑 +𝑚1

𝑚2�̈� +

𝐽

𝑚2𝑟2�̈� =

𝑀

𝑚2𝑟 (3.38) [5]

�̈� (1 +𝑚1

𝑚2+

𝐽

𝑚2𝑟2) + 𝐿�̈� 𝑐𝑜𝑠 𝜑 − 𝐿�̇�2 𝑠𝑖𝑛 𝜑 =

𝑀

𝑚2𝑟 (3.39) [5]

�̈� =

𝑀𝑚2𝑟

− 𝐿�̈� 𝑐𝑜𝑠 𝜑 + 𝐿�̇�2 𝑠𝑖𝑛 𝜑

(1 +𝑚1

𝑚2+

𝐽𝑚2𝑟2)

(3.40)

Dosazením zbylých parciálních derivací (rovnice 3.22, 3.24, 3.29 a 3.36) do

rovnice 3.12 získáme Lagrangeovu rovnici II. druhu pro zobecněné souřadnice 𝑞 = 𝜑 a

�̇� = �̇�:

𝑚2�̈�𝐿 𝑐𝑜𝑠 𝜑 + 𝑚2𝐿2�̈� − 𝑚2�̇�𝐿�̇� 𝑠𝑖𝑛 𝜑 + 𝑚2�̇�𝐿�̇� 𝑠𝑖𝑛 𝜑 − 𝑚2𝑔𝐿 𝑠𝑖𝑛 𝜑 = 𝑀

(3.41)

[5]

Po úpravě získáme:

𝑚2�̈�𝐿 𝑐𝑜𝑠 𝜑 + 𝑚2𝐿2�̈� − 𝑚2𝑔𝐿 𝑠𝑖𝑛 𝜑 = 𝑀 (3.42) [5]

�̈� 𝑐𝑜𝑠 𝜑 + 𝐿�̈� − 𝑔 𝑠𝑖𝑛 𝜑 =𝑀

𝑚2𝐿 (3.43) [5]

21

�̈� =

𝑀𝑚2𝐿

− �̈� 𝑐𝑜𝑠 𝜑 + 𝑔 𝑠𝑖𝑛 𝜑

𝐿 (3.44)

Výsledkem jsou tedy rovnice 3.40 a 3.44, které je potřeba následně linearizovat.

22

Ob

r.

3.3

: B

loko

vé s

chém

a m

od

elu

vyt

voře

né

v p

rost

řed

í M

atl

ab

Sim

uli

nk

23

Na Obr. 3.3 můžeme vidět blokové schéma modelu robota vytvořené v prostředí

Matlab Simulink podle rovnic 3.40 a 3.44. Vstupem do soustavy je točivý moment,

výstupem pak úhel vychýlení.

3.2 Linearizace modelu

Tato podkapitola řeší obecnou linearizaci modelu popsaného rovnicemi 3.40 a

3.44 a následné číselné dosazení pro vzpřímenou polohu robota.

Rovnice 3.40 a 3.44 vyjádříme ve tvaru vhodném pro linearizaci:

�̈� = 𝑓(�̈�, �̇�,𝑀) (3.45)[6]

�̈� = 𝑓(�̈�, 𝜑,𝑀) (3.46) [6]

Zavedeme pomocnou proměnnou a, pro usnadnění linearizace

𝑎 = 1 +𝑚1

𝑚2+

𝐽

𝑚2𝑟2 (3.47)

Potom:

�̈� =−𝐿

𝑎𝑐𝑜𝑠 𝜑 ∙ �̈� +

𝐿

𝑎𝑠𝑖𝑛 𝜑 ∙ 𝜑 ̇ 2 +

1

𝑟𝑚2𝑎𝑀 (3.48)

�̈� = −𝑐𝑜𝑠 𝜑

𝐿�̈� +

𝑔

𝐿𝑠𝑖𝑛 𝜑 +

1

𝑚2𝐿2𝑀 (3.49)

Provedeme linearizaci rovnic 3.48 a 3.49 v obecném pracovním bodě:

𝛥�̈� = |−𝐿

𝑎𝑐𝑜𝑠 𝜑| 𝛥�̈� + |

2𝐿

𝑎𝑐𝑜𝑠 𝜑| 𝛥�̇� + |

1

𝑟𝑚2𝑎| 𝛥𝑀 (3.50)

𝛥�̈� = |−𝑐𝑜𝑠 𝜑

𝐿| 𝛥�̈� + |

𝑔

𝐿𝑐𝑜𝑠 𝜑 +

�̈� 𝑠𝑖𝑛 𝜑

𝐿| 𝛥𝜑 + |

1

𝑚2𝐿2| 𝛥𝑀 (3.51)

Dosadíme pracovní bod odpovídající vzpřímené poloze robota, tedy

φ = 0 :

𝛥�̈� =𝐿

𝑎𝛥�̈� +

1

𝑟𝑚2𝑎𝛥𝑀 (3.52)

𝛥�̈� =1

𝐿𝛥�̈� +

𝑔

𝐿𝛥𝜑 +

1

𝑚2𝐿2𝛥𝑀 (3.53)

24

3.3 Stavový popis

Linearizované rovnice 3.52 a 3.53 musí být dále upraveny za účelem sestavení

stavového popisu systému.

Rovnice upravíme do tvaru:

𝛥�̈� = 𝑓(𝛥𝜑,𝑀) (3.54) [6]

𝛥�̈� = 𝑓(𝛥𝜑,𝑀) (3.55) [6]

Tedy:

𝛥�̈� =𝐿

𝑎(1

𝐿𝛥�̈� +

𝑔

𝐿𝛥𝜑 +

1

𝐿2𝑚2𝛥𝑀) +

1

𝑟𝑚2𝑎𝛥𝑀 (3.56)

𝛥�̈� =𝑔

𝑎 − 1𝛥𝜑 + (

𝑎

(𝑎 − 1)𝐿2𝑚2+

1

(𝑎 − 1)𝑟𝑚2)𝛥𝑀 (3.57)

𝛥�̈� =1

𝐿(𝐿

𝑎𝛥𝜑 +

1

𝑟𝑚2𝑎𝛥𝑀) +

𝑔

𝐿𝛥𝜑 +

1

𝐿2𝑚2𝛥𝑀 (3.58)

𝛥�̈� =𝑔𝑎

(𝑎 − 1)𝐿𝛥𝜑 + (

𝑎

(𝑎 − 1)𝐿2𝑚2+

1

(𝑎 − 1)𝑟𝑚2)𝛥𝑀 (3.59)

Abychom se zbavili druhých derivací v rovnicích 3.57 a 3.59, zavedeme další dvě

stavové proměnné, rychlost vozíku Δ𝑣 a úhlovou rychlost vychýlení robota od

rovnovážné polohy Δ𝜔:

𝛥�̇� =𝑔𝑎

(𝑎 − 1)𝐿𝛥𝜑 + (

𝑎

(𝑎 − 1)𝐿2𝑚2+

1

(𝑎 − 1)𝑟𝑚2)𝛥𝑀 (3.60)

𝛥�̇� = 𝛥𝜔 (3.61)

𝛥�̇� =𝑔

𝑎 − 1𝛥𝜑 + (

𝑎

(𝑎 − 1)𝐿2𝑚2+

1

(𝑎 − 1)𝑟𝑚2)𝛥𝑀 (3.62)

𝛥�̇� = 𝛥�̇� (3.63)

Obecný stavový popis získáme převedením zjištěných rovnic do maticového

tvaru:

25

𝛥�̇� = 𝑨𝛥𝑥 + 𝑩𝛥𝑢 (3.64) [6]

𝛥𝑦 = 𝑪𝛥𝑥 + 𝑫𝛥𝑢 (3.65) [6]

Kde vektor stavů je:

𝑥 = [ 𝛥𝜔 𝛥𝜑 𝛥𝑣 𝛥𝑥 ]𝑇 (3.66) [6]

Potom matice systému vypadají následovně:

𝑨 =

[ 0

𝑔𝑎

(𝑎 − 1)𝐿0 0

1 0 0 0

0𝑔

𝑎 − 10 0

0 0 1 0]

(3.67)

𝑩 =

[ (

𝑎

(𝑎 − 1)𝐿2𝑚2+

1

(𝑎 − 1)𝑟𝑚2)

0

(𝑎

(𝑎 − 1)𝐿2𝑚2+

1

(𝑎 − 1)𝑟𝑚2)

0 ]

(3.68)

𝑪 = [0 1 0 0] (3.69)

𝑫 = 0 (3.70)

3.4 Číselné dosazení a zjištění přenosu soustavy

Následují poslední dva kroky identifikace, číselné dosazení a určení přenosu

soustavy.

Nejprve číselně dosadíme známé konstanty do již zjištěných matic. Zde jsou

uvedeny všechny známé konstanty s jednotkami:

𝑔 = 9.81𝑚/𝑠2

𝐿 = 0.16 𝑚

𝑚1 = 0.083 𝑘𝑔

𝑚2 = 0.187 𝑘𝑔

26

Dále je potřeba dopočítat moment setrvačnosti 𝐽. Ten se skládá z momentu

setrvačnosti kol 𝐽𝐾 a z momentu setrvačnosti těla robota 𝐽𝑇.

Tělo robota bylo pro zjednodušení uvažováno za pravidelný kvádr. Rovnice 3.71

byla použita pro výpočet momentu setrvačnosti těla robota:

𝐽𝑇 =

1

12𝑚2(𝐿

2 + 𝑑2) =1

120.187(0,162 + 0.0452)

= 4.3 ∙ 10−4 𝑘𝑔 ∙ 𝑚2 (3.71)[7]

kde 𝑑 je šířka konstrukce (𝑑 = 0,045 𝑚).

Při výpočtu momentu setrvačnosti kola jsme jej uvažovali jako válec.

Rovnice momentu setrvačnosti válce je:

𝐽𝐾 =1

2𝑚1𝑟

2 =1

20.083 ∙ 0.057152 = 1.36 ∙ 10−4 𝑘𝑔 ∙ 𝑚2 (3.72) [7]

Výsledný moment setrvačnosti je dán součtem těchto dvou momentů setrvačnosti:

𝐽 = 𝐽𝑇 + 𝐽𝐾 = 4.3 ∙ 10−4 + 1.36 ∙ 10−4 = 5.66 ∙ 10−4𝑘𝑔 ∙ 𝑚2 (3.73) [7]

Před dosazováním ještě vypočteme hodnotu pomocné proměnné a, kterou jsme

zavedli na počátku linearizace:

𝑎 = 1 +𝑚1

𝑚2+

𝐽

𝑚2𝑟2= 1 +

0.083

0.187+

5.66 ∙ 10−4

0.187 ∙ 0.057152= 2.3706 (3.74)

A konečně můžeme vyčíslit zjištěné matice:

𝑨 = [

0 106.0462 0 01 0 0 00 7.1574 0 00 0 1 0

] (3.75)

𝑩 = [

787.98220

787.98220

] (3.76)

𝑪 = [0 1 0 0] (3.77)

𝑫 = 0 (3.78)

Tím jsme získali stavový popis modelu našeho robota. Za pomoci programu

Matlab zjistíme použitím funkce tf() přenosovou funkci soustavy:

27

𝐹𝑆(𝑠) =788

𝑠2 − 1.776 ∙ 10−15 − 106 (3.79)

Přenos soustavy z rovnice 3.79 můžeme vyjádřit jako:

𝐹𝑆(𝑠) =787.98

(𝑠 − 10.3)(𝑠 + 10.3) (3.80)

Jedná se o soustavu druhého řádu se dvěma póly p1,2 = ±10.3 a žádnou nulou.

4 NÁVRH REGULÁTORU

Při návrhu regulátoru je v prvé řadě potřeba určit jaký typ regulátoru aplikujeme

na námi určenou soustavu. Je třeba zvážit všechny klady a zápory a následně vybrat

nejvhodnější typ.

Abychom mohli podrobně rozebrat různé typy regulátorů a jejich vlastnosti,

potřebujeme znám blokové schéma uzavřeného regulačního obvodu, které je vyobrazeno

na Obr. 4.1.

Obr. 4.1: Blokové schéma uzavřeného regulačního obvodu [8]

kde

𝑆 řízená soustava

𝑅 regulátor

𝜔(𝑡) řídící (žádaná) hodnota

𝑒(𝑡) regulační odchylka

𝑥(𝑡) akční zásah

R S ω(t) e(t) x(t)

v1(t) v2(t)

y(t)

28

𝑣1(𝑡) porucha na vstupu soustavy

𝑣2(𝑡) porucha na výstupu soustavy

𝑦(𝑡) výstup soustavy

Regulační odchylka je definována jako rozdíl žádané a výstupní hodnoty:

𝑒(𝑡) = 𝜔(𝑡) − 𝑦(𝑡) (4.1)

4.1 Vlastnosti základních regulátorů

4.1.1 PID regulátor

Základní regulátory se dělí podle přítomnost jedné ze základních složek nebo

jejich kombinace:

P – proporciální

D – derivační

I – integrační

Proporciální složka P sníží dobu náběhu a ustálenou odchylku systému od

požadované hodnoty, ale nikdy ji neeliminuje. Integrační složka I eliminuje ustálenou

odchylku systému od požadované hodnoty, ale doba odezvy systému se prodlouží.

Derivační složka D zvýší stabilitu systému, sníží překmit a zkrátí dobu na ustálení

požadované hodnoty.[9]

Pro přehled jsou v Tabulka 4.1 uvedeny vlastnosti jednotlivých složek regulátoru,

respektive jejich vliv na chování systému.

Složka Doba náběhu Překmit Doba ustálení Ustálená

odchylka

P snižuje zvyšuje malá změna snižuje

I snižuje zvyšuje zvyšuje eliminuje

D malá změna snižuje snižuje žádná změna Tabulka 4.1: Vlastnosti jednotlivých složek PID regulátoru [9]

Vzhledem k těmto informacím můžeme označit regulátor PID za nejlépe

vyhovující našim požadavkům na regulaci vzhledem k jeho komplexnosti.

Je potřeba najít takové parametry regulátoru, aby výsledný systém splňoval tyto

požadavky:

co nejrychlejší odezvu systému

co nejmenší překmit

29

eliminace odchylky od ustálené hodnoty

co nejrychlejší ustálení požadované hodnoty

Mezi těmito parametry je potřeba najít vhodný kompromis a tím dosáhnout co

nejkvalitnější regulace. Za vhodný typ regulátoru byl zvolen regulátor typu PID,

respektive jeho diskrétní ekvivalent. Složka I zajistí eliminaci odchylky od ustálené

hodnoty, složka D sníží překmit a dobu ustálení regulačního děje. Samozřejmostí je i P

složka, jelikož samotný ID regulátor logicky nemůže existovat.

Časový průběh PID regulátoru vyjadřuje rovnice 4.2

𝑥(𝑡) = 𝑟0𝑒(𝑡) + 𝑟𝑑𝑑𝑒(𝑡)

𝑑𝑡+ 𝑟𝑖 ∫ 𝑒(𝑡)𝑑𝑡 + 𝑥(0)

𝑡

0

(4.2)[8]

Takže přenos je:

𝐹𝑅(𝑠) =𝑋(𝑠)

𝐸(𝑠)= 𝑟0 +

𝑟𝑖𝑠

+ 𝑟𝑑𝑠 = 𝐾𝑅 (1 + 𝑇𝐷𝑠 +1

𝑇𝐼𝑠)

= 𝑘𝑟 (𝑇1𝑠 + 1)(𝑇2𝑠 + 1)

𝑠

(4.3)[8]

Mezi jednotlivými vyjádřeními platí následující vztahy:

𝐾𝑅 = 𝑟0 (4.4)[8]

𝑇𝐷 =𝑟𝑑𝑟0

(4.5)[8]

𝑇𝐼 =𝑟0𝑟𝑖

(4.6)[8]

𝑇1,2 =−𝑇𝐼 ± √𝑇𝐼(𝑇𝐼 − 4𝑇𝐷)

2𝑇𝐼𝑇𝐷 (4.7)[8]

Důležitý tvar je s konstantami 𝐾𝑅 zesílení, TD derivační složka a 𝑇𝐼 integrační

složka (paralelní spojení P, I a D regulátorů). Umožňuje realizovat omezení akčního

zásahu a zamezit přebuzení integrační složky. Poslední tvar není rovnocenný předchozím

dvěma tvarům – neumožňuje zvolit komplexní nuly regulátoru, což není většinou pro

praktické použití důležité. [8]

4.1.2 PSD regulátor

PSD (proporcionálně sumačně diferenční) regulátor je diskrétním ekvivalentem

regulátoru PID. Při vytváření PSD regulátoru se vychází z rovnice přenosu:

30

𝐹𝑅(𝑠) = 𝐾𝑅 (1 + 𝑇𝐷𝑠 +1

𝑇𝐼𝑠) (4.8)[8]

Jejíž ekvivalentní diskrétní přenosová funkce pro PSD regulátor vypadá

následovně:

𝐹𝑅(𝑧−1) = 𝐾𝑅 (1 +𝑇𝐷

𝑇(1 − 𝑧−1) +

𝑇

𝑇𝐼

1

1 − 𝑧−1) (4.9)[8]

Kde 𝑇 je perioda vzorkování (𝑇 = 0.002 𝑠).

4.2 Návrh samotného regulátoru

Pro návrh samotného regulátoru bylo po delších úvahách využito prostředí

PIDTool dostupné za pomoci funkce pidtool(), kterou nabízí program Matlab.

V tomto prostředí je možno manuálně nastavovat parametry PID regulátoru a sledovat

odezvu soustavy na jednotkový skok.

Vzhledem k tomu, že mikroprocesor pracuje v diskrétním režimu, je potřeba

převést soustavu ne její diskrétní ekvivalent a následně navrhovat PSD regulátor.

Diskrétní přenos soustavy byl získán pomocí funkce c2d()s vzorkovací periodou 2ms

v programu Matlab a vypadá následovně:

𝐹𝑅(𝑧) =0.001576𝑧 + 0.001576

𝑧2 − 2𝑧1 + 1 (4.10)

S ohledem na hodnotu překmitu, dobu ustálení, akční zásah a stabilitu systému

bylo experimentálně dosaženo jistých parametrů, které byli následně odladěny na reálném

modelu, jelikož, jak se předpokládalo, matematický model neodpovídá naprosto přesně

modelu reálnému.

Parametry regulátoru byly nastaveny následovně:

𝐾𝑃 = 4.48 𝐾𝐼 = 7.01 𝐾𝐷 = 0.1

Potom je rovnice 4.11 rovnicí výsledného regulátoru, kde 𝑇𝐹 je realizační časová

konstanta.

𝐹𝑅(𝑧) = 𝐾𝑃 + 𝐾𝐼

𝑇

𝑧 − 1+ 𝐾𝐷

1

𝑇𝑧 − 1 + 𝑇𝐹

(4.11)

𝐹𝑅(z) = 4.48 + 7.010.002

𝑧 − 1+ 0.1

1

0.002𝑧 − 1 + 0.001

= 4.48 +0.014

𝑧 − 1+

0.1

0.002𝑧 − 1 + 0.001

31

4.2.1 Základní odezvy soustavy s navrhnutým regulátorem

Přenos řízení:

𝐹𝑊(𝑧) =𝑌(𝑧)

𝑊(𝑧)=

𝐹𝑅(𝑧)𝐹𝑆(𝑧)

1 + 𝐹𝑅(𝑧)𝐹𝑆(𝑧) (4.12)[8]

𝐹𝑊(𝑧) =0.1636𝑧3 − 0.1571𝑧2 − 0.1636𝑧 + 0.1572

𝑧4 − 2.971𝑧3 + 3.249𝑧2 − 1.57𝑧 + 0.2925 (4.13)

Obr. 4.2: Odezva na jednotkový skok řízení

Přenos poruchy:

𝐹𝑈(𝑧) =𝑌(𝑧)

𝑈(𝑧)=

𝐹𝑆(𝑧)

1 + 𝐹𝑅(𝑧)𝐹𝑆(𝑧) (4.14)[8]

𝐹𝑈(𝑧) =0.0016𝑧5 − 0.0034𝑧4 + 0.00043𝑧3 + 0.0032𝑧2 − 0.002𝑧 + 0.00021

𝑧6 − 4.973𝑧5 + 10.19𝑧4 − 11.04𝑧3 + 6.682𝑧2 − 2.155𝑧 + 0.2925 (4.15)

32

Obr. 4.3: Odezva na jednotkový skok poruchy

Přenos odchylky:

𝐹𝐸(𝑧) = 𝐹𝜖(𝑧) =𝐸(𝑧)

𝑊𝑧=

1

1 + 𝐹𝑅(𝑧)𝐹𝑆(𝑧) (4.16)[8]

𝐹𝐸(𝑧) =𝑧4 − 3.136𝑧3 + 3.406𝑧2 − 1.406𝑧 + 0.1353

𝑧4 − 2.972𝑧3 + 3.249𝑧2 − 1.57𝑧 + 0.2925 (4.17)

Obr. 4.4: Odezva na jednotkový skok regulační odchylky

Přenos akční veličiny:

𝐹𝐴(𝑧) =𝑋(𝑧)

𝑊(𝑧)=

𝐹𝑅(𝑧)

1 + 𝐹𝑅(𝑧)𝐹𝑆(𝑧) (4.18)[8]

33

𝐹𝐴(𝑧) =103.8𝑧6 − 529.1𝑧5 + 1092𝑧4 − 1152𝑧3 + 639.9𝑧2 − 167.8𝑧 + 13.5

𝑧6 − 4.107𝑧5 + 6.759𝑧4 − 5.661𝑧3 + 2.514𝑧2 − 0.5445𝑧 + 0.0396 (4.19)

Obr. 4.5: Velikost akčního zásahu

5 STAVBA ROBOTA

5.1 Podvozek

V první fázi stavby robota byl vyroben jeho podvozek. Podvozek se skládá

z hlavní kostry, na které je, po každé straně přimontován jeden motor, nesoucí kolo

s pneumatikou.

Průměr kol i s pneumatikou je 2,25“, což odpovídá 5,715 cm. Šířka pneumatiky

je zhruba 2 cm což zajistí lepší držení stability i lepší odolnost vůči nerovnostem. Výška

samotné konstrukce je 43 mm, šířka 45mm a délka 100mm. Jelikož jsou kola připevněna

zhruba v polovině výšky, konstrukce tedy nikde nepřesahuje. Vzájemná vzdálenost středů

kol je 134 mm.

Obr. 5.1: Diferenciální podvozek robota

34

5.2 Konstrukce

Dále byly do podvozku vyvrtány otvory pro montáž závitových tyčí, které složí

jako nosný prvek pro všechny periferie. Průměr závitových tyčí byl zvolen 6mm a délka

19mm. Tyče jsou upevněny několika kontramatkami jak je vidět na obrázku Obr. 5.4

popř. Obr. 5.5.

Obr. 5.2: Vyvrtané otvory pro montáž závitových tyčí

Rozměry a umístění otvorů stejně jako celkové rozměry konstrukce můžeme vidět

v technickém výkresu na Obr. 5.3.

Obr. 5.3: Technický výkres podvozku

Po montáži závitových tyčí byla do výšky 155mm od vrchní desky podvozku

umístěna plastová destička o rozměrech 100x75mm určená pro připevnění senzoru a

zavěšení akumulátoru.

35

Obr. 5.4: Kompletní konstrukce robota

5.3 Montáž periferií

Na podvozek byla připevněna deska plošných spojů (DPS), vzhledem k velikosti

a umístění montážních otvorů na DPS musí být umístěna v šikmé poloze vzhledem k ose

podvozku. Pro připevnění byly použity předem vyvrtané otvory o průměru 3,5mm jak je

vidět na Obr. 5.2 nebo Obr. 5.3. DPS od podvozku izoluje menší kousek izolačního

materiálu z pěny, který zamezí styku vodivých častí DPS s kovovým podvozkem.

Na horní destičku byl přimontován senzor měřící výchylku. Komunikaci s DPS

zajišťují vodiče vhodně připevněné k závitovým tyčím, aby volně nevisely v prostoru.

Pod touto destičkou byl následně upevněn akumulátor, který slouží zároveň jako

závaží pro lepší stabilitu robota. Akumulátor je zapojen do DPS přes kolébkový spínač,

kterým zapínáme a vypínáme robota. Taktéž je možné akumulátor nabíjet ve stavu kdy je

namontován díky korekčním konektorům na vodičích akumulátoru.

5.4 Hotový robot

Ve finále byly připojeny motory do DPS. Vzhledem k domácí výrobě konstrukce

není každý rozměr naprosto přesný, ale celkově je konstrukce funkční. Finální podobu

robota můžeme vidět na Obr. 5.5.

36

Obr. 5.5: Finální vzhled robota

6 HARDWARE

Obr. 6.1: Blokové schéma robota

6.1 Deska plošných spojů

Deska plošných spojů, neboli DPS, byla dodána vedoucím této práce. Jedná se o

desku Martinez V3. Její součástí je FTDI USB řídící čip, který zajišťuje komunikaci přes

USB. Na desce dále nalezneme 2 můstky pro bezkartáčové motory L6234D od firmy

STMicroelectronics, výstupy a vstupy pro měřící jednotku (senzor) komunikující po

37

sběrnici I2C, výstupy pro dva motory, napájecí kontakty a jejím jádrem je mikroprocesor

ATmega328 od firmy Atmel. Nacházejí se zde i analogové vstupy a výstupy, které nejsou

využity. Samozřejmostí jsou notifikační světelné diody signalizující připojené napájení

či komunikaci s PC. [10]

Vzhled desky s popisem všech častí můžeme vidět na Obr. 6.2.

Obr. 6.2: Martinez V3 [10]

Kompletní schéma zapojení DPS je pak na Obr. 6.3.

38

Ob

r.

6.3

: S

chém

a z

apo

jen

í D

PS

[1

1]

39

6.2 Senzory

Jediný senzor, kterým náš robot disponuje, je IMU (inertial measurement unit –

inerciální měřící jednotka). Jedná se elektronické zařízení, které měří rychlost, polohu a

gravitační síly použitím kombinace akcelerometru a gyroskopu, někdy také

magnetometru.

V naší aplikaci je jako IMU použit InvenSense MPU-6050, což je snímač, který

obsahuje MEMS gyroskop a akcelerometr v jednom jediném čipu. Jelikož využívá

16bitový AD převodník pro každý kanál, je velmi přesný a je schopný měřit ve třech

osách současně. S procesorem na DPS komunikuje po I2C sběrnici. [12]

Obr. 6.4: Deska GY-521 s MPU 6050 [10]

6.2.1 Princip MEMS gyroskopu

Gyroskopy jsou určené pro měření úhlové rychlosti. Rotaci je možné typicky

měřit ve třech osách (x,y,z). Integrované MEMS gyroskopy pracují na principu

Coriolisovy síly.

Coriolisova síla je tzv. virtuální síla, která působí na libovolný hmotný objekt,

který se pohybuje rychlostí 𝑣 v soustavě rotující kolem své osy rotace úhlovou rychlostí

𝜔:

𝐹𝐶 = −2𝑚𝜔 × 𝑣 (4.1)[13]

kde × je vektorový součet.

Při použití technologie MEMS se vytváří na čipu spolu s elektronickými obvody

i mechanické mikrosoučástky, které tvoří samotný snímač. Základem je periodicky se

pohybující struktura přesně dané hmotnosti upevněná pomocí pružin v rámu. Směr

pohybu musí však vždy být kolmý ke směru otáčení. Za těchto podmínek vzniká a na

hmotnou pohybující se část snímače působí Coriolisova síla, jejíž velikost je úměrná

úhlové rychlosti otáčení. Ta způsobuje stlačení vnějších pružin rámu a způsobí vzájemný

posuv měřících plošek fungujících jako elektrody vzduchových kondenzátorů. Výstupem

je tedy změna kapacity úměrná úhlové rychlosti otáčení. [13]

40

Obr. 6.5: Princip MEMS gyroskopu [13]

Obr. 6.6: Detail mechanického snímače gyroskopu [13]

6.2.2 Princip MEMS akcelerometru

Princip MEMS akcelerometru je prakticky stejný jako u gyroskopu až na jeden

rozdíl. U akcelerometru je mechanická část volná a hýbe se jen při působení síly

(zrychlení) zatímco u gyroskopu je touto mechanickou části záměrně pohybováno, aby

se projevil efekt Coriolisovy síly.[13]

6.3 Motory

K dispozici byly dva BLDC motory. Pojem BLDC motor vyjadřuje výraz

brushless DC motor (bezkartáčový stejnosměrný motor) nebo taky EC motor

(electronically comutated – elektronicky komutovaný), jejich název není pevně ucelen,

přesto se jedná o stále stejný princip.

Vznik BLDC motorů byl podnícen problémy s komutací stejnosměrných motorů

v malých aplikacích. Komutace kluzným kontaktem je nahrazena elektronickou

komutací.

Uspořádání pevných a rotujících součástí BLDC motoru je vzhledem ke

komutátorovému motoru obrácené. U běžného komutátorového motoru je budící část

pevná (stator) a rotuje vinutí napájené přes kartáče a komutátor (rotor). Motor BLDC má

pevné vinutí statoru a rotuje budič, obvykle osazený permanentními magnety. Řídící

jednotka přepíná jednotlivá vinutí statoru (vytváří iluzi třífázového střídavého napájení)

a tím motor roztáčí. [14]

41

Obr. 6.7: Princip DC motoru [13]

Obr. 6.8: Princip BLDC motoru [13]

V našem případě používáme motoru typu HJ2208 KV90, což jsou motoru určené

pro použití při stabilizaci. Tento typ využívá pro svojí funkčnost 14 pólů. Označení KV90

značí 90ot./min. na 1V. Bohužel tyto malé motory dovážené z Asie mají velmi špatnou

podporu v podobě dokumentace, a proto lze o tomto motoru zjistit velmi málo informací.

6.4 Akumulátor

Pro robota byla zvolen Li-Pol akumulátor s označením FlightPower EVO 25 2170

3S 11.1V. Rozměry akumulátoru jsou 110 x 34 x 24mm, hmotnost pak 174g. Jedná se o

tříčlánkový Li-Pol akumulátor, každý článek má napětí 3,7V, celý akumulátor tedy 11,1V

jak je uvedeno v označení. Jeho kapacita je více než dostačující, a to 2170mAh. Souhrn

vlastností je uveden v Tabulka 6.1.

Kapacita akumulátoru 2170mAh

Počet článků 3

Napětí jednoho článku 3,7V

Celkové napětí akumulátoru 11,1V

Stálý proud 25C = 54,3A

Krátkodobý proud 35C = 76A

Maximální špičkový proud 50C = 108,5A

Tabulka 6.1: Souhrn vlastností akumulátoru

6.4.1 Li-Pol akumulátor

Li-Pol je zkratka pro Lithium-Polymer, jedná se o evoluci starších Li-Ion

akumulátorů. Tento typ akumulátoru používá namísto tekutého elektrolytu elektrolyt

pevný. Katoda sestává z oxidu lithia a kobaltu nebo z oxidu lithia, niklu a kobaltu. Anodu

tvoří sloučeniny grafitu. V současné době jsou velmi oblíbené v modelářství do RC

modelu jakéhokoliv druhu díky jejich relativně malým rozměrům, váze, kapacitě a

rychlosti dobíjení. [15]

42

Vlastnosti Li-Pol akumulátoru:

netrpí paměťovým efektem – před nabíjením není třeba vybíjet

dobrá doba skladovatelnosti – ztráta 0,1% za měsíc

dají se vyrobit prakticky v jakémkoliv tvaru/rozměru

jsou relativně lehké

záporem je vyšší cena

s postupem času ztrácí kapacitu rychleji než původní Li-Ion

akumulátor [15]

7 SOFTWARE

7.1 Platforma Arduino

Celý projekt robota je vytvářen v prostředí Arduino. Arduino je otevřená

elektronická platforma založená hlavně na jednoduchosti co se týče hardwaru i softwaru.

V dnešní době můžeme považovat tuto platformu za jednu z nejpoužívanějších a

nejoblíbenějších v oblasti jednoduché elektroniky a řízení a to ať už pro amatéry

elektroniky nebo i celou řadu akademických prací.

Platforma je schopna vnímat okolní prostředí pomocí vstupů z rozličných senzorů.

Zároveň může své okolí i ovlivňovat připojitelnými LED, motory a mnohými dalšími

výstupními periferiemi.

Mikroprocesor na desce Arduina se programuje speciálním Arduino

programovacím jazykem, založeným na jazyku Wiring, což je obdoba jazyka C, ve

speciálním Arduino vývojovém prostředí. Projekty založené na platformě Arduino

mohou též jednoduše komunikovat se softwarem v PC.

Mezi hlavní výhody této platformy patří nízká cena, jednoduché programování,

s tím souvisí i spousty dostupných knihoven, jednoduché zapojení, velká komunita a tedy

hodně návodů a nápadu napříč celým internetem a v neposlední řadě také nezávislost vůči

platformě PC (Windows, Linux, Mac, atd.) [16]

V tomto projektu byla použita deska Arduino by Martinez V3 od autorů z FPV-

community.de založená na procesoru ATmega328 od firmy Atmel, jak již bylo

vzpomínáno výše.

7.2 Práce s IMU

Jako IMU bylo zvoleno MPU-6050. V této části se zaměříme na způsob

komunikace s MPU.

S MPU lze komunikovat dvěma způsoby. Jednodušší způsob je získávání

surových dat, ten složitější pracuje s DMP (Digital Motion Processor), který umožňuje

provádět výpočty s hodnotami z gyroskopu a akcelerometru, popřípadě ještě s externě

43

připojeným magnetometrem. Za další výhodu můžeme považovat fakt, že DMP využívá

vlastní filtr s názvem InvenSense’s MotionFusion™. Není tak potřeba vytvářet vlastní

filtr.

Bylo využito výborných vlastností DMP a práce se tedy ubírá druhou výše

popisovanou cestou. Ulehčí to práci hlavně procesoru na DPS.

Nejdůležitější je pro nás knihovna i2cdevlib, která se dokáže se všemi výpočty

spojenými s DMP a surovými hodnotami z gyroskopu a akcelerometru snadno vyrovnat

zavoláním několika stěžejních metod. Získáváme tak úhly, ve kterých se senzory nachází

a nám už zbývá jen tyto hodnoty přečíst a dále zpracovat. Celá komunikace mezi

Arduinem a MPU probíhá po I2C sběrnici rychlostí 400kHz. Pro přenos dat využívá

1024bytový FIFO (first-in first-out) zásobník. [17]

7.2.1 Nastavení IMU

V prvé řadě se inicializuje I2C sběrnice (Wire.begin()) a poté se přiřadí

připojené IMU příkazem mpu.initialize() kde mpu je název třídy MPU6050.

Následně se ověří funkčnost připojení příkazem mpu.testConnection() a pokud

je připojení úspěšné tak se inicializuje DMP příkazem mpu.dmpInitialize(), který

vrací hodnotu 0, je-li inicializace úspěšná. Pokud i inicializace proběhla v pořádku, povolí

se DMP (příkaz mpu.setDMPEnabled(true)) a nastaví se přerušení pro Arduino

(příkaz attachInterrupt(0, dmpDataReady, RISING), kde první parametr

označuje číslo přerušení, druhý parametr funkce, která bude při přerušení volána a

poslední, třetí, parametr značí, že přerušení nastane při změně hodnoty z úrovně L do H.

Funkce dmpDataReady() pouze nastavuje proměnnou mpuInterrupt na hodnotu

true, která nám v hlavní smyčce dává informaci, zda jsou k dispozici nějaká data

k příjmu od IMU.

Celé vyčítání hodnot poté probíhá v hlavní smyčce ( void loop()), která se

provádí s frekvencí 500Hz, tedy 500krát za vteřinu. Samotné přijímání dat probíhá

v následujících krocích:

1. zjištění zda došlo k přerušení (mpu.getIntStatus())

2. zjištění počtu bytů v zásobníku (mpu.getFIFOCount())

3. vyčtení zásobníku po paketech

4. získání kvaterionu z přijatých dat (mpu.dmpGetQuaternion())

5. následné zjištění gravitace pomocí kvarterionu

(mpu.dmpGetGravity())

6. a z těchto informací získání úhlů vychýlení ve všech třech osách již

v radiánech (mpu.dmpGetYawPitchRoll())

IMU má několik stupňů citlivosti jak ukazuje Tabulka 7.1.

44

Stupeň

citlivosti

Gyroskop Akcelerometr

Citlivost

v celém

rozsahu

Citlivost

Citlivost

v celém

rozsahu

Citlivost

[°/s] [LSB°/s] [g] [LSB/g]

0 ±250 131 ±2 16384

1 ±500 65.5 ±4 8192

2 ±1000 32.8 ±8 4096

3 ±2000 16.4 ±16 2014

Tabulka 7.1: Stupně citlivosti MPU 6050 [18]

V naší aplikaci bylo využito největší citlivosti jak u gyroskopu, tak u

akcelerometru – stupeň citlivosti 0.

7.2.2 Nastavení offsetu IMU

Každý gyroskop popř. akcelerometr má nějaký offset. Offset je pojem, který

určuje velikost odchylky hodnoty gyroskopu v počátečním bodě od reálné (nebo

požadované) hodnoty. Tato odchylka je dána už při výrobním procesu, kdy samotná

měřící jednotka většinou není dokonale přesně umístěná, nebo při montáži na samotné

zařízení, kdy namontovat senzor do dokonale vodorovné polohy je prakticky nereálné.

Kvůli tomu se zavádí tzv. offsetování, při němž se umístí měřící jednotka do

požadované polohy a vyčítají se hodnoty z gyroskopu a akcelerometru. Tyto data se

zpracují a statisticky se zjistí průměrná odchylka od předpokládané hodnoty. Zjištěné

průměrné odchylky se dosadí do programu jako offset v dané ose (např. funkce

setXGyroOffset() – nastavení offsetu pro gyroskop v ose X) a celý proces se

opakuje znovu. Tento proces opakujeme do té doby, než dosáhneme požadované

přesnosti.

7.3 I2C sběrnice

I2C (Internal Integrated Circuit) sběrnice je interní datová sběrnice sloužící pro

komunikaci a přenos dat mezi jednotlivými integrovanými obvody. Hlavní výhodou je,

že obousměrný přenos probíhá pouze na dvou vodičích – data (SDA – serial data) a

hodiny (SCL – serial clock), což především u mikrokontrolérů optimalizuje nároky na

počet vstupně-výstupních pinů a celkově zjednodušuje zapojení.

Pracuje v režimu MASTER-SLAVE, kde většinou (nemusí být pokaždé), je jeden

obvod MASTER a ostatní SLAVE. V našem případě je Arduino MASTER a MPU je

SLAVE. [19]

Popis přenosu po I2C sběrnici:

1) Stav klidu

logické jedničky na obou vodičích

45

MASTER negeneruje hodinový signál a tedy neprobíhá žádný přenos

2) Start bit

úroveň SDA se změní z logické jedničky na nulu

SCL zůstává v logické jedničce

zahajuje přenos

3) Přenos dat

data se přenáší po jednou byte, tedy po osmi bitech

po přenosu dat se může logická úroveň SDA změnit pouze tehdy, je-li SCL

v logické nule

při každém pulsu na SCL je přenesen jeden bit

4) Potvrzující bit ACK (acknowledge)

potvrzuje správné přijmutí dat

odesílá se stejným způsobem jako data s tím rozdílem, že je odesílán

opačnou stranou, tedy stranou, která data přijímala

úspěšný přenos signalizuje logická nula na SDA

neúspěšný přenos signalizuje logická jednička na SDA

pokud má dojít k ukončení přenosu neodešle se nic

5) Stop bit

Úroveň SDA se změní z logické nuly na jedničku

SCL zůstává v logické jedničce

ukončuje přenos

může být generován pouze po přijmutí ACK v logické jedničce, tedy

nepotvrzení přenosu [19]

Obr. 7.1: Princip fungování I2C sběrnice [19]

7.4 Řízení motorů

Řízení motorů je vyřešeno změnou polohy kola. Aktualizování této polohy

probíhá v přerušení s frekvencí 500Hz (tedy každé 2ms) a je vyvoláno přetečením

časovače TIMER1.

46

Nejprve se provede inicializace pinů, na které jsou můstky připojeny (jedná se o

piny číslo 3,5,6,9,10 a 11). Dále se naplní proměnná typu pole pwmSinMotor 256

hodnotami z jedné periody funkce sinus. Z tohoto pole se poté bude vybírat hodnota

funkce sinus odpovídající současné hodnotě PWM.

Samotná aktualizace motorů probíhá v hlavní smyčce programu (void loop())

s frekvencí 500Hz, koresponduje tedy s přerušením od časovače TIMER1. V této smyčce

se volá funkce MoveMotorPosSpeed(), která má tři parametry. Prvním parametrem

je číslo motoru (0 nebo 1), druhým parametrem je výstup z regulátoru, a tedy hodnota

PWM pro otáčení motorů a třetím parametrem je hodnota maximálního PWM (pro případ

přehřívaní motorů atp.), která je v našem případě na maximum (tedy 255). Tato funkce

převede jakoukoliv vstupní hodnotu na hodnotu z rozsahu 0-255, prostým logickým

součinem dané hodnoty z hexadecimálním číslem 0xFF. Tato hodnota se přiřadí hodnotě

z tabulky sinus uložené v proměnné pwmSinMotor pro první ‚fázi‘ motoru. Druhá

fáze je posunutá od první od 85 hodnot a třetí o dalších 85 hodnot, tedy 170 od původní

hodnoty. Tím jsme získali potřebné napětí pro všechny tři fáze motorů. Na takto získané

hodnoty se ještě aplikuje výkonový faktor daný třetím parametrem funkce

MoveMotorPosSpeed(). Tyto hodnoty jsou uloženy do globálních proměnných a při

přerušení od časovače TIMER1 se odešlou na vstupní piny můstků, které vytváří třífázové

sinusové napětí pro motory.

7.4.1 PWM – Pulsně šířková modulace

Pulsně šířková modulace je způsob kódování dat, která se přenášejí z vysílacího

zařízení k přijímacímu zařízení zvolenou přenosovou cestou.

Jde o signál s konstantní periodou T, kde se mění střída napětí (poměr délky

impulsu k délce mezery v jedné periodě). [20]

Princip PWM:

100%

90%

50%

10%

T

Obr. 7.2: Princip pulsně šířkové modulace [20]

H

H

H

H

L

L

L

L

47

Na Obr. 7.2 vidíme princip PWM.

1) 100% - výstup je po celou dobu periody na úrovni H

2) 90% - výstup je v 90% periody na úrovni H, 10% na úrovni L

3) 50% - výstup je polovinu doby periody na úrovni H, druhou polovinu na

úrovni L

4) 10% - výstup je v 10% periody na úrovni H, 90% na úrovni L

Perioda T je velmi krátká doba – perioda vzorkování je velmi vysoká, hodnota

napětí na výstupu se tedy jeví jako poměrná hodnota maximálního napětí a poměru doby

na H a L úrovní. Odpovídá-li tedy úroveň H=5V, při nastavené střídě 100% bude po celou

dobu periody na výstupu 5V. Při nastavené střídě 50% se bude výstup jevit jako by na

něm bylo po celou dobu 2,5V (polovina z 5V), i když technicky je na výstupu polovinu

periody hodnota 5V a druhou polovinu perioda hodnota 0V.

7.5 Regulátor

Pro výpočet regulátoru byla vytvořena funkce ComputePID(), do níž vstupuje

současná hodnota úhlu vychýlení robota z rovnovážné polohy (proměnná Input),

požadovaná hodnota, jež je rovna nule (proměnná SetPoint) a parametry PID

regulátoru (proměnné Kp, Ki a Kd).

V těle této funkce pak probíhá samotný výpočet regulátoru podle rovnice 4.2.

Nejprve se zjistí regulační odchylka (rozdílem požadované a současné hodnoty). Tato

odchylka se vynásobí zesílením I složky regulátoru Ki a přičte se do proměnné ITerm,

která je v prvním kroku nulová. Poté se vypočte derivace vstupu, a to rozdílem současné

hodnoty výchylky a předchozí hodnoty. Ve výsledné rovnici se vynásobí zesílení P složky

regulátoru Kp s regulační odchylkou, přičte se proměnná ITerm a nakonec odečte součín

zesílení D složky regulátoru Kd a derivace vstupu. Nakonec se ještě ověří, zdali je

výstupní hodnota regulátoru v předem vymezených mezích.

Výstupem je hodnota PWM, která se přivádí na vstup obou motorů, vynásobená

realizační konstantou, která byla zjištěna experimentálně.

8 VYHODNOCENÍ

Finální podobu robota můžeme vidět na Obr. 5.5. Při testování robota bohužel

nebylo dosaženo úspěšného dlouhodobého držení stabilní vzpřímené polohy. Robot má

tendenci držet stabilitu, ale s postupem času začne tělo kmitat a nastane pád. V jisté části

práce byl robot schopný udržet se více jak deset vteřin, ale s postupem času, jak se měnilo

tělo robota a kód programu, se již nepodařilo do tohoto stavu dostat. V současné době je

robot schopný držet ve vzpřímené poloze zhruba dvě až tři vteřiny. Tento jev je vidět

na následujících obrázcích Obr. 8.1 a Obr. 8.2. Můžeme pozorovat snahu robota držet

48

stabilní polohu s drobným kmitáním a zhruba od třetí vteřiny dochází k již k velkému

kmitání a robot padá.

Obr. 8.1: Odchylka robota od vzpřímenné polohy

Obr. 8.2: Akční zásah regulátoru

Tento stav může být způsoben nekvalitním regulátorem, kde dochází k moc

velkému překmitu soustavy. Dále pak značnými nedostatky motorů, jelikož nejsou přímo

uzpůsobeny pro podobné aplikace a vykazují velké množství vibrací a nedostatečný

výkon.

-0,1

-0,08

-0,06

-0,04

-0,02

0

0,02

0,04

0,06

0,08

0,1

0 500 1000 1500 2000 2500 3000 3500 4000

φ [

rad

]

t [ms]

-3

-2

-1

0

1

2

3

0 500 1000 1500 2000 2500 3000 3500 4000

Akční zásah [

-]

t [ms]

49

9 ZÁVĚR

V této bakalářské práci bylo sestaveno funkční tělo robota, které nese všechny

potřebné periferie pro samostatnou funkčnost robota. Tělo je tvořeno z hliníkového

podvozku, na kterém jsou přimontované dva BLCD motory s kolem a pneumatikou a

deska plošných spojů. Podvozek zároveň slouží jako hlavní montážní prvek pro celkovou

konstrukci robota tvořenou čtyřmi závitovými tyčemi. V dané výšce je umístěna plastová

destička s hlavním senzorem výchylky a akumulátor, který napájí celého robota a zároveň

představuje závaží pro lepší stabilitu. Podvozek byl vyroben již v první části této práce a

sloužil pro základní testování první verze programu, jako např. řízení motorů.

Dalším krokem bylo vytvoření ovladače pro čtení hodnot ze senzoru tvořeném

kombinací gyroskopu a akcelerometru. Program vyčítá hodnoty odchylky robota od

vzpřímené polohy v radiánech.

Poté následovalo vytvoření matematického modelu robota. Nejprve vznikl

stavový popis systému ve tvaru dvou rovnic, které byly získány výpočtem pomocí

Lagrangeových rovnic II. řádu. Stavový popis ve tvaru rovnic byl následně linearizován

v okolí bodu 𝜑 = 0 a upraven na maticový zápis. Maticový zápis byl převeden na

přenosovou funkci. Zároveň byl vytvořeno blokové schéma modelu robota za pomoci

programu Matlab, konkrétně aplikace Simulink. Vytvořený model poté sloužil jako

základ pro návrh regulátoru.

Za regulátor byl zvolen typ PID, který se jevil jako nejvhodnější pro tuto aplikaci.

Regulátor byl vytvářen v programu Matlab, konkrétně prostředí PID Tool, kde je možné

ladit parametry PID regulátoru a sledovat v přímém přenosu jeho chování a vliv na

soustavu. Jelikož, jak se předpokládalo, matematický model stoprocentně neodpovídal

reálné soustavě, byly parametry regulátoru ještě ručně experimentálně laděny na reálném

modelu.

V konečné fázi byl pořízen vhodný akumulátor pro napájení robota. Celá

konstrukce byla složena a robot je v současné době schopný samostatného provozu.

Cílem práce bylo vytvořit matematický model robota, navrhnout způsob regulace,

vytvořit ovladače pro senzory, řízení motorů a samostatný regulátor a následně celou

aplikaci odladit.

Jako vylepšení do budoucna by bylo vhodné nahradit PID regulátor LQR (lineárně

kvadratický regulátor) nebo LQG (lineárně kvadratický Gaussův regulátor) regulátorem.

Oba regulátory byly navrženy, ale bohužel se je nepodařilo oživit a proto od tohoto

nápadu upadlo a místo zaujal předem vytvořený PID regulátor. Dále by mohlo být využití

DMP nahrazeno výčtem surových hodnot, vlastním přepočtem a následným

komplementárním nebo ještě lépe kalmanovým filtrem. Zároveň by bylo vhodné nahradit

současné motory za motory uzpůsobené pro podobnou aplikaci.

50

REFERENCE

[1] ANDERSON, D. P. NBot Balancing Robot. Geophysics Research Archives [online].

2013 [cit. 2015-01-08]. Dostupné z: http://www.geology.smu.edu/~dpa-

www/robo/nbot/

[2] GRASSER, F., A. D'ARRIGO, S. COLOMBI a A.C. RUFER. JOE: a mobile, inverted

pendulum. IEEE Transactions on Industrial Electronics [online]. 2002, 49(1): 107-114

[cit. 2015-01-08]. DOI: 10.1109/41.982254. ISSN 02780046. Dostupné z:

http://ieeexplore.ieee.org/lpdocs/epic03/wrapper.htm?arnumber=982254

[3] HASSENPLUG, Steve. Steve's LegWay. Team Hassenplug [online]. 2002 [cit. 2015-

01-08]. Dostupné z: http://www.teamhassenplug.org/robots/legway/

[4] SEGWAY. Segway [online]. 2015 [cit. 2015-01-08]. Dostupné z:

http://www.segway.com/

[5] HORÁK, P. Řízení laboratorního modelu nestabilního balancujícího vozidla [online].

Brno: Vysoké učení technické v Brně, Fakulta strojního inženýrství 2011. Dostupné z:

https://www.vutbr.cz/www_base/zav_prace_soubor_verejne.php?file_id=42288.

Diplomová práce.

[6] KRATOCHVÍL, L. Inverzní kyvadlo [online]. Praha: České vysoké učení technické v

Praze, Fakulta elektrotechnická, 2008. Dostupné z:

https://support.dce.felk.cvut.cz/mediawiki/images/a/a3/Bp_2008_kratochvil_lukas.pd

f. Bakalářská práce.

[7] HESS, Lukáš. Návrh dvoukolového autonomního robota [online]. Brno: Vysoké učení

technické v Brně, Fakulta strojního inženýrství, Ústav automatizace a informatiky

2013. Dostupné z: http://autnt.fme.vutbr.cz/szz/2013/DP_Hess.pdf. Diplomová práce.

[8] BLAHA, P. a P. VAVŘÍN. Řízení a regulace 1 [online]. VUT Brno, 2005 [cit. 2015-

05-18]. Dostupné z:

http://www.uamt.feec.vutbr.cz/~richter/vyuka/0809_BRR1/texty/brr1.pdf

[9] MATLAB, Introduction: PID Controller Design. Control tutorials for Matlab &

Simulink [online]. 2012 [cit. 2015-05-19]. Dostupné z:

http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction§ion=ControlPI

D

[10] MARTINEZ, How To - Martinez Brushless Gimbal Controller Set-Up. Its-Qv Wiki

[online]. 2014 [cit. 2015-01-07]. Dostupné z:

http://www.itsqv.com/QVW/index.php?title=How_To_-

_Martinez_Brushless_Gimbal_Controller_Set-Up

[11] MARTINEZ, Gimbal Brushless Controller V3.0 50x50mm by Martinez. FPV

Community [online]. 2013 [cit. 2015-01-07]. Dostupné z: http://fpv-

community.de/showthread.php?22617-Gimbal-Brushless-Controller-V3-0-50x50mm-

by-Martinez&p=300925&viewfull=1#post300925

51

[12] MPU-6050 Accelerometer + Gyro. Arudino Playground [online]. 2013 [cit. 2015-01-

07]. Dostupné z: http://playground.arduino.cc/Main/MPU-6050

[13] VOJÁČEK, Antonín. Integrované MEMS GYROSKOPY. Automatizace.HW.cz

[online]. 2009 [cit. 2015-01-03]. Dostupné z: http://automatizace.hw.cz/integrovane-

mems-gyroskopy

[14] ŠIMON, Josef. BLDC aneb DC motor s nulovými náklady na údržbu. ELEKTRO

[online]. 2011, 2011(10) [cit. 2015-01-03]. Dostupné z:

http://www.odbornecasopisy.cz/res/pdf/44507.pdf

[15] BUCHMANN, Isidor. BU-206: Li-polymer Battery: Substance or Hype? Battery

University [online]. 2015 [cit. 2015-05-18]. Dostupné z:

http://batteryuniversity.com/learn/article/the_li_polymer_battery_substance_or_hype

[16] CZECHDUINO, Co je to Arduino. CzechDUINO [online]. 2012 [cit. 2015-05-17].

Dostupné z: http://czechduino.cz/?co-je-to-arduino,29

[17] ŠTRAUCH, Adam. Stavíme kvadrokoptéru #3: Digital Motion Processor na MPU-

6050. Root [online]. 2013 [cit. 2015-03-18]. Dostupné z:

http://www.root.cz/clanky/stavime-kvadrokopteru-3-digital-motion-processor-na-

mpu-6050/

[18] INVENSENSE, MPU-6000 and MPU-6050 Product Specification. In: Invensense

[online]. 2013 [cit. 2015-01-06]. Dostupné z:

http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf

[19] REDAKCE, HW serveru. Stručný popis sběrnice I2C. HW [online]. 2000 [cit. 2015-

01-06]. Dostupné z: http://www.hw.cz/navrh-obvodu/strucny-popis-sbernice-i2c-a-

jeji-prakticke-vyuziti-k-pripojeni-externi-eeprom-24lc256

[20] DH, servis. Pulsně šířková modulace. DH servis [online]. [cit. 2015-05-20]. Dostupné

z: http://www.dhservis.cz/psm.htm

52

SEZNAM POUŽITÝCH ZKRATEK

DPS Deska Plošných Spojů

PC Personal Computer – osobní počítač

IMU Inercial Measurement Unit – inerciální měřící jednotka

FPGA Field Programmable Gate Array – programovatelné hradlové pole

DAC Digital-Analog Converter – číslicově analogový převodník

ADC Analog-Digital Converter – analogovo číslicový převodník

MEMS Micro-Electro-Mechanical Systems – elektromechanický systém

s rozměry v řádu mikrometrů

FTDI Future Technology Devices International – skotská firma

USB Universal Serial Bus – univerzální sériová sběrnice

I2C Inter-Integred Circuit bus - vnitřní sběrnice mezi integrovanými obvody

BLDC BrushLess Direct Current – bezkartáčový stejnosměrný (motor)

DC Direct Current – stejnosměrný proud

LED Light Emitting Diode – světelná dioda

DMP Digital Motion Processor – číslicový pohybový procesor

SDA Serial DAta – vodič pro přenos dat po I2C sběrnici

SCL Seriál CLock – vodič pro hodinový impuls dat na I2C sběrnici

ACK ACKnowledge – potvrzující bit na I2C sběrnici

PWM Pulse Width Modulation – pulsně šířková modulace

LQR Linear-Quadratic Regulator – lineárně kvadratický regulátor

LQG Linear-Quadratic-Gaussian – lineárně kvadratický gaussův (regulátor)

PID Proportional-Integral-Derivative (controller) - proporčně-integračně-

derivační (regulátor)

PSD Proportional-Summation-Difference (controller) – proporčně-sumačně-

diferenční (regulátor)

53

SEZNAM POUŽITÝCH SYMBOLŮ

𝜑 natočení kyvadla [𝑟𝑎𝑑]

𝑣21 obvodová rychlost kyvadla kolem kola [𝑚/𝑠]

𝛿 natočení kola [𝑟𝑎𝑑]

�̇� translační rychlost kola [𝑚/𝑠]

𝑚1 hmotnost podvozku [𝑘𝑔]

𝑚2 hmotnost závaží [𝑘𝑔]

𝑀1 moment závaží [𝑁𝑚]

𝑀2 moment kola [𝑁𝑚]

𝐽 moment setrvačnosti kola [𝑘𝑔/𝑚2]

𝐿 délka kyvadla [𝑚]

𝑣2𝑥 rychlost 𝑣2 v ose 𝑥 [𝑚/𝑠]

𝑣2𝑦 rychlost 𝑣2 v ose 𝑦 [𝑚/𝑠]

𝑣2 rychlost hmotného bodu v těžišti závaží [𝑚/𝑠]

𝐸𝑘 kinetická energie [𝐽]

𝐸𝑘𝑘𝑜𝑙𝑜 kinetická energie kola [𝐽]

𝐸𝑘𝑧á𝑣𝑎ží kinetická energie závaží [𝐽]

𝐸𝑝 potenciální energie [𝐽]

𝐴 práce soustavy [𝐽]

𝑞 zobecněná souřadnice

𝑟 poloměr kola [𝑚]

𝑔 gravitační zrychlení Země [𝑚/𝑠2]

ℎ vzdálenost osy otáčení kola a těžiště závaží [𝑚]

𝜔(𝑡) soustava – řídící (žádaná) hodnota

𝑒(𝑡) soustava – regulační odchylka

𝑥(𝑡) soustava – akční zásah

𝑣1(𝑡) soustava – porucha na vstupu soustavy

𝑣2(𝑡) soustava – porucha na výstupu soustavy

𝑦(𝑡) soustava – výstup soustavy

𝐹𝑠(𝑠) přenosová funkce soustavy

𝐹𝑅(𝑠) přenosová funkce regulátoru

𝑋(𝑠) přenosová funkce akčního zásahu

𝐸(𝑠) přenosová funkce regulační odchylky

𝑊(𝑠) přenosová funkce řídící (žádané) hodnoty

𝑈(𝑠) přenosová funkce poruchy

𝑟𝑖, 𝐾𝐼 zesílení I složky

𝑟0, 𝐾𝑃 zesílení P složky

𝑟𝑑, 𝐾𝐷 zesílení D složky

54

𝑇𝑖 časová konstanta I regulátoru [𝑠]

𝑇𝐷 časová konstanta D regulátoru [𝑠]

𝑇1,2 časové konstanty PID regulátoru [𝑠]

𝐾𝑅 zesílení regulátoru

𝑇 vzorkovací frekvence

55

SEZNAM PŘÍLOH

PŘÍLOHA 1: DVD

Arduino – hlavní program robota

Dokumentace – bakalářská práce v elektronické verzi

Eagle – schéma zapojení DPS

Foto – foto robota v plném rozlišení

Matlab – výpočty, simulace a blokové schéma modelu robota