BOJTÁR ORSIKA TDK DOLGOZAT

BOJTÁR ORSIKA TDK DOLGOZAT

BUDAPESTI MŰSZAKI ÉS GAZDASÁGTUDOMÁNYI EGYETEM GÉPÉSZMÉRNÖKI KAR AUTOMATIZÁLÁSI ÉS ALKALMAZOTT INFORMATIKAI TANSZÉK

TUDOMÁNYOS DIÁKKÖRI KONFERENCIA DOLGOZAT

1

BUDAPESTI MŰSZAKI ÉS GAZDASÁGTUDOMÁNYI EGYETEM GÉPÉSZMÉRNÖKI KAR MECHATRONIKAI MÉRNÖKI ALAPSZAK

BOJTÁR ORSIKA TDK DOLGOZAT Többrotoros rendszerek tervezése és stabilizálása

Konzulens: Sütő Zoltán Egyetemi docens, GET Tanszék Zelei Ambrus Tudományos segédmunkatárs, MM Tanszék

Budapest, 2015.10.15

2

1 TARTALOMJEGYZÉK KIVONAT ............................................................................................................................. 4 1.

BEVEZETÉS................................................................................................................... 5

2.

QUADROTOROK DINAMIKÁJA ............................................................................. 6

3.

2.1

BEVEZETÉS A TÖBBROTOROS RENDSZEREK DINAMIKÁJÁBA............ 6

2.2

KOORDINÁTA RENDSZEREK........................................................................ 10

2.3

ÁLLAPOTVÁLTOZÓK ...................................................................................... 11

2.4

EULER-SZÖGEK KORLÁTAI ........................................................................... 12

2.5

LEÍRÓ EGYENLETEK ........................................................................................ 13

2.6

A RENDSZER DINAMIKAI SZIMULÁCIÓJA .............................................. 19

PID SZABÁLYOZÁS ................................................................................................. 21 3.1

4.

5.

MOZGÁSSZABÁLYZÁS PID ALGORITMUSSAL........................................ 21

3.1.1

MOZGÁS ‘Z’ TENGELY MENTÉN .......................................................... 22

3.1.2

FORGÁS Z TENGELY KÖRÜL (ψ)........................................................... 24

3.1.3

FORGÁS X TENGELY (Φ) és Y TENGELY KÖRÜL (θ) ........................ 26

3.1.4

MOZGÁS X TENGELY MENTÉN ............................................................ 29

3.1.5

MOZGÁS Y TENGELY MENTÉN ............................................................ 29

3.2

PÁLYAKÖVETÉS ALAPJAI.............................................................................. 30

3.3

PÁLYAKÖVETÉS PID SZABÁLYOZÓVAL................................................... 31

ÉRDEKESSÉGEK A PÁLYAKÖVETÉSRŐL .......................................................... 35 4.1

GLOBAL POSITIONING SYSTEM .................................................................. 37

4.2

GPS ALAPÚ ORIENTÁCIÓ MEGHATÁROZÁS .......................................... 46

4.3

RECEDING HORIZON CONTROL ................................................................. 48

KÉTROTOROS RENDSZER ..................................................................................... 49 5.1

ELEKTRONIKA .................................................................................................. 53

5.2

ORIENTÁCIÓ MEGHATÁROZÁS .................................................................. 54

5.2.1

SZENZOROK KALIBRÁLÁSA ................................................................. 54

5.2.2

JELFELDOLGOZÁS .................................................................................... 56

5.3

GÉPÉSZETI MEGVALÓSÍTÁS ......................................................................... 58

5.4

EREDMÉNYEK ................................................................................................... 59

6

ÖSSZEFOGLALÁS ..................................................................................................... 63

7

HIVATKOZÁSOK ...................................................................................................... 64

8

MELLÉKLET ............................................................................................................... 65 3

KIVONAT Manapság a többrotoros autonóm rendszerek egyre elterjedtebbé válnak. Napról napra többet alkalmazzák őket mentőakciókban, a hadiiparban, és nagyon hasznosak egyéb módon megközelíthetetlen területek feltérképezésére is. Újabb fellendülő ágazatként jelent meg több ilyen robot összekapcsolása egy nagy együttműködő rajjá, például az állatok etológiájának vizsgálata céljából. Az összekapcsolt rendszerek vizsgálata a számos jövőbeli, kecsegtető kutatási területek egyike. Ezen projekt hosszú távú célja egy quadrotor teljes összeállítása – beleértve az összes gépészeti, elektronikai és informatikai részletet. Egy saját hardver előnye boltban kapható társaihoz képest az, hogy a mechanikai kialakítás, a szabályozó algoritmus paraméterei, vagy akár maga az algoritmus bármikor megváltoztatható – ezáltal működő platformot biztosíthat jövőbeli kutatásoknak. Munkámban elsőként egy QUAV1 teljes dinamikai modelljét írtam fel, majd ez alapján elkészített szabályzókat vizsgáltam és szimuláltam annak reményében, hogy egy kész hardveren tesztelhetem majd őket. Ezután kitekintésként pár elterjedt röppályakövető módszert említek meg, beleértve azokat is, amelyek használhatók GPS által le nem fedett területeken. Végezetül a szabályozással és valódi dinamikával való jobb megismerkedés céljából egy kétrotoros libikókát vizsgáltam. A quadrotorok hagyományos pozíciómeghatározása, szabályozása az Euler-szögeken (yaw, roll, pitch) alapszik. Az utóbbi kettő (roll és pitch) könnyen szemléltethető egy fix tengely körül elforduló lehetőleg szimmetrikus merev testtel. Egy ilyen rendszer került modellezésre és stabilizálásra (Matlab/Simulink környezetben, és valós, megépített hardveren), hogy alapot adjon a későbbi többrotoros rendszer pitch és roll szögének beállítására.

Angol elnevezéssel QUAV quadrotor unmanned aerial vehicle: négyrotoros pilóta nélküli légijármű 1

4

1. BEVEZETÉS Egy sima repülőgéppel szemben egy többrotoros légijármű előnye, hogy mozgása kizárólag a rajta található 4 motorra rögzített propellerek felhajtóerején alapszik. Alapjában véve egyszerűbb kialakítású, kisebb és könnyebben manőverezhető, illetve lehetőséget ad kifutópálya nélküli, helyből történő felszállásra. Már a huszadik század eleje óta kísérleteznek ilyen helikopterek kialakításával (Breguet-Richet Gyroplane 1907, George de Bothezat 1922). Míg kezdetben gondot okozott a bonyolult mozgások összehangolása, mára már elterjedten alkalmazzák őket az iparban és a kutatások széles területén. Manapság egyre fontosabb az autonóm rendszerek fejlesztése: egy katonai bevetés vagy egy mentőakció túl veszélyes, vagy időben túl hosszú lehet emberi legénység számára. Gyakran olyan kevés felszerelést kell szállítani (pl. kémrobotok), hogy maga a pilóta megkétszerezné a költségeket. A pilóta nélküli járművekhez kapcsolódóan két fontosabb fejlesztési terület közül az egyik aktív kutatás több ilyen drón egy nagy csapattá történő összehangolására irányul (ld. például ELTE COLLMOT projekt). A másik, égetőbb kérdés a tápellátás biztosítása: a ma elérhető akkumulátorokkal körülbelül 20 perces repülések valósíthatók meg. Egy házhoz szállítás, permetezés, vándorló vadak megfigyelése több napig eltartó munka lehet. A probléma megoldása két oldalról közelíthető meg: a kutatások egyik fele jobb tápot fejleszt, a másik légi töltőállomásokat alakít ki, autonóm „légi tankoláshoz”. Egy ilyen UAV a maga komplexitásában is bonyolult mérnöki feladat, de növekvő fontossága megkérdőjelezhetetlen.

1. ábra Millenium Falcon Quadrotor

5

2. QUADROTOROK DINAMIKÁJA 2.1 BEVEZETÉS A TÖBBROTOROS RENDSZEREK DINAMIKÁJÁBA Egy quadrotort négy, általában egy négyzet csúcsaiban szimmetrikusan elhelyezett propeller emel meg és hajt. Közülük egy pár az óramutató járásával megegyező CW irányban, a másik pár ellentétesen CCW irányban forog2, így küszöbölve ki a Newton harmadik törvényéből adódó elfordulást. Az ilyen rendszerek könnyű gépészeti megvalósíthatóságuk és szabályozhatóságuk miatt igen elterjedtek - a különböző manőverekhez elég a négy motor fordulatszámát megváltoztatni. A valóságban – mivel egy quadrotor a tér minden irányába el tud mozdulni – egy 6 szabadságfokú rendszerről beszélünk, míg összesen 4 aktuátorunk van, ami egy alulaktuált rendszert eredményez. Szerencsére van számos jól működő módszer a helyzet egyszerűsítésére. A repülés szabályozására kétféle konfiguráció terjedt el: a ’’ és az ’’ típusú. A kettő nagyon hasonló, egyedül a repülés iránya különbözik. Tegyük fel, hogy az ábrán látható szerkezet a nyíl irányába halad, miközben tartja magasságát. Ezt elérendő, a ’ konfiguráció’ megnöveli a 4-es motor fordulatszámát, illetve lecsökkenti a 2-esét. Ez egy 1-3 tengely körüli elfordulást, és egy vízszintes erőkomponenst eredményez. Ez hajtja előre majd a járművet.3 Ugyanez az ’ konfigurációban’ és növelésével, valamint és csökkentésével érhető el.

2. ábra ’X’ és ’+’ konfiguráció

CW clockwise, CCW counterclockwise A kellő szögelfordulás után természetes minden erőt egyenlőre kell állítani. Így megakadályozható a további elfordulás, valamint – ha ez az „egyenlő” erő picit nagyobb, mint az eredeti, a rendszer megtartja magasságát is. 2 3

6

Ha mind a négy motor és ESC ugyanolyan, a két módszer között nincsen különbség. A való életben azonban minden aktuátor egy picit máshogy reagál egy adott bemenetre. Kettő helyett négy motor használata jobban átlagolhatja azok hibáit. Az ’’ verziót szokás emiatt ‘simábbnak’ tekinteni, ez a megfelelőbb például egy kamerát stabilan tartó rendszerre. Ezen kívül azonban nincs más különbség közöttük, így a kódolást leegyszerűsítendő, a továbbiakban a részmozgások bemutatására ’′ konfigurációt használtam. A következők feltételezik az Euler szögek (lásd [1] 68. oldal) ismeretét. A motorok fordulatszámától függően a következő felhajtóerő- és szögelfordulás kombinációk lehetségesek.4 A) LEBEGÉS / MAGASSÁG VÁLTOZTATÁS Amikor mind a négy motor egyenlő felhajtóerőt produkál, a szerkezet tartja magasságát, vagy megnöveli/lecsökkenti azt. Lebegés akkor lép fel, ha az összesített felhajtóerő épp a gravitációs erőt ellensúlyozza. Ha ez a gravitációnál nagyobb (kisebb), a szerkezet emelkedni (ereszkedni) fog.

3. ábra Lebegés

4

Felszállás

A’+’ és ’–’ értékek a mellettük látható koordinátarendszerre vonatkoznak.

7

Leszállás

B) YAW SZÖGELFORDULÁS Ha az erők kiegyensúlyozatlanok (a CW irányba forgó aktuátorok által produkált összerő nem egyezik meg a CCW forgókéval) – egy Z irányú nyomaték lép fel Newton harmadik törvénye értelmében.

4. ábra YAW +

YAW-

C) ROLL SZÖGELFORDULÁS Ha az aktuátorok egyike gyorsul/lassul a pitch tengelyen (Y), a kialakuló különbség miatt a quadrotor elkezd forogni X körül. A roll tengely (X) légi járművek esetén általában a jármű eleje felé mutat – az alábbi ábrán például a jármű jobbra repül.

5. ábra ROLL+

ROLL-

8

D) PITCH SZÖGELFORDULÁS Hasonlóan igaz ez a pitch-re: ha az X tengelyen lévő aktuátorok fordulatszáma megváltozik egymáshoz képest, Y körüli forgatás lép fel.

6. ábra PITCH+

PITCH-

A valódi alkalmazásokban ezen részmozgások bonyolult kombinációi jelennek meg. PID mozgásszabályozót alkalmazva külön-külön szükséges szabályozni őket individuális PID kontrollerekkel. Egy komplex pályakövetési feladatnál figyelni kell az egymás utáni sorrendre és időzítésre is. Egy nagyon gyors szabályozás vagy bonyolult pálya esetén majdnem biztos a hiba fellépése (első körben nincs idejük a szabályozóknak tökéletesen beállni, nyilván idővel - ha más nem -, az integráló tag korrigál), emellett akár be is lengethetik egymást a szabályozók. A komplex nemlineáris szabályozók (pl. visszacsatolásos linearizáció, backstepping algoritmus) a dinamika és szabályzáselmélet mélyebb megértését teszik szükségessé, de a fent említett problémák alkalmazásukkal még hatékonyabban kiküszöbölhetők.

9

2.2 KOORDINÁTA RENDSZEREK Ahhoz, hogy le tudjuk írni egy QUAV pozícióját és orientációját térben és időben, különböző koordinátarendszerek szükségesek. Megjegyzem, hogy a továbbiak részletesen olvashatók az [2] alábbi irodalomban.

7. ábra5 Koordinátarendszerek

Ϝ , , a Földhöz rögzített inertial frame6. A quadrotor pozíciójának követéséhez szükséges leírni annak Ϝ -hez viszonyított transzlációs mozgását.

Ϝ , , az úgynevezett ‘vehicle frame’, ami az előző ‘inertial frame’ jármű súlypontjába eltolt megfelelője. Mindig a jármű aktuális pozícióját jelöli.

Ϝ , , Ϝ yaw (Z) tengely körüli elforgatásával (ψ) kapható meg. Origója a jármű súlypontjában van. Ϝ , , hasonlóan Ϝ pitch tengely körüli (θ) forgatásával adott. Origója a jármű súlypontjában van.

Ϝ , , az aktuális orientációt jellemző body frame. Ϝ roll tengely körüli forgatásából (Φ) kapjuk, origója még mindig a súlypont. Az aktuátorokból származó felhajtóerők alapvetően ebben a rendszerben adottak.

Egy quadrotor mozgása Ϝ Ϝ -hez képesti transzlációs, valamint Ϝ Ϝ -hez képesti rotációs mozgásából tevődik össze. A testhez kötődő rendszerek különböző origói az ábrán csak a jobb láthatóság kedvéért nem esnek egybe annak súlypontjával. 6 Inertial frame: inerciarendszer, Body frame: testhez rögzített rendszer 5

10

Egy adott rendszerből egy másikba való transzformációt a következő mátrixok írják le.7 ௩௩ଵ ψ

θ 0 ψ ψ 0 ௩ଶ θ = −ψ ψ 0 ௩ଵ = 0 1 θ 0 0 0 1 1 0 0 = 0 Φ Φ 0 −Φ Φ

−θ ௕ Φ 0 ௩ଶ θ

௕ Φ ௩ଶ θ ௩ଵ ψ ௩௕ Φ, θ, ψ = ௩ଶ ௩ଵ ௩ = 1 0 0 θ 0 −θ ψ ψ 0 = 0 Φ Φ 0 1 0 −ψ ψ 0 0 −Φ Φ θ 0 θ 0 0 1 (θ)(ψ) (θ)S(ψ) −(θ) = Φθψ − (Φ)S(ψ) Φθψ + (Φ)C(ψ) Φθ Φθψ + (Φ)S(ψ) Φθψ − (Φ)C(ψ) Φθ

௕௩ Φ, θ, ψ = (௩௕ Φ, θ, ψ)ିଵ = (௩௕ Φ, θ, ψ)் = (θ)(ψ) Φθψ − (Φ)S(ψ) Φθψ + (Φ)S(ψ) = (θ)S(ψ) Φθψ + (Φ)C(ψ) Φθψ − (Φ)C(ψ) −(θ) Φθ Φθ

2.3

ÁLLAPOTVÁLTOZÓK

A dinamikai modellhez a következő állapotváltozókat használtam:

az eredeti Ϝ௜ -ben adott, a quadrotor pozícióját írja le,

az elforgatott Ϝ௕ -ben adott, a quadrotor sebességét írja le, Φ θ a quadrotor orientációját írja le, Φ Ϝ௩ଶ -ben adott, θ Ϝ௩ଵ -ben, ψ Ϝ௩ -ben, ψ

az elforgatott Ϝ௕ -ben adott, a quadrotor szögsebességét írja le.

7

C() jelenti a cosinus, S() a sinus függvényt.

11

2.4

EULER-SZÖGEK KORLÁTAI

Az első Euler-transzformáció körül forgat, létrehozva így egy új ௩ଵ , ௩ଵ tengelyt. A következő forgatás ௩ଵ körüli, ௩ଶ , ௩ଶ -t generálva. A legutolsó ௩ଶ körüli (roll) forgatás végén megkapjuk az új ௕ , ௕ , ௕ ‘body’ koordináta rendszert. A bázisvektorok tulajdonságai miatt biztos, hogy

⊾ ௩ଵ

௩ଵ ⊾ ௩ଶ

Egy 90°-os ௩ଵ körüli pitch forgatás esetén viszont megeshet, hogy az eredeti és a legutolsó tengely egybeesik (ez az úgynevezett gimbal lock probléma):

≡ ௩ଶ Ez esetben - ha a végeredmény szenzorjeleiből dolgozunk -, nem egyértelmű, hogy az adott forgatás hányad része volt a yaw, hányad a roll forgatás. Emiatt szokás kvaternió (ld. [1] 68. oldal) alapú dinamikával is dolgozni. Mivel • • •

a szimulációmban szenzorok nélkül közvetlenül a szögekkel (illetve azok deriváltjaival) dolgoztam, azok stabilitási problémák miatt amúgy is ± korlátok közé vannak szorítva, valamint a későbbi hardveres feladat egy 1 DoF8 rendszert vizsgál,

így egyelőre nem merülhetett fel ilyen probléma. A későbbi feladatokhoz természetesen hasznos lehet a kvaternió alapú modellezés is.

8

DoF: degree of freedom,szabadsági fok

12

2.5

LEÍRÓ EGYENLETEK

Newton mozgásegyenletei a következők () = () = = =

ahol a jármű tömege, a ‘body’ rendszerben a test súlyponti tengelyeire számított tehetetlenségi nyomatékok mátrixa. Ez utóbbi – szimmetrikus konfigurációt feltételezve =0 0

0 0

0 0

ିଵ

=

1 ! 0

0

0 1 0

$ 0# # 1 "

0

Mivel maguk az egyenletek a földi rendszerben adottak, egyes állapotváltozóink (, ) viszont a testhez vannak kötve, az Ϝ௜ és Ϝ௕ közötti kapcsolatot egyértelműsíteni kell. % Egy egyszerű ௩ = & vektor a föntebb leírt transzformációs mátrixokkal ' könnyedén át- (és vissza) vihető ௕ vektorba: ௕ = ௩௕ Φ, θ, ψ௩ ௩ = ௕௩ Φ, θ, ψ௕

Ezt használjuk majd például a külső erők, a pozíció és a sebesség vektorának transzformálásához. Egyes transzformációk azonban további részletezést igényelnek. Legyen adott két koordinátarendszer: az egyik a Földhöz kötött Ϝ௜ , a másik az ehhez képest rotációs és transzlációs mozgást végző Ϝ௕ . Egy Ϝ௕ rendszerben mozgó vektor deriváltját Ϝ௜ -ből a következőképp lehet felírni (először vizsgáljunk egy szimplán transzlációs mozgást végző Ϝ௕ -t, majd egy szimplán ௕,௜ szögsebességgel forgó rendszert, majd a kettőt összevetve):

13

= + ௕,௜ × ௜ ௕ Ez alapján a newtoni mozgásegyenletek

= ( + ௕,௜ ௕ ௜

() = ௜ × ) = + = ௚ + ௨ + ௔ௗௗ௜௧௜௢௡௔௟ =

− *θ 1 0 − = + −*θΦ + 0 − −*θΦ ௕

ahol ௕ = ∑ସ௜ୀଵ &௜ ,௜ଶ az egyes propellerek által generált felhajtóerő (,௜ a propeller szögsebessége, &௜ az úgynevezett ‘thrust factor’). Az egyenletek ‘body’ koordinátákban voltak adottak – ugyanez az ‘inertial frame’ben: ( Φθψ + ΦSψ )௕ 0 1 - = 0 + ( Φθψ − ΦCψ )௕ −*

Φθ௕

A föntebb említett indokokkal ( ௚,௕ = ௩௕ Φ, θ, ψ௚,௜ , ௨,௜ = ௕௩ Φ, θ, ψ௨,௕ , derivált kifejezése rögzített koordinátarendszerből) a kettő kapcsolata egyértelmű: =

() =

() = + ௕,௜ = ௘௫௧ + ௚௬௥௢௦௖௢௣௜௖ + ௔ௗௗ௜௧௜௢௡௔௟ ௜ ௕

௘௫௧ az alap, aktuátorok által keltett nyomaték. ௘௫௧ áll egyrészt az egyszerű erőerőkar elvű roll és pitch nyomatékokból, illetve a már többször említett, Newton harmadik törvénye okozta yaw nyomatékból. Ha . egy adott aktuátor és a quadrotor súlypontja közötti távolság, & a ‘thrust factor’, pedig a propellerek és környezetük egyéb aerodinamikai tulajdonságait figyelembe vevő tényező, akkor

14

8. ábra A quadrotor ezentúl használatos alapértelmezett koordinátarendszere

.

Ezen kívül figyelembe vehető még a giroszkopikus hatás is. Ha egy fix tengely körül forgó testet tengelyére merőleges erőhatás ér, mind a tengelyre, mind az erőhatásra merőlegesen tér ki. Jól megfigyelhető ez olyan gyerekkori játékainknál, mint a búgócsiga, vagy használható biciklik stabilizálására is, és ez az eltérülés az alapja sok műrepülő légi manővernek is. A propellerek forgásából adódó giroszkopikus hatás a következőképp írható le: ,

,

0 0 " # 0 % # 0 % , !

ahol & az egyes rotorok irányú tehetetlenségi nyomatéka. Összegezve:

& ) , ! ( + , ) , ) ( + + ! + ( # % ( & +. ( + (

(

+ ( + ( ! +

( + 1 * ' * ' & ' *

15

A fenti egyenlet ‘body’ koordinátákkal adott. A jobboldal első tagja maga is egyfajta giroszkóp hatást képvisel (azt mutatja meg, hogy két merőleges szögsebesség hogyan befolyásolja a harmadikat). A harmadik tag a valóságban nagyon kicsi hatással van a teljes szerkezet mozgására, így a további szimulációk során elhanyagolható. Egyelőre van egyrészt 3 egyenletünk a test koordinátarendszerében megadott sebességek deriváltjaira, ahol a sebességek egyszerűen áttranszformálhatók Ϝ௜ rendszerbe ௕௩ segítségével, másrészt adott másik három egyenlet a szögsebességek deriváltjaira szintén ‘body’ koordinátarendszerben. Mivel Euler-szögeket használunk az orientáció egzakt leírására, a szögsebességeket szükséges még Euler-szögekkel Φ kifejezni. A θ ↔ transzformáció - ahol ‘body’ koordinátákkal adott, Φ ψ

Ϝ௩ଶ -ben, θ in Ϝ௩ଵ -ben és ψ Ϝ௩ -ben - a deriváltak elkülönült, önálló transzformálását teszi szükségessé: 0 0 Φ ௕ ௕ ௩ଶ ௕ ௩ଶ ௩ଵ

= ௩ଶ Φ 0 + ௩ଶ Φ௩ଵ θ θ + ௩ଶ Φ௩ଵ θ௩ ψ 0 , ௕ ψ ௩ 0 ௩ଵ 0 ௩ଶ

ahol

௕ Φ ௩ଶ

és hasonlóan

1 Φ 0 = 0 0 0 ௩ଶ

௩ଶ θ ௩ଵ θ

0 0

௩ଵ

0 0 Φ Φ (Φ) (Φ) 0 = 1 0 , −(Φ) (Φ) 0 ௩ଶ 0 ௕

0 = 1 θ 0 ௩ଶ

௩௩ଵ ψ 0 0

ψ

௩

0 = 1 0 . ψ ௩ଵ

Ezeket felhasználva a fönti egyenletek a következőkre egyszerűsödnek: 0 0 Φ ௩ଶ ௕ ௕

= 0 + ௩ଶ Φ θ + ௩ଶ Φ௩ଵ θ 0 , ௕ ψ ௩ଵ 0 ௩ଶ 0 ௕ 1

= 0 ௕ 0

1 Φ 0 θ = ! ψ 0

0 −θ Φ Φ Φθ θ , −Φ Φθ ψ

Φ/%0θ Φ/%0θ Φ −Φ $ Φ Φ " θ θ 16

Végezetül a rendszert leíró 12 differenciálegyenlet (θ)(ψ) Φθψ − (Φ)S(ψ) Φθψ + (Φ)S(ψ) = (θ)S(ψ) Φθψ + (Φ)C(ψ) Φθψ − (Φ)C(ψ)

−(θ) Φθ Φθ − *θ 1 0 − + −*θΦ + 0 = − −*θΦ ௕

1 Φ 0 θ = ! ψ 0

Φ/%0θ Φ/%0θ Φ −Φ $ Φ Φ " θ θ

1 2,௜ .& − ! $ ( ,ସଶ − ,ଶଶ ) ௜ୀଵ # ସ $ ! $ ! − .& # + − 1 2,௜ # # + ( ,ଷଶ − ,ଵଶ ) = # # # # ௜ୀଵ # − ସ ଶ ଶ ଶ ଶ # 1 " (,ଶ + ,ସ − ,ଵ − ,ଷ )" 1 2, ప " ସ

௜ୀଵ

Φ Az egyenletek megadhatók közvetlenül és θ segítségével is. Ehhez a ψ

következő transzformációk szükségesek: 1

= 0 0

0 −θ Φ Φ Φθ θ −Φ Φθ ψ

0 0 −θθ Φ = 0 −ΦΦ ΦΦθ − Φθθ θ ψ 0 −ΦΦ −ΦΦ θ − Φθθ 1 0 −θ Φ+ 0 Φ Φθ θ- 0 −Φ Φθ ψ-

17

Ezeket behelyettesítve az eredeti egyenletekbe, és a giroszkóp hatással egyszerűsítve a következő differenciálegyenletek adódnak: ( Φθψ + ΦSψ )௕ 0 1 - = 0 + ( Φθψ − ΦCψ )௕ −*

Φθ௕

.&( ,ସଶ − ,ଶଶ ) Φ θ- = 3Φ, θ, ψିଵ 45(Φ, θ, ψ, Φ , θ , ψ ) + 6 .&( ,ଷଶ − ,ଵଶ ) ଶ ଶ ଶ ଶ -ψ (,ଶ + ,ସ − ,ଵ − ,ଷ ) ahol 3Φ, θ, ψ és 57Φ, θ, ψ, Φ , θ , ψ 8 transzformációs mátrixok 3Φ, θ, ψ = 0 0

0 − θ Φ Φθ − Φ Φθ

5ଵ 57Φ, θ, ψ, Φ , θ , ψ 8 = 5ଶ 5ଷ

5ଵ = θθ ψ + ( − )(−Φθ ଶ Φ + Φଶ θθ ψ − Φଶ θθ ψ + θଶ ΦΦψ ଶ ) 5ଶ = 7ΦΦ θ − ΦΦ θψ + Φθ θψ 8 + ( − )(Φθθ ψ − Φθθψ ଶ − ΦΦ θ + ΦθΦ ψ )

5ଷ = 7ΦΦ θ + ΦΦ θψ + Φθ θψ 8 + ( − )(Φ Φθ − θψ ଶ Φθ + θ θΦψ − Φψ θΦ )

18

2.6

A RENDSZER DINAMIKAI SZIMULÁCIÓJA

Nézzünk pár valódi példát a fenti rendszer dinamikájáról. Legyen adott egy – a gravitációnak ellen tartó – vízszintes helyzetben lebegő quadrotor. A négy rotor fordulatszáma megegyezik. Ha ezt a fönt leírt dinamikájú lebegő rendszert kitérítjük egyensúlyi helyzetéből – akár csak pár fokkal is – süllyedve megindul a kitérítés irányába.

9. ábra Adott szögű megdöntés hatása szabályzás nélkül

Jól látszik hogy a 8. ábrán bevezetett koordináta értelmezésnek megfelelően Φ . 0 és θ . 0 kitérítésekre / 0 és . 0 válaszokat kapunk.

10. ábra Adott szögű megdöntés hatása szabályzás nélkül (az alap értékek folytonos, deriváltjuk szaggatott vonallal jelöltek)

19

Jól megfigyelhető (szimmetrikus test esetén, külső zavarások nélkül) a kapcsolat a quadrotor Φ és θ szöge között. Külön-külön vizsgálva őket, ha megegyeznek, ugyanolyan hatással vannak az adott irányú elmozdulásra (az előjeltől eltekintve).

11. ábra Ugyanakkora roll és pitch szög hatása önmagában

Egyszerre működtetve viszont ezt a két egyenlő nagyságú szöget – az Eulertranszformációk értelmében – az egyik hatása a saját közvetlen tengelyére lecsökken, mely változás nagy szögek esetén igen nagy lehet. Az alábbi ábra egy Φ θ 1 rad esetet ábrázol.

12. ábra Ugyanakkora roll és pitch szög hatása egyszerre

A fentebb bemutatott dinamikával a quadrotor magára hagyva (vagy “pályakövetés” közben) nem kívánatos mozgásokat végezne. Ez szükségesség teszi a szabályozás alkalmazását.

20

3. PID SZABÁLYOZÁS 3.1 MOZGÁSSZABÁLYZÁS PID ALGORITMUSSAL A motorok bemenő feszültsége határozza meg a kimenő fordulatszámot, ezt szükséges tehát szabályozni az adott mozgások megvalósításához. Az általam használt motorok 1000 KV-s, 12 Voltról működtetett BLDC típusú eszközök. 1000 9 =

Innen a keletkező fordulatszám

: = :௠௔௫ ∗

;

:௠௔௫ 12 9

;௠௔௫

= :௠௔௫ ∗

:< 255

Fejezzük ki a rotorok szögsebességét a föntebb kifejtett differenciálegyenlet rendszerből. 0 −&. 4 − &

−&. 0 &

,ଵଶ ௫ &. ଶ$ ! , 0 ௬ ଶ 6 =4 6 ௭ ,ଷଶ # ଶ ௕ & ,ସ "

0 &. − &

Jól látszik, hogy a forgó propellerek által kifejtett felhajtóerő az aktuális ‘ ’ pozíciót befolyásolja; míg (az ezen erők nem egyenletes eloszlása esetén) keletkező nyomaték az orientációt. Ebből a fordulatszámok négyzetei kifejezve:

! ,ଵଶ 1 − ଶ !,ଶ $ 2&. = ,ଷଶ # 0 ଶ ,ସ " 1 2&.

0

−

1 2&. 0

1 2&. 0

1 4 1 4 1 − 4 1 4 −

1 4&$ ௫ 1 # 4&# 4௬ 6 1 # ௭ 4&# ௕ 1 4&"

Ez a fönti ; → , karakterisztikát figyelembevéve közvetlenül szolgáltatja a

, ψ, Φ, θ szabadságfokok szabályozójeleit. Innen is látható, hogy a hatféle lehetséges mozgás közül célszerű ezeket szabályozni. Az , menti haladás ezek kombinációjával valósítható meg. 21

3.1.1

MOZGÁS ‘Z’ TENGELY MENTÉN

Ahogy a fönti egyenletekből is látszik a z irányú mozgás csakis a rotorok felhajtóerejétől függ. Ez alaphelyzetben, azaz Φ = θ = 0 orientáció esetén:

- = −* +

1 1 Φθ௕ = −* + ௕

Egy ilyen pillanatban az egyes rotorok szögsebessége: ,௜ଶ =

1 4& ௕

Yaw forgás esetén (௭ ≠ 0) az összes felhajtóerő ugyanennyi marad. ௫ ≠ 0 vagy ௬ ≠ 0 esetén az össz-felhajtóerő a test koordináterendszeréből nézve szintén állandó, de annak elfordulása miatt csak egy része fordítódik a magasság emelésére. Adott emelkedés eléréséhez az ௕ bemenetet ilyenkor növelni kell.

A felhajtóerő szabályozására PID szabályozót alkalmaztam. ௠௚ állapotnak a - = 0 → ௕௔௦௘ = ସ lebegés állapotot.

9

Tekintsük alap

= ௥௘௙ − ()

:= = : + > ?? + = ௧

௧బ

Diszkrét jelekkel

:=௞ = : ௞ + 1 ௞ + = ( ௞ିଵ − ௞ ) ௞ୀ௡

௞ୀଵ

Végül az egyes kontrol bemenetek ଵ () = ௕௔௦௘ + :=( )

ଶ () = ௕௔௦௘ + :=( ) ଷ () = ௕௔௦௘ + :=( ) ସ = ௕௔௦௘ + :=

A szabályozó paramétereit egyelőre iteratív módon hangoltam be, emiatt látható sok helyen nulla I tag. 9

22

13. ábra Z irányú beállás 2 méteres referenciajel esetén

14. ábra PID paraméterek

23

3.1.2

FORGÁS Z TENGELY KÖRÜL (Ψ)

A fönti egyenlet nyomatékát egy, a föntihez hasonló struktúrájú PID szabályozóval befolyásolva !3 ψ ψ3

456 4!3 5 7 !8 8 6! 3

బ

456 4 ! 5 ! 6

az egyes bemenetek pedig:

9 3 9 456ψ 9 3 9 456ψ

9 3 9 456ψ 9 3 9 456ψ

15. ábra Yaw szög szabályozása alapjelre

24


25

3.1.3

FORGÁS X TENGELY (Φ) ÉS Y TENGELY KÖRÜL (Θ)

Hasonló elven működik ezen két szög szabályzása is, előbbi esetén a bemenetek !3 Φ Φ3 9 3 9

9 3 9 456Φ 9 3 9

9 3 9 456Φ

A pitch szabályzás a másik két motor fordulatszám különbségét befolyásolja: !3 θ θ3

9 3 9 456θ 9 3 9

9 3 9 456θ 9 3 9

17. ábra Roll szabályozás ϕ=0.7 rad alapjelre

26


19. ábra Pitch szabályozás θ =-0.5 rad alapjelre

27


28

3.1.4

MOZGÁS X TENGELY MENTÉN

Ez egy megfelelő θ orientáció és a ‘Z’ magasság tartásából adódik össze. A 8. ábrán látható előjelek szerint pozitív szög θ . 0 pozitív elmozdulást, negatív szög θ / 0 negatív elmozdulást okoz. Alapértelmezésben egy adott θ szöggel elforgatott állapotban az 9 erővektor felbontható egy gravitációt ellensúlyozó és egy vízszintes gyorsulást eredményező komponensre. 9 megfelelő nagyságúra növelésekor a föggőleges komponens épp a lebegést biztosítja: :; <Φ =θ <ψ =Φ Sψ 9 :; :? <Φ <θ 9

Legegyszerűbb Φ ψ 0 esetben:

:; =θ 9

:; :? <θ 9

21. ábra X irányú mozgás gyorsuláskomponensei

3.1.5

MOZGÁS Y TENGELY MENTÉN

A megvalósítás az előzőekhez hasonló.

Pozitív szög Φ . 0 negatív irányba mozgat, és fordítva. Ezen két szabályozó eredményességét a Pályakövetés alapjai c. fejezetben ismertetem.

29

3.2

PÁLYAKÖVETÉS ALAPJAI

Legyen adott egy 4 ∈ A B C térbeli pálya (lásd a [3] alatti irodalmat). 4 A darab térbeli pont kombinációja: beletartozik egy kezdőpont, egy végpont, és a köztes pontok. Egy adott 4 (D → D ) szegmens megtételekor a quadrotor pálya menti 9 10 és pályára merőleges 9 11 szabályozójeleket bocsát ki a kellően pontos pályakövetéshez. Az előző fejezetben bemutatott PID szabályozóval elterjedt megoldás a következő: minden szakasz előtt ráfordítjuk a pályára a quadrotort Fψ → ψ G, majd az így kapott , tengelyekkel közvetlenül generálható 9 és 9 :

22. ábra N pontból álló P térbeli pálya

ahol H az adott & és &+1 pontok közti szakasz pálya menti egységvektora,

I az adott & és &+1 pontok közti szakasz pályára merőleges egységvektora, D3 az aktuális pozíció vektora,

J3 az aktuális sebesség vektora,

K az elérni kívánt pálya menti sebesség a szakaszon.

Ezek segítségével a pozíció hibák:

L MD D3 N I L J3 I

L K J3 H

valamint egy egyszerűbb PID szabályzó pályakövetésre (PID a pályára merőleges hibákra, PI a pálya mentikre): 9 4 L 6 L 5 7 L 8 8

9 4 L 5 7 L 8 8

10 11

௔௧ pálya menti bemenet ~ along track ௖௧ pályára merőleges bemenet ~ cross track

30

Ekkor ௔௧ gyakorlatilag a z és pitch szabályzást, ௖௧ roll szabályzást jelent. Gyakori, hogy a pálya menti hibát elhanyagoljuk, csak annak deriváltjára (௜ௗ ) szabályzunk – így kikerülhető a célpontok előtti lelassuló fázis. :௜ szakaszból :௜ାଵ szakaszba érünk például, ha átléptük a :௜ -re annak végpontjában vetített normálist.

Fontos, hogy :௜ megtétele után az integrátorok lenullázandók – így nem okoz hamis hibát az addig felgyűlt I tag. Ez kis hajlásszögű pálya esetében elhanyagolható (pl. ha ugyanolyan marad a merőleges szélirány, felesleges megvárni, amíg újra eléri a szükséges értéket).

Megjegyzés: Ha mégis az @௔௧ hibára írunk szabályozót, célszerű a léptetést a következő feltételekhez kötni: 2) @௔௧ < %

22) @௔௧ < &(Aௗ௜ାଵ − Aௗ௜ )

ahol %, & adott konstans értékek. Mindkét feltétel a célpontok közelében való lelassulás elkerülésére való. Előbbi egy konstans értékhez köti a továbblépést, míg ଵ utóbbi 22) feltétel a pálya adott hányadának megtétele után enged tovább (pl. & = ହ ସ

esetén a táv ହ-ének megtétele után cél a következő pont). Ha mindkét feltétel alkalmazott, már az egyik elérésekor automatikusan Aௗ௜ାଶ legyen a következő célpont.

3.3

PÁLYAKÖVETÉS PID SZABÁLYOZÓVAL

Szimulációmban egy másik megoldást használok: a yaw szög konstans nulla alapjelet kap, így a pályakövetés az , , szabályozások összehangolása. irányban egy, a hibajelet is figyelembe vevő (itt, vagy a korábbi mozgásszabályozásoknál leírt) PID szabályozó használatos. irányban eredetileg egy ugyanilyen PID szabályozót használtam, majd ezt finomítottam tovább kisebb szakaszokra:

1) < Bଵ → θ௥௘௙ = θଵ Meginduláskor adott távon felgyorsítjuk a járművet a kellő sebességre. 2) Bଵ < < Bଶ → θ௥௘௙ = 0 Ezután az ‘úton’ a jármű felveszi alap orientációját – nem gyorsul tovább, a légellenállást elhanyagolva más nem is lassítja - halad az 1) pontban felvett sebességgel. 3) Bଶ < < Bଷ → θ௥௘௙ = −θଷ A cél előtti rövid szakaszon lefékezi magát. 4) Bଷ < → θ௥௘௙ = :=() Fékezés után a maradó hibát egy = x௥௘௙ − x() PID szabályozóval korrigálja.

Ez az algoritmus nagy távolságokra érvényes. Ha az Aௗ௜ାଵ − Aௗ௜ útszakasz rövid, a 2. lépés kimarad. Az következő ábrákon először egy messzi ponthoz kell eljutnunk - itt jól látszik a kezdeti gyorsuló szakasz utáni állandó sebességű rész, valamint a végső lassulás utáni PID-s korrigálás. A második ábra egy közeli célpontot esetét mutatja be. A 2. szakasz láthatóan eltűnt. 31

23. ábra X szabályozása távoli pontokra

24. ábra X szabályozása közeli pontokra

32

A továbbiakban a teljes (4-6) szabályozó által lekövetett pályákat mutatok be. A PID (a rendszer nemlinearitása miatti) korlátossága a bonyolultabb (pl. hirtelen, gyors váltásokat, nagy szögeket) igénylő pályák kialakításánál volt érezhető.

25. ábra A pálya megvalósítása

26. ábra A pálya xy és xz nézetekből

33

27. ábra B pálya megvalósítása

28. ábra B pálya xz és yz nézetekből

34

4. ÉRDEKESSÉGEK A PÁLYAKÖVETÉSRŐL Egy gépnek valamilyen mozgás megtételéhez szüksége van egy – a saját pozíciójától adott távolságra lévő célpontra. Az, hogy a kettő között vezető utat (köztes pontok kombinációja) pontosan hogyan is teszi meg, nagyban függ attól, hogy ezeket a köztes pontokat milyen módszerrel számítottuk. A két köztes pont közötti precíz haladás pedig már konkrétan az implementált mozgásszabályozó algoritmus dolga (lásd pl. PID szabályozás). Javarészt légijárművekre koncentrálva ezen három tényező (saját pozíció, végcél, köztes út) alapján a következő kategóriákat különítettem el.

EMBER lát, EMBER vezet Robotunk egy – a végcélt is tartalmazó - 3 dimenziós térben helyezkedik el. Amikor az emberi vezető saját maga látja ezt a teret (pl. saját szemével), lehetősége van a robot direkt irányítására. o Példa A) Kereskedelmi quadrotorok, gyerekjátékok: - POZÍCIÓ közvetlen látható - VÉGCÉL a felhasználó által adott - KÖZTES ÚT rövid FEL/LE/ELŐRE/FORDULJ/... mozgásokat jelent (általános távirányítóval, Wifin, Bluetooth-on, vagy egyéb csatornákon lekommunikálva) o Példa B) Robotikai versenyek: - POZÍCIÓ egy 3D marker-alapú képfeldolgozás eredménye - VÉGCÉL a pilóta által adott - KÖZTES ÚT rövid FEL/LE/ELŐRE/FORDULJ/... mozgásokat jelent (általános távirányítóval, Wifin, Bluetooth-on, vagy egyéb csatornákon lekommunikálva) Ebben az esetben a robotnak csak kellő precizitással végre kell hajtania a kapott parancsokat. A precizitás a drón által használt algoritmustól függ.

ROBOT lát, EMBER vezet Mikor a 3D teret helyettünk a robot térképezi föl, valamilyen feedback (pl. GPS koordináták, élő videó/kép közvetítése) segítségével még mindig lehetőségünk van direkt kontrollra. o Példa C) UAV-k irányítása a pilótaközpontból (mintegy videojáték szerűen): - POZÍCIÓ a robot által visszaküldött adatokból határozható meg (GPS/camera stream) 35

-

VÉGCÉL pilóta által adott KÖZTES ÚT még mindig rövid betáplált mozdulatokat jelent (mivel ez a módszer gyakran a következővel együtt használatos, ez általában kicsi korrigálásokat jelent az eredeti úthoz képest)

Ekkor a robot a mozgások precíz végrehajtásán kívül telemetriát is visszaküld. A komplex (pl. pálya) számító feladatokat a pilótaközpont gépein érdemes végezni, a drón vezérlő rendszerét kímélendő.

ROBOT lát, ROBOT vezet (UAV) Ez a kategória érvényes az összes autonóm navigációs rendszert használó járműre. Ez esetben a robot egy önálló intelligens lényként viselkedik, lokalizálva és végigvezetve magát a cél felé vezető úton. o Példa D) Autonóm navigációs rendszert használó UAV-k: - POZÍCIÓ szenzorjelekből és prediktív algoritmusokból - VÉGCÉL a pilóta által adott, vagy a robot által on the fly meghatározott - KÖZTES ÚT egy fedélzeti számítógép által meghatározott optimális útvonal (általában egy folyamatosan frissülő belső térkép adatbázis része (SLAM probléma)) Egy robot használhat tehát teljes mértékben a fedélzeti rendszerébe beépített navigációs algoritmust (Példa D), vagy tájékozódhat máshol kiszámított, rádiójelekkel kommunikált adatok alapján is (Példa A, B, C). A valódi autonóm rendszerek általában e kettő kombinációját használják fel – a legtöbb feladatot a járműnek magának kell megoldani, a fontos döntéseket és kis korrekciókat azonban egy emberi legénység adja meg egy távoli pilótaközpontból. Egy autonóm robot feladatai közé tartozhat a lokalizáció, a biztonságos és nem biztonságos területek elkülönítése, előbbiben egy költséghatékony pálya megtervezése, az ezen történő navigálás, az időközben jelentkező akadályok kikerülése, valamint egységesen egy belső térkép megteremtése az új környezetről. A térképezés alapproblémáját – folyamatosan frissíteni kell egy, a környezetről kialakított térképet, miközben maga az egyed is változtatja abban pozícióját – SLAM (simultaneous localization and mapping) problémának nevezik. Megjegyezném, hogy ez még egy lezáratlan kutatási terület. A köztes pontokat meg lehet adni GPS koordinátákkal, lehetnek egy fekete alakzat pixelei egy egyébiránt fehér háttér előtt (pl. vonalkövetési feladatok), lehetnek egy virtuális térkép részei, és így tovább. A következőkben a legelső, GPS koordinátás módszert írom le részletesebben. 36

4.1 GLOBAL POSITIONING SYSTEM A Global Navigation Satellite System (GNSS) több műhold összekapcsolásából keletkezett, navigációra használható rádiójeleket szolgáltató rendszer. Szokás PNT szolgáltatásnak is hívni (positioning, navigation and timing services). 20-30 - Föld körül elosztott MEO12 pályán keringő – műholdak segítségével biztosít globális lefedettséget. Egy egyszerű vevőkészülékkel - a leérkező adatok megfelelő dekódolása után – lehetőség nyílik saját pozíciónk meghatározására. Ennek végeredménye általában egy földrajzi szélesség-hosszúság-magasság adathármas (természetesen más földrajzi koordinátarendszerek is használatosak). Két – ismert időközű – mérésből egy adott test sebessége is kiszámítható. Egyszerű hangzása ellenére a feladat napjaink csúcstechnológiáját – az elektronika, távközlés, légkörfizika, matematika, űrkutatás, informatika és geodézia összefonódását igényli. Egy teljes lefedettséget biztosító rendszer fenntartását csak a vezető nagyhatalmak engedhetik meg maguknak. A világon ma 4 fő GNSS rendszer létezik: ezek közül egyedül az USA NAVSTAR GPS és Oroszország GLONASS rendszere teljesen működőképes. Utóbbit nemrég javították fel, így 2011-re egy 24 szatellitából álló, globálisan elérhető rendszerré vált. Az EU és ESA együttműködve fejleszti még a Galileo-t, mely várhatóan 2016-ban lép működésbe, és 2020-ra lesz teljes, 30 műholdból álló rendszer. Negyedikként ott van még Kína a saját fejlesztésű COMPASS (másképp Beidou-2) műholdakkal. A GPS alapú térinformatikai helyzetmeghatározás azt használja fel, hogy az egyes műholdak pozíciója az általuk közvetített adatokból ismert. A vevőeszközök a lesugárzott jelek alapján szatellitától vett távolságot számolnak, egyszerre legalább 34 műholdra. Egy távolságadat a térben egy gömböt feszít ki; kettő lecsökkenti a lehetséges helyeket egy körre; három esetén pedig már csak kettő pontról beszélünk. Ezek közül az egyik általában annyira irreális (nem is esik a Föld közelébe), hogy a végeredmény egyértelmű (3 egyenlet X, Y, Z-re). A valóságban általában egy negyedik mérés is szükséges a fogadóeszköz órajelében rejlő hiba kiküszöbölésére (4 egyenlet X, Y, Z, T -re). Ha egy koordináta (pl. magasság) már a mérés előtt is ismert, 3 mérés természetesen elegendő.

29. ábra GPS alapú térinformatikai helyzetmeghatározás

12

medium Earth orbit

37

A mélyebb megértés érdekében nézzük meg magukat a továbbított jeleket. A mai szatelliták az L-sávban sugároznak. Egy leküldött jel a vivőhullámból (S és S ), valamint az erre modulált kódolt infomrációból (C/A, P és D) áll. Mivel ezt a két frekvenciát nagyon sok adó használja egyszerre, a szimultán érkező jeleket meg kell különböztetni. Az elkülönítés lehet idő (TDMA), frekvencia (FDMA) vagy kód (CDMA) alapú13. Előbbi két technológia idő- vagy frekvencia alapon osztott csatornát használ míg utóbbi esetén minden műholdhoz saját PRN (pszeudorandom) kódot rendelnek. A C/A és P kódok úgynevezett PRN kódok: úgy néznek ki, mintha 1-esek és 0-ák teljesen random, zajszerű kombinációjából keletkeznének, noha a valóságban komoly szabályok alapján generálják őket. Ez a generálási kód minden NAVSTAR GPS szatellitán egyedi. Míg a NAVSTAR rendszerben tehát minden műhold konstans (S elsődleges, S másodlagos) frekvenciákon sugároz, és egyedi kódolásuk teszi megkülönböztethetővé őket (CDMA), addig más rendszerek, például a GLONASS ugyanolyan kódolást alkalmazó műholdakat használnak eltérő frekvencián (FDMA). A C/A kód hullámhossza körülbelül 300 méter, és csak az L1 frekvencián érhető el, ami civil felhasználásra is elérhető. A P(Y) kód nagyobb frekvenciájú (hullámhossza körülbelül 30 méter), és az L1 és L2 frekvenciákon is elérhető. Pontossága miatt viszont titkosított, csak az USA hadserege és egyéb NATO országok férhetnek hozzá. A C/A és P kódot távolságmérésre használják. Előbbit szokás SPS-ként (Standard Positioning System), utóbbit PSP-ként (Precise Positioning System) emlegetni. A D kód navigációs- és rendszer adatokat továbbít magáról a műholdról, mely további korrigálásra használható. Mivel sokkal kisebb frekvenciájú, mint társai, azoktól könnyen elkülöníthető.

30. ábra Modulált hullámok fajtái

13

time/frequency/code division multiple access

38

A fentieket felhasználva két fajta GPS-es távolságmérési módszer létezik: mindkettő a C/A vagy P14 elektromágneses jelek mérésén alapszik. Az első gyorsabb, de pontatlanabb. A legtöbb GPS vevőkészülék az első módszert használja - két jel összehasonlításából számít távolságot. Minden műhold (elvileg ismert időben) leküld egy kódolt jelet. Ezt generálja le (ebben az ismert időben) maga a készülék is – majd bevárja annak műholdról leérkező mását. A beérkezett jel időben való visszaléptetésével kiszámítható a két időpont között eltelt idő, s innen – a fény sebességével szorozva – a köztes távolság. Egyenlőség akkor áll fenn, ha a két jel bitjeinek szorzata 1 a teljes üzenet hosszán (C/A esetén ez 1023 bit). Ezt nevezik ‘kódmérésnek’, vagy ‘pszeudo-távolságmérésnek’.

31. ábra Kódmérés

A kódok egyidejű generálása a GPS idők összehangolását teszi szükségessé. Ennek érdekében minden NAVSTAR GPS műhold fedélzetén található 4 beépített atomóra – ezek egy közös GPS időre (GPST) vannak szinkronizálva. Ez a koordinált világidővel (UTC) szemben nem veszi figyelembe a Föld forgását, a világidővel 1980ban egyeztették, azóta elcsúszott. Adott időközönként ugyan korrigálják a műholdakat a lehető legjobb egyezésért, mégis minden egyes fedélzeti óra saját időhibát (T ) produkál. A GPS idő pontos számítására ezért külön földi mérőállomásokat hoztak létre, ezek folyamatosan korrekciós paraméterekkel látják el az egyes műholdakat. Az adott műhold paraméterei kiolvashatók a műhold által közvetített GPS jelekből: minden üzenet első egységéből, 30 másodpercenként. Ez Mivel egyhamar nem dolgozhatunk P kóddal (pontossága nem kihasználható), ezentúl csak a C/A-t említem meg. 14

39

alapján a műholdak órahibája pontosan kiszámítható, majd ismert értékként kezelhető. A fogadó eszközökbe viszont nem túl gazdaságos atomórákat szerelni– ezekben egy-egy kvarc oszcillátor méri az eltelt időt. A műholdakban használt órák még a korrekciós paraméterek nélkül is mintegy 14 nanoszekundumos tartományig pontosak, utóbbiak viszont 100 nanoszekundumos (T ) hibát véthetnek. Ezen időhibák miatt a gyakorlatban szinte soha nem keletkezik egyszerre a műhold által küldött és a fogadóeszköz által reprodukált jel. Ezért szükséges X, Y, Z mellett egy T paraméter (egy negyedik műhold) figyelembe vétele a pozíció meghatározásoknál még akkor is, ha az aktuális idő különösképpen nem releváns.

32. ábra Késleltetett kódmérés

A feladás és fogadás között eltelt idő alapján meghatározható tehát a két objektum távolsága (ahol a műhold pozícióját ismertnek tekintjük). |V, W, X !| Y3 Y3 YT T |V, W, X !| Z W X

ahol

V, W, X a vevő ismeretlen pozíciója

! a műhold ismert pozíciója (D kódban közvetített információ) 3′ és 3 az érzékelt és valódi időkülönbség 40

C ௦௔௧ és C ௥௘௖ a műhold és fogadó órahibája ' a fénysebesség (' ≅ 300 000 000 pedig a pszeudotávolság

௠ ௦

)

Ez tehát a pszeudo-távolságmérés alapegyenlete, ahol D, E, F és C ௥௘௖ négy műhold bemérése alapján meghatározható. A teljes, egyéb hibákat is tartalmazó alak pedig: = |D, E, F − | = ' + 'C ௦௔௧ − C ௥௘௖ + + ௜௢௡ + ௧௥௢௣ + B()

ahol

a műhold pálya hibája

௜௢௡ és ௧௥௢௣ az ionoszféra és troposzféra okozta késleltetés (Gaussian)

B() az adatgyűjtést terhelő, és a visszaverődő hullámok okozta zaj

A légkör késleltető hatása majdhogynem megjósolhatatlan. A troposzféra okozta hiba függ a hőmérséklettől, nyomástól, páratartalomtól és számos egyéb tényezőtől. A nagyobb hibát viszont – részben mert a réteg maga is szélesebb – ௜௢௡ okozza. Az ionoszferikus késleltetés a jel frekvenciájának és a légkörben található elektronok számának15 egy jól definiált függvénye. Mivel mind az L1, mind az L2 vivőhullámok késleltetettek, a kettő beérkezési idejének különbségéből (ami csak az ismert f୐ଵ , f୐ଶ és az ismeretlen TEC paraméterektől függ) ௜௢௡ kiszámítható. Ha a két frekvencia közül csak az egyik elérhető (L1, civil felhasználás), a műholdak által alapból szolgáltatott ionoszferikus légköri paraméterek is felhasználhatók. Precízebb mérésekre az utóbbi, hibákat is tartalmazó alak használatos. Azonban a kódmérés még így is pontatlannak számít, mivel maguk a C/A ciklusok túl szélesek. A pontosság egyetlenegy bit szállítási idejétől függ, ez kb. 1 ms. Ennek méterben kifejezett értéke (chip rate ≈ 300 m) adja a pontosság százszorosát. Ebből adódóan a kódmérés kb. méteres pontosságú.

15

total electron content, TEC 41

Ennél pontosabb távolságmérési módszer a ‘fázismérés’. Ekkor a leküldött jel vivőhullámának fázisát mérjük – az utolsó, befejezetlen szakasz hosszát a teljes 19 centiméteres hullámhosszhoz képest. Az egyetlen probléma ezzel a technikával csak az, hogy a maradék teljes hullámhosszak száma alapból nem meghatározható. Ezt szokás ‘carrier phase ambiguity’-nek, magyarul ciklustöbbértelműségnek hívni. Ha ez feloldható, a módszer az előbbinél sokkal pontosabb eredményeket biztosít.

33. ábra Vivőhullám

Ideális esetben a számolt távolság megegyezik a pszeudotávolsággal. |V, W, X !| A[ \[

|V, W, X !| Z W X

ahol A értéke ismeretlen. Nem ideális műhold és kvarc oszcillátor esetén viszont az egyenlet az alábbi formát ölti.

34. ábra Fázismérés

42

= |D, E, F − | = HI + JI + 'C ௦௔௧ − C ௥௘௖

Az összes, kódméréssel kapcsolatban említett hiba itt is figyelembe vehető. A fenti egyenletben H ismeretlenként jelenik meg. Egy nyugalomban hagyott vevőkészülék segíthet ezen a problémán. Ha a műszer mozdulatlanul marad, míg a műhold kering tovább a pályáján, a mért adat idővel biztos, hogy megváltozik. Ha figyeli és rögzíti ezeket a változásokat – észrevéve az új J fázisértékeket valamint a teljes hullámhosszak számának H változását – H adott számú mérés után kiszámítható. K(L − ௜ )ଶ + (M − ௜ )ଶ + (N − ௜ )ଶ = (O࢏ + H௜௝ + J௜௝ )I + ' PC ௦௔௧ ௜ − Q࢘ࢋࢉ ࢐ R ௝

ahol 2 az egyes műholdak azonosító számát, S az egy adott műholdon elvégzett mérések számát jelenti. A vevőkészülék órahibája a S-edik pillanatban C ௥௘௖ ௝ . Az ismeretlenek közül L, M, N, O࢏ az újabb mérésekkel is változatlan marad, egyedül a készülék Q࢘ࢋࢉ ࢐ órahibája változik. Tételezzük fel, hogy egyszerre négy szatellitát mérünk. Ez az egyenletek és ismeretlenek szempontjából a következőt jelenti. 40 @* @0.@

3(L, M, N) + 4(O࢏ ) + 0(Q࢘ࢋࢉ ࢐) 2T@@.@0@

Mikor 0 = 3, 12 egyenletünk lesz, akkorra már csak 10 ismeretlenre.

Ez a módszer feltételezte, hogy a GPS modul nem mozog (statikus mérés). Egy quadrotor esetében ez szinte lehetetlen, ott kinematikus mérésekről beszélünk. A gyakorlatban a fázismérést általában két fogadóeszközzel végzik el. Abszolút pozíciómeghatározás helyett relatív (egymáshoz képesti) pozíciót határozunk meg.

43

A vevőkészülékek közötti egyszeres különbségek módszere a következő. Mérjük meg a két készülék pozícióját ugyanazon műholdakkal. A két hely közül az egyik ismert (lehet például egy földi adóállomás – különbségi GPS, vagy szimplán kódméréssel meghatározott).

35. ábra Egyszeres különbségek

]F^",# G F_",# G F`",# G

,!

és ! pozíciók különbsége

^" , _" , `" ! ! , W! , X! a",$ A!, \

ahol ! ismert.

,

\!, [ YT ! b%&' "

c c( L? LcL3 3^" , _" , `" c a",$ c( b%&' " &e:L!L3Lc!L

c( 2 mérés c 5 műholdra 2 ∗ 5 3 5 2 10 elegendő. Fontos, hogy a módszer eltüntette a műhold órájának pontatlanságát.

44

A kettős különbségek módszere hasonló. Egy ismeretlen és ismert pozíciójú eszköz koordinátáit két műholddal is lemérjük, ezekből először egyszeres különbségeket képzünk az egyes műholdak adataira, majd a két műhold egyszeres különbségeit is kivonjuk egymásból.

36. ábra Kétszeres különbségek módszere

^" , _" , `" ^" , _" , `" ! ! , W! , X! ! ! , W! , X! a)" a*" A! A! \ \ \! \! [

ahol ! ismert. Itt már kiesett a fogadóeszköz órahibája is.

c 1 c( L? LcL3 3^, _, ` c 1 a+" a+* " &e:L!L3Lc!L

c( 2 mérés c 4 szatellitára elvégezve 4 1 ∗ 2 3 4 1 6 elegendő. Létezik pár egyéb megoldás is a többértelműség megszüntetésére. Most nézzük meg, miért is hasznos ez egyáltalán. A kereskedelemben kapható GPS modulok már alapból szolgáltatnak egy longitude–latitude–altitude értéket, ezért a GPS rendszerek felszínes ismerete általában elegendő az ilyen koordinátákkal megadott pályák lekövetésére. Pontosabb számításokhoz viszont ez a mélyebb ismeret szükséges. Ilyen módszerek nélkül a mai korszerű, gyakran centiméter-milliméter pontosságot igénylő orientációmeghatározási feladatok lehetetlenek lennének.

45

4.2 GPS ALAPÚ ORIENTÁCIÓ MEGHATÁROZÁS Egy szerkezet orientációja a test ’body’ koordinátarendszerének térbeli elfordulása a ’vehicle’ rendszerhez képest. Ez minimum három pont térbeli koordinátáinak meghatározása alapján következtethető ki. A hagyományos (6 vagy 916 tengelyes IMU alapú, gyrocompass-t használó, ...) orientáció meghatározásokon kívül a címbeli feladatra használhatunk többantennás GPS rendszert is. A pontos méréshez először egy fix origó meghatározása szükséges: ez lehet a (pl. kódméréssel meghatározott) főantenna pozíciója. Ezután következik (legalább) két másik antenna relatív pozíciójának pontos, fázisméréssel történő kiszámítása. A ciklustöbbértelműséget a föntiekben ismertetett módszerekkel oldhatjuk fel..

37. ábra GPS alapú orientáció meghatározás

Határozzuk meg az A1 (főantenna) és A2,3 (mellékantennák) közötti vektorokat GPS koordinátáik alapján. Ezen vektorok az antennák koordinátarendszerében adottak (kék). Minden antenna csak a saját látóteréből tud adatot gyűjteni. Emiatt forgó űrhajóknál, vagy egy folytonosan manőverező quadrotornál érdemes különböző irányban elhelyezett antennákat használni az állandó lefedettség érdekében. Az antennáknál megadott koordinátákat – az egyes antennák hossza és dőlésszöge ismeretében – át kell transzformálni a szerkezet ’body’ koordinátarendszerébe (piros). A fenti ábrán ezek az áttranszformált vektorok az egyszerűség kedvéért máris a szerkezet ortogonális bázisvektorait adják , 17. A

16 17

+3 tengely mágneses mérés Innen ௕ már könnyen meghatározható

46

‘vehicle’ rendszer (௩ , ௩ , ௩ ) bázisaival összehasonlítva őket az ௩ és Ϝ௕ közötti rotációs paraméterei meghatározhatók18. Az elmúlt évtizedekben számos finomabb megoldást dolgoztak ki a probléma matematikai, mérnöki megoldására – legtöbbjük redundáns antennákat használ. További részletek a [4] [5] [6] forrásokban olvashatók.

Az A3 antenna a valóságban A1-re tükrözve adná ki ‫ݔ‬௕ ≅ ࡭૚࡭૜ megfelelőjét. Pusztán a jobb láthatóság miatt került át fölülre. 18

47

4.3 RECEDING HORIZON CONTROL Mi történik, ha nem tudjuk, hol vagyunk? A GPS által lefedetlen területeken előtérbe kerül az autonóm viselkedés. Míg a pályát szolgáltató GPS adatok újra elérhetővé nem válnak, a robotnak magának kell megterveznie az optimális, legbiztonságosabb cél felé vezető utat. Ilyenkor lehet az úgynevezett ‘prediktív’ vagy ‘csúszó időhorizont’ (eredeti szövegekben Model Predictive Control, MPC vagy Receding Horizon Control RHC, RHMPC néven ismert) szabályozási struktúrára támaszkodni. A módszer alapja ismétlődő, véges időintervallumokon vett optimalizálás. Egy 3 időpillanatban a jármű mintavételezi környezetét, majd ismerve saját dinamikáját és céljait, számít egy optimumot a következő [3, 3 kl közeli időtartományra. Ezután végrehajtja ezen optimális stratégia első lépését majd újraszámolja a feladatot az új helyen begyűjtött paraméterek segítségével, egy új, ideiglenes optimumot generálva így. Az idősáv, melyre jóslunk, folyamatosan tolódik előre az időben - emiatt használatos a Receding Horizon Control elnevezés. Gyakran alkalmazott módszer ez röppályák on-the-fly módon történő meghatározására. Mivel ilyen számítási algoritmusok ezreit kellhet végrehajtani rövid időn belül, fontos, hogy azok könnyen számíthatók legyenek. Ma is komoly kutatási terület ilyen gyors, processzorkímélő megoldások találása. Látható, hogy nem ez a módszer a legtökéletesebb, de gyakorlatban való alkalmazhatósága és eredményessége miatt egyre elterjedtebb.

38. ábra Model Predictive Control

A Receding Horizon Control jól alkalmazható például sűrű - GPS által lefedetlen erdei területeken való tájékozódásra. Egy elülső kamera által szolgáltatott képből kellően fejlett képfeldolgozási módszerekkel - jól meghatározható a quadrotor elé kerülő fák és egyéb akadályok pozíciója. Ilyen feladatra optimalizált PD kontroller látható a [7] poszteren. 48

5. KÉTROTOROS RENDSZER Visszatérve a konkrét szabályzásra, a szimulációk tapasztalatait a gyakorlatban is megpróbáltam megvizsgálni. A quadrotor esetében egy roll/pitch elfordulás ideálisan csak két szemben lévő motort használ. Ez a mozgás közelíthető egy fix tengely körül forgó 1 szabadságfokú libikóka adott szögértékre való szabályozásával. A korábban látott quadrotor szimulációs eredmények az itt megépített, valós hardver adataival készültek el. jelentős különbség, hogy ez egy könnyebben kezelhető eset: itt nem lengethetik be egymást a különböző mozgások, és nem mozdul el a hardver a vízszintes erőkomponenstől. A további stabilizálás érdekében a kétrotoros rendszer propellereit úgy helyeztem a motorokra, hogy azok lefelé ható erőt fejtsenek ki, biztonságosan a földhöz rögzítve így a vázat. Az egyensúlyozó rendszer “agya” egy Arduino Mega 2560 mikrokontroller, ez két 6 tengelyes szenzor kiátlagolt adata alapján változtatja a motorok fordulatszámát. A rendszer fő célja egy adott szögértékre való pontos és gyors beállás volt.

39. ábra Libikóka rajza

49

A rendszer modellezésre került folytonos, és diszkrét időtartományban is. Utóbbi fontossága – tekintve, hogy digitális szabályzónk van – egyértelmű.

40. ábra Folytonos idejű rendszermodell

A diszkrét modellezéshez nulladrendű tartószervet használtam (me

షೞ೓

,

ahol a rendszer ciklusideje n 11 :e volt. A libikóka szabályzó jele eszerint csak 0,011 e-onként cserélődik, két loop között konstansnak tekinthető. Mivel a szögértéket szolgáltató szenzor ehhez képest elég nagy frekvencián tud IIC-n kommunikálni, a mintavételezés minden loopban új adatot szolgáltat. A két nulladrendű tartószerv megegyező ciklusidővel került modellezésre. S,,-( 400 om; S,,- 100 om

41. ábra Diszkrét idejű rendszermodell

50

A korábban megkapott differenciálegyenletünkből nullázzuk le az itt értelmet vesztő másik két Euler szöget, és azok deriváltjait. Mivel csak két motorról van szó, a továbbiakban az eddigi 4-es jelűt 1-esre cseréltem. A kétrotoros rendszert ekkor a következő differenciálegyenlet írja le: U௦ - = .ଵ () − .ଶ ()

ahol az eddigi jelöléseket alkalmazva () képviseli a roll szöget (lehetne pitch is, a szenzorok aktuális orientációja alapján ez éppen roll), U௦ pedig a tehetetlenségi nyomaték -et. Az általam implemetált szabályzó a szenzorokból visszaolvasott szögérték alapján kalkulál egy :=() jelet.

A PID szabályozó kimenete az egyes motorok feszültség bemenete lesz. Innen maga a felhajtóerő: ;ଵ = ;௕௔௦௘ଵ + ;௉ூ஽ሺఝሻ

;ଶ = ;௕௔௦௘ଶ − ;௉ூ஽ሺఝሻ

U௦ - = .ଵ − .ଶ = .& (

12000 ଶ ) 7;ଵ ଶ − ;ଶ ଶ 8 255

Feltételezzük, hogy a két motor egyenlő alapjelet ad ki, és csak ehhez képest változtatjuk őket. Lebegéskor ez a gravitációnak ellen tartó erő fele (4 motor esetén negyede) lenne. Ekkor a keletkező nyomaték csak az erők változásától függ: ܷଵ ଶ − ܷଶ ଶ = ൫ܷ௕௔௦௘ଵ ଶ + 2ܷ௕௔௦௘ଵ ܷ௉ூ஽ሺఝሻ + ܷ௉ூ஽ሺఝሻ ଶ ൯ − ൫ܷ௕௔௦௘ଶ ଶ − 2ܷ௕௔௦௘ଶ ܷ௉ூ஽ሺఝሻ + ܷ௉ூ஽ሺఝሻ ଶ ൯

;ଵ ଶ − ;ଶ ଶ = 4;௕௔௦௘ ;௉ூ஽ሺఝሻ

255 ଶ U௦ ( ) - = ;௉ூ஽ሺఝሻ 4.&;௕௔௦௘ 12000 Maga PID jel a szögérték hiba alapján ఝ = ௥௘௙ − ()

:=() = : ఝ + = ఝ () + > ఝ ?? ௧

௧బ

51

Egy nullára való szabályzáskor például (lebegés) q 3 0, így a fönti egyenlet a következőre egyszerűsödik: 456q 4 q3 6 q 3 5 7 q8 8

బ

rq; 3 4 q3 6 q 3 5 7 q8 8

ahol r /0ೞ .

್ೌೞ೐

M

11

N

బ

Az így felírt rendszer karakterisztikus polinomját Routh-Hurwitz kritériummal vizsgálva belőhetünk egy leszűkített tartományt a megfelelő paraméterekre: 5 r e 6e 4 0 e r; 6; 4; 5 . 0

6 m #r 0

5 4 6

0 46 .5 0% → r 5

42. ábra Paraméterek lehetséges tartománya

52

5.1 ELEKTRONIKA

43. ábra

44. ábra

Motorok és motorkontrollerek

Szenzorok

A projekt 4 darab megegyező A2212/13T brushless DC motort (1000KV voltage-to-rpm ratio) használ fel. Mindegyiket egy-egy 30 amperes ESC irányítja, melyek közvetlenül az Arduino-tól kapnak vezérlőjelet.

Kettő 6 DoF MPU6050 szenzor kiátlagolt adatai adják a megfelelő gyorsulás és szögsebesség értékeket. Emellett mindkettő képes a hőmérséklet mérése. IIC kommunikáción kapnak egyedi címet (AD0 regiszter HIGH vagy LOW) az Arduino-tól, illetve itt küldik vissza a mért értékeket is.

Tápellátás

A fő tápellátást egy három cellás 3000 mAh lítium-polimer akkumulátor biztosítja. Innen egy főkapcsolón keresztül jut áram az ESC-khez. Egy 9 Voltos feszültségszabályozó segítségével USB-n kívül innen is lehet tápolni az Arduino-t. A többi kisfeszültségű hardver az Arduino 5 Voltos pinjéből kap tápot.

46. ábra

45. ábra

Propellerek

A motorokra egy pár 10x4.5 propeller lett felszerelve. Az első szám az átmérőt jelöli inchben, míg a második a pitchet – mennyit haladna előre a propeller egy egyszerű furatban terhelés nélkül.

Mikrokontroller

A rendszer “agya” egy Arduino Mega 2560 board. A pinek közül a szabályozott 5 Voltos kimenet, egyéb digitális lábak, és az IIC busz került felhasználásra. 53

5.2

ORIENTÁCIÓ MEGHATÁROZÁS

Az orientáció meghatározáshoz kettő darab hattengelyes IMU (Inertial Measurment Unit) adatait átlagoltam ki. Három tengely gyorsulást, három tengely szögsebességet mér, míg hetedik adatként bekérem a szenzor hőmérsékletét is. Utóbbi hosszútávon használható túlmelegedés elleni védelemre. 5.2.1

SZENZOROK KALIBRÁLÁSA

Mint minden szenzor, a gyorsulásmérő és giroszkóp is rendelkezik egy saját karakterisztikával (offset, gain – utóbbi ideálisan 1).

47. ábra Szenzor karakterisztika

r203 r2 r4 ∗ r56

r2 és r4 meghatározásához ? és? környezetben végeztem méréseket.

r203, r2 r4 ∗ ? r203, r2 r4 ∗ ? r203, r203, 2 r203, r203, 2

r2 r4

Ezt mindhárom tengelyre külön elvégezve a valós értékünk végezetül r56

r203 r2 /r4

Ezt az int16_t értéket szükséges még átskálázni a megfelelő tartományba (esetünkben 2? … 2?, de ez konfigurálható), így előáll a használható gyorsulás adat.

54

A giroszkópok kalibrálása máshogy zajlott.

ை௎் ை௙௙௦௘௧ ீ௔௜௡ ∗ ோா஺௅

Állandósult állapotban a szenzor u203 0 értéket kell visszaadjon. Ha a hardvert egy helyben hagyva megmérjük a szenzor első A jelét, ezek átlaga kiadja u2 -et. Adódik, hogy így a három giro tengely offset-e minden felszállás előtt újra kalibrálható – ez hasznos is a különböző véletlenszerű eltolódások, hőmérsékleti hibák kiküszöbölésére. Repülés közbeni vész-újraindításra most nem tervezünk. Mivel a gyorsulásmérők legalább 1 tengelye alaphelyzetben is szolgáltat valamilyen nem nulla (gravitáció komponens) adatot, hasonló kalibrálás esetükben nem végezhető. u4 értékét a következőképp határoztam meg:

1) A hardver állandósult állapotban van (u2 mérése). 2) Forgassuk el a szenzort egy ismert szöggel (pl. 90°). Integráljuk az így kapott u203 u2 értékeket. u4,7°

x u203 u2 3

90°

3) Várjunk egy keveset megint mozdulatlanul. 4) Ismételjük meg az 2) lépést ellentétes irányba. u4,7°

x u203 u2 3

90°

5) Újabb mozdulatlan állapot (leállás). 6) Átlagoljuk ki a két ellentétes irányra kapott meredekség értéket. u4

u4,7° u4,7° 2

48. ábra Giroszkópok kalibrálása

Ez szintén elvégzendő mindhárom tengelyre külön-külön. Végül a helyes adat: u56

u203 u2 /r4

majd átskálázás után (int16_t → 250°/eLY … 250°/eLY) használhatjuk is.

55

5.2.2

JELFELDOLGOZÁS

Ebben a konfigurációban a gyorsulás adatok a gravitáció ? vektorának adott komponensét képviselik, így közvetlenül használhatók az aktuális orientáció meghatározására. q q

49. ábra Szögmérés

Y viszonylag haszontalan adat, nem függ q irányától, és X-nél kevésbé érzékeny annak változására. A pozíció közvetlen meghatározható az ? ∙ e&cq egyenletből egy arcsin függvénnyel. A trigonometrikus függvények használata azonban terheli a processzort és nemlineáris, így lehetőség szerint elkerülendő. Kis szögek linearizálhatók sin q ≅ q közelítéssel. Nagyobb értékekre look-up table használata ajánlott. Esetünkben a hardver súlypontja nem mozdult el, a két gyorsulásmérő szenzor pedig ehhez a lehető legközelebb, szimmetrikusan lett felhelyezve. Mozgó test esetén a rendszer teljes gyorsulása is megjelenik a szenzorok kimenetein. Ezt (pl. a bemenőjelekből számított erők nagyságából és irányából) becsülni szükséges.

A giroszkópok szögsebesség értéket szolgáltatnak, ezek integrálja visszaadja q-t. Első ránézésre a pozíció meghatározása egy túlhatározott feladatnak tűnhet, de a valóságban nem ez a helyzet. A gyorsulásmérők hosszútávon pontos, de rövidtávon zajos jeleket szolgáltatnak. Egy giroszkóp ezzel ellentétben a gyors változásokra reagál jól, és szépen lassan kúszik el.

A gyorsulásmérők jeleit egyszerűen átengedhetjük egy aluláteresztő szűrőn. Digitális szabályzás esetén ez a következő digitális szűrőt jelenti (0.8 és 0.2 példa értékek). c?L 0.8 ∗ c?L 0.2 ∗ c?L_YYL

Az egyszerű lekódolhatóság vonzó, de ez önmagában nem oldja meg a problémát, a gyors változásokat csak időkéséssel tudnánk lereagálni. Egy komplex megoldás a két szenzor adatainak közös felhasználását jelenti, előnyeik szinergikus kihasználásával, hibáik megfelelő kiszűrésével. A leggyakoribb ilyen szűrő a

56

Kálmán-szűrő és a Complementary Filter. A Complementary Filter koncepciója a következő: c?L ∗ Fc?L c?L ∗ 3G 1 ∗ c?L_YYL

ahol : 0.98 vagy más, 1-hez közeli érték. 1 így igen kicsi lesz, mely megfeleltethető egy, az előbbiekben is látott aluláteresztő (zaj) szűrőnek. Az első tag a giroszkóp elcsúszását szűri ki. a következők szerint meghatározandó: 8 8 3

ahol 3 a kód mintavételezési ideje, 8 a használt időállandó. Aluláteresztő szűrőnél például az időállandónál hosszabb jelek átjutnak, a rövidebbek kiszűrődnek. A tervezés során minél kisebbre vesszük 8 értékét, annál több gyro driftet zárunk ki, de annál több gyorsulást terhelő zajt is engedünk be. Ez egy kompromisszum szituáció, ahol az optimalizálás maga a kísérletezés. A Kálmán-filter egy igen jó dinamikai modell segítségével ad becslést a két mérés közötti állapotra. Komplikáltabb, de jobban specializálható egy konkrét modellre, és sokszor precízebb megoldást is szolgáltat. Ez esetben a ‘Complementary Filter’-t használtam egyszerű kódolhatósága miatt. Megoldotta a zaj és drift problémát, és a paraméterek megfelelő hangolása után gyors és megbízható adatokat szolgáltatott.

50. ábra Complementary Filter

A fenti ábra egy valós esetet ábrázol az implementált szűrőre: jól látszik hogy a gyorsulásmérő (zöld) zajos, míg a giroszkóp (piros) idővel elcsúszik; a kettő kombinációja viszont kellően pontosnak bizonyult az egész időtartamra. 57

5.3 GÉPÉSZETI MEGVALÓSÍTÁS

51. ábra

58

5.4

EREDMÉNYEK

Az alábbi kép a magára hagyott rendszer stabilizálás közben visszaküldött szenzorjeleit ábrázolja. A libikóka 27° közötti mozgásra képes.

52. ábra Stabilizálás

Kinagyítva jól látszik, hogy a hardver tizedfokos pontossággal képes autonóm módon stabilizálni saját magát.

53. ábra Pozíció közelről

59

A rendszer reakcióján jól megfigyelhető a szabályozó különböző tagjainak hatása19. Az alábbi két ábrán a libikókát szélső helyzetéből (kb. 27°) kibillentve engedtem szabadon. Az első eset nagyobb P és kisebb D paraméterű szabályozót használt.20

54. ábra Lassabb beállás – nagy P, kis D

Ugyanezt a kiindulási helyzetet alacsonyabb P és nagyobb D értékekkel gyakorlatilag rögtön sikerült stabilizálni, jelentősebb lengések nélkül:

55. ábra Behangolt beállás – kisebb P, nagyobb D

19 20

A paraméterek behangoláshoz Ziegler-Nichols módszert alkalmaztam. Az I tag egy időben egyre később tolódó fix tartományon (pl. az 50 utolsó hibaértéket) integrál.

60

A következő képek egy-egy nem nulla alapjelre való beállást szemléltetnek:

56. ábra +15°-os alapjelre való beállás

57. ábra -15°-os alapjelre való beállás

61

Végezetül így néz ki a hardver reakciója abban a szélsőséges esetben, mikor az egyik szélső helyzetből majdnem teljesen a másik oldalra kell beállnia.

58. ábra +27°-os szélsőhelyzetből való beállás -20 °-os alapjelre

62

6

ÖSSZEFOGLALÁS

Dolgozatom során létrehoztam egy – a quadrotorok mozgásának modellezésére szolgáló – szimulációs környezetet, irodalomkutatást végeztem a különféle navigációs módszerekről, végezetül megépítettem és beszabályoztam egy – az elméleti modell elemét képező – egyensúlyozó rendszert. A szimuláció és a valódi hardver kapcsán kezdetben tapasztalt kétféle megközelítési mód szépen lassan összeért. A feladat elmélyítette a többrotoros rendszerek kapcsán megszerzett tudásomat. További rövid távú tervem egy – a PID szabályozótól eltérő – nemlineáris algoritmus kifejlesztése pályakövetési célokra, összehasonlításképp az itt látottakkal. Hosszabb távú cél pedig az így nyert tapasztalatok alapján egy teljes quadrotor megépítése. A projekt mindhárom része segített közelebb kerülni ezen célokhoz, és jó alapul szolgált a további kutatásokhoz.

63

7

HIVATKOZÁSOK [1]

O. K. Bruno Siciliano, Handbook of Robotics, Springer.

[2]

R. W. Beard, Quadrotor Dynamics and Control, Brigham Young University, February 19, 2008.

[3]

S. L. W. C. J. T. Gabriel M. Hoffmann, Quadrotor Helicopter Trajectory Tracking Control.

[4]

G. Lu, Development of a GPS Multi-Antenna System for Attitude Determination, University of Calgary, January 1995.

[5]

J. B. Schleppe, Development of a Real-Time Attitude System Using a Quaternion Parameterization and Non-Dedicated GPS Receivers, University of Calgary, July 1996.

[6]

J. C. Adams, Robust GPS Attitude Determination for Spacecraft, Stanford University, December 1999.

[7]

M. T. A. A. D. B. Sam Zeng, Trajectory Following in GPS Denied Environments for UAVs using Receding Horizon Control, RISS Carnegie Mellon University, 2014.

[8] [9] [10] [11]

S. Colton, The Balance Filter, MIT, June 25, 2007. R. Gergely, Autonóm működésű négyrotoros algoritmusainak összehasonlítása, BUTE, 2009.

helikopterirányítási

L. R. S. M. K. A. Landau I.D., Adaptive Control, Springer. T. Gergely, Quadrotor mechanikai modellje, a modell numerikus szimulációja, BUTE, 2012.

64

8

MELLÉKLET

MELLÉKLET A Quadrotor szimuláció Matlab kódja MELLÉKLET B Kétrotoros libikóka Matlab kódja MELLÉKLET C Kétrotoros libikóka Arduino kódja

65

MELLÉKLET A

%MAIN FUNCTION OF QUADROTOR SIMULATION clc clear all close all %Initialize all the parameters global Par; Par = Param(); %Run simulation Sol = SimRK4(); %Plot and animate results figures_one( 1, Sol ); figures(Sol); animate(Sol);

%SIMULATION PARAMETERS function Par = Param(

)

%Step size Par.h = 0.01; %Number of iterations Par.imax = 500; %PHYSICAL PARAMETERS %Gravity vector Par.g = 9.81; %Distance between two opposite Motors Par.L = 0.365; %Propellers Par.mProp = 0.011; Par.lProp = 0.254; %Motors Par.mMotor = 0.05; Par.dMotor = 0.028; Par.hMotor = 0.024; %Propeller fixations Par.mFixation = 0.004; Par.dFixation = 0.01; Par.hFixation = 0.018; %The 2 perpedincular Arms Par.mBeam = 0.628 - 2*(Par.mProp + Par.mMotor + Par. mFixation); Par.dBeam = 0.509; Par.wBeam = 0.062; Par.hBeam = 0.0205; %Total mass Par.m = 2*Par.mBeam + 4*(Par.mProp + Par.mMotor + Par. mFixation); %Par.b = 1.57*10^(-7);

Par.b = 1.57*10^(-5); Par.d = 1.1*10^(-6);

%ELECTRONICAL PARAMETERS %ideal maximum RPM Par.BldcPowerSupply = 12; %Volts Par.BldcKV = 1000; %rpm/V Par.RpmMax = Par.BldcKV * Par.BldcPowerSupply; %Ubase for hovering FBase = (1/4) * Par.m * Par.g; Par.PWMBase = sqrt(FBase / Par.b) / Par.RpmMax * 255; Par.F = [FBase FBase FBase FBase];

%GEOMETRY PARAMETERS %initial frame base vectors Par.ei = [1 0 0; 0 1 0; 0 0 1]; Par.eix = Par.ei(1, :); Par.eiy = Par.ei(2, :); Par.eiz = Par.ei(3, :); %Path coordinates Par.path = [ 0 0 0 0 0 0; 0 0 1 0 0 0; 0 0 2 0 0 0; 1 0 3 0 0 0]; %Index corresponding for the actual target point Par.pathN = 1; %Path tracking in X, Y directions uses separate 'ranges', 'borders' for %control. Before reaching the target, a braking period occurs. In order not %to destroy the meaning of this braking with PID control, the system %waits until the hardware moves to the other side of the target, then %immediately starts correcting its position. These flags are used to store %the event of crossing the target's border.

Par.pathFlagx = 0; Par.pathFlagy = 0;

%CONTROLLER PARAMETERS %Target vector containing the current target point's parameters (~Nth Path point) Par.target = [ Par.path(Par.pathN,1); Par.path(Par.pathN, 2); Par.path(Par.pathN,3);... Par.path(Par.pathN,4); Par.path(Par.pathN, 5); Par.path(Par.pathN,6)]; %PID parameters for X, Y, Z, roll, pitch, yaw control Par.Kp = [0.5 0.5 6 35 35 3]; Par.Ki = [0 0 0 0 0 0]; Par.Kd = [122 122 3000 800 800 300]; %1 0 2 xy old %ICs - center of %Par.x0 = [x; % roll, Par.x0 = [0; 0;

end

mass coordinates and Euler dx=vx; y; dy=vy; z; droll, pitch, dpitch, yaw, 0; 0; 0; 0; 0; 0; 0; 0;

angles dz=vz;... dyaw]; 0;... 0];

%RUNGE-KUTTA METHOD FOR SOLVING THE 12 SECOND ORDER DIFFERENTIAL EQUATION %FOR THE SYSTEM function Sol = SimRK4( ) global Par; h = Par.h; imax = Par.imax; %Number of variables n = size(Par.x0, 1); %Simulation time vector Sol.t = h * (0:1:imax); %0 matrices as initialization Sol.x = zeros(n, imax+1); Sol.F = zeros(4, imax+1); %Initialization of x and F with the ICs x = Par.x0; Sol.x(:,1) = x; Sol.F(:,1) = Par.F(); waitbar_h = waitbar(0,'Simulation is running...'); for i=2:1:imax+1 t=Sol.t(1,i); [k1, [k2, [k3, [k4,

f1] f2] f3] f4]

= = = =

System(x, t); System(x + h*k1/2, t + h/2); System(x + h*k2/2, t + h/2); System(x + h*k3 , t + h);

%Calculate the exact next value based on the average of the 4 separate %predicted derivative x = x + h/6*(k1 + 2*k2 + 2*k3 + k4); f = (1/6)*(f1 + 2*f2 + 2*f3 + f4); Sol.x(:,i) = x; Sol.F(:,i) = f;

waitbar(i/imax); end close(waitbar_h); end

%SYSTEM MODEL BASED ON 12 DIFFERENTIAL EQUATIONS AND THE USE %OF CONTROL INPUTS function [ dq, force ] = System( q, t ) global Par; %State variables x = q(1); y = q(3); z = q(5); dx = q(2); dy = q(4); dz = q(6); roll = q(7); pitch = q(9); yaw = q(11); droll = q(8); dpitch = q(10); dyaw = q(12); %At first let's check whether the vehicle has reached a distance accurate %enough to an Nth point - if so, then let's move on to the next target distance_d = 1.1; %distance2 = (Par.path(Par.pathN,1)-x)^2 + (Par.path(Par. pathN,2)-y)^2 + (Par.path(Par.pathN,3)-z)^2; %if (Par.pathN < size(Par.path,1) && distance2 < distance_d) if (Par.pathN < size(Par.path,1) && abs(Par.path(Par.pathN, 1)-x) < distance_d && abs(Par.path(Par.pathN,2)-y) < distance_d && abs(Par.path(Par.pathN,3)-z) < distance_d ) Par.pathN = Par.pathN + 1; Par.target = [Par.path(Par.pathN,1); Par.path(Par. pathN,2); Par.path(Par.pathN,3); Par.path(Par.pathN,4); Par.path(Par.pathN,5); Par.path(Par.pathN,6)]; Par.pathFlagx = 0; Par.pathFlagy = 0; end

%roll = sym('roll'); %pitch = sym('pitch'); %yaw = sym('yaw'); %droll = sym('droll'); %dpitch = sym('dpitch'); %dyaw = sym('dyaw'); %Kx = sym ('Kx'); %Ky = sym ('Ky'); %Kz = sym ('Kz'); %Transformation matrices Rvb = matrix_Rvb(roll, pitch, yaw); Rbv = transpose(Rvb); Reulerw = [1 0 -sin(pitch); 0 cos(roll) sin(roll)*cos (pitch); 0 -sin(roll) cos(roll)*cos(pitch)]; %dReulerw = diff(Reulerw) %K = [Kx 0 0; 0 Ky 0; 0 0 Kz]; K = matrix_K(); Kx = K(1,1); Ky = K(2,2); Kz = K(3,3); A = K*Reulerw; B1 = Kx*cos(pitch)*dpitch*dyaw + (Ky-Kz)*(-cos(roll)*sin (roll)*dpitch^2 + cos(pitch)*dpitch*dyaw*cos(roll)^2 - cos (pitch)*dpitch*dyaw*sin(roll)^2 + cos(pitch)^2*sin(roll) *dyaw*dyaw*cos(roll)); B2 = Ky*(sin(roll)*droll*dpitch - cos(roll)*droll*cos (pitch)*dyaw + sin(roll)*dpitch*sin(pitch)*dyaw) + (Kz-Kx)* (sin(roll)*sin(pitch)*dpitch*dyaw - cos(roll)*cos(pitch) *sin(pitch)*dyaw^2 -sin(roll)*droll*dpitch + cos(roll)*cos (pitch)*droll*dyaw); B3 = Kz*(cos(roll)*droll*dpitch + sin(roll)*droll*cos (pitch)*dyaw + cos(roll)*dpitch*sin(pitch)*dyaw) + (Kx-Ky)* (droll*cos(roll)*dpitch - sin(pitch)*sin(roll)*cos(pitch) *dyaw^2 + dpitch*sin(pitch)*cos(roll)*dyaw - sin(roll) *dyaw*cos(pitch)*droll); B = [B1; B2; B3];

%PID control %PID( type, current value, time ) %X direction control

%Current X error value errx = Par.target(1) - x; %Determine the last target if (Par.pathN > 1) lasttarget = Par.path(Par.pathN-1, 1); else lasttarget = Par.path(Par.pathN, 1); end %Acceleration(s1) and braking(s2) ranges %Their meaning is explained a little bit later %Initially they are reset in every cycle to these tuned and tested values s1 = 1; s2 = 0.2; %Length of the current path part si = abs(lasttarget - Par.target(1)); %If the distance between Xd(i) and Xd(i+1) is smaller then value si, %then let's use smaller ranges if (si < s1+s2) s1 = 0.7*si; s2 = si - s1; end s %Target angle for the acceleration/braking part p1 = 0.2; p2 = 0.2; %Initial acceleration period if(abs(lasttarget - x) < s1 && ~Par.pathFlagx) Par.target(5) = sign(errx)*p1; %Fly with constant velocity period elseif(sign(errx)*errx > s1 && ~Par.pathFlagx) Par.target(5) = 0; %Breaking period elseif(sign(errx)*errx > s2 && ~Par.pathFlagx) Par.target(5) = -sign(errx)*p2; %Fine tuning around destination

else Par.pathFlagx = 1; Par.target(5) = PID(1, x, t); end

%Y direction control erry = Par.target(2) - y; if (Par.pathN > 1) lasttarget = Par.path(Par.pathN-1, 2); else lasttarget = Par.path(Par.pathN, 2); end %Acceleration and braking range s1 = 1; s2 = 0.2; si = abs(lasttarget - Par.target(2)); if (si < s1+s2) s1 = 0.7*si; s2 = si - s1; end %Initial acceleration period if(abs(lasttarget - y) < s1 && ~Par.pathFlagy) Par.target(4) = -sign(erry)*p1; %Fly with constant velocity period elseif(sign(erry)*erry > s1 && ~Par.pathFlagy) Par.target(4) = 0; %Breaking period elseif(sign(erry)*erry > s2 && ~Par.pathFlagy) Par.target(4) = sign(erry)*p2; %break %Fine tuning around destination else Par.pathFlagy = 1; Par.target(4) = -PID(1, x, t); end %Z direction control PWMz = PID(3, z, t);

%Input angle is bounded because of the system's nonlinearity tmax = pi()/4; if (Par.target(5) > tmax) Par.target(5) = tmax; elseif (Par.target(5) < -tmax) Par.target(5) = -tmax; end if (Par.target(4) > tmax) Par.target(4) = tmax; elseif (Par.target(4) < -tmax) Par.target(4) = -tmax; end if (Par.target(6) > tmax) Par.target(6) = tmax; elseif (Par.target(6) < -tmax) Par.target(6) = -tmax; end %Roll, pitch, yaw angle control PWMroll = PID(4, roll, t); PWMpitch = PID(5, pitch, t); PWMyaw = PID(6, yaw, t);

%4 control signal PWM1 = Par.PWMBase PWM2 = Par.PWMBase PWM3 = Par.PWMBase PWM4 = Par.PWMBase

+ + + +

PWMz PWMz PWMz PWMz

+ +

PWMyaw PWMyaw PWMyaw PWMyaw

+ +

PWMpitch; %+- PIDi PWMroll ; %+- PIDi PWMpitch; %+- PIDi PWMroll; %+- PIDi

%PWM should be between 0-255 PWM1 = PWMborders(PWM1); PWM2 = PWMborders(PWM2); PWM3 = PWMborders(PWM3); PWM4 = PWMborders(PWM4); PWM = [PWM1 PWM2 PWM3 PWM4]; %Actual rpm based on the input control signals omega = Motors(PWM); %omega2 = omega.*omega;

omega2 = bsxfun(@times, omega, omega); Par.F = Par.b * omega2; %Output of System (motor thrusts) force = Par.F; %Motor thrust in body coordinates ub = Par.F(1) + Par.F(2) + Par.F(3) + Par.F(4); u = [0; 0; ub]; %Moments acting on the quadrotor Mx = Par.L*(Par.F(4)-Par.F(2)); My = Par.L*(Par.F(3)-Par.F(1)); Mz = Par.d * (omega2(2)+omega2(4)-omega2(1)-omega2(3)); M = [Mx; My; Mz]; %X, Y and Z accelerations ddcoord = [0; 0; -Par.g] + (1/Par.m)*Rbv*u; ddx = ddcoord(1); ddy = ddcoord(2); ddz = ddcoord(3); %Roll, pitch, yaw angulare accelerations %ddeulerangles = inv(A)*(B + M); ddeulerangles = A\(B + M); ddroll = ddeulerangles(1); ddpitch = ddeulerangles(2); ddyaw = ddeulerangles(3); %Check if the system runs away if (isnan(ddroll)) pause; end

%Finally the output derivatives %dq = [dx; dvx; dy; % droll; ddroll; dpitch; dq = [dx; ddx; dy; droll; ddroll; dpitch;

of System dvy; dz; ddpitch; dyaw; ddy; dz; ddpitch; dyaw;

dvz;... ddyaw]; ddz;... ddyaw];

end

%FUNCTION WHERE PID TERMS ARE CALCULATED function [ PWM ] = PID( type, actual, time ) global Par; global PID; %Determine the actuak target target = Par.target(type,1); PID(type).now = time; %Initialize PID fields if needed if ~(isfield(PID(type), 'lasttime')) PID(type).lasttime = 0; end if ~(isfield(PID(type), 'error')) PID(type).n = 1; else PID(type).n = length(PID(type).error)+1; end %Calculate the current error value PID(type).error(PID(type).n) = target - actual; %Calculate the current I term %Since this method is called 4 times in every time step (at t, t+h/2, %t+h/2, t+h due to RK4) dt values loose their meaning %termD = Sum(error_i) instead of Sum(error_i*dt) if (PID(type).n == 1) PID(type).errorIntegral(PID(type).n) = Par.Ki(type) * PID(type).error(PID(type).n); else PID(type).errorIntegral(PID(type).n) = PID(type). errorIntegral(PID(type).n-1) + Par.Ki(type) * PID(type). error(PID(type).n); end %Similarly termD = error_i - error_i-1 instead of (error_i - error_i-1)/dt if (PID(type).n == 1) PID(type).errorDer(PID(type).n) = 0; else

%e1 - e0 = ref - value1 - ref + value0 PID(type).errorDer(PID(type).n) = Par.Kd(type) * (PID (type).lastactual - actual ); end %Finally the whole control signal PWM = Par.Kp(type) * PID(type).error(PID(type).n) + PID (type).errorIntegral(PID(type).n) + PID(type).errorDer(PID (type).n); %PID(type) %PWM % if(type == 3) % PID(type).error(PID(type).n) % Par.Kp(type) * PID(type).error(PID(type).n) % PID(type).errorDer(PID(type).n) % PID(type).errorIntegral(PID(type).n) % PWM % end PID(type).lasttime = now; PID(type).lastactual = actual; end

%THIS FUNCTION CUTS OUT TOO SLOW (<0) OR TOO HIGH (>255) CONTROL VALUES function [ PWMout ] = PWMborders( PWM ) if (PWM < 0) PWM = 0; elseif (PWM > 255) PWM = 255; end PWMout = PWM; end

%THIS FUNCTION CALCULATES THE APPROPRIATE %OUTPUT SPEED OF A MOTOR BASED ON THE INTPUT VOLTAGE function [ omega ] = Motors( PWM ) global Par; %output rpm omega = Par.RpmMax * PWM/255; end

%THE FUNCTION IS USED TO TRANSFORM VECTORS FROM THE %VEHICLE FRAME INTO THE BODY FRAME BASED ON EULER ANGLE %TRANSFORMATIONS function [ Rvb ] = matrix_Rvb( roll, pitch, yaw ) Rvv1 = [cos(yaw) sin(yaw) 0; -sin(yaw) cos(yaw) 0; 0 0 1]; Rv1v2 = [cos(pitch) 0 -sin(pitch); 0 1 0; sin(pitch) 0 cos (pitch)]; Rv2b = [1 0 0; 0 cos(roll) sin(roll); 0 -sin(roll) cos (roll)]; Rvb = Rv2b * Rv1v2 * Rvv1;

end

%THIS FUNCTION RETURNS WITH THE INERTIA MATRIX OF THE STRUCTURE function [ K ] = matrix_K( ) global Par; %Propellers - estimated, suppose they influence all directions similarly KxProp = (1/12) * Par.mProp * Par.lProp^2; KyProp = KxProp; KzProp = KxProp; KxCenterProp = KxProp + (1/4) * Par.mProp * Par.L^2; KyCenterProp = KxCenterProp; KzCenterProp = KxCenterProp; %Motors KxMotor = (1/12) * Par.mMotor * ((3/4)*Par.dMotor^2 + Par. hMotor^2); KyMotor = KxMotor; KzMotor = (1/2) * Par.mMotor * ((1/4)*Par.dMotor^2); KxCenterMotor = KxMotor + (1/4) * Par.mMotor * Par.L^2; KyCenterMotor = KxCenterMotor; KzCenterMotor = KzMotor + (1/4) * Par.mMotor * Par.L^2; %Propeller fixations KxFixation = (1/12) * Par.mFixation * ((3/4)*Par. dFixation^2 + Par.hFixation^2); KyFixation = KxFixation; KzFixation = (1/2) * Par.mFixation * ((1/4)*Par. dFixation^2); KxCenterFixation = KxFixation + (1/4) * Par.mFixation * Par.L^2; KyCenterFixation = KxCenterFixation; KzCenterFixation = KxFixation + (1/4) * Par.mFixation * Par.L^2; %Arm on axis Y KxBeam = (1/12)*Par.mBeam*(Par.hBeam^2 + Par.dBeam^2); KyBeam = (1/12)*Par.mBeam*(Par.hBeam^2 + Par.wBeam^2); KzBeam = (1/12)*Par.mBeam*(Par.wBeam^2+ Par.dBeam^2);

%Center of mass Kx = KxBeam + KyBeam + 2*(KxCenterProp + KxCenterMotor + KxCenterFixation) + 2*(KxProp + KxMotor + KxFixation); Ky = KyBeam + KxBeam + 2*(KyCenterProp + KyCenterMotor + KyCenterFixation) + 2*(KyProp + KyMotor + KyFixation); Kz = KzBeam + KzBeam + 4*(KzCenterProp + KzCenterMotor + KzCenterFixation); %Dualrotor only %a = KxBeam + 2*(KxCenterProp + KxCenterMotor + KxCenterFixation); K = [Kx 0 0 Ky 0 0 end

0; 0; Kz];

%PLOTS THE i, di RESULTS AND THE ACCORDING PID %PARAMETERS FOR THE CHOSEN TYPE OF DATA function [ ] = figures_one( k, Sol ) global Par global PID %Plot PID parameters figure('name', 'Error') hold on plot(Par.Kp(k)*PID(k).error(1:length(PID(k).error)), 'linewidth', 3, 'Color', 'k'); plot(PID(k).errorIntegral(1:length(PID(k).errorIntegral)), 'linewidth', 3, 'Color', 'r'); plot(PID(k).errorDer(1:length(PID(k).errorDer)), 'linewidth', 3, 'Color', 'g'); legend('P', 'I', 'D') xlabel('Index'); ylabel('Parameters'); grid on; hold off %Plot the result vector Sol k = 2*k; figure('name', 'Data') hold on plot(Sol.t(1,:),Sol.x(k-1,:),'linewidth', 3, 'Color', 'k') plot(Sol.t(1,:),Sol.x(k,:),'linewidth', 3, 'Color', 'r') legend('x', 'vx') %legend('y', 'vy') %legend('z', 'vz') %legend('roll', 'droll') %legend('pitch', 'dpitch') %legend('yaw', 'dyaw') xlabel('Sampling index'); ylabel('x [m] vx [m/s]'); %ylabel('y [m] vy [m/s]'); %ylabel('z [m] vz [m/s]'); %ylabel('roll [rad] droll [rad/s]'); %ylabel('pitch [rad] dpitch [rad/s]'); %ylabel('yaw [rad] dyaw [rad/s]');

grid on; hold off end

%PLOTS ALL RESULTS ALTOGETHER function [ ] = figures( Sol )

%Plots figure('name','Solution') hold on subplot(2,3,1) hold on plot(Sol.t(1,:),Sol.x(1,:),'b-') plot(Sol.t(1,:),Sol.x(2,:),'b--') grid on; hold off title('Subplot 1: X, Vx') subplot(2,3,2) hold on plot(Sol.t(1,:),Sol.x(3,:),'b-') plot(Sol.t(1,:),Sol.x(4,:),'b--') hold off grid on; title('Subplot 2: Y, Vy') subplot(2,3,3) hold on plot(Sol.t(1,:),Sol.x(5,:),'b-') plot(Sol.t(1,:),Sol.x(6,:),'b--') hold off grid on; title('Subplot 3: Z, Vz') subplot(2,3,4) hold on plot(Sol.t(1,:),Sol.x(7,:),'b') plot(Sol.t(1,:),Sol.x(8,:),'b--') hold off grid on; title('Subplot 4: Roll, DRoll') subplot(2,3,5) hold on plot(Sol.t(1,:),Sol.x(9,:),'b') plot(Sol.t(1,:),Sol.x(10,:),'b--')

hold off grid on; title('Subplot 5: Pitch, DPitch') subplot(2,3,6) hold on plot(Sol.t(1,:),Sol.x(11,:),'b') plot(Sol.t(1,:),Sol.x(12,:),'b--') hold off grid on; title('Subplot 6: Yaw, DYaw') drawnow; end

%ANIMATION FOR THE RESULTS function [ ] = animate( Sol ) global Par; imax = Par.imax; %Gain for the thrusts in order to be visible %on the animation Fgain = 0.2; figure('name','Animation','Color',[1 1 1]); xlabel('x') ylabel('y') zlabel('z') grid on box on hold on view([1,1,1]); axis equal xmin = 0; xmax = 4; axis([xmin xmax xmin xmax xmin xmax]); %Transformation to body coordinates Rvb = matrix_Rvb(Sol.x(7,1), Sol.x(9,1), Sol.x(11,1)); eb = Rvb*Par.ei; ebx = eb(1,:); eby = eb(2,:); ebz = eb(3,:); %Center xcenter ycenter zcenter

of mass = Sol.x(1,1); = Sol.x(3,1); = Sol.x(5,1);

%Beam1 on axis X (M1 - M3) xvector = ebx * Par.L; x1 = xcenter + dot(Par.eix, xvector/2); y1 = ycenter + dot(Par.eiy, xvector/2); z1 = zcenter + dot(Par.eiz, xvector/2);

x3 = xcenter - dot(Par.eix, xvector/2); y3 = ycenter - dot(Par.eiy, xvector/2); z3 = zcenter - dot(Par.eiz, xvector/2); Beam1 = plot3([x1,x3],[y1,y3],[z1,z3],'kO-','linewidth', 4,'Markersize',8,'Color','b'); %Beam2 on axis Y (M2 - M4) yvector = eby * Par.L; x2 = xcenter - dot(Par.eix, yvector/2); y2 = ycenter - dot(Par.eiy, yvector/2); z2 = zcenter - dot(Par.eiz, yvector/2); x4 = xcenter + dot(Par.eix, yvector/2); y4 = ycenter + dot(Par.eiy, yvector/2); z4 = zcenter + dot(Par.eiz, yvector/2); Beam2 = plot3([x2,x4],[y2,y4],[z2,z4],'kO-','linewidth', 4,'Markersize',8); %Motor f1x1 = f2x1 = f3x1 = f4x1 = f1y1 = f2y1 = f3y1 = f4y1 = f1z1 = f2z1 = f3z1 = f4z1 = f1x2 = f2x2 = f3x2 = f4x2 = f1y2 = f2y2 = f3y2 = f4y2 = f1z2 = f2z2 = f3z2 =

thrusts x1; x2; x3; x4; y1; y2; y3; y4; z1; z2; z3; z4; f1x1 + Fgain f2x1 + Fgain f3x1 + Fgain f4x1 + Fgain f1y1 + Fgain f2y1 + Fgain f3y1 + Fgain f4y1 + Fgain f1z1 + Fgain f2z1 + Fgain f3z1 + Fgain

* * * * * * * * * * *

dot(Par.eix, dot(Par.eix, dot(Par.eix, dot(Par.eix, dot(Par.eiy, dot(Par.eiy, dot(Par.eiy, dot(Par.eiy, dot(Par.eiz, dot(Par.eiz, dot(Par.eiz,

ebz ebz ebz ebz ebz ebz ebz ebz ebz ebz ebz

* * * * * * * * * * *

Sol.F(1,1)); Sol.F(2,1)); Sol.F(3,1)); Sol.F(4,1)); Sol.F(1,1)); Sol.F(2,1)); Sol.F(3,1)); Sol.F(4,1)); Sol.F(1,1)); Sol.F(2,1)); Sol.F(3,1));

f4z2 = f4z1 + Fgain * dot(Par.eiz, ebz * Sol.F(4,1)); Force1 = plot3([f1x1,f1x2],[f1y1,f1y2],[f1z1,f1z2], 'linewidth', 8, 'DisplayName','0.1', 'Color', [25 97 21] / 255); Force2 = plot3([f2x1,f2x2],[f2y1,f2y2],[f2z1,f2z2], 'linewidth', 8, 'DisplayName','0.1', 'Color', [255 41 58] / 255); Force3 = plot3([f3x1,f3x2],[f3y1,f3y2],[f3z1,f3z2], 'linewidth', 8, 'DisplayName','0.1', 'Color', [255 41 58] / 255); Force4 = plot3([f4x1,f4x2],[f4y1,f4y2],[f4z1,f4z2], 'linewidth', 8, 'DisplayName','0.1', 'Color', [255 41 58] / 255);

.

.

.

.

%Path points n = size(Par.path,1); scatter3(Par.path(1,1),Par.path(1,2),Par.path(1,3), 300, 'filled', 'r'); for i=2:1:(n-1) scatter3(Par.path(i,1),Par.path(i,2),Par.path(i,3), 300, 'filled', 'k'); end scatter3(Par.path(n,1),Par.path(n,2),Par.path(n,3), 300, 'filled', 'r'); for i=1:1:imax %Transformation to body coordinates Rvb = matrix_Rvb(Sol.x(7,i), Sol.x(9,i), Sol.x(11,i)); eb = Rvb*Par.ei; ebx = eb(1,:); eby = eb(2,:); ebz = eb(3,:); xcenter = Sol.x(1,i); ycenter = Sol.x(3,i); zcenter = Sol.x(5,i); plot3(Sol.x(1,1:i), Sol.x(3,1:i), Sol.x(5,1:i), 'linewidth', 3, 'color', 'b'); %Beam1 on axis X (M1 - M3) xvector = ebx * Par.L;

x1 = xcenter + dot(Par.eix, y1 = ycenter + dot(Par.eiy, z1 = zcenter + dot(Par.eiz, x3 = xcenter - dot(Par.eix, y3 = ycenter - dot(Par.eiy, z3 = zcenter - dot(Par.eiz, %Beam2 on axis Y (M2 - M4) yvector = eby * Par.L; x2 = xcenter - dot(Par.eix, y2 = ycenter - dot(Par.eiy, z2 = zcenter - dot(Par.eiz, x4 = xcenter + dot(Par.eix, y4 = ycenter + dot(Par.eiy, z4 = zcenter + dot(Par.eiz,

xvector/2); xvector/2); xvector/2); xvector/2); xvector/2); xvector/2);

yvector/2); yvector/2); yvector/2); yvector/2); yvector/2); yvector/2);

%Motor thrusts f1x1 = x1; f2x1 = x2; f3x1 = x3; f4x1 = x4; f1y1 = y1; f2y1 = y2; f3y1 = y3; f4y1 = y4; f1z1 = z1; f2z1 = z2; f3z1 = z3; f4z1 = z4; f1x2 = f1x1 + Fgain * dot(Par.eix, f2x2 = f2x1 + Fgain * dot(Par.eix, ebz f3x2 = f3x1 + Fgain * dot(Par.eix, ebz f4x2 = f4x1 + Fgain * dot(Par.eix, ebz f1y2 = f1y1 + Fgain * dot(Par.eiy, f2y2 = f2y1 + Fgain * dot(Par.eiy, ebz f3y2 = f3y1 + Fgain * dot(Par.eiy, ebz f4y2 = f4y1 + Fgain * dot(Par.eiy, ebz f1z2 = f1z1 + Fgain * dot(Par.eiz, f2z2 = f2z1 + Fgain * dot(Par.eiz, ebz f3z2 = f3z1 + Fgain * dot(Par.eiz, ebz f4z2 = f4z1 + Fgain * dot(Par.eiz, ebz

ebz * Sol.F(1,i)); * Sol.F(2,i)); * Sol.F(3,i)); * Sol.F(4,i)); ebz * Sol.F(1,i)); * Sol.F(2,i)); * Sol.F(3,i)); * Sol.F(4,i)); ebz * Sol.F(1,i)); * Sol.F(2,i)); * Sol.F(3,i)); * Sol.F(4,i));

set(Beam1,'Xdata',[x1,x3],'Ydata',[y1,y3],'Zdata',[z1, z3]); set(Beam2,'Xdata',[x2,x4],'Ydata',[y2,y4],'Zdata',[z2, z4]); set(Force1, 'XData', [f1x1,f1x2],'YData',[f1y1, f1y2],'ZData',[f1z1,f1z2]); set(Force2, 'XData', [f2x1,f2x2],'YData',[f2y1, f2y2],'ZData',[f2z1,f2z2]); set(Force3, 'XData', [f3x1,f3x2],'YData',[f3y1, f3y2],'ZData',[f3z1,f3z2]); set(Force4, 'XData', [f4x1,f4x2],'YData',[f4y1, f4y2],'ZData',[f4z1,f4z2]);

pause(0.01);

end

end

MELLÉKLET B

%SERIAL COMMUNICATION BETWEEN MATLAB AND ARDUINO %PLOT CONTROLLER RESULTS FOR THE DUALROTOR SYSTEM %This script reads all the Arduino data transmitted through Serial communication clc clear all close all %Length of reading length = 500; %Number of bytes read n = 56; %Initialize serial communication arduino = serial('COM5', 'BaudRate', 115200, 'InputBufferSize', n); fopen(arduino); %Initialize the vectors of the solution x0 = 0; x1 = 0; x2 = 0; x3 = 0; tempP = 0; tempI = 0; tempD = 0; %Flush the Serial input before reading flushinput(arduino);

for i = 1:length; %Wait for a specific 'Start' data (==119) clf; while 1 a = fread(arduino, 1); if a == 119 break; end end

%Read the left accel_angle value for Complementary Filter representation low = fread(arduino,1); first = fread(arduino, 1); second = fread(arduino, 1); high = fread(arduino, 1); lowneg = fread(arduino,1); firstneg = fread(arduino, 1); secondneg = fread(arduino, 1); highneg = fread(arduino, 1); %The real value (a float) is read by bytes %Then a checksum (bitwise negation) is read too for all bytes to eliminate false data if (low == bitcmp(lowneg, 'uint8') && first == bitcmp (firstneg, 'uint8') && second == bitcmp(secondneg, 'uint8') && high == bitcmp(highneg, 'uint8')) first = bitshift(first,8); second = bitshift(second,16); high = bitshift(high,24); b0 = uint32(high) + uint32(second) + uint32(first) + uint32(low); b0 = typecast(uint32(b0), 'single'); else length = length-1; flushinput(arduino); continue end %Read the left gyro_angle_sum value for Complementary Filter representation low = fread(arduino,1); first = fread(arduino, 1); second = fread(arduino, 1); high = fread(arduino, 1); lowneg = fread(arduino,1); firstneg = fread(arduino, 1); secondneg = fread(arduino, 1); highneg = fread(arduino, 1); if (low == bitcmp(lowneg, 'uint8') && first == bitcmp (firstneg, 'uint8') && second == bitcmp(secondneg, 'uint8')

&& high == bitcmp(highneg, 'uint8')) first = bitshift(first,8); second = bitshift(second,16); high = bitshift(high,24); b1 = uint32(high) + uint32(second) + uint32(first) + uint32(low); b1 = typecast(uint32(b1), 'single'); else length = length-1; flushinput(arduino); continue end %Read the left fused_angle value for Complementary Filter representation low = fread(arduino,1); first = fread(arduino, 1); second = fread(arduino, 1); high = fread(arduino, 1); lowneg = fread(arduino,1); firstneg = fread(arduino, 1); secondneg = fread(arduino, 1); highneg = fread(arduino, 1); if (low == bitcmp(lowneg, 'uint8') && first == bitcmp (firstneg, 'uint8') && second == bitcmp(secondneg, 'uint8') && high == bitcmp(highneg, 'uint8')) first = bitshift(first,8); second = bitshift(second,16); high = bitshift(high,24); b2 = uint32(high) + uint32(second) + uint32(first) + uint32(low); b2 = typecast(uint32(b2), 'single'); else length = length-1; flushinput(arduino); continue end %Read the (left + right)/2 averaged final angle_m value, that is %actually used for PID control

low = fread(arduino,1); first = fread(arduino, 1); second = fread(arduino, 1); high = fread(arduino, 1); lowneg = fread(arduino,1); firstneg = fread(arduino, 1); secondneg = fread(arduino, 1); highneg = fread(arduino, 1); if (low == bitcmp(lowneg, 'uint8') && first == bitcmp (firstneg, 'uint8') && second == bitcmp(secondneg, 'uint8') && high == bitcmp(highneg, 'uint8')) first = bitshift(first,8); second = bitshift(second,16); high = bitshift(high,24); b3 = uint32(high) + uint32(second) + uint32(first) + uint32(low); b3 = typecast(uint32(b3), 'single'); else length = length-1; flushinput(arduino); continue end %Read the actual P control term low = fread(arduino,1); first = fread(arduino, 1); second = fread(arduino, 1); high = fread(arduino, 1); lowneg = fread(arduino,1); firstneg = fread(arduino, 1); secondneg = fread(arduino, 1); highneg = fread(arduino, 1); if (low == bitcmp(lowneg, 'uint8') && first == bitcmp (firstneg, 'uint8') && second == bitcmp(secondneg, 'uint8') && high == bitcmp(highneg, 'uint8')) first = bitshift(first,8); second = bitshift(second,16); high = bitshift(high,24); bP = uint32(high) + uint32(second) + uint32(first) + uint32(low); bP = typecast(uint32(bP), 'single');

else length = length-1; flushinput(arduino); continue end %Read the actual I control term low = fread(arduino,1); first = fread(arduino, 1); second = fread(arduino, 1); high = fread(arduino, 1); lowneg = fread(arduino,1); firstneg = fread(arduino, 1); secondneg = fread(arduino, 1); highneg = fread(arduino, 1); if (low == bitcmp(lowneg, 'uint8') && first == bitcmp (firstneg, 'uint8') && second == bitcmp(secondneg, 'uint8') && high == bitcmp(highneg, 'uint8')) first = bitshift(first,8); second = bitshift(second,16); high = bitshift(high,24); bI = uint32(high) + uint32(second) + uint32(first) + uint32(low); bI = typecast(uint32(bI), 'single'); else length = length-1; flushinput(arduino); continue end %Read the actual D control term low = fread(arduino,1); first = fread(arduino, 1); second = fread(arduino, 1); high = fread(arduino, 1); lowneg = fread(arduino,1); firstneg = fread(arduino, 1); secondneg = fread(arduino, 1); highneg = fread(arduino, 1); if (low == bitcmp(lowneg, 'uint8') && first == bitcmp (firstneg, 'uint8') && second == bitcmp(secondneg, 'uint8')

&& high == bitcmp(highneg, 'uint8')) first = bitshift(first,8); second = bitshift(second,16); high = bitshift(high,24); bD = uint32(high) + uint32(second) + uint32(first) + uint32(low); bD = typecast(uint32(bD), 'single'); else length = length-1; flushinput(arduino); continue end %If no false error was read, store the data x0 = [x0 b0]; x1 = [x1 b1]; x2 = [x2 b2]; x3 = [x3 b3]; tempP = [tempP bP]; tempI = [tempI bI]; tempD = [tempD bD]; flushinput(arduino); end %Close Serial connection fclose(arduino); %Linewidth lw = 2.5; %Plot angle_m results figure; xlabel('Sample index'); ylabel('Angle [°]'); hold on; plot(2:length, x3(2:length),'linewidth', lw, 'Color', 'k') %!!!!!! title('ANGLE') grid on;

hold off; legend('used'); drawnow; %Plot Complementary Filter results figure; xlabel('Sample index'); ylabel('Angle [°]'); hold on; plot(2:length, x0(2:length), 'linewidth', lw, 'Color', [32 109 2] ./ 255); plot(2:length, x1(2:length), 'linewidth', lw, 'Color','r') plot(2:length, x2(2:length), 'linewidth', lw, 'Color','k') title('ANGLE') grid on; hold off; legend('accel', 'gyro', 'fused'); drawnow; %Plot PID terms figure; xlabel('Sample index'); ylabel('parameters'); hold on; plot(2:length, tempP(2:length), 'linewidth', lw, 'Color', [32 109 2] ./ 255); title('CONTROL') plot(2:length, tempI(2:length), 'linewidth', lw, 'Color','r') %!!!!!! grid on; plot(2:length, tempD(2:length), 'linewidth', lw, 'Color','k') %!!!!!! hold off; legend('p', 'i', 'd'); drawnow;

%DATA USED FOR DUALROTOR SIMULINK SIMULATION %Target angle fi_ref = 0; %Ubase for the motors are actualy given in PPM, between a range of %1170-1450 Ubase1 = 1256; Ubase2 = 1256; Ubase1 = (Ubase1-1170)/(1450-1170)*255; Ubase2 = (Ubase2-1170)/(1450-1170)*255; %Motor parameters KV = 1000; Usupply = 12; RPMmax = KV*Usupply; %omega = U * RPMmax/255; %Aerodynamical parameters b = 1.57*10^(-5); %Phísical parameters L = 0.365; %2l l = L/2; %Moment of Inertia Theta = 0.0152274;

MELLÉKLET C

//teeter.ino #include "Motor.h" //ECUs are controlled here (got from the internet) #include "ServoTimer2.h" //Communication with the MCU6050 #include <Wire.h> #include "config.h" //Acts as a filtering interface //between raw and finally used angle data #include "angle.h"

//Classes for controlling the ECUs Motor *brushless_bal; Motor *brushless_jobb; //Current speed values uint16_t seb_bal; uint16_t seb_jobb; //Variables for timing uint32_t ido0; uint32_t ido1; float dt; //Variable for angle calculation angle *ac; //Target angle, measured angle [fifo], //and a moving integral union data_union3 { float f; byte fBuff[sizeof(float)]; } angle_m[INTEGRAL_NUMBER], angle_t, tempP, tempI, tempD;

void setup() { //LED for indicating the setup procedure pinMode(13, OUTPUT); digitalWrite(13, HIGH); Serial.begin(115200); #ifdef SERIALHERE Serial.println("");

Serial.println("Starting..."); #endif brushless_bal =new Motor(BRUSHLESS_BAL_PIN); brushless_jobb =new Motor(BRUSHLESS_JOBB_PIN); //Call the angle() constructor //containing the MPU6050 (and other //angle) initialization ac =new angle(); //Configuring ESC control, //and setting minimum value for arming brushless_bal->write(BRUSHLESS_MIN); brushless_jobb->write(BRUSHLESS_MIN); ido0 =millis(); //Arming needs about 5 secs //Wait if there is still time to arm if(millis() < (ido0 + 5000)) { #ifdef SERIALHERE Serial.println("Waiting for ECUs to arm..."); #endif delay(5100 - (millis() - ido0)); } //Setup is done #ifdef SERIALHERE Serial.println("Setup done!"); #endif digitalWrite(13, LOW); //Initialize the target and measured angle angle_t.f = TARGET; angle_m[0].f = ac->m0_init(); for (int i =0; i
ido0 =millis(); }

void loop() { //Cycle time calculation ido1 = ido0; ido0 =millis(); dt = ido0 - ido1; //Calculate speed values based on //the angle calculated control(dt); //Set the before calculated speed as //ECU output values brushless_bal->write(seb_bal); brushless_jobb->write(seb_jobb); #ifdef SERIALPC //Communicate with PC - bytes and //their bitwise negations are sent Serial.write(B01110111); Serial.write(ac->mpu6050_bal->accel_angle.fBuff, 4); Serial.write(~(ac->mpu6050_bal->accel_angle.fBuff[0])); //checksum Serial.write(~(ac->mpu6050_bal->accel_angle.fBuff[1])); //checksum Serial.write(~(ac->mpu6050_bal->accel_angle.fBuff[2])); //checksum Serial.write(~(ac->mpu6050_bal->accel_angle.fBuff[3])); //checksum Serial.write(ac->mpu6050_bal->gyro_angle_sum.fBuff, 4); Serial.write(~(ac->mpu6050_bal->gyro_angle_sum.fBuff[0])); Serial.write(~(ac->mpu6050_bal->gyro_angle_sum.fBuff[1])); Serial.write(~(ac->mpu6050_bal->gyro_angle_sum.fBuff[2])); Serial.write(~(ac->mpu6050_bal->gyro_angle_sum.fBuff[3])); Serial.write(ac->mpu6050_bal->fused_angle[0].fBuff, 4); Serial.write(~(ac->mpu6050_bal->fused_angle[0].fBuff[0])); Serial.write(~(ac->mpu6050_bal->fused_angle[0].fBuff[1])); Serial.write(~(ac->mpu6050_bal->fused_angle[0].fBuff[2])); Serial.write(~(ac->mpu6050_bal->fused_angle[0].fBuff[3])); Serial.write(angle_m[0].fBuff, 4); Serial.write(~(angle_m[0].fBuff[0])); Serial.write(~(angle_m[0].fBuff[1])); Serial.write(~(angle_m[0].fBuff[2])); Serial.write(~(angle_m[0].fBuff[3]));

//checksum //checksum //checksum //checksum

Serial.write(tempP.fBuff, 4); Serial.write(~(tempP.fBuff[0])); Serial.write(~(tempP.fBuff[1])); //checksum Serial.write(~(tempP.fBuff[2])); Serial.write(~(tempP.fBuff[3])); //checksum Serial.write(tempI.fBuff, 4); Serial.write(~(tempI.fBuff[0])); Serial.write(~(tempI.fBuff[1])); Serial.write(~(tempI.fBuff[2])); Serial.write(~(tempI.fBuff[3])); Serial.write(tempD.fBuff, 4); Serial.write(~(tempD.fBuff[0])); Serial.write(~(tempD.fBuff[1])); Serial.write(~(tempD.fBuff[2])); Serial.write(~(tempD.fBuff[3])); #endif }

//Calculate speed values based on angular position void control(float dt) { //In order not to fulfil the integrator //with false values (due to drift and inaccuracy) //A moving integral was used //Integrator term is recalculated in every step tempI.f = 0; for(int i = 1; icalcangle(dt); //Calculate the necessarry control values tempI.f += I * (angle_t.f - angle_m[0].f); tempP.f = P*(angle_t.f-angle_m[0].f); tempD.f = D * (angle_m[1].f - angle_m[0].f); float temp = tempP.f + tempD.f + tempI.f; //Set the new speed seb_bal = V_BAL + temp; seb_jobb = V_JOBB - temp; }

int sign(float x) { int i = 888; (x > 0) ? i=1 : ((x < 0) ? return i; }

i=-1 : i=0);

//config.h //Contains configuration parameters //#define SERIALHERE #define

//Serial Monitor SERIALPC//Serial with PC

//fused_angle is stored in a ~ big fifo, //their average can be calculated for //further filtering #define AVERAGE_NUMBER 4 //Number of error values stored in //the moving integral #define INTEGRAL_NUMBER 50//20 #define TIME_CONSTANT 1.2 //Initial speed value for the motors #define V_BAL 1256 #define V_JOBB 1256 #define P 0.3//0.4 #define I 0.004 #define D 10//7.5 //IIC addresses of the MPU6050s #define I2C_ADDRESS_LOW 0x68 #define I2C_ADDRESS_HIGH 0x69 //Pin for configuring the IIC addresses #define MPU6050_BAL_AD0 37 #define MPU6050_JOBB_AD0 36 //Output for the ECUsss #define BRUSHLESS_BAL_PIN #define BRUSHLESS_JOBB_PIN

18 19

//Value boundaries for the ESC outputs #define BRUSHLESS_MIN 1170 #define BRUSHLESS_MAX 1450 //Target angle #define TARGET

25

//Motor.h #ifndef MOTOR_H #define MOTOR_H #include "Arduino.h" #include "ServoTimer2.h" //Interface between teeter.ino and ServoTimer2.cpp class Motor { private: public: int brushless_pin; ServoTimer2 *ServoMotor; Motor(int); //Constructor void write(int); }; #endif

//Motor.cpp #include "Motor.h" #include "ServoTimer2.h" //Interface between teeter.ino and //ServoTimer2.cpp Motor::Motor(int pin) { this->ServoMotor = new ServoTimer2(); this->brushless_pin = pin; this->ServoMotor->attach( this->brushless_pin); } void Motor::write(int pulsewidth) { this->ServoMotor->write(pulsewidth); }

//angle.h #ifndef ANGLE_H #define ANGLE_H #include "Arduino.h" #include "MPU6050.h" #include "config.h" //Acts as a filtering interface //between raw and finally used angle data class angle { private: // MPU6050 *mpu6050_bal; // MPU6050 *mpu6050_jobb; public: angle(); float m0_init(); float calcangle(float); float complementary(MPU6050*,float); MPU6050 *mpu6050_bal; MPU6050 *mpu6050_jobb; };

#endif

//angle.cpp #include "angle.h" //Acts as a filtering interface between //raw and finally used angle data angle::angle() { //Setting the address of the MPU6050s pinMode(MPU6050_BAL_AD0, OUTPUT); pinMode(MPU6050_JOBB_AD0, OUTPUT); digitalWrite(MPU6050_BAL_AD0, LOW); digitalWrite(MPU6050_JOBB_AD0, HIGH); //Instantiation this->mpu6050_bal = new MPU6050(I2C_ADDRESS_LOW); this->mpu6050_jobb = new MPU6050(I2C_ADDRESS_HIGH); //Waking up the sensors from //the default sleep mode this->mpu6050_bal->wakeUp(); this->mpu6050_jobb->wakeUp(); //Basic configurations this->mpu6050_bal->defaults(); this->mpu6050_jobb->defaults(); this->mpu6050_bal->angle_flag = -1; this->mpu6050_jobb->angle_flag = -1; #ifdef SERIALHERE Serial.println("MPU6050s online..."); #endif //Set the earlier calibrated values //gyro offsets are calibrated real /time in fifo_first_fill() int32_t offsets_bal[7] = {430, -190, -1839, 0, 0, 0, 0}; float gains_bal[7] = {1.002197266, 0.9954834, 1.019958496, 1, 0.9892468, 0.993, 0.957912868}; this->mpu6050_bal->calibrate(offsets_bal,gains_bal); //gyro offsets are calibrated real time in fifo_first_fill() int32_t offsets_jobb[7] = {598, -235, -665, 0, 0, 0, 0}; float gains_jobb[7] = {1.0083618, 1.000457764, 1.026306, 1, 0.987582736, 0.984841805, 0.97125}; this->mpu6050_jobb->calibrate(offsets_jobb, gains_jobb);

//Collection of the first //FILTER_NUMBER data this->mpu6050_bal->fifo_first_fill(); this->mpu6050_jobb->fifo_first_fill(); //First angle measurement //for initialization this->mpu6050_bal->fused_angle[0].f = this->mpu6050_bal->angle_flag *asin(this->mpu6050_bal->fifo_corrdata[1][0].val / 16384.0) * 57.29578; //FOR INITIALIZING THE FUSION FILTER this->mpu6050_jobb->fused_angle[0].f = this->mpu6050_jobb->angle_flag *asin(this->mpu6050_jobb->fifo_corrdata[1][0].val / 16384.0) * 57.29578; //FOR INITIALIZING THE FUSION FILTER for(int i = 1; impu6050_bal->fused_angle[i].f = this->mpu6050_bal->fused_angle[0].f; this->mpu6050_jobb->fused_angle[i].f = this->mpu6050_jobb->fused_angle[0].f; } this->mpu6050_bal->gyro_angle_sum.f = this->mpu6050_bal->fused_angle[0].f; this->mpu6050_jobb->gyro_angle_sum.f = this->mpu6050_jobb->fused_angle[0].f; } //Provides usable angle data to teeter.ino float angle::calcangle(float dt) { float return_angle = 0; //Read MPU6050 data #ifdef SERIALHERE Serial.println("Bal data:"); #endif mpu6050_bal->dataIn(); #ifdef SERIALHERE Serial.println("Jobb data:"); #endif mpu6050_jobb->dataIn(); return_angle = (this->complementary(this->mpu6050_bal,dt)

+this->complementary(this->mpu6050_jobb,dt)) /2.0;

return return_angle; } float angle::m0_init() { return (this->mpu6050_bal->fused_angle[0].f +this->mpu6050_jobb->fused_angle[0].f)/2.0; }

//Filter for fusing together accelerometer //and gyroscope data float angle::complementary( MPU6050 *mpu,float dt) { float return_angle = 0; float w = 0; //Shift fused_angle fifo for(int i = 1; ifused_angle[i].f = mpu->fused_angle[i-1].f; } //complementary filter w =asin(mpu->fifo_corrdata[1][0].val / 16384.0); if (!isnan(w)) mpu->accel_angle.f = mpu->angle_flag * w * 57.29578; else mpu->accel_angle.f = 0; mpu->gyro_angle.f = (1)*mpu->angle_flag * mpu->fifo_corrdata[4][0].val * 0.001 * dt / 32768.0 * 250.0; mpu->gyro_angle_sum.f += mpu->gyro_angle.f; float a = TIME_CONSTANT / (TIME_CONSTANT + 0.001 * dt); mpu->fused_angle[0].f = a * (mpu->fused_angle[0].f + mpu->gyro_angle.f) + (1 - a) * mpu->accel_angle.f;

//additional filter: average the last //AVERAGE_NUMBER angle values for(int i = 0; i < AVERAGE_NUMBER; i++) { return_angle += mpu->fused_angle[i].f; } return_angle /= (double)AVERAGE_NUMBER; return }

return_angle;//bit smoother

//MPU6050.h #ifndef MPU6050_H #define MPU6050_H #include "Arduino.h" #include <Wire.h> //I2C #include "config.h" //Sensor data is read here //Registers of MPU6050 #define PWR_MGMT_1 #define ACCEL_XOUT_H #define ACCEL_CONFIG #define GYRO_CONFIG

0x6B 0x3B 0x1C 0x1B

//Second dimension size of the //fifo containing imu data #define FILTER_NUMBER 20 class MPU6050 { private: public: MPU6050(int); //Constructor void wakeUp(); void defaults(); void fifo_first_fill(); void calibrate(int32_t offsets[7], float gains[7]); void dataIn(); union data_union { struct { uint8_t l; uint8_t h; } reg; int16_t val; } data[7], fifo_olddata[7][FILTER_NUMBER], fifo_corrdata[7][FILTER_NUMBER]; union data_union2 { float f; byte fBuff[sizeof(float)]; } accel_angle, gyro_angle,

fused_angle[AVERAGE_NUMBER], gyro_angle_sum; int i2c_address; int32_t offset[7]; float gain[7]; int angle_flag; };

#endif

//MPU6050.cpp #include #include #include #include

"Arduino.h" "MPU6050.h" <Wire.h> "config.h"

//Sensor data is read here MPU6050::MPU6050(int address) { this->i2c_address = address; this->accel_angle.f = 0; this->gyro_angle.f = 0; Wire.begin(); }

void MPU6050::wakeUp() { Wire.beginTransmission(this->i2c_address); Wire.write(PWR_MGMT_1); Wire.write(0); Wire.endTransmission(true); } void MPU6050::defaults() { Wire.beginTransmission(this->i2c_address); Wire.write(ACCEL_CONFIG); Wire.write(B00000000); Wire.write(GYRO_CONFIG); Wire.write(B00000000); Wire.endTransmission(true); } void MPU6050::calibrate(int32_t offsets[7], float gains[7]) { this->offset[0] = offsets[0]; this->offset[1] = offsets[1]; this->offset[2] = offsets[2]; this->offset[3] = offsets[3]; this->offset[4] = offsets[4]; this->offset[5] = offsets[5]; this->offset[6] = offsets[6]; this->gain[0] = gains[0]; this->gain[1] = gains[1];

this->gain[2] this->gain[3] this->gain[4] this->gain[5] this->gain[6]

= = = = =

gains[2]; gains[3]; gains[4]; gains[5]; gains[6];

} void MPU6050::fifo_first_fill() { //Fill the first 7xFILTER_NUMBER fifo for(int n = 0; n < FILTER_NUMBER; n++) { Wire.beginTransmission(this->i2c_address); Wire.write(ACCEL_XOUT_H); Wire.endTransmission(false); int number_of_bytes =Wire.requestFrom(this->i2c_address, 14, true); for(int i = 0; i < 7; i++) { data[i].reg.h =Wire.read(); data[i].reg.l =Wire.read(); } //If every data is 0 then MPU6050 is //sleeping if(!(data[0].val) && !(data[1].val) && !(data[2].val) && !(data[3].val) && !(data[4].val) && !(data[5].val) && !(data[6].val)) { #ifdef SERIALHERE Serial.println("I went to sleep but will wake up soon!"); #endif this->wakeUp(); this->defaults(); n--; } for (int i = 0; i < 7; i++) { this->fifo_olddata[i][FILTER_NUMBER-n-1].val = data[i].val; if(i<=3) //accel, temp corrected already { this->fifo_corrdata[i][FILTER_NUMBER-n-1].val =(data[i].val -this->offset[i])/this->gain[i]; } if(i>3) //gyro offset calculation after turning on { this->offset[i] += data[i].val;

} } //If every data is -1 then check the communication if (number_of_bytes < 14) { n--; } } //Calculate gyro offsets on the fly for (int i = 4; i<7; i++) { this->offset[i] /= (double)FILTER_NUMBER; #ifdef SERIALHERE Serial.print(i); Serial.print(": "); Serial.println(this->offset[i]); #endif } //Correct the gyro data with the //before calculated offsets for(int n = 0; n < FILTER_NUMBER; n++) { for (int i = 4; i < 7; i++) { this->fifo_corrdata[i][FILTER_NUMBER-n-1].val = (this->fifo_olddata[i][FILTER_NUMBER-n-1].val -this->offset[i])/this->gain[i]; } } //Show the first fifo #ifdef SERIALHERE Serial.println("first fifo: "); for(int i = 0; i<7; i++) { for(int j = 0; j
Serial.print(fifo_corrdata[i][j].val); Serial.print("\t"); } Serial.println(""); } #endif }

void MPU6050::dataIn() { Wire.beginTransmission(this->i2c_address); Wire.write(ACCEL_XOUT_H); Wire.endTransmission(false); int number_of_bytes =Wire.requestFrom( this->i2c_address, 14, true); for(int i = 0; i < 7; i++) { data[i].reg.h =Wire.read(); data[i].reg.l =Wire.read(); } //if every data is 0 then MPU6050 is sleeping if(!(data[0].val) && !(data[1].val) && !(data[2].val) && !(data[3].val) && !(data[4].val) && !(data[5].val) && !(data[6].val)) { #ifdef SERIALHERE Serial.println("I went to sleep but will wake up soon!"); #endif this->wakeUp(); this->defaults(); } //if every data is -1 then check the communication

//filter for salient values //continuously refreshing fifo with the last //FILTER_NUMBER data for (int i = 0; i<7; i++) { /* //Additional filter for raw values //Resulted in a certain small time delay

//Since complementary filter solved the //filtering itself, this is not used int32_t average = 0; int32_t scatter = 0; for (int j = 0; j < FILTER_NUMBER; j++) { average += this->fifo_olddata[i][j].val; } average /= FILTER_NUMBER; for (int j = 0; j < FILTER_NUMBER; j++) { scatter += (this->fifo_olddata[i][j].val - average) *(this->fifo_olddata[i][j].val - average); } scatter /= FILTER_NUMBER; scatter = sqrt(scatter); //shiftelés jobbra for (int j = FILTER_NUMBER-1; j>0; j--) { this->fifo_olddata[i][j].val = this->fifo_olddata[i][j-1].val; this->fifo_corrdata[i][j].val = this->fifo_corrdata[i][j-1].val; } //0 elem if ( abs(data[i].val-average)<(10*scatter) ) { this->fifo_olddata[i][0].val = data[i].val; this->fifo_corrdata[i][0].val = (data[i].val - this->offset[i])/this->gain[i]; } else { this->fifo_olddata[i][0].val = average; this->fifo_corrdata[i][0].val = (average - this->offset[i])/this->gain[i]; } */ this->fifo_olddata[i][0].val = data[i].val; this->fifo_corrdata[i][0].val = (data[i].val -this->offset[i]) /this->gain[i]; }

#ifdef SERIALHERE Serial.print("\t\t"); Serial.println(number_of_bytes); for (int i = 0; i < 7; i++) { Serial.print(this->fifo_corrdata[i][0].val); if (i==3) { Serial.print("="); Serial.print( this->fifo_corrdata[i][0].val/340.00 +36.5); Serial.print(" C"); } Serial.print("\t"); } Serial.print("\t\t"); Serial.println(number_of_bytes); #endif }

BOJTÁR ORSIKA TDK DOLGOZAT

Recommend Documents