grupo de robÓtica, visiÓn y control -...

UNIVERSIDAD DE SEVILLA

ESCUELA SUPERIOR INGENIEROS

DEPARTAMENTO DE INGENIERÍA DE SISTEMAS Y

AUTOMÁTICA

GRUPO DE ROBÓTICA, VISIÓN Y CONTROL

CONTROL DE QUADROTOR CON

BRAZO MANIPULADOR

Autor: Antonio Enrique Jiménez Cano Tutores: Guillermo Heredia Benot

Aníbal Ollero Baturone

Proyecto Fin de Carrera

20 SEPTIEMBRE DE 2012

ÍNDICE Lista de símbolos

1. INTRODUCCIÓN

1.1. MOTIVACIÓN Y OBJETIVOS................................................................................................ 4

1.2. ESTRUCTURA DE LA MEMORIA........................................................................................ 5

2. MODELADO DEL SISTEMA

2.1. CONCEPTOS Y GENERALIDADES....................................................................................... 7

2.2. MODELADO DEL QUADROTOR........................................................................................... 8

2.3. MODELO DEL MANIPULADOR............................................................................................ 16

2.4. MODELADO DEL QUADROTOR CON MANIPULADOR...................................................... 23

3. SISTEMA DE CONTROL


3.2. MODELO PARA EL CONTROL.............................................................................................. 28

3.3. CONTROLADOR PID............................................................................................................. 31

3.4. CONTROLADOR INTEGRAL BACKSTEPPING(IB)............................................................... 34

3.5. CONTROLADOR VARIABLE PARAMETER INTEGRAL BACKSTEPPING(VPIB)................ 40

3.6. SIMULACIONES.................................................................................................................... 44

4. EXPERIMENTOS


4.2. INTRODUCCIÓN A ARDUCOPTER....................................................................................... 68

4.3. EXPERIMENTOS CON ARDUCOPTER.................................................................................. 74

5. CONCLUSIONES............................................................................................................................... 77

6. LÍNEAS FUTURAS ............................................................................................................................ 78

ANEXOS

A.CÁLCULO DE MOMENTOS DE INERCIA................................................................................ 79

B.VARIACIÓN DE LOS MOMENTOS DE INERCIA..................................................................... 83

BIBLIOGRAFÍA ..................................................................................................................................... 86

Lista de símbolos

Ω Velocidad de giro dl rotor Matriz de inercia Ángulo roll Ángulo pitch Ángulo yaw , , , , , Términos matriz de inercia , , Momentos sobre ejes x y z Energía potencial Lagrangiano Energía cinética Vector velocidad Fuerza i , , Velocidad angular en roll, pitch y yaw , , Aceleraciones angulares en roll, pitch y yaw J Inercia rotor masa gravedad Entrada de control Posición angular de la articulación i Constantes controlador Errores angulares o de posición Factor de arrastre Factor de empuje Longitud brazo quadrotor Ω Velocidad angular residual hélices

CONTROL DE QUADROTOR CON BRAZO MANIPULADOR

INTRODUCCIÓN

- 4 -

1. INTRODUCCIÓN

1.1 MOTIVACIÓN Y OBJETIVOS

Este trabajo nace como respuesta a uno de los objetivos tecnológicos marcados dentro

del ámbito del proyecto ARCAS[1]. ARCAS es un proyecto, a nivel internacional, cuyo

principal objetivo es desarrollar robots que trabajen conjuntamente en el mantenimiento y el

montaje de piezas en lugares inaccesibles.

Fig. 1. Arcas-Project

Estas plataformas construidas gracias a la tecnología que incorporen los aviones no

tripulados en el futuro, pueden servir para la evacuación de personas o como pistas de aterrizaje

en lugares de difícil acceso, como en el caso de un accidente nuclear o fuga de gases tóxicos,

así como en otro tipo de desastres.

Para la realización de estos objetivos, se contempla la investigación y el desarrollo de

helicópteros y quadrotors equipados con manipuladores robóticos capaces de transportar

materiales de construcción o herramientas. Por lo tanto, se plantea necesario el desarrollo de un

sistema de control de movimiento.. Este trabajo entraría dentro de este objetivo tecnológico de

control de movimiento, intentando buscar una estrategia de control robusta, precisa e

innovadora que sirva de manera útil y eficaz a la ejecución del proyecto ARCAS.

Concretamente, en este proyecto se presentará una estrategia de control para un quadrotor con

manipulador robótico acoplado a su base.


INTRODUCCIÓN

- 5 -

1.2 ESTRUCTURA DE LA MEMORIA

El estado del arte del control de quadrotors ha evolucionado de manera drástica en los

últimos años, llegando a implementarse gran cantidad de técnicas de control diferentes en este

tipo de UAVs. Un claro ejemplo de esto, es la gran cantidad de artículos sobre este tema. En la

diversa e ingente bibliografía que podemos encontrar, se pueden ver desarrolladas técnicas de

control que van desde: PIDs[2], fuzzy control[3], LPV control[4], control usando técnicas de

Lyapunov, etc. Además, otro claro ejemplo del auge de este tipo de UAVs es la gran variedad

de quadrotors comerciales existentes a día de hoy, véase modelos como Dranganflyer o

Pelicans desarrollados por empresas privadas hasta modelos creados en distintos proyectos de

universidades como Mesicopter o Starmac (Stanford) o en tesis doctorales como la de S.

Bouabdallah [5] o la de P. Pounds[6].

Fig. 2. (Arriba)Modelo comercial Draganfly. (Abajo Izquierda) Starmac. (Abajo derecha) Modelo comercial AsTec Pelican

En este proyecto se abarca el problema de control de un quadrotor en el que infieren las

fuerzas provocadas por el movimiento e interacción con el medio. Este problema fue planteado

V. Ghadiok en [7] pero en su caso se trataba de un quadrotor al que se le acopla una pinza o

efector mediante el cual es capaz de recoger piezas. En nuestro caso, el quadrotor portará un


INTRODUCCIÓN

- 6 -

manipulador de tres grados de libertad que estaría acoplado en la base de nuestro UAV. Para

ello se estudiarán tres técnicas de control que serán evaluadas mediante un simulador

desarrollado en simulink y, posteriormente, implementadas en un quadrotor comercial tipo

Arducopter[8] al que será acoplado el manipulador de tres grados de libertad, que fue

desarrollado en colaboración entre el Grupo de Robótica, Control y Visión del departamento de

Automática de la Universidad de Sevilla y el CATEC.

En la primera parte de este trabajo, se estudiará el modelado del sistema. Primero,

desarrollando el modelo de un quadrotor. A continuación, se estudiará un modelo del

manipulador. Por último, se expondrá el modelo del quadrotor con manipulador. Para ello se

desarrollará un modelo de quadrotor en el que sus momentos de inercia son variables

dependientes del movimiento del brazo y sobre el que actúan pares y fuerzas tanto del propio

quadrotor como las que añadiría el manipulador al moverse.

En la segunda parte, se abordará el problema de control estudiándose tres técnicas. Dos

de ellas ya estudiadas y vistas en la bibliografía, y una tercera que será ajustada al modelo

propuesto. La primera será un PID, la segunda será Integral Backstepping (IB) y la tercera será

bautizada en este proyecto como Variable Parameter Integral Backstepping (VPIB). Por últmio,

Se evaluarán las distintas técnicas de control, desarrolladas en los apartados anteriores del

capítulo, mediante simulink. Realizando distintas simulaciones y viendo las respuestas que se

obtienen de los distintos controladores propuestos a diferentes movimientos realizados por

nuestro sistema.

En el capítulo cuarto de este proyecto, se realizará una introducción a Arducopter que es

el modelo comercial elegido para implementar nuestros controladores para, a continuación

mostrar los resultados de los distintos experimentos realizados.

Fig. 3. Modelo Arducopter


MODELADO DEL SISTEMA

- 7 -

2. MODELADO DEL SISTEMA

2.1 CONCEPTOS Y GENERALIDADES

Para describir el modelo del sistema, en este trabajo, se hacen uso de las ecuaciones

físicas que explican el comportamiento del mismo. Los parámetros del modelo se han obtenido

tanto de forma experimental como teórica, en el caso de quadrotor; como gracias al uso de un

programa CAD en el diseño del manipulador el cual nos ha proporcionado los parámetros del

mismo.

Primero se desarrollará un modelo para el quadrotor. Para ello, primero se expondrá las

suposiciones que se han considerado a la hora del modelado, las cuales ayudaran a la

simplificación de este. En segundo lugar, se desarrollará la formulación del modelo mediante

Euler-Lagrange. En tercer y último lugar, se formulará el modelo mediante Newton-Euler.

A continuación, se definirá un modelo simplificado de la cinemática del brazo, tanto

directa como inversa. Se explicará el proceso seguido para conseguir unas simulaciones ágiles

del sistema completo donde el efecto del movimiento del manipulador sobre el quadrotor sea

efectivo.

En tercer y último lugar, se desarrollará un modelo del quadrotor donde se incluyan los

efectos provocados por el manipulador acoplado a su base. Este modelo se formulará mediante

Euler-Lagrange y Newton-Euler. Desarrollando, finalmente, sus ecuaciones de movimiento.



- 8 -

2.2 MODELADO DEL QUADROTOR

Como se ha mencionado en el apartado anterior, se ha utilizado tanto formulación de

Newton-Euler como la de Lagrange-Euler para la identificación del modelo, la teoría de

momentos y la teoría de elementos de pala, así como el uso de los ángulos de Tait-Bryan para

modelar el quadrotor con los efectos provocados por el manipulador. El modelo desarrollado en

este trabajo supone lo siguiente:

• La estructura del quadrotor se supone rígida.

• La estructura del quadrotor se supone simétrica.

• El centro de gravedad y el origen de la estructura corporal del quadrotor se supone que

coinciden.

• Las hélices se suponen rígidas.

• El empuje y la resistencia es proporcional al cuadrado de la velocidad de hélice.

Un quadrotor es un sistema mecánico complejo que recoge numerosos efectos físicos de

la aerodinámica y la mecánica [15], el modelo de este deberá recoger los aspectos más

importantes además de los efectos giroscópicos. Los más importantes vienen descritos en la

siguiente tabla 2.1:

Tabla 2.1: Efectos físicos que actúan en un quadrotor

Efecto Fuente Formulación

Efectos Aerodinámicos Rotación hélices Ω Aleteo de la hoja

Pares Inerciales Cambios en la velocidad de rotación

de las hélices

Ω Efectos Gravitatorios Posición del centro de masa

Efectos Giroscópicos Cambio en la orientación del cuerpo

rígido

Cambio en la orientación del plano

de la hélice

!",

Fricción Todo el movimiento del quadrotor , ,



- 9 -

2.2.1 FORMULACIÓN DE EULER-LAGRANGE

El modelo dinámico del quadrotor se obtiene mediante el uso de la fórmula de Euler-

Langrange. No se explicará en este trabajo el proceso de obtención del modelo dinámico, de

forma detallada, mediante el uso de la fórmula de Euler-Lagrange ya que en la bibliografía se

pueden encontrar numerosos ejemplos de esto. En vez de esto, expondremos el resultado de los

cálculos para el modelo dinámico del quadrotor.

Fig. 4. Sistemas de coordenadas

CINEMÁTICA

Para cualquier punto de la estructura del quadrotor expresado en el sistema de coordenadas fijo

en tierra, resulta que (con c: coseno, s: seno).

#$% = '(()* + '(,, − ,(). + '(,( + ,()/$0 = ',()* + ',,, + ((). + ',,( − (,)/$ = '−,)* + '(,). + '(()/ 1 (1)

Las correspondientes velocidades se obtendrían mediante la derivada de (1), y la expresión que

corresponde al cuadrado de la velocidad para cada punto viene dado por:

= % + 0 + 2 (2)

E Z

X Y

F1

Q

F2 F3

F4

x y

z

mquad



- 10 -

ENERGÍA

De la ecuación (2) obtenemos la expresión para la energía cinética:

= 125. + / '6)'$) ∙ 8 − 2 sin + sin = +125/ + * '6)'$) ∙ 8 cos + 2 sin cos cos + sin cos = +125* + . '6)'$) ∙ 8 sin − 2 sin cos cos + cos cos = +5*.'6)'$) ∙ 8 sin sin cos + 8cos sin − sin cos = + cos= +5/*'6)'$) ∙ 8 cos sin cos + 8− cos cos − sin sin = + sin= +5./'6)'$) ∙ 8− sin cos cos + 8sin cos − cos cos= + sin =

(3)

Dado que el quadrotor es simétrico, la ecuación quedaría simplificada de la siguiente forma:

= 12 ' − (@,) + 12 '(@, − ,AB(@,) + 12 ',AB − (@,) (4)

Y la expresión para la energía potencial es:

= 5'−sin · * + sin cos · . + cos cos · /)'$) = 5*'*) · '− sin) + 5.'.) · ' sin cos ) + 5/'/) · ' cos cos )

(5)

ECUACIONES DE MOVIMIENTO

Usando el Lagrangiano y la fórmula de su derivada para las ecuaciones de movimiento:

= − , Γ = DDE F GHGI JK − GHGIJ (6)



- 11 -

Dado que, como se ha mencionado antes, el sistema no es simétrico, las tres ecuaciones de

movimiento serían:

L = 8 − = = ' − ) = 8 − =1 (7)

De la diferencia de empuje de cada par de motores se producen los diferentes pares que actuán

sobre el quadrotor:

L = 'ΩM −Ω ) = 'ΩN −ΩO ) = 'ΩO −Ω +ΩN −ΩM )1 (8)

Además, habría que añadir los efectos giroscópicos producidos pos la rotación de las hélices:

PQ = JΩQ = JΩ 1 (9)

Donde:

Ω =Ω +ΩM −ΩO −ΩN (10)

MODELO DINÁMICO

Las ecuaciones del modelo dinámico del quadrotor describe el movimiento en pitch,roll y yaw

serían:

L = 8 − = + JΩ + = ' − ) − JΩ + = 8 − = + 1 (11)



- 12 -

2.2.2 FORMULACIÓN DE NEWTON-EULER

La dinámica de un cuerpo rígido bajo las fuerzas externas aplicadas al centro de masa y

expresado en el sistema de referencia con origen en la base de la estructura del quadrotor

mediante la fórmula de Newton-Euler, viene dada por las siguientes ecuaciones:

+Ω × ') = S"TS + UV"T + W"UX Ω +Ω × 'JΩ) = TZ[Z + T\][ (12)

FUERZAS Y MOMENTOS AERODINÁMICOS

Los momentos y fuerzas aerodinámicos se obtienen mediante la combinación de momentos y el

uso de la teoría de elemento de pala, tal y como se explican en [9].

Fig. 5. Sistemas de coordenadas

A continuación, se describen brevemente las fuerzas y momentos que intervienen y describen la

dinámica del quadrotor:

• Fuerza de empuje T: Es el resultante de todas las fuerzas verticales que actúan sobre los

elementos de pala:

# = ^_`'Ω6"UD) ^ab = c16 + 14f g h − '1 + f ) Ei8 − 14k1 (13)

l mn o

1

p23

4

* ./

rsb



- 13 -

• Fuerza Hub H: Es el resultante de todas las fuerzas horizontales que actúan sobre los

elementos de pala:

# t = u_`'Ω6"UD) uab = 14b fDvvv + 14 kf ch − Ei2 g1 (14)

• Momento de arrastre Q: Este momento en el eje del rotor se produce debido a las

fuerzas aerodinámicas actuantes sobre los elementos de pala. El momento de arrastre

determina la potencia requerida para girar el rotor.

# p = I_`'Ω6"UD) 6"UDwab = 18b '1 + f )Dvvv + k c16h − Ei8 − 14kg1 (15)

• Momento de balanceo 6x: El momento de balanceo de una hélice se produce durante el

vuelo de avance.

# 6x = yz_`'Ω6"UD) 6"UDyzab = −f c16h − Ei8 − 18kg1 (16)

• Efecto suelo IGE: Se denomina efecto suelo al fenómeno aerodinámico que sucede

cuando un cuerpo, con una diferencia de presiones entre la zona que hay por encima de

él y la que hay por debajo, está muy cerca de la superficie terrestre, lo que provoca unas

alteraciones en el flujo de aire.

#| = |_`'Ω6"UD) |ab = |ab + ~4Ω6"UD 1 (17)

Los movimientos de un Quadrotor son causados por una serie de fuerzas y momentos

procedentes de diferentes efectos físicos.



- 14 -

• Momentos sobre Roll:

o efectos giroscópicos: 8 − =

o momento debido al vuelo de avance: '−1)O∑ 6xMO

o efecto giroscópico debido a giro de las hélices: JΩ

o momento debido al vuelo lateral: ℎ8∑ tMO =

o Acción sobre Roll debido a los actuadores: '− +M)

• Momentos sobre Pitch:

o efectos giroscópicos: ' − )

o momento debido al vuelo de avance: ℎ8∑ tMO =

o efecto giroscópico debido a giro de las hélices: JΩ

o momento debido al vuelo de avance: '−1)O∑ 6xMO

o Acción sobre Pitch debido a los actuadores: 'O−N)

• Momentos sobre Yaw:

o efectos giroscópicos: 8 − =

o fuerza hub debido al desequilibrio por el vuelo en avance 't −tM)

o par inercial: "Ω "

o fuerza hub debido al desequilibrio por el vuelo lateral: 8−tO +tN=

o par desequilibrado: '−1) ∑ pMO



- 15 -

• Dinámica rotores: Todos los rotores llevan incluidos para la simulación un error que

los hace diferentes entre sí para proporcionar mayor realismo. La función de

transferencia que describe su comportamiento sería:

',) = 0.985 ± 0.95'±), + 1 (18)


Las ecuaciones de movimiento se obtienen utilizando todos lo momentos y fuerzas obtenidos en

el sub-apartado anterior:

= 8 − = + JΩ + '− +M) − ℎ t

MO + '−1)O6x

MO

= ' − ) − JΩ + 'O−N) + ℎ tMO +'−1)O6x

MO

= 8 − = + "Ω " + '−1)pMO + 't −tM) + 8−tO +tN=

/ = − '(@,(@,)MO

* = ',AB,AB + (@,,AB(@,)MO −t

MO − 12`_* |* |

. = '−(@,,AB + ,AB,AB(@,)MO −t

MO − 12`_. |. |

1 (19)



- 16 -

2.3 MODELO DEL MANIPULADOR

Como se ha mencionado anteriormente, para este proyecto se construyó un manipulador

de tres grados de libertad que se acoplo a la base del quadrotor. Debido a que el control del

manipulador no forma parte de este proyecto, no se entrará en detalle. Pero sí interesa la

influencia del movimiento del manipulador en la estabilidad en vuelo del quadrotor, y en la

capacidad del quadrotor de mantener la posición deseada con el menor error posible durante la

ejecución de sus movimientos.

Fig. 6. Representación del manipulador con CATIA.

La figura 6. es una representación mediante CATIA del primer prototipo de

manipulador que se desarrollo para realizar los primeros experimentos del proyecto ARCAS.

Como podemos ver se trata de un manipulador con tres articulaciones y que tendría tres grados

de libertad, como se ha mencionado con anterioridad, aunque a la hora de modelar el sistema no

consideraremos la última articulación ya que no supone un cambio de plano, aunque sí en la

orientación y, por lo tanto, a efectos de momentos generados por esta última articulación estos

se considerarían despreciables con respecto a los generados por la otras dos articulaciones. A

continuación, en el subapartado modelo matemático, se estudiará el modelo cinemático directo e

inverso del manipulador robótico, para ello, se ha realizado un boceto esquemático, figura 7,

donde se representa el manipulador sobre el plano XZ, que será el plano del sistema sobre el que



- 17 -

ejecutará sus correspondientes movimientos. Sobre el boceto se dibujaran, además, los

diferentes parámetros necesarios para calcular los modelos cinemáticos directo e inverso que,

posteriormente, aclararemos su significado. Solo se tendrán en cuenta para la realización de este

modelo las dos primeras articulaciones, ya que la influencia de la tercera se podría considerar

despreciable dentro del caso de estudio que nos ocupa.

Fig. 7. Robot manipulador de 2 g.d.l con articulaciones rotacionales

El primer eslabón tiene una longitud constante O y la distancia que tiene a su centro de

masa, se llamará O. El segundo eslabón también tiene una longitud constante , la distancia a

su centro de masa será . Ambos eslabones poseen movimientos rotacionales que

denominaremos r1 y r2 respectivamente, el giro de las articulaciones puede ser de 0° a −180°

en el caso de r1 y de 360° en el caso de r2, para las simulaciones. En la práctica, para la

primera versión del robot manipulador diseñado y creado para los experimentos el rango de giro

será menor debido a limitaciones de los servos.

X

Z

LLc1

L

Lc2

m1

m2

q2

q1



- 18 -

2.3.1 MODELO MATEMÁTICO

Mediante el análisis matemático se obtuvieron los modelos cinemáticos directo e

inverso del robot manipulador. En el modelo dinámico no se entró ya que para nuestras

simulaciones no era necesario.

MODELO CINEMÁTICO DIRECTO

El modelo cinemático directo describe la relación entre la posición articular r, y la posición y

orientación en el plano de referencia coordenado cartesiano '*, ., /). *O/O = Ocos'rO)− Osin'rO) (20)

* / = O cos'rO) + cos'rO + r )− Osin'rO) − sin'rO + r ) (21)

donde rO es la posición angular de la articulación 1 y r es la posición angular de la

articulación 2. Derivando las ecuaciones anteriores encontramos las velocidades angulares O u :

O = *O/O = Osin'rO)rO Ocos'rO)rO (22)

= * / = O sin'rO)rO + sin'rO + r ) 'rO + r ) O cos'rO) rO + cos'rO + r )'rO + r ) (23)

De donde se obtiene:

O ∙ O = O ∙ rO ∙ = O rO + 'rO + r ) + 2 O 'rO + r )rOcos'r ) (24)

Otra forma de calcular el modelo cinemático directo es mediante el cálculo de todos los

parametros de la matriz de Denavit-Hartenberg(DH) y posteriormente la utilización de estos

para calcular todas las matrices de transformación O para cada uno de los eslabones. No se



- 19 -

explicará el algoritmo a seguir para obtener los parámetros de dicha matriz, para obtener más

información véase [10]. La matriz de DH para nuestro robot manipulador sería la mostrada a

continuación:

ℎ = O bO r 2⁄0− 2⁄

0 O0rOr rN

00 (25)

Ahora conocida la matriz de transformación homogénea:

O = cos'r) −sin'r)sin'r)cos'O) cos'r)cos'O) 0 bO−sin'O) −sin'O)sin'r)sin'O) cos'r)sin'O)0 0 cos'O) cos'O)0 1 (26)

Conseguimos relacionar la base con el efector final calculando las matrices homogéneas para

cada eslabón del manipulador:

Nh = Oh O N (27)

CINEMÁTICA INVERSA

Para completar el análisis se obtiene el modelo cinemático inverso que consiste en la

relación inversa del modelo cinématico directo, es decir, conocido el punto en coordenadas

cartesianas '*, ., /) del efector final del manipulador, se obtendrían el valor de ángulos de cada

una de las articulaciones. En un robot manipulador de más de dos grados de libertad las

soluciones serían múltiples, y por lo tanto, su resolución mucho más compleja, pero en este

caso, al ser un robot manipulador cuyo área de trabajo es un plano, se puede obtener un única

solución. Resolviendo el sistema ecuaciones obtenemos el siguiente resultado a nuestro

problema:

rOD = atan F *−/K − b bB c sin'r ) O + sin'r )g r D = b(@, ¡* + / − O − 2 O ¢

(28)

Donde rOD y r Drepresentan las posiciones finales de las articulaciones.



- 20 -

2.3.2 SIMULACIONES DEL MANIPULADOR

El primer problema que surge al intentar realizar simulaciones para este tipo de sistemas

es que los modelos son de tal grado de complejidad que la cantidad de cálculos a realizar es de

mucho volumen lo que provoca una ralentización, a veces, desesperante. Para solucionar este

problema, se optó por estudiar en profundidad el modelo del manipulador y los efectos que su

movimiento provocarían en el quadrotor. Dicho estudio reveló que, en modo simplificado, los

efectos del movimiento del manipulador sobre quadrotor se pueden resumir en tres:

• Variación de los momentos de inercia del sistema con respecto a los ángulos de las

articulaciones.

• Variación del centro de masas del sistema (inicialmente fijado en la base del

quadrotor) con respecto a los ángulos de las articulaciones.

• Par externo generado por la variación del centro de masas

Como vemos estos efectos dependen de la posición de las articulaciones del

manipulador, y dado que el movimiento del manipulador será sobre un único plano XZ y el

movimiento de las articulaciones está limitado en rango. Entonces, es posible generar

previamente los datos, anteriormente descritos, para que durante la simulación del sistema total

irlos "inyectando" cuando corresponda. Por lo tanto, se optó por fabricar una base de datos con

todos los valores de los efectos provocados por el manipulador que afectan al modelo del

quadrotor, Para ello se creó un conjunto de programas y funciones, con ayuda de MATLAB,

que dado el movimiento que iba a realizar el manipulador, devuelve las matrices de inercia

correspondientes al sistema total con cada posición que ocupaba el manipulador en cada uno de

los puntos de una trayectoria generada con una precisión de 0.01 rad; y además el par externo

que generaría la desviación del centro de masas del quadrotor debido a la carga descentrada que

provoca el manipulador en cada posición de la trayectoria ocupada.



- 21 -

Gracias a varias pruebas realizadas, es posible comprobar la variación de los términos

de la matriz de inercia dependiendo de la posición del brazo lo que nos puede dar una idea de en

posición el manipulador influirá más o menos en el vuelo del quadrotor.

Fig. 8. Representación del manipulador con HEMERO.



- 22 -

2.4 MODELADO DEL QUADROTOR CON MANIPULADOR

Como se ha especificado anteriormente, en este proyecto no se realizará el cálculo de un

modelo completo quadrotor más manipulador, en cambio, se desarrollará un modelo del

quadrotor mediante las formulaciones de Euler-Lagrange y Newton-Euler pero con la salvedad,

de que aquí añadiremos algunos de los efectos físicos que en este provocarían el hecho de

acoplar un manipulador a la base del mismo. Así conseguimos un modelo relativamente

completo sin entrar en la complejidad que supondría el cálculo del modelo conjunto. Las

diferencias que añadiremos a nuestro modelo de quadrotor desarrollado anteriormente serán las

siguientes:

-El sistema deja de ser simétrico. En la bibliografía encontramos que los distintos autores que se

enfrentan al modelado de un quadrotor consideran este un modelo simétrico con respecto a los

planos XZ e YZ. En nuestro caso, el manipulador rompe la simetría en el plano XZ.

-Matriz de inercia, , variable con respecto al movimiento del manipulador. En el modelo

clásico del quadrotor la matriz de inercia del sistema es fija ya que la forma del mismo no varía.

En nuestro caso, esta matriz es considerada variable con respecto a la posición que del

manipulador en cada instante de tiempo provocando que la matriz de inercia sea variable con

respecto a los ángulos que tomen las articulaciones del brazo,.

') = £') ') ')') ') ')') ') ')¤

-Se tendrán en cuenta las fuerzas o momentos que el movimiento de las articulaciones del

manipulador provoquen y afecten al quadrotor. El término U"x'), es el par externo generado

por el movimiento del manipulador y que, por consiguiente, depende de los valores angulares de

las articulaciones. El par externo es el momento generado por el desplazamiento de la posición

del centro de gravedad. Realmente, y debido a que el desplazamiento del centro de gravedad se

moverá dentro del plano de movimiento del manipulador, XZ, deberá existir además un

momento que afecte a la dinámica en yaw. Este momento se simplificó debido a que el

componente inercial sobre este eje hace que este momento deba ser muy grande para que afecte

a la estabilidad del quadrotor debido a la forma del aparato, y por la dimensión del brazo sería

imposible alcanzar. Por lo tanto, este momento solo afectará, de manera directa, a la dinámica

en pitch.



- 23 -

2.4.1 FORMULACIÓN DE EULER-LAGRANGE

Seguiremos el mismo proceso descrito anteriormente para obtener el modelo dinámico

del quadrotor, pero como hemos mencionado ahora el sistema no es simétrico con respecto al

plano YZ solo lo es respecto al plano XZ, los momentos de inercia serían variables con la

posición del manipulador y el movimiento de este provocaría un par externo que influiría sobre

uno de los ejes del quadrotor de manera directa y por efectos aerodinámicos en los demás ejes.

No se explicará en este trabajo el proceso de obtención del modelo dinámico mediante

el uso de la fórmula de Euler-Lagrange ya que en la bibliografía se pueden encontrar numerosos

ejemplos de esto. En vez de esto, expondremos el resultado de los cálculos para el modelo

dinámico del quadrotor y a este se añadirán los efectos que provocaría en su comportamiento el

manipulador más los efectos giroscópicos resultantes de las rotación de las hélices.

Fig. 9. Sistemas de coordenadas quadrotor más manipulador

E Z

X Y

F1

F3

F4

Q

F2

x y

z

Tarm1

Tarm2

mquad

mb1

mb2

mpieza Tarm3

B1

B2

B3 xf

yf

zf



- 24 -

CINEMÁTICA

Para cualquier punto de la estructura del quadrotor expresado en el sistema de coordenadas fijo

en tierra, resulta que (con c: coseno, s: seno).

#$% = '(()* + '(,, − ,(). + '(,( + ,()/$0 = ',()* + ',,, + ((). + ',,( − (,)/$ = '−,)* + '(,). + '(()/ 1 (29)

Las correspondientes velocidades se obtendrían mediante la derivada de (2.1), y la expresión

que corresponde al cuadrado de la velocidad para cada punto viene dado por:

= % + 0 + 2 (30)

ENERGÍA

De la ecuación (2.2) obtenemos la expresión para la energía cinética:

= 125. + / '6)'$) ∙ 8 − 2 sin + sin = +125/ + * '6)'$) ∙ 8 cos + 2 sin cos cos + sin cos = +125* + . '6)'$) ∙ 8 sin − 2 sin cos cos + cos cos = +5*.'6)'$) ∙ 8 sin sin cos + 8cos sin − sin cos = + cos= +5/*'6)'$) ∙ 8 cos sin cos + 8− cos cos − sin sin= + sin= +5./'6)'$) ∙ 8− sin cos cos + 8sin cos − cos cos= + sin =

(31)

Y la expresión para la energía potencial es:

= 5'− sin · * + sin cos · . + cos cos · /)'$) = 5*'*) · '− sin ) +5.'.) · ' sin cos ) +5/'/) · ' cos cos)

(32)



- 25 -


Usando el Lagrangiano y la fórmula de su derivada para las ecuaciones de movimiento:

= − , Γ = DDE F GHGI JK − GHGIJ (33)

Dado que, como se ha mencionado antes, el sistema no es simétrico, las tres ecuaciones

de movimiento serían:

L = 8 − = + 8 − = − 8 + = + 8 − = = ' − ) − 8 + = + 8 − = + 8 − = = 8 − = + 8 − = + 8 − = − 8 + = 1 (34)

De la diferencia de empuje de cada par de motores se producen los diferentes pares que actuán

sobre el quadrotor:

L = 'ΩM −Ω ) = 'ΩN −ΩO ) = 'ΩO −Ω +ΩN −ΩM )1 (35)

Además, habría que añadir los efectos giroscópicos producidos pos la rotación de las hélices:

P′ = JΩ′ = JΩ 1 (36)

Donde:

Ω =Ω +ΩM −ΩO −ΩN (37)

MODELO DINÁMICO

El modelo dinámico del quadrotor con un manipulador fijado en la base del mismo que

describe el movimiento en pitch,roll y yaw con una matriz de inercia que pierde la simetría y

que varía dependiendo de la posición del manipulador sería:

L = 8 − = + 8 − = − 8 + = + 8 − = − JΩ + = ' − ) − 8 + = + 8 − = + 8 − = − JΩ + + U"x') = 8 − = + 8 − = + 8 − = − 8 + = + 1 (38)



- 26 -

2.4.2 FORMULACIÓN DE NEWTON-EULER

La dinámica sería parecida a la del quadrotor pero, en este caso, habría que añadir la matriz de

inercia variable con los ángulos de las articulaciones del manipulador y el par externo

provocado por el movimiento de estas:

+Ω × ') = S"TS + UV"T + W"UX ')Ω +Ω × 'J')Ω) = TZ[Z + T\][ + T\¥'γ) (39)

En resumen, las fuerzas aerodinámicas y los momentos serían los mismos a excepción de que, al

perder la simetría, aparecerían momentos cruzados y un par externo provocado por las dos

primeras articulaciones del brazo. El par provocado por la tercera articulación se supondrá nulo

al ser muy pequeño y no influir apenas en el análisis. Por lo tanto, las ecuaciones de

movimiento se obtienen utilizando todos lo momentos y fuerzas obtenidos anteriormente más

los introducidos por el movimiento de manipulador serían:

= 8 − = + 8 − = − 8 + = + 8 − = +

+JΩ + '− +M) − ℎ tMO + '−1)O6x

MO = ' − ) − 8 + = + 8 − = + 8 − = −

−JΩ + 'O−N) + ℎ tMO +'−1)O6x

MO + U"x')

= 8 − = + 8 − = + 8 − = − 8 + = ++"Ω " + '−1)pM

O + 't − tM) + 8−tO + tN=/ = − '(@,(@,)M

O* = ',AB,AB + (@,,AB(@,)M

O −tMO − 12`_* |* |

. = '−(@,,AB + ,AB,AB(@,)MO −t

MO − 12`_. |. |

1

(40)


SISTEMA DE CONTROL

- 27 -

3. SISTEMA DE CONTROL


En este capítulo, primero se realizará una simplificación del modelo de quadrotor más

manipulador desarrollado en el apartado anterior. A continuación, se adaptará el modelo para

adecuarlo a las técnica de control principal desarrollada en este proyecto que ha sido bautizada

como Variable Parameter Integral Backstepping. Pero primero, se introducirá otra técnica de

control, usada en las posteriores simulaciones, como es el PID en cascada. La elección de un

PID en cascada como técnica de control de un quadrotor se tomo, primero, porque se adapta

especialmente bien la estructura en cascada del quadrotor y, segundo, porque su buen

funcionamiento y su fácil implementación hace que sea muy usada como controlador en

quadrotors comerciales. Por lo tanto, la técnica PID en cascada es ideal como base comparativa

para comprobar si la técnica aquí presentada es eficaz.

Variable Parameter Integral Backstepping es una técnica de control basada en Integral

Backstepping, la cual se puede ver implementada en quadrotors en [11]. Por lo tanto , se vio

conveniente explicar de forma detallada el proceso y desarrollo por el cual se obtienen las leyes

de control para cada uno de los controladores que gobiernan el quadrotor. Como se ha

comentado anteriormente, el objetivo principal del proyecto era el desarrollo de una técnica de

control adaptada a nuestro sistema, es por eso que surgió la necesidad de esta nueva técnica de

control especialmente diseñada para nuestro sistema de quadrotor con manipulador acoplado a

la base.

En conclusión, en este capítulo, después de detallar el modelo para el control, se

describirán, desarrollaran y simularan cada una de las técnicas de control, PI en cascada,

Integral Backstepping y Variable Parameter Integral Backstepping. Aunque se dejará para un

último apartado del capítulo para el análisis en profundidad del comportamiento ante distintas

situaciones de los tres controladores, el cual será llevado a cabo enfrentándolos a distintas

simulaciones.


SISTEMA DE CONTROL

- 28 -

3.2 MODELO PARA EL CONTROL

El modelo desarrollado en el apartado anterior, describe las ecuaciones diferenciales del

sistema. Se hace necesario debido a la complejidad de cálculo de los diferentes términos que

componen el modelo una simplificación del mismo. Por lo tanto, los momentos de arrastre y de

balanceo no serán considerados y los coeficientes de empuje y arrastre se supondrán constantes.

El sistema puede ser rescrito como vector de estado n = ¦'n, ) donde es el vector de

entrada y n el vector de estado que contendrá las siguientes variables:

n = [ //** .. ]^ (41)

1*O = * = *O = *N = © 1*M = *N = *ª = *« = *ª = © 1*¬ = /* = *¬ = /*® = * ¯ *Oh = *® = **OO = .*O = *OO = . (42)

= [O NM]^ (43)

Donde el vector de entrada puede rescribirse con respecto a las velocidades de los rotores de

la siguiente forma:

UO = K²'ΩO +Ω +ΩN +ΩM )U = K²'ΩM −Ω )UN = K²'ΩN −ΩO )UM = K³'Ω +ΩM −ΩO −ΩN )

1 (44)

Donde K² es el factor de empuje y K³ es el factor de arrastre.

Si suponemos que el quadrotor está en hover entonces podemos suponer que la matriz

de rotación es unitaria por lo tanto las velocidades de rotación del quadrotor con respecto al

sistema global y con respecto al sistema serán:


SISTEMA DE CONTROL

- 29 -

¦'n, ) =

´µµµµµµµµµµµµµ¶

bO + 8 − =b − 8 + =bN + 8 − =bM + bªΩ + O O − 8 + = + 8 − =N + 8 − =M −ªΩ + N + U"x')(O + 8 − =( + 8 − =(N − 8 + =(M + NM/ − cos cos O*sin sin + cos sin cos O.sin sin cos − sin cos O ·¸¸¸¸¹

(45)

Donde los vectores b') = [bO, b , bN, bM, bª], ') = [O, , N, M, ª], (') = [(O, ( , (N, (M] y ') = [O, , N] varían con la posición del manipulador ya que se obtendrían mediante los

términos de la matriz de inercia ') del sistema, la cual, es variable con los ángulos de las

articulaciones del manipulador.

1bO = 8 − =b = bN = bM = bª = " º

ººº1O = ' − ) = N = M = ª = " º

ººº1(O = 8 − =( = (N = (M = º

ºº O = = N = 1

(46)

Y Ω es una perturbación que puede ser obtenida mediante las velocidades angulares de los

cuatro rotores de la siguiente forma:

Ω =Ω +ΩM −ΩO −ΩN (47)

Se puede ver, en el sistema descrito anteriormente, como los ángulos y sus derivadas en el

tiempo no dependen de la traslación del sistema. Mientras que las ecuaciones que describen la

traslación del sistema si dependen de los ángulos. Por lo tanto, se podría imaginar el sistema

como formado por dos subsistemas uno de traslación que depende exclusivamente de las


SISTEMA DE CONTROL

- 30 -

entradas , N y M; y un subsistema de traslación de que dependería de la entrada O(empuje)

y de los ángulos.

Fig. 10. Conexión de los subsistemas de rotación y traslación


SISTEMA DE CONTROL

- 31 -

3.3 CONTROLADOR PID

Para el diseño de este controlador, tomaremos el par externo generado por el

manipulador como una perturbación externa y los valores de las matrices de inercia fijos aunque

respetando la no simetría del modelo, ya que nos ayudaran al ajuste de los distintos

controladores PID del sistema. No se detallarán las fórmulas de los controlades PID ya que se

considera que existe bastante información en bibliografía. Se puede representar la estructura del

control PID en cascada de forma esquemática en la figura 11. Esta será la misma que lleva

implementada Arducopter en su firmware predefinido:

Fig. 11. Estructura PID con estructura en cascada para el control sobre ángulo roll implementada en el firmware del Arducopter.

En este tipo de control, la velocidad angular deseada es calculada mediante el error de

ángulo multiplicado por la ganancia proporcional. El error de velocidad angular correspondiente

sería la entrada de un controlador PID, su salida es la señal de control más la parte integral del

error angular. Con este tipo de estructura de control se aprovecha la estructura en cascada del

quadrotor.

Nota: A lo largo de este trabajo, para referirse al control PID con estructura en cascada,

utilizaremos nombres con PID en cascada, PI cascade (gráficas) o simplemente PID.


SISTEMA DE CONTROL

- 32 -

SIMULACIONES

Se realizó un modelo simulink con el controlador PID en cascada y modelo del

quadrotor más el manipulador. En una primera simulación, se optó por probar la respuesta del

controlador ante el quadrotor con el manipulador, sin moverse y completamente extendido. Se

optó por esta posición debido a que en los experimentos con el quadrotor Arducopter más el

prototipo del manipulador, este debía estar completamente extendido a la hora del despegue.

En la figura 12, podemos observar la respuesta del control de actitud ante un salto de

más de 20 grados. La simulación se ha llevado a cabo introduciendo un error angular en la IMU

de un máximo de 0.02 rad y una frecuencia de refresco de 50 Hz; con el manipulador

completamente extendido acoplado a la base del quadrotor pero sin movimiento alguno.

Fig. 12. Respuesta del controlador PID de actitud de los ángulos roll, pitch y yaw.

Se diseña también un control de altura y otro de posición mediante la técnica de PID con

estructura en cascada. En la siguiente simulación, el quadrotor se pondrá a una altura de 1

metro, el despegue será desde un punto (2,3) y se desplazará a (0,0). La IMU simulada tendrá

una frecuencia de refresco de 50 Hz y un error angular menor de 1.2 grados. El GPS tendrá una

frecuencia de refresco de 10 Hz y un error en la medida menor de 0.05 metros. Se observan los

siguientes resultados:


SISTEMA DE CONTROL

- 33 -

Fig. 13. Respuesta de los controles PID de altitud y posición

En la figura 13. podemos ver la respuesta de los controles PID en cascada de posición y

altitud. La simulación se ha llevado a cabo introduciendo un error angular en la IMU de un

máximo de 0.02 rad y una frecuencia de refresco de 50 Hz; Para el GPS, la frecuencia de

refresco sería de 10 Hz y el error de 5 cm. Durante la simulación el brazo permaneció en

posición completamente extendido y sin moverse. Se podría decir que esta simulación

correspondería al despegue del UAV a la vez que se dirige a una posición inicial.

Fig. 14. Representación 3D de la trayectoria seguida


SISTEMA DE CONTROL

- 34 -

3.4 CONTROLADOR INTEGRAL BACKSTEPPING

Durante la búsqueda del control ideal para nuestro sistema de quadrotor más

manipulador, se estudiaron varias técnicas de control que habían sido antes implementadas para

el control de quadrotors: PID, LQR, fuzzy controllers, etc. Después de una evaluación y tras el

estudio de sus características se decidió utilizar un control integral backstepping(IB) que había

sido desarrollado antes en [11] para un quadrotor. Como se explica en [11], la idea de usar la

técnica de backstepping es debido a que esta permite una rechazo eficaz ante perturbaciones

fuertes aunque la estabilización en hover es delicada, para mejorar esto se introduce una acción

integral. La idea principal, era mejorar aún más, la ya de por si buena, respuesta que se obtiene

con el control PID. Al igual que la técnica de PID, la técnica de Backstepping se adapta muy

bien a la estructura en cascada de la dinámica del quadrotor. Además, este método garantiza

estabilidad asintótica y robustez ante algunas incertidumbres y, gracias a la acción integral,

cancelación de los errores en régimen permanente. Se desarrollará un control de actitud, altitud

y posición con esta técnica. En este caso, supondremos que el manipulador es una perturbación

que actúa sobre el quadrotor. Por lo tanto, no tendremos en cuenta en el modelo empleado para

el desarrollo del controlador los cambios que sobre este ejerce el hecho del acoplamiento a la

base de un manipulador.

Control de Actitud

El control de actitud es el principal conductor de quadrotor, ya que es el principal encargado de

la estabilidad y de la orientación del sistema. Se explicará el desarrollo de la obtención de la ley

de control roll. Para pitch y yaw el desarrollo es similar y por lo tanto se obviará. El primer paso

en el diseño de nuestro controlador es definir el error en roll O = D − y su dinámica:

O = D − » (48)

Crearemos un control virtual de la velocidad angular », que en este caso, no es una entrada de

control. Por lo tanto, nuestra velocidad angular deseada se define de la siguiente forma:

»D = OO + D + kO¼O (49)


SISTEMA DE CONTROL

- 35 -

siendo O y kO constantes positivas y ¼O = ½ O')¾Eh la integral del error en roll.

Como se ha definido un error para la velocidad angular, se calcula su dinámica de la

siguiente forma:

= O8D − »= + D + kOO − (50)

Donde , el error en la velocidad angular es definido por:

= »D − » (51)

Gracias a las ecuaciones (49) y (51) se puede rescribir la dinámica del error en roll como:

O = −OO − kO¼O + (52)

Ahora, se reemplaza en (50) por la ecuación en el modelo (19), y entonces, vemos como

aparecería nuestra señal de control en (53) :

= O8D − »= + D + kOO − bO − bªΩ − O (53)

Si combinamos el error O, el error de la velocidad angular y del término integral del error

angular ¼O, obtanemos:

= O'−OO − kO¼O + ) + D + kOO − bO − bªΩ − O (54)

Despejando la entrada de control obtenemos, finalmente:

= + 1O ['1 − O + kO)O + 'O + ) − OkO¼O + D − bO − bªΩ] (55)

Donde es una constante positiva que determina la velocidad de convergencia del bucle de la

velocidad angular. De forma similar se obtendrían los controles para pitch y yaw:


SISTEMA DE CONTROL

- 36 -

N = + 1 ['1 − N + k )N + 'N + M)M − Nk ¼ + D − O + ªΩ] (56)

M = + 1N [81 − ª + kN=ª + 'ª + «)« − ªkN¼N + D − (O] (57)

Con 'N, M, ª, «, k , kN)> 0, y '¼ , ¼N) los términos integrales de los errores angulares de

pitch y yaw respectivamente.

Control de Altitud

El control en altitud es el encargado de que la distancia del quadrotor al suelo sea la dada por

un valor deseado. En nuestro simulador, supondremos que la altura viene dada por un GPS. Para

desarrollar la ley de control en altitud, comenzaremos por la definición del error en altitud,

donde el efecto tierra es despreciado:

¬ = /D − / (58)

La dinámica del error viene dada por:

= ¬¬ + /D + kM¼M − / (59)

Luego entonces la ley de control será:

O = cos cos [ + '1 − ¬ + kM)¬ + '¬ + ) − ¬kM¼M] (60)

donde '¬, , kM) son constantes positivas.

Control en Posición

El control de posición es el encargado de que el quadrotor este sobre el punto deseado. Es

significa, sobre el punto '*, .) del plano horizontal con respecto a nuestro sistema de

referencias global donde el origen sería el punto de comienzo. El movimiento horizontal se

logra mediante la orientación del vector de empuje en la dirección de movimiento deseado. Esto

se realiza mediante la rotación del propio quadrotor. En la práctica, se realiza el control de

posición por balanceo o cabeceo del quadrotor como respuesta a un punto de referencia


SISTEMA DE CONTROL

- 37 -

dado'*D , .D) . Por lo tanto, el control de posición genera los valores deseados de D y D que

serán seguidas por el controlador de actitud.

Se define entonces la ley de control usando la técnica de IB comenzado por definir los errores

en posición para * e . de la siguiente forma:

P ® = *D − *OO = .D − .1 (61)

La dinámica del error será:

P ® = ®® + *D + kª¼ª − *OO = OOOO + .D + k«¼« − . 1 (62)

La ley de control se podrá definir como:

L = O ['1− ® + kª)® + '® + Oh)Oh − ®kª¼ª] = − O ['1− OO + k«)OO + 'OO + O )O − OOk«¼«]1 (63)

donde '®, Oh, OO, O , kª, k«) son constantes positivas.


SISTEMA DE CONTROL

- 38 -

Simulaciones

Estas primeras simulaciones que se realizaron con el control Integral Backstepping,

serán iguales que las que se realizaron con el control PID. Primero veremos al respuesta del

control de actitud desacoplado para un salto de más de 20 grados con unas características en la

simulación de la IMU de un error angular de un máximo de 0.02 rad y una frecuencia de

refresco de 50 Hz. La siguiente figura muestra los resultados obtenidos:

Fig. 15. Respuesta del controlador IB de actitud de los ángulos roll, pitch y yaw

Se diseña también un control de altura y otro de posición mediante la técnica de Integral

Backstepping, cuya ley de control hemos desarrollado anteriormente. En la siguiente

simulación, el quadrotor se pondrá a una altura de 1 metro, el despegue será desde un punto

(2,3) y se desplazará a (0,0). La IMU simulada tendrá una frecuencia de refresco de 50 Hz y un

error angular menor de 1.2 grados. El GPS tendrá una frecuencia de refresco de 10 Hz y un

error en la medida menor de 0.05 metros. Se observan los siguientes resultados:


SISTEMA DE CONTROL

- 39 -

Fig. 16. Respuesta de los controles IB de altitud y posición

En la figura 16. podemos ver la respuesta de los controles IB de posición y altitud. La

simulación se ha llevado a cabo introduciendo un error angular en la IMU de un máximo de

0.02 rad y una frecuencia de refresco de 50 Hz; Para el GPS, la frecuencia de refresco sería de

10 Hz y el error de 5 cm. Durante la simulación el brazo permaneció en posición completamente

extendido y sin moverse. Se podría decir que esta simulación correspondería al despegue del

UAV a la vez que se dirige a una posición inicial.



SISTEMA DE CONTROL

- 40 -

3.5 CONTROLADOR VARIABLE PARAMETER INTEGRAL BACKSTEPPING

Si tenemos en cuenta que, gracias a que el brazo se mueve, la matriz de inercia de

nuestro sistema es variable con respecto a los ángulos del brazo, y que es posible estimar

nuestra nueva matriz de inercia con cada movimiento y a su vez calcular el par externo que el

manipulador provocaría. Entonces podemos readaptar nuestro controlador introduciendo estas

variables medibles como parámetros de nuestro controlador en vez de dejarlas como

perturbaciones del sistema, y así mejorar nuestra respuesta. A este nuevo controlador con

parámetros variables le llamaremos Variable Parameter Integral Backstepping (VPIB). Dado

que, siguiendo la técnica IB, estos nuevos parámetros de nuestro controlador afectarían

únicamente, de forma directa, al control en actitud, desarrollaremos solamente el control en

actitud ya que el control en altitud y posición permanecerían igual.

Control de Actitud

El control de actitud es el principal conductor de quadrotor, ya que es el principal encargado de

la estabilidad y de la orientación del sistema. Se explicará el desarrollo de la obtención de la ley

de control roll. Para pitch y yaw el desarrollo es similar y por lo tanto se obviará. El primer paso

en el diseño de nuestro controlador es definir el error en roll O = D − y su dinámica:

O = D − » (64)

Crearemos un control virtual de la velocidad angular », que en este caso, no es una entrada de

control. Por lo tanto, nuestra velocidad angular deseada se define de la siguiente forma:

»D = OO + D + kO¼O¬ = /D − / (65)

siendo O y kO constantes positivas y ¼O = ½ O')¾Eh la integral del error en roll.

Como se ha definido un error para la velocidad angular, se calcula su dinámica de la

siguiente forma:

= O8D − »= + D + kOO − (66)


SISTEMA DE CONTROL

- 41 -

Donde , el error en la velocidad angular es definido por:

= »D − » (67)

Gracias a las ecuaciones (65) y (67) se puede rescribir la dinámica del error en roll como:

O = −OO − kO¼O + (68)

Ahora, se reemplaza en (66) por la ecuación en el modelo (40), y entonces, vemos como

aparecería nuestra señal de control en (69) :

= O8D − »= + D + kOO − bO − 8 − =b + 8 + =bN − 8 − =bM− bªΩ − O (69)

Si combinamos el error O, el error de la velocidad angular y del término integral del error

angular ¼O, obtenemos:

= O'−OO − kO¼O + ) + D + kOO − bO − 8 − =b + 8 + =bN− 8 − =bM − bªΩ − O (70)

Despejando la entrada de control obtenemos, finalmente:

= 1O À'1 − O + kO)O + 'O + ) − OkO¼O +D − bO − 8 − =b + 8 + =bN− 8 − =bM − bªΩÁ (71)

Donde es una constante positiva que determina la velocidad de convergencia del bucle de la

velocidad angular. De forma similar se obtendrían los controles para pitch y yaw:

N = 1 À'1 − N + k )N + 'N + M)M − Nk ¼ + D − O + 8 + = − 8 − =N− 8 − =M +ªΩ − U"x')Á (72)


SISTEMA DE CONTROL

- 42 -

4 = 1N [81− 52 + k3=5 + '5 + 6)6 − 5k3¼3 + D − (1 − 82 − 2=(2 − 8 − =(3

+ 8 + =(4] (73)

Con '3, 4, 5, 6, k2, k3)> 0, y '¼2, ¼3) los términos integrales de los errores angulares de

pitch y yaw respectivamente. Además, añadimos que los términos b, , (, y U"x') son

variables dependientes de la posición del manipulador.

Simulaciones

Estas primeras simulaciones que se realizaron con el control Variable Parameter Integral

Backstepping, serán iguales que las que se realizaron con el control PID y el control IB. Primero

veremos al respuesta del control de actitud desacoplado para un salto de más de 20 grados con

unas características en la simulación de la IMU de un error angular de un máximo de 0.02 rad y

una frecuencia de refresco de 50 Hz. La siguiente figura muestra los resultados obtenidos:

Fig. 18. Respuesta del controlador VPIB de actitud de los ángulos roll, pitch y yaw

Se diseña también un control de altura y otro de posición mediante la técnica de Variable

Parameter Integral Backstepping, cuya ley de control hemos desarrollado anteriormente. En la

siguiente simulación, el quadrotor se pondrá a una altura de 1 metro, el despegue será desde un

punto (2,3) y se desplazará a (0,0). La IMU simulada tendrá una frecuencia de refresco de 50 Hz


SISTEMA DE CONTROL

- 43 -

y un error angular menor de 1.2 grados. El GPS tendrá una frecuencia de refresco de 10 Hz y

un error en la medida menor de 0.05 metros. Se observan los siguientes resultados:

Fig. 19. Respuesta de los controles VPIB de altitud y posición

En la figura 19 podemos ver la respuesta de los controles VPIB de posición y altitud. La

simulación se ha llevado a cabo introduciendo un error angular en la IMU de un máximo de

0.02 rad y una frecuencia de refresco de 50 Hz; Para el GPS, la frecuencia de refresco sería de

10 Hz y el error de 5 cm. Durante la simulación el brazo permaneció en posición completamente

extendido y sin moverse. Se podría decir que esta simulación correspondería al despegue del

UAV a la vez que se dirige a una posición inicial.



SISTEMA DE CONTROL

- 44 -

3.6 SIMULACIONES

Se han realizado una serie de simulaciones con el objetivo de comparar cada una de las técnicas

de control desarrolladas en este proyecto. Se evaluará la respuesta de los distintos controladores

ante diferentes situaciones de vuelo. En total se expondrán tres casos distintos. Para cada uno, se

mostrarán las graficas que nos muestran la evolución en Roll, Pitch y Yaw, así como para el

desplazamiento sobre los ejes X e Y; y la altura en Z.

3.6.1 SIMULACIÓN 1

En la simulación 1, el quadrotor subirá a una altura de 1 metro. Después, el manipulador pasará

de estar completamente extendido en horizontal [r1= 0°, r2 = 0°] a una posición vertical

completamente extendido [r1 = −90°, r2= −90°]. Una representación gracias a HEMERO de

este movimiento podemos verla en la siguiente figura:

Fig. 21. Representación 3D movimiento manipulador


SISTEMA DE CONTROL

- 45 -

Este movimiento se ha escogido debido a que se alcanzarían los valores máximos en el par

externo. Por lo tanto es un buen indicador de la influencia del manipulador en la estabilidad del

quadrotor y, por consiguiente, es ideal para comprobar la respuesta de las diferentes técnicas de

control. La simulación tendrá una duración de 30 segundos y el manipulador empezará a

moverse a los 10 segundos. El manipulador realizará su movimiento durante los próximos 5

segundos.

La IMU tendrá una frecuencia de refresco de 50 Hz y el GPS de 10 HZ. Los errores en las

medidas de los sensores serán de un error angular en la IMU de un máximo de 0.02 rad y para el

GPS el error será de 5 cm.

Fig. 22. Evolución del par externo durante el movimiento del manipulador

En la figura 22, podemos ver la evolución de los valores desarrollados por el par externo que

provoca el manipulador al realizar el movimiento escrito anteriormente.

A continuación, veremos la respuesta del quadrotor con cada uno de los controladores

desarrollados anteriormente al realizar este un despegue, situarse a una altura de un metro y

realizar el manipulador el movimiento ya descrito.


SISTEMA DE CONTROL

- 46 -

Para el controlador PID obtenemos los siguientes resultados:

Fig. 23. Comportamiento en Roll, Pitch y Yaw con el controlador PID.

Fig. 24. Comportamiento de Altura y posición XY con el controlador PID.


SISTEMA DE CONTROL

- 47 -

Como podemos observar, el controlador PID no es capaz de corregir el efecto provocado por el

par externo de manera precisa e inmediata, como sería deseado, aunque sí es cierto, que la

desviación en eje X provocada por el movimiento del brazo lentamente es capaz de corregirla.

El error creado en el eje X debido al desplazamiento del manipulador alcanza su máximo en -

0.5688 metros

Fig. 25. Punto error máximo en eje X.


SISTEMA DE CONTROL

- 48 -

Para el controlador Integral Backstepping obtenemos los siguientes resultados:

Fig. 26. Comportamiento de Roll, Pitch y Yaw con el controlador IB.

Fig. 27. Comportamiento en Altura y posición XY con el controlador IB.


SISTEMA DE CONTROL

- 49 -

Como podemos observar, el resultado con IB es bastante mejor que en el PID, aunque el

desplazamiento sobre el eje X que provoca el movimiento del manipulador no es despreciable sí

podría ser corregido mediante un ajuste con el manipulador para así mantener la posición en el

efector final.

El error creado en el eje X debido al desplazamiento del manipulador alcanza su máximo en -

0.07657 metros



SISTEMA DE CONTROL

- 50 -

Para el controlador Variable Parameter Integral Backstepping obtenemos los siguientes

resultados:

Fig. 29. Comportamiento en Roll, Pitch y Yaw con el controlador VPIB.

Fig. 30. Comportamiento Altura y posición XY con el controlador VPIB.


SISTEMA DE CONTROL

- 51 -

En este caso, la corrección es total y el desplazamiento que provocaría el manipulador es

inapreciable.

Conclusiones simulación 1

En la figura 6.8. podemos observar la capacidad de mantenerse sobre un punto prefijado del

plano XY del quadrotor en vuelo hover a una altura de un metro, durante la simulación anterior:

Fig. 31. Comparación PID vs. IB vs. VPIB. en plano XY.

Como podemos observar el que nos ofrece mejor rendimiento es el controlador Variable

Parameter Integral Backstepping.


SISTEMA DE CONTROL

- 52 -

3.6.2 SIMULACIÓN 2

En la simulación 2, el quadrotor subirá a una posición de un metro para después ir a otra

aleatoria, a continuación, se realizará el movimiento del brazo definido anteriormente en la

simulación 1, que será un movimiento de arranque del manipulador, para luego realizar el

movimiento inverso.


El quadrotor empezará en el punto (0,0,0). A continuación, despegará y se irá a una altura de un

metro, para luego desplazarse al punto (2,1,2). Allí el manipulador ejecutará dos movimientos

de forma continua a partir de los 20 seg.:

1. r1[0°→ −90°].r2[0° → −90°] 2.r1[−90°→ 0°].r2[−90°→ 0°]


SISTEMA DE CONTROL

- 53 -



Fig. 34. Comportamiento en Altura y posición XY con el controlador PID.


SISTEMA DE CONTROL

- 54 -

Como podemos observar, en esta simulación el desplazamiento provocado por el manipulador

es mayor que en la simulación 1. Aunque, de nuevo, al cabo de 15 segundos desde que se inicio

el movimiento del manipulador, el quadrotor es capaz de corregir el error provocado y volver a

la posición deseada.



SISTEMA DE CONTROL

- 55 -


Fig. 36. Comportamiento en Roll, Pitch y Yaw con el controlador IB.



SISTEMA DE CONTROL

- 56 -

Como podemos observar, en esta simulación el desplazamiento provocado por el manipulador

es mayor que en la simulación 1. Aunque, de nuevo, al cabo de 10 segundos desde que se inicio

el movimiento del manipulador, el quadrotor es capaz de corregir el error provocado y volver a

la posición deseada.



SISTEMA DE CONTROL

- 57 -


resultados:



SISTEMA DE CONTROL

- 58 -

Fig. 40. Comportamiento en Altura y posición XY con el controlador VPIB.

3.6.3 SIMULACIÓN 3

En la simulación 3, el quadrotor subirá a una posición de un metro con el brazo completamente

extendido en posición horizontal r1[0°].r2[0°]. Previamente se le ha fijado la posición donde

debe recoger una pieza, en este caso será [* = 2, . = 2, / = 2]. El quadrotor calculará donde

debe colocar su base para poder alcanzar la pieza, de manera que el efector final del

manipulador este en el punto de recogida exactamente. Por lo tanto, el manipulador se moverá a

la vez que el quadrotor se traslada al punto ideal.


El quadrotor empezará en el punto (0,0,0). A continuación, despegará y se irá a una altura de un

metro, para luego desplazarse al punto (1.5897,2, 2.3922) que es el punto donde el manipulador

tiene alcance para posicionar su efector final al punto (2,2,2). El quadrotor y el manipulador

ejecutaran sus movimientos a partir de los 20 segundos de la simulación.

1. r1[0°→ −90°].r2[0° → −90°] 2.r1[−90°→ −60°].r2[−90°→ −30°]


SISTEMA DE CONTROL

- 59 -



Fig. 43. Comportamiento en Altura y posición XY con el controlador PID.


SISTEMA DE CONTROL

- 60 -

Como podemos observar, en esta simulación que, como ocurre en la simulación 2, los

movimientos continuos del manipulador hacen que la oscilación sea mayor, aunque al final se

alcanza la posición deseada.


Además estas oscilaciones en el eje X provocan por acoplamiento oscilaciones en el eje Y. Si

vemos ahora la evolución de la posición del efector de final del manipulador en los ejes X y Z :

Fig. 45. Posición efector final.


SISTEMA DE CONTROL

- 61 -

Se puede observar una respuesta curiosa en este sistema, y es que pese a que existe un

escalón de avance sobre X, la respuesta del sistema es precisamente la contraria. Esto es debido

a que se produce el escalón durante el movimiento del manipulador. El movimiento de este,

provoca un momento sobre pitch que se acopla a la vez con los escalones en Y y Z. El

controlador PID no es capaz de responder suficientemente rápido, luego el sistema retrocede en

su posición en X para poco a poco responder a ese momento y corregirlo. El controlador PID,

por lo tanto, no es capaz de anular esos momentos de forma rápida.


SISTEMA DE CONTROL

- 62 -


Fig. 46. Comportamiento en Roll, Pitch y Yaw con el controlador IB.



SISTEMA DE CONTROL

- 63 -

Como podemos observar, en esta simulación que, como ocurre en la simulación 2, los

movimientos continuos del manipulador hacen que la oscilación sea mayor, aunque al final se

alcanza la posición deseada.


Además estas oscilaciones en el eje X provocan por acoplamiento oscilaciones en el eje Y. Si

vemos ahora la evolución de la posición del efector de final del manipulador en los ejes X y Z :



SISTEMA DE CONTROL

- 64 -

Como podemos observar, en este caso el controlador sí que es capaz de corregir más

rápidamente que el controlador PID. Por lo que no se produce ese retroceso sobre X, contrario al

avance que si se producía con el controlador PID. Sin embargo, la estabilización no es

instantánea tampoco en este controlador y se producen periodos oscilaciones hasta que el

controlador es capaz estabilizarse sobre la posición deseada.


SISTEMA DE CONTROL

- 65 -


resultados:


Fig. 51. Comportamiento en Altura y posición XY con el controlador VPIB.


SISTEMA DE CONTROL

- 66 -

Si vemos ahora la evolución de la posición del efector de final del manipulador en los ejes X y

Z :


Como vemos, con el controlador Variable Parameter Integral Backstepping el efector

final consigue situarse en el punto deseado con un error mínimo. Y los momentos provocados

por el movimiento del manipulador son rechazados de manera rápida y eficaz, no alterando la

estabilidad del quadrotor.


EXPERIMENTOS

- 67 -

4. EXPERIMENTOS


Para poder realizar experimentos se optó por la adquisición de un quadrotor Arducopter

al que se le acopló en la base un manipulador robótico de 2 grados de libertad.

Fig. 53. Arducopter con manipulador acoplado a la base

A continuación, se explicará brevemente que es la plataforma arducopter, cuáles son sus

componentes y características principales, estructura del firmware, problemas que han surgido a

la hora de reprogramar el firmware de arducopter con el controlador, etc. Posteriormente, se

detallarán los experimentos realizado los cuales fueron todos al aire libre, aunque solo se

mostrarán los resultados de los mejores resultados obtenidos para así poder realizar un

comparación de los controladores implementados, PID y Variable Parameter Integral

Backstepping.


EXPERIMENTOS

- 68 -

4.2 INTRODUCCIÓN A ARDUCOPTER

Arducopter es una plataforma para multirotores y hélicopteros basado en el piloto

automático Ardupilot Mega, creado por DIY Drones. Es fácil de configurar y de volar y va

mucho más allá de los multirotores básicos radiocontrol que se encuentran en el mercado hoy en

día.

A diferencia de los multirotores por radiocontrol del mercado, el ArduCopter es una

solución completa para vehículos aéreos no tripulados. Ofrece tanto la posibilidad de vuelo por

control remoto como la de vuelo autónomo, incluyendo seguimiento de puntos de interés

(waypoints), planificación de misiones ó telemetría mostrada en una estación terrestre de gran

alcance.

Las características principales del ArduCopter son:

• Es el mejor de su clase en modo de vuelo por radiocontrol, incluyendo los modos de

control por nivel y por altitud. Tiene un impresionante modo de "vuelo sencillo" que lo

convierte en uno de los multirotores más fáciles de volar. Simplemente manejando el

radiocontrol, el piloto automático busca la forma de orientarse sea cual sea su posición

de partida, utilizando el magnetómetro de a bordo.

• Tiene la opción de poder mantener la posición en vuelo, utilizando el GPS y los

sensores de altura.

• Se pueden planificar todas las misiones con una conexión inalámbrica de dos vías.

Programar waypoints, cambiar el modo de vuelo, incluso cambiar las ganancias de los

parámetros de control, todo ello se puede hacer desde un ordenador, teniendo el

multirótor en el aire.

• Con el uso de sensores de sónar, se pueden automatizar los aterrizajes y despegues, y se

puede incluso mantener el vuelo a ras del suelo. Sin el sonar, se puede mantener la

altitud sin muchos problemas para alturas mayores a tres metros.

• Los planes de misiones se pueden programar con cientos de puntos de referencia,

simplemente haciendo click sobre el mapa en 3D de la interfaz gráfica de la estación de

tierra. El multirótor sigue los puntos dados y vuelve al origen al terminar la ruta.


EXPERIMENTOS

- 69 -

• Se pueden implementar controles de la cámara, si se tiene una cámara a bordo, y

mantenerla fija apuntando a un objeto del suelo.

• No se necesita programar nada. Se dispone de una interfaz de escritorio fácil de usar,

con despliegues visuales, una estación de tierra completa, y un planificador de misiones.

• Compatible con Windows, Mac y Linux. En Windows se tiene la utilidad de

configuración gráfica de Planificación de Misiones y para el resto de sistemas

operativos, se dispone de una interfaz de línea de comandos. Una vez que el

ArduCopter está configurado, se puede elegir entre tres estaciones de tierra, incluyendo

QGroundControl, que funciona en los tres sistemas operativos.

• Compatible con las normas líderes en la industria de la robótica, como el “Willow

Garages's Robot Operating System” o el protocolo de comunicaciones MAVLink. Así se

asegura seguir en la vanguardia de la robótica aérea, desde los multi-UAVs hasta la

compatibilidad con Android.

• Todo el código de Arducopter es abierto, lo nos que permite poder implementar los

distintos tipos de controladores de manera sencilla ya que se basa en Arduino.

• El proyecto ArduCopter se puede implementar en dos variantes: multirotores

(quadcopters, tri-helicópteros, hexacopters, etc) y helicópteros tradicionales. En este

caso se utilizará como quadcopter.

Componentes de Arducopter

• La estructura de aluminio para el ArduCopter

• Los cuatro motores AC2836-358, 880Kv. Peso 72 gr.

• Los cuatro ESC modelo m2mPower AC20-1.0. Peso 21 gr.

• Hélices APC, 12X47 SFP Style

• La placa de distribución de energía

• El kit ArduPilot Mega (controlador de vuelo, placa roja y la shield, la IMU, placa azul)

• Un magnetómetro

• El kit de telemetría

• El Sonar XL-MaxSonar EZ0

• La batería LiPo de 2200 mAh.


EXPERIMENTOS

- 70 -

• Equipo de radio-control. Se necesita uno de al menos seis canales.

Estructura básica del Firmware

El firmware de Arducopter está compuesto por una serie de librerías, archivos .h y .pde que

realizan todas las funciones requeridas para el correcto funcionamiento del quadrotor.

En Arducopter.pde se encuentra el programa principal que controla, sincroniza y realiza todas

las funciones necesarias. En el bucle principal de Arducopter.pde se encuentra subdividido en

cinco bucles que controlan funciones dependiendo de la velocidad de actualización requerida

para ellas o lo crítica que sean. Los cinco bucles serían:

• "fast_loop": En él se incluyen funciones de lectura de radio (read_radio), lectura de

datos de la IMU (read_AHRS), refresco de datos de controladores y de valores de los

servos, etc.

• "medium_loop": Se ocupa del GPS y del Compass, realiza cálculos de navegación

como valores deseados calculados para los Waypoints, etc. También se ocupa del envío

de telemetría de alta velocidad. Además controla el bucle de baja velocidad y el armado

de motores.

• "fifty_hz_loop": Se encarga de leer sensores de altitud o estimación de altitud y del

sonar.

• "slow_loop": Es el encargado de los leds de los motores, luces, etc.

• "super_slow_loop": es el encargado del desarmado de motores entre otras funciones.

Implementación del control VPIB en Arducopter

El tipo de controlador implementado de serie, dentro de arducopter, es un PID cascade. Por lo

tanto, para los experimentos con este controlador no hubo que reprogramar nada. Sin embargo,

para los experimentos con un controlador Variable Parameter integral backstepping si hubo que

reprogramar y realizar varias funciones. Para la creación del control VPIB en arducopter se

procedió de la siguiente manera:


EXPERIMENTOS

- 71 -

• Se creó una biblioteca con las funciones necesarias para el control VPIB, las funciones

que se crearon dentro de la biblioteca son las siguientes:

int32_t get_ib(int32_t error,int32_t rate_error, float dt);

int32_t get_term1(int32_t error);

int32_t get_term2(int32_t rate_error);

int32_t get_term3(int32_t error, float dt);

• Se crearon funciones para calcular los parámetros del modelo necesarios para el control

VPIB y que son variables dependiendo de la posición del manipulador.

En principio, el controlador VPIB se implementaría para el control de actitud, altura y posición.

Pero por limitaciones técnicas de arducopter, solo se implemento en actitud.

Las limitaciones técnicas de arducopter para la implementación del control completo del

quadrotor más el manipulador son las siguientes:

• Limitación de memoria y procesado de información.

• Limitaciones sensorial.

• Limitación de carga máxima.

A continuación, detallaremos las limitaciones antes nombradas:

Limitación de memoria y procesado de información

Arducopter trabaja con un microcontrolador Arduino ATmega2560, cuyas características son:

ATmega2560 Voltaje operativo 5 V Voltaje de entrada recomendado 7-12 V Voltaje de entrada límite 6-20 V Pines de I/O digital 86 (12 proporcionan PWM) Pines de entrada analógica 16 Intensidad de corriente 40 mA Memoria Flash 256 KB (4 KB reservados para el bootloader) SRAM 8 KB EEPROM 4 KB Frecuencia de reloj 16 MHz


EXPERIMENTOS

- 72 -

Fig. 54. Placa Ardupilot

El firmware básico de arducopter ocupa una cantidad considerable de memoria. Esto significa

que el código añadido para implementar el control VPIB debía de reducirse y simplificarse lo

suficiente como para que el tamaño total no fuera excesivo, ya que esto podría provocar fallos

en vuelo por sobrecarga o por incapacidad de procesado de datos lo suficientemente rápido.

El principal problema a la hora de implementar el control VPIB fue el cálculo de las variables y

parámetros de modelo necesarias para llevar a cabo esta técnica de control. Para realizar esta

tarea se optaron por varias estrategias:

• Ya que estos parámetros y variables estaban calculadas para el desarrollo del simulador,

en primer lugar se optó por guardar estas dentro del firmware en forma de tablas y

dependiendo de los ángulos que toman las articulaciones del manipulador, tomar el

valor correspondiente. Esta opción se descartó debido a que los datos calculados son

tipo float, lo que conlleva que ocupen una cantidad excesiva de memoria.

• En segundo lugar, se optó por realizar el cálculo de esta variables y parámetros para

cada valor de las articulaciones en tiempo real. El cálculo de los términos de las

matrices de inercia es bastante complejo, además estos cálculos son con variables tipo

float por lo que el nivel de cálculo se acrecienta. Este nivel de procesado resulto

excesivo y el sistema se bloqueaba por lo que se descartó.

• En tercer lugar se optó por realizar una simplificación de las tablas. Mediante un

análisis de los datos se observo que dependiendo de la evolución de los ángulo los


EXPERIMENTOS

- 73 -

valores de las matrices de inercia variaban de manera lineal a tramos. Por lo tanto, se

realizaron unas tablas que incluían los valores límites y mediante un sencillo cálculo de

interpolación se podía obtener una estimación del valor necesario, dependiendo de la

evolución de los valores de los ángulos de las articulaciones obtenidos en tiempo real.

Además se adecuaron los datos a tipo entero ya que el cálculo de este tipo de datos es

más rápido en este tipo de microcontroladores. Gracias a esto, se solvento las

limitaciones de memoria y procesado.

La tercera opción fue la escogida y nos proporcionó una estimación de los datos suficientemente

precisa para llevar a cabo la implementación de la técnica de control VPIB en Arducopter.

Limitaciones sensoriales

Aunque la IMU del sistema Arducopter es bastante buena para su coste y suficiente para realizar

un control preciso en actitud. El problema surgió a la hora de realizar el control de posición y

altitud. Dado que la precisión capaz de proporcionar (>3 mts.) no era suficiente para las

funciones que se requerían, para la que necesitaríamos una precisión de centímetros, no se

pudieron implementar dichos controladores.

Limitación de carga máxima

La carga máxima dada por nuestro arducopter era aproximadamente de 500 grs. para que su

comportamiento en vuelo fuera bueno, aunque podría llegar a ser de 600 grs. aproximadamente

para un comportamiento en vuelo aceptable. El peso del manipulador es de 490 grs.

aproximadamente por lo que la capacidad de carga del sistema completo quedaba limitada a

unos 110 grs. aproximadamente.

Como opción para poder implementar un controlador en posición y altitud con precisión

aceptable se estudió equipar al sistema con un GPS diferencial. El problema fue que el peso de

este excedía en demasía la carga máxima del sistema por lo que, y tras la realización de vuelos

de prueba que lo confirmaban, se descartó esta posibilidad.

En conclusión, debido a estas limitaciones del sistema se optó por implementar, únicamente, el

control en actitud y realizar los experimentos para demostrar la mejora que supondría la

implementación del controlador VPIB con respecto al controlador PID, tal y como hemos visto

en las simulaciones realizadas.


EXPERIMENTOS

- 74 -

4.3 EXPERIMENTOS CON ARDUCOPTER

Se realizaron una serie de experimentos con el quadrotor Arducopter con el manipulador

acoplado a la base. Se realizaron tres vuelos con el controlador PID y cuatro con el controlador

Variable Parameter Integral Backstepping. Todos ellos fueron realizados al aire libre y con

condiciones de viento variables aunque moderadas, por lo que los datos obtenidos en cada vuelo

pueden ser engañosos. A continuación, expondremos los datos obtenidos de los mejores

resultados de los vuelos realizados. Solo expondremos los datos de pitch y roll ya que es la

variación de estos ángulos la que nos interesaría para comparar cuál de los dos tipos de

controladores resulta más eficaz a la hora de rechazar el par externo provocado por el

movimientos del manipulador y el consecuente desplazamiento de centro de gravedad del UAV.

Las siguientes gráficas exponen los mejores datos de cada controlador obtenidos en los

experimentos.

Primero, veremos los resultados para el controlador PID:

Fig. 55. Pitch y Roll para PID.


EXPERIMENTOS

- 75 -

Y para el controlador VPIB:

Fig. 56. Pitch y Roll para VPIB.

Si vemos ahora una comparación de ambos:

Fig. 57. Comparación de Pitch y Roll para VPIB y PID


EXPERIMENTOS

- 76 -

Como podemos observar, las mayores oscilaciones se producen en el control PID, tal y

como habíamos visto en las simulaciones realizadas anteriormente. Por lo que habría que

suponer que el rechazo sobre el par externo provocado por el movimiento del manipulador es

peor que con el controlador VPIB. A simple vista, y dado que las condiciones de viento eran

variables ya que los experimentos se realizaron días distintos y con velocidades de viento

distintas, es difícil apreciar la mejoría que supone el controlador VPIB con respecto a el

controlador PID. A fin de aclarar esta incógnita se grabaron en video estos experimentos para

terminar de acreditar la mejoría que supone el uso del control VPIB. En estos se puede observar

como el desplazamiento que provoca el par externo sobre el quadrotor es mayor con el control

PID. En la siguiente figura, realizamos una ampliación en un tramo en el que el manipulador

está realizando un movimiento y podemos ver una comparación del comportamiento en Pitch de

los dos controladores

Fig. 58. Comparación de Pitch y Roll para VPIB y PID.

Tramo 1: movimiento manipulador (90º a 0º) Tramo 1: movimiento manipulador (0º a 90º)


CONCLUSIONES

- 77 -

5. CONCLUSIONES

En este proyecto se ha diseñado un control especifico para un quadrotor al que se la ha

acoplado un manipulador de dos grados de libertad. Para ello, primero se ha desarrollado, paso a

paso, un modelo matemático que describa de una manera lo más realista posible, sin entrar en

un modelo completo, la dinámica del sistema. Esto se ha realizado, primero, desarrollando un

modelo del quadrotor mediante la formulación de Euler-Lagrange y la formulación de Newton-

Euler. Posteriormente, se ha expuesto un modelo matemático del manipulador robótico. Por

último, se ha desarrollado un modelo del quadrotor en el que se incluían los efectos de un

manipulador acoplado a su base, además, de las diferencias inerciales (no simetría, variabilidad

de los momentos de inercia) que provocan este hecho con respecto al modelo del quadrotor

simple. Este modelo ha sido usado para realizar las simulaciones de tres tipos de técnicas de

control que en el capítulo siguiente se han expuesto. En primer lugar, se justificado el uso de un

control PID con estructura en cascada. A continuación, el uso de la técnica de control Integral

Backstepping. Por último, se ha desarrollado y explicado el diseño de una técnica de control

adaptada a este especialmente a este sistema, donde existen parámetros variables con la posición

de los ángulos del manipulador y que, por lo tanto, se adapta continuamente a los cambios en

los momentos de inercia que es provoca. Además, es capar de rechazar mediante feedback el par

externo que crearía el movimiento del manipulador y que afectaría a la dinámica de pitch. Este

nuevo control ha sido bautizado como Variable Parameter Integral Backstepping.

Mediante distintas simulaciones, en un simulador desarrollado en SIMULINK, se han

probado los distintos controles, pudiendo observar la mejoría que supone la técnica de control

Variable Parameter Integral Backstepping sobre las técnicas PID e Integral Backstepping en este

tipo de sistemas.

Por último, se han realizado una serie de experimentos, gracias a la implementación

realizada en un Arducopter al que se le ha acoplado un prototipo de manipulador de dos grados

de libertad, los que se ha podido observar mediante pruebas realizas al aire libre, con viento

variable, que la respuesta en actitud del control Variable Parameter Integral Backstepping

mejora al PID.


LÍNEAS FUTURAS

- 78 -

6. LÍNEAS FUTURAS

Con el objetivo de continuar con el desarrollo y mejora del control sobre UAVs con

manipuladores robóticos, los siguientes pasos a realizar debieran ser:

1. El estudio y desarrollo de un modelo dinámico del sistema completo. Estudiando los efectos

de interacción del sistema completo con elementos del medio (estudio de sistemas híbridos[12]),

para crear un modelo que incluya los efectos que provocarían sobre el sistema, por ejemplo, la

recogida de piezas y que por lo tanto interactúen con el ambiente [13].

2. Desarrollo de una técnica de control que incluya al manipulador gracias al modelo dinámico

del sistema completo (control conjunto) que incluya, entre otras cosas, la estabilización en

posición del efector final del manipulador.

4. Desarrollo de un nuevo simulador para el modelo completo.

3.Desarrollo de un nuevo prototipo de vehículo aéreo, diseñado a medida para un nuevo

prototipo de manipulador que permita mejores condiciones de control.


ANEXOS

- 79 -

A.CÁLCULO DE MOMENTOS DE INERCIA

Dado que nuestro sistema completo está formado por un quadrotor y un manipulador,

para el cálculo de los momentos de inercia variables con las posiciones angulares de las

articulaciones del manipulador se siguieron los siguientes pasos:

1.- Se realizó el cálculo de los momentos de inercia del quadrotor. Para ello se optó por varias

técnicas. La primera, fue considerar el quadrotor como un sólido rígido y para, a continuación,

calcular su momento de inercia siguiendo el siguiente algoritmo:

• Dividir el quadrotor en piezas más pequeñas para que sean más fácil de calcular. Por

ejemplo, el quadrotor estaría compuesto por: motores, los cuales por su forma se

pueden aproximar a cilindros; varillas y una caja central, que se tomarían como

paralelepípedos; etc.

• Como se aproximan a sólidos rígidos las coordenadas del centro de masa equivalen al

centro de masas de cada pieza. Determinar las coordenadas del centro de masas de

todas las piezas con respecto al centro de masas del sistema completo. Y calcular las

distancias existentes

• Por lo tanto los componentes de la matriz de inercia de cada una de las piezas que

componen el quadrotor referidos a un sistema de coordenadas cartesianas

rectangulares:

Ã = Ã = 5 [~Ã$ − **Ã]Ä = 5 _'$)[~Ã$ − **Ã]N$Å

donde '*O, * , *N) son las coordenadas cartesianas rectangulares. Y ~Ã es el símbolo de

Kronecker que es igual a 1 si A = Æ, y 0 si A ≠ Æ.

• Una vez calculados los momentos de inercia para cada uno de las piezas, mediante el

uso del teorema de Steiner, referidos al centro de masas del sistema completo. È = | +É⨂É


ANEXOS

- 80 -

donde | es la matriz de inercia referida a su centro de masas; , es la masa; y É, es el

vector distancia.

La segunda opción era calcular la matriz de inercia de forma experimental. La idea fue la

construcción de un péndulo compuesto para determinar los momentos respecto a los ejes X

e Y, y aplicando la siguiente fórmula general para el cálculo del momento de inercia:

Ë = Ì 4

• Ì = Í,@

• = Í$A@@sBb@,(Ab(AóB(@Ï bB,sB@, • = ÐA, bB(Ab,Æ@,(Ab(AóBb(B $@$bb

Resolviéndola para nuestro quadrotor:

= ÌOO O4 −Ì 4 −ÌN N

Donde:

• , Momento de Inercia del quadrotor respecto a un eje sobre el centro de gravedad y

paralelo al eje de oscilación.

O = Ì +ÌN NÌO

• O, Distancia desde el eje de oscilación al centro de gravedad del péndulo y el

quadrotor.

• , Distancia desde el eje de oscilación al centro de gravedad del péndulo.

• N, Distancia desde el eje de oscilación al centro de gravedad del quadrotor.

• ÌO, Peso total del quadrotor y del balancín.

• Ì , Peso del balancín.

• ÌN, Peso del quadrotor.

• O, Periodo de una oscilación del quadrotor y del balancín oscilando juntos.

• , Periodo del balancín oscilando solo.


ANEXOS

- 81 -

Fig. 59. Pendulo compuesto diseñado para el experimento

Y la construcción de un péndulo bifilar de torsión para calcular el momento de inercia

respecto al eje Z. Aplicando la siguiente ecuación para el cálculo del momento de inercia:

E = Ì` 16

• Ì = Í,@

• = Í$A@@sBb@,(Ab(AóB(@Ï bB,sB@, • = ÐA, bB(Ab,Æ@,(Ab(AóBb(B $@$bb

• ` = ÐA, bB(AbB $@,(b,, A(b,

Resolviéndola para nuestro quadrotor:

= ÌOO` 16 −Ì ` 16

Donde:

• ÌO, Peso total del quadrotor y del balancín.

• Ì , Peso del balancín.

• O, Periodo de una oscilación completa del quadrotor y del balancín oscilando

juntos.

• , Periodo de una oscilación completa del balancín oscilando solo.


ANEXOS

- 82 -

Fig. 60. Pendulo bifilar diseñado para el experimento

Ambos métodos fueron usados obteniendo valores similares para los momentos de inercia

del quadrotor.

2.- En segundo lugar, se debía calcular la matriz de inercia del manipulador para cada

posición angular dentro de su rango. Gracias a que se contaba con el diseño en CATIA de

este, se pudo disponer de las matrices de inercia de cada uno de los eslabones referidos a su

centro de gravedad. Por lo tanto, ya se contaba con los momentos de inercia fijos del

quadrotor y con los momentos de inercia de cada uno de los eslabones del manipulador

referidos a su centro de gravedad.

3.- En último lugar, gracias a MATLAB, se diseño un programa que crearía la base de datos

donde se calculan los momentos de inercia del sistema completo para cada posición angular

de las articulaciones con una precisión de 0.01 rad. Esto se consiguió realizando una

función que dados los momentos de inercia fijos del quadrotor y de los eslabones, mediante

el teorema de Steiner calculaba los momentos de inercia del sistema total, rotando los

momentos de inercia de los eslabones y referenciándolos al centro de masas del sistema

total.


ANEXOS

- 83 -

B.VARIACIÓN DE LOS MOMENTOS DE INERCIA

Se realizó un estudio de la variación de los términos que componen la matriz de inercia

del sistema con respecto a las posiciones angulares de las articulaciones del manipulador con el

objetivo de poder realizar una simplificación en su cálculo dentro del firmware de Arducopter.

A continuación, podemos observar una matriz compuesta por los términos máximos de que se

dan en la matriz de inercia dependiendo de la posición de brazo:

ÑxU = 0.0437 5.7574ª 0.01085.7574ª 0.0473 0.00440.0108 0.0044 0.0577

Ahora podemos observar la matriz compuesta por los términos mínimos de que se dan en la

matriz de inercia dependiendo de la posición de brazo:

ÑxÓ = 0.0157 −1.2911M −0.0108−1.2911M 0.0418 −0.0034−0.0108 −0.0034 0.0215

Diferencia entre los términos obtenidos:

ÑDÔV"ÓU = 0.0280 1.8669M 0.02161.8669M 0.0054 0.00780.0216 0.0078 0.0362

Como podemos observar la diferencia en los términos obtenidos el par cruzado es del orden

de 10M que en comparación con los otros términos es de una magnitud mucho menor. Esto

podría ser útil a la hora de la implementación si hubiera limitaciones de memoria ya que se

podría simplificar dejándolo como un valor fijo.


ANEXOS

- 84 -

Fig. 61. Variación del término Ixy

Como podemos observar (fig. 61) la variación en el término cruzado es muy pequeña por lo que

a la hora de realizar la implementación a Arduino, este término aparecería como una constante.

Con respecto a otros términos, por ejemplo, el término Ixx el cual podemos ver en la figura 62:

Fig. 62. Variación del término Ixx

-4-3

-2-1

0

-4

-2

0

2

4-1.5

-1

-0.5

0

0.5

1

1.5

x 10-4

q1

Variación Ixy

q2

-4-3

-2-1

0

-4

-2

0

2

40.015

0.02

0.025

0.03

0.035

0.04

0.045

q1

Variación Ixx

q2


ANEXOS

- 85 -

La variación es bastante mayor, y por lo tanto se realizó una identificación de los pares de las

posiciones angulares en donde el término de la matriz de inercia cambiaba su tendencia. Es

decir, se identificaron los valores límites que unían las distintas trayectorias, con distintas

pendientes, que componen la evolución total del término. Creando una matriz de enteros donde

se guardaban estos valores límites. Con esta matriz identificamos los pares posiciones angulares

de articulaciones donde se producen esos cambios. Esos pares se traducen al valor PWM que

envía la emisora. Por lo tanto, para cada par de valores se evaluará dentro de un rango que

equivaldrá a un término límite, con el cual se interpolará hasta obtener una estimación

relativamente precisa al valor real. Esto se realizará para cada término de la matriz de inercias.

Ixx[12][4]=3010,4114 ,3073 ,4258 , 3003 ,4115 ,3043 ,4200 , 3007 ,4124 ,3020 ,4151 , 3022 ,4141 ,3002 ,4111 , 3046 ,4164 ,2989 ,4079 , 3078 ,4193 ,2980 ,4054 , 3115 ,4224 ,2973 ,4035 , 3157 ,4256 ,2967 ,4020 , 3200 ,4288 ,2959 ,4008 , 3242 ,4315 ,2949 ,3997 , 3280 ,4337 ,2933 ,3984 , 3313 ,4351 ,2911 ,3969 ;

En la matriz podemos ver esos valores límites multiplicados por una constante, la cual sirve

para transformar el valor double a entero perdiendo poca precisión. Si se ingresa par de valores

PWM que equivaldrían a un par de posiciones angulares del manipulador. Mediante una función

con esos valores se obtendrían un par de índices, los cuales utilizaríamos para acceder los

términos necesarios para nuestro controlador.


BIBLIOGRAFÍA

- 86 -

BIBLIOGRAFÍA

[1] Arcas project. Aerial Robotics Cooperative Assembly System (ARCAS). http://www.arcas-project.eu/

[2] A. L. Salih, M. Moghavvemi, H. A. F. Mohamed and H. S. Caeid. Flight PID controller design for a UAV quadrotor. Scientific Research an Essays Vol 5(23), pp. 3660-3667, 4 December, 2010.

[3] M. Santos, V. López and F. Morata. Intelligent Fuzzy Controller of a Quadrotor. 978-1-4244-6793-8/10 2010 IEEE.

[4] Smarathunga L.M.D. Rangajeeva and James F. Whidborne. Linear Parameter Varying Control of a Quadrotor. ICIIS 2011, Aug 16-19, 2011, Sri Lanka.

[5] S. Bouabdallah. Design and Control of Quadrotors with Applications to Autonomous Flying. PhD thesis. EPFL. 2007.

[6] P. Pounds. Design, Constructions and Control of a Large Quadrotor Micro-air Vehicule. PhD thesis. ANU, 2007.

[7] V. Ghadiok. Autonomous aerial manipulation using a quadrotor, Master's thesis, Utah State University, Logan, Utah, 2011.

[8] Arducopter, Arduino-based autopilot for multicopters, http://code.google.com/p/arducopter/

[9] G. Fary. "Derivation of the aerodynamic forces for the mesicopter simulation". 2001.

[10] A. Ollero. Robótica, manipuladores y robots móviles. 2001

[11] S. Bouabdallah and R. Siegwart. Full control of a quadrotor. In Proceedings of IROS- 2007, pp. 153-158.

[12] R. Goebel, R. G. Sanfelice, A.R. Teel. Hybrid Dynamical Systems. IEEE Control Systems Magazine. April 2009.

[13] L. Marconi, R. Naldi, L. Gentili. Modelling and Control of a Flying Robot Interacting with the Enviroment. Automatica 47 (2011) 2571-2583.

grupo de robÓtica, visiÓn y control -...

Documents