VYSOKÉ UČENÍ TECHNICKÉ V BRNĚ BRNO UNIVERSITY OF TECHNOLOGY FAKULTA ELEKTROTECHNIKY A KOMUNIKAČNÍCH TECHNOLOGIÍ ÚSTAV AUTOMATIZACE A MĚŘÍCÍ TECHNIKY FACULTY OF ELECTRICAL ENGINEERING AND COMMUNICATION DEPARTMENT OF CONTROL AND INSTRUMENTATION M5. ODHAD A ŘÍZENÍ ORIENTACE MULTIKOPTÉRY SEMESTRÁLNÍ PROJEKT AUTOR PRÁCE Bc. ONDŘEJ KLAPIL (125475), Bc. PETR KOVÁŘ (125496) VEDOUCÍ PRÁCE PŘEDMĚT Mgr. RADEK BARÁNEK ROBOTIKA (MRBT) BRNO 213
ZADÁNÍ Na již funkční multikoptéře implementujte algoritmy odhadu (estimace) a řízení orientace. Multikoptéra je vybavena vývojovým kitem STM32F3Discovery. POUŽITÝ PŘÍPRAVEK Úloha byla vypracovávána na již hotovém hardware quadrokoptéry, viz obr. 1. Na kostře ve tvaru kříže je standardní baterie pro RC modely 11,1 V jištěná pojistkou, s velkou proudovou zatížitelností. Každé rameno quadrokoptéry je osázeno stejnosměrným motorkem s vrtulí, se jmenovitým napětím 7,2 V a výkonem 9W. Pro řízení byl vybrán vývojový kit STM32F3Discovery firmy ST Electronics. Více ohledem popisu přípravku lze nalézt v dokumentaci z minulého roku [1]. V rámci projektu minulý rok byla sestavena konstrukce, připojen RC přijímač, nainstalovány motory, zprovozněn gyroskop pro získávání úhlových rychlostí a následná regulace podle změn těchto úhlových rychlostí. V rámci tohoto projektu byl kód pro odhad orientace a řízení orientace. STM32F3Discovery již obsahuje všechny potřebné MEMS inerciální senzory, (gyroskop, akcelerometr, magnetometr), lze proto využít některých z algoritmů pro odhad orientace a regulaci. 2
ODHAD ORIENTACE Při orientaci je třeba určit vždy nějakou referenční souřadnou soustavu. V letectví a při použití magnetometrů se nejčastěji používá konvence NED (North East Down). Další osu poté tvoří osa našeho tělesa, u kterého orientaci určujeme, viz obr. 2. Orientace může být určena buď Eulerovými úhly (roll, pitch, yaw), určujícími natočení v určité ose tělesa, nebo kvaterniony či rotačními maticemi. Eulerové úhly mají oproti dalším dvou metodám nevýhodu v singulárním bodu, to je orientace tělesa, při které splývají dvě osy postupných rotací, kdy jde jednoznačně určit pouze jeden úhel a ostatní jsou ve vztahu. Pro reprezentaci proto byly zvoleny rotační matice 3x3, jelikož ji lze lehce naplnit bázovými vektory soustav pomocí MEMS snímačů. Obr. 1: Souřadné soustavy referenční a natočeného objektu Pro orientaci by byla dostačující informace z gyroskopu (snímače úhlových rychlostí) umístěném na vývojovém kitu STM32. Pokud známe počáteční polohu, pak pouhou znalostí úhlovým rychlostí jsme schopni určovat změny orientace, pomocí rekurentního vztahu. Problém v tomto případě je v tzv. drifftu snímačů, kdy se konstantně mění offset, který snímače ukazují při nulové úhlové rychlosti, a právě v rekurentním vztahu. Pokud se v jedné iteraci vyskytne chyba, tato chyba projeví i v dalších krocích a tím narůstá odchylka od skutečné orientace. U MEMS snímačů je to v řádu stupňů za minutu. 3
Pro určení orientace by také stačily pouze informace z akcelerometru a magnetometru, i když každý sám nese pouze částečnou informaci o orientaci. Pokud bychom uvažovali rotační matici získanou pomocí těchto dvou snímačů, pak akcelerometr měří zrychlení rovno 1 g prakticky v kolmém směru k zemi, což odpovídá třetímu řádku rotační matice, tj. bázový vektor z souřadné soustavy NED. Magnetometr měří vektor magnetické indukce, a jelikož až na výjimky (magnetické póly) není totožný tento vektor se směrem gravitačního zrychlení, je možno pomocí těchto dvou senzorů určit orientaci tělesa. Pro určování orientace pak byl použit Komplementární filtr, algoritmus založený na získávání základní orientace pomocí rotační matice založené na úhlových rychlostech, který je spolehlivý krátkodobě, a chyba je korigována pomocí matice orientace určené ze senzorů akcelerometru a magnetometru. Odchylka této matice od skutečné orientace je totiž konstantní, i když její dynamika je velice pomalá. Pokud si rotační matici z úhlových rychlostí označíme jako R1 a rotační matici z akcelerometru a magnetometru jako R2, pa princip algoritmu spočívá v zarovnávání matice R1 k matici R2 pokud by začala ujíždět. K tomu je nutné vyjádřit si číselně rozdíl těchto dvou matic. Pokud budeme vycházet ze základní orientace určené pomocí úhlových rychlostí kolem jednotlivých os měřené soustavy, pak můžeme vývoj rotační matice popsat diferenciální rovnicí: Ċ (t)=c (t) Ω(t ) kde C je rotační matice a Ω je kososymetrická matice definována jako: ω z ω y ω Ω=[ ] z ω x ω y ω x kde ω jsou aktuální úhlové rychlosti kolem jednotlivých os. Po diskretizaci a aproximaci prvním řádem dostaneme diferenční rovnici: C (T (n+1))=c (Tn) (I +Ω(Tn) T ) kde T je perioda vzorkování a n je číslo iterace. Matici R2 získáme jako složení třetího řádku, který se naplní po normalizaci vektoru gravitačního pole od akcelerometru a prvního řádku, kde se po odstranění průmětu směru gravitačního pole a normalizaci, vloží kolmá vektor, zároveň směřující k severu. Druhý řádek se získá jako vektorový součin prvního a třetího. 4
Vzhledem k tomu, že rotační matice lze chápat jako trojici bázových vektorů dané souřadné soustavy, lze počítat rozdíly orientace právě pomocí vektorových počtů. Výsledkem pak budou tři odchylky vztažené k jednotlivým osám, relativně vyjadřující potřebu otočení kolem dané osy k zarovnání obou soustav. V další části algoritmu je pak zpracování chyby a následný výpočet offsetů, tj. akčních zásahů, o které je třeba zarovnat matice. V nejjednodušším případě je offset dané složky přímo úměrný chybě. Výhodnější je ovšem chybu využívat pro zpracování chyby PI regulátor, kdy integrační složka ustálí výsledný algoritmus proti otřesům a nepřesnostem způsobeným maticí R2. Ta může někdy dávat nerelevantní informace o orientaci, díky rušení magnetického pole, či abnormálnímu zrychlení. Blokové schéma algoritmu je na obrázku. Obr. 2: Schéma výpočtu algoritmu odhadu orientace V samotném kódu se o odhad orientace stará rutina IMU_update(void), která je volána v hlavní rutině. Pro rychlost algoritmu a správné pracování je potřeba dobře nastavit konstanty KI_IMU a KP_IMU. Na obrázku 4 jsou vidět grafy orientace, přepočítaných a zobrazených v Eulerových úhlech, což bylo mnohem jednodušší na kontrolu, zda algoritmus dává správné výsledky než z rotačních matic. Přeskoky u třetího grafu (Yaw) jsou způsobeny přepočtem rotačních matic na úhly, resp. nespojitostí funkce arctg. 5
6
REGULACE NA ŽÁDANÉ ÚHLY Quadrokoptéra je řízena pomocí tahu motorů. Pro stabilní vznášení je třeba, aby vrtule vyvíjely stejný tah a směr otáčení vedlejších vrtulí byl opačný. Pro manévrování je zapotřebí měnit tah motorů: OBR. 3: Jak docílit manévru u quadrokoptéry [6] Postup regulace OBR. 4: Schéma regulace: 7
Zpracováním signálu z vysílačky dostaneme žádané Eulerovy úhly yaw, pitch a roll. Z těchto úhlů poté vypočítáme žádanou rotační matici Rref: Provedeme odhad orientace na základě dat ze senzorů (viz výše) a získáme rotační matici skutečné orientace R. Vypočteme odchylku orientace pro každou osu. Odchylku získáme tak, že maticově pronásobíme jednotlivé sloupce obou rotačních matic a skalárně vynásobíme (viz literatura [3]) Tato pak vstupuje do PI regulátoru, který je samostatný pro každou osu. Výstupem z PI regulátoru je žádaná úhlová rychlost pro jednotlivé osy, které vstupuje do P regulátoru, jehož výstupem je žádaný moment otáčení pro danou osu. P regulátor byl nastaven podle [1]. Momenty otáčení jsou dále přepočítány na akční zásahy jednotlivých motorů podle rovnic: 8
PI regulátor je realizován funkcí Control_Update(void), která běží v hlavní kontrolní smyčce. Jedná se o jednoduchou implementaci základního PI regulátoru, err_x je chyba orientace a sum_x je sumace odchylky: Nastavení PI regulátoru ((KP_ANGLE_X*err_x) + (KI_ANGLE_X*dt*sum_x))) Kde sum_x = sum_x + err_x; Prvotního nastavení PI regulátoru bylo dosaženo díky zkušenostem vedoucího práce s podobnými systémy. Hodnoty zesílení pro osu x a y jsou totožné, jelikož předpokládáme totožnost motorů a jedná se o stejné pohyby. Další nastavení z prvotního odhadu vycházela. Jednotlivá nastavení regulátoru jsme testovali na sérii pohybů quadrokoptéry, kdy jsme se snažili o co největší shodu jednotlivých testů. Test se skládal z pohybu dopředu, dozadu, doprava, doleva a kolečka o 36. Pro zhodnocení kvality nastavených hodnot regulátoru jsme použili součet kvadrátů odchylek dělený časem. Vzhledem k rozdílům mezi jednotlivými testy je toto hodnocení spíše orientační. 9
Regulátor 1 KP_ANGLE_X=15; KP_ANGLE_Y=15; KP_ANGLE_Z=12; KI_ANGLE_X=2; KI_ANGLE_Y=2; KI_ANGLE_Z=6; OBR. 5: Odezva quadrokoptéry na žádanou hodnotu z vysílačky s regulátorem 1 3 2 R o ll m e n é ž á d a n é 1-1 -2 5 1 1 5 2 2 5 3 3 5 4 4 5 a s [s ] 2 1 P itc h m e n é ž á d a n é -1-2 5 1 1 5 2 2 5 3 3 5 4 4 5 a s [s ] 1 Y a w m e n é ž á d a n é -1-2 -3-4 5 1 1 5 2 2 5 3 3 5 4 4 5 a s [s ] 1
Regulátor 2 KP_ANGLE_X=12; KP_ANGLE_Y=12; KP_ANGLE_Z=11; KI_ANGLE_X=3; KI_ANGLE_Y=3; KI_ANGLE_Z=55; OBR. 6: Odezva quadrokoptéry na žádanou hodnotu z vysílačky s regulátorem 2 3 2 1 R o ll m e né ž á d an é -1-2 -3 5 1 1 5 2 25 3 a s [s ] 3 2 P itc h m e né ž á d an é 1-1 -2 5 1 1 5 2 25 3 a s [s ] 1 Y a w m e né ž á d an é -1-2 -3-4 5 1 1 5 2 25 3 a s [s ] 11
Regulátor 3 KP_ANGLE_X=17; KP_ANGLE_Y=17; KP_ANGLE_Z=13; KI_ANGLE_X=35; KI_ANGLE_Y=35; KI_ANGLE_Z=6; OBR. 7: Odezva quadrokoptéry na žádanou hodnotu z vysílačky s regulátorem 3 3 2 R o ll m ené ž ádané 1-1 -2 5 1 1 5 2 25 3 as [s] 3 2 1 P itc h m ené ž ádané -1-2 -3 5 1 1 5 2 25 3 as [s] 1-1 Yaw m ené ž ádané -2-3 -4-5 5 1 1 5 2 25 3 as [s] 12
Regulátor 4 KP_ANGLE_X=16; KP_ANGLE_Y=16; KP_ANGLE_Z=14; KI_ANGLE_X=1; KI_ANGLE_Y=1; KI_ANGLE_Z=8; OBR. 8: Odezva quadrokoptéry na žádanou hodnotu z vysílačky s regulátorem 4 2 1 R o ll m ené ž ádané -1-2 5 1 15 2 25 3 35 as [s] 2 1 P itch m ené ž ádané -1-2 5 1 15 2 25 3 35 as [s] 1 Yaw m ené ž ádané -1-2 -3-4 5 1 15 2 25 3 35 a s [s] Zhodnocení Všechny regulátory se chovaly víceméně stejně. Jako nejpomalejší se jevil regulátor 2, nejlépe kopíroval žádanou hodnotu regulátor 3. Odezva je u všech regulátorů kolem,2s. Na ovládání quadrokoptery nebyla jednotlivá nastavení prakticky poznat, avšak podle kriteria byl nejlepší regulátor 4. 13
ZÁVĚR V rámci projektu byl vytvořen kód programu pro zpracovávání dat z dalších senzorů (akcelerometr a magnetometr), vytvořen kód pro odhad orientace založený na algoritmu komplementárního filtru, naprogramován PI regulátor využívající rotační matici z odhadu a otestován na reálném přípravku. V průběhu práce se objevovaly značné problémy s vývojovými prostředími, resp. s omezeními free verzí na velikost kódu, apod. V důsledku toho byl nakonec zvoleno vývojové prostředí IAR embedded studio, které po 3 dní neobsahuje žádné omezení. V rámci práce byl naprogramován algoritmus komplementárního filtru pro odhad orientace quadrokoptéry v prostoru, využívající MEMS inerciálních senzorů, schopný odstraňovat chybu vzniklou iteracemi hodnot z gyroskopů. Tento algoritmus byl otestován pro různá nastavení svých koeficientů Kp a Ki, optimální hodnoty poté byly určeny jako.3 pro Kp a.3 pro Ki. Bylo zjištěno, v souladu s teorií, že tento algoritmus, obsahující integrační složku pro zpracování chyby, je mnohem odolnější na vibrace a stabilnější ve zpětné smyčce. Dále byl navržen a realizován systém řízení quadrokoptéry na žádanou orientaci, založený na principu zarovnávání rotačních matic pomocí PI regulátoru. Takto navržený regulátor je oproti P regulátoru na úhlové rychlosti [1] znatelně stabilnější a řízení quadrokoptéry je plynulejší a jednodušší. Bylo navrženo několik regulátorů a podle námi zvoleného kritéria byl nejlepší regulátor 4. 14
POUŽITÁ LITERATURA [1] ARM, Jakub, Quadrokopter řídíci deska, Projekt z předmětu MRBT, Brno 213, Vysoké učení technické v Brně, Fakulta elektrotechniky a komunikačních technologií [2] BARÁNEK R., ŠOLC F., Tuning of Complementary Filter Attitude Estimator using precise model of multicopter, Electroscope, Brno 21, Vysoké učení technické v Brně, Fakulta elektrotechniky a komunikačních technologií [3] BARÁNEK R., ŠOLC F. Senzor orientace založený na zpracování dat z inerciálních snímačů, Vysoké učení technické v Brně, Fakulta elektrotechniky a komunikačních technologií [4] CHMELAŘ, Pavel, Stabilizace polohy létajícího objektu pomocí inerciálních senzorů, Diplomová práce, Pardubice 211, Univerzita Pardubice, Fakulta elektrotechniky a informatiky [5] GÁBRLÍK, Petr, Quadrocopter stabilizace a regulace, Bakalářská práce, Brno 21, Vysoké učení technické v Brně, Fakulta elektrotechniky a komunikačních technologií [6] ELKYN, F. Quadrotor chipkit MAX32. January 7, 213. Dostupné z http://diydrones.com/profiles/blogs/quadrotor-chipkit-max32 [7] PREMERLANI, W., BIZARD, P., Direction Cosine Matrix IMU: Theory, Draft 29 15