Anotace Bakalářská práce je zaměřena na problematiku mobilní robotiky. V první části je obecně popsána architektura AVR a jednotlivé použité mikroprocesory. Dále se práce zaměřuje na vyhodnocení vzdálenosti pomocí ultrazvukových snímačů, konkrétní popis použitých modulů a popis řešení vyhodnocení vzdálenosti. Poslední část se zabývá řízením a regulací stejnosměrných motorů. Jsou zde obecně popsány možnosti řízení pomocí PWM a konkrétně popsán použitý koncept řízení a návrh regulátoru.
Klíčová slova robot,
mobilní robot, DARPA Grand Challenge, AVR, mikroprocesor, ultrazvukový
snímač, řízení DC motoru, regulace
Annotation Bachelor`s thesis is bent on problems mobile robotics.
In forepart is genarally
circumscribed architecture AVR and individual used microprocessors. Next part is intend for distance evaluation by the help of ultrasonic sensores and concrete description of used modulus. Last part is specialized in motor control and regulation of DC motor. There are described possibilities of motor control by the help of PWM and used draft of motor control and projecting of regulator.
Keywords robot, mobile robot, DARPA Grand Challenge, AVR, mikroprocessor, ultrasonic sensor, DC motor control, regulation
Bibliografická citace GROULÍK, Tomáš. Elektronika pro mobilní robot Minidarpa. Brno: Vysoké učení technické v Brně, Fakulta elektrotechniky a komunikačních technologií, 2008. s.53, příloh.8. Vedoucí práce Ing. Lukáš Kopečný
Prohlášení „Prohlašuji, že svou bakalářskou práci na téma Elektronika pro mobilní robot Minidarpa jsem vypracoval samostatně pod vedením vedoucího bakalářské práce a s použ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ů 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 :
Podpis:
Poděkování
Děkuji tímto Ing. Lukáši Kopečnému, doc. Ing. Luďkovi Žaludovi, Ph.D. za cenné připomínky a rady při vypracování bakalářské práce.
V Brně dne :
Podpis:
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
OBSAH 1. ÚVOD ...............................................................................................................12 2. ŘEŠENÁ PROBLEMATIKA ........................................................................13 2.1 Jednočipy AVR ...............................................................................................14 2.1.1 Obecně o architektuře AVR..........................................................................14 2.1.2 Konkrétní mikroprocesory použité v robotu.................................................20 2.2 Snímání vzdálenosti pomocí ultrazvukových senzorů....................................21 2.2.1 Práce s elektrostatickým snímačem polaroid................................................21 2.2.2 Ovládání a vyhodnocení v single-echo módu...............................................22 2.2.3 Vlastní zpracování snímání vzdálenosti .......................................................23 2.3 Řízení a regulace stejnosměrných motorů Maxon..........................................29 2.3.1 Modul řízení s H-můstkem L6203................................................................30 2.3.2 Zpětná otáčková vazba zprostředkována pomocí enkodéru .........................35 2.3.3 Řídící systém pro řízení a regulaci motorů...................................................38 2.3.4 Nastavení parametrů regulátoru....................................................................43 3. ZÁVĚR .............................................................................................................51 4. POUŽITÁ LITERATURA .............................................................................53
7
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
SEZNAM POUŽITÝCH OBRÁZKŮ Obrázek 2.1 - Status registr a 32 víceúčelových registrů........................................... 16 Obrázek 2.2 - Příklad mapy paměti u mikrokotroléru AVR AT90S2323. ................ 17 Obrázek 2.3 - Příklad RISC-architektury AVR mikroprocesoru AT90S2323. ......... 18 Obrázek 2.4 - Schéma zapojení modulu 6500 series sonar ranging Polaroid ............ 22 Obrázek 2.5 - Diagram ultrazvukového snímače v single – echo módu.................... 23 Obrázek 2.6 - Algoritmus vyhodnocení vzdálenosti.................................................. 27 Obrázek 2.7 - Problém s propojením signálů ECHO do jednoho .............................. 28 Obrázek 2.8 - Řešení problému s propojením signálů ECHO do jednoho ................ 28 Obrázek 2.9 - Orientační schéma h-můstku............................................................... 30 Obrázek 2.10 - Kombinace sepnutí spínačů h-můstku............................................... 30 Obrázek 2.11 - Vnitřní zapojení výkonového obvodu ............................................... 34 Obrázek 2.12 - Příklad soustavy rotačního optického enkodéru, řízeného motoru a ovládané mechanické soustavy (ramene)................................................................... 35 Obrázek 2.13 - Princip rotačního optického enkodéru............................................... 36 Obrázek 2.14 - Provedení inkrementálního enkodéru s detekcí směru pohybu a definovaným počátkem .............................................................................................. 37 Obrázek 2.15 - časový diagram signálů A a B optického enkodéru .......................... 38 Obrázek 2.16 - Blokové schéma systému pro řízení a regulaci motorů..................... 42 Obrázek 2.17 - Odezva regulátoru D na jednotkový skok a jednotkový impuls ....... 46 Obrázek 2.18 - Porovnání přechodových charakteristik vypočítaného regulátoru D a nahrazeného Dps ........................................................................................................ 47 Obrázek 2.19 - Simulační schéma regulace otáček motoru robotu............................ 48 Obrázek 2.20 - Blokový model stejnosměrného motoru s cizím buzením ................ 48 Obrázek 2.21 - Aktuální stav robotu .......................................................................... 50
8
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
SEZNAM TABULEK Tabulka 2.1 - Kombinace sepnutí spínačů h-můstku ................................................. 31 Tabulka 2.2 - Řízení obecně pomocí různých kombinací sepnutí spínačů A,B,C,D. 31 Tabulka 2.3 - Řízení pomocí zapojení Sign-Magnitude 1 = směr velikost 2............. 32 Tabulka 2.4 – Řízení pomocí zapojení Sign-Magnitude 2 = směr velikost 2 ............ 32 Tabulka 2.5 - Řízení pomocí zapojení Locked - antiphase = střídané řízení............. 33
9
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
SEZNAM ZKRATEK Zkratka/Symbol Jednotka
Popis
Lv
[mH]
indukčnost vinutí
Rv
[ Ώ]
odpor vinutí
Km
[Nm/A]
momentová konstanta
Kω u
[rpm/V] [V]
rychlostní konstanta napájecí napětí motoru
ui i ω
[V] [A] [rad/s]
zpětné indukované napětí motoru proud vinutím motoru otáčky motoru
Mm ζ
[Nm] [Nm/A]
moment motoru konstrukční konstanta motoru
Jm
[kg*m2]
moment setrvačnosti motoru
Jp
[kg*m2]
moment setrvačnosti převodovky
Jk m r
2
[kg*m ] [kg] [m]
moment setrvačnosti kola hmotnost robotu poloměr kola robotu
J i
[kg*m ] [-]
celkový moment setrvačnosti vztažený na stranu motoru převod převodovky
Umax
[V]
amplituda pulsů PWM signálu
n
[-]
počet bitů registru, který určuje akční zásah
KPWM Tvz
[-] [ms]
zesílení zohledňující střední hodnotu signálu vzorkovací perioda
f osc n0 n1
[ MHz ] [ - ] [ - ]
frekvence hodinového signálu mikroprocesoru počet bitů časovače 0 počet bitů časovače 1
tz
[
s
]
požadovaná doba zatemňování
tUZ
[
s
]
doba letu UZ signálu
vUZ
[ m/s ]
rychlost letu UZ signálu
tUzmax s
[ [
maximální doba letu UZ signálu vzdálenost od první překážky
2
s ] m ]
10
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Az d0,d1 e M
[ [ [ [
-
] ] ] ]
akční zásah navržené konstanty PS regulátoru regulační odchylka paměť pro hodnoty z předchozího vzorku
11
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
1.
ÚVOD
Mobilní robotika je v současné době stále ve vývoji a zřídka je možno setkat se s robotem, který by plnil nějaké běžné úkoly. Pro rozvoj a popularizaci zejména autonomní robotiky jsou pořádány různé soutěže. Asi nejznámější je DARPA Grand Challenge. [7] Touto soutěží je inspirována i soutěž Robotour, která probíhá v Praze a pro kterou je náš robot navrhován. Úkolem robotů v této soutěži je v zadaném časovém limitu 30 minut projet dráhu cca 1 km dlouhou. Roboti musí být plně samostatní, nesmí sjíždět z cesty a musí se správně rozhodovat na křižovatkách podle zadané mapy. Pro konstrukci celého robotu byl vytvořen pětičlenný tým. Cílem celého týmu je tedy zpracovat robota po mechanické stránce, vytvořit systém pro ovládání podvozku, senzorický a navigační subsystém a hlavní řídicí systém. V této práci je popsána pouze část senzorického subsystému – ultrazvukové snímače a systém pro ovládání podvozku. Pro řízení a ovládání senzorů i podvozku je nutné zvolit vhodné řídicí mikroprocesory a teoreticky nastudovat a osvojit si jejich strukturu a ovládání. Pomocí ultrazvukových snímačů se bude robot orientovat v prostoru a vyhýbat se případným překážkám. Proto je potřeba vytvořit řídicí elektroniku, která bude zajišťovat postupné vysílání ultrazvukového signálu danými moduly. Dále bude pomocí měření času od vyslání po příjem ultrazvukového signálu zjišťovat vzdálenost od nejbližší překážky. Mobilní robot se samozřejmě musí pohybovat. Proto je nutné vytvořit systém, který umožní jízdu robotu. Je třeba se proto seznámit s možnostmi řízení stejnosměrných motorů, navrhnout výkonovou i řídicí část, sestavit vhodný řídicí algoritmus, který zajistí regulaci otáček. Pro správnou regulační funkci je také třeba vhodně nastavit konstanty regulátoru.
12
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
2.
ŘEŠENÁ PROBLEMATIKA
Robotika je obor, který se zabývá studiem a konstrukcí robotů a jim podobných zařízení. Dosud však neexistuje ustálená definice jak oboru, tak pojmu robot. Všeobecně je robot chápán jako stroj, který vykonává podobné činnosti jako člověk, především však činnosti pohybové a manipulační. Většinou musí takový stroj získávat informace o prostředí ve kterém se pohybuje a musí být schopen toto prostředí fyzikálně, především mechanicky, ovlivňovat. Robotika je moderní multidisciplinární obor zahrnující znalosti mechaniky, elektrotechniky, teorie řízení, měřicí techniky, umělé inteligence a celé řady dalších disciplin. Jak vypadá svět z pohledu robotů? A co to vlastně je ten robot a jak funguje? Budeme se zajímat pouze o autonomní mobilní roboty. Autonomní mobilní robot je inteligentní stroj, který je schopen vykonávat úkoly samostatně, bez lidské pomoci. Nejdůležitější vlastností autonomního robota je jeho schopnost reagovat na změny prostředí. Za tu většinou vděčí svému mozku — počítači, který je zodpovědný více méně za vše, počínaje zpracováváním vstupních údajů, rozhodováním, i konečným provedením vybraných akcí. V našem případě je centrální mozek systému dvoujádrový procesor INTEL mobile CORE DUO T2300E s jádrem Yonah, který komunikuje s podřízenými mikroprocesory AVR a přímo s některými senzory. Vstupní údaje počítači poskytují senzory. Senzor je zařízení, které je schopné měřit nějakou vlastnost prostředí. Příkladem takového senzoru může být třeba i obyčejný spínač. V našem případě se používají ultrazvukové snímače vzdálenosti pro určení překážky, elektronický kompas pro určení úhlu natočení robota, enkodér pro určení otáček motoru, kamera pro rozpoznání cesty a gps modul pro navigaci robota. Naopak působit na své prostředí může robot pomocí efektorů. Typickým příkladem efektoru je elektromotor. Po připojení na kolo umožní robotu pohyb. V závislosti na velikosti robota se můžeme setkat i s dalšími typy efektorů, ať už to jsou spalovací motory či hydraulika. V našem případě se používají dva stejnosměrné komutátorové motory MAXON o výkonu 90W. Každý z nich ovládá přes
13
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií
14
Vysoké učení technické v Brně
převodovku jednu stranu podvozku, což umožňuje při správném ovládání řídit rychlost a směr mobilního robota. Z nutných součástí zbývá zmínit ještě zdroj energie, kterým je nejčastěji akumulátor, který poskytuje energii senzorům, počítači i většině efektorů. Větší roboti vybaveni spalovacími motory mívají alternátory, které akumulátor průběžně dobíjí, a doba, po kterou je robot schopen operovat autonomně, je tím značně prodloužena. Někteří roboti používají i solární panely (např. roboti vysláni na Mars). V našem případě se jedná o dvě baterie o kapacitě 9Ah spojené do série, tak že celkové napájecí napětí je 24V. [8]
2.1
JEDNOČIPY AVR
2.1.1 Obecně o architektuře AVR Firma Atmel představila před třemi lety nové 8bitové mikroprocesory tzv. AVR-sérii s RISC-architekturou. Tyto 8bitové mikroprocesory dosahují takové početní výkony, které jsou typické pro 16bitové procesory. S AVR-rodinou ukazuje firma Atmel, že RISC-architektura nemusí být používána jen u výkonových procesorů pro pracovní stanice nebo 32bitových mikroprocesorů pro intenzivní početní úkony, ale má také smysl u 8bitových mikroprocesorů. AVR-série nabízí běžné přednosti RISC-architektury, tj. jednocyklové instrukce, vyšší taktovací frekvence spojená s vyšším pracovním výkonem stejně jako efektivní optimalizace překladu. Původně byla vyvinuta nová 8bitová RISC-architektura v norském vývojové centru Nordic VLSI v Trondheimu. Před více jak třemi lety koupila firma Atmel tuto koncepci na které je založena 8bitová mikroprocesorová rodina, která přišla před dvěmi lety na trh. Firma Atmel je známa svými produkty jako je programovatelná logika, paměti typu EPROM, EEPROM a FLASH a především také FLASH-mikroprocesory založené na „Intelovské“ rodině mikroprocesorů 8051. AVR-rodina
je
konkurentem
na
trhu
některých
dobře
zavedených
mikroprocesorových rodin, jako jsou 8bitové rodiny 6805 a 68HC11 od Motoroly stejně jako 8051 od Intelu, jejichž čipy vyrábějí různí vlastníci licencí včetně Atmelu. Hlavním konkurentem je firma Microchip s moderní 8bitovou PIC-rodinou,
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
která se stává v posledních letech značně populární. Microchip vsadil také na architekturu blízkou RISC-architektuře s mnoha jednocyklovými instrukcemi. Jádro AVR-série se podobá jádru většiny RISC-procesorů, které jsou dostupné na trhu. AVR jádro se skládá ze 32 stejných 8bitových registrů, které mohou obsahovat jak data tak adresy. Posledních 6 registrů můžeme ve dvojici použít jako ukazatele adresy pro nepřímé adresování paměti dat. Tyto registry označované písmeny X, Y a Z dovolují libovolné ukládací operace (Load/Store) viz. Obrázek 2.1 - Status registr a 32 víceúčelových registrů. Programátor má například na výběr, zda ukazatel adresy bude po zpracování určité instrukce inkrementovat nebo před zpracováním této instrukce dekrementovat. Užitečné je pro adresování využít možnosti 6bitového posunu v ukazateli adresy v dvojitých registrech Y a Z.
AVR architektura má 5 adresovacích módů pro paměť dat: • přímé adresování • nepřímé adresování s posunutím (6bitový posun) • nepřímé adresování • nepřímé adresování s dekrementací ukazatele adresy před zpracováním instrukce • nepřímé adresování s inkrementací ukazatele adresy po zpracování instrukce
15
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Status registr obsahuje příznak přetečení, přenos do vyššího řádu, znaménko a další příznaky viz. Obrázek 2.1.
Obrázek 2.1 - Status registr a 32 víceúčelových registrů
Jak je u mnoha jednoduchých mikroprocesorů obvyklé, registry jsou zobrazovány přímo v adresovém prostoru dat. Prvních 32-bytů paměti ( 0x00 až 0x1F) odpovídá registrům R0 až R31. Ty jsou také přímo zamapovány do paměti dat. Proto je možno s každým registrem zacházet použitím standardních odkazů bez toho, aby programátor potřeboval znát řídící instrukce registrů. AVR používá Harvardskou architekturu, tj. paměť programu a paměť dat jsou odděleny. Paměť programu je přístupná pomocí dvoustupňové pipeline. Když se určitá instrukce začíná vykonávat, další instrukce je připravována k zpracování. Tato konstrukce dovoluje zpracovávání instrukcí vždy v jednom hodinovém cyklu.
16
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Obrázek 2.2 - Příklad mapy paměti u mikroprocesoru AVR AT90S2323.
Zpracování operandů probíhá následovně. Během jednoho taktovacího cyklu se přivedou oba zdrojové operandy z pracovních registrů, uskuteční se potřebná operace a výsledek se uloží zpátky do registrů. Při úspěšném provedení je tedy potřebný čas jeden takt. Jedná se výhradně o operace typu registr - registr, na kterých je AVR architektura založena a důsledně je dodržuje podle modelu LOAD/STORE.
17
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Obrázek 2.3 - Příklad RISC-architektury AVR mikroprocesoru AT90S2323.
Většina všech instrukcí je 16bitových. Pouze 4 instrukce jsou 32bitové. Ty jsou však omezeny dovoleným 16bitovým adresováním. Ačkoli ukazatele na data mohou být až 16bitové, je programový čítač (PC) pouze maximálně 12bitový. Velikost programového čítače závisí na typu mikroprocesoru. Například AT90S2323 má PC 10bitový, ale AT90S8515 má PC 12bitový. Celá instrukční sada s maximálně 118 instrukcemi nutí také ke kompromisům. Většina instrukcí v instrukční sadě má přímý a jednocyklový přístup do všech registrů. Výjimku tvoří pouze aritmetické a logické instrukce jako SBCI, SUBI, CPI, ANDI a ORI mezi konstantou a registrem a instrukce LDI (load immediate constant data). Jedná se tedy hlavně o zpracování přímých datových typů. Jen výše uvedené instrukce akceptují přímé hodnoty a mohou být používány pouze v horní polovině registrů (R16 až R31). Ještě je možné pro zpracování přímých datových typů použít instrukce ADIW a SBIW, které jsou
18
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
přístupné pouze v posledních 8 registrech (R24 až R31). Pro podmíněné větvení a skoky lze použít různé instrukce. Na každý z osmi příznaků ve Status registru připadají nejméně 2 instrukce podmíněného skoku (např. BREQ, BRTS atd.). Další instrukce pro skoky je RJMP (12-bitová), kterou lze posunout relativně kód o 2 kB. Pak je zde absolutní skoková instrukce IJMP, která provede skok na danou adresu. Zajímavé jsou také mnohé "Skip" instrukce ( SBRC, SBRS, SBIC a SBIS ), které umožňují podmíněný skok přes následující instrukci, jeli nastaven některý bit v daném registru či nikoli. Je-li například přeskočená instrukce RJMP, nechají se využít tyto "Skip" instrukce k efektivní implementaci podmíněného větvení s většími skoky. Právě tak je také možné použít "Skip" instrukce k přeskočení jednotlivých aritmetických či logických operací při podmíněných operacích. Pro aritmetické a logické operace obsahuje instrukční soubor klasické instrukce sčítání ADD, ADC, odečítání SUB, SUBI, SBC, SBCI, logické instrukce jako AND, OR, EOR atd. Jako mnoho jiných mikroprocesorů AVR architektura disponuje také instrukcemi pro bitové operace. Se šestnácti definovanými instrukcemi je možno nastavit nebo vymazat každý příznak ( Flag ) ve Status registru. Ačkoliv stejný efekt vznikne také pomocí logických operací, dosáhne se tímto způsobem při programování mnoha aplikací značného zjednodušení. Jsou zde k dispozici instrukce, se kterými se nechá nastavit nebo smazat každý bit víceúčelového nebo vstup/výstupního registru. Například instrukce SER a CLR nastaví eventuálně smažou najednou celý registr, instrukce SBR a CBR mají vliv na vybrané bity v jednom registru.
[9]
19
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
2.1.2 Konkrétní mikroprocesory použité v robotu Pro řízení motoru a ovládání senzorů byly zvoleny mikroprocesory řady AVR od firmy Atmel, protože se poměrně snadno obsluhují, je snadné získat dobrou a přehlednou dokumentaci o jednotlivých mikroprocesorech a je možnost stažení freewareových překladačů a vývojových prostředí, které jsou uživatelsky přívětivé. 2.1.2.1 Mikroprocesor pro ovládání ultrazvukových snímačů Pro ovládání ultrazvukových snímačů vzdálenosti a komunikaci s nadřízeným výpočetním systémem byl vybrán mikroprocesor ATMEGA8. Je to jeden z nejjednodušších čipů z řady MEGA, ale protože zde nejsou zvláštní požadavky na velikost programovatelné paměti, na počet externích přerušovacích vstupů a časovačů ani na počet vstupně-výstupních portů, je pro naše účely zcela postačující. Disponuje sériovým rozhraním Serial USART, které se využije pro komunikaci s nadřízeným výpočetním systémem. 2.1.2.2 Mikroprocesor pro řízení motorů Maxon Pro řízení a regulaci obou stejnosměrných motorů je použit jak mikroprocesor ATMEGA8 viz. výše, tak výkonnější mikroprocesor ATMEGA32, který disponuje třemi časovači, které je možno přímo nastavit pro generování PWM signálu pro řízení motorů. Dále má 3 externí přerušovací vstupy, které lze využít pro čítání pulsů od enkodéru, AD převodník pro zpětnou proudovou vazbu a sériové rozhraní pro komunikaci s nadřízeným systémem. Disponuje 32KB programovatelné FLASH paměti, což plně dostačuje pro náš řídící software.
20
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
2.2
SNÍMÁNÍ VZDÁLENOSTI POMOCÍ ULTRAZVUKOVÝCH SENZORŮ
Pro snímání vzdálenosti jsou využívány moduly 6500 series sonar ranging Polaroid. Základní rysy tohoto modulu: • meze snímaných vzdáleností : 6 – 35 stop ( 1.8 – 10.7 metrů ) • 50kHz elektrostatický snímač bez přídavného rozhraní • Pracovní napájení pomocí jednoho zdroje • Přesný hodinový výstup pro externí použití • TTL kompatibilita • Použití TI TL851 a Polaroid 614906 sonarový čip • Vhodný terminální konektor • Nastavitelný potenciometr pro ovládání zesílení [3] 2.2.1 Práce s elektrostatickým snímačem polaroid Jsou zde dva základní módy pro práci s tímto modulem. Single–echo mód a multi–echo mód. Single–echo mód funguje tak, že vysílač vyšle několik pulsů ultrazvukového signálu a přijímač vyhodnotí pouze první odražený signál. Získá se tak informace pouze o nejbližší překážce. Multi–echo mód vysílá stejně, ale při přijímání zpracovává nejen první odražený signál. Získá se tak informace nejen o první překážce. Ovšem softwarové vyhodnocení více překážek musí být velmi náročné.
21
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Obrázek 2.4 - Schéma zapojení modulu 6500 series sonar ranging Polaroid
2.2.2 Ovládání a vyhodnocení v single-echo módu Ovládání a vyhodnocení v tomto módu spočívá ve správném nastavení a hlídání jednotlivých vstupů a výstupů ultrazvukového modulu. Napájecí přívody se připojí na 4.5 – 6.8 V. Potom se aktivuje nastavením vstupu INIT vysílač, který vyšle 16 pulsů ultrazvukového signálu. Vlivem interního zatemňování ( 2.38ms ), se vracející se signál nedá tuto dobu vyhodnotit. Tímto je omezena dolní mez měřitelné vzdálenosti. Po vyslání pulsů a přepnutí na přijímač se hlídá výstup ECHO. Když dorazí první odražený signál, nastaví se tento výstup a je možno z času od vyslání po příjem určit vzdálenost. Ostatní vstupy – BLINK a BINH jsou v tomto módu připojeny na zem. Nesmí se také zapomenout na to, že výstup ECHO je výstup s otevřeným kolektorem a požaduje mezi výstup a napájecí napětí rezistor 4k7.
22
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Obrázek 2.5 - Diagram ultrazvukového snímače v single – echo módu
2.2.3 Vlastní zpracování snímání vzdálenosti 2.2.3.1 Vyhodnocení vzdálenosti V našem případě byl po radě vedoucího projektu použit single-echo mód, ale upravený tak, že nepracuje s interním zatemňováním, ale délka zatemňování se řídí pomocí signálu BLNK. Pomocí vlastního ovládání doby zatemňování (dále blanking time) a zkrácení jeho doby se docílí nižší dolní snímatelné meze. Pokud by se však zkrátil blanking time příliš, docházelo by k tomu, že po odeslání UZ pulsů a přepnutí na přijímač by membrána snímače nebyla ještě v klidu a chvění by bylo chybně vyhodnoceno jako odražený UZ signál. Proto je vhodné zvolit správný kompromis. V našem případě je nastaven experimentálně blanking time na hodnotu 2 ms. Dosáhneme tak dolní snímatelné meze okolo 30 cm, což je v porovnáním s katalogovým údajem asi o 150 cm lepší výsledek. Algoritmus vyhodnocení vzdálenosti je znázorněn na Obrázek 2.6. Časovač 0 určuje blanking time. Časovač 1 měří čas letu UZ signálu.
23
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
2.2.3.2 Nastavení časovačů pro vyhodnocení vzdálenosti Potřebné parametry pro nastavení časovačů: f osc = 8
[ MHz ]
frekvence hodinového signálu mikroprocesoru
n0 = 8
[
-
]
počet bitů časovače 0
n1 = 16
[
-
]
počet bitů časovače 1
tz
[
s
]
požadovaná doba zatemňování
tUZ
[
s
]
doba letu UZ signálu
vUZ = 338.7
[ m/s ]
rychlost letu UZ signálu
tUzmax
[
s
maximální doba letu UZ signálu
s
[
m ]
]
vzdálenost od první překážky
Timer 0 pro určení doby zatemňování
•
Doba zatemňování se určí tak, aby minimální snímatelná vzdálenost byla zvolená hodnota 0.25 m. tz =
•
2 * 0.25m 2 * 0.25m = = 1.5ms vUZ 338.7 m / s
zvolí se frekvence časovače tak, aby se využilo co nejvíce bitů 8 bitového timeru 0 a byla tak co nejvyšší přesnost: ft0 = 125000 Hz
•
při časování do přetečení časovače se časuje dobu t0ovf: t 0 ovf =
•
2 n0 28 = = 2ms f t 0 125000 Hz
protože se požaduje doba zatemňování 1.5 ms, musí se vždy časovač inicializovat na hodnotu 28 - x , aby časoval do přetečení dobu 1.5ms: x = t z ⇒ x = f t 0 * t z = 125000 Hz * 1,5 * 10 −3 s = 188 ft0
•
hodnota x se odečte od 28, převede se na hexadecimální číslo a zadá se do programu: přednastavení = 256 – x = 68 = 44h
24
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Timer 1 pro určení vzdálenosti pomocí ultrazvuku
•
Maximální snímatelná vzdálenost udaná v katalogu je 35 stop, což je přibližně 11 m => maximální doba letu UZ signálu tUzmax. tUZ max =
•
2 * 11m 2 * 11m = = 65ms vUZ 338.7 m / s
pomocí maximální vzdálenosti se navrhne předdělička timeru1 – tzn frekvence timeru 1 se nastaví tak, aby se využilo co nejvíce bitů 16bitového timeru 1 a byla tak co nejvyšší přesnost.
•
Při frekvenci ft1 = 1MHz trvá čítání od 0 do FFFFh dobu t1ovf t 0 ovf =
2 n1 210 = 6 = 66ms f t1 10 Hz
Protože se doba t0ovf a tUzmax téměř shodují, může se použít celý časovač v rozsahu 16 bitů a nemusí se nastavovat inicializační hodnota timeru 1, protože je implicitně 0000h. Proto se nastaví frekvence časovače ft1 = 1MHz. •
určení výsledku : výsledek je po vstupu do rutiny přerušení časovače T1 zdrojem input capture uložen ve dvou 8-bitových registrech TCNT1L a TCNT1H. Je možno však použít i 16 bitový přístup pomocí TCNT1.Čas
čítání časovače v ms se pak určí následovně: tUZ =
TCNT 1 TCNT 1 = 6 f t1 10 Hz
Jednoduchým přepočtem se pak určí z rychlosti a času UZ signálu vzdálenost
s: s=
tUZ * vUZ 2
Informace o vzdálenosti se dále předává nadřazenému výpočetnímu systému, a proto je vhodné v rámci komunikace pracovat s celými čísly. Proto se výsledek převede na cm. Informace v cm totiž i v celočíselném vyjádření zaručuje dostatečnou přesnost.Všechny tyto vztahy se zahrnou do jedné převodní konstanty PREVODKONST
PREVODKONST =
vUZ 338.7 m / s *100 = *100 = 0.017 2 * f t1 2 *10 6 Hz
25
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Vzdálenost robota v cm od první překážky se tedy získá součinem 16bitového registru časovače 1 - TCNT1 a převodní konstanty PREVODKONST.
2.2.3.3 Multiplexerová obsluha více modulů Jednotlivé moduly se vyhodnocují postupně. Signály BLNK,BINH a ECHO jsou společné pro všechny moduly. Aktivace jednotlivých modulů se děje pomocí signálů INIT, který má každý modul vlastní. Počet obsloužených modulů je omezen pouze hardwarem. Software obsluhuje jakýkoliv počet modulů, přičemž jejich počet se zadá pomocí makra POCETMODULU. Signály INIT pro jednotlivé moduly jsou ovládány pomocí jednoho výstupního portu procesoru, kdy se pro přechod na další modul inkrementuje proměnná indikující aktivní modul - modul a pomocí bitového posuvu, řízeného touto proměnnou, v ovládacím registru portu nastavíme požadovaný bit a tím aktivujeme zvolený modul - PORT = 1<<(modul-1) Signál ECHO je, jak již bylo uvedeno výše, výstup s otevřeným kolektorem. Pokud se nevrátil odražený signál, je výstupní tranzistor sepnut a na jeho kolektoru je tedy nulové napětí a signál ECHO je v logické nule. Pokud je však vyhodnocen návrat odraženého signálu, je výstupní tranzistor rozepnut a na kolektoru je logická jednička, tudíž i signál ECHO je v logické jedničce. Tato skutečnost komplikuje spojení signálů ze všech modulů do jednoho pro vyhodnocení jedním vstupem procesoru – v našem případě vstupem INPUT CAPTURE pro přerušení časovače externí událostí. Protože pokud aktivovaný modul indikuje přijmutí odraženého signálu, rozepne svůj výstupní tranzistor a na výstupu ECHO očekáváme logickou jedničku. Pokud však signály ECHO od všech modulů spojíme, ostatní moduly nebudou indikovat odražený signál, tudíž jejich výstupní tranzistory budou sepnuty a proto na výstupu ECHO nebude logická jednička, ale stále logická nula – viz. Obrázek 2.7. Tento problém je možno vyřešit invertováním signálu do bází jednotlivých výstupních tranzistorů a hlídání logické nuly místo logické jedničky na výstupu ECHO. Invertování signálů v bázích je možno řešit pomocí SMD tranzistoru přidaného na každý ultrazvukový modul – viz. Obrázek 2.8.
26
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Obrázek 2.6 - Algoritmus vyhodnocení vzdálenosti
27
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Obrázek 2.7 - Problém s propojením signálů ECHO do jednoho
Obrázek 2.8 - Řešení problému s propojením signálů ECHO do jednoho 2.2.3.4 Komunikace s nadřízeným systémem Komunikace s nadřazeným systémem je realizována pomocí sériového rozhraní mikroprocesoru USART, sériového rozhraní RS232 nadřízeného systému a převodníkem napěťových úrovní MAX232, který převádí TTL úrovně od mikroprocesoru na úrovně rozhraní RS232 nadřazeného systému a naopak. Komunikační protokol se po domluvě určil tak, že se budou postupně vysílat údaje jednotlivých vzdáleností jako řetězec znaků s pevně danou délkou řetězce - 5 znaků.
28
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Přičemž první znak identifikuje číslem, který UZ modul právě vysílá. Následují 4 znaky udávají číselně hodnotu vzdálenosti první překážky v centimetrech.
2.2.3.5
Schéma zapojení modulu ultrazvukového systému
Viz. příloha
2.3
ŘÍZENÍ A REGULACE STEJNOSMĚRNÝCH MOTORŮ MAXON
Pro řízení a regulaci stejnosměrných motorů jsou využity PWM signály implementovány přímo v časovačích mikroprocesorů AVR, které jsou pomocí h – můstku L6203 zesíleny. Pomocí řídicích signálů připojených také do H–můstku se ovládá směr otáčení motoru. Zpětná otáčková vazba je zprostředkována pomocí enkodéru, mechanicky upevněném přímo na motoru. Zpětná proudová vazba je zprostředkována pomocí snímacího rezistoru, kde se převádí proud na napětí. To je potom zesíleno pomocí operačního zesilovače, zapojeného v diferenčním provedení, a pomocí AD převodníku implementovaného přímo v mikroprocesoru se převádí na digitální data, se kterýma je možno v mikroprocesoru pracovat. Případné nadproudy jsou pomocí této proudové vazby v mikroprocesoru vyhodnoceny a je dán povel bezpečnostnímu modulu k odpojení motorů pomocí relé.
29
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
2.3.1 Modul řízení s H-můstkem L6203 2.3.1.1 Obecný popis H–můstku a možnosti řízení motoru pomocí H – můstku
H-můstek je tvořen čtveřicí spínacích prvků dle schématu:
Obrázek 2.9 - Orientační schéma h-můstku Kombinací sepnutí spínačů realizujeme různé stavy můstku
Vpřed
Volné otáčení
Obrázek 2.10 - Kombinace sepnutí spínačů h-můstku
Brzda
30
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Tabulka 2.1 - Kombinace sepnutí spínačů h-můstku Řídicí elektronika musí zajistit, aby nikdy nenastalo současné sepnutí obou spínačů na levé nebo pravé straně. Zároveň je vhodné snížit počet potřebných ovládacích vodičů, proto se používá několika různých zapojení podle způsobu řízení můstku. Typické zapojení je dvou nebo tří-vodičové, kde jeden vstup IN1 řídí levou polovinu můstku (inverzně zapíná a vypíná horní a spodní spínač, vždy je jeden spínač sepnutý a druhý vypnutý), druhý IN2 řídí pravou polovinu můstku a třetí vstup ENABLE vypíná celý můstek
Tabulka 2.2 - Řízení obecně pomocí různých kombinací sepnutí spínačů A,B,C,D
Sign-Magnitude 1 = směr velikost
Vstupy IN1 a IN2 = ~IN1 volíme směr otáčení, na vstup ENABLE je přiveden PWM signál, řídicí rychlost. Příslušné dva tranzistory (dle směru otáčení) spínají napájení. Při vypnutí jsou všechny tranzistory uzavřeny, tj. motor se může „volně otáčet“.
31
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Tabulka 2.3 - Řízení pomocí zapojení Sign-Magnitude 1 = směr velikost 2
Sign-Magnitude 2 = směr velikost 2
Jeden vstup IN1 určuje směr otáčení, na druhý IN2 je přiveden signál PWM, pro řízení rychlosti, vstup ENABLE je trvale aktivní. Jedna strana můstku má trvale sepnutý jeden tranzistor, na druhé straně se střídavě spíná horní a dolní, dle PWM. Tzn. motor akceleruje nebo brzdí.
Tabulka 2.4 – Řízení pomocí zapojení Sign-Magnitude 2 = směr velikost 2
Locked-antiphase = střídané řízení
Oba vstupy IN1 a IN2 = ~IN1 jsou připojeny na PWM signál, vstup ENABLE je trvale aktivní. Pokud je plnění PWM 50%, je výsledná střední hodnota napětí nulová a motor stojí. Pro plnění <50% se otáčí jedním směrem pro >50% se otáčí druhým směrem.
32
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Tabulka 2.5 - Řízení pomocí zapojení Locked - antiphase = střídané řízení
[12]
2.3.1.2 Konkrétní popis použitého H–můstku Obvod L6203 obsahuje čtyři tranzistorové klíče, zapojené do dvou půlmostů. Tím je připraven pro symetrické řízení otáček stejnosměrného motoru. Obvod dále obsahuje nezbytné ochranné diody, ovládací a ochranné obvody. Je tedy přímo určen pro řízení stejnosměrných nebo krokových motorů v pulsním režimu. Vyroben je hybridní technologií, která sdružuje výkonové DMOS tranzistory, CMOS a bipolární obvody na jednom čipu. Této výhody se využívá hlavně z hlediska efektivnosti a spotřeby energie (účinnosti). Dva řídící vstupy (IN1 a IN2) jsou plně kompatibilní s TTL a každý ovládá jeden půlmost. Naznačená vnitřní logika ovládání koncových spínačů viz. Obrázek 2.11. automaticky zabraňuje sepnutí obou spínačů v jednom půlmostu nad sebou a tím i k zničení obvodu. Vstup ENABLE, také slučitelný s TTL logikou, umožňuje okamžité uzavření všech spínacích tranzistorů. Výkonová část spínačů je vyvedena na výstup SENSE, který je možné použít na proudovou ochranu připojeného motoru. Obvod má vestavěný zdroj referenčního napětí, potřebný pro ovládání horních dvou spínačů, ochranu proti zkratu obou půlmostů a tepelnou ochranu pracující při překročení hodnoty 150 °C. [11]
33
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Obrázek 2.11 - Vnitřní zapojení výkonového obvodu 2.3.1.3 Konkrétní popis zapojení h-můstku pro řízení motoru Z výše uvedených možností řízení je zde využita varianta Sign-Magnitude 1 = směr velikost viz.Tabulka 2.3, kde jsou možnosti vpřed, vzad a volné otáčení.
Řídicí vstupy signálové části IN1 a IN2 slouží k ovládání směru otáčení motoru. Na ovládací vstup ENABLE je přiveden PWM signál, pomocí kterého
řídíme rychlost otáčení motoru. Tlumící člen, tvořený sériovým zapojením odporu 10R a kondenzátoru 22K, je zapojen blízko výstupních pinů obvodu L6203. Mezi výkonový výstup OUT1, OUT2 a zem je zapojena dvojice rychlých ochranných diod BY228. K výstupům BOOT1 a BOOT2 jsou připojeny startovací kondenzátory 15K. Výstupu SENSE je možné použit pro signál proudové zpětné vazby pomocí snímacího odporu. Pokud jej nepoužijeme, lze odpor nahradit zkratem. V našem zapojení je použit výkonový odpor 0.1R. [11]
2.3.1.4 Schéma zapojení výkonového modulu řízení Viz. příloha
34
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
2.3.2 Zpětná otáčková vazba zprostředkována pomocí enkodéru V robotice a dnes vlastně již téměř ve všech aplikacích, kde se využívají pohony, je nutné zajistit přesné zjišťování momentální polohy natočení hřídele motoru, měřit úhlovou rychlost otáčení nebo zrychlení, aby bylo možné zajistit přesné řízení dané aplikace. V případě použití krokových motorů je možné přímo předem přesně určit a řídit pohyb, úhel natočení nebo rychlost otáčení bez nutnosti zpětné kontroly. U ostatních principů provedení motorů je to nutné zajistit použitím systému zpětné vazby, kdy se prakticky naslepo a nepřesně řídí pohyb motoru a až zpětně se vyhodnocuje o kolik, jak moc nebo jak rychle se vlastní pohyb uskutečnil. Existuje několik možných principů, jak lze vyhodnocení provést. Jednou z nich je použití tzv. rotačního optického enkodéru polohy hřídele (optical shaft encoder), kdy se využívá principu optické závory. Optická vazba optočlenu je zde vytvářena kotoučkem s průchozími nebo odraznými plochami připevněným na hřídeli řízeného motoru. [10]
Obrázek 2.12 - Příklad soustavy rotačního optického enkodéru, řízeného motoru a ovládané mechanické soustavy (ramene) 2.3.2.1 Obecný popis optických enkodérů Velmi stručně se dá uvést, že rotační enkodér (snímač) je elektromechanický převodník, který převádí rotační pohyb na sekvence elektrických digitálních impulsů. V případě zde popisovaného rotačního optického enkodéru je celý systém enkodéru složen z následujících hlavních částí: • Disk s posloupností opticky průhledných (Translucent) a neprůhledných (Opaque) otvorů nebo odrazných a neodrazných plošek
35
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
• Optickým vysílačem (Light source) - nejčastěji diodou LED vysílající světlo ve viditelném nebo infračerveném spektru • Optickým přijímačem (Photodetector) - fototranzistor nebo fotodioda • Případně může být součástí enkodéru i optika pro zaostření optického paprsku Princip funkce je zhruba následující: Disk je spojen s otočnou spojovací hřídelí enkodéru (shaft), která je pak v praxi připevněna k hřídeli motoru. Hřídel se tedy otáčí současně s diskem. Světlo vysílané světelným zdrojem (diodou LED), který je umístěn před diskem, může procházet diskem pouze průhlednými okénky (Translucent), zatímco zbývající část disku je neprůhledná (Opaque) a pohlcuje světlo. Světelné impulsy vytvářené otáčením disku aktivují optický snímač, který je převádí na elektrické impulsy nebo obdélníkový signál. Aby bylo u snímače zajištěno generování obdélníkových impulsů bez rušení, musí být elektrický signál zesílen a elektronicky zpracován. Pro zlepšení kvality a stability výstupních signálů se může snímat v diferenciálním režimu, kdy se porovnávají dva téměř totožné signály s opačnou fází (tj. fázový rozdíl 180 elektrických stupňů). Snímání rozdílu dvou signálů odstraňuje souhlasné rušení, protože stejné signály libovolného tvaru se vždy překryjí. [10]
Obrázek 2.13 - Princip rotačního optického enkodéru
V případě reflexních enkodérů jsou zdroj „světla” i jeho přijímač společně umístěny na jedné straně kolečka, na kterém jsou reflexní a matné plošky. Jejich střídání před přijímačem při otáčení kolečka na něm způsobuje změny napětí v důsledku změny množství detekovaného světla. Provedením prahování napětí na
36
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
přijímači se získá obdélníkový signál, kde každý obdélník odpovídá jedné reflexní či matné ploše. Při jejich rovnoměrném rozmístění po obvodu kolečka odpovídá jeden obdélník otočení vždy o shodný počet stupňů. Enkodérům generující takovýto pravidelný obdélníkový signál říkáme inkrementální. Mimo výše popsaných inkrementálních enkodérů existují i tzv. absolutní. Zatím co inkrementální enkodéry pouze detekují změnu polohy o pevně daný minimální krok, absolutní enkodéry nám
říkají přímo polohu v rámci 360 stupňů. Toho je dosaženo zakódováním každé polohy natočení kotouče, tzn. že každému určitému úhlovému rozsahu odpovídá určitá jedinečná kombinace impulsů.
Vyhodnocení signálu z inkrementálního enkodéru
Obrázek 2.14 - Provedení inkrementálního enkodéru s detekcí směru pohybu a definovaným počátkem Inkrementální enkodér obvykle generuje dva obdélníkové průběhy, vzájemně posunuté o 90 elektrických stupňů, které se nazývají kanál A a kanál B. Zpracováním samotného kanálu A lze získat informaci o rychlosti otáčení, pomocí druhého kanálu je možné podle sekvence stavů generovaných oběma kanály rozeznat i směr otáčení viz Obrázek 2.15. K dispozici je ještě další signál, nazývaný Z nebo také nulový
37
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
kanál, který udává referenční (nulovou) polohu hřídele enkodéru. Tento signál je obdélníkový a ve fázi s kanálem A. [10]
Obrázek 2.15 - časový diagram signálů A a B optického enkodéru
[10]
2.3.2.2 Konkrétní popis použitých optických enkodérů Námi použitý optický enkodér je od firmy MAXON. Je inkrementálního typu a poskytuje 3 výstupní signály – A , B , Z. Proto je možné rozeznat i směr otáčení. Jeho rozlišení je 500 pulsů na otáčku.
2.3.3 Řídící systém pro řízení a regulaci motorů Řídicí systém se skládá ze 4 modulů a motorů s enkodéry. Blokové schéma celého řídicího systému je možno vidět na Obrázek 2.16. Modul řídicí logiky obsahuje 2 mikroprocesory AVR a převodník napěťových úrovní MAX232. Schéma viz. příloha. ATMEGA8 má za úkol pomocí signálů z enkodérů zjistit směr otáčení obou motorů. Děje se to pomocí signálů A z obou enkodérů, které jsou připojeny jako zdroj externích přerušení. Nastane-li tedy náběžná hrana signálu A některého z enkodéru, zjistíme v obslužné rutině externího přerušení pomocí signálu B, který je snímán vstupními piny a je posunut o 90° od signálu A, směr otáčení. Ten pak
38
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
pomocí vstupně/výstupního portu předáme zakódován v jednom bitu mikroprocesoru ATMEGA32, který tuto informaci čte v průběhu každé vzorkovací periody snímáním stavu daných vstupních pinů. ATMEGA32 má za úkol řídit a regulovat otáčky obou motorů a komunikovat s nadřazeným výpočetním systémem. Signály A obou enkodérů jsou opět přivedeny jako zdroj externího přerušení. Při každé náběžné hraně signálu A některého z enkodéru se inkrementuje v obslužné rutině proměnná encodA resp. encodB, pomocí které určujeme otáčky motoru. Nulování provádíme každou vzorkovací periodu. Vzorkovací periodu určujeme 16bitovým časovačem 1. Tento časovač
časuje na frekvenci 16MHz a určuje periodu vzorkování přerušením vyvolaném při přetečení tzn.
Tvz =
216 = 4ms 16 * 10 6 Hz
Regulace je zajištěna pomocí PS regulátoru. Požadované otáčky se čtou pomocí sériového rozhraní USART od nadřazeného výpočetního systému. Ke čtení se využívá přerušení při příjmu. Protokol příjmu požadovaných otáček je ve tvaru: “ A ± XXXB ± XXX “. Pomocí znaků „A“ a „B“ se identifikuje motor A resp. motor B. Znaménko určuje směr požadovaných otáček. “XXX“ je tříciferné číslo od 0 do 255, které určuje požadované otáčky od nulových do maximálních otáček. Při maximálních otáčkách totiž signál od enkodéru vytvoří během vzorkovací periody asi 255 pulsů, a proto proměnná encodA resp. encodB má na konci periody hodnotu od 0 do 255 a po určení znaménka, pomocí informace o směru od mikroprocesoru ATMEGA8, nám přímo určuje skutečné otáčky. Proto je možno bez jakékoliv úpravy přímo počítat regulační odchylku jako rozdíl požadovaných a skutečných otáček. Akční zásah se pak vypočítá pomocí rovnice navrženého PS regulátoru. Pro plynulý rozjezd a zamezení velkých proudových rázů se upravují požadované otáčky tak, aby při změně plynule dosáhly nové požadované hodnoty. Toto je řešeno pomocí lineární nájezdové rampy. Další zamezení proudových rázů je
39
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
zajištěno pomocí hlídání diference akčního zásahu. Pokud překročí nastavenou mez, akční zásah se sníží nebo zvýší pouze tak, aby diference byla maximální dovolená. Toto ošetření proudových rázů chrání především výkonové H-můstky, protože jejich tepelná ochrana, releové vypnutí při nadproudu pomocí bezpečnostního modulu ani předřazené pojistky nestihnou zareagovat dostatečně rychle a mohlo by dojít ke zničení výkonových součástek. Jiné řešení by bylo naddimenzovat výkonové H-můstky. Bohužel v integrované formě se tyto součástky pro větší výkony již nevyrábí a H-můstek i jeho řídící elektronika by bylo nutné vytvořit z diskrétních součástek a celá konstrukce by tak byla složitější. Otáčky se řídí pomocí PWM signálu, který je generován časovači 0 a 2 a jehož střídu určuje 8bitový registr OCR0 pro motor A a OCR2 pro motor B. Rychlost otáčení se tedy zajistí pouhým přiřazením absolutní hodnoty vypočteného akčního zásahu do daného OCR registru. Pokud je akční zásah záporný, zajistí se reverzace pomocí inverze dvou bitů na výstupním portu, které jsou připojeny ke vstupům IN1 a IN2 na integrovaném H-můstku a řídí směr otáčení. Zpětná proudová vazba je zajištěna pomocí snímacího rezistoru. Zde se proud převede na napětí, které je pomocí operačního zesilovače, zapojeném v diferenčím provedení, zesíleno a je zpracováno pomocí integrovaného A/D převodníku přímo v mikroprocesoru na digitální informaci. Potom se digitalizovaná informace o napětí zpětně přepočítá na proud: proud =
napeti * GSENSE ZESILENI
proud………..proud tekoucí motorem napětí……….napětí na vstupu A/D převodníku ZESILENI….zesílení operačního zesilovače GSENSE……vodivost snímacího rezistoru
Pokud nadproud trvá déle jak přednastavený čas, dá mikroprocesor pomocí výstupního portu příkaz bezpečnostnímu modulu k odpojení motorů pomocí relé. Bezpečnostní modul také zajišťuje měření napětí na napájecích bateriích pomocí napěťového děliče. Napětí na děliči je potom přivedeno na A/D převodník ATMEGA32 a zpětně dopočítáno na napájecí napětí baterií. Tuto informaci pak dává
40
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
mikroprocesor nadřazenému výpočetnímu systému. Je to především proto, aby se systém stihl bezpečně vypnout, než poklesne napětí pod úroveň, kdy již nemůže fungovat. Výkonová část pro řízení motorů je popsána nahoře.
41
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Obrázek 2.16 - Blokové schéma systému pro řízení a regulaci motorů
42
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
2.3.4 Nastavení parametrů regulátoru 2.3.4.1
Identifikace regulované soustavy
Lv = 0.191
[mH]
indukčnost vinutí
Rv = 0.605
[ Ώ]
odpor vinutí
Km = 29.2*10-3
[Nm/A]
momentová konstanta
Kω = 328
[rpm/V]
rychlostní konstanta
u = 24
[V]
napájecí napětí motoru
ui
[V]
zpětné indukované napětí motoru
i
[A]
proud vinutím motoru
ω
[rad/s]
otáčky motoru
Mm
[Nm]
moment motoru
ζ = Km
[Nm/A]
konstrukční konstanta motoru
Jm = 78.7*10-7
[kg*m2]
moment setrvačnosti motoru
Jp = 9.4*10-7
[kg*m2]
moment setrvačnosti převodovky
Jk = 1,39*10-2
[kg*m2]
moment setrvačnosti kola
m = 30
[kg]
hmotnost robotu
r = 0.16
[m]
poloměr kola robotu 2
[kg*m ]
celkový moment setrvačnosti vztažený na stranu
i = 113
[-]
převod převodovky
Umax = 24
[V]
amplituda pulsů PWM signálu
n=8
[-]
počet bitů registru, který určuje akční zásah
KPWM
[-]
zesílení zohledňující střední hodnotu signálu
Tvz = 4
[ms]
vzorkovací perioda
J motoru
Určení celkového momentu setrvačnosti vztaženého na stranu motoru:
J = Jm
2J k mr 2 + Jp + 2 + = 41 * 10 − 6 kgm 2 i 2i
2
43
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Řešení v časové oblasti:
dω + m z = ξ i (t ) dt u i (t ) = ξω (t ) M m (t ) = J
u (t ) = R v i (t ) + Lv
di (t ) + u i (t ) dt
Řešení v Laplaceově rovině:
U ( p) = Rv I ( p) + Lv pI ( p) + ξΩ( p ) JpΩ( p) = ξI ( p) Z druhé rovnice se vyjádří obraz proudu a dosadí se do první: U ( p) =
Rv JΩ( p ) p
ξ
+
Lv JΩ( p ) p 2
ξ
+ ξΩ( p )
U ( p ) Rv Jp + Lv Jp 2 + ξ 2 = ξ Ω( p ) Ω( p ) 1 1 FS ( p ) = = U ( p ) ξ Lv J 2 Rv J p + 2 p +1 2
ξ
ξ
Protože se otáčky řídí střídou pulsů určenou 8bitovým registrem ( rozsah střídy určujeme číslem od 0 do 255), zesílení soustavy je nutné upravit s tímto ohledem. Budeme proto uvažovat střední hodnotu vstupního napětí a tento přepočet zahrneme do zesílení KPWM.
U MAX 2n 1 FS ( p ) = ξ Lv J K PWM =
ξ2
K PWM RJ p 2 + v2 p + 1
ξ
Po dosazení: FS ( p ) =
3.21 9.18 * 10 p + 29.09 * 10 −3 p + 1 −6
2
44
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
2.3.4.2
Návrh regulátoru
Modifikovaný diskrétní přenos spojitého přenosu soustavy - připojen je tvarovač nultého řádu, perioda vzorkování je Tvz = 4ms (pomocí c2d funkce MATLABU) :
FC ( z ) =
0.3853 z + 0.03132 P( z ) = -6 Q( z ) z - 0.8702 z + 3.127 * 10 2
Podle požadavků nulové ustálené regulační odchylky při konstantním řízení a konečného regulačního děje má přenos řízení vyhovovat rovnicím: FW(z) = P(z) R(z) 1 - FW(z) = (1 - z -1 ) G(z)
R(z) a G(z) jsou volitelné polynomy. Z hlediska realizovatelnosti musí platit: r0 = r1 = 0 g0 = 1
Další členy se určí z rovnice: 1 - (0.3853 z + 0.03132)r2 z -1 = (1 - z -1 )(1 + g 1z -1 )
Srovnáváním koeficientů u stejných mocnin získáme: 0.3853 r2 = 1 − g 1 0.031232r2 = g 1
45
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Soustavě rovnic vyhovují hodnoty : r2 = 2.4008 g1 = 0.0749
Požadovaný přenos řízení má potom tvar: FW(z) = 0.9250z -1 + 0.0752 z −2
Přenos číslicového korekčního členu je:
D( z ) =
FW 2.401 - 1.894 z -1 - 0.1698 z -2 + 6.103e - 007z -3 = FC − FW * FC 1 - 0.8437 z -1 - 0.1504 z -2 - 0.006113z -3
Obrázek 2.17 - Odezva regulátoru D na jednotkový skok a jednotkový impuls
46
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Vypočítaný přenos regulátoru lze nahradit jednodušším typem, který svými vlastnostmi odpovídá známým typům spojitých regulátorů. Zde je podle odezvy na jednotkový skok a jednotkový impuls (viz. Obrázek 2.17) možné nahradit vypočítaný přenos regulátoru přenosem regulátoru PS (diskrétní ekvivalent spojitého PI):
2.401 - 1.894 z -1 Dps ( z ) = 1 - z -1
Obrázek 2.18 - Porovnání přechodových charakteristik vypočítaného regulátoru D a nahrazeného Dps
47
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Obrázek 2.19 - Simulační schéma regulace otáček motoru robotu
Pomocí přenosu navrženého PS regulátoru sestavíme řídicí algoritmus. Celý regulační systém je možné simulovat viz. Obrázek 2.19. Při simulaci lze přenos regulátoru nahradit přímo algoritmem napsaným v S-funkci a ověřit si tak správnost navrženého algoritmu. Soustavu je možné nahradit blokovým modelem a můžeme tak sledovat kromě otáček i proud a moment. Blokový model motoru je znázorněn na Obrázek 2.20 - Blokový model stejnosměrného motoru s cizím buzením.
Obrázek 2.20 - Blokový model stejnosměrného motoru s cizím buzením Algoritmické vyjádření navrženého PS regulátoru v kódu:
Az = d0*(e + M) + M*d1; M = (e + M);
48
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
kde:
Az………….akční zásah d0,d1……….navržené konstanty PS regulátoru e…………....regulační odchylka M…………...paměť pro hodnoty z předchozího vzorku
2.3.4.3 Grafy viz. příloha
Výsledky navrženého regulátoru
49
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
Obrázek 2.21 - Aktuální stav robotu k 7.5.2008
50
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
3.
ZÁVĚR
Prvním úkolem bylo zajistit snímání vzdálenosti pomocí ultrazvukových snímačů. Narazili jsme zde na problém při vysílání ultrazvukového signálu. Při vysílání pulsů má modul ultrazvuku velký špičkový proudový odběr, snižuje špičkově napájecí napětí a mikroprocesor pak nepracuje správně. Problém byl původně řešen při testovacím zapojením na nepájivém poli připojením kondenzátoru 4.7uF paralelně na napájecí napětí, které tyto napěťové poklesy vyhladil. Ale při zkoušce na vyleptaném tištěném spoji bylo zjištěno, že mikroprocesor opět nepracuje správně. Vysvětlení je následující. Nepájivé pole má zaručeně jinou celkovou impedanci než tištěný spoj a tyto proudové špičky působí v zapojení na tištěném spoji jinak. Pro utlumení těchto proudových nárazů na tištěném spoji byla opět použita paralelní kapacita, kombinovaná však v sérii s indukčností – tlumivkou. Po poradě s vedoucím práce jsem dle rad přepracoval řídicí algoritmus pro ovládání jednotlivých ultrazvukových modulů. Problém s poklesy napájecího napětí zmizel a ultrazvukový systém funguje správně i bez filtrací. V tomto algoritmu ovládání se nepoužívá interní zatemňování, ale doba zatemňování se řídí pomocí časovače
řídicího mikroprocesoru. Dosáhneme tak nižších snímatelných mezí, dokonce mnohem nižších, než udávaných hodnot v manuálu ultrazvukových modulů. Dalším úkolem bylo zajistit řízení obou stejnosměrných motorů MAXON. Navrhnul jsem výkonové moduly, které ovládají pomocí řídicích signálů a PWM signálu od mikroprocesoru směr a otáčky motoru. Pro regulaci otáček bylo nutné získat zpětnou vazbu od enkodéru a dostat tak informaci o skutečných otáčkách. K regulaci dále potřebujeme požadované otáčky. Ty zadává nadřízený systém. Regulaci zajišťuje implementovaný regulátor, který bylo nutné navrhnout. Navržený regulátor má ve skutečnosti větší překmit i dobu ustálení (uvažujeme-li ustálení s určitou tolerancí), než při regulaci na modelu soustavy. Odchylka je způsobena převážně nepřesností modelu soustavy, protože do modelu nebyly zahrnuty všechny faktory ovlivňující jízdu robotu např. pružnost řemene spojující kola, valivý odpor. Z menší míry přispěla k odchylce také chyba při měření. Dále jsem řešil jak odstranit proudové nárazy a ošetřit případné nadproudy. Proudové nárazy se podařilo zmírnit
51
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
omezením diference akčního zásahu a nájezdovou rampu požadovaných otáček. Pro kontrolu nadproudů bylo nutné vytvořit zpětnou proudovou vazbu. Vypnutí při nadproudu, ohrožujícího motory a výkonové součástky, řeší samozřejmě nadřazené pojistky, ale také bezpečnostní modul, který pomocí relé odpojí motory, když mikroprocesor vyhodnotí situaci jako kritickou. Získal jsem tak informace a teoretické znalosti o dané problematice. Prakticky jsem se seznámil s řízením stejnosměrného motoru pomocí PWM signálu a zpětné vazby zprostředkované optickým enkodérem. Zdokonalil jsem se v oblasti programování mikročipů AVR v jazyce C a získal jsem trochu ucelený pohled na problematiku robotiky.
52
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
4. [1]
POUŽITÁ LITERATURA NOVÁK, R. Mobilní roboty pohony, senzory, řízení. Praha: BEN, 2007. ISBN 80-7300-141-1.
[2]
VÁŇA, V. Mikrokontroléry ATMEL AVR – programování v jazyce C. Praha: BEN, 2006. ISBN 80-7300-102-0.
[3]
Technická dokumentace pro 6500 Series Sonar Ranging Module. Dostupné na internetu: http://www.engr.udayton.edu/faculty/jloomis/ece445/topics/sonar/info.html
[4]
Technická dokumentace pro ATMEGA32. Dostupné na internetu: http://www.datasheetcatalog.com/datasheets_pdf/A/T/M/E/ATMEGA32.shtml
[5]
Technická dokumentace pro ATMEGA8. Dostupné na internetu: http://www.datasheetcatalog.com/datasheets_pdf/A/T/M/E/ATMEGA8.shtml
[6]
Technická dokumentace pro L6203. Dostupné na internetu: http://www.datasheetcatalog.com/datasheets_pdf/L/6/2/0/L6203.shtml
[7]
Wikipedie otevřená encyklopedie – Mobilní robotika. Dostupné na internetu: http://cs.wikipedia.org/wiki/Mobiln%C3%AD_robotika
[8]
CHURÝ, L. Robotika – úvod. 2005. Dostupné na internetu: http://programujte.com/index.php?akce=clanek&cl=2005121101-robotikauvod
[9]
DŘINEK,M. Architektura AVR v kostce. 2000. Dostupné na internetu: http://avr.hw.cz/
[10] Optické snímače polohy - enkodéry. Dostupné na internetu: http://jonatan.spse.pilsedu.cz/~mazanec/ [11] Ovladač stejnosměrného motoru využívající PWM. Dostupné na internetu: http://hw.cz/docs/motor_rizeni/ss_motor_pwm.html [12] LOCKER,M. Řízení stejnosměrného motoru. Dostupné na internetu: http://64.233.183.104/search?q=cache:n1yfAB5JPkAJ:www.eurobot.cz/Works hop2006/Rizeni_stejnosmerneho_motoru.ppt
53
ÚSTAV AUTOMATIZACE A MĚŘICÍ TECHNIKY Fakulta elektrotechniky a komunikačních technologií Vysoké učení technické v Brně
SEZNAM PŘÍLOH Příloha 1
Schéma zapojení modulu ultrazvukového systému
Příloha 2
Schéma zapojení výkonového modulu řízení
Příloha 3
Modul řídicí logiky
Příloha 4
Bezpečnostní modul
Příloha 5
Měřená závislost otáček a proudu na čase - nájezdová rampa
Příloha 6
Simulovaná závislost otáček a proudu na čase - nájezdová rampa
Příloha 7
Měřená závislost otáček a proudu na čase – porucha
Příloha 8
Simulovaná závislost otáček a proudu na čase – porucha
54
Příloha 1 - Schéma zapojení modulu ultrazvukového systému
Příloha 2 - Schéma zapojení výkonového modulu řízení
Příloha 3 - Modul řídicí logiky
Příloha 4 – Bezpečnostní modul
Příloha 5 – Měřená závislost otáček a proudu na čase - nájezdová rampa Měřená závislost otáček a proudu na čase - požadované otáčky : nájezdová rampa (7750rpm/s) na 1860 rpm 2500
1500
1000
500
čas [ ms ] Požadované Otáčky
Skutečné Otáčky
Proud
1700
1600
1500
1400
1300
1200
1100
1000
900
800
700
600
500
400
300
200
100
0 0
Otáčky [rpm] Proud [mA]
2000
Příloha 6 – Simulovaná závislost otáček a proudu na čase - nájezdová rampa Simulovaná závislost otáček a proudu na čase - požadované otáčky : nájezdová rampa (7750rpm/s) na 1860 rpm 2500
1500
1000
500
čas [ms] Skutečné otáčky
Požadované otáčky
Proud
1672
1628
1584
1540
1496
1452
1408
1364
1320
1276
1232
1188
1144
1100
1056
968
1012
924
880
836
792
748
704
660
616
572
528
484
440
396
352
308
264
220
176
132
88
0
0 44
otáčky [rpm] Proud [ mA ]
2000
Příloha 7 – Měřená závislost otáček a proudu na čase – porucha Měřená závislost otáček a proudu na čase - požadované otáčky : 2480 rpm, porucha asi od 3.5 s do 5 s 7000
3000
6000
2500
5000
1500 3000 1000 2000
500
1000
0
čas [ s ] Skutečné Otáčky
Požadované Otáčky
Proud
8,05
7,88
7,71
7,54
7,37
7,2
7,03
6,86
6,69
6,52
6,35
6,18
6,01
5,84
5,5
5,67
5,34
5,17
5
4,82
4,65
4,48
4,31
4,14
3,97
3,8
3,63
3,46
3,29
3,12
2,95
2,78
2,61
2,44
2,1
0
Proud [ mA]
4000
2,28
Otáčky [ rpm ]
2000
Příloha 8 – Simulovaná závislost otáček a proudu na čase – porucha Simulovaná závislost otáček a proudu na čase - požadované otáčky : 2480 rpm, skoková porucha od 3.5 s do 5 s
3000
7000
6000 2500 5000
1500
3000
2000 1000 1000 500 0
čas [ s ] Skutečné Otáčky
Požadované Otáčky
Proud
6,89
6,76
6,62
6,49
6,36
6,23
6,1
5,96
5,7
5,83
5,57
5,44
5,3
5,17
5,06
4,96
4,83
4,69
4,56
4,43
4,3
4,17
3,9
4,03
3,77
3,64
3,53
3,42
3,29
3,16
3,03
2,9
2,76
2,5
2,63
2,37
-1000 2,24
0
Proud [ mA ]
4000
2,1
Otáčky [ rpm ]
2000