Bakalářská práce
České vysoké učení technické v Praze
F3
Fakulta elektrotechnická Katedra kybernetiky
Návrh a implementace řídícího algoritmu pro model závodního vozidla Michal Mrosko
Květen 2013 Vedoucí 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é ruce při mechanických úpravách závodního modelu, partnerce Šárce za neustálou podporu a příliv pozitivní energie a společnosti Freescale Semiconductor za pořádání soutěže.
Prohlašuji, že jsem předloženou práci vypracoval samostatně a že jsem uvedl veškeré použité informační zdroje v souladu s Metodickým pokynem o dodržování etických principů při přípravě vysokoškolských závěrečných prací. V Praze dne 24. 5. 2013
........................................
iii
Abstrakt / Abstract Tato bakalářská práce popisuje vývoj a implementaci řídícího algoritmu modelu závodního vozidla v rámci technické soutěže Freescale Cup. Závodní dráha, vymezena bílým podkladem s černou vodící čárou, je snímána jednořádkovou kamerou umístěnou na vozidle, jejíž obraz je následně zpracováván za účelem detekce tvaru dráhy a na základě této informace se uzpůsobuje natočení předních kol řízené polohovým servomechanismem a rychlost otáčení hnacích stejnosměrných motorů. Pro návrh řídícího algoritmu byl v programu MATLAB/Simulink vytvořen zjednodušený matematický model vozidla. Algoritmus napsaný v jazyce C byl poté implementován do řídící jednotky a jeho výsledky zhodnoceny na základě chování matematického a reálného modelu vozidla.
This bachelor thesis describes the development and implementation of the control algorithm of racing car model in the technical competition Freescale Cup. Racetrack, defined by white background with black guide line, is scanned by one-line camera placed on the vehicle whose image is then processed to detect the shape of the track and on the basis of this information it adapts the front wheels angle controlled by a positional servomechanism and the speed of the rotation of driving motors. For the design of the control algorithm was created a simplified mathematical model of the vehicle in program MATLAB/Simulink . The algorithm written in C language was then implemented to the control unit and its results were evaluated on the basis of behavior of mathematical and real vehicle model.
iv
Obsah / 1 Úvod . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .1 1.1 The Freescale Cup . . . . . . . . . . . . . . .1 1.2 Závodní trať . . . . . . . . . . . . . . . . . . . . . .2 1.3 Závodní vozidlo . . . . . . . . . . . . . . . . . .2 1.3.1 Šasi . . . . . . . . . . . . . . . . . . . . . . . . .2 1.3.2 Obrazový senzor . . . . . . . . . . .3 1.3.3 Polohový servomotor . . . . . .3 1.3.4 Hnací motory . . . . . . . . . . . . . .4 1.3.5 Řídící elektronika . . . . . . . . . .5 1.3.6 Výkonová elektronika . . . . . .5 2 Matematický model vozidla . . . . . . .7 2.1 Geometrický model zatáčení vozidla . . . . . . . . . . . . . . . . . . . . . . . . . . . .7 2.2 Servořízení . . . . . . . . . . . . . . . . . . . . . . .8 2.2.1 Model polohového servomotoru . . . . . . . . . . . . . . . . . . .8 2.3 Pohon. . . . . . . . . . . . . . . . . . . . . . . . . . . . .8 2.3.1 Úprava rovnic pro operátorový přenos . . . . . . . . . . . .9 2.3.2 Model pohonného ústrojí . . . . . . . . . . . . . . . . . . . . 10 2.4 Rovinný matematický model vozidla . . . . . . . . . . . . . . . . . . . . . . . . . . 10 2.4.1 Odvození dostředivého a tečného zrychlení . . . . . . 10 2.4.2 Sestavení pohybových rovnic . . . . . . . . . . . . . . . . . . . . . 11 2.4.3 Linearizace pohybových rovnic . . . . . . . . . . . . . . . 12 3 Identifikace parametrů matematického modelu. . . . . . . . . . . . . . . . 13 3.1 Nástroje použité k identifikaci . 13 3.1.1 Filtrování signálu z akcelerometru. . . . . . . . . . . . . . . 13 3.2 Identifikace modelu pro pohyb v ose x . . . . . . . . . . . . . . . . . . . . . 14 3.3 Úprava modelu pro pohyb v ose y . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15 3.4 Identifikace modelu pro pohyb v ose y . . . . . . . . . . . . . . . . . . . . . 16 4 Zpracování signálu z obrazového senzoru . . . . . . . . . . . . . . . . . . . . . . 18 4.1 Nastavení zorného pole a zaostření . . . . . . . . . . . . . . . . . . . . . . . . 18 4.2 Nastavení expozičního času. . . . 19 4.3 Detekce hran . . . . . . . . . . . . . . . . . . . 20
4.4 Algoritmus detekce pozice vodící čáry . . . . . . . . . . . . . . . . . . . . . 5 Návrh a implementace řízení vozidla . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.1 Zpětná vazba rychlosti. . . . . . . . . 5.2 Model vozidla v prostředí MATLAB/Simulink . . . . . . . . . . . . 5.3 Návrh a implementace regulátorů . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 Závěr . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Literatura . . . . . . . . . . . . . . . . . . . . . . . . . A Zadani práce . . . . . . . . . . . . . . . . . . . . . . B Obsah CD . . . . . . . . . . . . . . . . . . . . . . . . .
v
20 22 22 23 24 26 27 30 31
Kapitola Úvod 1.1
1
The Freescale Cup
Soutěž 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 na zá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žitě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ým kolem, čas nebude počítán. Pokud vozidlo po projetí cílem nezastaví do vzdálenosti 3 metrů od cílové čáry, bude k č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, vymezena bí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ého dílu se startovní/cílovou čárou, retardérů a kopců. Specifický cílový díl se zakótovanými vý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 1.3.1
Závodní vozidlo Š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řipevněna výkonová a řídící elektronika, NiMH akumulátor a obrazový senzor. 2
.........................................
160
24
200
1.3 Závodní vozidlo
50
280
Obrázek 1.4. Rozměry modelu vozidla – půdorys.
1.3.2
Obrazový senzor
300
Pro snímání polohy černé vodící čáry na dráze je použit CMOS obrazový senzor TSL1401CL od výrobce TAOS, který je umístěn na stojanu na předním nárazníku vozidla, viz obrázek 1.5. Protože má senzor pevnou ohniskovou vzdálenost, šířku záběru obrazu lze měnit pouze změnou výšky připevnění senzoru ke stojanu a nastavením vzdálenosti, do které hledí. Ostření obrazu se provádí ručně.
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ích 128 pulsech signálu CLK postupně nabíjejí jednotlivé kapacity vlivem dopadajícího svě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ý servomotor
Použ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řída urč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.
-90
1 ms
1,5 ms
0
2 ms
+90
Obrázek 1.7. Závislost úhlu natočení hřídele na střídě PWM.
1.3.4
Hnací motory
Zá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řídele převáděn na samostatně připevněná kola – každé kolo se tak může otáčet rozdílnou rychlostí. Motory jsou připojeny na H-můstek umístěný na výkonové desce, který se stará o jejich napájení a řízení rychlosti otáčení. Otáčky motoru jsou tak prostřednictvím Hmůstku regulovány PWM signálem. Pokud je střída PWM signálu rovna 100:0, motory se 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 potř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.5
1.3 Závodní vozidlo
Řídící elektronika
K řízení celého modelu vozidla je použit 32-bitový mikrokontrolér Quorivva MPC5604B od 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ázek 1.8, disponuje mikroprocesorem s taktovací frekvencí až 64 MHz postaveným na Power architektuř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í programu 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. Komunikační vstupně/výstupní systém je složen z jednotlivých funkčních jednotek, které jsou nezbytné pro většinu průmyslových nasazení a nebo jen usnadňují práci programátorovi tím, že dokáží jako podsystém pracovat samostatně, bez intervence procesoru. Zde vyjmenuji 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 na vývojové desce. Rozšířený modulární vstupně/výstupní systém (eMIOS) – použit ke generování PWM signálů řídících polohový servomotor a hnací motory. LINFlex – sériové komunikační rozhraní použité pro komunikaci s PC prostřednictvím UART (univerzální asynchronní přijímač/vysílač). Používán převážně pro účely ladění – např. zobrazení výstupního signálu obrazového senzoru.
1.3.6
Výkonová elektronika
Napá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í stejnosmě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 frekvenci 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 2 Matematický model vozidla 2.1
Geometrický model zatáčení vozidla
Pro boční řízení vozidla je požadován vstup v podobě poloměru zatáčky, který je vypočí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ý poloměr zatáčky. Přibližný z toho důvodu, protože vozidlo se nemusí v daný okamžik sejmutí obrazu nacházet ve středu zatáčky. Tuto možnou nejistotu výpočtu však bude kompenzovat robustní řídící algoritmus. Na obrázku 2.1 je zakreslena detekce pozice vodící čáry a důležité parametry, podle kterých se bude poloměr zatáčky počítat.
b α
a
Obrázek 2.1. Detekce pozice černé vodící čáry.
Pro úhel α platí vzorec b α = arcsin √ . 2 a + b2 Za použití Thaletovy věty vypočtu poloměr zatáčky R, pro který platí √ a2 + b2 R= . 2 cos( π2 − α)
(2.1)
(2.2)
Pro poloměr zatáčení vozidla Rv (t) si vystačím se zjednodušeným vzorcem Rv (t) =
l , tan θ(t)
kde l značí rozvor náprav a θ úhel natočení předních kol. 7
(2.3)
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ůsobuje natoč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ávislosti těchto úhlů lze zapsat rovnici θ(t) =
2.2.1
35 ϕ(t). 30
(2.4)
Model polohového servomotoru
Polohový 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, jeho rychlost 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 model na obrázku 2.3 nerespektuje, protože ten dosahuje požadované ustálené hodnoty úhlu natočení hřídele servomotoru ϕ(t) vždy za stejný čas. Protože však vozidlo nepojede v zatáčkách příliš velkou rychlostí a minimální poloměr zatáček 0,5 m je vzhledem k minimá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řenos naladěn pro dostačující reprezentativnost reálného polohového servomechanismu.
2.3
Pohon
Pohonné ú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řována na jednostopém modelu, bude na motory pohlíženo jako na jeden motor. Rovnice popisující dynamiku stejnosměrného motoru jsou 8
............................................. di(t) = −Ri(t) − ke ω(t) + u(t), dt
(2.6)
dω(t) = km i(t) − bω(t) − Mz (t), dt
(2.7)
L J
2.3 Pohon
kde L je indukčnost motoru, i(t) protékající elektrický proud, R elektrický odpor vedení motoru, Ke elektrická konstanta motoru, ω(t) úhlová rychlost hřídele motoru, u(t) vstupní napětí motoru, J moment setrvačnosti motoru, km mechanická konstanta motoru, 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řenos
V rovnicích (2.6) a (2.7) se vyskytuje úhlová rychlost ω(t), pro účely pozdější identifikace parametrů rovnic mě bude zajímat přenos ze vstupního napětí na obvodovou rychlost kol, 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
Mz (t) = mr
L
dv(t) , dt
di(t) ke = −Ri(t) − v(t) + u(t), dt r
J dv(t) b dv(t) = km i(t) − v(t) − mr . r dt r dt
(2.8) (2.9)
(2.10) (2.11)
Tyto upravené rovnice nyní převedu pomocí Laplaceovy transformace na operátorový tvar ke (2.12) sLI(s) = −RI(s) − V (s) + U (s), r J b s V (s) = km I(s) − V (s) − smrV (s). r r
(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). rkm I(s) V (s) = , (2.14) s(J + mr2 ) + b U (s) = sLI(s) + RI(s) +
ke rkm I(s) . r 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 tedy je rkm I(s)
P (s) =
V (s) s(J+mr2 )+b = = rkm I(s) U (s) sLI(s) + RI(s) + kre s(J+mr 2 )+b rKm = 2 . 2 s (JL + mr L) + s(JR + mr2 R + LR) + ke km + bR 9
(2.16)
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 vozidla nahlíženo jako na jeden motor. Dále všechny jednotlivé konstanty v přenosové funkci modelu 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 stejným řádem jmenovatele i čitatele, ale konstantami, které již nebudou mít fyzikální charakter.
2.4
Rovinný matematický model vozidla
Př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 pohyb pouze v rovině, tedy ve dvou osách. V tomto konkrétním případě se tak jako nejvýhodnější model jeví ten rovinný, neboť závodní model vozidla je nízký a pohybuje se pouze v rovině. Nedochází tak prakticky k téměř žádné rotaci kolem osy x nebo y a proto je jeho pohyb v ose z zanedbatelný. Dynamiku vozidla budu vyšetřovat na jednostopém modelu, viz obrázek 2.4, kde H(t) je hnací síla produkovaná stejnosměrnými motory, v(t)) vektor rychlosti pohybu těž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ému systému.
y
y0 θ x
v β
Ψ Sf x0
H Sr
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)
˙ cos(β(t) + Ψ(t)). ˙ v˙ y0 (t) = v(t) ˙ sin(β(t) + Ψ(t)) + v(t)(β(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) =
q
v˙ x20 (t) + v˙ y20 (t) =
= ... = q ˙ 2 [cos2 (β + Ψ) + sin2 (β + Ψ)] = = v˙ 2 [cos2 (β + Ψ) + sin2 (β + Ψ)] + v 2 (β˙ + Ψ) q 2. ˙ + Ψ(t)) ˙ = v˙ 2 (t) + v 2 (t)(β(t) (2.21) Podle [7] lze rovnici (2.21) vyjádřit pomocí dostředivého a tečného zrychlení jako q a(t) = a2t (t) + a2d (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)
˙ + Ψ(t)). ˙ ad (t) = v(t)(β(t)
(2.24)
Pro názornost jsou směry tečného a dostředivého zrychlení zakresleny do obrázku 2.5. y0
y
θ x
ad
at β
β
Ψ Sf x0
H Sr
Obrázek 2.5. Působení tečného a dostředivého zrychlení na jednostopý model.
2.4.2
Sestavení pohybových rovnic
Nyní 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 a tření v tomto případě zanedbávám, pro síly působící v osách vozidla x, y tak platí ˙ + Ψ(t)) ˙ x : H(t) − Sf (t) sin θ(t) = mv(t) ˙ cos β(t) − mv(t)(β(t) sin β(t),
(2.25)
˙ + Ψ(t)) ˙ y : Sr (t) + Sf (t) cos θ(t) = mv(t) ˙ sin β(t) + mv(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í na přední kola Sf (t) a zadní kola Sr (t) platí podle [8], za předpokladu lineárního modelu pneumatiky, 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é úchylky náprav. Dále pro tyto směrové úchylky náprav platí vztahy αf (t) = θ(t) − β(t) − αr (t) = β(t) −
lf ˙ Ψ(t), v(t)
lr ˙ Ψ(t), v(t)
(2.30) (2.31)
kde lf značí vzdálenost od těžiště vozidla ke středu předního kola a lr značí vzdálenost od těžiště vozidla ke středu zadního kola.
2.4.3
Linearizace pohybových rovnic
V rovnicích (2.25) a (2.26) se vyskytují nelineární funkce sinus a cosinus, které mají jako parametr směrovou úchylku vozidla β(t) nebo úhel natočení kol θ(t). Za předpokladu malý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)
˙ + Ψ(t)). ˙ y : Sr (t) + Sf (t) = mv(t)(β(t)
(2.35)
12
Kapitola 3 Identifikace parametrů matematického modelu
3.1
Nástroje použité k identifikaci
Identifikace byla prováděna na modelu vozidla s vyměněnou řídící elektronikou, vývojová deska Quorivva MPC5604B tak byla nahrazena deskou Kinetis K60, která disponuje 3-osým akcelerometrem MMA8451Q. Vše ostatní bylo zachováno a změnu hmotnosti vozidla vzhledem k výměně řídící elektroniky zanedbám, protože se jejich hmotnosti liší o 20 gramů. Díky údajům z akcelerometru tak lze vyhodnotit průběh zrychlení závodního modelu vozidla 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í na ose y bude rovno dostředivému zrychlení. Pro integraci získaného diskrétního signálu z 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í je použit System Identification Toolbox v programu MATLAB.
3.1.1
Filtrování signálu z akcelerometru
Naměř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 ose vozidla x na obrázku 3.1.
Acceleration [mg]
1000
500
0
−500
−1000 0
300
600
900
1200 1500 Time [ms]
1800
2100
2400
Obrázek 3.1. Nefiltrovaný výstup z akcelerometru.
13
2700
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á „velikost okna“. Tento typ filtru dosahuje poměrně dobrých výsledků, na obrázku 3.2 je porovnání nefiltrovaného výstupu akcelerometru s výstupem filtrovaným, použitá velikost okna klouzavého průměru je v tomto případě 16.
Nefiltrovany Filtrovany
Acceleration [mg]
1000
500
0
−500
−1000 0
200
400
600
800 1000 1200 1400 1600 1800 2000 2200 Time [ms]
Obrázek 3.2. Porovnání nefiltrovaného a filtrovaného výstupu z akcelerometru.
3.2
Identifikace modelu pro pohyb v ose x
Identifikaci modelu pro pohyb v ose x budu provádět při jednorozměrném pohybu vozidla – 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é jsem následně zintegroval a obdržel tak průběhy rychlosti. Závislost těchto průběhů na vstupním PWM signálu jsou na obrázku 3.3.
1.2 1
Speed [ms−1]
0.8 0.6 0.4 50:50 45:55 40:60
0.2 0 −0.2 0
500
1000
1500 Time [ms]
2000
2500
3000
Obrázek 3.3. Závislost průběhu rychlosti na střídě vstupního PWM signálu.
14
.................................. Střída
3.3 Úprava modelu pro pohyb v ose y
Rychlost [m s−1 ]
40:60 45:55 50:50
0.95 1.016 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 kol ve tvaru P (s) =
s2 (JL
+
mr2 L)
rKm , + s(JR + mr2 R + LR) + ke km + bR
(3.1)
jedná se tedy o přenos 2.řádu bez nuly. V System Identification Toolboxu v programu MATLAB tedy vytvořím přenosovou funkci stejných parametrů a z průběhu rychlosti pro 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 bude skok 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, 96 . s2 + 47, 22 s + 63, 96
(3.2)
1.2
Speed [ms−1]
1 0.8 0.6 0.4 puvodni identifikovany
0.2 0 0
0.5
1
1.5 Time [s]
2
2.5
3
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 a na 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 promě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 a malé skoky změny směru, potom podle [7] lze napsat vztah pro dostředivé zrychlení v2 , (3.3) R kde v je obvodová rychlost a R je poloměr zatáčení. Dosazením vztahu (2.3) za R dostanu ad =
tan θ(t) . l Pro malé úhly natočení kol mohu tuto rovnici linearizovat nahrazením ad (t) = v 2
(3.4)
tan θ(t) ≈ θ(t). Dále se v rovnici pro dostředivé zrychlení vyskytuje nelineární člen v 2 , který aproximuji Taylorovým polynomem 1.řádu, viz [11], pro pracovní bod rychlosti 1 m s−1 v 2 ≈ 1 + 2(v − 1) = 2v − 1. Nová, linearizovaná rovnice sil v ose vozidla y, která vznikla nahrazením bočních sil Sr a Sf , je tak m(2v(t) − 1)
3.4
θ(t) ˙ + Ψ(t)). ˙ = mv(t)(β(t) l
(3.5)
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ého zrychlení na různé skoky referenčního úhlu natočení předních kol θ. Tyto odezvy jsou na obrázku 3.5. Přerušovanou čarou jsou k jednotlivým průběhům vyznačeny jejich přibližné ustálené hodnoty spočtené pomocí mediánu. Tyto hodnoty jsou zapsány v tabulce 3.2. Z hodnot je patrné, že ustálená hodnota je téměř přímo úměrná referenčnímu úhlu natočení.
1.4 Accelaration [ms−2]
1.2 1 0.8 0.6 0.4
θ = 20°
0.2
θ = 15°
0
θ = 10°
−0.2 0
500
1000
1500 Time [ms]
2000
2500
3000
Obrázek 3.5. Závislost průběhu bočního zrychlení na skoku referenčního úhlu θref .
16
................................ θref [◦ ]
3.4 Identifikace modelu pro pohyb v ose y
Zrychlení [m s−2 ]
10 15 20
0.877 1.163 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ě prakticky nemožná, neboť se jedná o změny poměrně rychlé a filtrováním se získané průběhy zpožďují. Na obrázku 3.6 je průběh zrychlení na skok θref = 10◦ ve větším detailu a s menší filtrací. Je z něj patrné že ho s dostatečnou přesností lze aproximovat přenosovou funkcí 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ři skoku na 10◦ přibližně 33 ms. Dále platí závislost úhlů hřídele servomotoru a úhlu natočení kol popsaná rovnicí (2.4). Doba ustálení by tak měla být ještě kratší, vzhledem k vůli uchycení předních kol se však odezva zpožďuje. V praxi se také často zpožďuje i samotná odezva polohových servomechanismů. Přenos (2.5) tak pomocí konstanty rychlosti naladím na dobu ustálení 250 ms, která by přibližně odpovídala i získaným průběhům z akcelerometru. Pro výchylku θ = 10◦ , rychlost 1 m s−1 si ještě ověřím platnost rovnice (2.35) v ustáleném stavu. θ(t) ˙ + Ψ(t)), ˙ = v(t)(β(t) l (3.6) 0, 1745 = 0.8725. 0.2 Porovnáním s odpovídající hodnotou v tabulce 3.2 lze usoudit, že rovnice je pro malé úhly θ dostatečně přesná. (2v(t) − 1)
1.2
Acceleretion [ms−2]
1 0.8 0.6 0.4 0.2 0 0
200
400
600 Time [ms]
800
1000
1200
Obrázek 3.6. Méně filtrovaný detail průběhu zrychlení na skok θref = 10◦ .
17
Kapitola 4 Zpracová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 by byla vzdálenost příliš malá, informace o příchozí zatáčce by byla získávána vzhledem k 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 by dojít k opuštění závodní tratě. S příliš velkou vzdáleností by naopak získával informaci o příchozí zatáčce s předstihem, nicméně i v méně ostrých zatáčkách by vzhledem k poměrně malému úhlu záběru obrazu dlouho hleděl mimo trať a řídící algoritmus by tak neměl informaci o tvaru tratě před vozidlem a v případě rychlých změn za zatáčkou by opět nestíhal včasně přizpůsobit řízené parametry. Dohled senzoru tak byl nastaven až při samotném testování na trati, kde byl nalezen kompromis mezi požadovanou dynamikou 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ími pixely 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 na zí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ým podkladem a černou čárou co nejstrmější. Vedlejším efektem detailního zaostření je v tomto 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
120 Daleko Blizko Idealne
100
Brightness
80 60 40 20 0 0
20
40
60 Pixel
80
100
120
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,5 mm šířky záběru z každé strany.
4.2
Nastavení expozičního času
Spolehlivá detekce vodící čáry na bílém podkladu je závislá na dostatečném odstupu hodnot 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í intenzitě osvětlení je demonstrován na obrázku 4.3. Pro účely detekce čáry musí být nastaven tak, 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 tvaru tratě s větší frekvencí a tak řídící systém může plynuleji řídit akční členy.
250
Brightness
200 Preexponovany Podexponovany Idealni
150
100
50
0 0
20
40
60 Pixel
80
100
120
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í detektory založené na hledání maxim prvních derivací dané obrazové funkce. Protože je získaný obraz diskrétní funkcí, podle [12] tak lze derivaci nahradit konvolucí jejíž symetrický tvar je f 0 ≈ [−1, 0, 1] ∗ f.
(4.1)
Výsledné zvýraznění hran v obraze pořízeného senzorem za použití symetrického tvaru konvoluce je na obrázku 4.4. Je patrné, že použití konvoluce k detekci hran dosahuje v tomto případě velice dobrých výsledků a proto bude implementována do samotného algoritmu detekce pozice vodící čáry.
Brightness
120 90 60 30 0 0
20
40
60 Pixel
80
100
120
20
40
60 Pixel
80
100
120
Convolution
60 30 0 −30 −60 0
Obrázek 4.4. Zvýraznění hran obrazu pomocí konvoluce.
4.4
Algoritmus detekce pozice vodící čáry
Ze 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 minima a následně globálního maxima, které mají vzhledem k šířce vodící čáry jasně danou minimální vzdálenost v ose pixelů. Přestože se na trati mohou vyskytovat odlesky od okolního osvětlení nebo stíny, během testování se ukázalo, že tyto parazitní jasové změny nedosahují 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ých zatáčkách stávalo, že obrazový senzor ztrácel dohled z vodící čáry. Proto byly během testová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ť šířka stopy 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
................................ Obraz
4.4 Algoritmus detekce pozice vodící čáry
NajdiSnejmenšíShodnotu.
Konvoluce
Ano
NajdiSnejvyššíShodnotu.S
JeSmenšíSnežSé25? Ne
JeSvětšíSnežS25?
Ne
Ano
SplňujeSvzdálenostSodSminima >S11SpixelůSaS<S28SpixelůS?
Ne
InformujSřídícíSsystém
Ano
VypočtiSstřed
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ále pohybuje, 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 globálního minima na ose pixelů Gmin a maximum Gmax , výpočet pozice jejího středu vzhledem k ose vozidla y se pak vypočítá jako Gmax − Gmin ) · 2, 52 [mm]. (4.2) 2 Tato hodnota y pak odpovídá délce b na obrázku 2.1, která se používá k výpočtu poloměru zatáčky. y = (Gmin − 64, 5 +
21
Kapitola 5 Návrh a implementace řízení vozidla
5.1
Zpětná vazba rychlosti
K 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 kol je zpětná vazba získávána prostřednictvím elektrického proudu tekoucího motorem. Hmůstek umožňuje získávat tuto informaci skrze analogový výstup FB, jehož výstupem je proudová hodnota lineárně závislá na protékajícím proudu. Ten je následně AD převodníkem převeden na digitální, jehož hodnota již nemá fyzikální rozměr, nicméně je přímo úměrná proudu protékajícímu motorem. Závislost obvodové rychlosti na výstupu FB (normalizováno) při referenci na rychlost 1 m s−1 je znázorněna na obrázku 5.1.
Speed [ms−1]; FB [−/300]
1.2 1 0.8 0.6 0.4 0.2 0 0
500
1000
1500 Time [ms]
2000
2500
3000
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 ] 0.95 1.01 1.08
FB [-] 127 140 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 F B ≈ 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ři zrychlení 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 funguje je demonstrováno na obrázku 5.2.
1.2 1
Speed [ms−1]
0.8 0.6 0.4 0.2 odhadnuty realny
0 −0.2 0
500
1000
1500 Time [ms]
2000
2500
3000
Obrázek 5.2. Reálný a vypočtený průběh rychlosti.
5.2
Model vozidla v prostředí MATLAB/Simulink
V prostředí programu MATLAB/Simulink jsem vytvořil model, který simuluje dynamiku polohového servomotoru a hnacích motorů. Jeho schéma se zavedenými zpětnými vazbami 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žadovanému úhlu natočení předních kol, výstupem pak úhel θ, který je roven aktuálnímu natoč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 s ne příliš velkým překmitem dostat na požadovanou referenční hodnotu. Zároveň však musí 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:100 do 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í rychlosti 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ý vliv na výslednou odezvu, neboť při návrhu byly respektovány meze akčních zásahů a proto ani 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 polohový servomotor je 50 Hz, a obvodovou rychlost lze měnit jednou za 0,09 ms, frekvence PWM 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 Ts
1 . 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.
35 Zasah regulatoru θref
30 25
θ
Angle [°]
20 15 10 5 0 −5 −10 0
0.5
1
1.5 Time [s]
2
2.5
3
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
Speed [ms−1]
..................................
5.3 Návrh a implementace regulátorů
1.6
v vref
1.4
Zasah regulatoru
1.2 1 0.8 0.6 0.4 0.2 0 0
2
4
6
8
10
Time [s]
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 odchylky od referenční hodnoty řízené veličiny. Požadavek na referenční rychlost je generován na 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, proto jsou implementovány funkce, které se starají o přepočet úhlu natočení kol a obvodovou rychlost kol na odpovídající střídu PWM signálů.
25
Kapitola Závěr
6
Podle očekávání se během testování na trati ukázalo, že reálný model vozidla na rozdíl od 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ůsobovalo hlavně na rovinkách mírné kmitání kolem vodící čáry. Další zjištěnou nelinearitou byla mrtvá 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áno navrž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 prahu vstupní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í se maximá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ě riskantní úpravu, vzhledem k dispozici pouze jedním H-můstkem jsem tuto úpravu neprovedl. 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/Numerickemetody.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 A Zadani práce
30
Příloha B Obsah CD Na CD jsou umístěny adresáře s následujícím obsahem:
.
Kod MATLAB
.
Kod C
. Funkce pro výpočet integrace a zrychlení. . Funkce pro filtrování a vykreslování naměřených dat. . Naměřená data. . 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
