Estudi per a la fusió de dades de posició i actitud en un multirotor AscTec Hummingbird
Abstract
Estudi dels protocols de comunicació de dispositius GPS convencionalsEstudi del software de control del Hummingbird, apartat de lectura de sensorsIntegració del MB100 i modificació del software si convéEstudi del software de control del Hummingbird, apartat de fusió de dadesEstudi del mètodes de fusió de dades (observadors de Luenberger, filtre de Kalman, filtre estés de Kalman)Integració de les noves dades i modificació del softwares si convé
Full text
ETSEIAT Estudi per a la fusió de dades de posició i actitud en un multirotor AscTec Hummingbird (Memòria) Pol Capella Roca Treball Final de Grau Convocatòria: Octubre del 2015 Director: Bernardo Morcego Seix Codirector: Josep Cugueró Escofet Grau en Enginyeria de Vehicles Aeroespacials
Pol Capella Roca Índex de continguts 1. Introducció ................................................................................................ 9 1.1. Justificació ............................................................................................. 9 1.2. Objectius ............................................................................................. 10 1.3. Abast de l’estudi .................................................................................. 10 1.4. Requeriments ...................................................................................... 12 2. Estat de l’art ............................................................................................ 13 2.1. Quadrotors .......................................................................................... 13 2.1.1. Introducció .................................................................................... 13 2.1.2. Principis de vol ............................................................................. 15 2.2. Fusió de dades .................................................................................... 19 3. AscTec Hummingbird .............................................................................. 21 3.1. Introducció ........................................................................................... 21 3.2. Característiques .................................................................................. 22 3.3. Hardware ............................................................................................. 23 3.3.1. Processadors ............................................................................... 23 3.3.2. Sensors ........................................................................................ 25 3.3.3. Mòduls de comunicació Xbee ....................................................... 27 3.3.4. Control Remot .............................................................................. 28 3.4. Sistemes de referència ........................................................................ 29 3.5. Software .............................................................................................. 30 3.5.1. Software de control ....................................................................... 30 3.5.2. Eines de desenvolupament .......................................................... 30 4. Tecnologia RTK ...................................................................................... 31 4.1. Introducció ........................................................................................... 31 4.2. Sistemes de Navegació per Satèl·lit .................................................... 31 4.2.1. Xarxes GNSS ............................................................................... 33 4.2.2. Senyal GNSS ............................................................................... 35
Pol Capella Roca 4.2.3. Fonts d’error ................................................................................. 38 4.2.4. Sistemes d’augmentació GNSS .................................................... 39 4.2.5. GNSS diferencial amb tecnologia RTK ......................................... 39 5. AshTech MB100...................................................................................... 42 5.1. Característiques .................................................................................. 42 5.2. Hardware ............................................................................................. 44 5.2.1. Antenes ........................................................................................ 45 5.2.2. Kit d’avaluació .............................................................................. 45 5.3. Protocol de comunicació ...................................................................... 48 5.3.1. NMEA0183 ................................................................................... 48 6. Geometria terrestre ................................................................................. 52 6.1. El geoide ............................................................................................. 52 6.2. L’el·lipsoide ......................................................................................... 53 6.3. Comparativa el·lipsoide – geoide ......................................................... 54 6.4. El Datum .............................................................................................. 55 6.5. Coordenades geogràfiques.................................................................. 56 7. Observadors d’estat ................................................................................ 58 7.1. Introducció ........................................................................................... 58 7.2. Observabilitat de sistemes en temps discret ........................................ 61 7.3. Observadors de Luenberger ................................................................ 62 7.4. Filtre de Kalman .................................................................................. 63 7.4.1. Sistemes lineals estocàstics ......................................................... 63 7.4.2. Algoritme d’estimació.................................................................... 67 7.5. Filtre estès de Kalman ......................................................................... 69 7.6. Exemple didàctic: pèndul invertit.......................................................... 70 7.6.1. Model ........................................................................................... 71 7.6.2. Simulació ...................................................................................... 73 7.6.3. Resultats ...................................................................................... 77 8. Fusió de dades ....................................................................................... 83
Pol Capella Roca 8.1. Introducció ........................................................................................... 83 8.2. Sistemes de referència ........................................................................ 84 8.2.1. Body Frame .................................................................................. 84 8.2.2. Earth Frame ................................................................................. 85 8.2.3. Relacions entre EF y BF ............................................................... 86 8.3. Model .................................................................................................. 88 8.3.1. Particularitats ................................................................................ 91 8.3.2. Identificació de les pertorbacions .................................................. 92 8.3.3. Mesures ....................................................................................... 95 8.3.4. Linealització del model en espai d’estats .................................... 103 8.4. Recepció de dades ............................................................................ 109 8.4.1. Sensors de la IMU ...................................................................... 109 8.4.2. Sensor RTK ................................................................................ 109 8.5. Validació experimental ....................................................................... 121 8.5.1. Model simulink ............................................................................ 121 8.5.2. Plataforma .................................................................................. 122 8.5.3. Resultats .................................................................................... 123 9. Impacte ambiental ................................................................................. 134 10. Resum del pressupost........................................................................... 135 11. Conclusions .......................................................................................... 136 12. Treball futur ........................................................................................... 138 13. Planificació ............................................................................................ 139 13.1. Planificació del projecte.................................................................. 139 13.2. Planificació del treball futur ............................................................ 139 14. Bibliografia ............................................................................................ 140
Pol Capella Roca Índex de figures Figura 1: arquitectura clàssica dels quadrotors .................................................. 13 Figura 2: equilibri del quadrotor [2] .................................................................... 15 Figura 3: empenta global del quadrotor [2]......................................................... 15 Figura 4: tracció del quadrotor [3] ...................................................................... 16 Figura 5: moviment de balanceig [3] .................................................................. 17 Figura 6: moviment de capcineig [3] .................................................................. 17 Figura 7: moviment de guinyada [3] ................................................................... 18 Figura 8: efecte de la inclinació del quadrotor [2] ............................................... 18 Figura 9: AscTec Hummingbird [39] ................................................................... 21 Figura 10: placa Auto Pilot de l’AscTec Hummingbird [10] ................................ 24 Figura 11: esquema de funcionament de la placa Auto Pilot. [10] ...................... 25 Figura 12: Control remot [40] ............................................................................. 28 Figura 13:sistema de referència de l’ATH [1] ..................................................... 29 figura 14: eixos cos del quadrotor [1] ................................................................. 29 Figura 15: esquema del funcionament del GPS ................................................. 32 Figura 16: comparativa de les òrbites GPS, GLONASS i Galileo, amb altres òrbites de referència [41] ............................................................................................... 34 Figura 17: Composició de la senyal GNSS [15] ................................................. 35 Figura 18: bandes de freqüència de les senyals GNSS [15] .............................. 36 Figura 19: efecte de la diferència de fase [17]: la diferència de fase entre les senyals dels dos receptors permet saber quant més (o menys) temps ha de viatjar la senyal del rover i, determinar així, quant més lluny (o aprop) està del satèl·lit. .......................................................................................................................... 40 Figura 20: esquema del funcionament de la tecnologia RTK [17]: les xarxes RTK estan compostes dels satèl·lits GNSS, les estacions RTK de referència (envien les correccions al servidor central), el servidor RTK (processa les correccions i les envia al rover) i el rover ..................................................................................... 41 Figura 21: AshTech MB100 [18] ........................................................................ 42
Pol Capella Roca Figura 22: vista detallada de la placa Ashtech MB100 [18] ................................ 44 Figura 23: vista externa de l’antena ASH-661 [18] ............................................. 45 Figura 24: esquema i dimensions del kit d’evaluació [18] ................................... 46 Figura 25: disposició dels adaptadors per a les antenes [18] ............................. 48 Figura 26: superposició geoide-superfície terrestre [19] ..................................... 53 Figura 27: superposició el·lipsoide-superfície terrestre [19] ............................... 54 Figura 28: efecte de la composició terrestre sobre el geoide [19] ...................... 55 Figura 29: coordenades geocèntriques i geodèsiques [19] ................................ 56 Figura 30: representació de HAE i HMM [19] ..................................................... 57 Figura 31: esquema bàsic del funcionament d’un observador [20] ..................... 59 Figura 32: evolució de l’error d’estimació en les diferents etapes del filtre de Kalman [22] ....................................................................................................... 67 Figura 33: model físic del pèndul invertit ............................................................ 71 Figura 34: senyal d’entrada (µ(t)) de la simulació .............................................. 74 Figura 35: model de simulink usat per a fer les simulacions del pèndul invertit .. 75 Figura 36: subsistema “model” ........................................................................... 75 Figura 37: subsistema de l’observador de Luenberger ...................................... 76 Figura 38: posició lineal i posició angular del sistema carro-pèndul al llarg del temps ................................................................................................................. 77 Figura 39:comparativa de les senyals de sortida en l’observador de Luenberger .......................................................................................................................... 78 Figura 40: error de posició lineal i angular de l’observador de Luenberger ........ 79 Figura 41: comparativa de les senyals de sortida en el filtre de Kalman ............ 80 Figura 42: error de posició lineal i angular del filtre de Kalman .......................... 80 Figura 43: Comparativa de les senyals de sortida en el filtre de Kalman estès. . 81 Figura 44: error de posició lineal i angular del filtre de Kalman estès. ................ 82 Figura 45: esquema del procés de fusió de dades ............................................. 83 Figura 46: Sistemes de referència utilitzats [1] ................................................... 84 Figura 47: seqüència de rotació de EF a BF ...................................................... 87 Figura 48: magneto-resistència [24] ................................................................... 97
Pol Capella Roca Figura 49: declinació magnètica [31].................................................................. 98 Figura 50: esquema de funcionament de l’acceleròmetre [35] ......................... 101 Figura 51: configuració del block “serial receive” ............................................. 110 Figura 52: definició del sistema de referència WGS 84 [19] ............................. 112 Figura 53: a l’esquerra, projecció de Mercator, a la dreta, projecció transversa de Mercator [36] ................................................................................................... 114 Figura 54: xarxa UTM [37] ............................................................................... 114 Figura 55: característiques de l’hus [36] .......................................................... 118 Figura 56: situació del meridià central per a l’hus 30 [19] ................................. 120 Figura 57: model de simulink usat per a la fusió de dades ............................... 121 Figura 58: muntatge de la plataforma per a realitzar la validació experimental 122 Figura 59: validació experimental de l’estimació de l’actitud en repòs .............. 124 Figura 60: components normalitzades del camp magnètic durant la simulació, comparades amb les components del camp magnètic terrestre (consultar 8.3.3.2) ........................................................................................................................ 125 Figura 61: valor del mòdul del camp magnètic adimensionalitzat durant la simulació, comparat amb el mòdul del camp magnètic terrestre ...................... 125 Figura 62: validació experimental de l’estimació de la velocitat angular en repòs ........................................................................................................................ 126 Figura 63: validació experimental de l’estimació de l’acceleració lineal en repòs. ........................................................................................................................ 127 Figura 64: validació experimental de la velocitat lineal en repòs ...................... 128 Figura 65: validació experimental de l’estimació de la posició en repòs ........... 129 Figura 66: mòdul de camp magnètic mesurat durant la simulació de vol, comparat amb el mòdul de camp magnètic terrestre ....................................................... 130 Figura 67: validació experimental de l’estimació de l’actitud en vol .................. 131 Figura 68: validació experimental de la velocitat lineal en vol .......................... 131 Figura 69: validació experimental de l’estimació de l’acceleració lineal ............ 132 Figura 70: validació experimental de l’estimació de la velocitat angular ........... 132 Figura 71: validació experimental de l’estimació de la posició.......................... 133
Pol Capella Roca Índex de Taules Taula 1: Característiques de l’ATH .................................................................... 22 Taula 2:senyals dels sensors en el model onboard.m de matlab proporcionat per AscTec .............................................................................................................. 27 Taula 3: pinout dels ports RS-232 [18] ............................................................... 47 Taula 4: estimació del soroll del procés ............................................................. 93 Taula 5: covariàncies del soroll dels sensors de la IMU ..................................... 94 Taula 6: variància en les mesures de l’RTK ....................................................... 95 Taula 7: mesures experimentals del bias del giroscopi ...................................... 96 Taula 8: mesures experimentals del bias de l’acceleròmetre ........................... 102 Taula 9: paràmetres primaris del sistema WGS 84 .......................................... 113
15 Pol Capella Roca 2.1.2.Principis de vol En la configuració clàssica dels quadrotors els rotors estan instal·lats de tal manera que tots els vectors d’empenta són paral·lels i verticals apuntant cap amunt. No hi ha articulacions de canvi de pas, de manera que el pas de l’hèlix és fix. Figura 2: equilibri del quadrotor [2] Figura 3: empenta global del quadrotor [2]
16 Pol Capella Roca Davant la falta d’articulacions de canvi de pas el control d’aquests UAVs es basa en la variació de les rpms dels diferents rotors, per obtenir la combinació de forces i moments desitjada per a realitzar el moviment esperat. Així doncs podem distingir quatre moviments fonamentals del quadrotor: thrust (tracció), roll (balanceig), pitch (capcineig) i yaw (guinyada). A continuació, en les figures Figura 4, Figura 5, Figura 6, Figura 7 , es pot veure una breu descripció d’aquestes maniobres. Tracció (o col·lectiu): aquest control consisteix a fer variar, en la mateixa mesura, les rpm’s de cada rotor, per tal de variar la magnitud del vector d’empenta, sense canviar-ne la direcció. Figura 4: tracció del quadrotor [3] Roll (balanceig): per a executar el moviment de balanceig, s’augmenta la velocitat de rotació d’un dels rotors laterals (2 ó 4) i es redueix, en la mateixa magnitud, la de l’altre. D’aquesta manera es genera un moment positiu o negatiu al voltant de l’eix x, que fa que el quadrotor s’inclini lateralment.
17 Pol Capella Roca Figura 5: moviment de balanceig [3] Pitch (capcineig): l’execució del moviment de capcineig és molt semblant a la del balanceig, amb l’única diferència que en aquest cas, la variació de rpm’s es produeix sobre els rotors davanter i cuer (1 i 3). Així s’assoleix un moment positiu o negatiu al voltant de l’eix y. Figura 6: moviment de capcineig [3] Yaw (guinyada): aquest control consisteix en incrementar (o reduir) la velocitat de gir de la parella de rotors que giren en sentit antihorari – lateralsi en reduir (o incrementar) la velocitat dels que ho fan en sentit horari –davanter i cuer-. D’aquesta manera, complint la tercera llei de Newton (acció/reacció), es genera un moment al voltant de l’eix z de
18 Pol Capella Roca l’estructura, de sentit contrari al de la suma dels generats pels quatre rotors. Figura 7: moviment de guinyada [3] Seguint aquestes maniobres bàsiques, l’aeronau serà capaç d’assolir la posició xyz desitjada amb un determinat angle de guinyada. Malgrat tot cal tenir en compte que la tracció total (força Fthrust en la Figura 8) no sempre estarà alineada amb l’eix Terra vertical (consultar capítol 8.2) i es descompondrà en una component en l’eix vertical (Fz), i dues components en el pla horitzontal (Fx i Fy). D’aquesta manera es genera una acceleració horitzontal. Figura 8: efecte de la inclinació del quadrotor [2]
19 Pol Capella Roca De l’explicació anterior es dedueix doncs que, en control manual i executant un moviment de capcineig o balanceig, es produirà una reducció en l’acceleració ascensional o, fins i tot, un descens d’altitud de l’aeronau. 2.2.Fusió de dades La fusió de dades fa referència a l’ús sinèrgic de la informació provinent de diferents sensors, per a assolir una tasca requerida per el sistema. La fusió de dades és d’especial importància en qualsevol aplicació on una gran quantitat de dades han de ser combinades, fusionades i agrupades per a obtenir la millor qualitat possible en la presa de decisions. Aquesta tècnica apareix doncs en sistemes multi-sensors. Els principals avantatges d’aquests sistemes, en front dels mono-sensors són els següents: Les observacions realitzades per cada un dels sensors són incertes i, ocasionalment, incorrectes. Un sistema mono-sensor no ofereix la possibilitat de reduir la incertesa de les mesures. Diferents tipus de sensors poden proveir diferents tipus d’informació i permetre així, una millor caracterització de l’entorn d’operació. En casos de sistemes mono-sensors, la fallada d’un sensor resulta en la fallada del sistema. Així doncs, actualment la fusió de dades és una tasca necessària i prioritària en qualsevol sistema, de certa complexitat, que requereixi de les mesures de sensors per al seu funcionament. És per això que es poden trobar diferents tècniques i procediments per a executar aquesta funció, classificades segons diferents criteris. Un dels criteris de classificació més utilitzats és el basat en el tipus d’arquitectura o algoritme de fusió. Aquest mètode subdivideix les tècniques de fusió de dades en tres grans subgrups: les tècniques d’estimació, les d’inferència i les basades en la intel·ligència artificial. Mètodes d’estimació: aquesta tècnica consisteix bàsicament en prendre una mitja ponderada de la informació redundant que prové de diferents sensors i utilitzar-la com el valor de fusió. Dins d’aquesta categoria, en la majoria de casos s’emplea un filtre de Kalman [4] [5] [6], en qualsevol de les seves variants, degut al seu bon funcionament en sistemes contaminats per soroll blanc Gaussià, però també trobem algun cas on s’utilitzen
20 Pol Capella Roca observadors amb algoritmes d’estimació menys complexos. En algun estudi es [7] proposa la utilització d’un conjunt d’observadors de Luenberger per a l’observació de sistemes lineals. Mètodes d’inferència: la fusió de dades basada en la inferència Bayesiana 1 proporciona un formalisme per a sospesar la validesa de les dades seguint les regles de la teoria de probabilitats. La incertesa es representa amb valors de l’interval [0,1], on 0 indica falla total de la creença i 1 una creença absoluta. Mètodes d’intel·ligència artificial: inferències d’alt nivell requereixen de raonaments humans tals com el reconeixement de patrons, planificació, deducció i aprenentatge. Les xarxes neuronals i la lògica borrosa [6] són exemples d’aquest tipus de mètodes. Un dels trets que comparteixen la majoria de mètodes és la necessitat d’utilitzar un model que representi el comportament del sistema. En la majoria de casos s’usen models cinemàtics, degut a la bona resposta que ofereixen sense ser models enormement complexos. Per exemple [6] usa un model d’acceleració basat en un procés de Wiener. Els processos de Wiener són models que s’utilitzen per a representar la integral del soroll blanc Gaussià. També ens trobem amb estudis que utilitzen l’error dels sensors [8] per a definir un model d’error del sistema de navegació que corregeixi aquestes imprecisions. Aquest estudi emplea les dades del GPS per a corregir l’error de deriva de la IMU i poder fer unes estimacions més precises amb el filtre de Kalman. En sistemes menys complexos es poden usar models del sistema [9] que representin la dinàmica real del procés amb tanta precisió com sigui possible. Quan es treballa amb sistemes més complexos (com podria ser el nostre quadrotor) l’ús d’aquests models és més limitat degut a la impossibilitat de representar mitjançant equacions dinàmiques la física real del sistema, motivada per la complexitat del mateix i per la desconeixença de molts dels paràmetres físics que afecten al procés. 1 La inferència Bayesiana és un tipus d’inferència estadística que utilitza les observacions per a actualitzar la probabilitat de que una hipòtesis pugui ser certa.
21 Pol Capella Roca 3.AscTec Hummingbird 3.1.Introducció Aquest estudi ha estat realitzat i pensat per a la seva futura implementació en el quadrotor de l’empresa alemanya AscTec conegut amb el nom de Hummingbird. Es tracta d’un quadrotor desenvolupat amb objectius acadèmics i experimentals i, per tant, el seu disseny no busca oferir unes grans prestacions sinó una facilitat d’accés i modificació de les seves característiques. Al llarg d’aquest apartat es descriuran els principals components del quadrotor i la funció que desenvolupen cada un d’ells, així com el software que s’usa per establir comunicació amb la plataforma i que permet dissenyar i embarcar els algoritmes de control i fusió de dades a l’usuari. Figura 9: AscTec Hummingbird [39]
22 Pol Capella Roca 3.2.Característiques En la Taula 1 es poden consultar les principals característiques de l’AscTec Hummingbird. Onboard Computer ARM7 (LPC2146) Dimensions 540 x 540 x 85.5 mm MTOW 0.71 kg MPL 200 g Autonomia 20 min (sense Payload) Abast 4500 m ASL, 1000 m AGL Velocitat màxima 15 m/s Velocitat d'ascens 5 m/s Empenta màxima 20 N Comunicació inalàmbrica 2.4 GHz Xbee link, 10-63 mW Sistema de guiat inercial AscTec AutoPilot with 1000 HZ update rate Modes de vol GPS Mode, Height Mode, Manual Mode Modes d'emergència Direct Landing, Comehome straight, Comehome high Bateria 3 cel·les en sèrie 11.1 V, capacitat de 2100 mAh (Liti Polímer) Hèlix 8" de diàmetre (20,32 cm), flexible standard propellers PP-plastics Telemetria Xbee 2.4 GHz Ràdio control Futaba FAAST 2.4 GHz Motors AscTec X-BL 52s amb controladors X-BLDC X-CSM Estructura (braços) de fibra de carboni rígida - sandwich de fusta de balsa X-CSM Core Aleacions d'alumini i magnesi Taula 1: Característiques de l’ATH
23 Pol Capella Roca 3.3.Hardware En aquest apartat es descriuen tots els components físics que constitueixen l’AscTec Hummingbird en la seva configuració original. També es dóna informació sobre el funcionament i constitució del control remot que permet que l’aeronau pugui ser pilotada des de terra. 3.3.1.Processadors L’ATH disposa de dos processadors model ARM7 (LPC2146) [10], integrats en la placa de circuit imprès Auto Pilot, que realitzen totes les tasques de lectura de sensors, fusió de dades, comunicació amb l’exterior i control del quadrotor. En concret els dos processadors de l’UAV són un processador de baix nivell (LLP) i un altre d’alt nivell (HLP): El LLP es comunica amb la resta de hardware de l’ATH: rep i processa les dades dels sensors, realitza el procés de fusió de dades, envia instruccions als controladors dels quatre motors i emmagatzema els controladors dissenyats pel fabricant: Height Control mode i GPS mode [11]. Es pot comunicar amb un PC extern i enviar-li les seves dades a través del programa AscTec AutoPilot Control Software. El HLP constitueix la part programable del quadrotor: permet a l’usuari integrar els seus algoritmes de control i fusió de dades. A més rep les dades del GPS i les transmet al LLP. També pot rebre les dades de la resta dels sensors a través del LLP. La seva comunicació amb PCs externs és possible mitjançant AscTec Comunication Interface (ACI) o l’AscTec Simulink Toolkit.
24 Pol Capella Roca Els dos microprocessadors es comuniquen externament entre ells amb senyals d’alta freqüència (1000 Hz), amb la qual cosa el HL Processor té una bona disponibilitat de les dades dels sensors. Els algoritmes de control integrats en el HL Processor poden ser enviats de tornada al LL Processor amb la mateixa freqüència. En la Figura 11 es mostra l’esquema de transmissió de dades de la placa Autopilot i la seva freqüència d’actualització en funció de si la comunicació es fa via cable o a través dels mòduls de ràdio Xbee. Figura 10: placa Auto Pilot de l’AscTec Hummingbird [10]
31 Pol Capella Roca 4.Tecnologia RTK 4.1.Introducció Com s’ha explicat en la introducció del projecte, una de les particularitats d’aquest estudi i que fa que tingui un grau de complexitat i interès més elevat, és el fet que equiparem el nostre quadrotor amb un receptor capaç de treballar amb la tecnologia GPS diferencial (DGPS) coneguda com a Real Time Kinematic (RTK), posicionament cinemàtic en temps real. Aquest sistema permet millorar la precisió de les senyals GPS i passar d’errors d’uns quants metres a pocs centímetres. Però abans d’introduir-nos a fons en el món RTK i estudiar-ne el seu funcionament, és necessari tenir clar com treballen els Sistemes de Navegació per Satèl·lit (GNSS) i quins són els motius que generen les seves imprecisions. En aquest capítol es farà una introducció al funcionament dels GNSS en general i a la tecnologia RTK en particular, per tal de tenir una millor comprensió del comportament del nostre receptor RTK i poder interpretar les seves dades de forma més adequada. 4.2.Sistemes de Navegació per Satèl·lit Un Sistema de Navegació per Satèl·lit (GNSS) és un sistema compost per un conjunt de satèl·lits que proporcionen un posicionament geo-espacial de forma autònoma i continua, amb una cobertura global. Aquests sistemes permeten a petits aparells elèctrics (receptors) determinar la seva posició (longitud, latitud i altitud) amb una certa precisió, usant senyals de temps transmeses a través d’ones de ràdio des dels satèl·lits. El seu funcionament s’explica a través del principi de la triangulació: el receptor rep l’efemèride (veure apartat 4.2.2.3) que li transmet un satèl·lit de la xarxa, amb les dades de posició i temps d’aquest satèl·lit. Les efemèrides són transmeses a la velocitat de la llum, amb la qual cosa és senzill calcular la distància entre el satèl·lit i el receptor, si es coneix el temps de viatge de la informació. Per a calcular el temps de viatge, els receptors comparen una senyal pseudo aleatòria que és enviada pel satèl·lit, amb una còpia interna generada pel mateix aparell. Com que la senyal del satèl·lit tarda una estona en arribar al receptor, les
32 Pol Capella Roca dues senyals no s’alineen de forma perfecta inicialment. Per a aconseguir la coincidència final es va aplicant un retràs a la senyal del receptor. Aquest retràs és el temps necessari per a que la senyal del satèl·lit arribi al receptor. Un cop coneguda la distància, es construeix una esfera al voltant del satèl·lit, de radi la longitud entre el satèl·lit i el receptor, amb totes les possibles posicions on es podria trobar el receptor. Aquest procés es repeteix amb, com a mínim, tres satèl·lits més per a tal de descartar desdoblaments de posició i assegurar que la posició del receptor és la indicada pel sistema. És necessari destacar que els satèl·lits usen rellotges atòmics, molt més precisos que els rellotges convencionals que porten els receptors GNSS, i per això s’utilitza la senyal del quart satèl·lit per a corregir aquestes imprecisions tant com sigui possible, i aconseguir així una posició amb un marge d’error més acotat. Figura 15: esquema del funcionament del GPS
33 Pol Capella Roca 4.2.1.Xarxes GNSS Des de l’aparició del concepte de la navegació per satèl·lit són diversos els països i agències espacials que s’han llençat a la recerca i desenvolupament d’aquesta tecnologia. Actualment podem distingir sis xarxes principals de navegació global: GPS: el sistema de Navegació per Satèl·lit dels Estats Units, GPS (de l’anglès Global Positioning System), està constituït per fins a 32 satèl·lits orbitant a 20180 km de la superfície terrestre, on el nombre total de satèl·lits varia a mesura que els aparells més vells són substituïts. Operacional des del 1978 i globalment accessible des del 1994, la xarxa GPS és actualment la xarxa més utilitzada a nivell global. GLONASS: l’inicialment soviètic i ara rus, Global’naya Navigatsionnaya Sputnikovaya Sistema (GLObal Navigation Satellite System) o GLONASS, va sorgir com a xarxa totalment operacional l’any 1995. Amb 31 satèl·lits orbitant a 19130 km, va caure en l’oblit després de la desaparició de la Unió Soviètica, donant pas a l’aparició de buits en la cobertura i fent-lo accessible només parcialment. El sistema va ser recuperat i totalment restablert l’any 2011. DORIS: la Doppler Orbitography and Radio-positioning Integrated by Satellite (DORIS) és una xarxa de navegació de precisió francesa. Malgrat tot, al contrari d’altres sistemes GNSS, està basada en emissors estàtics situats a la superfície terrestre, amb els receptors situats en els satèl·lits, per tal de determinar la seva posició orbital amb precisió (també pot ser usada per receptors terrestres, amb unes prestacions i cobertura més limitades). Utilitzada comunament en combinació amb els sistemes GNSS tradicionals, augmenta la precisió de les mesures de posició fins a marges d’error de centímetres, per tal de construir un sistema geodèsic de referència molt més precís. Galileo: la Unió Europea i l’Agència Espacial Europea van acordar el Març de 2002 construir la seva pròpia xarxa GNSS com a alternativa al GPS, coneguda amb el nom de sistema de posicionament Galileo. Composta de 30 satèl·lits orbitant a 23222 km de la Terra, està pensada per a estar completament operativa l’any 2020. Un cop estigui en funcionament els receptors seran capaços de combinar les senyal GPS i Galileo, per tal d’obtenir precisions més altes.
34 Pol Capella Roca BeiDou: es tracta d’una xarxa de navegació regional xinesa, que ofereix cobertura a la zona del pacífic asiàtic. Actualment està constituïda per 16 satèl·lits orbitant a 21150 km, però està pensada per a ser ampliada a 35 satèl·lits i convertir-se així, en una xarxa de cobertura global. IRNSS: la Indian Regional Navigational Satellite System és un sistema de navegació per satèl·lit autònom i regional, desenvolupat per la Organització de Recerca Espacial India (ISRO), que estarà sota el control total del govern indi. La xarxa estarà composta de 7 satèl·lits orbitant en una òrbita geoestacionària (36000 km), per tal d’aconseguir una major cobertura amb el menor número possible de satèl·lits. El llançament de l’últim satèl·lit, que farà que el sistema sigui totalment operatiu, està programat per a mitjans de 2016. Figura 16: comparativa de les òrbites GPS, GLONASS i Galileo, amb altres òrbites de referència [41]
35 Pol Capella Roca 4.2.2.Senyal GNSS Els satèl·lits GNSS transmeten de forma contínua informació per a la navegació en dues o més freqüències de la banda L. Aquesta informació ve inclosa en senyals de ràdio, de les quals en podem identificar tres components principals [15]: Carrier Ranging Code Navigation Data Figura 17: Composició de la senyal GNSS [15] A continuació s’explica de manera detallada la funció de cadascun d’aquests components, per a tal de facilitar una millor comprensió de la informació rebuda i el funcionament del receptor GNSS usat en el projecte. 4.2.2.1.Carriers En la indústria de les telecomunicacions els carriers o senyals portadores, són senyals, generalment sinusoïdals, que es modulen amb altres senyals d’entrada, per tal de fer arribar la informació desitjada de forma adequada al receptor. Les bandes de freqüència destinades a la Radio Navigation Satellite Systems (RNSS), que és l’organisme que s’encarrega de gestionar les senyals GNSS, varien segons sigui l’aplicació i la xarxa que s’estigui usant. En la Figura 18 es pot veure un petit resum de les bandes de freqüència destinades al GPS, GLONASS i Galileo:
36 Pol Capella Roca Figura 18: bandes de freqüència de les senyals GNSS [15] Existeixen dues bandes destinades a la Aeronautical Radio Navigation Services (ARNS) que estan especialment pensades per a aplicacions de seguretat vital, ja que cap altre organisme o usuari pot accedir-hi. Aquestes freqüències es troben a la banda alta L (1559 – 1610 MHz) i a la part més lenta de la banda baixa L (1164 – 1214 MHz). 4.2.2.2.Ranging Code El ranging code és una seqüència de 0s i 1s que s’envia des del transmissor (satèl·lit) i que s’utilitza per a conèixer el temps que tarda la senyal en arribar al receptor. Per tant, dóna una primera estimació de la distància que existeix entre emissor i receptor. Permet també identificar quin satèl·lit envia la informació. Aquestes seqüències són conegudes també com a soroll pseudo-aleatori (PRN, de l’anglès Pseudo-Random Noise) o codis PRN, degut a la seva naturalesa semblant al soroll aleatori. La senyal GNSS original conté dos ranging codes: el Coarse/Adquisition (C/A) code, que és d’accés públic, i el Precision (P) code, que és d’ús restringit i normalment reservat per a aplicacions militars.
37 Pol Capella Roca Coarse/Adquisition Code: el codi C/A és un codi PRN de 1023 bits de longitud. Cada satèl·lit transmet un C/A completament diferent, permetent al receptor rebre i interpretar diferents codis a la vegada. Precision Code: el codi P és també un codi PRN, però en aquest cas de 6.1871 x 1012 bits. La seva longitud i complexitat elimina qualsevol ambigüitat de rang que pogués aparèixer dins el sistema solar però, en molts casos, n’impossibilita la seva interpretació directa. És per això que habitualment els receptors se sincronitzen primer amb el codi C/A i, posteriorment, amb el P. Com s’ha comentat aquest és un ranging code reservat per a ús militar i és per això que s’envia encriptat. 4.2.2.3.Navigation data A part dels codis PRN el receptor necessita obtenir informació detallada sobre la posició de cada satèl·lit i l’estat de la xarxa. Aquesta informació s’envia a través de la navigation data integrada per tres components principals. La primera part conté informació sobre la data i el temps, així com de l’estat i la salut del satèl·lit. La segona conté la informació orbital, coneguda com a efemèride, i permet al receptor calcular la posició del satèl·lit. La última part, que rep el nom d’almanac, proporciona informació de tots els satèl·lits de la xarxa; les seves localitzacions i números PRN. Efemèrides: envien informació als receptors GNSS de on s’hauria de trobar el satèl·lit en cada moment del dia. Cada satèl·lit envia la seva pròpia efemèride, mostrant informació únicament vàlida per a aquell aparell. Com que la informació de les efemèrides ha de ser molt precisa, es considera obsoleta després de quatre hores. Almanac: descriu els cursos orbitals dels satèl·lits. Cada satèl·lit envia informació d’almanac per a cada satèl·lit de la xarxa. El receptor GNSS utilitza aquesta informació per a determinar quins satèl·lits estaran visibles en el seu camp de visió i concentrar-se únicament en aquests. Com que aquesta informació no és necessari que sigui tant precisa com la de les efemèrides, es considera vàlida fins a durant 180 dies.
38 Pol Capella Roca 4.2.3.Fonts d’error De l’explicació del funcionament de les xarxes GNSS se’n poden extreure els diferents motius que generen les imprecisions d’aquests aparells i que en limiten el seu ús. Les principals fonts d’error són les següents [16]: 1. Retards deguts a la ionosfera i a la troposfera: la senyal dels satèl·lits perd velocitat a mesura que creua l’atmosfera. Els sistemes GNSS utilitzen models interns que fan un càlcul del retràs mitjà acumulat, per tal de reduir parcialment aquest error. 2. Senyal multiruta: aquest fenomen té lloc quan la senyal GNSS és reflectida per objectes com edificis alts o grans superfícies rocoses, abans d’arribar al receptor GNSS. Això augmenta el temps de viatge de la senyal, generant imprecisions. 3. Errors del rellotge del receptor: la precisió dels rellotges dels receptors GNSS no és tant acurada com la dels rellotges atòmics dels satèl·lits, amb la qual cosa es produeixen lleus errors de sincronització. 4. Errors d’orbitals: també coneguts com a errors d’efemèrides, són inexactituds en la posició dels satèl·lits reportada. 5. Nombre de satèl·lits visibles: com més satèl·lits sigui capaç de localitzar el receptor GNSS, major serà la precisió de les mesures de posició. 6. Bloqueig de la senyal: els edificis, el relleu del terreny, interferències electròniques o, fins i tot, la presència de fullatge dens poden bloquejar la recepció de la senyal, causant errors de posicionament o la no recepció de dades de posició en els casos més extrems. 7. Posició geomètrica relativa dels satèl·lits: el posicionament ideal dels satèl·lits es produeix quan es troben posicionats de tal manera que es generin grans angles relatius entre ells. Una geometria pobre succeeix quan els satèl·lits estan agrupat, o formant una línia entre si. 8. Degradament intencionat de la senyal: la disponibilitat selectiva (SA) és un degradament intencionat de la senyal imposat per l’organisme regulador de la xarxa GNSS. L’SA està pensat per a evitar l’ús de precises senyals GNSS per part d’adversaris militars. Totes aquestes fonts de distorsió són les que provoquen que la precisió mitja de les diferents xarxes GNSS sigui de l’ordre de pocs metres.
39 Pol Capella Roca 4.2.4.Sistemes d’augmentació GNSS Per tal de minimitzar tant com sigui possible els errors de la senyal GNSS, existeixen una sèrie de sistemes d’augmentació que basen el seu funcionament en la integració d’informació externa en el procés de càlcul natural dels equips GNSS. Alguns dels sistemes aporten informació addicional sobre les fonts d’error (com la deriva del rellotge, les efemèrides o el retràs en la ionosfera), d’altres sobre quant de temps va estar inaccessible la senyal en el passat i una altra categoria proporciona dades extres sobre el cos que suporta el receptor. Segons la localització del sistemes d’augmentació, en podem distingir dos grans subgrups: SBAS: de l’anglès Satellite-based augmentation systems, són sistemes que suporten augmentacions de senyals en àrees extenses o petites regions a través de l’ús de missatges addicionals proporcionats pels satèl·lits. Aquests sistemes estan compostos comunament de múltiples estacions de terra, que prenen mesures de un o més satèl·lits de la xarxa GNSS, de les senyals, o d’altres factors ambientals que poden afectar la senyal rebuda pels usuaris. Utilitzant aquestes mesures es generen missatges que s’envien a un o més satèl·lits per a ser transmesos als receptors finals. GBAS: de l’anglès Ground-Based Augmentation Systems, el seu funcionament és força similar al dels sistemes SBAS, però amb la diferència que en aquest cas els missatges són transmesos a través d’estacions terrestres. Això fa que generalment les xarxes GBAS siguin més localitzades, suportant receptors en un radi d’uns 50 km. 4.2.5.GNSS diferencial amb tecnologia RTK Un dels sistemes GBAS més utilitzats és el GNSS diferencial (DGNSS). Aquesta tècnica basa el seu funcionament en dos principis: En una regió d’espai limitada, amb condicions meteorològiques favorables (cel net), els errors en el processament de la senyal GNSS són constants i iguals per a tots els receptors. El soroll de les mesures de la senyal portadora (carrier) és molt més petit que el de les mesures dels senyals pseudo aleatoris.
40 Pol Capella Roca Una xarxa DGNSS està composta de diverses estacions de referència, cadascuna amb una posició totalment coneguda, un servidor que processa les dades, els satèl·lits GNSS convencionals i els receptors o rovers. Les estacions de referència processen les dades rebudes dels satèl·lits i les envien als receptors, juntament amb la seva posició coneguda prèviament, per a que els rovers puguin aplicar les correccions convenients sobre la sincronització dels rellotges i el retràs de la senyal. A més, les estacions de referència mesuren també la fase de la senyal portadora (carrier) que reben en cada instant de temps i la comparen amb la fase de la senyal rebuda pels receptors. La diferència de fase permet als rovers calcular la seva posició relativa a les estacions de referència, obtenint així precisions de fins a pocs centímetres. La Figura 19 mostra de forma esquemàtica com la diferència de fase entre l’estació de referència i el rover, permet obtenir la posició del receptor amb bastanta precisió. Figura 19: efecte de la diferència de fase [17]: la diferència de fase entre les senyals dels dos receptors permet saber quant més (o menys) temps ha de viatjar la senyal del rover i, determinar així, quant més lluny (o aprop) està del satèl·lit. Segons el moment en que les dades són processades podem distingir dos tipus de sistemes DGNSS: Real Time Kinematic (RTK): el processament de les dades és produeix sobre el terreny en temps real. Post Processing Kinematic (PPK): les dades són enviades a servidors externs que les processen, per aplicar les correccions més tard. Així dons la tecnologia RTK permet obtenir correccions en temps real, assolint així precisions molt més elevades.
47 Pol Capella Roca adaptadors de cable per a antenes, una entrada d’alimentació de 5V DC i una carcassa de protecció. Els dos ports RS-232 són del tipus 9-pin subD male i tenen la mateixa configuració de pins, que s’indica en la Taula 3 : Taula 3: pinout dels ports RS-232 [18] El primer port transmet a una velocitat de 921.6 kbits/s mentre que el segon ho fa a 460.8 kbits/s. El port USB és un port USB 2.0 estàndard amb un rati de transmissió de dades de 12 Mbits/s. Pel que fa a les entrades d’antena com s’ha comentat, el kit aporta dues femelles MMCX per a cables coaxials.
48 Pol Capella Roca Figura 25: disposició dels adaptadors per a les antenes [18] L’adaptador ANT1 (Figura 25) està preparat per a rebre senyals L1/L2 GPS i senyals L1 GLONASS. Per la seva banda el connector ANT2 només admet senyals GPS i GLONASS en el mode de freqüència L1. 5.3.Protocol de comunicació En qualsevol sistema de comunicació és necessària l’existència d’un protocol, un conjunt de normes, que possibiliti l’intercanvi d’informació entre dos o més entitats de forma senzilla i accessible. Actualment existeixen diversos protocols, cadascun amb les seves particularitats, però la plataforma AshTech MB100 utilitza un dels més reconeguts a nivell internacional: l’NMEA0183. 5.3.1.NMEA0183 L’Associació Nacional d’Electrònica Marina (NMEA) és una associació sense ànim de lucre composta de fabricants, distribuïdors, empresaris, institucions educatives
49 Pol Capella Roca i altres organismes interessats en l’electrònica marina. L’NMEA0183 és el nom de l’estàndard de comunicació desenvolupat per aquesta associació, que defineix l’interfaç elèctrica i el protocol de dades per comunicacions entre instrumentació marina. Els dispositius NMEA0183 estan dissenyat per ser comunicadors (talkers), receptors (listeners) o els dos a la vegada. La interfaç elèctrica del protocol és capaç de suportar un únic comunicador i diversos receptors a la vegada. 5.3.1.1.Format general de les sentències Totes les dades són transmeses en forma de sentències. Només s’admeten caràcters ASCII, més CR (carriage return) i LF (line feed). Cada sentència s’inicia amb el símbol $ i acaba amb <CR><LF>. Es poden distingir tres tipus bàsics de sentències: talker sentences, propiertary sentences i query sentences. Talker sentences: el format general per a les sentències dels comunicadors és: $ttsss,d1,d2,. . . .<CR><LF> Les dues primeres lletres que segueixen l’$ són l’identificador del comunicador. Els següents tres caràcters (sss) són l’identificador de la sentència, seguits per un número de camps de dades separats per comes, seguits per un checksum opcional i acabats amb el carriage return/line feed. Cada sentència pot contenir fins a 80 caràcters. Si les dades d’un camp no estan disponibles, s’ometen, però les comes que el delimiten són enviades igualment, sense espai entre elles. Proprietary sentences: el protocol permet als fabricants definir els seus propis formats de sentències. Aquests missatges comencen amb $P, seguits de tres lletres amb l’identificador del fabricant i de qualsevol informació que el manufacturador desitgi, seguint el format general de les sentències estàndard. Query sentences: aquestes sentències són la manera que té el receptor de reclamar alguna informació en particular al comunicador. El format general és: $ttllQ,sss, [CR] [LF]
50 Pol Capella Roca Els dos primers caràcters del missatge són l’identificador del receptor que demana la informació, i els dos següents, els del receptor que l’ha d’enviar. Els cinquè caràcter és sempre una Q, mostrant que la sentència és del tipus query. Les tres lletres que segueixen (sss) mostren el tipus de sentència que s’està reclamant. 5.3.1.2.Sentències d’interès A continuació es fa un resum de l’arquitectura de les sentències que ens seran de més utilitat a l’hora de determinar la posició del nostre UAV. ALM - Dades d’almanac GSV - Satèl·lits visibles
51 Pol Capella Roca GGA – Dades GNSS. Temps, posició i altres dades relacionades
52 Pol Capella Roca 6.Geometria terrestre La lectura i representació de les dades de posició comporten una complexitat que moltes vegades és desconeguda pels usuaris. És per això que es dedica aquest capítol a la interpretació del datum i la geometria terrestre, que ens permetrà construir un algoritme de lectura de dades posicionals, amb unes bases molt més fermes (veure capítol 8.4.2). 6.1.El geoide Es defineix el geoide com a la superfície teòrica de la Terra que uneix tots els punts d’igual gravetat. Aquesta forma es construeix obviant totes les pertorbacions exteriors – atracció de la lluna (marees) i les interaccions amb tot el sistema solar – i considerant els oceans en calma. Lluny del que es podria pensar, aquesta superfície no és uniforme, sinó que presenta una serie d’irregularitats, causades per la diferent composició mineral de l’interior de la Terra i per les seves diferents densitats; el que implica que per a cada punt del geoide existeix una distància diferent al centre de la Terra. La Figura 26 mostra una representació teòrica de la superposició del geoide i la superfície terrestre;
53 Pol Capella Roca Figura 26: superposició geoide-superfície terrestre [19] 6.2.L’el·lipsoide Com és conegut per tothom la Terra no és rodona i la seva imatge s’assembla a la d’una esfera aixafada pels pols, amb la qual cosa no existeix una figura geomètrica que representi a la perfecció la seva forma, degut fonamentalment a les iregularitats existents. Malgrat tot s’ha trobat un model matemàtic que constitueix una solució de compromís i permet representar amb certes garanties la geometría terreste. Aquest model és l’el·lipsoide – el resultat de revolucionar una el·lipse sobre el seu semieix major – i les seves característiques geomètriques poden variar, segons sigui la zona a cartografiar, ja que les irregularitats de la Terra no són simètriques i canvien per als diferents punts de l’escorça. En la Figura 27 es pot observar una superposició teòrica d’un el·lipsoide de referència i la superfície terrestre:
54 Pol Capella Roca Figura 27: superposició el·lipsoide-superfície terrestre [19] Les principals característiques que defineixen la geometria de l’el·lipsoide són: Semieix major (a) Semieix menor (b) Aixafament (1/f = 1 – (b/a)) 6.3.Comparativa el·lipsoide – geoide La desigual distribució de la gravetat superficial i la presència de pertorbacions locals, provoca que existeixin zones en les que l’el·lipsoide queda per sobre del geoide i al revés. Aquestes diferències gravitatòries són generades per la canviant composició terrestre i la presència d’una gran massa d’aigua als oceans, que causa una menor atracció gravitatòria fent que, per norma general, l’el·lipsoide quedi per sobre en les zones oceàniques i el geoide en les zones continentals (veure Figura 28).
55 Pol Capella Roca Figura 28: efecte de la composició terrestre sobre el geoide [19] 6.4.El Datum Es dedineix el Datum com el punt on l’el·lipsoide i el geoide són tangents. Els Datums geodèsics s’utilitzen per a establir un orígen i la situació d’un sistema de coordenades vàlid per a una determinada zona de la Terra, no extrapolable a tota la superfície terrestre. Cada Datum consta de: Un el·lipsoide de referència definit per els paràmetres a, b i l’aixafament. Un punt conegut com a fonamental, en el qual l’el·lipsoide i la superfície real de la Terra són tangents. A l’hora de definir doncs les coordenades d’un determinat punt és necessari donar el Datum i l’el·lipsoide de referència utilitzats, ja que poden existir diferències entre les mesures segons sigui la combinació utilitzada. A més el coneixement d’aquestes dades ens permetrà canviar el sistema de representació utilitzat (3D o 2D).
56 Pol Capella Roca Com exemple la xarxa GPS utilitza el Datum i l’el·lipsoide de referència WGS 84. Que modelitza la Terra de tal manera que pot ser utilitzat de forma global, sense grans desviacions. 6.5.Coordenades geogràfiques L’orígen de mesura de les coordenades geogràfiques pot coincidir o no amb el centre de masses de la Terra, generant-se així dos tipus de coordenades geogràfiques diferents: Coordenades geodèsiques: aquelles que estan referides a l’el·lipsoide. Coordenades geocèntriques: definides respecte al centre de gravetat de la Terra; (x,y,z) o (λ,ω,h). La principal diferència entre les dues es troba en la medició de la latitud. Les coordenades geodèsiques mesuren la latitud traçant la normal a l’el·lipsoide de referència, mentre que les geocèntriques ho fan unint el punt objecte amb el centre de la Terra (Figura 29). Figura 29: coordenades geocèntriques i geodèsiques [19] Com a norma general les coordenades geocèntriques s’expressen en format xyz, mentre que les geodèsiques ho fan per mitjà de la latitud, longitud i alçada (λ,ω,h).
63 Pol Capella Roca 7.4.Filtre de Kalman L’algoritme del filtre de Kalman va ser desenvolupat als voltants de l’any 1960 per Rudolf E. Kalman. Existeix una versió d’aquest filtre per a sistemes en temps continu i diverses variants per a sistemes en temps discret. En aquesta secció és desenvoluparà la versió de predicció-correcció del filtre de Kalman, ja que és la que s’utilitza més en la indústria del control automàtic. El filtre de Kalman és un estimador d’estats que proporciona una estimació òptima, en el sentit que el valor mig de la suma dels errors d’estimació pren un valor mínim. En altres paraules, el filtre de Kalman dóna a la següent suma d’errors quadrats 𝐸[𝑒𝑢𝑇(𝑘)·𝑒𝑢(𝑘)]=𝐸[𝑒𝑢12(𝑘)+⋯+𝑒𝑢𝑛 2] ( 17 ) un valor mínim. On: 𝑒𝑢=𝑥(𝑘)−𝑥(𝑘) ( 18 ) és l’error d’estimació. Es tracta d’un estimador pensat per a sistemes lineals afectats per pertorbacions aleatòries (“soroll blanc”) i els mesuraments dels quals estan contaminats pel mateix tipus de soroll. 7.4.1.Sistemes lineals estocàstics Les equacions que defineixen l’algoritme de predicció-correcció del filtre de Kalman que es presenten més avall, parteixen de l’assumpció que s’està treballant amb un procés lineal estocàstic del tipus: 𝑥(𝑘)=𝐴𝑥(𝑘−1)+𝐵𝜇(𝑘)+𝐺𝑤(𝑘) ( 19 ) 𝑦(𝑘)=𝐶𝑥(𝑘)+𝐻𝑤(𝑘)+𝑣(𝑘) ( 20 ) En el model presentat (19) - (20) apareixen noves variables i funcions que s’expliquen a continuació: w representa un vector de pertorbació aleatòria (blanca) que es coneix com a soroll del procés:
64 Pol Capella Roca 𝑤=[𝑤1 𝑤2 ⋮ 𝑤𝑞] ( 21 ) Es treballa sempre amb la suposició que la pertorbació wk té un valor mitjà wk =0 i una autocovariància Q: 𝑤𝑘1𝑤𝑘2𝑇=𝑄𝛿(𝑘1−𝑘2) ( 22 ) on δ(𝑘) representa la funció delta de Kronecker i Q se sol presentar com una matriu de dimensions n x n de la següent forma: 𝑄=[𝑄11 00 0 0 𝑄22 0 0 0000⋱ 0 0 𝑄𝑛𝑛 ] ( 23 ) en la majoria de casos s’assumeix que el número q de pertorbacions del procés és igual al número n d’estats. Qii és la variància de wi. G és la matriu de guany del soroll del procés. Normalment es considera q=n, fent la matriu G quadrada: 𝐺=[𝐺11 00 0 0 𝐺22 0 0 0000⋱ 0 0 𝐺𝑛𝑛 ] ( 24 ) A més és habitual atorgar valor 1 als valors de la matriu G, amb la qual cosa es transforma en una matriu identitat. H és una matriu de guany que relaciona directament les pertorbacions del procés amb les mesures (existirà una relació indirecta, ja que les pertorbacions afecten als estats i alguns d’aquests estats són mesurats). Malgrat tot és habitual prendre-la com una matriu de zeros de dimensió r x q, on r és el número d’estats mesurats.
65 Pol Capella Roca 𝐻=[0 0 0 0 0 0 ⋱ ⋮ 0 0 ⋯ 0 ] ( 25 ) v és el vector de pertorbacions aleatòries (soroll blanc) de les mesures: 𝑣=[𝑣1 𝑣2 ⋮𝑣𝑟] ( 26 ) Es treballa sempre amb la suposició que la pertorbació vk té un valor mitjà vk = 0 i una autocovariància R: 𝑣𝑘1𝑣𝑘2𝑇=𝑅𝛿(𝑘1−𝑘2) ( 27 ) On R s’acostuma a presentar com una matriu de la següent forma: 𝑅=[𝑅11 00 0 0 𝑅22 0 0 0000⋱ 0 0 𝑅𝑟𝑟 ] ( 28 ) Un cop coneguda la descripció del model que defineix el procés que estem estudiant, és necessari tenir en compte algunes consideracions que ens permetran definir amb més facilitat l’algoritme que condueix el filtre de Kalman. Així doncs es partirà de la suposició que l’estat inicial x0 és desconegut, però que en podem tenir un coneixement “a priori” que ens permet definir que serà una variable gaussiana de mitjana 𝑥0 i covariància Px0, és a dir, x0~(x0 ,Px0). A més s’assumirà que x0, vk i wk, no estan relacionats entre si, de tal manera que wjvkT= 0 i així per a tots els valors de j i k i per als diferents casos. Sota aquestes circumstàncies l’estat xk serà una variable de mitjana 𝑥𝑘 i covariància Pxk, i de la mateixa manera la sortida yk~(yk ,Pzk). El valor de 𝑥𝑘+1 vindrà definit per:
66 Pol Capella Roca 𝑥𝑘+1=𝐴𝑥𝑘+𝐵𝜇𝑘 ( 29 ) Conegudes aquestes premisses doncs, és fàcil veure com es propaga la covariància de l’estat xk+1 que ens permetrà dimensionar l’error d’estimació en cada instant de temps, i que jugarà un paper fonamental a l’hora d’actualitzar i corregir les estimacions fetes. Així doncs: 𝑃𝑥𝑘+1=(𝑥𝑘+1−𝑥𝑘+1)·(𝑥𝑘+1−𝑥𝑘+1)𝑇 =[𝐴(𝑥𝑘−𝑥𝑘)+𝐺𝑤𝑘]·[𝐴(𝑥𝑘−𝑥𝑘)+𝐺𝑤𝑘]𝑇 =𝐴(𝑥𝑘−𝑥𝑘)·(𝑥𝑘−𝑥𝑘)𝑇 𝐴𝑇+𝐺𝑤𝑘(𝑥𝑘−𝑥𝑘)𝑇 𝐴𝑇 +𝐴(𝑥𝑘−𝑥𝑘)𝑤𝑘𝑇 𝐺𝑇+ 𝐺𝑤𝑘𝑤𝑘𝑇 𝐺𝑇 o escrit de forma més compacta: 𝑃𝑥𝑘+1=𝐴𝑃𝑥𝑘𝐴𝑇+𝐺𝑃𝑤𝑘𝑥𝑘𝐴𝑇+𝐴𝑃𝑤𝑘𝑥𝑘𝐺𝑇+𝐺𝑄𝐺𝑇 i com que hem dit que les diferents variables i pertorbacions no estaven relacionades entre si: 𝑃𝑥𝑘+1=𝐴𝑃𝑥𝑘𝐴𝑇+𝐺𝑄𝐺𝑇 ( 30 ) De manera anàloga podem definir la variància de la predicció, i la variància de la mesura: 𝑃𝑥𝑘𝑧𝑘=𝑃𝑥𝑘𝐶𝑇 ( 31 ) 𝑃𝑧𝑘=𝐶𝑃𝑥𝑘𝐶𝑇+𝑅 ( 32 )
67 Pol Capella Roca 7.4.2.Algoritme d’estimació Un cop entesa la dinàmica bàsica d’un sistema lineal estocàstic, estem en condicions d’introduir l’algoritme del filtre de Kalman. Com s’ha comentat a l’inici d’aquest apartat es tracta d’un algoritme dividit en dues fases clarament diferenciades: 1. En la primera, coneguda com l’etapa de predicció o d’estimació a priori (k1), es fa una primera estimació de l’estat actual basada en els valors de l’estimació de l’estat en l’instant de temps anterior. 2. En la segona, coneguda com etapa de correcció, actualització o d’estimació a posteriori (k), es tenen en compte les mesures del sistema real (yk) i s’aplica una correcció a l’estimació de la fase anterior, reduint l’error d’estimació. En la Figura 32 es pot observar de forma esquemàtica com disminueix l’error d’estimació degut a la introducció de l’etapa de correcció. Figura 32: evolució de l’error d’estimació en les diferents etapes del filtre de Kalman [22]
68 Pol Capella Roca Estimació a priori: 𝑥𝑘|𝑘−1=𝐴𝑥𝑘−1|𝑘−1+𝐵𝜇𝑘−1 (33) 𝑃𝑘|𝑘−1=𝐴𝑃𝑘−1|𝑘−1𝐴𝑇+𝑄𝑘−1 (34) Estimació a posteriori: 𝑆𝑘=𝐶𝑃𝑘|𝑘−1𝐶𝑇+𝑅𝑘 (35) 𝐿𝑘=𝑃𝑘|𝑘−1𝐶𝑇𝑆𝐾−1 (36) 𝑥𝑘|𝑘=𝐴𝑥𝑘|𝑘−1+𝐿𝑘(𝑦𝑘−𝐶𝑥𝑘|𝑘−1) (37) 𝑃𝑘|𝑘=(𝐼−𝐿𝑘𝐶)𝑃𝑘|𝑘−1 (38) En l’algoritme del filtre de Kalman apareixen alguns conceptes que no havien intervingut fins ara: xk|k−1 és l’estimació de l’estat a priori, basada en l’estimació de l’instant k1. Pk|k−1 és la covariància de l’estimació a priori (30). Sk es coneix com la covariància d’innovació i coincideix amb la covariància de la sortida (32). Lk és el guany òptim de Kalman i, a diferència de l’observador de Luenberger, s’actualitza a cada instant de temps. Si ens fixem bé en l’equació i consultem l’apartat 7.4.1, ens adonarem que aquest guany és simplement un quocient entre la incertesa en la predicció i la incertesa en la mesura o, dit d’una altra manera, el guany s’ajusta en un sentit o un altre per afavorir les mesures o la predicció, segons quina ens doni un error més petit. xk|k és l’estimació actualitzada de l’estat a posteriori. Pk|k és la covariància de l’estimació actualitzada a posteriori. En el següent instant de temps l’instant k, passa a ser el k-1, i se segueix la mateixa mecànica. És important destacar també que en la primera estimació (k=1), caldrà disposar d’una primera mesura o suposició de l’estat x0 i s’haurà de suposar la covariància de l’instant anterior (P0|0).
69 Pol Capella Roca Així doncs es poden distingir dos principals avantatges respecte els observadors de Luenberger: El filtre de Kalman calcula i actualitza de forma automàtica els valors de la matriu de realimentació L, per tal de reduir al mínim l’error d’estimació- Permet actuar de forma adequada amb sistemes contaminats per soroll blanc. D’altra banda però, el dissenyador no pot escollir si prefereix sacrificar una mica de precisió per a guanyar velocitat en l’estimació i, òbviament, l’algoritme d’estimació és més complex que per als observadors comuns. 7.5.Filtre estès de Kalman El filtre de Kalman proporciona una estimació òptima per a sistema lineals, però això no és suficient ja que en la majoria de casos, els processos que es volen estudiar són no lineals. Per això als voltants del 1960 el centre de recerca de la NASA, AMES, va desenvolupar un algoritme basat en el filtre de Kalman però que anava un pas més enllà, ja que permetia obtenir estimacions òptimes de sistemes no lineals, Aquest algoritme es coneix com el filtre estès de Kalman (EKF, de l’anglès Extended Kalman Filter). La dinàmica d’aquest estimador està basada en els mateixos principis que el filtre de Kalman; dues etapes, una de predicció i una altra de correcció, que permeten obtenir la millor estimació possible d’un procés estocàstic, en aquest cas, no lineal. El model que descriu el procés en aquest cas és: 𝑥𝑘=𝑓(𝑥𝑘−1,𝜇𝑘−1)+ 𝑤𝑘−1 (39) 𝑦𝑘=ℎ(𝑥𝑘)+ 𝑣𝑘−1 (40) La funció f representa la funció de transició d’estats i es tracta d’una funció no lineal, que descriu la dinàmica del procés. La funció g és la funció de sortida i també és una funció no lineal. L’algoritme que defineix l’EKF és:
70 Pol Capella Roca Estimació a priori: 𝑥𝑘|𝑘−1=𝑓(𝑥𝑘−1|𝑘−1,𝜇𝑘−1) (41) 𝑃𝑘|𝑘−1=𝐴𝑃𝑘−1|𝑘−1𝐴𝑇+𝑄𝑘−1 (42) Estimació a posteriori: 𝑆𝑘=𝐶𝑃𝑘|𝑘−1𝐶𝑇+𝑅𝑘 (43) 𝐿𝑘=𝑃𝑘|𝑘−1𝐶𝑇𝑆−1 (44) 𝑥𝑘|𝑘=𝐴𝑥𝑘|𝑘−1+𝐿𝑘[𝑦𝑘−𝑔(𝑥𝑘|𝑘−1)] (45) 𝑃𝑘|𝑘=(𝐼−𝐿𝑘𝐶)𝑃𝑘|𝑘−1 (46) Com es pot observar l’algoritme de càlcul és molt semblant al del filtre de Kalman, amb l’única diferència que a l’hora de calcular l’estimació a priori i la sortida, s’utilitzen les funcions no lineals que descriuen el procés original i, a més, actualitzades a cada instant de temps. 7.6.Exemple didàctic: pèndul invertit Per tal de reforçar els conceptes introduïts en l’explicació anterior, s’ha realitzat un exemple teòric, més senzill que el cas del quadrotor que es tracta en aquest projecte, que pot ajudar a resoldre molts dels dubtes que sorgeixin i que, a més, facilitarà el treball de programació posterior amb el model real. Es tracta d’un cas clàssic en la teoria de control actual i que va molt lligat a l’aprenentatge, pel seu balanç entre complexitat i realisme: el pèndul invertit. A continuació es presenta el model i l’observació que s’ha fet amb els tres tipus d’estimadors, per tal d’entendre millor les seves diferències i poder fer una millor elecció per el nostre cas.
71 Pol Capella Roca 7.6.1.Model El model (Figura 33) es tracta d’un carro equipat amb un pèndul, ambdós de densitat coneguda i constant, que quan se li aplica una força (f) es desplaça a través d’una via recta, fent que el pèndul surti de la seva posició d’equilibri. Suposarem que el carro disposa d’un encoder que ens proporciona la posició del pèndul (θ) amb una freqüència de 100 Hz i un sensor de posició que ens proporciona la seva posició longitudinal (x) amb la mateixa freqüència que l’encoder. Els estats que ens interessarà mesurar seran la posició lineal (x), la velocitat lineal (𝑥) la posició angular (𝜃) i la velocitat angular (𝜃). Les constants que defineixen el model són [23]: Massa del carro: M=0.5 kg. Massa del pèndul: m=0.2 kg. Coeficient de fricció dinàmic amb el terra: b=0.1 N·s/m Moment d’inèrcia del pèndul: i=0.006 kg·m2. Acceleració de la gravetat: g=9.81 m/s2. Longitud del centre de masses del pèndul a un dels extrems: l=0.3 m. f θ x Figura 33: model físic del pèndul invertit
72 Pol Capella Roca Les equacions que defineixen la dinàmica del model són: 𝑥=(𝑚𝑙2−𝑖)·𝑓+(𝑖−𝑚𝑙2)·𝑏𝑥+𝑚2𝑙2𝑔·𝑠𝑖𝑛𝜃·𝑐𝑜𝑠𝜃+(𝑚2𝑙3−𝑚𝑙𝑖)·𝜃2·𝑠𝑖𝑛𝜃 [(𝑚+𝑀)·(𝑚𝑙2−𝑖)−𝑚2𝑙2·𝑐𝑜𝑠2𝜃] (47) 𝜃=𝑚𝑙𝑓·𝑐𝑜𝑠𝜃−𝑚𝑙𝑏𝑥·𝑐𝑜𝑠𝜃+(𝑚+𝑀)·𝑚𝑙𝑔·𝑠𝑖𝑛𝜃+𝑚2𝑙2𝜃2·𝑠𝑖𝑛𝜃·𝑐𝑜𝑠𝜃 [(𝑚+𝑀)·(𝑖−𝑚𝑙2)+𝑚2𝑙2𝑐𝑜𝑠2𝜃] (48) A partir d’ara el denominador de 𝑥 l’anomenarem s i el de 𝜃, q. Com s’ha comentat a l’inici, s’està treballant amb un model on els sensors donen mesures cada cert instant de temps, amb la qual cosa, abans de poder escriure el model en espai d’estats, s’haurà de discretitzar. Existeixen diferents mètodes de discretització de models continus, però en aquest exemple s’ha utilitzat el mètode d’Euler: 𝑑𝑥 𝑑𝑡=𝑥𝑘−𝑥𝑘−1 𝑇 (49) I el model discretitzat queda de la següent manera: 𝑥𝑘=(𝑚𝑙2−𝑖)·𝑓𝑘−1+(𝑖−𝑚𝑙2)·𝑏𝑣𝑥|𝑘−1+𝑚2𝑙2𝑔·𝑠𝑖𝑛𝜃𝑘−1·𝑐𝑜𝑠𝜃𝑘−1+(𝑚2𝑙3−𝑚𝑙𝑖)·𝜃𝑘−12·𝑠𝑖𝑛𝜃𝑘−1 𝑠 ·𝑇2 2+𝑥𝑘−1·𝑇+𝑥𝑘−1 (50) 𝑣𝑥|𝑘=(𝑚𝑙2−𝑖)·𝑓𝑘−1+(𝑖−𝑚𝑙2)·𝑏𝑣𝑥|𝑘−1+𝑚2𝑙2𝑔·𝑠𝑖𝑛𝜃𝑘−1·𝑐𝑜𝑠𝜃𝑘−1+(𝑚2𝑙3−𝑚𝑙𝑖)·𝜃𝑘−12·𝑠𝑖𝑛𝜃𝑘−1 𝑠 ·𝑇+𝑥𝑘−1 (51) 𝜃𝑘=𝑚𝑙𝑓𝑘−1·𝑐𝑜𝑠𝜃𝑘−1−𝑚𝑙𝑏𝑣𝑥|𝑘−1·𝑐𝑜𝑠𝜃𝑘−1+(𝑀+𝑚)·𝑚𝑙𝑔·𝑠𝑖𝑛𝜃𝑘−1+𝑚2𝑙2𝜃𝑘−1 2·𝑠𝑖𝑛𝜃𝑘−1·𝑐𝑜𝑠𝜃𝑘−1 𝑞 ·𝑇2 2+𝜃𝑘−1·𝑇+𝜃𝑘−1 (52) 𝜃𝑘=𝑚𝑙𝑓𝑘−1·𝑐𝑜𝑠𝜃𝑘−1−𝑚𝑙𝑏𝑣𝑥|𝑘−1·𝑐𝑜𝑠𝜃𝑘−1+(𝑀+𝑚)·𝑚𝑙𝑔·𝑠𝑖𝑛𝜃𝑘−1+𝑚2𝑙2𝜃𝑘−12·𝑠𝑖𝑛𝜃𝑘−1·𝑐𝑜𝑠𝜃𝑘−1 𝑞 ·𝑇+𝜃𝑘−1 (53) Un cop tenim les equacions que defineixen el model discretitzades, cal linealitzarles per tal de trobar les matrius A, B i C que defineixen el nostre model en espai d’estats i que ens serviran per desenvolupar els diferents estimadors.
79 Pol Capella Roca Com es va comentar en l’explicació teòrica de l’observador de Luenberger (7.3), aquest no està preparat per a estimar de forma correcta sistemes no lineals contaminats amb soroll, i això es pot observar amb molta claredat en la Figura 39 i en la Figura 40. Es pot veure com des de l’inici, degut al soroll dels sensors, l’estimació dona errors de l’ordre d’unitats, fet que seria catastròfic en el cas de voler realitzar missions de gran precisió. A més, en el moment en què el pèndul s’allunya del seu estat d’equilibri (0 radiants, estat en el qual s’ha linealitzat el model), els errors es disparen ràpidament cap a més i menys infinit. Figura 40: error de posició lineal i angular de l’observador de Luenberger
80 Pol Capella Roca Figura 42: error de posició lineal i angular del filtre de Kalman Figura 41: comparativa de les senyals de sortida en el filtre de Kalman
81 Pol Capella Roca La Figura 41 i la Figura 42 donen una informació molt clara sobre el comportament del filtre de Kalman. En aquest cas es pot observar com l’estimació manté un error de l’ordre del soroll introduït pels sensors duran quasi tot el temps de simulació, però en el moment en que el pèndul torna a allunyar-se de l’estat en el qual s’ha linealitzat el model, l’error es dispara cap a més i menys infinit, en aquest cas, de forma una mica més gradual que en el cas de l’observador de Luenberger. Aquests resultats tenen molt sentit ja que exemplifiquen clarament les virtuts i els defectes del filtre de Kalman: és capaç de filtrar les pertorbacions dels sensors (per això obtenim un error de l’ordre d’aquestes pertorbacions), però falla quan s’utilitza en models no lineals (per això es dispara quan ens allunyem de la posició d’equilibri). Figura 43: Comparativa de les senyals de sortida en el filtre de Kalman estès.
82 Pol Capella Roca Finalment en el cas del filtre de Kalman estès (Figura 43 i Figura 44) podem veure com, degut a la seva capacitat de filtrar pertorbacions i d’estimar amb bastanta precisió sistemes no lineals, és capaç de mantenir un error de l’ordre de la pertorbació dels sensors en tot moment, independentment de la posició del pèndul. Figura 44: error de posició lineal i angular del filtre de Kalman estès.
83 Pol Capella Roca 8.Fusió de dades 8.1.Introducció Un cop entesa tota la teoria que envolta el problema de la fusió de dades i els equips que seran usats han estat descrits, en aquest apartat es descriuran les decisions preses per a obtenir la millor estimació del comportament real del quadrotor possible i es mostraran tots els processos seguits. Així doncs bàsicament el que es farà serà pendre una decisió sobre el model a usar, llegir les dades dels sensors del quadrotor i de l’RTK, identificar-ne les pertorbacions i validar experimentalment els resultats obtinguts. AscTec Hummingbird AshTech MB100 Pc extern Fusió de dades Dades d’actitud Dades de posició Figura 45: esquema del procés de fusió de dades
84 Pol Capella Roca 8.2.Sistemes de referència Existeixen diversos sistemes de referència que ens permeten definir de forma clara, per mitjà d’equacions dinàmiques i cinemàtiques, el moviment de l’aeronau representada com a un sòlid rígid. En aquest estudi es treballarà amb dos sistemes de referència de coordenades cartesianes que segueixen la regla de la mà dreta. Es tracta del sistema d’eixos terra (EF, de l’anglès Earth Frame), un sistema inercial lligat a la Terra, i el sistema d’eixos cos (BF, de l’anglès Body Frame), un sistema no inercial lligat al cos que s’està estudiant (el quadrotor en aquest cas). És propi de la notació aeroespacial treballar amb l’eix z positiu apuntant cap amunt, malgrat això la plataforma AscTec Humminbird usa una terna dextrogira XYZ amb l’eix z positiu apuntant cap a baix. Serà necessari tenir això en compte a l’hora de realitzar i interpretar algunes mesures. En la Figura 46 es poden veure els sistemes de referència escollits: Figura 46: Sistemes de referència utilitzats [1] 8.2.1.Body Frame El sistema d’eixos cos (BF) és un sistema de referència no inercial lligat al moviment del quadrotor. S’utilitza el subíndex B per a aclarir que estem treballant amb aquest sistema de referència. Les principals característiques que defineixen el sistema són: OB: l’origen de coordenades se situa sobre el centre de masses del cos.
85 Pol Capella Roca XB: l’eix X apunta cap endavant i es manté en tot moment dins del pla de simetria del quadrotor. YB: l’eix Y apunta cap a la dreta i és perpendicular al pla de simetria. ZB: l’eix Z apunta cap a baix, complint així la regla de la mà dreta. Es defineix a continuació la notació utilitzada per a la posició, la velocitat lineal, l’acceleració lineal, la velocitat angular i l’acceleració angular: [𝑣𝐵]=[𝑣𝑥𝐵 𝑣𝑦𝐵 𝑣𝑧𝐵] [𝑎𝐵]=[𝑎𝑥𝐵 𝑎𝑦𝐵 𝑎𝑧𝐵] [𝑤𝐵]=[𝑝𝑞𝑟] [𝑤𝐵]=[𝑝𝑞𝑟] (57) 8.2.2.Earth Frame El sistema d’eixos Terra és un sistema topocèntric giratori lligat a un punt fixe de la superfície terrestre. Malgrat rota a la mateixa velocitat que la Terra, es considera com a inercial, degut als petits efectes que l’acceleració (centrípeta o centrífuga, segons com es miri) produeix sobre el seu orígen. Es representa amb el subíndex E. Les principals característiques que defineixen el sistema són: ONED: l’origen de coordenades se situa sobre qualsevol punt de referència de l’escorça terrestre. El subíndex NED correspont a les sigles de NorthEst-Down, l’orientació dels eixos del sistema. XE: l’eix x apunta al nord geodèsic, tangent als paral·lels. Representa la latitud (φ), es medeix en graus sexagesimals i va de -90o (Pol Sud) a +90o (Pol Nord), sent 0o l’Equador de la Terra. YE: l’eix y apunta a l’est geodèsic, tangent als meridians. Representa la longitud (λ), es mesura en graus sexagesimals i va de -180o (Oest) a +180o (Est), sent 0o el meridià de Greenwich. ZE: l’eix z apunta cap al centre de la Terra, complint així amb la regla de la mà dreta. Vel. lineal Acc. lineal Vel. angular Acc. angular
86 Pol Capella Roca Cal fixar-se en que com que l’eix z està definit com a positiu apuntant al centre de la Terra, qualsevol alçada del vehicle per sobre de la superfície terrestre serà definida com a negativa (-h). A continuació es defineix la notació utilitzada per a definir la posició, la velocitat lineal, l’acceleració lineal, l’orientació i la velocitat angular de l’aeronau: posició: [𝑷𝑬]= [𝑥𝐸 𝑦𝐸 𝑧𝐸] (58) [𝒗𝑩]=[𝑣𝑥𝐸 𝑣𝑦𝐸 𝑣𝑧𝐸] [𝒂𝑬]=[𝑎𝑥𝐸 𝑎𝑦𝐸 𝑎𝑧𝐸] 𝑎𝑛𝑔𝑙𝑒𝑠=[𝜙𝜃𝜓] [𝑤𝐵]=[𝜙𝜃𝜓] (59) En el cas del dispositiu RTK MB100 l’origen de coordenades a partir del qual es definirà la posició de la plataforma ATH, ve donat per la pròpia plataforma en encendre’s. 8.2.3.Relacions entre EF y BF Durant el procés de fusió de dades, el nostre observador rebrà dades de diferents sensors que, alhora, usen diferents sistemes de referència. És per això que en alguns casos caldrà canviar de referència algunes dades per assegurar que els resultats obtinguts siguin coherents. Existeixen diferents formalismes per a convertir dades descrites per un sistema de referència solidari al cos a la seva descripció en un sistema de referència fix en l’espai tridimensional [1] [24]. Els dos principals són la utilització de matrius de rotació amb els angles d’Euler i l’ús de quaternes. S’escull el primer per simplicitat i per tenir una interpretació física clara. Les matrius de rotació amb angles d’Euler fan servir els tres angles d’Euler (ϕ, θ, ψ) per a descriure l’actitud del vehicle, és a dir, la relació que existeix entre els dos sistemes de referència representats. Els angles d’Euler representen rotacions elementals successives al voltant de diferents eixos. Cadascuna d’aquestes rotacions pot ser representada mitjançant una matriu de gir. La matriu de canvi Vel. lineal Acc. lineal Angles d’Euler Vel. angular
87 Pol Capella Roca de base final entre BF i EF (i a l’inrevés) s’obté multiplicant les tres matrius de rotació. El problema de l’ús d’angles d’Euler és que apareixen singularitats degudes a l’efecte conegut com a “Gimbal lock”. Aquest fenomen apareix quan l’angle θ és igual a ± 90o, generant que els eixos x i z coincideixin en la mateixa direcció i, per tant, no es pugui definir un mapping que relacioni les velocitats angulars i les derivades dels angles d’Euler. Malgrat tot en aquest projecte s’escull la convenció ZYX per ser la més utilitzada en les referències consultades [1] [24] [25]. El fenomen del “Gimbal lock” no hauria de produir-se en condicions de vol normal en el nostre quadrotor, amb la qual cosa no representa un gran contratemps [26]. La convenció ZYX implica la següent transformació. Partint del sistema de referència EF, es gira al voltant de l’eix ZE un angle ψ, obtenint el triedre x1y1z1. Després es gira al voltant del nou eix y1 un angle θ, obtenint el triedre x2y2z2. Per acabar es gira un angle ϕ al voltant de x2, obtenint el sistema de referència final desitjat XBYBZB. Figura 47: seqüència de rotació de EF a BF La matriu de canvi de base que transforma un vector columna descrit en EF a BF s’obté al multiplicar les tres matrius de gir que roten el vector per a cada angle d’Euler. La matriu obtinguda rep el nom de 𝑅𝐸𝐵 i la seva seqüència de gir es mostra en l’equació ( 60 ). 𝑅𝐸𝐵=𝑅(𝑥,𝜙)𝑅(𝑦,𝜃)𝑅(𝑧,𝜓) ( 60 ) 𝑅𝐸𝐵=[1 0 0 0 𝑐𝑜𝑠𝜙 −𝑠𝑖𝑛𝜙 0 𝑠𝑖𝑛𝜙 𝑐𝑜𝑠𝜙][𝑐𝑜𝑠𝜃 0 𝑠𝑖𝑛𝜃 0 1 0 −𝑠𝑖𝑛𝜃 0 𝑐𝑜𝑠𝜃][𝑐𝑜𝑠𝜓 −𝑠𝑖𝑛𝜓 0 𝑠𝑖𝑛𝜓 𝑐𝑜𝑠𝜓 0 0 0 1] ( 61 )
88 Pol Capella Roca 𝑅𝐸𝐵=[ 𝑐𝑜𝑠𝜃𝑐𝑜𝑠𝜓 𝑐𝑜𝑠𝜃𝑠𝑖𝑛𝜓 −𝑠𝑖𝑛𝜃 𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜃𝑐𝑜𝑠𝜓−𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜓 𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜃𝑠𝑖𝑛𝜓+𝑐𝑜𝑠𝜙𝑐𝑜𝑠𝜓 𝑠𝑖𝑛𝜙𝑐𝑜𝑠𝜃 𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜃𝑐𝑜𝑠𝜓+𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜓 𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜃𝑠𝑖𝑛𝜓−𝑠𝑖𝑛𝜙𝑐𝑜𝑠𝜓 𝑐𝑜𝑠𝜙𝑐𝑜𝑠𝜃] ( 62 ) Cal tenir en compte que al tractar-se d’una matriu de rotació és ortonormal i, per tant, la matriu de rotació per a passar de EF a BF s’obté de la següent manera: 𝑅𝐵𝐸=(𝑅𝐸𝐵)−1=(𝑅𝐸𝐵)𝑇 (63) D’altra banda la relació entre les velocitats angulars entre BF i EF es defineix de la següent manera [24]. [𝜙𝜃𝜓]=[1 𝑠𝑖𝑛𝜙𝑡𝑎𝑛𝜃 𝑐𝑜𝑠𝜙𝑡𝑎𝑛𝜃 0 𝑐𝑜𝑠𝜙 −𝑠𝑖𝑛𝜙 0 𝑠𝑖𝑛𝜙𝑠𝑒𝑐𝜃 𝑐𝑜𝑠𝜙𝑠𝑒𝑐𝜃][𝑝𝑞𝑟] (64) En l’equació (64) es pot observar de forma molt clara l’efecte del “Gimbal lock”, ja que quan l’angle theta sigui igual a ± 90o, tant les tangents com les secants tendiran a infinit, impossibilitant així establir una relació entre les velocitats angulars en eixos cos i les derivades d’angles d’Euler. 8.3.Model Després de l’estudi realitzat en l’apartat 2.1.2 s’ha optat per la utilització d’un model cinemàtic, ja que ens permet avaluar de forma correcta el comportament del quadrotor sense incorporar una gran dificultat o complexitat de càlcul. L’objectiu d’aquest projecte és validar l’algoritme de fusió de dades amb els motors del quadrotor sense engegar (es mourà manualment), amb la qual cosa no comptarem amb senyals d’entrada sobre l’estat de les hèlixs i les forces que estan desenvolupant. És per això que s’ha decidit optar per models que descriguin processos estocàstics de forma general i en cap cas necessitin conèixer les característiques físiques o de forma del cos. En aquest sentit podem distingir tres tipus diferents de models [27], [28]: White Noise Models: l’entrada de control és modelada com un procés de soroll aleatori. Markov Process Models: l’entrada de control es modela com un procés de Markov.
95 Pol Capella Roca RTK L’estimació del soroll del GPS diferencial s’ha fet a partir de les dades de precisió (desviació típica) del sensor que ens proporciona el fabricant [30]. La variància de les mesures de posició serà doncs la que es mostra en la : xE yE zE σ2 (m2) 0.0025 0.0025 0.0004 Taula 6: variància en les mesures de l’RTK 8.3.3.Mesures Abans de que les senyals dels sensors entrin en el nostre algoritme d’estimació, és necessari tractar-les per eliminar fonts d’error que el nostre estimador no detecta. Així doncs s’han construït “models d’error” de les senyals dels diferents sensors per tractar-ne les pertorbacions. A més caldrà tenir en compte alguns fenòmens que afecten a les nostres lectures i que afectaran a la construcció de la matriu de sortida del nostre model. A continuació es mostren els tractaments que s’ha donat a cadascuna de les senyals: 8.3.3.1.Giroscopi Com s’ha comentat a l’apartat 3.3.2, la nostra plataforma utilitza un giroscopi 3D, model ADXRS610, del fabricant Analog Devices. Aquest sensor ens proporciona la velocitat angular del quadrotor mesurada en eixos cos. Malgrat tot aquesta senyal ve contaminada per diferents tipus de pertorbacions que han de ser tractades per a assegurar un bon funcionament del UAV. Podem distingir dos agents contaminants principals: Soroll dels sensors : fa referència la covariància de les mesures. Bias: és un petit offset que té el sensor i que genera una desviació, més o menys constant, en les mesures de l’aparell. A més, aquest error
96 Pol Capella Roca afectarà de forma lineal a les variables que tonguin a veure amb la integració de la velocitat angular (actitud). Això ens permet construir un model d’error del giroscopi, que tractarà els errors abans que la senyal entri en el filtre de Kalman: 𝜔 =𝜔 0−𝑏 𝑔𝑖𝑟+𝜎2 𝑔𝑖𝑟 (80) On ω fa referència a la nostra variable d’estat velocitat angular,ω0 és la mesura del sensor, bgir és el bias i σ2gir el soroll aleatori. El soroll blanc que contamina la senyal ja és tingut en compte pel nostre observador un cop la senyal entre a l’algoritme, però el bias no. Per tant, abans de treballar amb la senyal del sensor, serà necessari eliminar el bias. Això ens permetrà minimitzar l’error de deriva que tindríem, al tractar-se d’un model que estima algunes variables a partir de la integració d’unes altres. Per a mesurar el bias s’han fet mesures del valor de la senyal del giroscopi en estàtic, obtenint els següents resultats: p q r bgir (rad/s) -0.0014 -3.9082·10-4 -5.0757·10-4 Taula 7: mesures experimentals del bias del giroscopi 8.3.3.2.Magnetòmetre El magnetòmetre és un sensor que mesura la intensitat de camp magnètic en els 3 eixos cos. Gràcies a aquestes mesures s’obté un vector de camp que dona informació de l’actitud del quadrotor. El camp es mesura a través d’unes magneto-resistències que canvien el seu valor en funció del camp que les travessa en la seva direcció.
97 Pol Capella Roca Figura 48: magneto-resistència [24] El seu funcionament és doncs molt semblant al de les brúixoles, amb l’única diferència que en el cas del magnetòmetre no es parteix de la suposició que s’està mesurant en el pla horitzontal de la Terra. Camp magnètic terrestre Per entendre millor el funcionament d’aquest dispositiu i utilitzar-lo correctament en aplicacions de navegació, és necessari entendre a grans trets com actua el camp magnètic terrestre. La Terra es comporta com un dipol, però aquest no està alineat amb els seus eixos de rotació. Per això sempre apareix un terme de Declinació, que és l’angle existent entre el nord magnètic i el nord geogràfic (veure Figura 49). Per nord magnètic s’entén aquella regió de la Terra on les línies del camp magnètic són perpendiculars a la superfície. La intensitat d’aquest camp no és constant i varia dels 60000 ηT als pols, als 25000 ηT a l’equador (1 ηT = 100000 G). A més també es poden produir variacions de la intensitat en el temps a llarg i curt termini degut, per exemple, a fenòmens com les tempestes geomagnètiques. Cal mencionar però els canvis a curt termini solen ser bastant infreqüents i de baixa magnitud.
98 Pol Capella Roca Figura 49: declinació magnètica [31] Model d’error El model d’error serà molt semblant a l’utilitzat en el cas del giroscopi, ja que les fonts de distorsió que afecten a la senyal del magnetòmetre són les mateixes. Tindrem doncs el següent model: 𝐵 =𝐵 0−𝑏 𝑚𝑎𝑔+𝜎2 𝑚𝑎𝑔 (81) On: B és el camp magnètic que s’usarà per al nostre vector de mesures i B0 és la mesura directa del magnetòmetre El component de soroll blanc (σ2mag) torna a estar contemplat en el nostre observador, amb la qual cosa l’única operació prèvia necessària serà l’eliminació del bias (bmag). Per a calcular el bias s’han usat les dades d’actitud que ens ofereix el sistema de fusió de dades de bord de l’AscTec Hummingbird; s’ha situat el quadrotor en posició de repòs, de tal manera que tots els angles fóssin 0, i s’han pres mesures del camp magnètic, quedant aquestes, per tant, expressades en eixos Terra. Cal tenir en compte que la plataforma ATH proporciona dades del camp magnètic adimensionalitzades, amb la qual cosa no ens permet conèixer la intensitat del camp magnètic en el punt de mesura, però ens ofereix informació de les magnituds de les components d’aquest camp en eixos cos.
99 Pol Capella Roca Les dades de camp magnètic recollides s’han normalitzat i comparat amb els valors de camp magnètic que proporcionen diferents organismes de prestigi internacional [32]–[34], en el punt de mesura. Finalment s’ha pres la diferència entre les mesures i les dades proporcionades com el bias d’aquest sensor, obtenint els resultats mostrats en la Taula 8: uBx uBy uBz Bmag -0.0347 -0.0263 0.0214 Taula 8: mesures experimentals del bias del magnetòmetre En la Taula 8 les mesures de bias es presenten normalitzades ja, doncs el nostre vector de mesures inclourà les mesures del camp magnètic normalitzades també. Model de mesura Tenint en compte el funcionament del magnetòmetre i el comportament del camp magnètic terrestre, construirem un model que ens permeti avaluar les components del camp magnètic terrestre en eixos cos, en funció de l’actitud del quadrotor. El model consistirà bàsicament en projectar el vector de camp magnètic expressat en eixos Terra, en eixos cos. Aquest model planteja dues dificultats, degudes principalment a la particularitat del camp magnètic terrestre: la variació en la intensitat del camp magnètic, deguda a la zona i a efectes temporals, i la variació en l’angle de declinació, deguda a la posició. Per a solucionar el primer problema s’ha optat per despreciar els efectes dels canvis temporals, ja que aquests solen produir.se en llargs períodes de temps, i normalitzar el camp magnètic amb els valors porporcionats pel magnetòmetre en la zona de mesura de referència. Pel que fa a l’angle de declinació s’ha optat per no tenir en compte els seus efectes, ja que en la zona on es farà la validació de l’algorítme, aquest angle es pràcticament nul (0o 27’ [33]). Aquesta declinació, al igual que el valor de la intensitat de camp magnètic, també pot variar amb el temps. En 11 es comenta una possible solució a aquest problema. Així doncs el model consistirà en projectar el vector de camp magnètic proporcionat per les referències usades, normalitzat, en eixos cos:
100 Pol Capella Roca [𝑢𝐵𝑥𝐵 𝑢𝐵𝑦𝐵 𝑢𝐵𝑧𝐵]=[ 𝑐𝑜𝑠𝜃𝑐𝑜𝑠𝜓 𝑐𝑜𝑠𝜃𝑠𝑖𝑛𝜓 −𝑠𝑖𝑛𝜃 𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜃𝑐𝑜𝑠𝜓−𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜓 𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜃𝑠𝑖𝑛𝜓+𝑐𝑜𝑠𝜙𝑐𝑜𝑠𝜓 𝑠𝑖𝑛𝜙𝑐𝑜𝑠𝜃 𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜃𝑐𝑜𝑠𝜓+𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜓 𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜃𝑠𝑖𝑛𝜓−𝑠𝑖𝑛𝜙𝑐𝑜𝑠𝜓 𝑐𝑜𝑠𝜙𝑐𝑜𝑠𝜃]·[𝑢𝐵𝑥𝐸 𝑢𝐵𝑦𝐸 𝑢𝐵𝑧𝐸] (82) on: [𝑢𝐵𝑥𝐸 𝑢𝐵𝑦𝐸 𝑢𝐵𝑧𝐸]=[0.5463 0.0053 0.8375] (83) Les equacions desenvolupades són les següents: 𝑢𝐵𝑥𝐵=𝑐𝑜𝑠𝜃𝑐𝑜𝑠𝜓𝑢𝐵𝑥𝐸+𝑐𝑜𝑠𝜃𝑠𝑖𝑛𝜓𝑢𝐵𝑦𝐸−𝑠𝑖𝑛𝜃𝑢𝐵𝑧𝐸 (84) 𝑢𝐵𝑦𝐵=(𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜃𝑐𝑜𝑠𝜓−𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜓)𝑢𝐵𝑥𝐸 +(𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜃𝑠𝑖𝑛𝜓+𝑐𝑜𝑠𝜙𝑐𝑜𝑠𝜓)𝑢𝐵𝑦𝐸+𝑠𝑖𝑛𝜙𝑐𝑜𝑠𝜃𝑢𝐵𝑧𝐸 (85) 𝑢𝐵𝑧𝐵=(𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜃𝑐𝑜𝑠𝜓+𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜓)𝑢𝐵𝑥𝐸 +(𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜃𝑠𝑖𝑛𝜓−𝑠𝑖𝑛𝜙𝑐𝑜𝑠𝜓)𝑢𝐵𝑦𝐸+ 𝑐𝑜𝑠𝜙𝑐𝑜𝑠𝜃𝑢𝐵𝑧𝐸 (86) on u s’utilitza per a expressar que es tracta de vectors normalitzats i B representa el camp magnètic en cadascuna de les direccions dels eixos cos. Utilitzar aquest model ens permetrà validar l’actitud calculada amb el model d’estimació a priori i els acceleròmetres, i a més establirà una referència per a les mesures d’angle de guinyada (d’altra manera s’hagués pres com a orígen la primera estimació). 8.3.3.3.Acceleròmetre El quadrotor usat en aquest projecte usa un acceleròmetre 3D, que permet mesurar variacions en l’acceleració lineal en qualsevol dels tres eixos cos. En concret el funcionament del nostre acceleròmetre es basa en la tecnologia de flux de calor implementada per l’empresa MEMSIC. El sensor, en cada un dels tres eixos, consta d’un calentador i de dues termopiles situades als extrems del mateix. Si l’acceleració és nul·la, no es produeix cap flux de calor per sobre de les termopiles, amb la qual cosa els voltatges a la seva sortida són idèntics. Si per contra el vehicle està accelerant, apareix un flux de calor, que genera uns perfils
101 Pol Capella Roca tèrmics asimètrics en cadascuna de les dues piles, amb les conseqüents variacions en els voltatges de sortida (veure Figura 50). Figura 50: esquema de funcionament de l’acceleròmetre [35] Model d’error El model d’error de l’acceleròmetre serà igual que el dels sensors anteriors: 𝑎 𝑏=𝑎 0−𝑏 𝑎+𝜎2 𝑎 (87) On ab és la lectura que s’usarà en el vector de mesures i a0 la mesura directa de l’acceleròmetre. Tal com s’ha comentat amb el giroscopi i el magnetòmetre, l’error aleatori ja està contemplat en l’algoritme d’estimació, amb la qual cosa l’únic tractament previ necessari serà l’eliminació del bias. Per a mesurar el bias s’han fet experiments amb l’aeronau en estàtic i, utilitzant les dades d’actitud proporcionades pel sistema de fusió de dades de fàbrica del quadrotor, s’han calculat els valors teòrics d’acceleració (components de la gravetat expresstas en eixos cos) que hauríem d’estar mesurant a cada eix. Aquests càlculs s’han comparat amb els valors mesurats, obtenint els següents resultats de bias:
102 Pol Capella Roca ax ay az ba (m/s2) 0.0081 -0.0108 -0.1402 Taula 9: mesures experimentals del bias de l’acceleròmetre Durant la realització dels diferents experiments no s’han apreciat grans variacions en el bias, fet que confirma la validesa dels models d’error usats. Model de mesura El model de mesura que ens permetrà obtenir les lectures dels sensors en funció de les nostres variables d’estat, haurà de tenir en compte, en aquest cas, els fenòmens que afecten a la lectura de l’acceleròmetre. Primer de tot el camp gravitatori de la Terra que afecta de forma permanent a les mesures. Quan el quadrotor està horitzontal en estat de repòs, l’acceleròmetre ens dona una mesura de –g (corresponent a la força de reacció al pes) amb la qual cosa s’haurà de sumar aquest valor per a obtenir l’acceleració real del vehicle. A més caldrà tenir en compte també que la plataforma es troba en un sistema inercial que rota. Per tant s’haurà de corregir l’acceleració resultant que apareixerà als acceleròmetres. L’expressió que ens permetrà obtenir l’acceleració en un sistema idealment inercial serà: 𝑎 𝐼𝑏=𝑎 𝑏+(𝜔 +Ω )×𝑣 𝑏+𝑔 𝑏 (88) on: (𝜔 +Ω )=[𝑝𝑞𝑟] (89) 𝑔𝑏=[−𝑔𝑠𝑖𝑛𝜃 𝑔𝑐𝑜𝑠𝜃𝑠𝑖𝑛𝜙 𝑔𝑐𝑜𝑠𝜃𝑐𝑜𝑠𝜙] (90) El subíndex I, fa referència a que està treballant en un sistema inercial. Cal tenir en compte que no s’està realitzant un canvi de coordenades, sinó que simplement s’està adequant l’acceleració mesurada en eixos cos a un sistema d’eixos cos teòricament inercial.
103 Pol Capella Roca Les omegues de l’equació (88) simbolitzen la rotació total de la Terra Ω i la del propi cos ω. El giroscopi no distingeix entre les dues rotacions i, per tant, el vector de mesures que ens proporciona [p,q,r] serà la suma de les dues rotacions. L’equació (88) desenvolupada dona els següents resultats: 𝑎𝐼𝑏𝑥=𝑎𝑏𝑥−𝑣𝑏𝑦𝑟+𝑣𝑏𝑧𝑞−𝑔𝑠𝑖𝑛𝜃 (91) 𝑎𝐼𝑏𝑦=𝑎𝑏𝑦+𝑣𝑏𝑥𝑟−𝑣𝑏𝑧𝑝+𝑔𝑐𝑜𝑠𝜃𝑠𝑖𝑛𝜙 (92) 𝑎𝐼𝑏𝑧=𝑎𝑏𝑧−𝑣𝑏𝑥𝑞+𝑣𝑏𝑦𝑝+𝑔𝑐𝑜𝑠𝜃𝑐𝑜𝑠𝜙 (93) Amb aquestes equacions obtenim la variable d’estat acceleració, en funció de la mesura de l’acceleració i altres variables d’estat, però a nosaltres ens interessarà tenir la mesura en funció de les variables d’estat, amb la qual cosa obtindrem les següents equacions de mesura: 𝑎𝑏𝑥=𝑎𝐼𝑏𝑥+𝑣𝑏𝑦𝑟−𝑣𝑏𝑧𝑞+𝑔𝑠𝑖𝑛𝜃 (94) 𝑎𝑏𝑦=𝑎𝐼𝑏𝑦−𝑣𝑏𝑥𝑟+𝑣𝑏𝑧𝑝−𝑔𝑐𝑜𝑠𝜃𝑠𝑖𝑛𝜙 (95) 𝑎𝑏𝑧=𝑎𝐼𝑏𝑧+𝑣𝑏𝑥𝑞−𝑣𝑏𝑦𝑝−𝑔𝑐𝑜𝑠𝜃𝑐𝑜𝑠𝜙 (96) 8.3.4.Linealització del model en espai d’estats El model d’estimació a priori que s’utilitza ja és un model lineal, a excepció d’aquelles variables que requereixen d’un canvi de base per a adaptar-se al procés: la posició i l’actitud. Les equacions que modelitzen la posició, sense linealitzar, són les següents: [x𝐸 y𝐸 z𝐸]= [ 𝑥𝐸+𝑣𝑥𝐸·𝑇+𝑎𝑥𝐸·𝑇2 2 𝑦𝐸+𝑣𝑦𝐸·𝑇+𝑎𝑦𝐸·𝑇2 2 𝑧𝐸+𝑣𝑧𝐸·𝑇+𝑎𝑧𝐸·𝑇2 2 ] 𝑘−1 (97) amb:
104 Pol Capella Roca 𝑣𝑥𝐸=𝑐𝑜𝑠𝜃𝑐𝑜𝑠ѱ𝑣𝑥𝐵+(𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜃𝑐𝑜𝑠ѱ−𝑐𝑜𝑠𝜙𝑠𝑖𝑛ѱ)𝑣𝑦𝐵 +(𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜃𝑐𝑜𝑠ѱ+𝑠𝑖𝑛𝜙𝑠𝑖𝑛ѱ)𝑣𝑧𝐵 (98) 𝑎𝑥𝐸=𝑐𝑜𝑠𝜃𝑐𝑜𝑠ѱ𝑎𝑥𝐵+(𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜃𝑐𝑜𝑠ѱ−𝑐𝑜𝑠𝜙𝑠𝑖𝑛ѱ)𝑎𝑦𝐵 +(𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜃𝑐𝑜𝑠ѱ+𝑠𝑖𝑛𝜙𝑠𝑖𝑛ѱ)𝑎𝑧𝐵 (99) 𝑣𝑦𝐸=𝑐𝑜𝑠𝜃𝑠𝑖𝑛ѱ𝑣𝑥𝐵+(𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜃𝑠𝑖𝑛ѱ+𝑐𝑜𝑠𝜙𝑐𝑜𝑠ѱ)𝑣𝑦𝐵 +( 𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜃𝑠𝑖𝑛ѱ−𝑠𝑖𝑛𝜙𝑐𝑜𝑠ѱ)𝑣𝑧𝐵 (100) 𝑎𝑦𝐸=𝑐𝑜𝑠𝜃𝑠𝑖𝑛ѱ𝑎𝑥𝐵+(𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜃𝑠𝑖𝑛ѱ+𝑐𝑜𝑠𝜙𝑐𝑜𝑠ѱ)𝑎𝑦𝐵 +( 𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜃𝑠𝑖𝑛ѱ−𝑠𝑖𝑛𝜙𝑐𝑜𝑠ѱ)𝑎𝑧𝐵 (101) 𝑣𝑧𝐸=−𝑠𝑖𝑛𝜃𝑣𝑥𝐵+𝑠𝑖𝑛𝜙𝑐𝑜𝑠𝜃𝑣𝑦𝐵+𝑐𝑜𝑠𝜙𝑐𝑜𝑠𝜃𝑣𝑧𝐵 (102) 𝑎𝑧𝐸=−𝑠𝑖𝑛𝜃𝑎𝑥𝐵+𝑠𝑖𝑛𝜙𝑐𝑜𝑠𝜃𝑎𝑦𝐵+𝑐𝑜𝑠𝜙𝑐𝑜𝑠𝜃𝑎𝑧𝐵 (103) Les equacions que ens proporcionen l’actitud del quadrotor són les que continuen: [𝜙𝜃𝜓]=[𝜙+𝜙·𝑇 𝜃+𝜃·𝑇 𝜓+𝜓·𝑇]𝑘−1 (104) amb: 𝜙=𝑝+𝑠𝑖𝑛𝜙𝑡𝑎𝑛𝜃𝑞+𝑐𝑜𝑠𝜙𝑡𝑎𝑛𝜃𝑟 (105) 𝜃=𝑐𝑜𝑠𝜙𝑞−𝑠𝑖𝑛𝜙𝑟 (106) 𝜓=𝑠𝑖𝑛𝜙𝑠𝑒𝑐𝜃𝑞+𝑐𝑜𝑠𝜙𝑠𝑒𝑐𝜃𝑟 (107) A continuació es mostra el model linealitzat, seguint el procediment explicat en l’apartat 7.6. Es construeixen unes matrius de 3x3 (M, N, P...) per a facilitar l’escriptura posterior de les matrius del procés i sortida. Els coeficients d’aquestes matrius poden ser consultats en l’Annex B.
111 Pol Capella Roca 8.4.2.1.Parser La paraula parser fa referència a la decodificació de missatges, en aquest cas, de protocol NMEA 0183. En aquest projecte s’ha construit una funció en matlab que permet llegir les dades enviades pel satèl·lit i agafar-ne únicament les que ens interessen. S’ha utilitzat aquesta funció per a parsejar les sentències del tipus $GPGGA que són les que ens proporcionen dades de latitud, longitud i altitud. En concret el que fa l’algoritme es descodificar les sentències rebudes en codi ASCII i aïllar-ne les subtrames que ens interessen, per a enviar-les com a dades de tipus double al filtre de Kalman. A més la funció també té en compte la qualitat de les dades rebudes (evalua si s’estàn rebent amb la funcionalitat GPS habitual, DGPS o RTK) i transmet la informació a l’estimador, per tal que ho tingui en compte en el soroll dels sensors (adapta la variància a usar en l’estimació). 8.4.2.2.Adaptació d’unitats S’haurà de treballar també sobre les unitats i els sistemes de referència d’aquestes dades; els receptors GPS utilitzen un sistema de referència 3D conegut com a World Geodic System 1984 –WGS 84que haurà de ser transformat a un sistema 2D que rep el nom de Universal Transverse Mercator –UTM-, per tal de poder treballar amb les dades de posició en les unitats desitjades. A continuació s’expliquen les particularitats de cada sistema i es mostren les equacions necessàries normalitzades per al canvi de coordenades. World Geodic System 1984 El Sistema Geodèsic Mundial 1984 és un Sistema Terrestre Convencional (CTS), realitzat l’any 1984 modificant el Navy Navigation Satellite System (NNSS) en orígen i escala, i rotant-lo per tal de fer coincidir el seu meridià de referència amb el meridià zero definit per la Bureau International de l’Heure 3 (BIH). L’origen i els eixos del sistema de coordenades WGS 84 es defineixen de la següent manera: Origen: se sitúa en el centre de masses de la Terra. 3 El BIH localitzat a l’observatori de París, era l’organisme internacional responsable de combinar diferents criteris i mesures per a establir el temps universal.
112 Pol Capella Roca ZWGS-84: en la direcció del Pol Terrestre Convencional (CTP), tal com el defineix el BIH basat en les coordenades proporcionades per les seves estacions. XWGS-84: és la intersecció del pla que defineix el meridià de referència del sistema WGS 84 i el pla de l’equador definit pel CTP. YWGS-84: completa el trièdre seguint la regla de la mà dreta –apunta a l’Est- . En la Figura 52 es pot observar una reproducció del sistema de referència WGS 84: Figura 52: definició del sistema de referència WGS 84 [19] Es tracta doncs d’un sistema global de referència fixe a la Terra, que inclou un model de la mateixa, i que es defineix per un seguit de paràmetres primaris i secundaris. Els paràmetres primaris –consultar Taula 10 – defineixen la forma d’un el·lipsoide terrestre, la seva velocitat angular i la massa de la Terra. Els paràmetres secundaris donen informació d’un Model de Camp Gravitatori Terrestre (EGFM) detallat.
113 Pol Capella Roca Paràmetre Símbol WGS 84 Semi eix major a 6 378 137 m Velocitat angular ω 7.292115 x 10-5 rad s-1 Constant gravitacional geocèntrica (inclosa la massa de l'atmosfera) GM 398 600.5 km3 s-2 Coeficient harmònic zonal de segon del potencial gravitacional normalitzat C2,0 - 484,16685 x 10-6 Aplanament (derivat de C2,0 ) α 1/298.257223563 Universal Transverse Mercator El Sistema Universal Transvers de Mercator (UTM) és un sistema de coordenades basat en la projecció transversa de Mercator, que s’utilitza en la majoria de projeccions cartogràfiques. A diferència dels sistemes de coordenades geogràfiques (com ho és el WGS 84), on les magnituds s’expressen en latituds i longituds, les posicions en el sistema UTM s’expressen en metres a nivell del mar, que és la base de la projecció de l’el·lipsoide de referència. Projecció Transversa de Mercator Lo projecció comuna de Mercator, utilitzada en una gran quantitat de mapes mundials, és una projecció cilíndrica, el que significa que el globus terraqui és encerclat per un cilindre imaginari tangent a l’equador, i la Terra es projecta dins del cilindre. Es tracta d’una projecció conforme o, dit en altres paraules, la projecció conserva els angles i les petites formes de la Terra. El preu que es paga però, és una gran variació en escala, lluny de les porcions centrals del mapa (l’equador, en una projecció de Mercator normal). Com a exemple, en un mapa de Mercator, Groenlàndia sembla tant gran com Amèrica del Sud, quan, en realitat, representa aproximadament 1/8 part de la superfície del continent Sudamericà. En canvi si un para atenció a una petita regió de la costa, veurà que la reprodució, en forma, és pràcticament idèntica a la realitat. Taula 10: paràmetres primaris del sistema WGS 84
114 Pol Capella Roca Per tant a l’hora de representar superfícies verticalment allargades, la projecció de Mercator deixa de ser vàlida. És aquí on apareix la projecció transversa, en la qual, en comptes de pendre com a l’inia de referència l’equador, el cilindre és tangent als meridians (veure Figura 53). Figura 53: a l’esquerra, projecció de Mercator, a la dreta, projecció transversa de Mercator [36] Com que la projecció transversa de Mercator és molt acurada en seccions estretes de la Terra, ha esdevingut la base del sistema de referència que rep el seu nom: Universal Transverse Mercator System o UTM. El que es fa en aquest sistema és dividir la Terra en zones estretes en longitud, que es representen mitjançant una projecció transversa de Mercator. Amb aquestes divisions es construeix una xarxa sobre la projecció, que permet localitzar de forma fàcil qualsevol punt de la Terra. Figura 54: xarxa UTM [37] La xarxa UTM (Figura 54) divideix la Terra en 60 husos de 6o de longitud. Els husos es numeren de l’1 al 60, estant el primer limitat entre les longitud 180o i 174o W, amb meridià central als 177oW. Cada hus té assignat un meridià central, que és on se situa el punt de referència per a prendre mesures.
115 Pol Capella Roca D’aquesta manera s’aconsegueix que cap punt estigui massa allunyat del seu meridià de referència, evitant distorsions excessives, però es genera una discontinuïtat en el límit de cada zona, ja que un punt es pot projectar amb dues coordenades diferents. Per evitar aquestes discontinuïtats, a vegades s’estenen les zones per incloure el punt situat en un meridià de conflicte, però això suposa una major distorsió d’escala. Pel que fa a les divisions horitzontals el sistema UTM divideix el planeta en 20 zones conegudes com a bandes. Aquestes zones es troben entre els paral·lels 80oS i 84oN, ja que, degut a la forma de la Terra, les distorsions en els pols són massa elevades. A cada banda se li assigna una lletra de la C a la X, excloent-n’hi la I i la O, per la seva semblança amb els números 1 i 0. Aquest sistema doncs presenta l’avantatge que es pot identificar una posició de forma molt més intuïtiva que amb les coordenades geogràfiques, però per altra banda necessita de l’establiment de punts de referència en cada mesura. 8.4.2.3.Canvi de base Com s’ha comentat en el capítol 6.5 la xarxa GPS treballa amb coordenades geogràfiques, del format latitud, longitud i alçada, en un sistema de referència geocèntric, mentre que el nostre model expressa la posició en coordenades cartesianes, referenciades en eixos Terra. És per això que serà necessari realitzar un canvi de base del WGS 84 al sistema UTM. Per a realitzar aquest procés existeixen tres mètodes fonamentals: Taules de projecció UTM Fórmules de transformació directa del US Army Fórmules de Coticchia-Surace En aquest projecte s’ha decidit utilitzar les fórmules de Coticchia-Surace, per la seva simplicitat de programació i la bona precisió que ofereixen [38]. A continuació es mostra el procés de càlcul que segueixen les fórmules de Coticchia-Surace:
116 Pol Capella Roca Càlculs previs Conegudes les particularitats geomètriques de l’el·lipsoide de referència utilitzat per el nostre GPS (consultar 6), es realitzen una sèrie de càlculs relacionats amb la seva geometria: Excentricitat:𝑒=√𝑎2−𝑏2 𝑎 (110) Segona excentricitat: 𝑒′=√𝑎2−𝑏2 𝑏 (111) Radi Polar de Curvatura:𝑐=𝑎2 𝑏 (112) Aplanament: 𝛼=𝑎−𝑏 𝑎 (113) En realitat l’aplanament i la primera excentricitat no són necessàries en l’aplicació posterior de les equacions de Coticchia-Surace, però en algunes ocasions no es proporcionen totes les dades geomètriques de l’el·lipsoide i és necessari conèixer aquestes relacions per a extreure-les. També serà necessari tractar les unitats de les dades de longitud (λ) i latitud (φ). La majoria de vegades aquestes dades venen expressades en format de graus sexagesimals que han de ser transformats a graus decimals, expressats en radiants: 𝑔𝑟𝑎𝑢𝑠 𝑑𝑒𝑐𝑖𝑚𝑎𝑙𝑠=𝑔𝑟𝑎𝑢𝑠+𝑚𝑖𝑛 60+𝑠𝑒𝑔 60 (114) 𝑅𝑎𝑑𝑖𝑎𝑛𝑡𝑠=𝑔𝑟𝑎𝑢𝑠 𝑑𝑒𝑐𝑖𝑚𝑎𝑙𝑠·𝜋 180 (115) En quan a les dades de longitud, ens haurem d’assegurar també si estan referides al Est (E) o a l’Oest (W), ja que això afectarà al seu signe (est positives, oest negatives). Un cop les dades de longitud i latitud han estat preparades, podem procedir al càlcul de l’hus o la zona UTM on cau la posició actual: 𝐻𝑢𝑠=𝑖𝑛𝑡[𝑔𝑟𝑎𝑢𝑠 𝑑𝑒𝑐𝑖𝑚𝑎𝑙𝑠 6+31] (116)
117 Pol Capella Roca En l’equació 83 es poden veure diversos conceptes explicats en l’apartat 0: la longitud, en graus decimals, es divideix entre 6 perquè aquesta és l’amplitud de les zones UTM i es suma 31 al resultat d’aquest quocient, ja que l’origen de latitud del sistema WGS 84 se situa al meridià oposat (180o) de l’origen del sistema UTM. Un cop coneixem l’hus el següent pas és calcular el meridià central, que servirà de referència per a les mesures de longitud: 𝜆0=ℎ𝑢𝑠·6−183 (117) I així podrem establir la distància existent entre la nostra posició i l’origen de referència UTM: ∆𝜆=𝜆−𝜆0 (118) En la Figura 55 es pot veure de forma esquemàtica una explicació del procés de càlcul de l’hus:
118 Pol Capella Roca Figura 55: característiques de l’hus [36] Un cop realitzats tots els càlculs previs ja es pot procedir a l’aplicació de les fórmules de Coticchia-Surace en si. (∆λ)
119 Pol Capella Roca Càlcul de paràmetres A continuació es calculen una sèrie de paràmetres que van encadenats els uns amb els altres, i que són el nucli de les fórmules de Coticchia-Surace. Es tracta d’un seguit d’operacions bastant rutinàries i que són fàcilment programables: 𝐴=𝑐𝑜𝑠φ·sin∆λ (119) 𝜉=12·𝑙𝑛[1+𝐴 1−𝐴] (120) 𝜂=arctan(𝑡𝑎𝑛𝜑 𝑐𝑜𝑠∆λ)−𝜑 (121) 𝜈= 𝑐 (1+𝑒′2·𝑐𝑜𝑠2𝜑)1/2·0.9996 (122) 𝜁=𝑒′2 2·𝜉2·𝑐𝑜𝑠2𝜑 (123) 𝐴1=sin (2𝜑) (124) 𝐴2=𝐴1·𝑐𝑜𝑠2𝜑 (125) 𝐽2=𝜑+𝐴1 2 (126) 𝐽4=3·𝐽2+𝐴2 4 (127) 𝐽6=5·𝐽4+𝐴2·𝑐𝑜𝑠2𝜑 3 (128) 𝛼=34·𝑒′2 (129) 𝛽=53·𝛼2 (130) 𝛾=35 27·𝛼3 (131) 𝐵𝜙=0.9996·𝑐·(𝜑−𝐽2+𝛽·𝐽4−𝛾·𝐽6) (132)
120 Pol Capella Roca Càlcul final de coordenades Un cop disposem de tots els paràmetres necessaris, ja es pot procedir al càlcul de les coordenades UTM finals: 𝑋=𝜉· 𝜈·(1+𝜁3)+500000 (133) 𝑌= 𝜂· 𝜈·(1+𝜁)+𝐵𝜙 (134) En el cas de les coordenades X, se suma la quantitat de 500000 metres per tal de desplaçar el meridià central, i evitar així valors de longituds negatives. Es mou el meridià de referència 500 km perquè aquest és el valor que prèn en tots els husos de les coordenades UTM, que van dels 0 metres en la coordenada situada més a l’oest a 1000000 metres en la coordenada més oriental. En la Figura 56 s’en pot veure una explicació gràfica. En el cas de les coordenades Y se suma un quantitat de 10000000 de metres, només en el cas de que les coordenades estiguin situades en l’hemisferi Sur, pel mateix motiu que en el cas de la longitud. Figura 56: situació del meridià central per a l’hus 30 [19]
127 Pol Capella Roca Acceleració lineal Figura 63: validació experimental de l’estimació de l’acceleració lineal en repòs. Com es pot observar en les estimacions de les acceleracions (que haurien de tenir un valor de 0) s’obté un bias més o menys constant, sobretot apreciable en l’eix x i l’eix y. Això és degut a la mala estimació de l’actitud, que afecta al model de mesura de l’acceleròmetre.
128 Pol Capella Roca Velocitat lineal Figura 64: validació experimental de la velocitat lineal en repòs Els resultats en aquesta variable són molt semblants als de l’acceleració i s’expliquen pels mateixos motius que aquesta.
129 Pol Capella Roca Posició Figura 65: validació experimental de l’estimació de la posició en repòs Per al cas de la posició s’obtenen unes estimacions pràcticament iguals a les senyals del sensor de posició. Malgrat que en el nostre model s’usa l’actitud per a estimar la posició, l’algorítme del filtre de Kalman estès otorga molt poca fiabilitat a aquestes estimacions, i és per això que els resultats són tant semblants a la senyal d’esntrada. 8.5.3.2.Simulació de vol Per a intentar estimar els estats durant un teòric vol real, s’ha realitzat una experimentació en la que es movia la plataforma quadrotor/rover agafant-la amb les mans, simulant situacions que es donarien en un vol normal. En les gràfiques s’observa com entre els 130 i 150 segons s’obtenen alguns valors desproporcionats, inclús per sobre dels límits calculats en 8.3.2.1. Això fa pensar que durant aquest període es pot haver perdut la comunicació amb la plataforma, per tant, aquestes dades no s’han de tenir en compte.
130 Pol Capella Roca Pel que fa al camp magnètic una primera revisió del seu comportament torna a mostrar la influència de l’MB100 (Figura 66). En la construcció del model s’ha suposat un valor de camp magnètic (mòdul) aproximadament constant i, durant aquesta simulació, apareixen variacions de fins aproximadament ±10% del valor de camp magnètic terrestre calculat en 8.3.3.2. Això torna a generar unes fortes distorsions en l’estimació de l’actitud i, com a conseqüència, en la resta de variables que guarden relació amb ella. Figura 66: mòdul de camp magnètic mesurat durant la simulació de vol, comparat amb el mòdul de camp magnètic terrestre En aquest cas és difícil comentar i mesurar fins a quin punt el comportament de l’estimador és fiable, ja que no es disposa de valors de referència per a totes les variables, com era el cas d’una situació de repòs. El que si que sembla clar és que el comportament de l’angle de guinyada és força inestable, amb pics bastant pronunciats en algun instant de temps que no tenen cap sentit (Figura 67).
131 Pol Capella Roca Figura 67: validació experimental de l’estimació de l’actitud en vol La velocitat lineal i l’acceleració es mouen dins d’uns valors llògics, però es fa difícil evaluar fins a quin punt són correctes, ja que no es disposa de cap referència. El que sí que es pot saber és que el seu valor, com es veu en la simulació, hauria d’estar al voltant de zero. Figura 68: validació experimental de la velocitat lineal en vol
132 Pol Capella Roca Figura 69: validació experimental de l’estimació de l’acceleració lineal Pel que fa als estats dels quals sí que en tenim mesures directes (velocitat angular i posició), les estimacions tornen a ser bastant fidels a les mesures, mostrant la robustesa de l’algorítme d’estimació de Kalman. Figura 70: validació experimental de l’estimació de la velocitat angular
133 Pol Capella Roca Figura 71: validació experimental de l’estimació de la posició
134 Pol Capella Roca 9.Impacte ambiental D’entrada, degut a la naturalesa teòrica d’aquest treball, es tracta d’un estudi, es pot pensar que l’impacte ambiental del projecte és nul; es tracta d’un projecte realitzat pràcticament en la seva totalitat amb simulacions fetes a través d’ordinadors i amb una validació experimental feta amb un vehicle 100% elèctric. A més el consum energètic d’ambdós components (ordinador i quadrotor) és pràcticament imperceptible. Però si un amplia més el seu camp de visió i és capaç de veure-hi més enllà s’adonarà que es tracta d’un estudi realitzat sobre un tipus de vehicles que reporten grans beneficis sobre el medi ambient. Es tracta de vehicles no tripulats, amb la qual cosa el seu pes i consum de combustible és més reduït que el de vehicles aeris convencionals, i que a més s’usen en una gran quantitat de tasques destinades a la protecció i conservació de la flora i la fauna. A més, la integració d’un dispositiu de posicionament global com l’RTK MB100, constitueix una millora que possibilitarà que el quadrotor sigui vàlid per a treballar en un camp de missions molt més extens.
135 Pol Capella Roca 10.Resum del pressupost Adjunt a aquesta memòria es pot trobar un document que recull els aspectes econòmics relacionats amb aquest estudi. Per al càlcul del pressupost necessari s’han tingut en compte tres factors fonamentals: Software Hardware Hores de treball Tenint en compte aquests factors s’ha arribat a una estimació del pressupost necessari de 30.519 €.
136 Pol Capella Roca 11.Conclusions El desenvolupament d’aquest estudi ha permès a l’estudiant adquirir una gran quantitat de coneixements nous relacionats amb la indústria del control automàtic i, especialment, amb el procés de fusió de dades. Durant tot el procés d’elaboració del projecte ha estat necessària una tasca d’investigació i recerca constant, ja no tan sols de tècniques de fusió de dades diverses, sinó també de conceptes de control més o menys bàsics, que han finalitzat en el disseny del que s’ha pensat que seria el millor estimador possible per al nostre cas. Les principals dificultats trobades durant la realització de l’estudi són les que es deriven del treball amb conceptes completament desconeguts fins aleshores i del treball experimental amb un plataforma real, l’AscTec Hummingbird. A això cal sumar-hi també Després de l’estudi realitzat sobre els observadors d’estat, s’ha escollit el filtre de Kalman estès com a l’estimador més òptim per al nostre procés. Tot i que requereix d’un algoritme de càlcul més complex que el dels altres estimadors estudiats en l’apartat 7, els avantatges que ens reportava en un procés no lineal i contaminat per soroll superaven amb escreix les dificultats proposades per l’algoritme de càlcul. Malgrat el disseny del modelat del quadrotor no entrava dins dels objectius marcats en aquest estudi, s’han estudiat diversos tipus de modelats que apareixien en diferents articles i estudis relacionats amb la fusió de dades, per a poder escollir un model que ens permetés fer una bona estimació i que no reportés grans dificultats a l’hora de linealitzar-lo i incorporar-lo a l’algoritme d’estimació. Finalment s’ha optat per un model cinemàtic basat en els processos de Wienner, ja que s’ha pensat que és el que millor podia complir els requisits anteriors. Degut a la complexitat que porta associada aquest projecte, no s’ha aconseguit embarcar l’algoritme d’estimació a l’aeronau. Malgrat tot s’ha aconseguit simular la fusió de dades en temps real a través d’un ordinador extern, que ha permès validar l’estimador i determinar-ne els punts febles. La validació experimental s’ha dut a terme en unes condicions que no serien les òptimes, ja que s’ha hagut d’afegir una bateria extra i usar una plataforma externa per a subjectar-la. Això ha generat una distorsió en el camp magnètic al voltant del quadrotor que ha provocat que els resultats en l’estimació d’algunes variables d’estat (la guinyada especialment) no fóssin els esperats. Per a solucionar-ho es
143 Pol Capella Roca [40] “Custom - AscTec Research - Ascending Technologies Customer Wiki.” [Online]. Available: http://wiki.asctec.de/display/AR/Custom. [Accessed: 16Jun-2015]. [41] “GLONASS - Wikipedia, la enciclopedia libre.” [Online]. Available: https://es.wikipedia.org/wiki/GLONASS. [Accessed: 15-Jul-2015].