Bakalářská práce

Českévysokéučení technickév Praze

F3 Fakulta elektrotechnickáKatedra kybernetiky

Návrh a implementace řídícíhoalgoritmu pro model závodníhovozidla

Michal Mrosko

Květen 2013Vedoucí práce: Ing. Tomáš Haubert

Poděkování / ProhlášeníDěkuji Ing. Tomáši Haubertovi

za cenné rady a příkladné vedení mépráce, otci Michalovi za jeho zručné rucepři mechanických úpravách závodníhomodelu, partnerce Šárce za neustá-lou podporu a příliv pozitivní energiea společnosti Freescale Semiconductorza pořádání soutěže.

Prohlašuji, že jsem předloženou prácivypracoval samostatně a že jsem uvedlveškeré použité informační zdroje v sou-ladu s Metodickým pokynem o dodržo-vání etických principů při přípravě vy-sokoškolských závěrečných prací.

V Praze dne 24. 5. 2013

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

iii

Abstrakt / AbstractTato bakalářská práce popisuje vývoj

a implementaci řídícího algoritmu mo-delu závodního vozidla v rámci technic-ké soutěže Freescale Cup. Závodní drá-ha, vymezena bílým podkladem s černouvodící čárou, je snímána jednořádkovoukamerou umístěnou na vozidle, jejíž ob-raz je následně zpracováván za účelemdetekce tvaru dráhy a na základě tétoinformace se uzpůsobuje natočení před-ních kol řízené polohovým servomecha-nismem a rychlost otáčení hnacích stej-nosměrných motorů.

Pro návrh řídícího algoritmu byl vprogramu MATLAB/Simulink vytvo-řen zjednodušený matematický modelvozidla. Algoritmus napsaný v jazyceC byl poté implementován do řídícíjednotky a jeho výsledky zhodnocenyna základě chování matematického areálného modelu vozidla.

This bachelor thesis describes thedevelopment and implementation of thecontrol algorithm of racing car modelin the technical competition FreescaleCup. Racetrack, defined by white back-ground with black guide line, is scannedby one-line camera placed on the vehiclewhose image is then processed to detectthe shape of the track and on the basisof this information it adapts the frontwheels angle controlled by a positionalservomechanism and the speed of therotation of driving motors.

For the design of the control algo-rithm was created a simplified mathe-matical model of the vehicle in programMATLAB/Simulink . The algorithmwritten in C language was then im-plemented to the control unit and itsresults were evaluated on the basisof behavior of mathematical and realvehicle model.

iv

Obsah /1 Úvod . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .11.1 The Freescale Cup . . . . . . . . . . . . . . .11.2 Závodní trať . . . . . . . . . . . . . . . . . . . . . .21.3 Závodní vozidlo . . . . . . . . . . . . . . . . . .2

1.3.1 Šasi . . . . . . . . . . . . . . . . . . . . . . . . .21.3.2 Obrazový senzor . . . . . . . . . . .31.3.3 Polohový servomotor . . . . . .31.3.4 Hnací motory . . . . . . . . . . . . . .41.3.5 Řídící elektronika . . . . . . . . . .51.3.6 Výkonová elektronika . . . . . .5

2 Matematický model vozidla . . . . . . .72.1 Geometrický model zatáčení

vozidla . . . . . . . . . . . . . . . . . . . . . . . . . . . .72.2 Servořízení . . . . . . . . . . . . . . . . . . . . . . .8

2.2.1 Model polohového ser-vomotoru . . . . . . . . . . . . . . . . . . .8

2.3 Pohon. . . . . . . . . . . . . . . . . . . . . . . . . . . . .82.3.1 Úprava rovnic pro ope-

rátorový přenos . . . . . . . . . . . .92.3.2 Model pohonného

ústrojí . . . . . . . . . . . . . . . . . . . . 102.4 Rovinný matematický model

vozidla . . . . . . . . . . . . . . . . . . . . . . . . . . 102.4.1 Odvození dostředivého

a tečného zrychlení . . . . . . 102.4.2 Sestavení pohybových

rovnic . . . . . . . . . . . . . . . . . . . . . 112.4.3 Linearizace pohybo-

vých rovnic . . . . . . . . . . . . . . . 123 Identifikace parametrů mate-

matického modelu. . . . . . . . . . . . . . . . 133.1 Nástroje použité k identifikaci . 13

3.1.1 Filtrování signálu z ak-celerometru. . . . . . . . . . . . . . . 13

3.2 Identifikace modelu pro po-hyb v ose x . . . . . . . . . . . . . . . . . . . . . 14

3.3 Úprava modelu pro pohyb vose y . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3.4 Identifikace modelu pro po-hyb v ose y . . . . . . . . . . . . . . . . . . . . . 16

4 Zpracování signálu z obrazo-vého senzoru . . . . . . . . . . . . . . . . . . . . . . 18

4.1 Nastavení zorného pole azaostření . . . . . . . . . . . . . . . . . . . . . . . . 18

4.2 Nastavení expozičního času. . . . 194.3 Detekce hran . . . . . . . . . . . . . . . . . . . 20

4.4 Algoritmus detekce pozicevodící čáry . . . . . . . . . . . . . . . . . . . . . 20

5 Návrh a implementace řízenívozidla . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

5.1 Zpětná vazba rychlosti. . . . . . . . . 225.2 Model vozidla v prostředí

MATLAB/Simulink. . . . . . . . . . . . 235.3 Návrh a implementace regu-

látorů . . . . . . . . . . . . . . . . . . . . . . . . . . . 246 Závěr . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

Literatura . . . . . . . . . . . . . . . . . . . . . . . . . 27A Zadani práce . . . . . . . . . . . . . . . . . . . . . . 30B Obsah CD . . . . . . . . . . . . . . . . . . . . . . . . . 31

v

Kapitola 1Úvod

1.1 The Freescale CupSoutěž The Freescale Cup, jež je každoročně pořádána firmou Freescale Semiconductor,spočívá v závodění autonomních modelů vozidel na předem neznámé trati, kde vyhrávásoutěžní tým, jehož vozidlo projede trať v nejkratším čase.

Obrázek 1.1. Logo soutěže The Freescale Cup.

Každý tým obdrží stejný závodní kit. Povinné originální vybavení, které musí být nazávodním modelu použito, je následující:.pneumatiky a kola,.hnací motory,.převodový mechanismus mezi hnacími motory a koly,.polohový servomechanismus se zavěšením kol,.baterie,.H-můstek MC33931 nebo MC33932,.mikrokontrolér Qorivva MPC5604B nebo Kinetis K40X256,. šasi – úpravy jsou možné až na následující:

. tvar rámu musí být zachován,. vzdálenost mezi koly musí být zachována,. model vozidla nesmí přesáhnout rozměry 400(d)×250(š)×300(v) mm.

Pro ucelenější pohled na danou problematiku ještě uvádím výňatek těch nejdůleži-tějších pravidel soutěže:.Žádný přídavný mikrokontrolér nebo programovatelné zařízení není povoleno kromě

mikrokontroléru Freescale (viz povinné vybavení)..Během závodu není povolena jakákoliv bezdrátová komunikace s vozidlem..Pokud vozidlo vyjede ze závodní tratě vymezené bílým podkladem byť jen jedinýmkolem, čas nebude počítán..Pokud vozidlo po projetí cílem nezastaví do vzdálenosti 3 metrů od cílové čáry, budek času připočtena penalizace.

1

1. Úvod . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .1.2 Závodní trať

Tvar závodní tratě je předem neznámý, jsou však známy důležité parametry elementů,ze kterých je složena. Základním parametrem je šířka tratě minimálně 60 cm, vymezenabílým podkladem z matného ABS plastu, tloušťka vodící čáry má pevnou šířku 1 palec,tedy 2,54 cm. Na obrázku 1.2 je pro názornost ilustrován možný tvar závodní dráhy.

Obrázek 1.2. Možný tvar závodní dráhy.

Trať se skládá z rovinek, zatáček o minimálním poloměru 50 cm, křížení, cílovéhodílu se startovní/cílovou čárou, retardérů a kopců. Specifický cílový díl se zakótovanýmivýznamnými rozměry je zobrazen na obrázku 1.3.

Obrázek 1.3. Cílový díl tratě. Převzato z [1]

1.3 Závodní vozidlo

1.3.1 ŠasiŠasi závodního vozidla pochází od čínské společnosti Dongguan BDS Technology, kteráse specializuje na výrobu kitů pro RC modely. Dodaný kit kromě šasi obsahuje taképolohový servomotor, hnací DC motory a kola s gumovými pneumatikami. Významnérozměry celého podvozku s koly jsou zakótovány na obrázku 1.4. K šasi je dále připev-něna výkonová a řídící elektronika, NiMH akumulátor a obrazový senzor.

2

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3 Závodní vozidlo

280

200

160

24

50

Obrázek 1.4. Rozměry modelu vozidla – půdorys.

1.3.2 Obrazový senzorPro snímání polohy černé vodící čáry na dráze je použit CMOS obrazový senzorTSL1401CL od výrobce TAOS, který je umístěn na stojanu na předním nárazníkuvozidla, viz obrázek 1.5. Protože má senzor pevnou ohniskovou vzdálenost, šířku záběruobrazu lze měnit pouze změnou výšky připevnění senzoru ke stojanu a nastavenímvzdálenosti, do které hledí. Ostření obrazu se provádí ručně.

300

Obrázek 1.5. Přichycení obrazového senzoru.

Důležité technické parametry obrazového senzoru jsou:. rozlišení obrazu 128×1 pixelů,. světelnost objektivu f/2,4,. expoziční čas 267 µs až 68 ms.

Vstupními signály CLK a SI je řízeno získávání analogového obrazového signálu AO,viz obrázek 1.6. Signálem SI je tak řízeno započetí snímání obrazu, kde se v následujících128 pulsech signálu CLK postupně nabíjejí jednotlivé kapacity vlivem dopadajícíhosvětla. Expoziční čas, po který se kapacity reprezentující obrazové body nabíjí, je takřízen periodou signálu CLK.

1.3.3 Polohový servomotorPoužitý polohový servomotor je model S3010 od společnosti Futuba. Tento servomotorřídí natáčení kol přední nápravy. Napájení je řešeno prostřednictvím H-můstku. Důležitéparametry použitého servomotoru jsou:.vstupní napětí 5 V

3

1. Úvod . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Obrázek 1.6. Řídící signály obrazového senzoru. Převzato z [2].

. rychlost otáčení 60◦/0.2 s.kroutící moment 5.2 kg·cm

Řídícím vstupem tohoto servomotoru je PWM signál o frekvenci 50 Hz, jehož střídaurčuje natočení hřídele. Závislost takového úhlu natočení hřídele servomotoru na stříděPWM signálu je ilustrována na obrázku 1.7.

1 ms

1,5 ms

2 ms

-90

0

+90

Obrázek 1.7. Závislost úhlu natočení hřídele na střídě PWM.

1.3.4 Hnací motoryZávodní vozidlo pohánějí 2 stejnosměrné motory nespecifikovaného výrobce připevněnéna zadní nápravě. Prostřednictvím převodů z ozubených kol je otáčivý pohyb hřídelepřeváděn na samostatně připevněná kola – každé kolo se tak může otáčet rozdílnourychlostí.

Motory jsou připojeny na H-můstek umístěný na výkonové desce, který se stará ojejich napájení a řízení rychlosti otáčení. Otáčky motoru jsou tak prostřednictvím H-můstku regulovány PWM signálem. Pokud je střída PWM signálu rovna 100:0, motoryse otáčejí maximální možnou rychlostí, se snižující střídou se motory otáčejí pomaleji.

Důležité parametry použitého stejnosměrného motoru jsou:

. řídící napětí 7,2 V. rychlost otáčení 16000±3200 otáček/min (při nezatížení).kroutící moment 80 g·cm

Motory svým otáčením produkují do elektroniky nezanedbatelný šum, který je po-třeba potlačit, neboť signál z obrazového senzoru je na výkonové desce veden poblížH-můstků. Proto byly k vodičům motorů připojeny tlumivky, které ho razantně potla-čují.

4

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3 Závodní vozidlo

1.3.5 Řídící elektronikaK řízení celého modelu vozidla je použit 32-bitový mikrokontrolér Quorivva MPC5604Bod společnosti Freescale, který je zaměřen převážně na použití v automobilovém prů-myslu. Tento mikrokontrolér osazený na vývojové desce s mnoha periferiemi, viz obrázek1.8, disponuje mikroprocesorem s taktovací frekvencí až 64 MHz postaveným na Powerarchitektuře. Nejdůležitější parametry tohoto mikrokontroléru jsou:. taktovací frekvence až 64 MHz,.paměť 32 KB RAM,.paměť 512 KB Flash – pro kód,.paměť 64 KB Flash – pro data.

Obrázek 1.8. Vývojová deska s mikrokontrolérem MPC5604B. Převzato z [3].

Vývojová deska disponuje standardizovanými komunikačními porty. V této aplikaciřízení vozidla je využíván JTAG port, prostřednictvím kterého probíhá nahrávání pro-gramu do Flash paměti a přenos informace mezi mikrokontrolérem a PC. Napájecínapětí 9 až 12 V je přivedeno přes samostatnou výkonovou desku.

Schéma vnitřního uspořádání mikrokontroléru je zobrazeno na obrázku 1.9. Komuni-kační vstupně/výstupní systém je složen z jednotlivých funkčních jednotek, které jsounezbytné pro většinu průmyslových nasazení a nebo jen usnadňují práci programáto-rovi tím, že dokáží jako podsystém pracovat samostatně, bez intervence procesoru. Zdevyjmenuji pár takových jednotek, které jsou použity v rámci řídícího algoritmu včetněkonkrétní aplikace:.10-bitový AD převodník (ADC) – použit k digitalizaci analogového výstupu kamery

a během fáze ladění byl použit k digitalizaci výstupu potenciometru osazeném navývojové desce..Rozšířený modulární vstupně/výstupní systém (eMIOS) – použit ke generování PWMsignálů řídících polohový servomotor a hnací motory..LINFlex – sériové komunikační rozhraní použité pro komunikaci s PC prostřednic-tvím UART (univerzální asynchronní přijímač/vysílač). Používán převážně pro účelyladění – např. zobrazení výstupního signálu obrazového senzoru.

1.3.6 Výkonová elektronikaNapájení všech elektrických zařízení je realizováno prostřednictvím výkonové desky,viz obrázek 1.10, ke které je připojena baterie. Výkonová deska tak obstarává napá-jení servomotoru a obrazového senzoru, které vyžadují napětí 5 V. Dále napájí hnací

5

1. Úvod . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Obrázek 1.9. Vnitřní blokové uspořádání mikrokontroléru MPC5604B. Převzato z [3]

Obrázek 1.10. Výkonová deska.

stejnosměrné motory a řídící desku napětím 7,2 V. Podle specifikací řídící deska vy-žaduje napětí 9 V, nicméně i s nižším napájecím napětím funguje vše včetně periferiíbezproblémově.

Na výkonové desce jsou také dva H-můstky MC33931, které se starají o řízení stejno-směrných motorů. Schéma propojení mikrokontroléru s H-můstkem a k němu připojenýstejnosměrný motor je na obrázku 1.11.

V praxi jsou použity vstupy H-můstku IN1 a EN. Vstup IN1 je PWM signál o frek-venci 11KHz, který svou střídou ovlivňuje rychlost otáčení motoru. Další vstup ENřídí stav motoru. Ten tak může být ve stavu „zapnutý“ (logická 1) nebo „vypnutý“(logická 0). Dále je použit výstup FB, jež zprostředkovává zpětnou vazbu v podoběproudu protékajícího kotvou motoru.

Obrázek 1.11. Schéma zapojení mikrokontrolér – H-můstek – motor. Převzato z [4].

6

Kapitola 2Matematický model vozidla

2.1 Geometrický model zatáčení vozidlaPro boční řízení vozidla je požadován vstup v podobě poloměru zatáčky, který je vy-počítáván na základě výstupu z obrazového senzoru. Obrazový senzor detekuje pozicičerné vodící čáry před vozidlem a z této pozice jsem schopen vypočítat přibližný po-loměr zatáčky. Přibližný z toho důvodu, protože vozidlo se nemusí v daný okamžiksejmutí obrazu nacházet ve středu zatáčky. Tuto možnou nejistotu výpočtu však budekompenzovat robustní řídící algoritmus. Na obrázku 2.1 je zakreslena detekce pozicevodící čáry a důležité parametry, podle kterých se bude poloměr zatáčky počítat.

a

b

α

Obrázek 2.1. Detekce pozice černé vodící čáry.

Pro úhel α platí vzorec

α = arcsin b√a2 + b2

. (2.1)

Za použití Thaletovy věty vypočtu poloměr zatáčky R, pro který platí

R =√a2 + b2

2 cos(π2 − α) . (2.2)

Pro poloměr zatáčení vozidla Rv(t) si vystačím se zjednodušeným vzorcem

Rv(t) = l

tan θ(t) , (2.3)

kde l značí rozvor náprav a θ úhel natočení předních kol.

7

2. Matematický model vozidla . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .2.2 Servořízení

Mechanismus zatáčení předních kol je na obrázku 2.2. Rameno servomotoru, na obrázkužlutě, se může otáčet v rozmezí ± 30 stupňů kolem středové polohy. Tento otáčivýpohyb je prostřednictvím ramen zavěšení přenášen na posuvný pohyb, který způsobujenatočení předních kol v rozmezí ± 35 stupňů.

Obrázek 2.2. Mechanismus zatáčení.

Označím-li úhel natočení servomotoru ϕ(t), pak za předpokladu lineární závislostitěchto úhlů lze zapsat rovnici

θ(t) = 3530 ϕ(t). (2.4)

2.2.1 Model polohového servomotoruPolohový servomechanismus se v praxi podle [5] často modeluje jako ideální polohovýservomechanismus, jehož schéma v prostředí programu Simulink je na obrázku 2.3.Jedná se tedy o systém 1.řádu, který zanedbává samotnou dynamiku servomotoru, jehorychlost se nastavuje konstantou Kv Přenosová funkce tohoto systému má tvar

P (s) = Kv

s+Kv. (2.5)

Obrázek 2.3. Model ideálního polohového servomechanismu.

Jedním z parametrů servomotoru je jeho rychlost otáčení 60◦/0.2 s. Toto modelna obrázku 2.3 nerespektuje, protože ten dosahuje požadované ustálené hodnoty úhlunatočení hřídele servomotoru ϕ(t) vždy za stejný čas. Protože však vozidlo nepojedev zatáčkách příliš velkou rychlostí a minimální poloměr zatáček 0,5 m je vzhledem kminimálnímu poloměru zatáčení vozidla velký, předpokladem jsou tak malé skokovézměny úhlu natočení kol. Během identifikace modelu vozidla tak bude tento přenosnaladěn pro dostačující reprezentativnost reálného polohového servomechanismu.

2.3 PohonPohonné ústrojí, které produkuje hnací sílu, je složeno ze dvou stejnosměrných motorů.Protože je k jejich řízení používán stejný vstupní signál a dynamika vozidla je vyšetřo-vána na jednostopém modelu, bude na motory pohlíženo jako na jeden motor. Rovnicepopisující dynamiku stejnosměrného motoru jsou

8

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.3 Pohon

Ldi(t)dt = −Ri(t)− keω(t) + u(t), (2.6)

Jdω(t)

dt = kmi(t)− bω(t)−Mz(t), (2.7)

kde L je indukčnost motoru, i(t) protékající elektrický proud, R elektrický odpor ve-dení motoru, Ke elektrická konstanta motoru, ω(t) úhlová rychlost hřídele motoru,u(t) vstupní napětí motoru, J moment setrvačnosti motoru, km mechanická konstantamotoru, b je konstanta suchého tření motoru a Mz(t) vnější zatěžovací moment.

2.3.1 Úprava rovnic pro operátorový přenosV rovnicích (2.6) a (2.7) se vyskytuje úhlová rychlost ω(t), pro účely pozdější identifikaceparametrů rovnic mě bude zajímat přenos ze vstupního napětí na obvodovou rychlostkol, proto si pomocí vztahů (2.8) a (2.9) upravím rovnice (2.6) a (2.7) na tvar (2.10) a(2.10).

ω(t) = v(t)r, (2.8)

Mz(t) = mrdv(t)dt , (2.9)

Ldi(t)dt = −Ri(t)− ke

rv(t) + u(t), (2.10)

J

r

dv(t)dt = kmi(t)−

b

rv(t)−mrdv(t)

dt . (2.11)

Tyto upravené rovnice nyní převedu pomocí Laplaceovy transformace na operátorovýtvar

sLI(s) = −RI(s)− kerV (s) + U(s), (2.12)

sJ

rV (s) = kmI(s)−

b

rV (s)− smrV (s). (2.13)

Z rovnice (2.13) si nyní vyjádřím obraz V (s) a dosadím ho do rovnice (2.12), ze kterési vyjádřím obraz U(s).

V (s) = rkmI(s)s(J +mr2) + b

, (2.14)

U(s) = sLI(s) +RI(s) + ker

rkmI(s)s(J +mr2) + b

. (2.15)

Přenosová funkce ze vstupu U(s) na výstup Y (s) je rovna jejich podílu, její tvar tedyje

P (s) = V (s)U(s) =

rkmI(s)s(J+mr2)+b

sLI(s) +RI(s) + ke

rrkmI(s)

s(J+mr2)+b

=

= rKm

s2(JL+mr2L) + s(JR+mr2R+ LR) + kekm + bR.

(2.16)

9

2. Matematický model vozidla . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .2.3.2 Model pohonného ústrojí

Jak již bylo zmíněno výše, na stejnosměrné motory je v matematickém modelu vozidlanahlíženo jako na jeden motor. Dále všechny jednotlivé konstanty v přenosové funkcimodelu stejnosměrného motoru (2.16) nelze jednoduše identifikovat. Z těchto důvodůbudu hnací stejnosměrné motory modelovat prostřednictvím přenosové funkce se stej-ným řádem jmenovatele i čitatele, ale konstantami, které již nebudou mít fyzikálnícharakter.

2.4 Rovinný matematický model vozidlaPři matematickém popisu vozidla je nejprve nutné zvolit vhodný dynamický model.Rozlišují se dva základní modely, viz [6] – prostorový nebo rovinný. Model prostorovýzkoumá pohyb vozidla ve všech jeho třech osách, zatímco model rovinný popisuje pohybpouze v rovině, tedy ve dvou osách. V tomto konkrétním případě se tak jako nejvýhod-nější model jeví ten rovinný, neboť závodní model vozidla je nízký a pohybuje se pouzev rovině. Nedochází tak prakticky k téměř žádné rotaci kolem osy x nebo y a proto jejeho pohyb v ose z zanedbatelný.

Dynamiku vozidla budu vyšetřovat na jednostopém modelu, viz obrázek 2.4, kdeH(t) je hnací síla produkovaná stejnosměrnými motory, v(t)) vektor rychlosti pohybutěžiště, Sr(t) a Sf (t) jsou boční reakční síly, β(t) je směrová úchylka těžiště vozidla, θ(t)je úhel natočení předních kol a Ψ(t) úhel stáčení vozidla vůči globálním souřadnémusystému.

x0

y0

x

y

v

H

Sr

Sf

θ

Ψβ

Obrázek 2.4. Jednostopý model vozidla.

2.4.1 Odvození dostředivého a tečného zrychleníPro složky rychlosti těžiště vozidla vůči globálnímu souřadnému systému platí

vx0(t) = v(t) cos(β(t) + Ψ(t)), (2.17)

vy0(t) = v(t) sin(β(t) + Ψ(t)). (2.18)

Rovnice (2.17) a (2.18) nyní zderivuji podle času a dostanu tak rovnice zrychlení těžištěvozidla na jednotlivých osách.

v̇x0(t) = v̇(t) cos(β(t) + Ψ(t))− v(t)(β̇(t) + Ψ̇(t)) sin(β(t) + Ψ(t)), (2.19)

v̇y0(t) = v̇(t) sin(β(t) + Ψ(t)) + v(t)(β̇(t) + ˙Ψ(t)) cos(β(t) + Ψ(t)). (2.20)

10

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.4 Rovinný matematický model vozidla

Pro velikost zrychlení těžiště vozidla a(t) dále platí, že

a(t) =√v̇2x0(t) + v̇2

y0(t) =

= ... =

=√v̇2[cos2(β + Ψ) + sin2(β + Ψ)] + v2(β̇ + Ψ̇)2[cos2(β + Ψ) + sin2(β + Ψ)] =

=√v̇2(t) + v2(t)(β̇(t) + Ψ̇(t))2.

(2.21)Podle [7] lze rovnici (2.21) vyjádřit pomocí dostředivého a tečného zrychlení jako

a(t) =√a2t (t) + a2

d(t), (2.22)

je tedy patrné, že pro tečné zrychlení at(t) a dostředivé zrychlení ad(t) platí, že

at(t) = v̇(t), (2.23)

ad(t) = v(t)(β̇(t) + Ψ̇(t)). (2.24)

Pro názornost jsou směry tečného a dostředivého zrychlení zakresleny do obrázku 2.5.

x0

y0

x

y

H

Sr

Sf

θ

Ψβ

β

atad

Obrázek 2.5. Působení tečného a dostředivého zrychlení na jednostopý model.

2.4.2 Sestavení pohybových rovnicNyní mohu za znalosti známého vzorce F(t) = ma(t) sestavit rovnice popisující silovépůsobení v jednotlivých osách vozidla o hmotnosti m. Vratné momenty pneumatik atření v tomto případě zanedbávám, pro síly působící v osách vozidla x, y tak platí

x : H(t)− Sf (t) sin θ(t) = mv̇(t) cosβ(t)−mv(t)(β̇(t) + Ψ̇(t)) sin β(t), (2.25)

y : Sr(t) + Sf (t) cos θ(t) = mv̇(t) sin β(t) +mv(t)(β̇(t) + Ψ̇(t)) cosβ(t). (2.26)

Pro hnací sílu H(t), kterou produkují stejnosměrné motory, platí vztah

H(t) = mrω̇(t) (2.27)

kde r je poloměr kol a ω(t) je jejich úhlová rychlost. Pro boční reakční sílu působící napřední kola Sf (t) a zadní kola Sr(t) platí podle [8], za předpokladu lineárního modelupneumatiky, vztahy

11

2. Matematický model vozidla . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Sf (t) = Cfαf (t), (2.28)

Sr(t) = Crαr(t), (2.29)

kde Cf a Cr jsou směrové tuhosti pneumatik a αf (t) a αr(t) jsou směrové úchylkynáprav. Dále pro tyto směrové úchylky náprav platí vztahy

αf (t) = θ(t)− β(t)− lfv(t) Ψ̇(t), (2.30)

αr(t) = β(t)− lrv(t) Ψ̇(t), (2.31)

kde lf značí vzdálenost od těžiště vozidla ke středu předního kola a lr značí vzdálenostod těžiště vozidla ke středu zadního kola.

2.4.3 Linearizace pohybových rovnicV rovnicích (2.25) a (2.26) se vyskytují nelineární funkce sinus a cosinus, které mají jakoparametr směrovou úchylku vozidla β(t) nebo úhel natočení kol θ(t). Za předpokladumalých hodnot těchto úhlů pak mohu tyto funkce nahradit jako

sin β(t) ≈ sin θ(t) ≈ 0, (2.32)

cosβ(t) ≈ cos θ(t) ≈ 1. (2.33)

Rovnice (2.25) a (2.26) se tak po dosazení změní na lineární tvar

x : mrω̇(t) = mv̇(t), (2.34)

y : Sr(t) + Sf (t) = mv(t)(β̇(t) + Ψ̇(t)). (2.35)

12

Kapitola 3Identifikace parametrů matematického mo-delu

3.1 Nástroje použité k identifikaciIdentifikace byla prováděna na modelu vozidla s vyměněnou řídící elektronikou, vývo-jová deska Quorivva MPC5604B tak byla nahrazena deskou Kinetis K60, která dispo-nuje 3-osým akcelerometrem MMA8451Q. Vše ostatní bylo zachováno a změnu hmot-nosti vozidla vzhledem k výměně řídící elektroniky zanedbám, protože se jejich hmot-nosti liší o 20 gramů.

Díky údajům z akcelerometru tak lze vyhodnotit průběh zrychlení závodního modeluvozidla v osách x a y. Jelikož se tato identifikace bude vztahovat na linearizovanýmatematický model, zrychlení na ose x bude rovno tečnému zrychlení a zrychlení naose y bude rovno dostředivému zrychlení. Pro integraci získaného diskrétního signáluz akcelerometru jsem si napsal funkci, která realizuje integraci metodou lichoběžníků,viz [9].

K samotné aproximaci naměřených průběhů veličin vhodnou přenosovou funkcí jepoužit System Identification Toolbox v programu MATLAB.

3.1.1 Filtrování signálu z akcelerometruNaměřené údaje z akcelerometru je potřeba filtrovat. Ukázka, jak takový nefiltrovanývýstup akcelerometru vypadá, je pro případ jednorozměrného pohybu – zrychlení v osevozidla x na obrázku 3.1.

0 300 600 900 1200 1500 1800 2100 2400 2700−1000

−500

0

500

1000

Time [ms]

Acc

eler

atio

n [m

g]

Obrázek 3.1. Nefiltrovaný výstup z akcelerometru.

13

3. Identifikace parametrů matematického modelu . . . . . . . . . . . . . . . . . . . . . . . . . . . . .K vyhlazení takového průběhu jsem vybral filtr používající klouzavý průměr, jehož

funkce je popsána například v [10]. Parametrem filtru s klouzavým průměrem je po-čet vzorků, ze kterých se dělá průměr. Tento parametr se také často nazývá „velikostokna“. Tento typ filtru dosahuje poměrně dobrých výsledků, na obrázku 3.2 je porov-nání nefiltrovaného výstupu akcelerometru s výstupem filtrovaným, použitá velikostokna klouzavého průměru je v tomto případě 16.

0 200 400 600 800 1000 1200 1400 1600 1800 2000 2200−1000

−500

0

500

1000

Time [ms]

Acc

eler

atio

n [m

g]

NefiltrovanyFiltrovany

Obrázek 3.2. Porovnání nefiltrovaného a filtrovaného výstupu z akcelerometru.

3.2 Identifikace modelu pro pohyb v ose xIdentifikaci modelu pro pohyb v ose x budu provádět při jednorozměrném pohybuvozidla – akcelerace pouze v ose vozidla x. Obvodové zrychlení kol je tak podle rovnice(2.25) rovno zrychlení vozidla v jeho ose x. Linearizovaný model s touto rovností počítái pro nenulové natočení kol, viz rovnice (2.34). Pro jednotlivé rychlosti platí totéž.

Pro různé střídy vstupního PWM signálu jsem změřil průběhy zrychlení, které jsemnásledně zintegroval a obdržel tak průběhy rychlosti. Závislost těchto průběhů na vstup-ním PWM signálu jsou na obrázku 3.3.

0 500 1000 1500 2000 2500 3000−0.2

0

0.2

0.4

0.6

0.8

1

1.2

Time [ms]

Spe

ed [m

s−1 ]

50:5045:5540:60

Obrázek 3.3. Závislost průběhu rychlosti na střídě vstupního PWM signálu.

14

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.3 Úprava modelu pro pohyb v ose y

Střída Rychlost [m s−1]40:60 0.9545:55 1.01650:50 1.08

Tabulka 3.1. Ustálené hodnoty rychlosti v ose x v závislosti na střídě.

V tabulce 3.1 jsou zapsány přibližné ustálené hodnoty rychlosti v závislosti na stříděvstupního PWM signálu.

Pohon je modelován přenosovou funkcí vstupního napětí na obvodovou rychlost kolve tvaru

P (s) = rKm

s2(JL+mr2L) + s(JR+mr2R+ LR) + kekm + bR, (3.1)

jedná se tedy o přenos 2.řádu bez nuly. V System Identification Toolboxu v programuMATLAB tedy vytvořím přenosovou funkci stejných parametrů a z průběhu rychlostipro střídu 45:55 PWM signálu identifikuji konstanty aproximující přenosové funkce,přičemž jako referenční vstup budu uvažovat rychlost. Vstupním signálem tedy budeskok o hodnotě 1,016. Přenosová funkce získaná identifikací má tak tvar (3.2) a jejíodezva na skok 1,016 je na obrázku 3.4.

P (s) = 63, 96s2 + 47, 22 s+ 63, 96 . (3.2)

0 0.5 1 1.5 2 2.5 30

0.2

0.4

0.6

0.8

1

1.2

Time [s]

Spe

ed [m

s−1 ]

puvodniidentifikovany

Obrázek 3.4. Porovnání odezvy původního systému a jeho modelu na skok rychlosti 1,016.

3.3 Úprava modelu pro pohyb v ose y

Pro pohyb v ose y platí rovnice (2.26) a její linearizovaný tvar (2.35) ze kterého plyne,že boční zrychlení je závislé na konstantách směrové tuhosti pneumatik Cf a Cr ana směrových úchylkách náprav αf (t) αr(t). Tyto směrové úchylky náprav jsou závislékromě vzdálenosti od těžiště vozidla k přední nápravě lf a zadní nápravě lr také na pro-měnných parametrech. Jmenovitě rychlosti vozidla v(t) úhlu natočení kol θ(t), stáčivérychlosti Ψ̇(t) a směrové úchylky β(t).

15

3. Identifikace parametrů matematického modelu . . . . . . . . . . . . . . . . . . . . . . . . . . . . .Problémem je, že přímo řízenými parametry jsou pouze v(t) a θ(t). Rovnici tedy

přepíšu do jiného tvaru, ze kterého se lépe vyjádří boční zrychlení v závislosti na přímořízených parametrech. Předpokladem je malá změna rychlosti v průjezdu zatáčkami amalé skoky změny směru, potom podle [7] lze napsat vztah pro dostředivé zrychlení

ad = v2

R, (3.3)

kde v je obvodová rychlost a R je poloměr zatáčení. Dosazením vztahu (2.3) za Rdostanu

ad(t) = v2 tan θ(t)l

. (3.4)

Pro malé úhly natočení kol mohu tuto rovnici linearizovat nahrazením

tan θ(t) ≈ θ(t).Dále se v rovnici pro dostředivé zrychlení vyskytuje nelineární člen v2, který aproximujiTaylorovým polynomem 1.řádu, viz [11], pro pracovní bod rychlosti 1 m s−1

v2 ≈ 1 + 2(v − 1) = 2v − 1.Nová, linearizovaná rovnice sil v ose vozidla y, která vznikla nahrazením bočních sil Sra Sf , je tak

m(2v(t)− 1)θ(t)l

= mv(t)(β̇(t) + Ψ̇(t)). (3.5)

3.4 Identifikace modelu pro pohyb v ose y

Pro konstantní rychlost 1 m s−1 jsem změřil odezvu v podobě průběhu dostředivéhozrychlení na různé skoky referenčního úhlu natočení předních kol θ. Tyto odezvy jsouna obrázku 3.5. Přerušovanou čarou jsou k jednotlivým průběhům vyznačeny jejichpřibližné ustálené hodnoty spočtené pomocí mediánu. Tyto hodnoty jsou zapsány v ta-bulce 3.2. Z hodnot je patrné, že ustálená hodnota je téměř přímo úměrná referenčnímuúhlu natočení.

0 500 1000 1500 2000 2500 3000−0.2

0

0.2

0.4

0.6

0.8

1

1.2

1.4

Time [ms]

Acc

elar

atio

n [m

s−2 ]

θ = 20°

θ = 15°

θ = 10°

Obrázek 3.5. Závislost průběhu bočního zrychlení na skoku referenčního úhlu θref .

16

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.4 Identifikace modelu pro pohyb v ose y

θref [◦] Zrychlení [m s−2]10 0.87715 1.16320 1.419

Tabulka 3.2. Ustálené hodnoty bočního zrychlení v závislosti na referenčním úhlu natočeníkol θref .

Identifikace na základě údajů zrychlení z akcelerometru je v tomto případě praktickynemožná, neboť se jedná o změny poměrně rychlé a filtrováním se získané průběhyzpožďují. Na obrázku 3.6 je průběh zrychlení na skok θref = 10◦ ve větším detailu a smenší filtrací. Je z něj patrné že ho s dostatečnou přesností lze aproximovat přenosovoufunkcí 1. řádu, která odpovídá ideálnímu polohovému servomechanismu, z rovnice (3.5)lze také vyčíst, že by si řády přenosů pro takto malé skokové změny měl být rovny.S ohledem na rychlost 60◦/0.2 s udávanou výrobcem by měla být doba ustálení přiskoku na 10◦ přibližně 33 ms. Dále platí závislost úhlů hřídele servomotoru a úhlunatočení kol popsaná rovnicí (2.4). Doba ustálení by tak měla být ještě kratší, vzhledemk vůli uchycení předních kol se však odezva zpožďuje. V praxi se také často zpožďujei samotná odezva polohových servomechanismů. Přenos (2.5) tak pomocí konstantyrychlosti naladím na dobu ustálení 250 ms, která by přibližně odpovídala i získanýmprůběhům z akcelerometru. Pro výchylku θ = 10◦, rychlost 1 ms−1 si ještě ověřímplatnost rovnice (2.35) v ustáleném stavu.

(2v(t)− 1)θ(t)l

= v(t)(β̇(t) + Ψ̇(t)),0, 1745

0.2 = 0.8725.(3.6)

Porovnáním s odpovídající hodnotou v tabulce 3.2 lze usoudit, že rovnice je pro maléúhly θ dostatečně přesná.

0 200 400 600 800 1000 12000

0.2

0.4

0.6

0.8

1

1.2

Time [ms]

Acc

eler

etio

n [m

s−2 ]

Obrázek 3.6. Méně filtrovaný detail průběhu zrychlení na skok θref = 10◦.

17

Kapitola 4Zpracování signálu z obrazového senzoru

4.1 Nastavení zorného pole a zaostřeníObrazový senzor je pevně přichycen k šasi vozidla, proto vždy hledí do konstantnívzdálenosti před vozidlo a získávaný „obrazový řádek“ je rovnoběžný s osou vozidla y.Vyvstává tak problém správného nastavení vzdálenosti, do které senzor hledí. Pokud bybyla vzdálenost příliš malá, informace o příchozí zatáčce by byla získávána vzhledemk rychlosti vozidla pozdě a řídící algoritmus, který je omezen dynamikou jednotlivýchřízených prvků, by tak nemusel včasně přizpůsobit požadované parametry a mohlo bydojít k opuštění závodní tratě. S příliš velkou vzdáleností by naopak získával informacio příchozí zatáčce s předstihem, nicméně i v méně ostrých zatáčkách by vzhledem kpoměrně malému úhlu záběru obrazu dlouho hleděl mimo trať a řídící algoritmus bytak neměl informaci o tvaru tratě před vozidlem a v případě rychlých změn za zatáčkouby opět nestíhal včasně přizpůsobit řízené parametry. Dohled senzoru tak byl nastavenaž při samotném testování na trati, kde byl nalezen kompromis mezi požadovanoudynamikou vozidla a reálnými možnosti akčních členů. Nastavený dohled senzoru ašířka získávaného obrazu jsou zakótovány na obrázku 4.1. Vzdálenost mezi sousednímipixely je vzhledem k obrazu 2,52 mm.

230

320

Obrázek 4.1. Dohled obrazového senzoru a šířka obrazu.

S pevně nastaveným dohledem kamery je již možné nastavit ostření. Jeho vliv nazískaný obraz je demonstrován na obrázku 4.2. Pro účely co nejpřesnější detekce pozicečáry je tedy potřeba zaostřit tak, aby změna jasu získaného obrazu byla mezi bílýmpodkladem a černou čárou co nejstrmější. Vedlejším efektem detailního zaostření je vtomto případě nízká intenzita krajních pixelů, z tohoto důvodu bude 5 prvních a 5

18

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 Nastavení expozičního času

0 20 40 60 80 100 1200

20

40

60

80

100

120

Pixel

Brig

htne

ss

DalekoBlizkoIdealne

Obrázek 4.2. Vliv ostření na získaný obraz.

posledních pixelů ze zpracovávaného obrazu vyloučeno, celkově tak dojde ke ztrátě 12,5mm šířky záběru z každé strany.

4.2 Nastavení expozičního časuSpolehlivá detekce vodící čáry na bílém podkladu je závislá na dostatečném odstupuhodnot jasu, který je také ovlivňován intenzitou okolního osvětlení. Pro různá prostředítak musí být tento faktor vyvažován nastavením správného expozičního času, kterýovlivňuje celkovou světlost získaného obrazu ze senzoru. Jeho vliv při konstantní inten-zitě osvětlení je demonstrován na obrázku 4.3. Pro účely detekce čáry musí být nastaventak, aby byl obraz černé vodicí čáry jasně rozlišitelný od podkladu a zároveň byl tentočas podle možností co nejkratší, což ve výsledku umožňuje získávat informaci o tvarutratě s větší frekvencí a tak řídící systém může plynuleji řídit akční členy.

0 20 40 60 80 100 1200

50

100

150

200

250

Pixel

Brig

htne

ss

PreexponovanyPodexponovanyIdealni

Obrázek 4.3. Vliv expozičního času na získaný obraz.

19

4. Zpracování signálu z obrazového senzoru . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .4.3 Detekce hran

K detekci hran v jednorozměrném obrazu ze senzoru se podle [12] často používají de-tektory založené na hledání maxim prvních derivací dané obrazové funkce. Protože jezískaný obraz diskrétní funkcí, podle [12] tak lze derivaci nahradit konvolucí jejíž sy-metrický tvar je

f ′ ≈ [−1, 0, 1] ∗ f. (4.1)

Výsledné zvýraznění hran v obraze pořízeného senzorem za použití symetrického tvarukonvoluce je na obrázku 4.4. Je patrné, že použití konvoluce k detekci hran dosahujev tomto případě velice dobrých výsledků a proto bude implementována do samotnéhoalgoritmu detekce pozice vodící čáry.

0 20 40 60 80 100 1200

30

60

90

120

Pixel

Brig

htne

ss

0 20 40 60 80 100 120−60

−30

0

30

60

Pixel

Con

volu

tion

Obrázek 4.4. Zvýraznění hran obrazu pomocí konvoluce.

4.4 Algoritmus detekce pozice vodící čáryZe signálu zpracovaného pomocí konvoluce na obrázku 4.4 lze jednoznačně určit pří-znaky, kterými se vyznačuje vodící čára v obraze. Je to posloupnost globálního minimaa následně globálního maxima, které mají vzhledem k šířce vodící čáry jasně danouminimální vzdálenost v ose pixelů. Přestože se na trati mohou vyskytovat odlesky odokolního osvětlení nebo stíny, během testování se ukázalo, že tyto parazitní jasové změnynedosahují takové strmosti, že by hodnota konvoluce v daném místě vytvořila globálníminimum nebo maximum. Vzhledem k požadované dynamice vozidla se však v ostrýchzatáčkách stávalo, že obrazový senzor ztrácel dohled z vodící čáry. Proto byly běhemtestování nastaveny podmínky, které musí být pro detekci čáry splněny:.Absolutní hodnota globálních extrémů musí být větší než 25..Vzdálenost globálních extrémů na ose pixelů musí být větší než 11 pixelů (ekvivalent

2.52 cm) a musí být menší než 28 pixelů (ekvivalent 7 cm).

Minimální požadavek na vzdálenost extrémů vyplývá ze samotné šířky vodící čáry.Maximální požadavek na vzdálenost byl stanoven na základě testů na trati, neboť šířkastopy vodící čáry v obraze je závislá na úhlu, pod kterým se vůči automobilu v místěpořízení obrazu nachází, to je patrné například z obrázku 2.1, dále na expozičním

20

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.4 Algoritmus detekce pozice vodící čáry

Konvoluce NajdiSnejmenšíShodnotu.

Ano

Ne

NajdiSnejvyššíShodnotu.SAno

Ne

VypočtiSstřed

JeSmenšíSnežSé25?

JeSvětšíSnežS25?

SplňujeSvzdálenostSodSminima>S11SpixelůSaS<S28SpixelůS?

Obraz

InformujSřídícíSsystém

Ano

Ne

Obrázek 4.5. Diagram algoritmu detekce vodící čáry.

čase obrazového senzoru a rychlosti vozidla, protože během expozice se vozidlo stálepohybuje, výsledná stopa čáry v obraze se tak „rozmazává“.

Algoritmus detekce vodící čáry je popsán na diagramu 4.5. Označím-li pozici glo-bálního minima na ose pixelů Gmin a maximum Gmax, výpočet pozice jejího středuvzhledem k ose vozidla y se pak vypočítá jako

y = (Gmin − 64, 5 + Gmax −Gmin

2 ) · 2, 52 [mm]. (4.2)

Tato hodnota y pak odpovídá délce b na obrázku 2.1, která se používá k výpočtupoloměru zatáčky.

21

Kapitola 5Návrh a implementace řízení vozidla

5.1 Zpětná vazba rychlostiK získávání aktuálního stavu řízené veličiny je požadována zpětná vazba. V případěřízení poloměru zatáčení vozidla je touto zpětnou vazbou poloměr zatáčky vypočítá-vaný z obrazu pořízeného obrazovým senzorem. V případě řízení obvodové rychlosti kolje zpětná vazba získávána prostřednictvím elektrického proudu tekoucího motorem. H-můstek umožňuje získávat tuto informaci skrze analogový výstup FB, jehož výstupemje proudová hodnota lineárně závislá na protékajícím proudu. Ten je následně AD pře-vodníkem převeden na digitální, jehož hodnota již nemá fyzikální rozměr, nicméně jepřímo úměrná proudu protékajícímu motorem. Závislost obvodové rychlosti na výstupuFB (normalizováno) při referenci na rychlost 1 m s−1 je znázorněna na obrázku 5.1.

0 500 1000 1500 2000 2500 30000

0.2

0.4

0.6

0.8

1

1.2

Time [ms]

Spe

ed [m

s−1 ];

FB

[−/3

00]

Obrázek 5.1. Dohled obrazového senzoru a šířka obrazu.

Podle rovnice (2.6), lze v ustáleném stavu předpokládat lineární závislost protékají-cího proudu na obvodové rychlosti kol. V tabulce 5.1 jsou zaznamenány hodnoty vý-stupu FB při dalších měřených ustálených rychlostech.

Rychlost[m s−1] FB [-]0.95 1271.01 1401.08 155

Tabulka 5.1. Závislost výstupu FB při ustálených rychlostech.

22

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2 Model vozidla v prostředí MATLAB/Simulink

Pro přibližné hodnoty FB v závislosti na ustálené rychlosti lze tedy napsat vzorec

FB ≈ 127 + (v − 0.95) · 215.Pro odhad aktuální rychlosti jsem si napsal algoritmus, který počítá její odhad na

základě diskrétní integrace průběžných hodnot FB zmenšených o hodnotu FB příslu-šející požadované referenční rychlosti. V případě zpomalení dochází k odečítání, přizrychlení k přičítání. Při dosažení referenční rychlosti, se odhadovaná rychlost prohlásíza referenční, aby se případná chyba v čase nezvětšovala. Jak tento algoritmus fungujeje demonstrováno na obrázku 5.2.

0 500 1000 1500 2000 2500 3000−0.2

0

0.2

0.4

0.6

0.8

1

1.2

Time [ms]

Spe

ed [m

s−1 ]

odhadnutyrealny

Obrázek 5.2. Reálný a vypočtený průběh rychlosti.

5.2 Model vozidla v prostředí MATLAB/SimulinkV prostředí programu MATLAB/Simulink jsem vytvořil model, který simuluje dyna-miku polohového servomotoru a hnacích motorů. Jeho schéma se zavedenými zpětnýmivazbami a příslušnými regulátory je na obrázku 5.3.

Obrázek 5.3. Simulinkový model vozidla.

Vstupem do modelu polohového servomechanismu je θref , který je roven požado-vanému úhlu natočení předních kol, výstupem pak úhel θ, který je roven aktuálnímunatočení kol. Vstupem do modelu hnacích motorů je rychlost vref , tedy požadovanáobvodová rychlost kol a výstupem aktuální obvodová rychlost kol v.

23

5. Návrh a implementace řízení vozidla . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .5.3 Návrh a implementace regulátorů

Od regulátorů je požadováno, aby řízenou veličinu dokázaly v co nejkratším čase a sne příliš velkým překmitem dostat na požadovanou referenční hodnotu. Zároveň všakmusí regulátory respektovat meze akčního zásahu, tedy v konkrétním případě se jednáo limitovaný rozsah úhlu natočení předních kol a rozsah střídy PWM signálu od 0:100do 100:0 (používán je přepočet střídy PWM signálu na ustálenou hodnotu obvodovérychlosti).

Pomocí nástroje PID tuning tool v Simulinku jsem navrhl regulátory pro řízení rych-losti a úhlu natočení kol. Typ regulátoru jsem pro oba řízené podsystémy zvolil PI, neboťpožaduji nulovou ustálenou odchylku. Složka D neměla během testování příliš velký vlivna výslednou odezvu, neboť při návrhu byly respektovány meze akčních zásahů a protoani nedocházelo k příliš velkému kmitání, které se složka D snaží potlačovat. Protožeúhel natočení kol θ lze měnit jednou za 20 ms, frekvence PWM signálu řídícího polo-hový servomotor je 50 Hz, a obvodovou rychlost lze měnit jednou za 0,09 ms, frekvencePWM signálu řídící hnací motory je 11 KHz, regulátory byly navrhovány v diskrétníverzi, proporciálně - integrační regulátor (PI) se tak mění na proporcionálně - sumační(PS) se vzorkovací periodou Ts, který má tvar

P + S Ts1

z − 1 .

Pro řízení úhlu natočení předních kol tak byl naladěn regulátor se složkami

P = 1, 104 ,S = 25.288 .

Výsledek řízení tímto regulátorem je na obrázku 5.4.

0 0.5 1 1.5 2 2.5 3−10

−5

0

5

10

15

20

25

30

35

Time [s]

Ang

le [°

]

Zasah regulatoruθ

ref

θ

Obrázek 5.4. Řízení úhlu natočení předních kol θ.

Pro řízení obvodové rychlosti kol byl naladěn regulátor se složkami

P = 1, 278 ,S = 4.719 .

24

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3 Návrh a implementace regulátorů

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

Time [s]

Spe

ed [m

s−1 ]

vv

ref

Zasah regulatoru

Obrázek 5.5. Řízení obvodové rychlosti hnacích kol.

Výsledek řízení tímto regulátorem je na obrázku 5.5.Navržené PS regulátory jsou prostřednictvím kódu v jazyce C implementovány do

řídícího algoritmu. Vždy vycházejí ze znalosti aktuální a předchozí hodnoty odchylkyod referenční hodnoty řízené veličiny. Požadavek na referenční rychlost je generovánna základě údaje o tvaru tratě, v zatáčkách je tak vždy reference na rychlost menší,než v případě rovinky. Akční členy jsou řízeny prostřednictvím PWM signálu, protojsou implementovány funkce, které se starají o přepočet úhlu natočení kol a obvodovourychlost kol na odpovídající střídu PWM signálů.

25

Kapitola 6Závěr

Podle očekávání se během testování na trati ukázalo, že reálný model vozidla na rozdílod matematického vykazuje různé druhy nelinearit. První z nich byla rozdílná ustálenáhodnota úhlu natočení předních kol θ při pohybu zprava nebo zleva, to způsobovalohlavně na rovinkách mírné kmitání kolem vodící čáry. Další zjištěnou nelinearitou bylamrtvá zóna polohového servomotoru při malých požadovaných změnách natočení, kterábyla ještě posílena vůlí uchycení předních kol.

Původní navržený regulátor pro zatáčení musel být mírně upraven, aby vykazoval„agresivnější“ chování a navíc byla zavedena různá opatření, kdy bylo řízení odebránonavrženému regulátoru a nastavila se pevná hodnota výstupu – například v případě delšírovinky, na jejímž konci vozidlo dosahuje vyšší rychlosti, se po překročení určitého prahuvstupního poloměru příchozí zatáčky nastaví úhel natočení předních kol na maximálnívýchylku, ve které setrvá, dokud nedojde k detekci vodící čáry kolem středové polohy.Stejně se postupuje i v případě, že není v obraze detekována vodící čára – nastaví semaximální možná výchylka ve směru posledního výskytu vodící čáry.

Částečným řešením problému vyjíždění ze závodní dráhy se zachováním dostatečnérychlosti by bylo zavedení aktivního brzdění. To je možné realizovat za podmínkyúpravy H-můstku, konkrétně odpájení nožičky řídícího výstupu IN2, viz obrázek 1.11,který je standardně připojen na společnou zem. Jelikož se však jedná o poměrně ris-kantní úpravu, vzhledem k dispozici pouze jedním H-můstkem jsem tuto úpravu nepro-vedl. Jako nejlepší řešení se tak ukázalo kromě rozšíření řídícího algoritmu o speciálnístavy také celkové snížení rychlosti vozidla.

26

Literatura[1] Freescale Semiconductor, Inc. The Freescale Cup 2012-2013 Season Rules, 2012.https://community.freescale.com/docs/DOC-93225.

[2] TAOS Inc. Datasheet of tst1401r-lf, May 2006.http://www.farnell.com/datasheets/315815.pdf.

[3] Freescale Semiconductor, Inc. Product pages of MPC560xB, April 2013.http://www.freescale.com/webapp/sps/site/prod_summary.jsp?code=MPC560xB.

[4] Freescale Semiconductor, Inc. Fact sheet of H-Bridge MC33931, 2012.http://cache.freescale.com/files/analog/doc/fact_sheet/MC33931FS.pdf.

[5] Souček Pavel. Článek v časopise Automa 2008/5.http://www.odbornecasopisy.cz/download/au050351.pdf.

[6] Klečka Radim. Článek v časopise Automa 2008/4.http://www.odbornecasopisy.cz/res/pdf/37011.pdf.

[7] Bednařík Michal. Fyzika 1. Nakladatelství ČVUT v Praze, 1. edition, 2011.[8] Brabec Pavel a Voženílek Robert. Jednostopý a dvoustopý model vozidla, 2002.http://www3.fs.cvut.cz/web/fileadmin/documents/12241-BOZEK/publikace/2002/.

[9] Hasík Karel. Numerické metody.http://www.slu.cz/math/cz/knihovna/ucebni-texty/Numericke-metody/Numericke-

metody.pdf.[10] Wikipedia: Moving average.

https://en.wikipedia.org/wiki/Moving_average.[11] Průcha Ladislav. Řady. Nakladatelství ČVUT v Praze, 2005.[12] Hlaváč Václav a Sedláček Miloš. Zpracování signálů a obrazů. Nakladatelství

ČVUT v Praze, 3. edition, 2009.

27

29

A Zadani práce . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Příloha AZadani práce

30

Příloha BObsah CD

Na CD jsou umístěny adresáře s následujícím obsahem:.Kod MATLAB. Funkce pro výpočet integrace a zrychlení.. Funkce pro filtrování a vykreslování naměřených dat.. Naměřená data..Kod C. Projekt ve vývojovém prostředí CodeWarrior.. Zdrojové kódy v jazyce C..Simulink. Simulinkový model vozidla..Datasheety. Data sheety a fact sheety k jednotlivým použitým komponentám..Prace. Elektronická verze této práce ve formátu PDF.

31