MarkoZaraˇc Razvojinvrednotenjetriosnegaelektronskegastabilizatorja

(1)

UNIVERZA V LJUBLJANI Fakulteta za strojniˇstvo

Razvoj in vrednotenje triosnega elektronskega stabilizatorja

Zakljuˇ cna naloga Univerzitetnega ˇstudijskega programa I. stopnje Strojniˇstvo - Razvojno raziskovalni program

Marko Zaraˇ c

Ljubljana, avgust 2021

(2)

(3)

UNIVERZA V LJUBLJANI Fakulteta za strojniˇstvo

Razvoj in vrednotenje triosnega elektronskega stabilizatorja

Zakljuˇ cna naloga Univerzitetnega ˇstudijskega programa I. stopnje Strojniˇstvo - Razvojno raziskovalni program

Marko Zaraˇ c

Mentor: doc. dr. Rok Vrabiˇ c, univ. dipl. inˇ z.

Ljubljana, avgust 2021

(4)

(5)

Zahvala

Hvala mentorju za akademsko vodenje in pomoˇc pri pisanju tega zakljuˇcnega dela.

Hvala druˇzini za podporo in razumevanje.

Iskreno hvala.

(6)

(7)

Izvleˇ cek

UDK 62-559.8:681.5:531.7(0.43.2) Tek. ˇstev.: UN I/1545

Razvoj in vrednotenje triosnega elektronskega stabilizatorja

Marko Zaraˇc

Kljuˇcne besede: stabilizacija

merjenje orientacije inercijska merilna enota Madgwickov filter Arduino

ˇSirok spekter razliˇcnih senzorjev, predvsem optiˇcnih senzorjev, laserjev in daljinomerov, ima potrebo po rotacijski stabilizaciji. Z razvojem sistema stabilizacije, ki med drugim vkljuˇcuje mikrokrmilnik, programsko kodo, senzoriko in mehanske komponente, poskuˇsamo omenjeni problem reˇsiti. Naloga je sestavljena iz razvoja sistema, dimenzioniranja in izbire komponent ter vrednotenja konˇcnih karakteristik stabilizacijskega sistema. Glavni sklep naloge, poleg sklepa o delovanju sistema, je odvisnost mirnega delovanja servomotorja od ekscentra teˇziˇsˇca bremena.

(8)

Abstract

UDC 62-559.8:681.5:531.7(0.43.2) No.: UN I/1545

Development and evaluation of a triaxial electronic stabilizer

Marko Zaraˇc

Key words: stabilization

measurment of orientation inertial measurment unit Madgwick filter

Arduino

A wide array of different sensors, especially optical sensors, lasers and rangefinders, have a need for rotational stabilization. With the development of a stabilization system that includes, a microcontroller, software, sensors and mechanical components, solving of this problem was attempted. The thesis itself consists of the development of the system, the dimensioning, selection of the components and the evaluation of the final characteristics of the stabilization system. The main conclusion of the thesis in addition to the conclusion on the successful operation of the system is the dependence of the smooth operation of the servo motor from the eccentric position of the center of mass of the load.

vi

(9)

Kazalo

Kazalo slik . . . viii

Kazalo preglednic . . . ix

Seznam uporabljenih simbolov . . . x

Seznam uporabljenih okrajˇsav . . . xi

1 Uvod . . . 1

1.1 Ozadje problema . . . 1

1.2 Cilji naloge . . . 1

2 Teoretiˇcne osnove . . . 2

2.1 Orientacija v prostoru . . . 2

2.2 Senzorji MEMS . . . 3

2.3 Madgwickov filter . . . 4

2.4 Rotacijska matrika . . . 5

2.5 Elektromotorji in krmilje . . . 6

3 Metodologija razvoja in vrednotenja . . . 7

3.1 Preliminarna doloˇcitev komponent . . . 7

3.1.1 Tehniˇcne karakteristike komponent . . . 7

3.2 Doloˇcitev geometrije . . . 8

3.3 Dimenzioniranje stabilizatorskih rok . . . 9

3.3.1 Doloˇcitev obremenitev . . . 9

3.3.2 Simulacija FEM . . . 10

3.4 Dimenzioniranje elektro-servomotorjev . . . 12

3.5 Elektriˇcno vezje . . . 13

3.6 Programska koda . . . 15

3.7 Vrednotenje . . . 15

4 Rezultati in diskusija . . . 18

5 Zakljuˇcki . . . 21

Literatura . . . 22

(10)

Kazalo slik

Slika 2.1: Slika primarnih osi letala [1]. . . 2

Slika 2.2: Prikaz delovanja pospeˇskomera [4]. . . 3

Slika 2.3: Posnetek pospeˇskomera z mikroskopom [4]. . . 3

Slika 2.4: Prikaz delovanja ˇziroskopa [5]. . . 4

Slika 2.5: Posnetek ˇziroskopa z mikroskopom [5]. . . 4

Slika 2.6: Blokovni diagram Madgwickovega filtra [6]. . . 5

Slika 2.7: Prikaz elektro-servomotorja [9]. . . 6

Slika 3.1: Zajem zaslona iz CAD-programa NX. . . 9

Slika 3.2: Prikaz teˇziˇsˇca komponent, ki jih nosi odklonska roˇcica. . . 10

Slika 3.3: Prikaz statiˇcnih modelov, uporabljenih za preraˇcun obremenitev roˇcice odklona. . . 10

Slika 3.4: (a) Prikaz mreˇzenja med simulacijo v programu Ansys. (b) Prikaz podpor in obremenitev. . . 11

Slika 3.5: Prikaz rezultatov analize FEM v Ansysu: (a) predstavlja deformacijo, (b) predstavlja napetosti po von-Misesu. . . 11

Slika 3.6: Shema elektriˇcnega vezja. Izdelano v programu EasyEDA. . . 14

Slika 3.7: Prikaz elektronskih komponent. . . 14

Slika 3.8: Fiziˇcni prototip. . . 16

Slika 3.9: Prikaz merilnih mest. . . 16

Slika 3.10: Kontrolna meritev. . . 17

Slika 3.11: Meritev z uravnoteˇzenim stabilizacijskim bremenom (a) in meritev z neuravnoteˇzenim stabilizacijskim bremenom (b). . . 17

Slika 4.1: Umerjanje ˇziroskopa. . . 18

Slika 4.2: Umerjanje pospeˇskomera. . . 19

Slika 4.3: Meritev rotacije brez delovanja sistema rotacijske kompenzacije. . . 19

Slika 4.4: Meritev rotacije brez stabilizacijskega objekta. . . 19

Slika 4.5: Meritev rotacije s stabilizacijskim objektom (z bremenom), ki je uravnoteˇzen. . . 20

Slika 4.6: Meritev rotacije s stabilizacijskim objektom (z bremenom), ki ni uravnoteˇzen. . . 20

viii

(11)

Kazalo preglednic

Preglednica 3.1: Karakteristike MPU9250 [11]. . . 7 Preglednica 3.2: Karakteristike MG996R [9]. . . 8

(12)

Seznam uporabljenih simbolov

Oznaka Enota Pomen

F N sila

I,J kg mm² masni vztrajnostni moment

L m, mm dolˇzina

m kg, g masa

ω rad/s kotna hitrost

T Nm navor

t s, ms ˇcas

X^′′, α ^◦ kot okoli osi X (nagib) Y^′, β ^◦ kot okoli osi Y (naklon)

Z, γ ^◦ kot okoli osi Z (odklon)

Indeksi

g gravitacijski

L bremena

m motorja

no load brez bremena stall zadrˇzni t teˇziˇsˇcni

x

(13)

Seznam uporabljenih okrajˇ sav

Okrajˇsava Pomen

BLDC brezkrtaˇcni DC motorji (ang. Brushless DC motor)

CAD raˇcunalniˇsko podprto modeliranje (ang. Computer Aided Design) DAQ zajem podatkov (ang. Data Acquisition)

DC enosmerni tok (ang. Direct Current)

DMP digitalni procesor gibanja (ang. Digitral Motion Processor) DOF prostorska stopnja (ang. Degrees Of Freedom)

FEM metoda konˇcnih elementov (ang. Finite Element Method) FIFO prvi noter prvi ven (ang. First In First Out)

I2C inter-integrirano vezje (ang. Inter-Integrated Circuit) IMU inercijska merilna enota (ang. Inertial Measurment Unit) MEMS mikro-elektromehanski sistem

(ang. Micro-Electromechanical Systems)

MPU merska procesorska enota (ang. Motion Procesing Unit) PLA poliaktiˇcna kislina (ang. polylactic acid)

PWM pulzno-ˇsirinska modulacija (ang. Pulse Width Modulation) SCL serijska ˇcasovna linija (ang. Serial Clock Line)

SDA serijska podatkovna linija (ang. Serial Data Line) YPR odklon, naklon, nagib (ang. yaw, pitch, roll)

(14)

1 Uvod

1.1 Ozadje problema

V razliˇcnih aplikacijah se z razvojem tehnike vse bolj pojavlja potreba po implemen- taciji optiˇcnih senzorjev, laserjev ali daljinomerov na sistemih, ki se premikajo. To so lahko kamere na letalih, laserski daljinomeri na dronih in akcijske kamere na ˇceladah.

Problematika stabilizacije opreme na premikajoˇcih se platformah izvira iz njihove di- namiˇcne narave. ˇClovek, ki se premika ali vozilo, ki se vozi, posreduje svoje premike kameri ali senzorju. Zaradi problematiˇcnosti je cilj ustrezno izniˇcenje premikov z rotacijsko kompenzacijo. Nekoˇc mehanske reˇsitve dane problematike danes ˇcedalje bolj reˇsujemo elektronsko. Rotacijska kompenzacija se lahko dodeli trem ortogonalno po- zicioniranim elektromotorjem, ki posamiˇcno poskrbijo za kompenzacijo na dodeljeni osi.

1.2 Cilji naloge

Cilj naloge je razvoj in vrednotenje elektronskega triosnega stabilizatorja. To vkljuˇcuje integracijo treh elektro-servomotorjev z mikrokrmilnikom Arduino, ki bo s pomoˇcjo pripadajoˇce inercijske senzorike (IMU) krmilil elektromotorje. Naloga bo obsegala razumevanje in popis fizikalnih dogajanj, razvoj in vrednotenje mehanskih komponent, vkljuˇcevanje ˇze obstojeˇcih polizdelkov/izdelkov na trgu in sintezo programske kode, ki bo povezovala sistemske gradnike v funkcijsko celoto. Poleg tega bodo preverjene in ovrednotene stabilizacijske lastnosti sistema.

1

(15)

2 Teoretiˇ cne osnove

2.1 Orientacija v prostoru

Za uspeˇsno doloˇcanje potrebne rotacijske kompenzacije je najprej treba doloˇciti iz- hodiˇsˇcni koordinatni sistem in pravilno doloˇciti rotacijske kote. Pogosta praksa, sploh v aeronavtiki je, da se koordinatni sistem postavi, kot je prikazano na sliki 2.1.

Slika 2.1: Slika primarnih osi letala [1].

Pri tem velja, da je nagib rotacijski kôt okoli osi x, naklon rotacijski kôt okoli osi y in odklon rotacijski kôt okoli osi z. Osnova je torej desnosuˇcni koordinatni sistem, kateremu osz kaˇze v smeri gravitacijskega pospeˇska. Za doloˇcanje pozicije v tridimen- zionalnem prostoru bo uporabljena ena izmed dvanajstih moˇznih kombinacij Eulerjevih in Tait-Bryanovih kotov [2]. Pri doloˇcanju orientacije s pomoˇcjo rotacij je treba paziti na zaporedje rotacij in referenˇcni koordinatni sistem. Pri tej aplikaciji bo uporabljeno zaporedje Z, Y^′, X^′′. To pomeni, da bo orientacija telesa doloˇcena s pomoˇcjo treh intrinziˇcnih rotacij okoli treh osi. Torej se bo izhodiˇsˇcni koordinatni sistem rotiral skupaj z vsako rotacijo. Prva rotacija bo okoli osi z, temu bo sledila rotacija okoli osi y, pri ˇcem je upoˇstevano, da se je izhodiˇsˇcni koordinatni sistem rotiral skupaj s prvo rotacijo. Zato rotacija okoli osi y dobi apostrof in je kôt zapisan kot Y^′. Po rotaciji okoli osi y se je koordinatni sistem ponovno zasukal. Rotacija okoli osi x se izvede okoli novodobljenega koordinatnega sistema in, ker se je koordinatni sistem ˇze dvakrat zasukal, rotacija okoli osi x dobi dva apostrofa in se zapiˇse kot X^′′.

(16)

Teoretiˇcne osnove

2.2 Senzorji MEMS

Za merjenje orientacije v prostoru bosta uporabljena pospeˇskomer in ˇziroskop. Po- speˇskomer bo primarno sluˇzil za doloˇcitev smeri gravitacijskega pospeˇska, medtem ko bo ˇziroskop meril kotno hitrost. Za fuzijo meritev in konˇcno pridobitev orientacijskega kota bo uporabljen ustrezen filter.

Uporabljena senzorika se lahko oznaˇci kot MEMS. Pospeˇskomer deluje na principu sistema masa vzmet z eno prostorsko stopnjo (DOF) [3]. Ker gre za 3D senzor, imamo torej tri sisteme masa-vzmet; enega za vsako os. Sistem masa-vzmet je izdelan kot kon- denzator, tako da vsaka zunanja obremenitev spreminja kapacitivnost kondenzatorja, kar je mogoˇce spremeniti v elektriˇcni signal, kot je prikazano na sliki 2.2

Slika 2.2: Prikaz delovanja pospeˇskomera [4].

Ker so MEMS v osnovi majhni, lahko na sliki 2.3 vidimo, da se dimenzije samega zaznavala merijo v mikrometrih.

Slika 2.3: Posnetek pospeˇskomera z mikroskopom [4].

Ziroskop MEMS deluje na podobnem principu kondenzatorja kot sistema masa-vzmet,ˇ le da se pri ˇziroskopu izkoriˇsˇca Coriolisova sila [3], ki vpliva na referenˇcne nihajoˇce mase. Sprememba nihanja (oznaˇceno kot M_i) referenˇcnih mas, ki so prikazane na 3

(17)

Teoretiˇcne osnove sliki 2.4 z vplivom Coriolisove sile, doloˇca magnitudo in smer kotne hitrosti. Odvisno od kombinacije spremembe nihanja se lahko doloˇci okoli katere osi se rotacija dogaja.

Kasneje filter s pomoˇcjo ˇcasovne integracije lahko izraˇcuna kˆot rotacije.

Slika 2.4: Prikaz delovanja ˇziroskopa [5].

Podobno kot pospeˇskomer MEMS je tudi ˇziroskop podobnega videza pod mikroskopom, kar prikazuje slika 2.5.

Slika 2.5: Posnetek ˇziroskopa z mikroskopom [5].

2.3 Madgwickov filter

Za doloˇcitev orientacije v prostoru bo uporabljen Madgwickov filter. Filter omogoˇca fuzijo podatkov senzorjev IMU ali MARG in izraˇcun Eulerjevih ali kotov YPR. Ozadje Madgwickovega filtra je uporaba podatkov pospeˇskomera kot znano gravitacijsko referenco in integracija meritev ˇziroskopa po ˇcasu ter nadaljnji preraˇcun le teh v matrike znotraj kvaternionskega prostora [6]. Prednost takega izraˇcuna, kljub kompleksnim matematiˇcnim preraˇcunom v ozadju, je izogibanje robnih primerov, pri katerih v dru- gih sistemih izraˇcuna prihaja do poruˇsitve matematiˇcnih modelov. Dober primer tega je t.i. gimbal lock, ki zniˇza prostorsko stopnjo sistema in je posledica poravnave dveh ravnin npr. x-y in x-z. Preraˇcun v kvaternionskem prostoru se temu izogne. Postopek izraˇcuna Madgwickovega filtra je predstavljen v blokovnem diagramu na sliki 2.6.

(18)

Teoretiˇcne osnove

Slika 2.6: Blokovni diagram Madgwickovega filtra [6].

2.4 Rotacijska matrika

Rezultate Madgwickovega filtra, torej kote odklona, naklona in nagiba, bo potrebno z ustrezno rotacijsko matriko [7] pretvoriti v kote zasuka stabilizacijske platforme, ki bodo nato posredovani elektro-servomotorjem za rotacijsko kompenzacijo. Ob predpo- stavki, da sta rotacija okoli osiz oziroma odklon, vedno enaka za IMU in stabilizator, lahko rotacijo stabilizacijske platforme zapiˇsemo s pomoˇcjo rotacijske matrike predsta- vljene v enaˇcbi (2.1).

Rz(γ) =

⎡

⎣

cosγ sinγ 0

−sinγ cosγ 0

0 0 1

⎤

⎦ (2.1)

Koti IMU so predstavljeni z grˇskimi ˇcrkamiα,β inγ in predstavljajo nagib, naklon in odklon po vrsti. Prav takoX^′′,Y^′ in Z predstavljajo kote nagiba, naklona in odklona po vrsti, le da se nanaˇsajo na rotacijsko platformo.

⎡

⎣ X^′′

Y^′ Z

⎤

⎦=

⎡

⎣

cosγ sinγ 0

−sinγ cosγ 0

0 0 1

⎤

⎦

⎡

⎣ α β γ

⎤

⎦ (2.2)

⎡

⎣ X^′′

Y^′ Z

⎤

⎦ =

⎡

⎣

αcosγ+βsinγ βcosγ−αsinγ

γ

⎤

⎦

Z mnoˇzenjem matrik na desni strani enaˇcbe (2.2) se dobi matriko, ki izraˇza rotacije stabilizacijske platforme v odvisnosti od rotacij IMU. Z izpisom posameznih vrstic se

5

(19)

Teoretiˇcne osnove dobijo enaˇcbe (2.3), (2.4), (2.5), ki predstavljajo rotacijske kote stabilizacijske platforme odklon, naklon, nagib, izraˇzene kot koti, ki so rezultat meritev IMU.

Z =γ (2.3)

Y^′ =βcosγ−αsinγ (2.4)

X^′′=αcosγ+βsinγ (2.5)

2.5 Elektromotorji in krmilje

Za potrebe rotacijske kompenzacije bodo uporabljeni trije elektro-servomotorji. Elektro- servomotor je sestav, ki vkljuˇcuje DC elektromotor, reduktor in krmilno elektroniko [8].

Elektro-servomotor se krmili s signalom PWM, ki bo v dani aplikaciji proizveden z mikrokrmilnikom Arduino Uno.

Slika 2.7: Prikaz elektro-servomotorja [9].

Na sliki 2.7 je prikazan elektro-servomotor. Na levi strani slike 2.7 je viden tudi konek- tor, ki je sestavljen iz treh ˇzic. Rdeˇca in rjava ˇziˇcka sta moˇcnostni (+ in -), medtem ko je rumeno-oranˇzna ˇziˇcka signalna PWM [8]. S pomoˇcjo signala PWM je mogoˇce elektro-servomotorju doloˇciti kˆot zasuka osi.

(20)

3 Metodologija razvoja in vredno- tenja

3.1 Preliminarna doloˇ citev komponent

Za potrebe doloˇcanja orientacije v prostoru bo uporabljen MPU9250. Gre za senzor, ki vkljuˇcuje pospeˇskomer, ˇziroskop in magnetometer. Zaradi poˇcasne karakteristike magnetometra in njegove odvisnosti od okolice bo le ta zanemarjen. MPU9250 vse- buje tudi integrirani DMP, ki bo prav tako zanemarjen, saj je njegova hitrost proce- siranja nekoliko omejena [10]. Procesor merilnih signalov in glavni krmilnik elektro- servomotorjev bo mikrokrmilnik Arduino Uno. Za potrebe doloˇcitve geometrije bodo preliminarno doloˇceni tudi elektro-servomotorji, ki bodo naknadno po potrebi korigi- rani. Preliminarno je izbran elektro-servo motor MG996R.

3.1.1 Tehniˇ cne karakteristike komponent

Preglednica 3.1: Karakteristike MPU9250 [11].

Pospeˇskomer

Obmoˇcje ±2,±4, ±8 in ±16 g

Resolucija od 0,0060 m/s² do 0,06098 m/s² Inicialni pomik niˇcle X, Y: 60 mg; Z: 80 mg Inicialni faktor skalne variacije 3 %

Nelinearnost 0,5 %

Ziroskopˇ

Obmoˇcje ±250, ±500, ±1000 in±2000^◦/s Resolucija od 0,0060 m/s² do 0,06098 m/s² Inicialni pomik niˇcle ±5^◦/s

Inicialni faktor skalne variacije 3 %

Nelinearnost 0,1 %

7

(21)

Metodologija razvoja in vrednotenja Preglednica 3.2: Karakteristike MG996R [9].

MG996R

Masa 55 g

Dimenzije 40,7 x 19,7 x 42,9 mm

Zadrˇzni navor (T_stall) of 9,4 kgf·cm (4,8 V) do 11 kgf·cm (6 V) Kotna hitrost (ω_no−load) od 0,17 s/60^◦ (4,8 V) do 0,14 s/60^◦ (6 V)

Delovna napetost od 4,8 V do 7,2 V

El. tok od 500 mA do 900 mA (6 V)

Zadrˇzni tok 2,5 A (6 V)

Mrtva signalna dolˇzina 5 µs

Uleˇzajenje Dvojni krogliˇcni leˇzaj Temperaturni razpon od 0 ^◦C do 55 ^◦C

3.2 Doloˇ citev geometrije

Ob zastavljeni zahtevi izvedbe triosne stabilizacije z rotacijsko kompenzacijo z uporabo treh elektro-servomotorjev je smiselno dodati zahtevani objekt stabilizacije. Z namenom laˇzjega razvojnega dela bo izbran diskreten objekt, kar nikakor ne omejuje aplikacije izdelanega stabilizatorja samo na ta objekt. Zahtevani objekt stabilizacije naj bo akcijska kamera dimenzij 62 x 28 x 45 mm in mase 92,4 g. Akcijska kamera naj bo montirana na standardizirani montaˇzni platformi, ki omogoˇca nameˇsˇcanje na razliˇcnih povrˇsinah.

Po postopku izbire stabilizacijskega objekta bo treba zagotoviti njegovo nemoteno rotacijo. Poleg tega bo treba namestiti tri ortogonalno pozicionirane elektro-servomotorje, drˇzalo za roko uporabnika, prostor za baterije in elektroniko. Obenem je treba paziti, da je teˇziˇsˇce objektov, ki jih posamezen elektro-servomotor vrti, pozicionirano na osi rotacije elektro-servomotorja.

Doloˇcitev geometrije je vidna na sliki 3.1. Na sliki 3.1 so vidni tudi vsi glavni sestavni deli. ˇStevilka 1 predstavlja objekt stabilizacije, ki je v tem primeru akcijska kamera.

ˇStevilke 2, 3, in 4 predstavljajo stabilizacijske roˇcice. Roˇcica 2 je poimenovana roˇcica nagiba, roˇcica 3 opravlja funkcijo naklonske roˇcice, medtem ko roˇcica 4 predstavlja roˇcico odklona.

Elektro-servomotorji so predstavljeni s ˇstevilkami 5, 6 in 7. ˇStevilka 8 predstavlja drˇzaj;

ˇstevilka 9 predstavlja ohiˇsje elektronike, senzorike in baterije.

(22)

Metodologija razvoja in vrednotenja

Slika 3.1: Zajem zaslona iz CAD-programa NX.

3.3 Dimenzioniranje stabilizatorskih rok

Dimenzioniranje stabilizatorskih rok bo izvedeno na najbolj kritiˇcnih komponentah. Za analizo statiˇcnih obremenitev bo uporabljeno programsko simulacijsko okolje Ansys.

Uporabljen bo modul Static Structural. Najbolj kritiˇcna komponenta je odklonska roˇcica, oznaˇcena s ˇstevilko 4 na sliki 3.1.

3.3.1 Doloˇ citev obremenitev

S pomoˇcjo virtualnega dvojˇcka v programskem okolju NX je mogoˇce doloˇciti obremenitve, ki jih ˇcuti odklonska roˇcica. To je storjeno z doloˇcitvijo teˇziˇsˇca komponent in z znano maso komponent, ki jih nosi roˇcica odklona.

Na sliki 3.2 se vidi, kateri elementi so oznaˇceni in kje je njihovo teˇziˇsˇce (zeleni krog).

Z uporabo znaˇcilke Show Mass Properties Panel, se poleg prikazanega teˇziˇsˇca lahko odˇcitata tudi masa in poloˇzaj teˇziˇsˇca glede na izhodiˇsˇcni koordinatni sistem. Ker je poloˇzaj vpetja komponent na roˇcico odklona prav tako znan, je mogoˇce izraˇcunati navor, ki se prenaˇsa na odklonsko roˇcico in je posledica sile teˇze komponent, ki jih roˇcica odklona nosi.

S poenostavljenima statiˇcnima modeloma je doloˇcena obremenitev, ki jo roˇcica odklona obˇcuti.

Podatki, pridobljeni s pomoˇcjo znaˇcilke Show M ass P roperties P anel:

F_g = 4,3624 N y_t=−10,8779 mm y_t,yaw = 80 mm

9

(23)

Slika 3.2: Prikaz teˇziˇsˇca komponent, ki jih nosi odklonska roˇcica.

(a) (b)

Slika 3.3: Prikaz statiˇcnih modelov, uporabljenih za preraˇcun obremenitev roˇcice odklona.

L=y_t,yaw−y_t= 90,8779 mm L₂ = 36,9 mm

L₂ = 12,9 mm

S pomoˇcjo podatkov, s katerimi se reˇsijo statiˇcne enaˇcbe na sliki 3.3, so rezultati sledeˇci:

F_ya = 4,3624 N Fxa= 7,9237 N F_xb=−7,9237 N

3.3.2 Simulacija FEM

Pridobljene obremenitve v poglavju 3.3.1 se v simulacijskem okolju Ansys aplicirajo

(24)

(a) (b)

Slika 3.4: (a) Prikaz mreˇzenja med simulacijo v programu Ansys. (b) Prikaz podpor in obremenitev.

(a) (b)

Slika 3.5: Prikaz rezultatov analize FEM v Ansysu: (a) predstavlja deformacijo, (b) predstavlja napetosti po von-Misesu.

Osnovna velikost elementa je 0,005 m, pri ˇcem je na mestih, kjer je sprememba geometrije oˇcitna ali so napetosti velike, uporabljena enkratna izboljˇsava mreˇze. Na sliki 3.4b so prikazane obremenitve in vpetja strukture.

Po nastavljenih robnih pogojih in mreˇzenju se izvede statiˇcna simulacija (Static Structural).

Rezultati so predstavljeni na sliki 3.5. Na sliki 3.5a se vidi skupna deformacija. Zgornji del ima maksimalno deformacijo 4 mm. Maksimalna napetost rahlo presega 7 MPa kar je razvidno iz slike 3.5b. Napetosti so krepko pod natezno trdnostjo uporabljenega materiala (PLA), ki znaˇsa 40 MPa [12]. Glede na to, da bo izdelovalna tehnika za nestandardne komponente 3D tisk, je treba paziti, da imajo pri 3D tisku deli v glav- nem deloma votlo notranjost z doloˇceno matrico/mreˇzo, ki skrbi za trdnost. Vendar sodeˇc po viru [13] zniˇzanje trdnosti pri najmanj gostih matricah ni veˇcje od 30 %.

To pomeni, da bodo dano obremenitev zdrˇzali tudi deli z najmanj gosto trdnostno mreˇzico/matriko.

11

(25)

3.4 Dimenzioniranje elektro-servomotorjev

Pri dimenzioniranju elektro-servomotorjev bo osnova za izraˇcun preliminarno doloˇcen MG996R. Tekom izraˇcuna bo preverjeno, kako se bo dani elektro-servomotor obnaˇsal v sistemu.

Dimenzioniranje bo izvedeno na odklonskem motorju, saj le ta rotira najveˇcjo vztrajnost.

Iz preglednice 3.2 je mogoˇce odˇcitati zadrˇzni navor in kotno hitrost brez obremenitve.

Ker je proizvajalec koliˇcine podal v nestandardnih enotah, se te pretvori v Nm in rad/s.

T_stall = 11 kgf·cm = 11·g·10⁻² = 11·9,81·10⁻² = 1,0791 Nm w_{no load} = 0,14 s/60^◦ =

(︃0,14 60

)︃⁻¹

· π

180 = 7,48 rad/s

Iz digitalnega dvojˇcka se v programskem okolju NX s pomoˇcjo znaˇcilke Show Mass Properties Panel odˇcita masni vztrajnostni moment globalne osi z. S pomoˇcjo Stei- nerjevega pravila se doloˇci, kolikˇsen je masni vztrajnostni moment okoli odklonske osi elektro-servomotorja. Iz programa odˇcitani masni vztrajnostni moment je:

I_zz = 2400,34 kg mm²

Masa elementov, ki jih odklonski motor rotira je:

m= 0,5607 kg.

Razdalja med osjo odklonskega motorja in globalno osjoz je:

e= 10 mm

S pomoˇcjo Steinerjevega pravila, zapisanega v enaˇcbi 3.1 [14], se izraˇcuna dejanski masni vztrajnostni moment okoli osi odklonskega motorja. V enaˇcbi 3.1 je J_a vztrajnostni moment na osi vzporedni z osjo, ki gre skozi teˇziˇsˇce telesa, J_t je po drugi strani vztrajnostni moment na osi skozi teˇziˇsˇce; epredstavlja razdaljo med tem dvema osema.

J_a=J_t+m·e² (3.1)

J_t=J_a−m·e²

J_t= 2400,34−0,5607·10² J_t= 2,3443·10⁻³ kg m²

Ob priporoˇcilih izbire elektromotorja, da je T ≤ T_stall

in ω ≥ ω_stall

[15], je moˇc

(26)

Metodologija razvoja in vrednotenja enak poloviˇcnemu T_stall in maksimalna kotna hitrost enaka ω_stall. To je izvedljivo z uporabo enaˇcbe 3.2 [15].

T_m =J_L· ∆ω

∆t (3.2)

∆t= J_L·∆ω T_m

∆t= 2,3443·10⁻³ ·7,48 1,0791

2

∆t= 32,5 ms

Iz reˇsitve enaˇcbe 3.2 je mogoˇce sklepati, da bo ˇcas, potreben za pospeˇsek do maksi- malne kotne hitrosti elektro-servomotorja zadostno majhen in bo omejitveni faktor maksimalna kotna hitrost motorja.

3.5 Elektriˇ cno vezje

Vse komponente je treba med seboj povezati v delujoˇce elektriˇcno vezje. Shema ele- ktriˇcnega vezja je predstavljena na sliki 3.6. Glavna komponenta elektriˇcne sheme je mikrokrmilnik Arduino Uno. Na mikrokrmilnik so povezani trije elektromotorji in sicer njihovi terminali PWM. Ti so povezani na digitalne prikljuˇcke 9, 10 in 11, saj omogoˇcajo pulzno-ˇsirinsko modulacijo. Poleg tega so vsi negativni terminali elektro- servomotorjev povezani na prikljuˇcek GND Arduina za referenco signala PWM.

Izvir moˇcnostnega dela vezja je 6V-baterija, ki skrbi za delovanje elektromotorjev in napajanje mikrokrmilnika, natanˇcneje je njen pozitivni terminal povezan na pozitivne terminale elektromotorjev in na pozitivni terminal step-up DC-DC transformatorja.

Negativni terminali elektromotorjev in DC-DC transformatorja so povezani na negativni terminal baterije. Vodnik, ki vodi s pozitivnega terminala baterije, je povezan z drsnim stikalom, ki omogoˇca izklop/vklop napajanja celotnega sistema. Arduino dobi napetost z izhodnih terminalov transformatorja DC-DC in sicer pozitivni terminal transformatorja se poveˇze na prikljuˇcek Vin, negativni pa na poljubni GND prikljuˇcek, saj so vsi med seboj povezani.

Poleg navedenih komponent elektriˇcno vezje vkljuˇcuje tudi dva senzorja MPU9250.

Za njuno napajanje je poskrbljeno z Arduinom, in sicer sta prikljuˇcka Vcc na obeh MPU9250 povezana na 5V-prikljuˇcek. Prikljuˇcka GND na MPU9250 sta povezana na GND na Arduinu. Terminala SCL in SDA, ki omogoˇcata komunikacijski protokol I2C, sta povezana na pripadajoˇca terminala SCL SDA na Arduinu ali na prikljuˇcka A5 in A4, ki imata isto vlogo. Eden od dveh MPU9250 ima ˇse dodatno napetost 3,3 V pripeljano na prikljuˇcek ADO z Arduinovega prikljuˇcka 3,3 V zato, da se mu naslov I2C spremeni z 0x68 na 0x69, sicer bi bila komunikacija hkrati z dvema MPU9250 nemogoˇca.

13

(27)

UNO_R3 MC

1D8 2D9 3D10 4D11 5D12 6D13 7 GND 8 AREF 9 SDA 10SCL 11D0 12D1 13D2 14D3 15D4 16D5 17D6 18D7

A0 19 A1 20 A2 21 A3 22 A4 23 A5 24

POWER 25RESET 27IOREF 26GND 30GND 313V3 28VIN 325V 29

MPU9250 SENS1 1VCC 2GND 3SCL 4SDA 5EDA 6ECL 7ADD 8INT 9NCS

FSYNC 10 DC-DC (6V -> 7V)

STEP_UP

VIN VIN+

GND VIN-

VOUT+ VOUT VOUT- GND

MG996R - Nagib M09

PWM+- 123

MG996R - Naklon M10

PWM+- 123

MG996R - Odklon M11

PWM+- 123 4XAA - Baterija - 6V

BAT1

Drsno stikalo SW1

2 13

MPU9250 SENS2 1VCC 2GND 3SCL 4SDA 5EDA 6ECL 7ADD 8INT 9NCS

FSYNC 10

Slika 3.6: Shema elektriˇcnega vezja. Izdelano v programu EasyEDA.

Na sliki 3.7 je prikazano ohiˇsje elektronike s komponentami. ˇStevilka ena predstavlja mikrokrmilnik Arduino Uno, ˇstevilka dve predstavlja step-up DC-DC transformator.

ˇStevilka tri je senzor MPU9250, ˇstevilka ˇstiri pa predstavlja ˇstiri baterije AA.

Slika 3.7: Prikaz elektronskih komponent.

(28)

3.6 Programska koda

Programska koda je naloˇzena na mikrokrmilnik Arduino in skrbi za DAQ in krmiljenje.

Programska koda je napisana v aplikaciji Arduino IDE, ki hkrati sluˇzi za nalaganje programa na mikrokrmilnik in serijsko komunikacijo s slednjim. V programski kodi so uporabljene knjiˇznice [16] [17] [18]. Celotna koda je zaradi preglednosti predstavljena v prilogi A.

V kodi se sprva vkljuˇcijo potrebne knjiˇznice. Temu sledi doloˇcitev objektov za servo, filter in senzorje. Po doloˇcitvi potrebnih objektov iz knjiˇznic se definirajo vse potrebne spremenljivke.

V delusetup se sprva vsi elektro-servomotorji postavijo v nevtralen poloˇzaj. Naslednji korak je nastavitev senzorja MPU9250. Med drugim je treba nastaviti obmoˇcje meritev ˇziroskopa in pospeˇskomera ter frekvenco registra FIFO. Nato je treba roˇcno nastaviti frekvenco vzorˇcenja samega zaznavala, saj knjiˇznica tega ne dopuˇsˇca.

Po nastavitvah se preveri odzivnost senzorja, in ˇce se zgodi napaka, se ta izpiˇse na serijskem monitorju.

Temu sledi umerjanje senzorja in zapis napake. Za umerjanjem se zaˇzene filter, ki mu je zavoljo pravilnega delovanja treba doloˇciti enako frekvenco, kot je vzorˇcenje senzorja.

V loop delu programske kode se najprej osveˇzijo meritve in zapiˇsejo v spomin. Hkrati se jim tudi odˇsteje napaka, ki je bila izmerjena pri umerjanju. Meritve se nato poˇslje v filter, ki doloˇci kote zasuka (odklon, naklon, nagib). Kote zasuka se nato posreduje elektro-servomotorjem, da izvedejo primerne rotacijske kompenzacije, pri ˇcemer je upoˇstevano razmerje med potrebno rotacijsko kompenzacijo in izmerjenimi koti, ki je predstavljeno v enaˇcbah (2.3), (2.4) in (2.5). S pomoˇcjo spremenljivk, ki so namenjene sledenju ˇcasa, potekata zapis meritev v spomin in osveˇzitev filtra, z istim ˇcasovnim intervalom kot meritve na senzorju. Za potrebe vrednotenja se meritve izpiˇsejo tudi v serijski monitor.

3.7 Vrednotenje

Ko je celoten stabilizator sestavljen, je treba tudi delovanje le tega ovrednotiti. Na sliki 3.8 je mogoˇce videti fiziˇcni prototip stabilizatorja. Na le tem bodo izvajane meritve namenjene vrednotenju sistema stabilizacije. Merilni mesti sta dve. Eno se nahaja v ohiˇsju elektronike, drugo na stabilizacijski platformi. Na zaˇcetku vrednotenja bo pote- klo umerjanje tako pospeˇskomera kot ˇziroskopa. S tem bo potrjeno pravilno delovanje pospeˇskomera in ˇziroskopa.

Na sliki 3.9 sta vidni merilni mesti. ˇStevilka 1 predstavlja merilno mesto stabilizacijska platforma oz. stab. platf. ˇStevilka 2 predstavlja merilno mesto sistem. Enaka nomenklatura bo uporabljena v poglavju 4.

Sprva bo preverjena umerjenost senzorja, primerjane bodo meritve pred in po umerjanju. Tako bo potrjeno oz. preverjeno pravilno delovanje senzorike. Za razliko od 15

(29)

Slika 3.8: Fiziˇcni prototip.

Slika 3.9: Prikaz merilnih mest.

kasnejˇsih meritev, kjer bo merjena orientacija v prostoru, bo v primeru vrednotenja umerjanja izhodna velikost identiˇcna izhodni velikosti senzorja. Pri pospeˇskomeru bo tako merjen pospeˇsek izraˇzen v G, pri ˇcem velja, da je en G enak gravitacijskemu pospeˇsku. Po drugi strani bo ˇziroskop meril kotno hitrost.

Kontrolna meritev rotacije bo izvedena ob izklopljenih elektro-servomotorjih in bo sluˇzila za potrditev, da sta oba senzorja identiˇcna in da je ob nedelovanju sistema njuna rotacija enaka. Izvedba kontrolne meritve je prikazana na sliki 3.10. Kontrolna meritev in kasnejˇse meritve namenjene vrednotenju stabilizacijskih lastnosti sistema bodo izvedene tako, da se bo sistem roˇcno rotiral pribliˇzno 45 stopinj v pozitivno in

(30)

Slika 3.10: Kontrolna meritev.

(a) (b)

Slika 3.11: Meritev z uravnoteˇzenim stabilizacijskim bremenom (a) in meritev z neuravnoteˇzenim stabilizacijskim bremenom (b).

Ob delujoˇcem sistemu stabilizacije brez stabilizacijskega objekta oz. bremena bosta izmerjena rotacija sistema in odziv na stabilizacijski platformi. Temu bo sledila meritev s stabilizacijskim bremenom, ki je uravnoteˇzeno. Videz stabilizacijskega sistema z urav- noteˇzenim bremenom je prikazan na sliki 3.11a. Zadnja meritev bo izvedena z bremenom in s pomaknjenimi stabilizacijskimi roˇcicami iz ravnovesne lege tako, da so teˇziˇsˇca bremen vsakega elektro-servomotorja izven njegove osi rotacije. Teˇziˇsˇca bremen bodo izmaknjena, kolikor dopuˇsˇcajo montaˇzni pogoji. Pri izbiri smeri pomika bo upoˇstevano, da se pri premiku teˇziˇsˇca minimizira vpliv prostega premika elektro-servomotorja. Pre- mik teˇziˇsˇca bremena bo v nevtralnem poloˇzaju poveˇcal masni vztrajnostni moment na odklonskem in nagibnem motorju, medtem ko bo pri naklonskem motorju deloval ˇse moment okoli osi rotacije, ki je posledica teˇze bremena. Na sliki 3.11b je prikazan videz stabilizacijskega sistema z bremeni, pomaknjenimi izven ravnoteˇzja.

17

(31)

4 Rezultati in diskusija

Umerjanje ˇziroskopa in pospeˇskomera je predstavljeno na slikah 4.1 in 4.2. Umerjanje izvede premik niˇcle. Na vseh grafih umerjanja je vidno, da je uspeˇsno. Na meritvah ˇziroskopa, ki so vidne na sliki 4.1, je viden ˇsum. ˇCeprav je na grafu ˇziroskopa Z, ki je viden na sliki 4.1c, ˇsum nekoliko manjˇsi, ima ta po umerjanju ˇse vedno rahel premik niˇcle v negativni smeri.

Sodeˇc po rezultatih meritev ob izklopljenem sistemu stabilizacije, ki sluˇzi kot kontrolna meritev, sta merilna senzorja delujoˇca. Poleg tega je na podlagi meritev mogoˇce sklepati, da ob nedelujoˇcem sistemu stabilizacije, tako merilno mesto sistem, kot merilno mestostabilizacijska platformadoˇzivljata enako rotacijo. Na grafu odklona na sliki 4.3c je po osmi sekundi vidno rahlo odstopanje dveh meritev. Vzrok za to je odsotnost refe- renˇcne smeri. Pri nagibu in naklonu gravitacijski pospeˇsek sluˇzi kot referenca, ker je pa ravnina odklona pravokotna na gravitacijski pospeˇsek, je ta neuporaben kot referenca.

Rezultati meritev z vklopljenim sistemom stabilizacije in brez bremena oz. stabilizacijskega objekta so prikazani na sliki 4.4. Iz meritev se vidi, da sistem stabilizacije uspeˇsno kompenzira rotacijo, saj je kljub rotaciji sistema stabilizacijska platforma skoraj brez zasuka.

Pri meritvah z uravnoteˇzenim stabilizacijskim objektom, ki so prikazane na sliki 4.5, je vidna oˇcitno poˇcasnejˇsa odzivnost na stabilizacijski platformi v primerjavi z meritvami brez bremena. Vzrok za to je veˇcja vztrajnost, ki jo premagujejo elektro-servomotorji.

(a) (b) (c)

(32)

Rezultati in diskusija

(a) (b) (c)

Slika 4.2: Umerjanje pospeˇskomera.

(a) (b) (c)

Slika 4.3: Meritev rotacije brez delovanja sistema rotacijske kompenzacije.

(a) (b) (c)

Slika 4.4: Meritev rotacije brez stabilizacijskega objekta.

19

(33)

Rezultati in diskusija

(a) (b) (c)

Slika 4.5: Meritev rotacije s stabilizacijskim objektom (z bremenom), ki je uravnoteˇzen.

(a) (b) (c)

Slika 4.6: Meritev rotacije s stabilizacijskim objektom (z bremenom), ki ni uravnoteˇzen.

Poleg tega je oˇciten tudi tresljajoˇci pomik zlasti pri meritvah naklona, ki so vidne na sliki 4.5b. Razlog za to verjetno tiˇci v tem, da so bremena, ki jih vrtijo elektro- servomotorji uravnoteˇzena. To bremenu omogoˇca oscilacijo znotraj mrtve signalne dolˇzine elektro-servomotorja.

Meritve z bremenom oz. stabilizacijskim objektom, ki ni uravnoteˇzen, so prikazane na sliki 4.6. Odzivna karakteristika priˇcakovano kaˇze enake ali veˇcje magnitude rotacije, vendar so veˇcja vztrajnost in dodatni momenti zaradi ekscentra izniˇcili oscilacije v mrtvi coni, ki so bili prisotni v meritvah z uravnoteˇzenim bremenom prikazanim na sliki 4.5.

(34)

5 Zakljuˇ cki

Tekom naloge je bilo opravljeno:

1. Razvit in ovrednoten je bil stabilizacijski sistem, ki vkljuˇcuje elektriˇcno vezje, programsko opremo in mehanske komponente. Pokazano je bilo, da izbrane komponente lahko skupaj tvorijo delujoˇc stabilizacijski sistem. Izvedeno dimenzioniranje mehanskih komponent se je izkazalo za pravilno, saj so vse mehanske komponente nosile obremenitve brez poruˇsitve. Uspeˇsno je bilo opravljeno za- jemanje podatkov, obdelovanje podatkov in krmiljenje elektro-servomotorjev s pomoˇcjo mikrokrmilnika.

2. Izmerjene so bile stabilizacijske lastnosti sistema. Izkazalo se je, da sistem deluje najbliˇzje idealnemu brez stabilizacijskega bremena. Najslabˇse stabilizacijske lastnosti je sistem imel, ko je stabiliziral breme, ˇcigar teˇziˇsˇce je bilo kolinearno z osjo rotacije elektro-servomotorjev oz. je bilo uravnoteˇzeno.

3. Ugotovljeno je, da izbrani elektro-servomotorji povzroˇcajo oscilacije, ˇce je teˇziˇsˇce bremena kolinearno z osjo rotacije. Boljˇsa stabilizacijska odzivnost je bila doseˇzena z ekscentriˇcnim poloˇzajem bremena.

Na podlagi zakljuˇckov ugotovimo, da delo predstavlja koherentni pregled razvoja stabilizacijskega sistema ter njegovega dimenzioniranja in vrednotenja. Zakljuˇcki, ki se nanaˇsajo na samo uravnoteˇzevanje bremen elektromotorjev so vsekakor pomembni, saj predstavljajo kontraintuitiven sklep o tem, da popolnoma uravnoteˇzeno obremenjen elektro-servomotor ne bo nujno imel najboljˇse delovne karakteristike.

Predlogi za nadaljnje delo

V nadaljnjem delu bi bilo mogoˇce implementirati BLDC motorje, ki so precej pogosti za podobne namene v industriji. Poleg tega bi bilo mogoˇce narediti raziskavo iskanja optimalnega poloˇzaja elektro-servomotorja v smislu ekscentriˇcnosti teˇziˇsˇca bremena.

Vsekakor ˇse ostaja prostor za nadaljnji razvoj same geometrije izdelka in napredek v senzoriki z integracijo veˇcih senzorjev.

21

(35)

Literatura

[1] NASA-Glen-Research-Center, Aircraft Rotations Body Axes. dostopno na:

https://www.grc.nasa.gov/www/k-12/airplane/rotations.html

[2] F. L. Markley in J. L. Crassidis,Fundamentals of spacecraft attitude determination and control, 2014.

[3] LastMinuteEngineers.com, Interface MPU6050 Accelerometer and Gyroscope Sensor with Arduino, 2021. dostopno na: https://lastminuteengineers.com/

mpu6050-accel-gyro-arduino-tutorial/

[4] M. Andrejaˇsiˇc in I. Poberaj, MEMS ACCELEROMETERS, 3 2008.

[5] STMicroelectronics, Everything about STMicroelectronics’ 3-axis digital MEMS gyroscopes, 2011. dostopno na: https://www.elecrow.com/download/TA0343.pdf [6] S. Madgwick, “An efficient orientation filter for inertial and inertial/magnetic

sensor arrays,” Report x-io Technologies, 2010.

[7] E. W. Weisstein, Rotation Matrix, 7. dostopno na: https://mathworld.wolfram.

com/RotationMatrix.html

[8] A. Bhargava in A. Kumar, “Arduino controlled robotic arm,” let. 2017-January, 2017.

[9] ElectronicosCaldas, MG996R High Torque Metal Gear Dual Ball Bearing Servo. dostopno na: https://www.electronicoscaldas.com/datasheet/MG996R Tower-Pro.pdf

[10] T. Charlotte in van Wietmarschen Luc,Position and orientation determination of a probe with use of the IMU MPU9250 and a ATmega328 microcontroller, 6 2016.

[11] J. M. Núñez, M. G. Araújo in I. Garc´ıa-Tuñón, “Real-time telemetry system for monitoring motion of ships based on inertial sensors,” Sensors (Switzerland), let. 17, 2017.

[12] MatWeb-MaterialPropertyData,Overview of materials for Polylactic Acid (PLA) Biopolymer, 2021.

[13] L. Baich in G. Manogharan, “Study of infill print parameters on mechanical

(36)

Literatura [14] B. Kraut,Krautov strojniˇski priroˇcnik, 16. izd., J. Puhar in J. Stopnik, ur. Buˇca,

2017.

[15] R. Vrabiˇc, “Izbira dc motorja in uvod v kibernetiko - osnove mehatronike,” 2021.

[16] H. Tai, MPU9250 Library for Arduino. dostopno na: https://github.com/

hideakitai/MPU9250

[17] FreeSoftwareFoundation-Inc, Servo Library for Arduino. dostopno na: https:

//github.com/arduino-libraries/Servo

[18] ArduinoLL, Madgwick Library, 2016. dostopno na: https://github.com/

arduino-libraries/MadgwickAHRS

23

(37)

Priloga A

Programska koda.

#include <MadgwickAHRS.h>

#include <MPU9250.h>

#include <Servo.h>

Servo servo_09;

Servo servo_10;

Servo servo_11;

Madgwick filter;

Madgwick filter_1;

MPU9250 mpu;

MPU9250 mpu_1;

float Angle_0 = 90;

unsigned long microsPerReading, microsPrevious, microsNow;

int yaw_error = 0;

float ax, ay, az;

float gx, gy, gz;

float mx, my, mz;

float roll, pitch, yaw, heading;

float roll_1, pitch_1, yaw_1, heading_1;

float drift_gx, drift_gy, drift_gz;

float drift_ax, drift_ay, drift_az;

float drift_gx_1, drift_gy_1, drift_gz_1;

float drift_ax_1, drift_ay_1, drift_az_1;

int divisor = 49;

(38)

Priloga A

float rd = 180 / 3.141592654;

float dr = 3.141592654 / 180;

float gacc = 9.81;

float rolldr, pitchdr, yawdr;

void setup() {

servo_09.write(90);

servo_10.write(90);

servo_11.write(90);

servo_09.attach(9);

delay(250);

servo_10.attach(10);

delay(250);

servo_11.attach(11);

delay(250);

Serial.begin(9600);

Wire.begin();

delay(4000);

MPU9250Setting setting;

setting.accel_fs_sel = ACCEL_FS_SEL::A4G;

setting.gyro_fs_sel = GYRO_FS_SEL::G500DPS;

setting.mag_output_bits = MAG_OUTPUT_BITS::M16BITS;

setting.fifo_sample_rate = FIFO_SAMPLE_RATE::SMPL_125HZ;

setting.gyro_fchoice = 0x03;

setting.gyro_dlpf_cfg = GYRO_DLPF_CFG::DLPF_41HZ;

setting.accel_fchoice = 0x01;

setting.accel_dlpf_cfg = ACCEL_DLPF_CFG::DLPF_45HZ;

Wire.beginTransmission(0x69);

Wire.write(0x19);

Wire.write(0x31);

Wire.endTransmission(true);

Wire.beginTransmission(0x68);

Wire.write(0x19);

Wire.write(0x31);

Wire.endTransmission(true);

25

(39)

Priloga A if (!mpu.setup(0x69, setting)) {

while (1) {

Serial.println("MPU povezava neuspeˇsna.");

delay(3000);

} }

if (!mpu_1.setup(0x68, setting)) { while (1) {

Serial.println("MPU povezava neuspeˇsna.");

delay(3000);

} }

mpu.setup(0x69, setting);

mpu.calibrateAccelGyro();

delay(1000);

drift_gx = mpu.getGyroBiasX()/(float)MPU9250::CALIB_GYRO_SENSITIVITY;

drift_gy = mpu.getGyroBiasY()/(float)MPU9250::CALIB_GYRO_SENSITIVITY;

drift_gz = mpu.getGyroBiasZ()/(float)MPU9250::CALIB_GYRO_SENSITIVITY;

drift_ax = mpu.getAccBiasX()/(float)MPU9250::CALIB_ACCEL_SENSITIVITY;

drift_ay = mpu.getAccBiasY()/(float)MPU9250::CALIB_ACCEL_SENSITIVITY;

drift_az = mpu.getAccBiasZ()/(float)MPU9250::CALIB_ACCEL_SENSITIVITY;

mpu_1.setup(0x68, setting);

mpu_1.calibrateAccelGyro();

delay(1000);

drift_gx_1=mpu_1.getGyroBiasX()/(float)MPU9250::CALIB_GYRO_SENSITIVITY;

drift_gy_1=mpu_1.getGyroBiasY()/(float)MPU9250::CALIB_GYRO_SENSITIVITY;

drift_gz_1=mpu_1.getGyroBiasZ()/(float)MPU9250::CALIB_GYRO_SENSITIVITY;

drift_ax_1=mpu_1.getAccBiasX()/(float)MPU9250::CALIB_ACCEL_SENSITIVITY;

drift_ay_1=mpu_1.getAccBiasY()/(float)MPU9250::CALIB_ACCEL_SENSITIVITY;

drift_az_1=mpu_1.getAccBiasZ()/(float)MPU9250::CALIB_ACCEL_SENSITIVITY;

// zagon IMU in filtra filter.begin(samplerate);

filter_1.begin(samplerate);

// spremenljivke ˇcasa za pravilno frekvenco vzorˇcenja microsPerReading = 1000000 / samplerate;

microsPrevious = micros();

}

(40)

Priloga A

void loop() {

mpu.update();

mpu_1.update();

microsNow = micros();

if (microsNow - microsPrevious >= microsPerReading) { /////// SENZOR 1 - M.M. Sistem

ax = mpu.getAccX() - drift_ax;

ay = mpu.getAccY() - drift_ay;

az = mpu.getAccZ() - drift_az;

gx = mpu.getGyroX() - drift_gx;

gy = mpu.getGyroY() - drift_gy;

gz = mpu.getGyroZ() - drift_gz;

filter.updateIMU(gx, gy, gz, ax, ay, az);

roll = filter.getRoll();

pitch = filter.getPitch();

heading = filter.getYaw();

yaw = heading - 180;

/////// SENZOR 2 - M.M. Stab. platf./////

ax = mpu_1.getAccX() - drift_ax_1;

ay = mpu_1.getAccY() - drift_ay_1;

az = mpu_1.getAccZ() - drift_az_1;

gx = mpu_1.getGyroX() - drift_gx_1;

gy = mpu_1.getGyroY() - drift_gy_1;

gz = mpu_1.getGyroZ() - drift_gz_1;

filter_1.updateIMU(gy, -gx, gz, ay, -ax, az);

roll_1 = filter_1.getRoll();

pitch_1 = filter_1.getPitch();

heading_1 = filter_1.getYaw();

yaw_1 = heading_1 - 180;

rolldr = roll*dr;

pitchdr = pitch*dr;

27

(41)

Priloga A

yawdr = yaw*dr;

if((roll < 85) && (roll > -85)) { if((pitch < 85) && (pitch > -85)){

if((yaw < 85) && (yaw > -85)){

servo_09.write(Angle_0-yaw);

servo_10.write(Angle_0-(roll*sin(yawdr)+pitch*cos(yawdr)));

servo_11.write(Angle_0+(roll*cos(yawdr)-pitch*sin(yawdr)));

} } }

Serial.print(microsNow);

Serial.print(",");

Serial.print(roll);

Serial.print(",");

Serial.print(pitch);

Serial.print(",");

Serial.println(yaw);

Serial.print(",");

Serial.print(roll_1);

Serial.print(",");

Serial.print(pitch_1);

Serial.print(",");

Serial.println(yaw_1);

microsPrevious = microsPrevious + microsPerReading;

} }

(42)