proyecto fin de carrera - universidad de...

Equation Chapter 1 Section 1

Proyecto Fin de Carrera

Ingeniero Aeronáutico

MODELADO, SIMULACIÓN Y CONTROL DE

UN VEHÍCULO HEXAROTOR

Autor: Manuel Burgers Oñate

Tutor: Manuel Vargas Villanueva

Dep. Ingeniería de Sistemas y Automática

Escuela Técnica Superior de Ingeniería

Universidad de Sevilla

Sevilla, 2015

Proyecto Fin de Carrera

Ingeniero Aeronáutico

MODELADO, SIMULACIÓN Y CONTROL DE

UN VEHÍCULO HEXAROTOR

Autor:

Manuel Burgers Oñate

Tutor:

Manuel Vargas Villanueva

Profesor titular

Dep. de Ingeniería de Sistemas y Automática

Escuela Técnica Superior de Ingeniería

Universidad de Sevilla

Sevilla, 2015

Proyecto Fin de Carrera: MODELADO, SIMULACIÓN Y CONTROL DE UN VEHÍCULO

HEXAROTOR

Autor: Manuel Burgers Oñate

Tutor: Manuel Vargas Villanueva

El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes

miembros:

Presidente:

Vocales:

Secretario:

Acuerdan otorgarle la calificación de:

Sevilla, 2015

El Secretario del Tribunal

vii

Resumen

El proyecto aquí expuesto es de carácter experimental y consiste en la puesta en marcha de un helicóptero con seis rotores, también conocido como hexarotor.

Para la puesta en marcha ha sido necesario familiarizarse con el controlador de vuelo PX4 Autopilot y con el proyecto Ardupilot, para así poder programarlo y editar el código según fuese necesario. Para ello se ha usado un editor de texto, la cadena de herramientas del PX4 para programar el controlador y el programa Mission Planner para editar los parámetros de vuelo.

Además ha sido necesario configurar el sensor de flujo óptico, conectándolo con el controlador y configurándolo para su correcto funcionamiento.

Una vez habituado al controlador de vuelo y su programación, se ha realizado un modelo dinámico. Para ello se ha hecho uso de las leyes de Newton, y posteriormente se han hecho experimentos tanto en un banco de pruebas en el interior del laboratorio, como en el exterior sin ninguna restricción en su movimiento. Estos experimentos son necesarios para la identificación de los parámetros del sistema y su posterior simulación en MATLAB Simulink.

El control del hexarotor está estructurado en tres niveles anidados: estabilización, altura y traslación. Estos controladores son de tipo PID, y ha sido necesaria una sintonización para su correcto funcionamiento.

ix

Índice

Resumen............................................................................................................................................................................... vii

Índice ..................................................................................................................................................................................... ix

ÍNDICE DE TABLAS ...................................................................................................................................................................... xi

ÍNDICE DE FIGURAS ................................................................................................................................................................... xiii

Notación ............................................................................................................................................................................. xvii

1 Equipo Físico ..................................................................................................................................................................1 Componentes primarios .........................................................................................................................................1 Equipo de computación y control: PX4 Autopilot ...................................................................................................7 1.1.1 PX4FMU (PX4 Flight Management Unit) ...............................................................................................7 1.1.2 PX4IO (PX4 Input/Output module) ..................................................................................................... 10 Sensores .............................................................................................................................................................. 12

2 Modelado Del Sistema ............................................................................................................................................... 13 2.1 Modelo dinámico ................................................................................................................................................ 13

2.1.1 Sistema de referencia ................................................................................................................................. 13 2.1.2 Matrices de rotación ................................................................................................................................... 14 2.1.3 Ecuaciones de movimiento ......................................................................................................................... 16

2.2 Ecuaciones de estado .......................................................................................................................................... 20 2.3 Identificación ....................................................................................................................................................... 21

2.3.1 Inercia respecto al eje-y .............................................................................................................................. 21 2.3.2 Inercia respecto al eje-z .............................................................................................................................. 26 2.3.3 Masa y Ku .................................................................................................................................................... 26 2.3.4 Parámetro 𝑲𝑿𝒀 ......................................................................................................................................... 28

3 Software y Puesta en Marcha .................................................................................................................................... 33 3.1 Software .............................................................................................................................................................. 33

3.1.1 Mission Planner .......................................................................................................................................... 33 3.1.2 QGroundControl ......................................................................................................................................... 33 3.1.3 PX4 Toolchain ............................................................................................................................................. 34

3.2 Compilación......................................................................................................................................................... 35 3.3 Sensor de flujo óptico (Optical Flow) ................................................................................................................... 37 3.4 ArduCopter .......................................................................................................................................................... 39

4 Control De Estabilización ............................................................................................................................................ 40 4.1 Controlador en cascada para pitch y roll ............................................................................................................ 40

4.1.1 Ziegler-Nichols ............................................................................................................................................ 40 4.1.2 Sintonización del controlador ..................................................................................................................... 42 4.1.3 Pruebas de vuelo libre y resintonización .................................................................................................... 45

4.2 Controlador en cascada para yaw ...................................................................................................................... 50

5 Control De Altura ........................................................................................................................................................ 51 5.1 Controlador en cascada ...................................................................................................................................... 51 5.2 Experimento ........................................................................................................................................................ 52

6 Control De Traslación ................................................................................................................................................. 55 6.1 Cálculo de la velocidad lineal .............................................................................................................................. 55 6.2 Controlador ......................................................................................................................................................... 58 6.3 Experimentos ...................................................................................................................................................... 59

Anexo A. Códigos ................................................................................................................................................................63

Referencias ...........................................................................................................................................................................71

xi

ÍNDICE DE TABLAS

Tabla 1.1 Características del motor 2

Tabla 1.2 Características del variador 3

Tabla 1.3 Características de la batería 4

Tabla 2.1 Resultados experimentarles de 𝐼𝑦𝑦/𝑘𝜏 𝑟𝑎𝑑 𝑠2 𝑢. 𝑎. 25

Tabla 2.2 Masa del sistema 27

Tabla 2.3 Parámetros identificados del sistema 31

Tabla 4.1 Parámetros según Ziegler-Nichols 41

Tabla 4.2 Parámetros del controlador PID 41

Tabla 4.3 Parámetros PID tras la primera sintonización 42

Tabla 4.4 Parámetros del controlador definitivos 46

Tabla 4.5 Parámetros del controlador en yaw 50

Tabla 5.1 Parámetros del controlador iniciales y finales (sintonizados). 52

Tabla 6.1 Parámetros del controlador de traslación 59

Tabla 6.2 Valores finales del control de traslación 61

xiii

ÍNDICE DE FIGURAS

Figura 1.1 Sistema Real 1

Figura 1.2 Frame 2

Figura 1.3 T-MOTOR MN4010 2

Figura 1.4 Hélices CW y CCW 3

Figura 1.5 ESC T-MOTOR T30A 3

Figura 1.6 Placa distribuidora de potencia 4

Figura 1.7 Batería Turnigy nano-tech 6000 mAh 5

Figura 1.8 Variador Turnigy 5

Figura 1.9 Emisora Turnigy 9XR 6

Figura 1.10 Módulo FrSky DJT 6

Figura 1.11 Receptor FrSky D8R-II Plus 6

Figura 1.12 PX4FMU Parte inferior. 7

Figura 1.13 PX4FMU Parte superior 8

Figura 1.14 Distribución de los conectores en la placa PX4FMU 9

Figura 1.15 PX4IO Parte superior 10

Figura 1.16 PX4IO Parte inferior 10

Figura 1.17 . Distribución de los conectores en la placa PX4IO 11

Figura 1.18 GPS 3DR uBlox 12

Figura 1.19 PX4Flow con sonar HRLV-EZ4 12

Figura 2.1 Sistema de referencia 13

Figura 2.2 Giro alrededor del eje-x 14

Figura 2.3 Giro alrededor del eje-y 14

Figura 2.4 Giro alrededor del eje-z 15

Figura 2.5 Par de roll 19

Figura 2.6 Par de pitch 19

Figura 2.7 Modelo hexa-rotor 22

Figura 2.8 Sistema en bucle cerrado 23

Figura 2.9 Banco de pruebas 24

Figura 2.10 Experimento sobre el ángulo pitch 25

Figura 2.11 Modelo Simulink 26

Figura 2.12 Experimento para hallar el valor de la actuación en hovering 28

Figura 2.13 Sistema a identificar para el control de posicion. 29

Figura 2.14 Resultados del experimento realizado para el cálculo de Kxy 30

Figura 3.1 Mission Planner 33

Figura 3.2 QGroundControl 34

Figura 3.3 Línea de comandos del PX4 Toolchain 34

Figura 3.4 Repositorio del código del ArduCopter 35

Figura 3.5 PX4 Console 37

Figura 3.6 Actualización del firmware a través de QGroundControl 37

Figura 3.7 Dispositivos instalados en el PX4 38

Figura 3.8 Distintas configuraciones de multirotor 39

Figura 4.1 Control en cascada 40

Figura 4.2 Pitch con oscilaciones mantenidas 41

Figura 4.3 Comparación del ángulo pitch entre sistema real y simulado 42

Figura 4.4 Comparación del ángulo pitch entre sistema real sintonizado y simulado 43

Figura 4.5 Modelo Simulink del sistema 43

Figura 4.6 Justificación del uso de un retardo en Simulink. Representación en fase 44

Figura 4.7 Justificación del uso de un retardo en Simulink. Representación en fase 44

Figura 4.8 Primera prueba sin restricciones. Ángulo pitch 45

Figura 4.9 Primera prueba sin restricciones. Ángulo roll 45

Figura 4.10 Empuje en banco y en vuelo libre 46

Figura 4.11 Prueba sobre el ángulo pitch con el controlador definitivo 47

Figura 4.12 Simulación realista con parámetros del controlador definitivos 47

Figura 4.13 Prueba de vuelo libre final. Ángulo pitch 48

Figura 4.14 Prueba de vuelo libre final. Ángulo roll 48

Figura 4.15 Saturación 49

Figura 4.16 Prueba de vuelo libre final con simulación. Ángulo pitch 49

Figura 4.17 Prueba de vuelo para el ángulo yaw 50

Figura 5.1 Control de altura del hexa-rotor 51

Figura 5.2 Modelo Simulink del control de altura 51

Figura 5.3 Comparación entre sintonización inicial (rojo) y final (verde). 52

Figura 5.4 Resultados en altura del control de altura. 53

Figura 5.5 Resultados en velocidad del control de altura. 53

Figura 5.6 Resultados en aceleración del control de altura. 54

Figura 5.7 Escalón aplicado en el control de altura 54

Figura 6.1 Esquema de un punto P y su proyección p en el plano imagen 55

Figura 6.2 Fotograma en el instante t 57

xv

Figura 6.3 Fotograma en el instante t + dt 57

Figura 6.4 Dirección del movimiento 58

Figura 6.5 Modelo Simulink del control de traslación 58

Figura 6.6 Respuesta del modelo de control de traslación ante un escalón 59

Figura 6.7 Modelo del control de traslación 60

Figura 6.8 Experimento para el control de posición 60

Figura 6.9 Representación del plano XY durante el experimento en posición 61

Figura 6.10 Actuaciones del control de traslación 62

Figura 6.11 Comparación del sistema real y el modelo simulado 62

xvii

Notación

PID Proporcional-Integral-Derivativo

i.e. Es decir

sin Función seno

cos

sec

𝑐𝑜

IMU

u.a.

Función coseno

Función secante

Centésimas de grado

Unidad de Medidas Inerciales

Unidades de actuación

1

1 EQUIPO FÍSICO

En este capítulo se hará una descripción del Firmware y Hardware de control y de los sensores empleados. Además se explicará de forma breve el Software usado. Como Firmware se ha utilizado el ArduCopter, de código abierto; mientras que para el Hardware se emplea el PX4 Autopilot.

Componentes primarios

El sistema real usado en este proyecto es un hexarotor, mostrado en la Figura 1.1. En ella se observa que dos de los brazos son de color rojo, indicando la dirección positiva del eje-x. Además se observan unas pequeñas bandas de color beige cuyo propósito es proteger los brazos ante las pruebas realizadas en el banco que se muestra en la Figura 2.9.

Figura 1.1 Sistema Real

Dicho sistema consta de varias partes que se describen a continuación, excepto por los sensores que serán descritos más adelante.

Frame

Se trata de la estructura del sistema. Está formada por seis brazos, dos patas de apoyo y varias placas centrales. En el extremo de los brazos se colocan los motores con sus respectivas hélices, mientras que las placas centrales sirven de apoyo para el controlador de vuelo y los sensores.

Equipo Físico

2

Figura 1.2 Frame

Motores

Los motores empleados son de la marca T-MOTOR de la serie Navigation, más concretamente el modelo MN4010-11 de 475 kV mostrado en la siguiente figura,

Figura 1.3 T-MOTOR MN4010

A continuación se muestra una tabla con sus principales características obtenidas de la página web del fabricante,

Tabla 1.1 Características del motor

Diámetro exterior 44.5 mm

Longitud 30.5 mm

Peso 110 g

Celdas (alimentación) Entre 4 y 8

Corriente máxima 30 A

Corriente que proporciona eficiencia máxima Entre 4 y 11 A

Eficiencia máxima 82%

Hélices

3

El hexarotor usa unas hélices de 15 pulgadas (38.1 cm) y un paso de 5.5 grados. Tres de ellas de giro horario y otras tres de giro anti-horario.

Figura 1.4 Hélices CW y CCW

Variadores (ESC)

El hexarotor emplea variadores de la marca T-Motor, al igual que los motores, como el de la siguiente imagen,

Figura 1.5 ESC T-MOTOR T30A

Estos variadores (electronic speed controller) van conectados a los motores y permiten controlar las revoluciones por minuto a las que giran. El piloto envía las señales a los variadores, y estos se encargan de transformar estas señales de modo que los motores puedan girar a la velocidad deseada por el piloto. Cada variador va conectado a la batería para su alimentación, y además va conectado al receptor que obtiene la señal del piloto.

En la siguiente tabla se muestran sus características principales,

Tabla 1.2 Características del variador

Corriente máxima 30 A

Equipo Físico

4

Celdas (alimentación) Entre 2 y 6

Tamaño 45 x24 x11 mm

Peso 11g

Placa de distribución de potencia

Su función es permitir conectar varios variadores a una misma batería. Esta en particular tiene unas medidas de 55x55mm y pesa 6.7g.

Figura 1.6 Placa distribuidora de potencia

Batería

Se usan baterías Turnigy nano-tech 6000mAh cuyas características se muestran en la siguiente tabla,

Tabla 1.3 Características de la batería

Capacidad 6000 mAh

Tensión nominal 14.8 V (4 celdas)

Tasa de descarga constante 25C

Tasa de descarga máxima 50C

Dimensiones 175x49x38 mm

Peso 579 g

Hay que prestar atención al mantenimiento de las baterías. Es importante no dejarlas cargadas al 100% si no se van a usar durante un cierto periodo de tiempo, ya que esto las deteriora.

5

Figura 1.7 Batería Turnigy nano-tech 6000 mAh

Regulador

Se usa un regulador de la marca Turnigy, más concretamente el Turnigy HV SBEC 5A Switch Regulator que proporciona una salida de, o bien 5V o bien 6V elegible por el usuario.

Tiene un peso de 21g y unas dimensiones de 59x10x25 mm, y funciona con una alimentación de entre 8 y 42 V (entre 2 y 10 celdas).

La razón de usar un regulador es para poder conectar el emisor del que se hablará a continuación, ya que dicho emisor funciona entre los 3.5 y 10V.

Figura 1.8 Variador Turnigy

Emisora y receptor

Se usa la emisora Turnigy 9XR. Dispone de nueve canales y se alimenta con baterias lipo de 2 y 3s celdas. Usa modulación PPM (modulación por posición de pulsos), por lo que sólo es necesario un cable en el receptor y no un cable por cada canal. La emisora permite controlar el empuje, roll, pitch, yaw y modo de vuelo entre otras cosas. El hecho de usar una emisora se debe a la necesidad de hacer las pruebas para la identificación de los parámetros y la sintonización de los controladores.

Equipo Físico

6

Figura 1.9 Emisora Turnigy 9XR

Con esta emisora se usa el módulo FrSky DJT.

Figura 1.10 Módulo FrSky DJT

Como receptor se usa también la marca FrSky, en este caso FrSky D8R-II Plus. Trabaja en la banda de frecuencia de 2.4GHz y la alimentación va conectada al regulador mencionado anteriormente.

Figura 1.11 Receptor FrSky D8R-II Plus

7

Equipo de computación y control: PX4 Autopilot

El PX4 es un sistema de autopiloto, es decir, el ordenador de a bordo de un avión o multirotor. Grosso modo está compuesto de los siguientes componentes:

- Procesador: se encarga de realizar los cálculos para poder llevar a cabo un vuelo estable

- Giróscopo: permite tomar medidas de velocidades angulares

- Acelerómetro: como su nombre indica, proporciona medidas de aceleración

- Magnetómetro: se trata básicamente de una brújula

- Barómetro: permite medir la presión a la altura de vuelo en la que se encuentre el multirotor

- GPS: proporciona las coordenadas en la que se encuentra el multirotor

El PX4 ejecuta un RTOS (Real-Time Operating System) llamado NuttX. El hardware siempre ejecuta el NuttX, lo que varía es el firmware empleado. Los dos más usados son el propio proporcionado por PX4, y el proyecto ArduPilot. En este caso se emplea el ArduCopter, que se trata del ArduPilot destinado a multirotores.

El middleware del PX4 se ejecuta en primer lugar en el sistema operativo (NuttX) proporcionando drivers para los distintos dispositivos y un ORB (object request broker) que permite realizar comunicaciones asíncronas entre las distintas tareas ejecutándose en el PX4. Sobre este middleware se ejecuta el proyecto ArduPilot del que se hablará en el apartado 3.4 .

El hardware empleado (PX4 autopilot) está compuesto de dos placas, la PX4FMU (PX4 Flight Management Unit) y la PX4IO (PX4 Input/Output), de las que se habla a continuación.

1.1.1 PX4FMU (PX4 Flight Management Unit)

Se trata de un controlador de vuelo apto para ser usado en casi cualquier plataforma robótica que requiera movimiento, entre ellas los multirotores, rovers y aviones.

Figura 1.12 PX4FMU Parte inferior.

Equipo Físico

8

Figura 1.13 PX4FMU Parte superior

Sus características son las siguientes:

- Procesador de 168 Mhz de ARM (196 KB RAM, 1 MB)

- Sensores de presión (MS5611), magnetómetro (HMC5883L), acelerómetro (MPU-6000) y giróscopo (L3GD20).

- I2C, 3x UART, PPM, analógico, GPS, 2x 5V GPIO, 4x PWM/Servo

- Ranura para tarjeta MicroSD

- Bus de expansión

- Puerto USB y bootloader

- 50 x 36 x 6 mm (1.38x1.97x0.24“), 8g

- Elegibles 3.3V o 5V IO para los puertos UART2 y GPS

En la siguiente imagen se muestran los conectores disponibles en la placa,

9

Figura 1.14 Distribución de los conectores en la placa PX4FMU

Equipo Físico

10

1.1.2 PX4IO (PX4 Input/Output module)

Se trata del módulo de entradas y salidas del PX4. En las siguientes figuras se puede ver su diseño,

Figura 1.15 PX4IO Parte superior

Figura 1.16 PX4IO Parte inferior

Sus características principales son,

- Procesador de 24 Mhz

- 8x Salida para servos (hasta 400 Hz)

- Receptor PPM RC, entrada Spektrum, S.Bus

- Bus de expansión

- 50x36x14 mm, 20g

A continuación se muestran los conectores,

11

Figura 1.17 . Distribución de los conectores en la placa PX4IO

Equipo Físico

12

Sensores

Se lleva a cabo una descripción de los sensores externos empleados en el hexarotor: GPS, compass, sensor de flujo óptico y sonar. El resto de los sensores (barómetro, compass, acelerómetro, giróscopo) se encuentran en el mismo controlador de vuelo PX4.

- GPS 3DR uBlox con Compass

Figura 1.18 GPS 3DR uBlox

Sus medidas son 38x38x8.5 mm, su peso 16.8 gramos; y posee una tasa de refresco de 5 Hz. El GPS proporciona las coordenadas del hexarotor, mientras que el compass (brújula) facilita la dirección que se está encarando.

- Sensor de flujo óptico (PX4FLOW) con el sonar HRLV-EZ4

Tiene una resolución de 752x480 píxeles y procesa las imágenes a 400 Hz. Además se le puede añadir un sonar, ya que viene con los pines necesarios para conectarlo. El sonar empleado es el HRLV-EZ4 de Maxbotix, quedando el montaje como se muestra en la siguiente figura,

Figura 1.19 PX4Flow con sonar HRLV-EZ4

13

2 MODELADO DEL SISTEMA

En este apartado se hará una descripción del sistema empleado. Dicha descripción constará de dos partes: modelo físico e identificación de los parámetros necesarios para la simulación.

En la parte del modelo se obtendrán las ecuaciones del movimiento de dicho sistema, que posteriormente se emplearán para la identificación.

2.1 Modelo dinámico

A continuación se desarrollará el modelo dinámico del hexarotor, obteniendo las ecuaciones del movimiento. Para ello hay que definir los sistemas de referencia a usar y las matrices de rotación.

2.1.1 Sistema de referencia

Primeramente se definen los sistemas de referencia empleados para desarrollar las ecuaciones.

- Sistema de ejes {B} (Body coordinate frame): se trata de un sistema ligado al hexarotor, situado en el centro de masas. Los giros alrededor de los ejes XB, YB y ZB reciben el nombre de Roll, Pitch y Yaw respectivamente, o si se usa la notación en castellano son alabeo, cabeceo y guiñada respectivamente.

- Sistema de ejes {W} (World coordinate frame): se trata de un sistema de referencia inercial.

Figura 2.1 Sistema de referencia

Modelado Del Sistema

14

2.1.2 Matrices de rotación

Los ángulos empleados son los llamados ángulos de Tait-Brian, también conocidos como ángulos de Cardan debido al físico y matemático italiano Gerolamo Cardano. La matriz de rotación de {B} sobre {W} expresada en el sistema {W} la denominaremos 𝐵

𝑤𝑅. A continuación se muestran las matrices de rotación alrededor de los distintos ejes:

1- Rotación 𝜙 alrededor de x obteniendo la matriz 𝑅(𝑥, 𝜙).

Figura 2.2 Giro alrededor del eje-x

(

𝑥𝑦𝑧) = (

1 0 00 cos𝜙 −sin𝜙0 sin𝜙 cos 𝜙

)(

𝑥′𝑦′

𝑧′) ( 2.1 )

2- Rotación 𝜃 alrededor de y’ obteniendo la matriz 𝑅(𝑦′, 𝜃).

Figura 2.3 Giro alrededor del eje-y

(

𝑥′𝑦′

𝑧′) = (

cos 𝜃 0 sin 𝜃0 1 0

− sin𝜃 0 cos 𝜃)(

𝑥′′𝑦′′

𝑧′′) ( 2.2 )

15

3- Rotación 𝜓 alrededor de z’’ obteniendo la matriz 𝑅(𝑧′′, 𝜓).

Figura 2.4 Giro alrededor del eje-z

(

𝑥′′𝑦′′

𝑧′′) = (

cos 𝜓 −sin𝜓 0sin𝜓 cos𝜓 00 0 1

)(

𝑥′′′𝑦′′′

𝑧′′′) ( 2.3 )

Por lo tanto, si queremos obtener dicha matriz de rotación sólo basta con multiplicar las matrices de rotación anteriores en orden inverso, obteniendo la siguiente matriz,

𝑅𝐵𝑤 = 𝑅(𝑧′′, 𝜓)𝑅(𝑦′, 𝜃)𝑅(𝑥, 𝜙) ( 2.4 )

Y sustituyendo con las matrices anteriores se obtiene,

𝑅𝐵

𝑤 = (cos𝜓 cos 𝜃 cos𝜓 sin𝜙 sin 𝜃 − cos𝜙 sin 𝜓 sin 𝜙 sin𝜓 + cos𝜙 cos𝜓 sin 𝜃 cos 𝜃 sin 𝜓 cos𝜙 cos𝜓 + sin 𝜙 sin 𝜓 sin 𝜃 cos𝜙 sin 𝜓 sin 𝜃 − cos𝜓 sin 𝜙−sin 𝜃 cos 𝜃 sin𝜙 cos𝜙 cos𝜃

) ( 2.5 )

Dicha matriz será usada en la obtención de las ecuaciones del movimiento.


16

2.1.3 Ecuaciones de movimiento

A continuación se obtendrán las ecuaciones del movimiento haciendo uso de la segunda ley de Newton.

2.1.3.1 Segunda ley de Newton. Ecuación del movimiento

La segunda ley de Newton, siendo la masa constante en el tiempo, se puede escribir en el sistema inercial como,

∑ �⃗�𝑊 =

𝑑

𝑑𝑡(𝑚 �⃗�𝑊 ) ≃ 𝑚

𝑑 �⃗�𝑊

𝑑𝑡 ( 2.6 )

Siendo F cada una de las fuerzas reales actuando en el sistema (i.e., fuerzas no inerciales). En este caso se tiene en cuenta el peso del sistema y el empuje generado por los motores, que se escribe de acuerdo a la siguiente notación,

∑ �⃗�𝑊 = 𝑚 �⃗�𝑊 +∑ 𝑓𝑖⃗⃗⃗

𝑊

6

𝑖=1

= 𝑚 �⃗�𝑊 + �⃗⃗�𝑊 ( 2.7 )

Siendo fi la fuerza ejercida por cada uno de los seis motores, y Tw el empuje total del sistema. Las fuerzas aerodinámicas no serán tenidas en consideración en este modelo.

Ahora bien, pasando el empuje del sistema de referencia en ejes cuerpo {B} al sistema de referencia inercial {W} mediante la matriz de rotación vista anteriormente,

�⃗⃗�𝑊 = 𝑅𝐵𝑤 �⃗⃗�𝐵 ( 2.8 )

Siendo

�⃗⃗�𝐵 = (

00𝑇) ( 2.9 )

Por tanto se puede reescribir la ecuación del movimiento de la siguiente forma,

𝑚𝑑 �⃗�𝑊

𝑑𝑡 = 𝑚 �⃗�𝑊 + 𝑅𝐵

𝑤 �⃗⃗�𝐵 ( 2.10 )

Dividiendo por la masa del sistema,

17

𝑑 �⃗�𝑊

𝑑𝑡 = �⃗�𝑊 +

𝑅𝐵𝑤 �⃗⃗�𝐵

𝑚 ( 2.11 )

Que en formato vectorial queda,

(

�̈��̈��̈�) = (

00−𝑔

) + 𝑅𝐵𝑤 (

00𝑇𝑚

) ( 2.12 )

Finalmente, se sustituye el valor de la matriz de rotación vista con anterioridad para obtener la siguiente ecuación,

{

�̈� =

𝑇

𝑚(sin𝜙 sin𝜓 + cos𝜙 cos𝜓 sin 𝜃)

�̈� = −𝑇

𝑚(sin𝜙 cos𝜓 − cos𝜙 sin𝜓 sin 𝜃)

�̈� =𝑇

𝑚cos𝜙 cos 𝜃 − 𝑔

( 2.13 )

( 2.14 )

( 2.15 )

2.1.3.2 Segunda ley de Newton para rotación. Ecuación del movimiento

La ecuación del movimiento de rotación se puede escribir de manera análoga a la lineal. Desde el sistema inercial {W},

𝜏 =

𝑑 (𝐼 · Ω⃗⃗⃗)𝑊

𝑑𝑡 ( 2.16 )

O bien desde el sistema {B}, la ecuación queda,

𝜏 =

𝑑 (𝐼 · Ω⃗⃗⃗)𝐵

𝑑𝑡+ Ω⃗⃗⃗ ∧ (𝐼 · Ω⃗⃗⃗) ( 2.17 )

Siendo 𝜏 el par real actuando sobre el sistema, 𝐼 la matriz de inercia y Ω⃗⃗⃗𝐵 la velocidad angular en el sistema de

referencia {B}.

Por un lado, la matriz de inercia se ha supuesto diagonal debido a la simetría del sistema, quedando de la siguiente forma,


18

𝐼 = (

𝐼𝑥𝑥 0 00 𝐼𝑦𝑦 0

0 0 𝐼𝑧𝑧

) ( 2.18 )

En este caso, el par constará sólo del desarrollado por los motores, despreciando la aerodinámica del sistema,

𝜏 = (

𝜏𝜙𝜏𝜃𝜏𝜓) ( 2.19 )

Y el vector de velocidades angulares en el sistema de referencia {B} consistente de,

Ω⃗⃗⃗𝐵 = (

𝑝𝑞𝑟) ( 2.20 )

Ahora basta con sustituir dichas definiciones en la ecuación del movimiento para la rotación, obteniendo el siguiente resultado,

�̇� =1

𝐼𝑥𝑥(𝜏𝜙 − 𝑞𝑟(𝐼𝑧𝑧 − 𝐼𝑦𝑦)) ( 2.21 )

�̇� =1

𝐼𝑦𝑦(𝜏𝜃 − 𝑝𝑟(𝐼𝑧𝑧 − 𝐼𝑥𝑥)) ( 2.22 )

�̇� =1

𝐼𝑧𝑧(𝜏𝜓 − 𝑝𝑞(𝐼𝑦𝑦 − 𝐼𝑥𝑥)) ( 2.23 )

Ahora bien, el vector velocidad angular se puede relacionar con los ángulos de Euler de la siguiente forma,

𝑝 = �̇� − �̇� sin 𝜃 ( 2.24 )

𝑞 = �̇� cos 𝜙 + �̇� cos 𝜃 sin 𝜙 ( 2.25 )

𝑟 = −�̇� sin 𝜙 + �̇� cos 𝜃 cos𝜙 ( 2.26 )

O la relación inversa,

�̇� = 𝑝 + 𝑞 sin𝜙 tan𝜃 + 𝑟 cos𝜙 tan𝜃 ( 2.27 )

19

�̇� = 𝑞 cos𝜙 − 𝑟 sin𝜙 ( 2.28 )

�̇� =𝑞 sin𝜙

cos 𝜃+𝑟 cos𝜙

cos 𝜃 ( 2.29 )

Por tanto, bastaría sustituir las ecuaciones ( 2.24 ), ( 2.25 ) y ( 2.26 ) en las ecuaciones ( 2.21 ), ( 2.22 ) y ( 2.23 ) para obtener tres ecuaciones diferenciales de los ángulos de Euler.

El par de roll, i.e., el par alrededor del eje-x se puede escribir, tal y como se ve en la Figura 2.5 de la siguiente forma,

Figura 2.5 Par de roll

𝜏𝜙 = 𝑙(𝑇5 − 𝑇2 + sin(30𝑜) (𝑇6 − 𝑇1 + 𝑇4 − 𝑇3)) ( 2.30 )

Haciendo un cálculo similar para el par de pitch se obtiene,

Figura 2.6 Par de pitch


20

𝜏𝜃 = 𝑙(𝑇1 + 𝑇6 − 𝑇3 − 𝑇4) cos 30𝑜 ( 2.31 )

Ahora bien, el par de yaw se relaciona a través de un coeficiente de resistencia, kdrag, con el cuadrado de la velocidad de giro de los motores, quedando,

𝜏𝜓 = 𝐾𝑑𝑟𝑎𝑔(𝑤12 + 𝑤3

2 + 𝑤52 − 𝑤2

2 − 𝑤42 − 𝑤6

2) ( 2.32 )

Por otro lado, si relacionamos el empuje i-ésimo con la velocidad de giro i-ésima al cuadrado,

𝑇𝑖 = 𝐾𝑡ℎ𝑟𝑢𝑠𝑡𝑤𝑖2 ( 2.33 )

Entonces es fácil obtener la siguiente expresión para el par de roll y pitch,

𝜏𝜙 = 𝑙𝐾𝑡ℎ𝑟𝑢𝑠𝑡 (𝑤52 −𝑤2

2 + sin(30𝑜) (𝑤62 −𝑤1

2 + 𝑤42 − 𝑤3

2)) ( 2.34 )

𝜏𝜃 = 𝑙𝐾𝑡ℎ𝑟𝑢𝑠𝑡(𝑤12 +𝑤6

2 −𝑤32 −𝑤4

2) cos 30𝑜 ( 2.35 )

2.2 Ecuaciones de estado

En el modelo, el vector de estado esta formado por las variables,

𝑋 = [ �̇� �̇� �̇� �̇� �̇� �̇� 𝑥 �̇� 𝑦 �̇� 𝑧 �̇� ]𝑇 ( 2.36 )

Y el sistema será,

21

�̇� =

(

�̇�

�̇��̇��̇��̇��̇��̇��̈��̇��̈��̇��̈�)

= 𝑓(𝑋, 𝑈) =

(

𝑝 + 𝑞 sin 𝜙 tan 𝜃 + 𝑟 cos𝜙 tan 𝜃𝑞 cos𝜙 − 𝑟 sin 𝜙

𝑞 sin 𝜙 sec 𝜃 + 𝑟 cos𝜙 sec 𝜃1

𝐼𝑥𝑥(𝑈1 − 𝑞𝑟(𝐼𝑧𝑧 − 𝐼𝑦𝑦))

1

𝐼𝑦𝑦(𝑈2 − 𝑝𝑟(𝐼𝑧𝑧 − 𝐼𝑥𝑥))

1

𝐼𝑧𝑧(𝑈3 − 𝑝𝑞(𝐼𝑦𝑦 − 𝐼𝑥𝑥))

�̇�𝑈4𝑚(sin𝜙 sin 𝜓 + cos𝜙 cos𝜓 sin 𝜃)

�̇�

−𝑈4𝑚(sin𝜙 cos𝜓 − cos𝜙 sin𝜓 sin 𝜃)

�̇�𝑈4𝑚cos𝜙 cos 𝜃 − 𝑔 )

( 2.37 )

En el que 𝑈 representa el vector de entrada y se puede escribir como función de las velocidades angulares de los rotores,

𝑈 = (

𝑈1𝑈2𝑈3𝑈4

) = (

𝜏𝜙/𝑙

𝜏𝜃/𝑙𝜏𝜓𝑇

) =

(

𝐾𝑡ℎ𝑟𝑢𝑠𝑡 (𝑤52 −𝑤2

2 + sin(30𝑜) (𝑤62 −𝑤1

2 +𝑤42 −𝑤3

2))

𝐾𝑡ℎ𝑟𝑢𝑠𝑡(𝑤12 +𝑤6

2 −𝑤32 −𝑤4

2) cos 30𝑜

𝐾𝑑𝑟𝑎𝑔(𝑤12 +𝑤3

2 +𝑤52 −𝑤2

2 −𝑤42 −𝑤6

2)

𝐾𝑡ℎ𝑟𝑢𝑠𝑡(𝑤12 + 𝑤2

2 +𝑤32 +𝑤4

2 +𝑤52 +𝑤6

2) )

( 2.38 )

A esto se le conoce como control mixer.

2.3 Identificación

En este capítulo se explicará cómo se ha llevado a cabo la identificación del hexarotor en términos de inercia, masa y coeficientes 𝐾𝑢 y 𝐾𝑋𝑌.

2.3.1 Inercia respecto al eje-y

2.3.1.1 Simetría

A continuación se demuestra por qué se pueden suponer ambas inercias iguales, Ixx e Iyy. Suponiendo una simetría total en el hexarotor, y haciendo un modelo simplificado de masas puntuales en los extremos,


22

Figura 2.7 Modelo hexarotor

Las inercias quedarían, siendo m la masa de cada motor,

𝐼𝑦𝑦 ≃ 4𝑚𝑙2 cos2 30𝑜 = 3𝑚𝑙2 ( 2.39 )

𝐼𝑥𝑥 ≃ 4𝑚𝑙2 sin2 30𝑜 + 2𝑚𝑙2 = 3𝑚𝑙2 ( 2.40 )

Por lo tanto se pueden considerar ambas inercias iguales y los experimentos para el cálculo de dichas inercias se harán solo alrededor del eje Y. El hecho de hacer las pruebas alrededor del eje Y se debe al banco de pruebas disponible en el laboratorio, que se mostrará en la Figura 4.3.

2.3.1.2 Identificación del parámetro de inercia

Como se había visto en el capítulo 2.1.3.2 , si sustituimos las ecuaciones ( 2.24 ), ( 2.25 ) y ( 2.26 ) en la ecuación (2.21) y simplificamos asumiendo pequeños ángulos, se obtiene,

�̈� =1

𝐼𝑦𝑦𝜏𝜃 ( 2.41 )

Siendo 𝑢 la señal de actuación, se puede escribir,

�̈� =𝑘𝜏𝐼𝑦𝑦

𝑢 ( 2.42 )

23

Siendo 𝑘𝜏 el parámetro que relaciona la actuación con el par de pitch.

Haciendo la transformada de Laplace a dicha ecuación, se puede escribir,

𝜃(𝑠) =𝑘𝜏𝐼𝑦𝑦𝑠

2 𝑢(𝑠) ( 2.43 )

Por lo tanto al hablar de identificación, estamos hablando de obtener el valor 𝑘𝜏/𝐼𝑦𝑦.

Se ha hecho la identificación sobre el sistema en bucle cerrado con dos proporcionales en cascada,

Figura 2.8 Sistema en bucle cerrado

Se trata de un sistema subamortiguado, por lo que se verá la sobreoscilación y el tiempo de pico. La función de transferencia en bucle cerrado del sistema anterior es la siguiente,

𝐺𝐵𝐶(𝑠) =𝐾𝑝𝐾𝑣

𝐼𝑦𝑦𝑘𝜏𝑠2 + 𝐾𝑣𝑠 + 𝐾𝑣𝐾𝑝

( 2.44 )

Que se puede relacionar con un sistema de segundo orden de la siguiente forma,

𝑆(𝑠) =

𝐾𝑤𝑛2

𝑠2 + 2𝜉𝑤𝑛𝑠 + 𝑤𝑛2 ( 2.45 )

Dónde la sobreoscilación y el tiempo de pico se relacionan con la frecuencia de la siguiente forma,

𝑆𝑜 =𝑦𝑚𝑎𝑥 − 𝑦∞𝑦𝑚𝑖𝑛 − 𝑦∞

, 𝑡𝑝 =𝜋

𝑤𝑑 ( 2.46 )

Además el amortiguamiento y la frecuencia natural tienen la siguiente expresión,


24

𝜉 =log (𝑆𝑜)

√𝜋2 + log2(𝑆𝑜), 𝑤𝑛 =

𝑤𝑑

√1 − 𝜉2 ( 2.47 )

Comparando ambas ecuaciones se obtienen dos ecuaciones para el cálculo de la inercia,

2𝜉𝑤𝑛 =𝐾𝑣

𝐼𝑦𝑦/𝑘𝜏⇒𝐼𝑦𝑦

𝑘𝜏=

𝐾𝑣2𝜉𝑤𝑛

( 2.48 )

𝑤𝑛2 =

𝐾𝑣𝐾𝑝

𝐼𝑦𝑦/𝑘𝜏⇒𝐼𝑦𝑦

𝑘𝜏=𝐾𝑣𝐾𝑝

𝑤𝑛2 ( 2.49 )

El experimento realizado para identificar el hexarotor consiste en aplicar un escalón de 20o en la inclinación. Con ello se mide la sobreoscilación y el tiempo de pico ante dicho escalón, y empleando las fórmulas obtenidas para la inercia se puede calcular su valor aproximado.

A continuación se muestra una imagen del banco de pruebas,

Figura 2.9 Banco de pruebas

Dicho experimento se ha hecho para dos de valores de Kv y para un valor de Kp, y se han aplicado dos escalones en cada experimento. Como se tienen dos expresiones para el cálculo de 𝐼𝑦𝑦/𝑘𝜏 , y con los valores usados de los

términos proporcionales Kv y Kp, se obtienen un total de ocho valores.

En la siguiente figura se ve el pitch (giro alrededor del eje y) para uno de los experimentos: escalón de -20o, Kv de 0.1 y Kp de 4.5.

25

Figura 2.10 Experimento sobre el ángulo pitch

A continuación se muestra una tabla con todos los valores de inercia experimentales,

Tabla 2.1 Resultados experimentarles de 𝐼𝑦𝑦/𝑘𝜏 (𝑟𝑎𝑑

𝑠2 𝑢.𝑎.)

Kv

Prueba

0.1 0.2

1 0.0256

0.0147

0.0238

0.0117

2 0.0179

0.0098

0.0220

0.0113

Cada elemento de la tabla muestra dos valores, correspondientes con las dos expresiones para el cálculo de 𝐼𝑦𝑦/𝑘𝜏 . El valor final tomado será la media de dichos valores, que en este caso es de 0.0171 rad/(s2 u.a.). Las

unidades de 𝐼𝑦𝑦 son 𝑁𝑚 𝑠2/𝑟𝑎𝑑 y las unidades de 𝑘𝜏 son 𝑁𝑚/𝑢. 𝑎., de ahí que las unidades de 𝐼𝑦𝑦/𝑘𝜏 sean 𝑟𝑎𝑑/(𝑠2 𝑢. 𝑎. ).

El modelo Simulink empleado se muestra en la siguiente figura,

55.5 56 56.5 57 57.5 58

-25

-20

-15

-10

-5

0

T (s)

(

º)

Referencia (º)

Pitch (º)


26

Figura 2.11 Modelo Simulink

2.3.2 Inercia respecto al eje-z

En este caso se puede asumir que la inercia alrededor del eje-z, Izz, será el doble de la inercia respecto al eje-x y eje-y, Ixx e Iyy.

Esto se puede comprobar haciendo uso de la Figura 2.7. Siendo el eje-z perpendicular al plano, podemos aproximar la inercia del hexarotor de la siguiente forma,

𝐼𝑧𝑧 ≃∑𝑙2𝑚𝑖

6

𝑖=1

= 6𝑚𝑙2 ( 2.50 )

Y como ya se había visto anteriormente,

𝐼𝑥𝑥 ≃ 𝐼𝑦𝑦 ≃ 3𝑚𝑙2 ( 2.51 )

Por tanto se puede asumir la siguiente relación respecto a la inercia alrededor del eje-z,

𝐼𝑧𝑧 ≃ 2𝐼𝑥𝑥 ≃ 2𝐼𝑦𝑦 ( 2.52 )

2.3.3 Masa y Ku

Es necesario identificar estos dos parámetros para poder hacer un modelo Simulink del sistema. Para el cálculo de la masa se usa un peso, obteniendo el siguiente resultado,

27

Tabla 2.2 Masa del sistema

Batería Sistema con batería

Masa (kg) 0.579 2.979

Se observa que la batería ocupa casi el 20% de la masa total del sistema.

Por otro lado se tiene el parámetro Ku, que relaciona la actuación en tanto por mil del empuje con la fuerza en N. Para su cálculo se parte de la ecuación de Newton en el eje-z, ya vista en el capítulo 2.1.3.1 .

∑𝐹𝑧 = 𝑚�̈� ( 2.53 )

Que se pasa a escribir del siguiente modo,

−𝑚𝑔 + 𝑇 = 𝑚�̈� ( 2.54 )

Relacionándolo con la actuación a través de Ku se tiene,

−𝑚𝑔 +𝑢

𝐾𝑢= 𝑚�̈� ( 2.55 )

Sin embargo, en el sistema real el control se realiza midiendo la aceleración en 𝑐𝑚/𝑠2, por tanto es necesario multiplicar por 100 a ambos lados de la ecuación,

100 (−𝑚𝑔 +𝑢

𝐾𝑢) = 100𝑚�̈� = 𝑚�̈�𝑐𝑚 ( 2.56 )

Se despeja de esta ecuación la ganancia 𝐾𝑢,

𝐾𝑢 =100𝑢

𝑚(�̈� + 100𝑔) ( 2.57 )

Ahora bien, si se particulariza dicha ecuación en condición de hovering, se puede obtener un valor numérico para 𝐾𝑢 que será posteriormente empleado para realizar el modelo Simulink.


28

𝐾𝑢 =

𝑢

𝑚𝑔 ( 2.58 )

La masa 𝑚 y la aceleración de la gravedad 𝑔 son conocidas, mientras que para el cálculo del valor de la actuación que proporciona un vuelo de hovering se ha obtenido experimentalmente. A continuación se muestra una figura en la que se puede obtener dicho valor de actuación,

Figura 2.12 Experimento para hallar el valor de la actuación en hovering

La línea verde muestra que se ha realizado un vuelo en condición de hovering a una altura aproximada de 2.7 metros, mientras que el valor de la actuación (línea azul), en media, es 453.5 ‰. Esto proporciona un valor de 𝐾𝑢 igual a 15.52 ‰/N.

2.3.4 Parámetro 𝑲𝑿𝒀

Este parámetro es necesario para poder hacer un modelo del control de traslación, ya que permitirá relacionar la inclinación en centésimas de grado con la velocidad en el plano en cm/s proporcionada por el sensor de flujo óptico.

Dada la simetría del problema, se obtendrá dicho parámetro para el eje-y y el ángulo roll. Primeramente se hará de manera teórica, y posteriormente se hará una comprobación de manera experimental.

Si se parte de la ecuación (1.14), y simplificando teniendo en cuenta pequeños ángulos, se puede escribir,

�̈� = −𝑇

𝑚𝜙 ( 2.59 )

Pasando la aceleración a cm/s2 y la inclinación a centésimas de grados,

294 296 298 300 302 304 306 308 310 312 314

500

X: 301.8Y: 446

T (s)294 296 298 300 302 304 306 308 310 312 314

10

X: 301.8Y: 2.74

T (s)

Altura[m]

Thrust[‰]

29

�̈� =

�̈�𝑐𝑚100

( 2.60 )

𝜙 = 𝜙𝑐𝜋

180

1

100 ( 2.61 )

Ahora bien, en primera aproximación se supone que el empuje será constante, ya que se mantendrá a una altura fija. Por tanto según la ecuación (1.15) se obtiene,

0 =𝑇

𝑚− 𝑔 ( 2.62 )

𝑇 = 𝑚𝑔 ( 2.63 )

Por tanto es posible reescribir la ecuación (2.59) del siguiente modo,

�̈�𝑐𝑚 = −𝜋𝑔

180 𝜙𝑐 ( 2.64 )

Y pasando al dominio de Laplace,

𝑠�̇�(𝑠) − �̇�(0) = −𝜋𝑔

180 Φ𝑐(𝑠) ( 2.65 )

Por tanto, de manera teórica se obtiene un valor de,

𝐾𝑋𝑌 =𝜋𝑔

180= 0.171

𝑐𝑚/𝑠

𝑐𝑜 ( 2.66 )

A continuación se va a corroborar que dicho valor sea acertado de manera experimental, aplicando una entrada escalón en el siguiente sistema,

Figura 2.13 Sistema a identificar para el control de posición.

En relación a la figura anterior se puede escribir,


30

�̇�𝑐𝑚(𝑠) = −𝐾𝑋𝑌𝑠Φ𝑐(𝑠) ( 2.67 )

El escalón en el dominio de Laplace,

Φ𝑐(𝑠) = Φ𝑐1

𝑠 ( 2.68 )

Por tanto sustituyendo en la ecuación (4.29) se obtiene,

�̇�𝑐𝑚(𝑠) = −𝐾𝑋𝑌Φ𝑐1

𝑠2 ( 2.69 )

Y haciéndole la antitransformada,

�̇�𝑐𝑚(𝑡) = −𝐾𝑋𝑌Φ𝑐𝑡 , 𝑡 > 0 ( 2.70 )

Por tanto, y siendo m la pendiente de la curva �̇�𝑐𝑚(𝑡), se obtiene finalmente,

𝐾𝑋𝑌 = −𝑚

Φ𝑐 ( 2.71 )

El experimento realizado ha consistido en aplicar un escalón en el ángulo roll de 1000 𝑐𝑜. La siguiente figura

muestra en rojo dicho escalón, en azul la respuesta en velocidad con signo cambiado, y el espacio comprendido entre las dos líneas verticales verdes es dónde se ha calculado la pendiente de dicha curva,

Figura 2.14 Resultados del experimento realizado para el cálculo de Kxy

138.2 138.4 138.6 138.8 139 139.2 139.4

-40

-20

0

20

40

60

80

100

120

T (s)

-Vy (cm/s)

Roll*10 (º)

31

El valor obtenido en este experimento ha sido,

𝐾𝑋𝑌 = 0.119 𝑐𝑚/𝑠

𝑐𝑜 ( 2.72 )

Se ha realizado el mismo experimento con un escalón de 500 𝑐𝑜, obteniendo en este caso

𝐾𝑋𝑌 = 0.139 𝑐𝑚/𝑠

𝑐𝑜 ( 2.73 )

El valor empleado será la media de ambos valores.

En la siguiente tabla se recopilan los valores de los parámetros obtenidos en este capítulo de identificación,

Tabla 2.3 Parámetros identificados del sistema

𝑰𝒙𝒙(𝒌𝒈/𝒎𝟐) 0.0171

𝑰𝒚𝒚(𝒌𝒈/𝒎𝟐) 0.0171

𝑰𝒛𝒛(𝒌𝒈/𝒎𝟐) 0.0342

𝒎(𝒌𝒈) 2.979

𝑲𝒖(‰/𝐍) 15.52

𝑲𝑿𝒀 (𝒄𝒎/𝒔

𝒄𝒐) 0.129


32

33

3 SOFTWARE Y PUESTA EN MARCHA

En este capítulo se hará una descripción de los pasos a seguir para poder programar la PX4FMU (PX4 Flight Management Unit) de la que ya se ha hablado anteriormente.

3.1 Software

A continuación se hace una breve descripción de los programas específicos usados. Otros programas empleados que no requieren explicación son MATLAB y Sublime Text 2 (editor de texto).

3.1.1 Mission Planner

Se trata de un programa que sirve de estación terrestre para el proyecto ArduPilot. Entre sus usos, los más destacados son:

- Subir firmware

- Modificar parámetros

- Conexión MAVLINK (Micro Air Vehicle Communication Protocol) con el vehículo

- Análisis de logs

Otra gran utilidad es la capacidad de transformar logs a archivos .mat (MATLAB)

Figura 3.1 Mission Planner

3.1.2 QGroundControl

Al igual que el Mission Planner, se trata de una estación terrestre. Sus usos son muy similares a los de este, pero con la diferencia de que el QGroundControl es de los desarrolladores del PX4 mientras que el Mission Planner ha sido desarrollado para el proyecto ArduPilot.

34 Software y Puesta en Marcha

Figura 3.2 QGroundControl

3.1.3 PX4 Toolchain

Se trata de una versión de eclipse preparada para el PX4. Se usa esencialmente para compilar el código y subirlo al controlador de vuelo.

Se puede usar tanto su interfaz gráfica como su línea de comandos.

Figura 3.3 Línea de comandos del PX4 Toolchain

35

3.2 Compilación

Se comienza por descargar la cadena de herramientas del PX4 (PX4 Toolchain) de la página web del autor, herramientas que se usarán posteriormente para compilar el código. El toolchain está compuesto de:

- PX4 Console

- PX4 Eclipse

- PX4 Software Download

- HTerm

- TeraTerm

En este proyecto se usa el PX4 Console para compilar el código del ArduCopter, pero también se puede usar el PX4 Eclipse.

Por otra parte, de GitHub/diydrones son necesarios los siguientes repositorios,

- ardupilot

- PX4Firmware

- PX4NuttX

Es muy importante elegir las mismas versiones de estos tres repositorios. Por ejemplo si elegimos la versión del Ardupilot que contiene el ArduCotper 3.1.2, también hay que elegir la versión 3.1.2 del Firmware del PX4 y del Nuttx. Se puede ver en la siguiente imagen como escoger la versión,

Figura 3.4 Repositorio del código del ArduCopter

Estos tres repositorios se deben extraer en el directorio del PX4, quedando tal que así:

- C:\px4\ardupilot

- C:\px4\PX4Firmware

- C:\px4\ardupilot\PX4NuttX


Llegados a este punto, ahora se ejecutará el PX4 Console del Toolchain, y escribiremos el comando ‘cd ardupilot/ArduCopter’ (sin comillas y prestando atención a las mayúsculas).

Ahora bien, la primera vez que compilemos tendremos que escribir el comando ‘make configure’, que generará un archivo llamado ‘config.mk’. Este archivo se modifica añadiendo la ruta del firmware del PX4 y la ruta del NuttX, quedando de la siguiente forma,

# Select 'mega' for the 1280 APM1, 'mega2560' otherwise

BOARD = mega2560

# HAL_BOARD determines default HAL target.

HAL_BOARD ?= HAL_BOARD_APM2

# The communication port used to communicate with the APM.

PORT = /dev/ttyACM0

# uncomment and fill in the path to Arduino if installed in an exotic location

# ARDUINO = /path/to/Arduino

# PX4Firmware tree: fill in the path to PX4Firmware repository from github.com/diydrones:

PX4_ROOT=../PX4Firmware

# PX4NuttX tree: fill in the path to PX4NuttX repository from github.com/diydrones:

NUTTX_SRC=../PX4NuttX/nuttx

# VRBRAIN Firmware tree:

VRBRAIN_ROOT=../VRNuttX

# VRBRAIN NuttX tree:

VRBRAIN_NUTTX_SRC=../VRNuttX/NuttX/nuttx

Ya está todo preparado para compilar, que se hace con el comando ‘make px4-v1’ dentro de la ruta ‘C:\px4\ardupilot\ArduCopter’. Si se fija uno bien, detrás de ‘make px4’ hemos escrito ‘-v1’, ya que la versión de la que se dispone es la PX4FMU v1.7.

Si sólo se realizan cambios en el código del ArduPilot o del PX4, basta usar el comando mencionado anteriormente. Sin embargo, si se realizan modificaciones en el NuttX, será necesario ejecutar los comandos ‘make distclean’ y ‘make archives’.

Como consejo, es importante saber que si se desactiva el antivirus en la carpeta del PX4 el tiempo de compilación

se reduce notablemente.

37

Figura 3.5 PX4 Console

Esto generará en la carpeta ArduCopter un archivo llamado ArduCopter-v1.px4, que se puede subir al PX4FMU usando el Mission Planner o el QGroundControl. Otra opción es usar el comando ‘make px4-v1-upload’ y cuando aparezca el mensaje ‘Loaded firmware for X,X, waiting for the bootloader...’ se conectará el USB.

3.3 Sensor de flujo óptico (Optical Flow)

Primero de todo hay que instalar el firmware en el sensor. Para ello se tienen dos opciones, o bien descargándolo desde el repositorio GitHub/PX4/Flow, o bien subiéndolo directamente con el programa QGroundControl.

Si se elige la primera opción, primero tenemos que descargar la versión estable del firmware. Seguidamente, haciendo uso del PX4 Console desde el directorio dónde se haya descargado el firmware, se usan los comandos ‘make all’ (compilación) y ‘make upload-usb’ (subida) conectando el sensor al ordenador mediante USB.

Con la segunda opción, basta con conectar el sensor por USB al ordenador, y en el programa QGroundControl se pincha sobre la pestaña ‘Vehicle Setup’ seguidamente de ‘Firmware Upgrade’. A partir de este punto basta con seguir las instrucciones del programa.

Figura 3.6 Actualización del firmware a través de QGroundControl


Ahora bien, en función de la versión del ArduCopter, puede o no venir activado el driver del PX4Flow.

En el caso de este proyecto, con la versión 3.2-rc14 no viene activado, por lo que hay que realizar varios pasos.

Primeramente se edita el fichero con nombre config_px4fmu-v1_APM.mk cuya ubicación se encuentra en ‘C:\px4\PX4Firmware\makefiles’, y se añade la línea ‘MODULES += drivers/px4flow’. Misma línea que se añade en el fichero config_px4fmu-v1_APM.mk con ubicación en ‘C:\px4\ardupilot\mk\PX4’.

Para comprobar que funciona el sensor de flujo podemos hacer un test desde el NuttShell. Para ello conectamos el PX4 por USB al terminal del Mission Planner, y escribimos los siguientes comandos,

- test

- shell

- px4flow start

También podemos ver una lista de las aplicaciones incluidas en el PX4 usando el comando ‘ls dev’, entre ellas el px4flow. En la siguiente figura se puede ver,

Figura 3.7 Dispositivos instalados en el PX4

En este punto ya se tiene el Optical Flow funcionando, pero hay que activarlo manualmente con el comando mencionado con anterioridad (px4flow start). Para hacer que se active al conectar el PX4 a la batería basta con modificar el archivo ‘rcS’ con ubicación en ‘C:\px4\ardupilot\mk\PX4\ROMFS\init.d\’ y añadir la línea ‘px4flow start’.

Para comprobar su funcionamiento, volvemos a conectar mediante USB con el terminal del Mission planner y escribimos los siguientes comandos,

- test

- optflow

Ya se tiene el sensor de flujo óptico listo para su funcionamiento.

39

3.4 ArduCopter

Es un código libre empleado para el control de multirotores y forma parte del proyecto ArduPilot, el cuál consta del ArduRover, ArduPlane, ArduCopter y AntennaTracker.

El objeto de interés en este proyecto es el ArduCopter, ya que se trata de controlar un hexarotor.

Este controlador puede llevar a cabo tanto vuelos FPV (First Person View), como vuelos destinados a la grabación aérea, como misiones de vuelo totalmente autónomas.

Figura 3.8 Distintas configuraciones de multirotor

En un principio estaba diseñado para ser usado con el APM (ArduPilot Mega), controlador de vuelo basado en Arduino. Actualmente es, en gran medida, compatible con el controlador de vuelo PX4 Autopilot. Este código se ejecuta a través del middleware del PX4, ya que las librerías del proyecto ArduPilot han sido condicionadas por los desarrolladores del proyecto para hacerlas compatibles con el PX4.

El código principal se encuentra en una carpeta llamada ArduCopter, mientras las librerías están en una carpeta llamada Libraries, que son compartidos con los demás proyectos del ArduPilot.

Como se ha dicho anteriormente, el código puede ser ejecutado en el PX4. Esto es posible gracias a una capa de abstracción de hardware (HAL, Hardware Abstraction Layer), incluida como una librería con nombre AP_HAL. Esta proporciona una interfaz entre software y hardware, con lo que hace posible su exportación de una manera sencilla al PX4.

40 Control De Estabilización

4 CONTROL DE ESTABILIZACIÓN

En este capítulo se llevará a cabo el diseño del controlador de estabilización del hexarotor. Con el sistema ya identificado, se hará un diseño en Simulink que servirá para realizar la sintonización sin necesidad de usar el sistema real.

Se ha elegido un controlador de tipo PID en cascada, ya que su implementación es sencilla y su uso muy extendido en el campo de los multirotores.

Como primera aproximación se usará Ziegler-Nichols, y a partir de ahí se irá sintonizando manualmente con la ayuda de MATLAB y Simulink.

Una vez realizada la primera aproximación, se realizará un experimento en el banco de pruebas, y si este ensayo fuera satisfactorio se probará al aire libre sin restricciones.

4.1 Controlador en cascada para pitch y roll

Una vez calculado el parámetro de inercia, se va a diseñar un controlador en cascada para la tasa de variación del ángulo. Se trata de un controlador PID en velocidad con un proporcional en posición. En la siguiente figura se muestra un esquema de dicho controlador,

Figura 4.1 Control en cascada

En primera aproximación se hará uso del método Ziegler-Nichols, con el que se obtendrán los valores del controlador a partir del periodo crítico y de la ganancia Kv crítica.

4.1.1 Ziegler-Nichols

Gracias a los experimentos hechos para la identificación, se sabe que el Kv crítico será algo mayor a 0.2. Por prueba y error se obtendrá dicho valor y se usará con el método Z-N.

Se toma el sistema en bucle cerrado con un proporcional, es decir, anulando la parte integral y derivativa. Los valores de Z-N son los siguientes,

41

Tabla 4.1 Parámetros según Ziegler-Nichols

Kv Ti Td

0.6 Kvcritico 0.5 Pcritico 0.125 Pcritico

En la figura se ve la respuesta para Kvcritico igual a 0.35, aquel que consigue unas oscilaciones mantenidas. El valor de 𝐾𝑝 se deja fijo e igual a 4.5.

Figura 4.2 Pitch con oscilaciones mantenidas

El periodo de dichas oscilaciones es igual a 0.4 segundos, obteniéndose los siguientes valores para el PID,

Tabla 4.2 Parámetros del controlador PID

Kp Kv Ti Td

4.5 0.21 0.2 0.05

Este controlador se ha implementado en Simulink y posteriormente en el sistema real, obteniendo la siguiente respuesta ante un escalón de 20o,

45 45.5 46 46.5 47 47.5 48 48.5

-2

-1.5

-1

-0.5

0

0.5

1

1.5

T (s)

(

º)


Figura 4.3 Comparación del ángulo pitch entre sistema real y simulado

Como se puede comprobar en la figura anterior, el modelo es bastante fiable, aunque no es perfecto. Además se puede observar que la respuesta es buena ya que tiene un tiempo de establecimiento de 1.04 segundos (punto de corte del pitch real con la línea negra horizontal), aunque aún es posible afinarla un poco más.

4.1.2 Sintonización del controlador

El siguiente paso es afinar el controlador, sintonizando el PID con ayuda de Simulink y posteriormente probándolo en el sistema real hasta obtener una respuesta buena. Se ha aumentado el valor de Kp con objeto de reducir el tiempo de subida. Tras la sintonización se han obtenido los siguientes parámetros del controlador,

Tabla 4.3 Parámetros PID tras la primera sintonización

Kp Kv Ti Td

5 0.21 0.09 0.05

En la siguiente figura están superpuestos el modelo Simulink y el sistema real,

36 36.5 37 37.5 38 38.5 39

0

2

4

6

8

10

12

14

16

18

20

X: 37.58Y: 19.5

T (s)

(

º)

Referencia

Real

Simulink

43

Figura 4.4 Comparación del ángulo pitch entre sistema real sintonizado y simulado

Como se puede observar, el tiempo de establecimiento ha bajado 0.24 segundos respecto al valor obtenido mediante Ziegler-Nichols, siendo ahora de 0.8 segundos (punto de corte del pitch real con la línea negra horizontal).

El sistema Simulink empleado es el ya descrito en la Figura 2.11, pero en este caso se ha sustituido el controlador P por un PID. En la siguiente figura se muestra dicho sistema,

Figura 4.5 Modelo Simulink del sistema

El hecho de añadir un retardo (Transport Delay) se puede explicar gracias a los filtros. Al tomar la medida del giróscopo, se está realizando un filtro de tipo Butterworth (filtro paso baja), que introduce un cierto retardo en la señal.

A continuación se ha representado el sistema con retardo y, dado que no se conoce el filtro Butterworth implementado, el sistema con un filtro Butterworth que lo hace similar al sistema con retardo.

Así se observa que el efecto de implementar un filtro Butterworth es similar a un retardo, ya que tanto en magnitud como en fase son similares para la frecuencia de corte calculada (línea vertical negra).

131.5 132 132.5 133 133.5 134

-20

-15

-10

-5

0

X: 132.3Y: -0.5

T (s)

(

º)

Referencia

Real

Simulink


Figura 4.6 Justificación del uso de un retardo en Simulink. Representación en fase

Figura 4.7 Justificación del uso de un retardo en Simulink. Representación en magnitud

10-1

100

101

-150

-140

-130

-120

-110

-100

-90

-80

w(rad/s)

Fa

se

(º)

Sistema con retardo

Sistema con filtro Butterworth

10-1

100

101

102

0

10

20

30

40

50

60

70

w(rad/s)

Ma

gn

itud

(dB

)

Sistema con retardo

Sistema filtrado

45

4.1.3 Pruebas de vuelo libre y resintonización

Una vez sintonizado el PID y ensayado en el banco de pruebas, se realiza un experimento al aire libre. En las siguientes dos figuras se observan las referencias y las respuestas en color rojo y azul respectivamente, tanto para el ángulo pitch como para el roll,

Figura 4.8 Primera prueba sin restricciones. Ángulo pitch

Figura 4.9 Primera prueba sin restricciones. Ángulo roll

Se puede observar que la prueba llevada a cabo al aire libre no se hace corresponder con la prueba realizada en el banco, por lo que ha sido necesario realizar una nueva sintonización.

La razón por la que no son similares ambas pruebas es simple: las pruebas realizadas en el banco han sido hechas usando alrededor de un 65% del empuje total disponible, sin embargo para poder sustentar el hexarotor,

40 42 44 46 48 50 52 54 56

-10

-5

0

5

10

T (s)

(

º)

Referencia

Pitch

62 64 66 68 70 72 74 76

-10

-8

-6

-4

-2

0

2

4

6

T (s)

(

º)

Referencia

Pitch


tan solo es necesario un 35-40%. Este dato no se conocía hasta que no se ha dejado el sistema libre y se ha observado cuando sustentaba.

En la siguiente figura se puede observar el empuje en el banco de pruebas y el empuje en vuelo libre, razón por la que la sintonización del controlador en el banco de pruebas no ha sido satisfactoria.

Figura 4.10 Empuje en banco y en vuelo libre

Por tanto se han vuelto a hacer las pruebas en el banco, esta vez con un empuje similar al necesario para sustentar el hexarotor y se han obtenido de nuevo los parámetros del PID, que en este caso son,

Tabla 4.4 Parámetros del controlador definitivos

Kp Kv Ti Td

5 0.19 1.056 0.053

Con estos valores del controlador se obtiene la siguiente respuesta en el banco de pruebas,

10 20 30 40 50 60 70 80 90 100 1100

10

20

30

40

50

60

70

T (s)

% T

hru

st

Aire libre

Banco de pruebas

47

Figura 4.11 Prueba sobre el ángulo pitch con el controlador definitivo

Dicha respuesta es bastante buena, ya que para obtener una respuesta del 97.5% de la referencia, es decir, una respuesta de 19º se necesitan 0.9 segundos (tiempo de establecimiento).

Llegados a este punto se ha hecho una simulación de estos parámetros del controlador, pero tomando como referencia unos valores realistas,

Figura 4.12 Simulación realista con parámetros del controlador definitivos

Ahora se ha procedido a realizar la prueba de vuelo libre, esta vez obteniendo una respuesta bastante mejor que en el primer caso,

31.5 32 32.5 33 33.5 34

-20

-18

-16

-14

-12

-10

-8

-6

-4

-2

0

T (s)

(

º)

X: 32.68Y: -19.46

Referencia

Respuesta

40 50 60 70 80 90 100 110 120 130

-30

-20

-10

0

10

20

T (s)

(

º)

Referencia

Simulink


Figura 4.13 Prueba de vuelo libre final. Ángulo pitch

Figura 4.14 Prueba de vuelo libre final. Ángulo roll

Estos datos difieren en cierta medida a los datos del banco de prueba debido a que el banco no permite el vuelo libre y tiene mayor rozamiento. Aun así se puede observar que la respuesta es admisible.

A continuación se muestra una figura en la que las líneas rojas horizontales marcan los límites de saturación, y lo representado en azul indica la señal de actuación,

170 180 190 200 210 220 230 240

-15

-10

-5

0

5

T (s)

(

º)

Referencia

Pitch

170 180 190 200 210 220 230 240

-25

-20

-15

-10

-5

0

5

10

15

T (s)

(

º)

Referencia

Roll

49

Figura 4.15 Saturación

Se observa claramente que el sistema no llega a saturar en ningún momento.

Ahora se representa también esta prueba con la simulación en MATLAB, para poder verificar nuevamente que el modelo empleado es fiable,

Figura 4.16 Prueba de vuelo libre final con simulación. Ángulo pitch

170 180 190 200 210 220 230 240

-5000

-4000

-3000

-2000

-1000

0

1000

2000

3000

4000

5000

T (s)

Actu

ació

n

170 180 190 200 210 220 230 240

-20

-15

-10

-5

0

5

10

T (s)

(

º)

Referencia

Simulink

Real


4.2 Controlador en cascada para yaw

El sistema empleado en la simulación es similar al de la Figura 4.5, pero cambiando la inercia alrededor del eje-y por la inercia alrededor del eje-z.

Siguiendo un proceso similar a la sintonización del ángulo pitch se obtienen los siguientes parámetros,

Tabla 4.5 Parámetros del controlador en yaw

Kp Kv Ki

5 0.2 0.025

A continuación se muestra una figura en la que se observa el experimento final, en el que se ha superpuesto la simulación.

Figura 4.17 Prueba de vuelo para el ángulo yaw

115 120 125 130 135 140 145 150 155 160 165 170130

140

150

160

170

180

190

200

210

220

230

T (s)

(

º)

Referencia

Simulink

Real

51

5 CONTROL DE ALTURA

En este capítulo se va a llevar a cabo una descripción del control de altura realizado sobre el hexarotor. Se hará una descripción del PID en cascada que se ha usado y de dónde toma el controlador de vuelo los valores de altura, velocidad y aceleración usados en el controlador.

5.1 Controlador en cascada

Se trata de un controlador en cascada, formado por dos proporcionales y un proporcional-integrador. En la siguiente figura se puede ver un esquema de dicho controlador,

Figura 5.1 Control de altura del hexarotor

Las medidas de altura se obtienen gracias a la medida del barómetro y a la integración de los valores obtenidos del acelerómetro. Un parámetro importante para obtener una buena medida de altitud es INAV_TC_Z, ya que un valor bajo da más importancia a la medida del barómetro, mientras que un valor alto hace que tenga más importancia el acelerómetro. Los valores de la aceleración vertical se obtienen directamente del acelerómetro.

Figura 5.2 Modelo Simulink del control de altura

52 Control De Altura

En la Figura 5.2 se observa el modelo Simulink empleado para la simulación. Se trata de un controlador en cascada, en el que hay dos proporcionales y un PID. Se utiliza el control en cascada, ya que las variaciones que pueda haber tanto en la velocidad como en la aceleración se van arreglando, y por tanto la variable primaria que se quiere controlar tiene pocas variaciones y se trata de un controlador más estable.

Una vez hecho el modelo Simulink del controlador, se le ha aplicado una referencia en altura para ver su comportamiento. Más concretamente se ha aplicado una rampa con pendiente unitaria y se ha hecho una sintonización de los parámetros del controlador,

Figura 5.3 Comparación entre sintonización inicial (rojo) y final (verde).

Como se puede ver el tiempo de respuesta es menor una vez hecha la sintonización. A continuación se muestra una tabla con los valores iniciales y finales de la sintonización,

Tabla 5.1 Parámetros del controlador iniciales y finales (sintonizados).

Sintonización

inicial

Sintonización

final

𝑲𝒑 𝟏 𝟑

𝑲𝒗 5 6.5

𝑲𝒂 0.5 0.5

𝑲𝒊 1 0.5

5.2 Experimento

Una vez verificada la fiabilidad del modelo, se realiza una prueba en el exterior. Para ello se ha cambiado la configuración del canal 5 de la radio, pudiendo variar el modo de vuelo entre el control de estabilización y el

0 1 2 3 4 5 6 7 8 9-0.2

0

0.2

0.4

0.6

0.8

1

1.2

T (s)

H (

m)

Referencia

Sintonización final

Simulación

53

control de altura. En las siguientes gráficas se muestra dicho prueba de vuelo,

Figura 5.4 Resultados en altura del control de altura.

Figura 5.5 Resultados en velocidad del control de altura.

0 20 40 60 80 100 120 140 160 1800

2

4

6

8

10

12

14

16

18

T (s)

h (

m)

Altura

Referencia

Real

Simulado

0 20 40 60 80 100 120 140 160 180-300

-200

-100

0

100

200

300

400

T (s)

Vz (

cm

/s)

Velocidad

Referencia

Real

Simulado

54 Control De Altura

Figura 5.6 Resultados en aceleración del control de altura.

Observando estas tres gráficas de los experimentos, se puede observar primeramente que el modelo empleado para realizar la simulación es bastante realista.

Si se presta atención a la Figura 5.4 se puede observar que la respuesta es buena. A continuación se hace zoom en uno de los escalones de dicha figura,

Figura 5.7 Escalón aplicado en el control de altura

Se observa que tarda en estabilizarse la altura 1.43 segundos desde que se deja de aplicar la referencia en altura.

0 20 40 60 80 100 120 140 160 180-600

-400

-200

0

200

400

600

800

T (s)

a (

cm

/s2)

Aceleración

Referencia

Real

Simulado

49 50 51 52 53 542

3

4

5

6

7

8

9

10

11

X: 52Y: 9.87

T (s)

h (

m)

X: 50.57Y: 10.14

Altura

Referencia

Real

55

6 CONTROL DE TRASLACIÓN

En este capítulo se lleva a cabo el diseño de un control de traslación. Para ello se hace uso del sensor de flujo óptico PX4FLOW, mostrado en la Figura 1.19. Ha sido necesario enfocar la lente para obtener medidas correctas. Además, se hará una breve descripción de cómo obtener las velocidades a partir del sensor.

6.1 Cálculo de la velocidad lineal

A continuación se describe brevemente como obtener las velocidades lineales haciendo uso del sensor de flujo óptico.

Figura 6.1 Esquema de un punto P y su proyección p en el plano imagen

En relación a la imagen anterior se define,

- El sistema de referencia (X,Y,Z) de la cámara, a partir de ahora sistema C,

- El sistema de referencia (x,y) sobre el plano imagen, a partir de ahora sistema I,

- �⃗⃗� = (X, Y, Z), un punto en el sistema C,

- 𝑝 = (x, y, f), la coordena en pixeles de la proyección de �⃗⃗� sobre el plano imagen,

- 𝑓 longitud focal,

- 𝐶𝑃 centro de proyección coincidente en el origen del sistema C,

En relación al punto �⃗⃗� y su proyección es posible escribir,

𝑝𝐼

𝑓=�⃗⃗�𝑐

𝑍 ( 6.1 )

Ahora bien, lo que nos proporciona el sensor de flujo óptico es la velocidad (flujo) de 𝑝 en el plano imagen en píxeles por segundo,

56 Control De Traslación

�⃗�𝐼 =

𝑑𝑝𝐼

𝑑𝑡 ( 6.2 )

Usando la ecuación (7.1) se puede escribir,

�⃗�𝐼 =

𝑑𝑝𝐼

𝑑𝑡=𝑑

𝑑𝑡(𝑓

𝑍�⃗⃗�𝑐) =

1

𝑍(𝑓�⃗⃗�𝑐 − 𝑝𝑉𝑧

𝑐) ( 6.3 )

Ahora bien, esta velocidad �⃗⃗�𝑐 se puede descomponer como la velocidad de traslación de la cámara y la velocidad debida a la rotación de los ejes del sistema C,

�⃗⃗�𝑐 =

𝑑�⃗⃗�𝑐

𝑑𝑡= −�⃗⃗� − �⃗⃗⃗� ∧ �⃗⃗�𝑐 = −�⃗⃗� −

𝑍

𝑓�⃗⃗⃗� ∧ 𝑝𝐼 ( 6.4 )

Sustituyendo esta última expresión en la ecuación (7.3) ,

�⃗�𝐼 =

1

𝑍(𝑓 (−�⃗⃗� −

𝑍

𝑓�⃗⃗⃗� ∧ 𝑝𝐼) − 𝑝 (−�⃗⃗� −

𝑍

𝑓�⃗⃗⃗� ∧ 𝑝𝐼)

𝑍

) ( 6.5 )

Si se desarrolla esta ecuación y se expresa en forma escalar, se obtiene,

𝑣𝑥 =

−𝑇𝑥𝑓 + 𝑇𝑧𝑥

𝑍− 𝑤𝑦𝑓 + 𝑤𝑧𝑦 +

−𝑤𝑥𝑦𝑥 + 𝑤𝑦𝑥2

𝑓 ( 6.6 )

𝑣𝑦 =

−𝑇𝑦𝑓 + 𝑇𝑧𝑦

𝑍+ 𝑤𝑥𝑓 − 𝑤𝑧𝑥 +

−𝑤𝑦𝑥𝑦 + 𝑤𝑥𝑦2

𝑓 ( 6.7 )

Si se supone que la distancia 𝑍 es constante (𝑇𝑧 = 0), y despreciando los términos divididos por la distancia focal 𝑓 en píxeles,

𝑣𝑥 =

−𝑇𝑥𝑓

𝑍−𝑤𝑦𝑓 + 𝑤𝑧𝑦 ( 6.8 )

𝑣𝑦 =

−𝑇𝑦𝑓

𝑍+𝑤𝑥𝑓 − 𝑤𝑧𝑥 ( 6.9 )

Por tanto es posible despejar las velocidades lineales con la velocidad angular compensada que se quieren obtener usando el sensor de flujo óptico,

𝑇𝑥 = −(𝑣𝑥 +𝑤𝑦𝑓 − 𝑤𝑧𝑦)

𝑍

𝑓 ( 6.10 )

𝑇𝑦 = −(𝑣𝑦 − 𝑤𝑥𝑓 + 𝑤𝑧𝑥)

𝑍

𝑓 ( 6.11 )

Este sensor lleva incorporado una IMU para el cálculo de las velocidades angulares. El cálculo del flujo óptico (𝑣𝑥 , 𝑣𝑦) se describe brevemente a continuación.

57

Cada fotograma, de 64𝑥64 píxeles, se divide en 64 bloques de 8𝑥8 píxeles. A continuación se toma un fotograma en el instante 𝑡, y se elige un bloque de 8x8 en el que se calcula la intensidad de la imagen.

Figura 6.2 Fotograma en el instante t

Esta figura representa un fotograma, en el que el bloque elegido es de color negro, y la ventana de búsqueda para el instante 𝑡 + 𝛿𝑡 es de color gris. Esta ventana de búsqueda representa las posibles direcciones del movimiento.

Para poder saber en qué dirección se ha producido el movimiento, en todos los posibles puntos de la ventana de búsqueda se calcula la intensidad de la imagen en el instante 𝑡 + 𝛿𝑡, y se calcula la diferencia en valor absoluto de la intensidad de la imagen con el instante 𝑡. Aquella diferencia que sea mínima proporciona la dirección del movimiento.

Figura 6.3 Fotograma en el instante t + dt

Si el bloque azul proporciona la diferencia mínima de intensidades, entonces la dirección del movimiento será la marcada con flecha roja en la siguiente figura, mientras que el flujo será el desplazamiento de dichos píxeles en ese tiempo 𝑑𝑡.


Figura 6.4 Dirección del movimiento

6.2 Controlador

Se ha realizado un control de traslación con un PI en velocidad. El modelo Simulink se muestra a continuación,

Figura 6.5 Modelo Simulink del control de traslación

Como se puede ver, la entrada del bloque de control es la velocidad (cm/s) y la salida es el ángulo de referencia (cº), que pasa al control de actitud.

Este modelo ha sido posible realizarlo gracias a la identificación del parámetro 𝐾𝑥𝑦 realizada más atrás. Se hace

uso del modelo para realizar una primera sintonización del control de traslación, que posteriormente se probará en el sistema real.

Se obtienen los siguientes valores del controlador,

59

Tabla 6.1 Parámetros del controlador de traslación

𝑲𝒗 𝑲𝑰 𝑲𝑫

11 1.8 0

A continuación se muestra el resultado de la simulación ante un escalón de 30 𝑐𝑚/𝑠 en velocidad,

Figura 6.6 Respuesta del modelo de control de traslación ante un escalón

Se ha obtenido con Simulink un tiempo de subida de 1.215 𝑠, un tiempo de pico de 1.98 𝑠 y un tiempo de

establecimiento de 9.54 𝑠.

A continuación se han realizado experimentos para resintonizar el sistema real y verificar el modelo en Simulink.

6.3 Experimentos

Los experimentos se han llevado a cabo sobre césped, ya que esta superficie proporciona una mejor calidad de la imagen y por tanto una mejor medida de las velocidades en el plano XY.

El controlador implementado en el sistema real es el siguiente,

0 5 10 15 20 25 300

5

10

15

20

25

30

35

40

X: 6.98Y: 37.85

cm

/s

X: 6.215Y: 30

X: 14.54Y: 30.79

T (s)

Referencia

Respuesta


Figura 6.7 Modelo del control de traslación

A continuación se muestra una imagen de dicho experimento,

Figura 6.8 Experimento para el control de posición

Primeramente se ha realizado un experimento con los parámetros del controlador obtenidos con Simulink, y tras varias pruebas, se ha resintonizado obteniendo los siguientes parámetros finales,

61

Tabla 6.2 Valores finales del control de traslación

𝑲𝒗 𝑲𝑰 𝑲𝑫

10 2.2 0

Se van a exponer los resultados del experimento, que consiste en aplicar una referencia nula en velocidad al

control de posición para que el hexarotor se mantenga en un mismo punto sobre el plano XY. El experimento ha durado 34 segundos a una altura media de 60 cm sobre el suelo,

Figura 6.9 Representación del plano XY durante el experimento en posición

Se puede ver que el sistema real oscila con un radio de unos 15 cm.

Se observa un mayor error en el eje X que en el eje Y, y se debe principalmente a una razón. Al realizar dicho experimento había rachas de viento soplando en la dirección del eje Y.

Ahora se muestra la velocidad del sistema medida por el sensor de flujo óptico (azul), la respuesta obtenida tras pasar dicha velocidad por el controlador PID (rojo), y el ángulo en grados tras pasar dicha respuesta al control de estabilización (verde),

-15 -10 -5 0 5 10 15 20

-15

-10

-5

0

5

10

X (cm)

Y (

cm

)

Plano XY


Figura 6.10 Actuaciones del control de traslación

A continuación se ha realizado un experimento en el que se le ha aplicado un escalón de 40 cm/s, y se ha comparado dicho experimento con la simulación en MATLAB, para así verificar que el modelo es correcto. En la siguiente imagen se pueden observar superpuestos el real y el simulado,

Figura 6.11 Comparación del sistema real y el modelo simulado

Se puede ver que el sistema real y simulado tienen una respuesta similar ante una entrada escalón. No obstante, en el régimen permanente hay cierta diferencia, y esto se debe a que la velocidad medida por el sensor de flujo óptico tiene cierto error.

245 250 255 260 265 270 275

-4

-2

0

2

4

T(s)

Vx/10 (cm/s)

Pitch deseado (º)

Pitch (º)

245 250 255 260 265 270 275-4

-2

0

2

4

T(s)

Vy/10 (cm/s)

Roll deseado (º)

Roll (º)

234 236 238 240 242 244 246-40

-20

0

20

40

60

80

100

T (s)

cm

/s

Vy Sistema Real

Referencia

Vy Simulada

63

ANEXO A. CÓDIGOS

Sensor de flujo óptico

A continuación se muestra el código realizado para realizar el control con el sensor de flujo óptico. El archivo principal es el siguiente,

‘px4\ardupilot\ArduCopter\Control_ofloiter.pde’

// ofloiter_init - initialise ofloiter controller

static bool ofloiter_init(bool ignore_checks)

{

// initialize vertical speed and acceleration

pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);

pos_control.set_accel_z(g.pilot_accel_z);

// initialise altitude target to stopping point

pos_control.set_target_to_stopping_point_z();

distanciax=0.0f;

distanciay=0.0f;

distanciaxreal=0.0f;

distanciayreal=0.0f;

ultimotiemporoll = micros() - 2500.0f;

ultimotiempo = micros() - 2500.0f;

integradorr=0;

integradorp=0;

}

// ofloiter_run - runs the optical flow loiter controller

// should be called at 100hz or more

static void ofloiter_run()

{

int16_t target_r, target_p;

float target_yaw_rate = 0;

float target_climb_rate = 0;

float final_roll, final_pitch;

// if not auto armed set throttle to zero and exit immediately

if(!ap.auto_armed) {

attitude_control.relax_bf_rate_controller();

attitude_control.set_yaw_target_to_current_heading();

attitude_control.set_throttle_out(0, false);

pos_control.set_alt_target_to_current_alt();

integradorr=0;

integradorp=0;

return;

}

// process pilot inputs

if (!failsafe.radio) {

// apply SIMPLE mode transform to pilot inputs

update_simple_mode();

// convert pilot input to lean angles

get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, target_r,

target_p);

// get pilot's desired yaw rate

target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);

// get pilot desired climb rate

target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);

// check for pilot requested take-off

if (ap.land_complete && target_climb_rate > 0) {

// indicate we are taking off

set_land_complete(false);

// clear i term when we're taking off

set_throttle_takeoff();

}

64 Anexo A. Códigos

}

// when landed reset targets and output zero throttle

if (ap.land_complete) {

attitude_control.relax_bf_rate_controller();

attitude_control.set_yaw_target_to_current_heading();

// move throttle to between minimum and non-takeoff-throttle to keep us on the ground

attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false);

pos_control.set_alt_target_to_current_alt();

integradorr=0;

integradorp=0;

}else{

// mix in user control with optical flow

of_roll(target_r, final_roll);

of_pitch(target_p, final_pitch);

// call attitude controller

attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(final_roll, final_pitch,

target_yaw_rate, get_smoothing_gain());

// run altitude controller

if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {

// if sonar is ok, use surface tracking

target_climb_rate = get_throttle_surface_tracking(target_climb_rate,

pos_control.get_alt_target(), G_Dt);

}

// update altitude target and call position controller

pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);

pos_control.update_z_controller();

}

}

static int16_t of_roll(int32_t input_roll, float &roll_out)

{

float p,i,d;

float new_roll=0;

float dtint;

float ahoramismoroll = micros();

//Calculo dt

dtint = (ahoramismoroll - ultimotiemporoll);

float dtbueno = (float)dtint/1000000.0f;

ultimotiemporoll = micros();

velocidadx=optflow.vx_cm_s();

distanciaxreal+=velocidadx*dtbueno;

if (velocidadx> 50.0f) {

velocidadx = 50.0f;

}

if (velocidadx < -50.0f) {

velocidadx = -50.0f;

}

distanciax+=velocidadx*dtbueno;

//Cambio en la referencia del controlador con el canal 6

float refvelo;

uint32_t canal6 = g.rc_6.radio_in;

if (canal6 <= 1500){

refvelo=0.0f ;

} else if (canal6 >= 1500) {

refvelo=(float)g.optflow_cascada;

}

Log_Write_Data(1,refvelo);

// Si actuamos con el stick no tenemos cuenta el optflow

if( input_roll == 0 && current_loc.alt < 1500) {

65

// Calculo P

p = -100.0f*g.optflowp*(refvelo-velocidadx);

// Calculo I (optflow I es 100 veces mayor)

integradorr += -(100.0f*(refvelo-velocidadx) * g.optflowi) * dtbueno; if (integradorr < -100.0f*g.optflow_imax) {

integradorr = -100.0f*g.optflow_imax;

} else if (integradorr > 100.0f*g.optflow_imax) {

integradorr = 100.0f*g.optflow_imax;

}

i=integradorr;

p=constrain_float(p, -100.0f*g.optflow_pmax, 100.0f*g.optflow_pmax);

new_roll = p+i;

}else{

integradorr=0;

p = 0;

i = 0;

new_roll=input_roll;

}

new_roll=new_roll;

// Ángulo máximo de actuación debido al optflow es 10

roll_out = constrain_float(new_roll, -1000, 1000);

Log_Write_PIDroll(roll_out/100.0f,p/100.0f,i/100.0f,d/100.0f);

}

static int16_t of_pitch(int32_t input_pitch, float &pitch_out)

{

float new_pitch=0;

float p,i,d;

float dtint;

float ahoramismo = micros();

//Calculo dt

dtint = (ahoramismo - ultimotiempo);

Log_Write_dt(dtint, ahoramismo, ultimotiempo);

float dtbueno = (float)dtint/1000000.0f;

ultimotiempo = micros();

velocidady=optflow.vy_cm_s();

distanciayreal+=velocidady*dtbueno;

if (velocidady> 50.0f) {

velocidady = 50.0f;

}

if (velocidady < -50.0f) {

velocidady = -50.0f;

}

distanciay+=velocidady*dtbueno;

// Vx > 0 se corrige con Pitch negativo

// Vy > 0 se corrige con Roll positivo

// Si actuamos con el stick no tenemos cuenta el optflow

if( input_pitch == 0 && current_loc.alt < 1500 ) {

// Calculo P (optflow P es 100 veces mayor)

p = 100.0f*g.optflowp_pitch*velocidady;

// Calculo I (optflow I es 100 veces mayor)

integradorp += (100.0f*velocidady * g.optflowi_pitch) * dtbueno; if (integradorp < -100.0f*g.optflow_imax) {

integradorp = -100.0f*g.optflow_imax;

} else if (integradorp > 100.0f*g.optflow_imax) {

integradorp = 100.0f*g.optflow_imax;

}

// el iMAX pasarlo a centésimas de grado (400)

i = integradorp;

// Se trata de un PI, ya que el término derivativo no es bueno (señal ruidosa)


p=constrain_float(p, -100.0f*g.optflow_pmax, 100.0f*g.optflow_pmax);

new_pitch = p + i;

}else{

integradorp=0;

p = 0;

i = 0;

d = 0;

new_pitch = input_pitch;

}

new_pitch=new_pitch;

// Ángulo máximo de actuación debido al optflow es 10

pitch_out=constrain_float(new_pitch, -1000, 1000);

Log_Write_PID(pitch_out/100.0f,p/100.0f,i/100.0f,d/100.0f);

}

‘px4\ardupilot\ArduCopter\Control_ofloiter.h’

static float velocidadx;

static float velocidady;

static float distanciax;

static float distanciay;

static float distanciaxreal;

static float distanciayreal;

static float integradorr;

static float integradorp;

static float ultimotiemporoll;

static float ultimotiempo;

Además se han modificado las librerías del sensor de flujo óptico añadiendo las líneas (en color) que se muestran a continuación, siendo las grises parte del código original.

‘px4\Firmware\src\drivers\px4flow\px4flow.cpp’

...

struct optical_flow_s report;

report.timestamp = hrt_absolute_time();

report.pixel_flow_x_integral = static_cast<float>(f_integral.pixel_flow_x_integral) /

10000.0f;//convert to radians

report.pixel_flow_y_integral = static_cast<float>(f_integral.pixel_flow_y_integral) /

10000.0f;//convert to radians

report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout;

report.ground_distance_m = static_cast<float>(f_integral.ground_distance) / 1000.0f;//convert

to meters

report.quality = f_integral.qual; //0:bad ; 255 max quality

report.gyro_x_rate_integral = static_cast<float>(f_integral.gyro_x_rate_integral) / 10000.0f;

//convert to radians

report.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f;


67

report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f;


report.integration_timespan = f_integral.integration_timespan;

report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;

report.gyro_temperature = f_integral.gyro_temperature;

report.flow_comp_m_x = (static_cast<float>(f.flow_comp_m_x) / 1000.0f) * 100.0f; //cm/s

report.flow_comp_m_y = (static_cast<float>(f.flow_comp_m_y) / 1000.0f) * 100.0f; //cm/s

report.sensor_id = 0;

...

‘px4\Firmware\src\modules\uORB\topics\optical_flow.h’

...

struct optical_flow_s {

uint64_t timestamp;/**< in microseconds since system start */

uint8_t sensor_id;/**< id of the sensor emitting the flow value */

float pixel_flow_x_integral; /**accumulated optical flow in radians around x axis */

float pixel_flow_y_integral;/**<accumulated optical flow in radians around y axis */

float gyro_x_rate_integral; /**< accumulated gyro value in radians around x axis */

float gyro_y_rate_integral; /**< accumulated gyro value in radians around y axis */

float gyro_z_rate_integral; /**< accumulated gyro value in radians around z axis */

float ground_distance_m; /**< Altitude / distance to ground in meters */

uint32_t integration_timespan; /**<accumulation timespan in microseconds */

uint32_t time_since_last_sonar_update;/**< time since last sonar update in micros */

uint16_t frame_count_since_last_readout;/**numberof accumulatedframes in timespan */

int16_t gyro_temperature;/**< Temperature * 100 in centi-degrees Celsius */

uint8_t quality; /**< Average of quality of accumulated frames, 0:bad, 255: max*/

float flow_comp_m_x;

float flow_comp_m_y;

};

...

‘px4\ardupilot\libraries\AP_OpticalFlow\AP_OpticalFlow_PX4.cpp’

...

state.bodyRate.y = integralToRate * float(report.gyro_y_rate_integral); // rad/sec measured

inertially about the Y sensor axis

} else {

state.flowRate.zero();

state.bodyRate.zero();

}

// Correción de los picos de error del sonar

if (report.ground_distance_m-primaltura>0.5f || -(report.ground_distance_m -primaltura)> 0.5f

){

state._ground_distance_m = primaltura;

primaltura=state._ground_distance_m;

} else {

state._ground_distance_m = report.ground_distance_m;

primaltura=report.ground_distance_m;

}

// Velocidad en el plano XY

state._vx_cm_s= report.flow_comp_m_x;

state._vy_cm_s= report.flow_comp_m_y;

_update_frontend(state);

}

}


#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4

‘px4\ardupilot\libraries\AP_OpticalFlow\OpticalFlow.h’

...

// device_id - returns device id

uint8_t device_id() const { return _state.device_id; }

// last_update() - returns system time of last sensor update

uint32_t last_update() const { return _last_update_ms; }

float vx_cm_s() const { return _state._vx_cm_s; }

float vy_cm_s() const { return _state._vy_cm_s; }

// parameter var info table

static const struct AP_Param::GroupInfo var_info[];

...

Escritura de datos en la tarjeta SD

Para poder realizar las gráficas del apartado anterior ha sido necesario grabar los datos de altura, velocidad y aceleración en la tarjeta microSD, asi como sus valores de referencia.

A continuación se describe detalladamente los archivos del proyecto Ardupilot que se deben modificar para conseguir dicho propósito.

En primer lugar necesitamos modificar el archivo del añadiendo las siguientes líneas al archivo indicado a continuación.

‘px4/ardupilot/ArduCopter/Log.pde’

struct PACKED log_ALTURA {

LOG_PACKET_HEADER;

uint32_t timems;

float Altur;

float Veloci;

float Acele;

float AlturRe;

float VelocRe;

float AcelerRe;

};

static void Log_Write_ALTURA()

{

struct log_ALTURA pkt = {

LOG_PACKET_HEADER_INIT(LOG_ALTURA_MSG),

timems : hal.scheduler->millis(),

Altur : current_loc.alt / 100.0f,

Veloci : inertial_nav.get_velocity().z,

Acele : -(ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f,

AlturRe : pos_control.get_alt_target() / 100.0f, // m

VelocRe : pos_control.get_vel_target().z, //cm/s

AcelerRe : pos_control.get_acelerref() //cm/s^2

};

DataFlash.WriteBlock(&pkt, sizeof(pkt));

69

}

Seguidamente, en el mismo archivo añadimos el siguiente par de líneas dentro de la estructura log_structure,

{ 0x1D, sizeof(log_ALTURA),

"ALT", "ffffff", "timems,alt,vel,acel,altRef,velRef,acelRef" },

Y como última modificación a este archivo, se añade tras #else // LOGGING_ENABLED la siguiente línea,

static void Log_Write_ALTURA() {}

Y por último, en el archivo control_althold.pde de la ruta anterior, dentro de la función althold_run() se ejecuta la función Log_Write_ALTURA().

Con estas modificaciones ya se pueden hacer uso de los valores empleados para el control, que son altura, velocidad y acerelación, así como sus valores de referencia.

71

REFERENCIAS

[1] N. Salim, «PID plus LQR attitude control for hexarotor MAV in indoor environments,» Feb 2014.

[2] T. Luukkonen, «Modelling and control of quadcopter,» Aug 2011.

[3] F. Hover, «Kinematics of Moving Frames,» de System Design for Uncertainty, Massachusetts, 2009.

[4] J. Fogelberg, «Navigation and Autonomous,» 2013.

[5] S. Bouabdallah, «Full Control of a Quadrotor,» Oct 2007.

[6] V. Artale, «Mathematical Modeling of Hexacopter,» 2013.

[7] «https://pixhawk.org/,» [En línea].

[8] «http://ardupilot.com/,» [En línea].

[9] «http://diydrones.com/,» [En línea].

[10] «https://groups.google.com/forum/#!forum/px4users,» [En línea].

[11] D. Honegger, «An Open Source and Open Hardware Embedded Metric Optical Flow CMOS Camera for

Indoor and Outdoor Applications,» 2013.

[12] F. Massanes, «Compute-unified device architecture implementation of a block-matching algorithm for

multiple graphical processing unit cards,» Journal of Electronic Imaging , Jul 2011.

proyecto fin de carrera - universidad de...

Documents