Professional Documents
Culture Documents
ndice
I
Inters y objetivos
1. Inters
2. Objetivos
II
Revisin bibliogrfica
3.3. Quadrotor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.3.2. Maniobrabilidad . . . . . . . . . . . . . . . . . . . . . . . . . .
4. Filtro de Kalman
11
12
14
15
5. Estrategias de control
19
21
24
24
27
27
28
30
31
33
III
Materiales y mtodos
35
6. Materiales
35
35
6.2. Arduino . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
35
6.3. Sensores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
39
6.3.1. Acelermetro . . . . . . . . . . . . . . . . . . . . . . . . . . . .
40
6.3.2. Giroscopio . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
41
6.3.3. Magnetmetro . . . . . . . . . . . . . . . . . . . . . . . . . . .
45
45
6.4. MATLAB . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
47
6.4.1. Simulink . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
47
7. Mtodos
49
49
49
52
7.1.3. Fuerzas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
54
7.1.4. Momentos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
55
7.1.5. Dinmica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
57
59
62
63
64
66
66
68
68
70
7.5.2. Sensores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
72
73
75
7.5.5. Controlador . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
77
ii
IV
Resultados y discusin
79
8. Resultados y discusin
79
79
79
81
82
84
84
86
86
87
8.3. Controlador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
89
89
92
94
Conclusiones
95
9. Conclusiones
95
10.Proyectos futuros
96
VI
97
VII
Bibliografa
Anexos
102
102
iv
Parte I
Inters y objetivos
1.
Inters
El trmino UAV, siglas en ingls de Unmanned Aerial Vehicle, se utiliza para definir
a los vehculos areos sin tripulacin humana a bordo. Los UAV se han convertido
recientemente en una plataforma viable para tareas de supervivencia y exploracin
donde la presencia humana es peligrosa, imposible o inadecuada.
Los quadrotors son unos vehculos areos de cuatro rotores que poseen la capacidad de despegue y aterrizaje vertical (VTOL). Su simple diseo mecnico, su gran
maniobrabilidad y la capacidad VTOL los hacen adecuados para que se usen como
vehculos UAV en la vida real, por ejemplo es posible observar en equipos de polica
de los Estados Unidos y Canad que han usado los quadrotors de forma exitosa como
equipamiento en tareas de supervivencia.
Adems, en el mbito de la investigacin, los quadrotors se utilizan como plataforma
para probar tcnicas de control avanzado. Pese a su simple diseo mecnico, el modelo
dinmico que define el comportamiento de los quadrotors es complejo debido a que se
trata de un modelo no lineal, es por ello que se utilizan como banco de ensayos para
el estudio sistemas de control y observadores no lineales.
El filtro de Kalman ha sido uno de los descubrimientos que ha permitido el avance
en el campo del control de sistemas dinmicos complejos. Para controlar un sistema dinmico, primero se tiene que conocer y capturar su comportamiento. En estas
aplicaciones, no siempre es posible o deseable medir todas las variables a controlar,
el filtro de Kalman proporciona un medio para inferir la informacin desconocida a
partir de mediciones indirectas y ruidosas.
2.
Objetivos
Parte II
Revisin bibliogrfica
3.
Un vehculo areo no tripulado, UAV por las siglas en ingls (Unmanned Aerial Vehicle), es una aeronave que es capaz de realizar una misin sin tripulacin humana a
bordo. Su vuelo puede ser dirigido por controladores autnomos incorporados en la
aeronave, o mediante control remoto desde tierra u otro vehculo.
Puesto que la anterior definicin no excluye el telecontrol de la aeronave, cabe definir
vehculo areo autnomo, AAV (Autonomous Aerial Vehicle) como aqul capaz de
desarrollar la misin sin necesidad de intervencin humana. En este caso cabra la
posibilidad de que la aeronave transportara personal, no dedicado a la misin, pero
esta posibilidad, similar al piloto automtico con el que cuentan la mayor parte de los
aviones actuales, queda fuera del contexto en el que los sistemas considerados operan.
El principal uso de los UAV ha sido en tareas militares, realizando tanto misiones de
reconocimiento como de ataque. Pero los UAV tambin se emplean en un pequeo
pero creciente nmero de aplicaciones civiles; relacionadas con la teledeteccin en su
mayor parte, por ejemplo en vigilancia y seguridad civil (lucha contra incendios o
la supervisin de oleoductos), y en menor grado con el transporte. En general, los
UAV suelen ser preferidos para misiones que son demasiado "montonas, sucias o
peligrosas" para aeronaves tripuladas [4, 55].
3.1.
3.2.
***
****
***
***
***
**
****
*
***
*
*
****
**
****
*
**
****
**
Auto Estabilidad
***
****
**
Capacidad de vuelos
****
**
****
Capacidad de carga
***
****
**
Capacidad de vuelo
**
***
****
**
****
***
Velocidad de desplazamiento
Maniobrabilidad
Autonoma de vuelo
Resistencia a perturbaciones
externas
verticales
en interiores
Techo de vuelo
Por ltimo, otro tipo de clasificacin, es la que se realiza por su funcin. Los vehculos
areos no tripulados se pueden clasificar por su funcin en alguna de las seis categoras
siguientes:
Blanco y seuelo, proporcionan a la artillera terrestre y area un blanco que
simula un avin o misil enemigo.
Reconocimiento, proporcionan informacin en el campo de batalla, en escenarios
de emergencia o en ambientes de alta toxicidad qumica o radiolgica.
Combate, proporcionan capacidad de ataque en misiones de alto riesgo.
Logstica, son diseados especficamente para la carga y operaciones de logstica.
Investigacin y desarrollo, usados para desarrollar la futura tecnologa de los
UAV.
Civil y comercial, vehculos areos no tripulados diseados especficamente para
aplicaciones civiles y comerciales. Cartografa, gestin de cultivos, prospeccin
de yacimientos geolgicos, gestin del medio ambiente, hidrologa, control urbanstico, etc.
3.3.
Quadrotor
Figura 4: tres quadrotors volando en el interior del Flying Machine Arena del ETH
vdeos publicados en internet por este grupo de investigacin han dado la vuelta al
mundo gracias a las redes sociales ayudando a que aumente el inters del pblico
general as como de otros grupos de investigacin en materia de quadrotors. [42]
Como gran desventaja se suele citar la dificultad de establecer un sistema de control,
especialmente para aquellos quadrotors de dimensiones reducidas. Esto se debe al
hecho de que las fuerzas y pares necesarios para controlar el sistema son conseguidos
no slo a travs de efectos aerodinmicos, sino tambin a travs del acoplamiento de
la dinmica de los cuatro rotores [28].
3.3.1.
Aplicaciones actuales
Maniobrabilidad
Cada rotor produce un empuje y un momento sobre su eje de rotacin, as como una
pequea fuerza de arrastre de sentido opuesto a la direccin de vuelo del vehculo.
8
4.
Filtro de Kalman
Tericamente el filtro de Kalman es un estimador para lo que se conoce como problema lineal cuadrtico, que consiste en la estimacin del estado actual de un sistema
dinmico perturbado por ruido blanco, usando mediciones relacionadas linealmente
9
Figura 7: cambio en la velocidad de los rotores para ejecutar una rotacin positiva en
alabeo, cabeceo y guiada, respectivamente
10
pero corrompidas con ruido blanco aditivo. El estimador resultante es estadsticamente ptimo con respecto a cualquier funcin cuadrtica de estimacin del error.
En la prctica, ha sido uno de los grandes descubrimientos en la historia de la teora de la estimacin estadstica y ha posibilitado grandes descubrimientos en el siglo
XX. De ha hecho tan indispensable como el silicio en la fabricacin de componentes
electrnicos. Su ms inmediata aplicacin ha sido el control de sistemas dinmicos
complejos como sistemas de produccin en continuo, aeronaves, barcos o naves espaciales. Para controlar un sistema dinmico, primero hay que conocer y capturar su
comportamiento. En estas aplicaciones, no siempre es posible o deseable medir cada una de las variables a controlar, y es el filtro de Kalman quien provee un medio
para inferir la informacin desconocida de mediciones indirectas y ruidosas. El filtro
de Kalman es usado tambin para predecir el futuro comportamiento de sistemas
dinmicos que probablemente no se van a poder controlar, como el curso de los ros
durante una inundacin, la trayectoria de cuerpos celestes o los precios de las acciones
en los mercados de valores.
Puede parecer extrao que se aplique el trmino filtro a un estimador. Ms comnmente, un filtro es un instrumento fsico que separa la parte no deseada de una mezcla.
Originalmente, un filtro resolva el problema de separar componentes no deseados de
una mezcla. En la era de las radios de cristal y los tubos de vaco, el trmino fue
aplicado al circuito analgico que filtraba las seales electrnicas. Estas seales son
una mezcla de componentes de diferentes frecuencias, y estos instrumentos fsicos
atenuaban las frecuencias no deseadas.
Este concepto fue extendido en los aos 30 y 40 al de separar seales del ruido,
ambas caracterizadas por su densidad espectral de potencia. Kolmogorov y Wiener
usaron esta caracterstica estadstica de las distribuciones de probabilidad para obtener una estimacin ptima de la seal, dada la suma de seal y ruido.
Con el filtro de Kalman el trmino filtrado asumi un significado que va ms all de la
idea original de separar componentes de una mezcla. Ha llegado a incluir la solucin
de un problema de inversin, en el cual se conoce como representar las variables
medidas como funcin de las principales variables de inters. En esencia, invierte la
relacin funcional y estima las variables independientes como funciones inversas de
las variables dependientes (medidas). Estas variables de inters tambin pueden ser
dinmicas, con dinmicas que son slo parcialmente predecibles [16, 47].
4.1.
Filtro de Wiener
mediante un operador integral que puede ser implementado mediante circuitos analgicos, dadas ciertas limitaciones en la regularidad de las funciones de autocorrelacin
o, de forma equivalente, sus transfomadas de Fourier.
Una solucin anloga del estimador lineal ptimo para sistemas en tiempo discreto
fue publicada por A. N. Kolmogorov en 1941, cuando Wiener estaba terminando su
trabajo en el estimador en tiempo continuo [16].
4.2.
Filtro de Kalman
(4.1)
(4.2)
(4.3)
p(v) N (0, R)
(4.4)
En la prctica, la matriz de covarianza del ruido del proceso Q y la matriz de covarianza del ruido de las mediciones R pueden variar en cada paso de tiempo o medicin,
sin embargo se asume que son constantes.
La matriz A, de dimensiones n n, relaciona el estado en el etapa anterior k 1 con
el estado en la etapa actual k, en ausencia de una funcin de entrada controladora
o de ruido en el proceso. La matriz B, de dimensiones n l, relaciona la entrada de
control u <l con el estado x. La matriz H, de dimensiones m n, en la ecuacin
de medicin relaciona el estado con la medicin zk .
n
Definindose x
k R como la estimacin a priori del estado en una etapa k conociendo el proceso en el estado del proceso antes de sta, y x
k Rn como el estado
12
k xk x
k, y
(4.5)
ek xk x
k .
(4.6)
(4.7)
Pk = E ek eTk .
Al deducir las ecuaciones del filtro de Kalman, se comienza con el objetivo de encontrar una ecuacin que calcule una estimacin del estado a posteriori x
k como una
combinacin lineal de la estimacin a priori x
y
un
promedio
ponderado
de la difek
rencia entre la actual medida zk y una prediccin de la medida H x
como
se
muestra
k
a continuacin.
x
k = x
k + Kk zk H x
k
(4.9)
k condicionado
a todas las mediciones anteriores zk (regla de Bayes). El filtro de Kalman mantiene
los dos primeros momentos de la distribucin del estado,
E [xk ] = x
k ,
(4.10)
h
i
T
E (xk x
k ) (xk x
k ) = Pk .
(4.11)
(4.12)
La diferencia zk H x
1
(4.13)
Pk H T
HPk H T + R
Observando la ecuacin 4.13 se puede ver cmo en el caso de que la covarianza del
error de la medicin R se aproxime a cero, la ganancia K le da ms peso a ponderacin
del residuo. Especficamente,
lim Kk = H 1 .
Rk 0
(4.14)
P 0
k
(4.15)
k [16, 35].
4.2.1.
El filtro de Kalman estima el estado del proceso mediante el uso de una forma de
control retroalimentado: el filtro estima el estado del proceso en algn momento y
despus obtiene una retroalimentacin en forma de mediciones ruidosas. De esta forma
las ecuaciones del filtro de Kalman se dividen en dos etapas: la etapa de prediccin y
la etapa de correccin.
La etapa de prediccin es la responsable de proyectar hacia delante en el tiempo el
estado actual y la covarianza del error estimada para obtener estimaciones a priori
para la siguiente etapa. La etapa de correccin es la encargada de la retroalimentacin,
por ejemplo, incorporando una nueva medida en la estimacin a priori para obtener
una estimacin a posteriori mejor.
Las ecuaciones de las etapas de prediccin y correccin se muestran en las tablas 2 y
3.
14
xk1 + Buk1
k = A
Pk = APk1 + AT + Q
x
k = x
k + Kk zk H x
k
1
Pk = (I Kk H) Pk
Otra vez se puede ver cmo las ecuaciones de prediccin de la tabla 2 proyectan el
estado y la covarianza hacia delante de la etapa k 1 a la etapa k. Las matrices A y
B provienen de (4.1) mientras que Q lo hace de (4.3).
La primera tarea durante la etapa de correccin es el clculo de la ganancia de Kalman,
Kk . El siguiente paso es actualizar las mediciones para obtener un nuevo zk , y despus,
operar para generar una estimacin a posteriori incorporando las mediciones. El paso
final es obtener la estimacin a posteriori de la covarianza del error.
Despus de cada etapa de prediccin y correccin, el proceso se repite con las estimaciones a posteriori previas siendo usadas para proyectar o estimar las nuevas estimaciones a priori. Esta naturaleza recursiva es una de las caractersticas ms apreciadas
del filtro de Kalman, no es necesario trabajar con todos los datos directamente en
cada etapa como ocurre con el filtro de Weiner [16, 35].
4.2.2.
(4.16)
que es
zk = h (xk , vk )
(4.17)
donde las variables aleatorias wk y vk representan, otra vez, el ruido del proceso y el
ruido de las mediciones respectivamente. La funcin no lineal f relaciona el estado
xk1 en el paso de tiempo anterior k 1 con el estado xk en el paso actual k. Tiene
como parmetros adems una funcin controladora uk1 y el ruido blanco del proceso
wk1 . La funcin no lineal h en la ecuacin de medicin (4.17) relaciona el estado xk
con la medicin zk .
En la prctica no se conocen los valores individuales del ruido wk y vk en cada paso.
Sin embargo, se puede aproximar el vector de estados y el vector de mediciones sin el
ruido como
xk = f (
xk1 , uk1 , 0)
(4.18)
zk = h (xk , 0)
(4.19)
donde x
k es una estimacin a posteriori del estado proveniente del paso anterior k.
Es importante mencionar que uno de los problemas fundamentales del filtro de Kalman
extendido es que las distribuciones, o densidades en el caso continuo, de las diferentes
variables aleatorias no son normales tras pasar por sus respectivas transformaciones
no lineales.
Para estimar con diferencias y relaciones de mediciones no lineales, se comienza escribiendo nuevas ecuaciones de gobierno que linearizan una estimacin sobre (4.18) y
(4.19),
xk x
k + A (xk1 x
k1 ) + W wk1
(4.20)
zk zk + H (xk x
k ) + V vk
(4.21)
donde
16
f[i]
(
xk1 , uk1 , 0) ,
w[j]
(4.23)
h[i]
(
xk , 0) ,
x[j]
(4.24)
h[i]
(
xk , 0) .
v[j]
(4.25)
(4.26)
ezk = zk zk .
(4.27)
y la innovacin de la medicin,
(4.28)
ezk H exk + k ,
(4.29)
Cabe destacar que las ecuaciones (4.28) y (4.29) son lineales, y recuerdan a las ecuaciones (4.1) y (4.2) del filtro de Kalman discreto. Esto motiva el uso de el actual
residuo de medicin ezk en (4.27) y un segundo (hipottico) filtro de Kalman para
estimar el error de la prediccin exk dado por (4.28). Esta estimacin, denotada por
ek , puede ser usada junto con (4.26) para obtener una estimacin a posteriori del
estado para el proceso no lineal original como
(4.30)
x
k = x
k + ek .
Las variables aleatorias de 4.28 y 4.29 tienen aproximadamente las siguientes distribuciones de probabilidad:
p (
exk ) N 0, E exk eTxk
(4.32)
(4.33)
p (k ) N 0, W Qk W T
p (k ) N 0, V Rk V T
(4.31)
Dadas estas aproximaciones y dejando que el valor predicho de ek sea cero, la ecuacin
del filtro de Kalman usado para estimar ek es
ek = Kk ezk .
(4.34)
=x
k + Kk ezk
=x
k + Kk (zk zk )
(4.35)
La ecuacin (4.35) puede ser usada ahora para la prediccin en el filtro de Kalman
extendido, con x
k y zk provenientes de (4.18) y (4.19), y una ganancia de Kalman
Kk proveniente de (4.13) con la apropiada sustitucin para la covarianza de error de
medicin.
El juego completo de ecuaciones del filtro de Kalman extendido se muestra en las
tablas 4 y 5.
Como en el filtro de Kalman discreto, las ecuaciones de la etapa de prediccin en la
tabla 4 proyectan el estado y la covarianza estimados en la etapa anterior k 1 a la
etapa actual k. Otra vez f viene de (4.18), Ak y Wk son las Jacobianas en la etapa
k, y Qk es la matriz de covarianza del proceso en la etapa k.
Al igual que en filtro de Kalman discreto, las ecuaciones de la etapa de correccin en
la tabla 5 corrigen el estado y la covarianza estimadas con la medicin zk . Otra vez h
18
xk1 , uk1 , 0)
k = f (
x
k = x
k + Kk zk h x
k ,0
1
Pk = (I Kk Hk ) Pk
,
0
que
afecta
al estado. Por supuesto si sobre todas
k
las medidas no existe una correspondencia directa entre las mediciones zk y el estado
por medio de h, como cabra esperar, el filtro diverge rpidamente. En este caso el
proceso no es observable [16, 35].
5.
Estrategias de control
5.1.
Un PID (Proporcional Integral Derivativo) es un mecanismo de control por realimentacin que calcula la desviacin o error entre un valor medido y el valor que se quiere
obtener, para aplicar una accin correctora que ajuste el proceso. Es interesante sealar que ms de la mitad de los controladores industriales que se utilizan hoy en da
utilizan esquemas de control PID o PID modificados.
La utilidad de los controles PID estriba en que se aplican de forma casi general a la
mayora de los sistemas de control. En particular, cuando el modelo matemtico de la
planta no se conoce y, por tanto, no se pueden emplear mtodos de diseo analticos,
es cuando los controles PID resultan ms tiles. En el campo de los sistemas de
control de procesos, es un hecho bien conocido que los esquemas de control bsicos y
modificados han demostrado su utilidad para aportar un control satisfactorio, aunque
tal vez en muchas situaciones no aporten un control ptimo.
El control PID est compuesto, como se ve en la figura 8, de tres acciones que se
suman entre s: la accin de control proporcional, la integral y la derivativa. El valor
proporcional determina la reaccin al error actual. La accin integral genera una
accin proporcional a la integral del error, esto nos asegura que aplicando un esfuerzo
de control suficiente, de modo que el error de seguimiento se reduce a cero. La accin
derivativa determina la reaccin del tiempo en el que el error se produce.
La seal de salida estar determinada por el siguiente algoritmo
t
u (t)P ID = Kp e (t) + Ki
e ( ) d + Kd
de (t)
.
dt
(5.1)
(5.2)
(a)
(b)
Figura 9: la grfica (a) muestra una accin de control integral, mientras que la grfica
(b) muestra una accin de control proporcional
en el caso de un controlador proporcional, ya que una seal de control diferente de
cero requiere una seal de error diferente de cero. Una seal de error diferente de cero
en estado estacionario significa que hay una equivalencia. La figura 9 (b) muestra la
curva e (t) frente t y la curva u (t) correspondiente frente t cuando el controlador es
de tipo proporcional.
Obsrvese que la accin de control integral, aunque elimina el offset o el error en estado
estacionario, puede conducir a una respuesta oscilatoria de amplitud decreciente lenta
o, incluso, de amplitud creciente, y en ambos casos, por lo general, se consideran
indeseables.
La frmula de la accin integral esta dada por
t
u (t)i = Ki
e (t) dt.
(5.3)
El error es integrado, lo cual tiene como funcin promediarlo o sumarlo por un periodo
determinado, para luego multiplicarlo por la constante Ki conocida como ganancia
integral. Posteriormente, la respuesta integral es sumada a la respuesta proporcional
para formar el control PI con el propsito de obtener una respuesta estable del sistema
sin error estacionario [21, 29].
Accin de control derivativa. Cuando una accin de control derivativa se agrega
a un controlador proporcional, aporta un modo de obtener un controlador con alta
sensibilidad. Una ventaja de usar una accin de control derivativa es que responde a
la velocidad de cambio del error y produce una correccin significativa antes de que la
magnitud del error se vuelva demasiado grande. Por tanto, el control derivativo prev
el error, inicia una accin correctiva oportuna y tiende a aumentar la estabilidad del
sistema.
23
Kp
Ki
Kd
Tiempo de
levantamiento
Sobrepaso
Tiempo de
Error estado
asentamiento
estable
Estabilidad
Disminuye
Aumenta
Cambio pequeo
Disminuye
Disminuye
Disminuye
Aumenta
Aumenta
Se elimina
Disminuye
Disminuye
Disminuye
No afecta
Cambio
pequeo
Mejora si
Kd es pequea
de (t)
.
dt
(5.4)
Ajuste manual
(5.5)
T
= 1, 2
L
1
+ 0, 5Ls
1+
2Ls
= 0, 6T
s + L1
s
(5.6)
2
por tanto, el controlador PID tiene un polo en el origen y un cero doble en s = 1/L.
Segundo mtodo. En el segundo mtodo, primero se fija Ki = 0 y Kd = 0. Usando
slo la accin de control proporcional se incrementa Kp desde 0 hasta un valor crtico
Kcr , a partir del cual la salida presente oscilaciones sostenidas. Si la salida no presenta
oscilaciones sostenidas para cualquier valor de Kp entonces no se puede aplicar este
mtodo. As, la ganancia crtica Kcr y el periodo Pcr correspondiente se determinan
experimentalmente (figura 11). Ziegler-Nichols sugirieron que se establecieran los valores de los parmetros Kp , Ki y Kd de acuerdo con la frmula que se muestra en la
tabla 8.
Obsrvese que el controlador PID sintonizado mediante el segundo mtodo de las
reglas de Ziegler-Nichols produce
Ki
+ Kd s
Gc (s) = Kp 1 +
s
26
= 0, 6Kcr 1 +
2
Pcr s
= 0, 075Kcr Pcr
+ 0, 125Pcr s
4
Pcr
s+
(5.7)
2
En el artculo de Bouabdallah et al. [9] se utiliz un PID para control de la orientacin de un micro helicptero de cuatro rotores. El controlador PID obtuvo mejores
resultados que el LQR pese a que el modelo matemtico que se utiliz era ms simple, logrndose el vuelo autnomo estable. El autor apunta a que esto es debido a la
mayor tolerancia de este tipo de controladores a las variaciones de los parmetros del
modelo.
Otro ejemplo de que el regulador PID es apropiado para el control de la posicin y
orientacin de un quadrotor lo encontramos en Jun Li et al. [23], donde tras analizar
la dinmica del quadrotor se disea se implementa un regulador PID llegando a la
conclusin de que la estabilizacin puede ser lograda mediante un regulador PID si
los parmetros de este son los apropiados.
5.2.
J1 =
0
27
(5.8)
(5.9)
5.3.
Control difuso
fro
templado
caliente
temperatura
Figura 12: escala difusa de temperatura
esa escala tiene tres valores de verdad, uno para cada una de las tres funciones. La
lnea vertical de la figura representa una temperatura particular. La flecha roja en
cero, esta temperatura puede ser interpretada como no caliente. La flecha de color
naranja (sealando a 0,2) puede describirla como un poco caliente y la flecha azul
(sealando a 0,8) bastante fro.
Mientras que las variables en matemticas suelen tener valores numricos, en lgica
difusa, las variables lingsticas no numricas se utilizan a menudo para facilitar la
expresin de reglas y hechos. Una variable lingstica, como la edad, pueden tener un
valor como el joven o su antnimo viejo. Sin embargo, la gran utilidad de variables
lingsticas es que pueden ser modificados mediante los modificadores lingsticos
aplicados a trminos primarios. Los modificadores lingsticos puede estar asociado
con ciertas funciones.
La teora de conjuntos difusos define operadores difusos en los conjuntos difusos. El
problema en la aplicacin de esto es que el operador difuso apropiado puede no ser
conocido. Por esta razn, la lgica difusa por lo general utiliza reglas SI-ENTONCES,
o construcciones que sean equivalentes, tales como matrices asociativas difusas. Las
reglas se expresan normalmente de la manera: SI variable ES propiedad ENTONCES
accin. Por ejemplo un regulador de temperatura que usar un ventilador podra
quedar as:
SI
SI
SI
SI
temperatura
temperatura
temperatura
temperatura
ES
ES
ES
ES
5.4.
Control robusto
5.5.
Control predictivo
Existen multitud de algoritmos de control predictivo que han sido aplicados con xito.
Uno de ellos es el Control Predictivo Basado en Modelo (CPBM), tambin conocido
como Model Predictive Control (MPC). El control predictivo basado en modelo se
puede definir como una estrategia de control que se basa en la utilizacin explcita de
un modelo matemtico interno del proceso a controlar (modelo de prediccin), el cual
se utiliza para predecir la evolucin de las variables a controlar a lo largo del horizonte
temporal de prediccin especificado por el operador. De este modo se pueden calcular
las variables manipuladas futuras (seal de control futura) para lograr que en dicho
horizonte las variables controlada converjan a sus respectivos valores de referencia.
El MPC se enmarca dentro de los controladores ptimos, es decir, aquellos en los que
las actuaciones responden a la optimizacin de un criterio. El criterio a optimizar, o
funcin de coste, est relacionado con el comportamiento futuro del sistema que se
predice gracias al denominado modelo de prediccin.
El intervalo de tiempo futuro que se considera en la optimizacin se denomina horizonte de prediccin. Dado que el comportamiento futuro del sistema depende de las
actuaciones que se aplican a lo largo del horizonte de prediccin, son estas variables de
decisin respecto a las que se optimiza el sistema. La aplicacin de estas actuaciones
sobre el sistema conducen a un control en bucle abierto.
La posible discrepancia entre el comportamiento predictivo y el comportamiento real
del sistema crean la necesidad de imponer cierta robustez al sistema incorporando
una realimentacin al mismo. Esta realimentacin se consigue gracias a la tcnica
de horizonte deslizante que consiste en aplicar las actuaciones obtenidas durante un
periodo de tiempo, tras el cual se muestra el estado del sistema y se resuelve un nuevo
problema de optimizacin. De esta manera el horizonte de prediccin se va deslizando
a lo largo del tiempo.
Una de las propiedades ms atractivas del MPC es su formulacin abierta, que permite
la incorporacin de distintos tipos de modelos de prediccin, sean lineales o no lineales,
monovariables o multivariables, y la consideracin de restricciones sobre las seales del
sistema. Esto hace que sea una estrategia muy utilizada en diversas reas de control.
El MPC es una de las pocas tcnicas de control que permiten controlar sistemas con
restricciones incorporando stas en el propio diseo del controlador.
Estas caractersticas han hecho del control predictivo una de las escasas estrategias
de control avanzado con un impacto importante en problemas de mbito industrial.
Por tal motivo es importante resaltar que el control predictivo se ha desarrollado en
el mundo de la industria, y ha sido la comunidad investigadora la que se ha esforzado
en dar un soporte terico a los resultados prcticos obtenidos [34, 48].
En el artculo de K. Alexis et al. [2] se disea una estrategia de control predictivo
basada en modelo para el control de la orientacin y la posicin del quadrotor. El sistema desarrollado es capaz de rendir de manera precisa en vuelos en interior mediante
la utilizacin de un sistema de fusin de sensores IMU/Sonar/IMU. La novedad en de
este artculo resida en la propuesta del control MPC, siendo el resultado experimental
satisfactorio.
32
5.6.
En teora de control, los modos deslizantes son una forma de control de estructura
variable. Es un mtodo de control no lineal que altera la dinmica de un sistema no
lineal aplicando un control alternante. La ley de control no es una funcin constante
en el tiempo. En cambio, va alternando de una estructura continua a otra basndose
en la posicin en ese instante en el espacio de estados. Las estructuras de control
mltiple se disean para que las trayectorias siempre se muevan hacia la condicin
de alternancia y, as, la trayectoria ltima no existir completamente en una sola
estructura de control. Lo que ocurrir es que la trayectoria ltima se deslizar por la
frontera entre las diferentes estructuras de control.
La mayor ventaja del control por modo deslizante es su robustez, ya que el control
puede ser tan simple como el salto entre dos estados, no necesita ser preciso y no se
ver afectado a variaciones de parmetros que entren en el canal de control. Adems,
como la ley de control no es una funcin continua, el modo deslizante se puede alcanzar
en tiempo finito.
El control en modo deslizante debe ser aplicado con mucho ms cuidado que otras
formas de control no lineal que poseen una accin de control ms moderada. En
particular, porque los actuadores tienen retardos y otras imperfecciones, y el modo de
control puede llegar a chattering, perdida de energa, dao en la planta y excitacin
de dinmicas no modeladas [7, 27].
El control por modos deslizantes se utilizo para la estabilizacin de un quadrotor
en presencia de fallos en los actuadores en el artculo de H. Khebbache et al. [20].
Los resultados de la simulacin demostraron una gran eficiencia del la estrategia de
control al mantener la estabilidad del quadrotor a pesar del fallo en los actuadores.
33
34
Parte III
Materiales y mtodos
6.
Materiales
A lo largo de este captulo se describirn brevemente los distintos materiales que han
sido necesarios para la realizacin de este proyecto. Se comenzar con del Draganflyer
SAVS, que es quadrotor utilizado como plataforma, para continuar con el microcontrolador y los sensores utilizados para poder estimar la orientacin del quadrotor. Por
ltimo se analizar del software utilizado en la simulacin.
6.1.
Draganflyer SAVS
6.2.
Arduino
Arduino es plataforma de hardware libre, basada en una placa con un microcontrolador y un entorno de desarrollo que tienen como objetivo facilitar el uso de la
electrnica en proyectos multidisciplinares.
35
37
38
6.3.
Sensores
6.3.1.
Acelermetro
(6.1)
Giroscopio
42
una direccin diferente de lo que lo hara sin el momento angular asociado al disco
con gran velocidad de giro y momento de inercia. Como el par externo es minimizado
por el montaje del dispositivo en cardanes, su orientacin permanece casi fija, con
independencia de cualquier movimiento de la plataforma en la que se monta.
Un giroscopio exhibe un nmero de comportamiento incluyendo la precesin y la
nutacin. Los giroscopios pueden ser usados para construir girocompases, los cuales
complementa o remplazan a los compases magnticos, para asistir en la estabilidad o
como parte de un sistema de medicin inercial. Los efectos giroscpicos se utilizan en
boomerangs y yoyos.
La ecuacin fundamental que describe el funcionamiento de un giroscopio es
~ =
~
d (I~
)
dL
=
= I
~
dt
dt
(6.2)
(6.3)
En la literatura aparecen gran cantidad de tipos de giroscopios MEMS, aunque la mayora caen en las categoras de giroscopio de tenedor ajustado (tuning-fork), volante
oscilante, pndulo de Foucault, y resonador Helmholtz. Los giroscopios convencionales de rueda giratoria, no MEMS, son comunes, pero la levitacin y rotacin en
dispositivos MEMS sin muelles no ha sido comercializado an.
Los giroscopios de tenedor ajustado contienen un par de masas que estn forzadas a
oscilar con igual amplitud pero en direcciones opuestas. Cuando rotan, la fuerza de
Coriolis crea una vibracin ortogonal que puede ser sentida de diferentes maneras.
La rotacin provoca que las masas de prueba vibren fuera del plano, siendo este
movimiento es detectado por sensores capacitativos. Esta tecnologa ha sido patentada
por Rockwell, Boeing, Honeywell y otros.
Los modos resonantes de un sensor MEMS inercial son extremadamente importantes.
En un giroscopio, normalmente hay un modo de vibracin que es provocado y un
segundo modo como salida detectada. En algunos casos, los modos de entrada y
salida son degenerados o estn cerca de serlo. Si los modos de entrada y salida son
elegidos de manera que estos se encuentren separados por un 10 % aproximadamente,
la sensibilidad en bucle abierto se incrementar debido a efectos de resonancia. Es
crtico que no haya otros modos de resonancia cercanos a las frecuencias de resonancia
de entrada y salida.
43
Magnetmetro
Los magnetmetros permiten determinar la fuerza o la direccin de un campo magntico. Los magnetmetros se pueden clasificar en dos tipos bsicos: escalares, miden
la fuerza del campo magntico al que se encuentran sometidos, y vectoriales, miden
la componente de campo magntico en una direccin concreta. Estos ltimos son conocidos como brjulas o comps y son los ms interesantes para la navegacin. Entre
las tecnologas de magnetmetros destacar los siguientes tipos:
Bobina rotatoria. El campo magntico induce una onda senoidal en una bobina
de rotacin. La amplitud de la seal es proporcional a la intensidad del campo,
siempre que sea uniforme, y al seno del ngulo entre el eje de rotacin de la
bobina y las lneas de campo. Este tipo de magnetmetro est obsoleto.
Efecto Hall, son los magnetmetros ms comunes. Estos sensores producen un
voltaje proporcional al campo magntico aplicado y con la polaridad relativa al
sentido del campo. Se utilizan en aplicaciones donde la intensidad del campo
magntico es relativamente grande, tal como en los sistemas de antibloqueo de
frenado en los automviles que detectan la velocidad de rotacin de la rueda a
travs de ranuras en los discos de las ruedas.
Los magnetmetros se caracterizan por ser muy susceptibles a errores, debido principalmente a la contaminacin electromagntica [45].
6.3.4.
Sensores de distancia
46
6.4.
MATLAB
MATLAB (abreviatura de MATrix LABoratory, laboratorio de matrices) es un software matemtico que ofrece un entorno de desarrollo integrado (IDE) con un lenguaje
de programacin propio (lenguaje M). Desarrollado por MathWorks, est disponible
para las plataformas Unix, Windows y Mac OS X.
MATLAB permite manipular matrices, hacer grficas de funciones y datos, implementar algoritmos, creacin de interfaces de usuario, e interfaces con programas escritos
en otros lenguajes de programacin entre los que se que se encuentran C, C++, Java
y Fortran. Aunque MATLAB est diseado principalmente para la computacin numrica, una toolbox opcional usa el motor simblico MuPAD, permitiendo el acceso
a las capacidades de clculo simblico.
El paquete adicional Simulink, aade un entorno grfico de simulacin multidomino y
de diseo basado en modelo de sistemas de sistemas dinmicos y sistemas empotrados
[49].
6.4.1.
Simulink
Simulink es un entorno para la simulacin multidominio y el diseo basado en modelos para sistemas dinmicos y embebidos. Presenta un entorno grfico interactivo y
47
48
7.
Mtodos
En este capitulo se explica como se disearon las diferentes partes del sistema: modelo
dinmico, unidad de medicin inercial, filtro de Kalman y controlador (ver figura 23).
Adems se describe cmo se ha realizado la simulacin mediante el software Simulink
de MathWorks.
7.1.
Antes de comenzar con la obtencin del modelo dinmico del quadrotor se realiza una
introduccin a los sistemas de referencia utilizados y las transformaciones para pasar
de uno a otro. Despus se describen las diferentes fuerza y momentos que actan sobre
el quadrotor. Esta seccin concluye con la obtencin del modelo dinmico completo
del quadrotor.
7.1.1.
Sistema de ejes
51
El uso de los ngulos de Tait-Bryan est muy extendido a causa de que numerosas
organizaciones han publicado diversos estndares para ser seguidos debido a la especial
importancia de las convenciones internacionales en vehculos areos. Uno de estos
estndares, la norma DIN 9300 para la aeronavegacin, fue adoptada por la ISO
como ISO 1151-2:1985 [43].
7.1.2.
ox3
1
oy3 = 0
oz3
0
ox2
0
sen oy2 .
cos
oz2
(7.1)
ox1
0 sen
oy1 .
1
0
0 cos
oz1
(7.2)
0
cos
sen
ox2
cos
oy2 = 0
oz2
sen
ox1
cos
oy1 = sen
oz1
0
sen
cos
0
0
ox0
0 oy0 .
1
oz0
(7.3)
ox3
1
oy3 = 0
oz3
0
0
c
s
0
c
s 0
c
s
0 s
c
1
0 s
0 c
0
s
c
0
0
ox0
0 oy0
1
oz0
(7.4)
ox3
ox0
oy3 = D oy0
oz3
oz0
52
(7.5)
cc
D = ssc cs
csc + ss
cs
sss + cc
css sc
s
sc .
cc
(7.6)
La transformacin de (ox3 y3 z3 ) a (ox0 y0 z0 ) es obtenida invirtiendo la matriz de cosenos directores D y se obtiene a travs de la siguiente ecuacin
ox0
ox3
oy0 = D1 oy3
oz0
oz3
(7.7)
D1
cc
= sc
s
css sc csc + ss
sss + cc ssc cc
cs
cs
(7.8)
Transformacin de velocidades angulares. La aplicacin ms til de la transformacin de cantidades angulares relaciona las velocidades angulares p, q y r en los
y
ejes del sistema de referencia mvil y las componentes de la velocidad angular, ,
p = sen
(7.9)
q = cos
sencos
(7.10)
r = coscos
sen
53
(7.11)
Las ecuaciones (7.9), (7.10) y (7.11) pueden combinarse en notacin matricial como
p
1
q = 0
r
0
sen
sencos
coscos
0
cos
sen
(7.12)
1
= 0
0
sentg
cos
sencos
costg
p
sen q .
cossec
r
(7.13)
Cuando los ngulos , y son pequeos las ecuaciones (7.12) y (7.13) pueden
aproximarse como
p =
q = .
r =
7.1.3.
(7.14)
Fuerzas
0
E ~
Fg = 0 .
mg
(7.15)
sen
b~
Fg = DE F~g = mg sencos .
coscos
(7.16)
(7.17)
Si se considera que los rotores estn perfectamente alineados con respecto al sistema
de referencia mvil la direccin de la fuerza de empuje tendr como direccin el eje
zb , siendo la componente en el resto de ejes nula.
Numerando cada rotor como j = 1..,4, el empuje total puede ser descrito como
Tz =
4
X
(7.18)
Tj .
j=1
La fuerza de empuje generada por los rotores que se mueven solidarios al marco de
referencia mvil ser
0
b ~
FT = 0 .
Tz
(7.19)
Cx
1
E~
Fd = Ac 0
2
0
0
Cy
0
0
x |x|
0 y |y|
Cz
z |z|
(7.20)
Momentos
l
b ~
MT = 0
0
0 0
T4 T2
l(T4 T2 )
l 0 T3 T1 = l(T3 T1 )
0 0
0
0
55
(7.21)
0
b ~
.
0
Md =
Karrastre (T2 + T4 T1 T3 )
(7.22)
Efecto giroscpico de los rotores. Los cuatro rotores inducen momentos debidos
a efectos giroscpicos por su velocidad angular, j , siendo un mecanismo adicional
que hacen al quadrotor rotar respecto al eje zb , movimiento de guiada. El efecto
giroscpico tambin afecta a los otros dos ejes aunque de forma menos intensa.
Si se considera un nico rotor j, el momento resultante de la interaccin entre el rotor
y la rotacin del quadrotor est dado por la ecuacin
b
~ (I~
~ j = dLo + L = I +
)
M
g
dt S
(7.23)
Sustituyendo, se obtiene
Ixx xj
b ~ j
Mg = Iyy yj +
Izz zj
Ix xj
Iy yj .
Iz zj
(7.24)
El giro de los rotores j coincide con el eje zb por lo que las componentes xj y yj son
cero. Resolviendo el producto vectorial
0
b ~ j
Mg = 0 +
Izz j
0
Iz j
0 = Iz j
Izz j
Iz j
(7.25)
Si se tienen en cuenta todos los rotores, las velocidades angulares deben ser sumadas
teniendo en cuenta su respectivo signo. Si adems se asume que todos los rotores son
56
iguales y por tanto el momento de inercia de estos Izz , al que se denotar a partir de
ahora como Jr , es el mismo para todos definimos Lr como
Lr = Jr
4
X
j = Jr (1 2 + 3 4 ) .
(7.26)
j=1
Lr
b ~
Mg = Lr .
L r
7.1.5.
(7.27)
Dinmica
d(mv)
,
F~ =
dt
(7.28)
dv
F~ = m .
dt
(7.29)
Generalizando para un slido rgido compuesto de tales partculas, cada una con una
masa infinitesimal dm y con un vector de posicin ~r
X
d2
F~ = 2
dt
(7.30)
~rdm
Sea M la masa total del slido rgido, la cual es constante, se puede multiplicar y
dividir por M el lado derecho
X
d2
F~ = M 2
dt
~
r dm
~rdm
M
(7.31)
F~ = M ~rcdg
(7.32)
F~ = M ~rcdg
(7.33)
~
~ O = dHO
M
dt
(7.34)
Expresado en trminos de las componentes del vector de momentos angulares, aplicando el teorema del momento cinemtico, se puede escribir la ecuacin como
X
(7.35)
0
= z
y
z
0
x
y
x .
0
(7.36)
1
M~O [I] [] [I]
~
(7.37)
Ixx
Ixx
l(T3 T1 )
Izz Ixx
=
+ Lr +
Iyy
Iyy
58
(7.38)
Izz
Izz
7.2.
La actitud del quadrotor se calcular a travs de las mediciones, donde estas sern
obtenidas a partir de los sensores con los que est equipado el quadrotor. El giroscopio
no dar el ritmo de cambio de los diferentes ngulos de Tait-Bryan, mediante los
acelermetros se corregir el ngulo de alabeo () y de cabeceo (), y el magnetmetro
se utilizara corregir la guiada (). Para realizar la fusin de los datos de los diferentes
sensores se utilizar el algoritmo propuesto por en el artculo de Euston et al. [14].
Como se analiz en la seccin dedicada a las matrices de cambio de base, en la
pgina 52, una matriz de rotacin de rotacin describe la orientacin de un marco
de referencia respecto a otro. Las columnas de la matriz son los vectores directores
unitarios de un marco respecto del otro.
rxx
R = ryx
rzx
rxy
ryy
rzy
rxz
ryz
rzz
(7.39)
t
d~ ( ) ~r ( )
~r (t) = ~r (0) +
(7.40)
d~ ( ) =
~ ( ) d
La estrategia ser aplicar la ecuacin (7.40) a la filas o columnas de la matriz de
rotacin R, tratndolos como vectores que giran.
El primer problema que aparece es que los vectores que se dedea seguir, y el vector de
rotacin, no se miden en el mismo marco de referencia. Idealmente, se deseara seguir
los ejes del quadrotor en el marco de referencia fijo. Hay una solucin fcil mediante
el reconocimiento de la simetra en la rotacin. En el sistema de referencia mvil,
la rotacin del marco de referencia fijo es igual y de sentido opuesto a la rotacin
del sistema de referencia mvil respecto al fijo. Se pueden seguir los ejes de la tierra
vistos desde el sistema mvil cambiando el signo de la seal de los giroscopios. Por
comodidad se pueden dejar los signos iguales e intercambiar los factores del producto
vectorial.
t
~re ( ) d~ ( )
(7.41)
d~ ( ) =
~ ( ) d
Los vectores de la ecuacin (7.41) son las filas de la matriz de rotacin R. A la hora de
implementar la ecuacin (7.41) se utiliza la misma aproximacin que realiz Euston
en sus artculo [14]. Se comienza volviendo a la ecuacin (7.41)
~re (t + dt) = ~re (t) + ~re (t) d~ (t)
(7.42)
d~ (t) =
~ (t) dt
Cuando se repite la ecuacin (7.42) en cada uno de los ejes del marco de referencia
fijo, se puede expresarlo matricialmente como
1
R (t + dt) = R (t) z dt
y dt
z dt
1
x dt
y dt
x dt .
1
(7.43)
Los errores numricos harn que la matriz R deje de ser ortogonal. Al proceso de
mantener las condiciones de ortogonalidad renormalizacin. Se comienza calculando
el producto escalar de las filas X e Y de la matriz, el cual debera ser cero. El resultado
indicar cunto estn rotando las filas X e Y entre ellas
rxx
X = rxy
rxz
ryx
Y = ryy
ryz
error = XY = X T Y = rxx
rxy
ryx
rxz ryy
ryz
(7.44)
Se aporta la mitad del error a cada una de las filas X e Y, y se rotan aproximadamente
las filas X e Y en direcciones opuestas
rxx
rxy = Xort = X error Y
2
rxz ort
(7.45)
ryx
ryy = Yort = Y error X
2
ryz ort
(7.46)
(7.47)
El ltimo paso de la renormalizacin es escalar las filas de la matriz R para que sean
vectores unitarios. Se divide cada elemento de la fila entre la raz cuadrada de la suma
de los cuadrados de los elementos de esa fila
Xnorm =
p
Xort Xort
Ynorm =
Znorm =
61
Yort Yort
Zort Zort
(7.48)
7.2.1.
Correccin de la deriva
Aunque los giroscopios tienen un buen rendimiento, con una desviacin del orden de
unos pocos grados por segundo, se obtendra lo que se conoce como deriva. Se utilizan
vectores de referencia para detectar la desviacin de los giroscopios y proporcionar
una retroalimentacin negativa que corrija esta desviacin. El principal requisito de
los vectores de referencia de orientacin es que no tengan deriva.
Los dos vectores de referencia que se utilizan los proporcionarn el acelermetro y el
magnetmetro.
El error de orientacin se calcula tomando el producto vectorial del vector medido con
el vector estimado por la matriz de cosenos directores. La magnitud del resultado ser
proporcional al seno del ngulo formado por los dos vectores y perpendicular a ambos.
Representa un eje de rotacin y la cantidad de rotacin necesaria para que el vector
medido sea paralelo al estimado. Retroalimentaremos la seal de los giroscopios a
travs de un controlador PI de manera que la orientacin estimada siga gradualmente
a los vectores de referencia y se cancele la deriva de los giroscopios.
Los acelermetros se utilizan para la correccin del alabeo y el cabeceo y estos nos
proporcionan un vector de referencia para el eje zb .
Los acelermetros miden adems del vector gravedad las aceleraciones en el marco de
referencia mvil. Por tanto, sern tiles para estimar el alabeo y el cabeceo cuando
no existan aceleraciones en el marco de referencia mvil.
El vector de correccin del alabeo y cabeceo en el sistema de referencia mvil se
calcula tomando el producto escalar del la fila Z de la matriz de cosenos directores
con el vector unitario gravedad de referencia
rzx
Acelx
errorAlabeoCabeceo = rzy Acely
rzz
Acelz
(7.49)
1
R = 0
0
0
cos
sen
0
cos
sen 0
cos
sen
0 sen
1
0 ,
0 cos
(7.50)
mag = arctan
magy
magx
.
(7.51)
(7.52)
(7.53)
Controlador de retroalimentacin PI
(7.54)
(7.55)
correcion = P + I
En este punto, el vector de correccin, se aade al vector de seal de los giroscopios
y se inicia el proceso de nuevo.
Los ngulos de Tait-Bryan se obtinen a partir de la matriz de cosenos directores de
la siguiente forma
r
= arctan rzy
zz
= arcsen
(rzx )
= arctan
63
ryx
rxx
(7.56)
7.3.
Filtro de Kalman
x=
(7.57)
En la ecuacin (7.38) se tena el modelo dinmico, donde ahora se escriben las ecuaciones en funcin de las variables de entrada , , y ug . Adems, se implifica
I I
I I
la notacin de los momentos de inercia siendo Ixyz = xxIzz yy , Iyzx = yyIxx zz e
xx
Izxy = IzzII
,
yy
r
= Ixx
+ JIxx
ug + Iyzx
= + Ixyz
(7.58)
Izz
T2 +T4 T1 T3
.
Kempuje
= Ixx
= Iyy
.
= Izz
(7.59)
(7.60)
donde
0
0
0
A=
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
(7.61)
y
0
0
0
0
I1
xx
0
B=
0
0
0
1
Iyy
0
0
0
0
0
0
0
(7.62)
1
Izz
Como las variables son medidas en tiempo discreto, se utilizar un filtro de Kalman
discreto. Se asume que el tiempo de muestreo Ts es lo suficientemente pequeo como
para que la discretizacin de Euler sea usada de manera efectiva en este modelo. El
modelo lineal discreto es
(7.63)
xk+1 = Ad xk + Bd uk ,
con
1
0
0
Ad =
0
0
0
0
1
0
0
0
0
0
0
1
0
0
0
Ts
0
0
1
0
0
0
Ts
0
0
1
0
0
0
0
0
0
0
0
0
0
0
Ts
0
1
(7.64)
s
ITxx
0
Bd =
0
0
0
Ts
Iyy
0
0
(7.65)
Ts
Izz
El vector de entrada uk vendr dado por los momentos provocados por la diferencia
de empuje entre los rotores
65
uk = .
(7.66)
IM U
IM U
IM U
.
zk =
0
0
0
7.4.
(7.67)
T
2
T
2
T
2
T
2
+
+
T
4
T
4
T
4
T
4
(7.68)
El empuje generado por los motores no puede ser negativo nunca, con el objetivo de
que esto no ocurra se han limitado las seales de control mediante el uso de funciones
de saturacin.
7.4.1.
T L
=
Ixx
(7.69)
T L
=
.
Iyy
(7.70)
Como se expone en el artculo de Hoffman et al. [17], a bajas velocidades y con perturbaciones pequeas, como por ejemplo en vuelo en interiores, la funcin de transferencia
que describe el comportamiento del quadrotor se aproxima a un doble integrador con
un retardo de primer orden debido a la dinmica de los motores.
Si se toma la transformada de Laplace de ambos miembros de las ecuaciones (7.69)
y (7.70), se supone que todas las condiciones iniciales son cero, y se aade el retardo
de primer orden se llega a las siguientes funciones de transferencia
G (s) =
L
(s)
=
2
T (s)
Ixx s (T s + 1)
(7.71)
G (s) =
L
(s)
=
.
T (s)
Iyy s2 (T s + 1)
(7.72)
(s)
L (Kp + Kd s)
=
T (s)
Ixx s2 (T s + 1)
(7.73)
G (s) =
(s)
L (Kp + Kd s)
=
.
T (s)
Iyy s2 (T s + 1)
(7.74)
(s)
L (Kp + Kd s)
=
T (s)
Ixx s2 (T s + 1) + L (Kp + Kd s)
(7.75)
G (s) =
(s)
L (Kp + Kd s)
=
.
2
T (s)
Iyy s (T s + 1) + L (Kp + Kd s)
(7.76)
67
7.4.2.
Controlador de guiada
(7.77)
Karrastre
(s)
=
.
T (s)
Izz s2 (T s + 1)
(7.78)
(s)
Karrastre (Kp + Kd s)
=
.
T (s)
Izz s2 (T s + 1)
(7.79)
7.5.
(s)
Karrastre (Kp + Kd s)
=
.
F (s)
Izz s2 (T s + 1) + Karrastre (Kp + Kd s)
(7.80)
Modelado en Simulink
El modelador y simulacin del sistema completo se realiz en Simulink. Para el modelado en Simulink se dividi el sistema completo en cinco subsistemas principales:
Modelo dinmico del quadrotor, a partir del empuje generado por los rotores
determina el comportamiento dinmico del quadrotor.
Simulacin de los sensores, la salida de este subsistema recrea la seal de los
sensores con los que est equipado el quadrotor real.
Unidad de medicin inercial, a partir de la seal de los sensores obtiene una
aproximacin de la orientacin del quadrotor.
68
g = 9.81;
% Aceleracion de la gravedad [ m / s2 ]
2
3
4
5
6
7
8
9
10
masa = 2.85;
% Masa del quadrotor [ kg ]
L = .2;
% Distancia entre los rotores y el cdg
[m]
T_inercia = [.05 0 0;
% Tensor de inercia
0 .05 0;
% del quadrotor
0 0 .15]; % [ kg / m ^2]
I_rotor = 6*10^ -5;
% Momento de inercia del rotor
K_empuje = 0.2*10^ -3;
% Relacin entre la velocidad del rotor
y el empuje que genera [ N / w ^2]
K_arrastre = .05;
% Relacin entre el empuje y el momento
en un rotor
11
12
13
Ini_pos = [0 0 0] ;
quadrotor
Ini_ang = [0 0 0] ;
69
14
15
16
17
18
Kp = 70;
Kd = 5;
Kp_yaw = 100;
Kd_yaw = 22;
7.5.1.
rotado para obtener sus componentes respecto al sistema de referencia mvil solidario
al quadrotor (ver figura (30)).
Los momentos causados por diferencia de empuje generado por los diferentes rotores
se calculan en el subsistema de la figura 32.
Los efectos giroscpicos producidos por el giro de los rotores se calculan en otro
subsistema, figura 33.
71
7.5.2.
Sensores
Este subsistema, a partir de la posicin real del quadrotor, estima la salida de los
sensores con los que esta equipado el quadrotor real: acelermetro, giroscopio y magnetmetro (ver figura (35)). Se aade el ruido blanco adictivo y en el caso de los
giroscopios una desviacin. Las caractersticas de este ruido han sido obtenidas experimentalmente a partir de muestras de mediciones de los sensores.
72
La unidad de medicin inercial es una implementacin del algoritmo descrito en anteriormente y realizada mediante una funcin en lenguaje de programacin M. La
funcin es llamada con una frecuencia de 100 Hz.
En la primera ejecucin se inicializan las variables que sern utilizadas en las siguientes
llamadas a la funcin. Estas variables se almacenan como variables del tipo persistent.
Las variables de este tipo son locales a la funcin que las ha declarado y su valor es
almacenado entre llamadas a la funcin. Se diferencian de las variables globales en
que son solamente conocidas por la funcin que las ha declarado (ver listado 2).
1
2
3
4
5
%
%
%
%
%
6
7
8
9
10
11
giro = u (1:3) ;
acel = u (4:6) ;
mag = u (7:9) ;
12
13
14
15
IMU_Kp = .1;
73
16
IMU_Ki = .0001;
17
18
IMU_Kp_yaw = 1.2;
19
20
21
22
23
24
25
26
27
28
29
30
31
else
32
33
34
35
36
% Calculo el error
X = R (: ,1) ;
Y = R (: ,2) ;
error = X * Y ;
37
38
39
40
41
42
43
44
45
46
% Normalizo
X_norm = X_orth ./ sqrt ( sum ( X_orth .^2) ) ;
Y_norm = Y_orth ./ sqrt ( sum ( Y_orth .^2) ) ;
Z_norm = Z_orth ./ sqrt ( sum ( Z_orth .^2) ) ;
47
48
49
% Saco X corregida
R = [ X_norm Y_norm Z_norm ];
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
% Peso de la aceleracin
if abs ( atan2 ( R (3 ,2) ,R (3 ,3) ) ) > .1745 ||
> .1745
74
AccelWeight = 0;
elseif 1 - 10* abs (1 - ( sqrt ( sum ( acel .^2) ) /9.81) ) < 0
AccelWeight = 0;
else
AccelWeight = 1 - 10* abs (1 - ( sqrt ( sum ( acel
.^2) ) /9.81) ) ;
end
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
end
83
84
85
7.5.4.
Filtro de Kalman
Como era el caso de la unidad de medicin inercial, para la implementacin del filtro
de Kalman, se ha recurrido al uso de una funcin en lenguaje M. Esta funcin tambin
es ejecutada con una frecuencia de 100 Hz (ver listado 3).
En la primera llamada se definen las diferentes matrices que forman parte de este
algoritmo recursivo. stas se almacenan en variables de tipo persistent para que se
encuentren disponibles en la siguiente llamada a la funcin.
1
2
3
%
%
%
FILTRO KALMAN
4
5
6
7
persistent x A B H P Q R dt
8
9
QuadrotorVar ;
10
11
12
13
14
15
16
17
18
19
if isempty ( P )
75
20
% El estado inicial es
x = [0 0 0 0 0 0] ;
21
22
23
% Paso
dt = 0.01;
24
25
26
27
28
29
% A y B
A = [1
0
0
0
0
0
30
31
32
33
34
35
36
representan el modelo
0 0 dt 0 0
1 0 0 dt 0
0 1 0 0 dt
0 0 1 0 0
0 0 0 1 0
0 0 0 0 1];
37
B = [0
0
0
0
0
0
38
39
40
41
42
43
0
0
0
0
0
0
0
0
0
0
0
0
0 0 0
0 0 0
0 0 0
dt / T_inercia (1 ,1) 0 0
0 dt / T_inercia (2 ,2) 0
0 0 dt / T_inercia (3 ,3) ];
44
% H representa
H = [1 0 0 0 0
0 1 0 0 0
0 0 1 0 0
0 0 0 0 0
0 0 0 0 0
0 0 0 0 0
45
46
47
48
49
50
51
la matriz de medicin
0
0
0
0
0
0];
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
else
R = [.01 0
0 0 0 0
0 .01 0 0 0 0
0
0 .01 0 0 0
0
0
0 1 0 0
0
0
0 0 1 0
0
0
0 0 0 1];
% Filtro Kalman
69
70
71
72
% Etapa de prediccion
x = A * x + B *[0 0 0 par_roll par_pitch par_yaw ] ;
P = A * P *A + Q ;
73
74
75
76
76
77
78
79
80
81
end
%
K
x
P
Etapa de actualizacin
= P *H * pinv ( H * P *H + R ) ;
= x + K *( z - H * x ) ;
= ( eye (6) -K * H ) * P ;
82
83
7.5.5.
Controlador
77
78
Parte IV
Resultados y discusin
8.
Resultados y discusin
Para evaluar el desempeo del sistema se realizaron diferentes ensayos sobre cada uno
de los componentes. Se comienza por la unidad de medicin inercial, para continuar
con el filtro de Kalman y el controlador PD.
En este captulo se exponen los resultados de los ensayos realizados, as como el
procedimiento usado para realizar el ajuste de los diferentes componentes del sistema.
8.1.
8.1.1.
0.25
Alabeo [rad]
0.2
0.15
0.1
0.05
0
0.05
0.2
0.4
0.6
0.8
1
1.2
Tiempo [s]
1.4
1.6
1.8
1.5
Acel.
Real
Error
Alabeo [rad]
0.5
0
0.5
1
1.5
10
15
Tiempo [s]
20
25
30
Figura 39: estimacin del alabeo de la IMU con la ponderacin de la correccin original
En el algoritmo original de la unidad de medicin inercial, la ponderacin del error
en alabeo y cabeceo (W ), se calcula a partir del mdulo de la aceleracin medida
por el acelermetro. Cuando esta es mayor que 1,5 g o menor 0,5 g se deja de realizar
la correccin. Es estos casos el valor de W es 0. Entre esos dos valores se calcula el
peso de la correccin como
W
k~ak
.
= 1 2
9, 81
(8.1)
0.08
Error
0.07
Error IMU Alabeo [rad]
0.06
0.05
0.04
0.03
0.02
0.01
0
0.01
10
15
Tiempo [s]
20
25
30
Respuesta en alabeo
Para validar la estimacin del ngulo de alabeo y cabeceo realizada por la unidad de
medicin inercial, el sistema, fue expuesto a un tren de pulsos de amplitud creciente.
Durante los 3 primeros segundos el ngulo de alabeo es 0 radianes, a partir de ese
momento se introduce un tren de pulsos compuesto por 4 pulsos con un valor mximo
de 0,1 radianes y un mnimo de -0,1 radianes. Tras el primer tren de pulsos, durante
1 segundo, se vuelve a la posicin de equilibrio para, a continuacin, repetir el tren
de pulsos con valores de 0,3 y -0,3 radianes y por ltimo con valores de 0,5 y -0,5
radianes.
En las grficas de las figuras 41 y 42 se aprecia como la fuente del error vara en
funcin del ngulo de alabeo a causa del algoritmo de ponderacin de la correccin
del alabeo y del cabeceo. Durante los primeros pulsos de 0,2 radianes de amplitud, el
controlador PI provoca un error en el ngulo de alabeo al intentar seguir al ngulo
estimado por el acelermetro, el cual no es correcto. En los pulsos de 0,6 radianes y
81
1.5
Acel.
Real
IMU
Alabeo [rad]
0.5
0
0.5
1
1.5
10
15
Tiempo [s]
20
25
30
8.1.3.
Respuesta en guiada
La entrada a la que se someti al sistema para determinar el rendimiento en la estimacin realizada por la unidad de medicin inercial de la guiada es similar a la usada
en alabeo-cabeceo, pero en este caso los pulsos reducen su nmero y frecuencia. El
sistema comienza con 3 segundos a 0 radianes para ser sometido a un tren de impulsos
de amplitud creciente de amplitud 0,2, 0,6 y 1 radianes con un periodo de 8 segundos
cada pulso.
Como se puede apreciar en las grficas de las figuras 43 y 44, el error en la estimacin
del ngulo de guiada es pequeo en comparacin con el que se tiene en la estimacin
del alabeo y el cabeceo. Adems, al contrario que estos, el error no vara con la
amplitud del pulso al que se ve sometido el sistema.
En la tabla 11 se realiza un resumen de las caractersticas de la estimacin del ngulo
de alabeo realizada por la unidad de medicin inercial.
82
0.08
Error
0.06
0.04
0.02
0
0.02
0.04
10
15
Tiempo [s]
20
25
30
0.8
Magnet.
Real
IMU
0.6
Guinada [rad]
0.4
0.2
0
0.2
0.4
0.6
0.8
10
15
Tiempo [s]
20
25
30
83
20
x 10
Error
15
10
10
15
Tiempo [s]
20
25
30
8.2.
Filtro de Kalman
Las pruebas utilizadas para determinar el desempeo del filtro de Kalman son las
mismas que se realizaron a la unidad de medicin inercial. Adems, con el fin de
comprobar los efectos de las no linealidades sobre el estimador lineal, se realizo otro
ensayo en el que se combinaba un movimiento en alabeo con otro en guiada.
Para que el error en la estimacin de la orientacin realizado por la unidad de medicin no afectase a los resultados del filtro de Kalman, se us como vector medida la
orientacin real del quadrotor a la que se le aadi ruido blanco aditivo de varianza
0,1.
8.2.1.
0, 01
0
0
0,
01
0
0
R=
0
0
0
0
0
0
0
0
0
0
0, 01 0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
(8.2)
0.05
Coste
0.04
0.03
0.02
0.01
5
10
10
6
10
8
10
10
Qbx
10
10
10
Qax
Figura 45: valor de la funcin coste en el eje x para diferentes valores de Qax y Qbx
usando datos de una simulacin previa como juego de entrenamiento. La funcin de
coste escogida fue el error cuadrtico medio, que viene dado por
rP
(xest i xreal i )2
(8.3)
errorRM S =
N
Los valores a determinar de la matriz de covarianza del error Q eran
Qax
0
0
Q=
0
0
0
0
Qay
0
0
0
0
0
0
Qaz
0
0
0
0
0
0
Qbx
0
0
0
0
0
0
Qby
0
0
0
0
.
0
0
Qbz
(8.4)
0
Q=
0
0
0
0
107
0
0
0
0
0
0
106
0
0
0
85
0
0
0
106
0
0
0
0
0
0
106
0
0
0
0
108
(8.5)
1
Medicin
Real
Kalman
Alabeo [rad]
0.5
0.5
10
15
Tiempo [s]
20
25
30
Respuesta en guiada
La entrada a la que se someti al sistema para determinar el rendimiento en la estimacin realizada por el filtro Kalman de la guiada es similar a la usada en alabeo86
0.12
Error
0.1
0.08
0.06
0.04
0.02
0
0.02
0.04
0.06
10
15
Tiempo [s]
20
25
30
cabeceo. El sistema comienza con 3 segundos a 0 radianes para ser sometido a un tren
de impulsos de amplitud creciente de amplitud 0,2, 0,6 y 1 radianes con un periodo
de 8 segundos cada pulso.
Una vez ms, como se puede apreciar en las grficas de las figuras 48 y 49, la mayor
desviacin se produce durante la inicializacin del filtro. En este caso, la amplitud del
pulso no afecta al error en la estimacin. Si se recuerdan las ecuaciones diferenciales no
lineales que rigen el comportamiento en guiada, se observa cmo las no linealidades
son de segundo orden, por lo que su influencia es limitada.
En la tabla 13 se realiza un resumen de las caractersticas de la estimacin de ngulo
de guiada realizada por el filtro de Kalman.
8.2.4.
0.8
Medicin
Real
Kalman
0.6
Guiada [rad]
0.4
0.2
0
0.2
0.4
0.6
0.8
10
15
Tiempo [s]
20
25
30
0.1
Error
Error Kalman Guiada [rad]
0.08
0.06
0.04
0.02
0
0.02
0.04
0.06
10
15
Tiempo [s]
20
25
30
88
0.6
Medicin
Real
Kalman
0.4
Alabeo [rad]
0.2
0
0.2
0.4
0.6
0.8
10
Tiempo [s]
12
14
16
18
20
Figura 50: estimacin del alabeo del filtro de Kalman cuando aumenta el peso de los
trminos no lineales
Tabla 14: caractersticas de la estimacin realizada por el filtro de Kalman cuando
aumenta el peso de los trminos no lineales
Alabeo () Guiada ()
Error cuadrtico medio 0,0273 rad
0,0184 rad
Error medio
0,0163 rad
-0,0029 rad
Error mximo
0,1153 rad
0,0872 rad
Como se observa en la grfica de la figura 50 y en la grfica del error de la estimacin
del alabeo de la figura 52, la influencia de los trminos no lineales en la estimacin
del alabeo se hace notable cuando la amplitud de los pulsos aumenta. No ocurre los
mismo en la guiada donde, como se explic anteriormente, los trminos no lineales
tienen un peso mucho menor, como se puede ver en las grficas de las figuras 51 y 53.
En la tabla 14 se resumen las caractersticas de la estimacin realizada por el filtro
de Kalman cuando la influencia de los trminos no lineales aumenta. El aumento del
error cuadrtico medio es notable en la estimacin del alabeo, se intua por la grfica
de la figura 50, mientras que el error cuadrtico en la estimacin de la guiada se
mantiene prcticamente igual.
8.3.
8.3.1.
Controlador
Ajuste del controlador
Para realizar el ajuste del controlador PD se opt por usar la simulacin con las
funciones de transferencia obtenidas anteriormente, ecuaciones (7.71), (7.72) y (7.78)
89
0.5
Guinada [rad]
Medicin
Real
Kalman
0.5
10
Tiempo [s]
12
14
16
18
20
Figura 51: estimacin de la guiada del filtro de Kalman cuando aumenta el peso de
los trminos no lineales
0.12
Error
0.1
0.08
0.06
0.04
0.02
0
0.02
0.04
0.06
10
Tiempo [s]
12
14
16
18
20
Figura 52: error en la estimacin del alabeo del filtro de Kalman cuando aumenta el
peso de los trminos no lineales
90
0.1
Error
Error Kalman Guiada [rad]
0.08
0.06
0.04
0.02
0
0.02
0.04
0.06
10
Tiempo [s]
12
14
16
18
20
Figura 53: error en la estimacin de la guiada del filtro de Kalman cuando aumenta
el peso de los trminos no lineales
en lugar de realizar la simulacin con modelo dinmico completo que se deriv tambin
previamente. El modelo utilizado es el de la figura (54).
Esta eleccin se realizo debido a que la velocidad de ejecucin de la simulacin sobre el
modelo dinmico completo es muy lenta y, como se ver a continuacin, las funciones
de transferencia obtenidas se ajustan bastante bien al proceso.
Por las caractersticas del sistema no fue posible ajustar el controlador PD usando
uno de los dos mtodos de Ziegler-Nichols. Al poseer un doble integrador el primer
mtodo no es vlido y, como la funcin de transferencia tiene slo dos polos en el
origen, el sistema oscila constante para cualquier valor de Kp por lo que el segundo
mtodo tampoco se poda aplicar. Como consecuencia el ajuste de las ganancias del
controlador se hizo de manera manual.
0.6
r(t)
y(t)
0.5
Alabeo [rad]
0.4
0.3
0.2
0.1
0
0.1
3
Tiempo [s]
Figura 55: respuesta a entrada pulso del controlador PD usando la funcin de transferencia
Con el ajuste del controlador se busc un tiempo de asentamiento lo suficientemente
rpido sin que se superase una oscilacin del 10 %. Las ganancias del controlador PD
de alabeo-cabeceo y del controlador de guiada se muestran en la tabla 15.
Como se observa en la grfica de la figura 55 de la respuesta a una entrada pulso de
0,5 radianes y 2 segundos de duracin, el sobrepaso es del 11,96 % siendo el tiempo
de asentamiento de 0,37 segundos.
8.3.2.
Respuesta a escaln
Tras la obtencin de unos valores para las ganancias del controlador usando la funcin
de transferencia, se evalu el rendimiento del controlador usando esta vez el modelo
dinmico completo.
Aunque la idea inicial era realizar un ajuste fino del controlador partiendo de los
valores obtenidos previamente, tras la realizacin de los primeros ensayos, se decidi
que no era necesario. Como se puede apreciar al comparar los valores de sobrepaso
y de tiempo de asentamiento de las grficas de las figuras 55 y 56, la respuesta del
modelo dinmico es prcticamente igual a la de la funcin de transferencia.
La respuesta del sistema a una entrada de tipo escaln de 0,5 radianes en alabeo y en
guiada se puede ver en las grficas de las figuras 56 y 57. El sobrepaso y el tiempo
de asentamiento del sistema frente a la entrada escaln se encuentra en la tabla 16.
92
0.6
r(t)
y(t)
0.5
Alabeo [rad]
0.4
0.3
0.2
0.1
0
0.1
0.5
1.5
2.5
Tiempo [s]
3.5
4.5
0.6
r(t)
y(t)
0.5
Guiada [rad]
0.4
0.3
0.2
0.1
0
0.1
0.5
1.5
2.5
Tiempo [s]
3.5
4.5
93
0.07
r(t)
y(t)
0.06
Alabeo [rad]
0.05
0.04
0.03
0.02
0.01
0
0.01
0.5
1.5
Tiempo [s]
2.5
Respuesta perturbaciones
Entre los objetivos del proyecto se encontraba que el vuelo del quadrotor fuese esttico,
autnomo y estable frente a perturbaciones externas. Para comprobar este ltimo
punto se le aplic al quadrotor un par sobre el eje xb de 1 Nm durante 0,1 segundos.
La respuesta del controlador a esta perturbacin se puede apreciar en la grfica de la
figura 58, siendo por tanto, una respuesta aceptable para este problema de control.
Desde que el momento deja de aplicarse el sistema tarda 0,34 segundos en volver
al punto de consigna, producindose la mxima desviacin a los 0,13 segundos de
haberse aplicado el momento, con un valor de 0,069 radianes.
94
Parte V
Conclusiones
9.
Conclusiones
Sobre el modelo dinmico del quadrotor diseado en Simulink, se simul el comportamiento de los sensores y se hicieron correr los algoritmos de la unidad de medicin
inercial, del filtro de Kalman y del controlador PD.
La unidad de medicin inercial estima los ngulos de Tait-Bryan a partir de los sensores. El algoritmo utilizado es el propuesto por Euston et al. [14], el cual est diseado
para la estimacin de la orientacin en aeronaves de ala fija. En el artculo de Euston
et al., a la aceleracin medida por los acelermetros se le resta la aceleracin centrpeta, mientras que en el algoritmo que se implemento esto no era posible al no
tener la referencia externa proporcionada por un GPS o un sensor de flujo ptico, lo
que afect a la calidad de la estimacin. Aun as en alabeo el error cuadrtico medio
obtenido en los ensayos fue de 0,0281 radianes (1,6100 ), llegando a un error mximo
en la estimacin de 0,0659 radianes (3,7758 ) cuando el alabeo es de 0,5 radianes
(28,6479 ).
Con la orientacin obtenida por la unidad de medicin inercial y el modelo dinmico,
el filtro de Kalman realiza una estimacin de la orientacin. El modelo dinmico
del quadrotor se rige por ecuaciones diferenciales no lineales, por lo que al usar un
estimador lineal dicho modelo se linealiz respecto al punto de equilibrio. Los efectos
de la aproximacin realizada al linealizar se hacen notables al combinar el movimiento
en alabeo o cabeceo con una rotacin en guiada. En este caso el error cuadrtico
medio en la estimacin del alabeo fue de 0,0273 radianes (1,5641 ). Cuando el peso
de los trminos no lineales no tiene tanta influencia, el error cuadrtico medio en la
estimacin del alabeo es de 0,0154 radianes (0,8824 ). Como se puede observar, el
error cuadrtico medio, an en condiciones desfavorables, de la estimacin realizada
por el filtro de Kalman es menor que el de la estimacin realizada por la unidad de
medicin inercial.
Para controlar la orientacin se recurri a tres controladores PD, uno por cada eje
de rotacin. Los criterios de diseo fueron lograr el menor tiempo de asentamiento
sin que el sobrepaso superase el 10 %. Con estos criterios de diseo se ajustaron las
ganancias de forma manual. Como resultado, en el controlador de alabe y cabeceo, el
tiempo de asentamiento fue de 0,35 segundos siendo el sobrepaso del 11,44 % siendo
la entrada una seal escaln de 0,5 radianes (28,6479 ) de amplitud. En guiada el
tiempo de asentamiento fue de 0,94 segundos con un sobrepaso del 6,46 %. La salida
de los controladores se limit mediante saturaciones de forma que el empuje realizado
por un rotor nunca fuese negativo.
A la hora de implementar el algoritmo de la unidad de medicin inercial y del filtro de
Kalman el Arduino, la velocidad de ejecucin del bucle de control resulta fundamental.
Se consigui ejecutar el bucle completo en 6 milisegundos, lo que significa que se
95
10.
Proyectos futuros
En el campo del control automtico este puede ser el primero de otros muchos proyectos relacionados con vehculos areos autnomos del tipo quadrotor. Por ejemplo,
se puede comparar el resultado del uso de diferentes tcnicas de control como pueden
ser el control lineal cuadrtico o el control difuso en el control de la orientacin del
quadrotor, aunque, tras lograr el control en orientacin, el siguiente paso sera el control en posicin, sin embargo, para ello habra que equipar al quadrotor de sensores
de posicin, sea bien mediante GPS o sensores de flujo ptico.
El filtro de Kalman lineal ha demostrado un buen desempeo a la hora de estimar la
orientacin del quadrotor pese a que se trata de un sistema dinmico no lineal. Un
posible proyecto sera el diseo de un estimador no lineal como puede ser un filtro de
Kalman extendido.
Otra opcin es equipar al quadrotor con sensores de distancia gracias a los cuales
sera posible implementar un algoritmo que evitase la colisin de este con los objetos
del entorno. Fuera del campo del control automtico se podra disear una proteccin
para las hlices similar a la que utiliza el AR.Drone 2.0 de Parrot (figura 59) o equipar
al quadrotor de motores sin escobillas.
96
Parte VI
Bibliografa
[1] Al-Younes, Y.M.; Al-Jarrah, M.A.; Jhemi, A.A. Linear vs. nonlinear control techniques for a quadrotor vehicle. 2010 7th International Symposium on Mechatronics and its Applications (ISMA). Sharjah, 20-22 de abril de 2010.
[2] Alexis, K. Model predictive quadrotor indoor position control. 19th Mediterranean Conference on Control & Automation (MED). Corfu, 20-23 de junio de
2011.
[3] Aracil Santonja, J; Gordillo lvarez, F. Regulacin Automtica. s.f.
[4] Barrientos, A.; del Cerro, J.; Gutirrez, P.; San Martn, R.; Martnez, A.; Rossi, C.
Vehculos areos no tripulados para uso civil. Tecnologa y aplicaciones. Congreso
Espaol de Informtica. Zaragoza, 2007.
[5] W. Beard, Randal. Quadrotor Dynamics and Control. Brigham: Briham Young
University, 2008.
[6] Bedford, A; Fowler W. Engineering Mechanics: Dynamics SI Edition. Exxes:
Addison Wesley Longman, 1996. ISBN: 0-201-40341-2.
[7] Berna Ferri, A. Desarrollo de una plataforma de tiempo real para la implementacin de algoritmos de control multivariables: Ampliacin al control de orientacin
de vehculos areos. Valencia: Universidad de Politcnica de Valencia, 29 de septiembre de 2010.
[8] Berstein, Jonathan. An Overview of MEMS Inertial Sensing Technology.
<http://www.sensorsmag.com/>. 1 de Febrero de 2003. Consultado el 23 de
junio de 2012.
[9] Bouabdallah, S.; Noth, A.; Siegwart, R. PID vs LQ Control Techniques Applied
to an Indoor Micro Quadrotor. Lausanne: Swiss Federal Institute of Technology,
11-14 de septiembre de 2007.
[10] Chan, Ai-Ling; Tan, Su-Lim; Kwek, Chu-Lih. Sensor data fusion for attitude stabilization in a low cost Quadrotor system. 2011 IEEE International Symposium
on Consumer Electronics. Singapur, 14-17 de junio de 2011.
[11] V. Cook, Michael. Flight Dynamics Principles. 2 edicin. Burlington: Elsevier,
2007. ISBN: 978-0-7506-6927-6.
[12] Criado Rodrguez, Javier. Guiado por computador de un helicptero con cuatro
rotores. Almera: Universidad de Almera, 2009.
97
[13] Etter, W.; Martin, P.; Mangharam, P. Cooperative Flight Guidance of Autonomous Unmanned Aerial Vehicles. University of Pennsylvania, 2011.
[14] Euston, Mark; Coote, Paul; Mahony, Robert; Kim, Jonghyuk; Hamel, Tarek. A
Complementary Filter for Attitude Estimation of a Fixed-Wing UAV. 2008 IEEE
International Conference on Intelligent Robots and Systems (IROS). Niza, 22-26
de septiembre de 2008.
[15] Garca de Marina, Hector; J. Pereda, Fernando; Giron-Sierra, Jose M; Espinosa,
Felipe. UAV Attitude Estimation Using Unscented Kalman Filter and TRIAD.
IEEE Transactions on Industrial Electronics. 11 de noviembre de 2012, vol. 59,
nm 11.
[16] S. Grewal, Mohinfer; P. Andrews, Angus. Kalman Filtering: Theory and Practice
Using MATLAB. 2 edicin. Nueva York: Wiley-Interscience, 2001. ISBN: 0-47126638-8.
[17] Hoffmann, Frank; Goddemeier, Niklas; Bertram, Tortsen. Attitude estimation
and control of a qudcopter. 2010 IEEE International Conference on Intelligent
Robots and Systems. Taipei, 18-22 de octubre de 2010.
[18] Jimnez Gajate, R. Diseo y desarrollo de un sistema de control para un quadrotor. Almera: Universidad de Almera, 2012.
[19] Jiong, Yi; Lei, Zhang; Jiangping, Deng; Rong, Shu; Jianyu, Wang. GPS/SINS/BARO Integrated Navigation System for UAV. 2010 International Forum on
Information Technology and Applications (IFITA). Kunming, 16-18 de julio de
2010.
[20] Khebbache, Hicham; Sait, Belkacem; Yacef, Fouad; Soukkou, Yassine. Robust
Stabilization of a Quadrotor Aerial Vehicle in Presence of Actuator Faults.
2012 International Journal of Information Technology, Control and Automation
(IJITCA). Abril 2012, vol. 2, nm. 2.
[21] C. Kuo, B. Sistemas de Control Automtico. 7 edicin. Naucalpan de Jurez:
Prentice-Hall Hispanoamericana, 1996. ISBN: 968-880-723-0.
[22] Lendel, Z.; Berna, A.; Guzmn-Gimnez, J.; Sala, A.; Garca, P. Application of
Takagi-Sugeno observers for state estimation in a quadrotor. Valencia: Universidad de Politcnica de Valencia, 2011.
[23] Li, J.; Li, Y. Dynamic Analysis and PID Control for a Quadrotor. IEEE International Conference on Mechatronics and Automation. Beijing, 7-10 de agosto
de 2011.
[24] Lupashin, S.; Schllig, A.; Sherback, M.; DAndrea, R. A simple learning strategy
for high-speed quadrocopter multi-flips. 2010 IEEE International Conference on
Robotics and Automation (ICRA). Anchorage, 3-8 de mayo de 2010.
[25] Lupashin, S.; DAndrea, R. Adaptive Open-Loop Aerobatic Maneuvers for Quadrocopters. 2010 IEEE International Conference on Robotics and Automation
(ICRA). Anchorage, 3-8 de mayo de 2010.
98
[26] Martin, Philippe; Salan, Erwan. The True Role of Accelerometer Feedback in
Quadrotor Control. 18th International Federation of Automatic Control World
Congress. Miln, 28 de agosto-2 de septiembre de 2011.
[27] Muoz Cerezo, J. S. Desarrollo de un sistema de tiempo real para el control de un
mini-helicptero de 4-rotores en vuelo libre. Valencia: Universidad de Politcnica
de Valencia, 16 de julio de 2010.
[28] Nadales Real, Christian. Control de un Quadrotor mediante la plataforma Arduino. Barcelona: Escola Politcnica Superior de Castelldefels, 2009.
[29] Ogata, K. Ingeniera de Control Moderna. 4 edicin. Madrid: Pearson Educacin, 2003. ISBN: 84-205-3678-4.
[30] Petersen, Christian Fink; Hansen,Henrik; Larsson, Steen; Theilgaard Madsen,
Lars Bo; Rimestad, Michael. Autonomous Hovering with a Quadrotor Helicopter.
Aalborg: Aalborg Universitet, junio de 2008.
[31] V. Raffo, Guilherme; G. Ortega, Manuel; R. Rubio, Francisco. Backstepping/Nonlinear H Control for Path Tracking of a QuadRotor Unmanned Aerial Vehicle. American Control Conference. Seattle, 11-13 de junio de 2008.
[32] F. Roley, William; D. Sturger, Leroy. Ingeniera Mecnica: Dinmica. Madrid:
Reverte, 1996. ISBN: 84-291-4256-8.
[33] Simons, M. Model Aircraft Aerodynamics. 4 edicin. Dorset: Nexus Special Interests, 1999. ISBN: 18-548-6190-5.
[34] Thompshon Dydek, Zachary. Adaptative Control of Unmanned Aerial Systems.
Massachesetts: Massachuetts Institute of Technology, septiembre de 2010.
[35] Welch, Greg; Bishop, Gary. An Introduction to the Kalman Filter. 2006.
[36] Zhou, Quin-Li; Zhang, Youmin; Qu, Yao-Hong; Rabbath, Camille-Alain. Dead
reckoning and Kalman filter design for trajectory tracking of a quadrotor UAV.
2010 IEEE/ASME International Conference on Mechatronics and Embedded Systems and Applications (MESA). Suzhou, 8-10 de julio de 2010.
[37] Analog Devices | Semiconductors. <http://www.analog.com/en/index.html>.
Consultado el 25 de junio de 2012.
[38] AR.Drone 2.0. Parrot new Wi-Fi quadricopter. <http://ardrone2.parrot.com/>
Consultado el 10 de agosto de 2012.
[39] Arduino - Homepage. <http://www.arduino.cc/>. Consultado el 26 de junio de
2012.
[40] Arduino
Wikipedia,
the
free
encyclopedia.
<http://en.wikipedia.org/wiki/Arduino>. Consultado el 26 de junio de
2012.
99
[41] Draganfly.com
Industrial
Aerial
Video
Systems
&
<http://www.draganfly.com/>. Consultado el 10 de agosto de 2012.
UAVs.
encyclopedia.
28 de abril
[44] Fuzzy
Logic
Wikipedia,
the
free
encyclopedia.
<http://en.wikipedia.org/wiki/Fuzzy_logic>. Consultado el 26 de julio de
2012.
[45] Honeywell Magnetic Sensors. <http://www.magneticsensors.com/>. Consultado
el 25 de junio de 2012.
[46] InvenSense | MotionProcessing | MEMS Gyro | MotionSensing.
<http://www.invensense.com/>. Consultado el 25 de junio de 2012.
[47] Kalman
filter
Wikipedia,
the
free
encyclopedia.
<http://en.wikipedia.org/wiki/Kalman_filter>. Consultado el 28 de abril
de 2012.
[48] Model
Predictive
Control
Wikipedia,
the
free
encyclopedia.
<http://en.wikipedia.org/wiki/Model_predictive_control>.Consultado
el
29 de julio de 2012.
[49] MathWorks Espaa - MATLAB - El lenguaje del clculo tcnico.
<http://www.mathworks.es/products/matlab/>. Consultado el 28 de agosto de
2012.
[50] MathWorks Espaa - Simulink - Simulacin y diseo basado en modelos.
<http://www.mathworks.es/products/simulink/>. Consultado el 28 de agosto
de 2012.
[51] PID
Controler
Wikipedia,
the
free
encyclopedia.
<http://en.wikipedia.org/wiki/PID_controler>. Consultado el 28 de julio
de 2012.
[52] Quadrotor
Wikipedia,
the
free
encyclopedia.
<http://en.wikipedia.org/wiki/Quadrotor>. Consultado el 12 de agosto de
2012.
[53] Robust
Control
Wikipedia,
the
free
encyclopedia.
<http://en.wikipedia.org/wiki/Robust_control>. Consultado el 28 de julio de 2012.
[54] sf9domahrs
AHRS
for
Sparkfuns
"9DOF
Razor
IMU".
<http://code.google.com/p/sf9domahrs/>. Consultado el 28 de julio de
2012.
100
[55] Unmanned
aerial
vehicle
Wikipedia,
the
free
encyclopedia.
<http://en.wikipedia.org/wiki/Unmanned_aerial_vehicle>. Consultado el
12 de agosto de 2012.
101
Parte VII
Anexos
11.
11.1.
1
2
3
4
5
Mdulo principal
/*
*
* Quadrotor
*
*/
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
// Gravedad
# define G_ACEL
# define DT 10
256
28
29
30
31
// Ganancias PI DCM
# define K p _ R O L L P I T C H _ D C M .05
# define K i _ R O L L P I T C H _ D C M 0.00002
32
33
34
35
36
37
// Ganancias controlador PD
# define Kp_ROLLPITCH 50
# define Kd_ROLLPITCH 10
# define Kp_YAW 10
# define Kd_YAW 2
38
39
40
41
42
43
44
45
102
46
47
48
49
50
51
52
53
54
55
56
57
58
59
// Angulos de Euler
double pitch , roll , yaw ;
60
61
62
63
64
65
66
// Contadores
unsigned int count ;
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
// Variable IMU
float DCM [3][3] = {
{
1 ,0 ,0
}
,{
0 ,1 ,0
}
,{
0 ,0 ,1
}
};
float UpdateMatrix [3][3] = {
{
0 ,1 ,2
}
,{
3 ,4 ,5
}
,{
6 ,7 ,8
}
};
float TempMatrix [3][3] = {
{
0 ,0 ,0
}
,{
0 ,0 ,0
}
,{
0 ,0 ,0
}
};
93
94
95
96
97
98
99
100
101
102
103
103
104
0 ,0 ,0};
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
// Variables Kalman
float A [2][2] = {
{
1 , DT
}
,{
0 ,1
}
};
// float B [3][2] = {{0 , DT * K_RP / I [0]} ,{0 , DT * K_RP / I [1]} ,{0 , DT *
K_YAW / I [2]}}; <-- ********* CORREGIR **********
float B [3][2] = {
{
0 , DT *1
}
,{
0 , DT *1
}
,{
0 , DT *1
}
};
float H [2][2] = {
{
1 ,0
}
, {
0 ,1
}
};
float P [3][2][2] = {
{
{
1 ,0
}
,{
0 ,1
}
}
,{
{
1 ,0
}
,{
0 ,1
}
}
,{
{
1 ,0
}
,{
0 ,1
}
}
};
float Q [3][2][2] = {
{
{
.1 ,0
}
,{
0 ,.1
}
}
,{
{
.1 ,0
}
,{
0 ,.1
}
}
104
161
,{
162
163
,{
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
.1 ,0
0 ,.1
};
float R [3][2][2] = {
{
{
.0001 ,0
,{
0 ,.0001
}
,{
{
.0001 ,0
,{
0 ,.0001
}
,{
{
.0001 ,0
,{
0 ,.0001
}
};
float K [3][2][2] = {
{
{
1 ,0
,{
0 ,1
}
,{
{
1 ,0
,{
0 ,1
}
,{
{
1 ,0
,{
0 ,1
}
};
}
}
}
}
}
}
}
}
}
}
}
}
208
209
210
211
212
213
214
// Variable de los
double setpoint [3]
0 ,0 ,0};
double PWM_Offset ,
float PWM_Motor [4]
0 ,0 ,0 ,0};
controladores PD
= {
PWM_roll , PWM_pitch , PWM_yaw ;
= {
215
216
PID rollPD (& roll , & PWM_roll , & setpoint [0] , Kp_ROLLPITCH , 0.0 ,
Kd_ROLLPITCH , DIRECT ) ;
105
217
218
PID pitchPD (& pitch , & PWM_pitch , & setpoint [1] , Kp_ROLLPITCH , 0 ,
Kd_ROLLPITCH , DIRECT ) ;
PID yawPD (& yaw , & PWM_yaw , & setpoint [2] , Kp_YAW , 0 , Kd_YAW ,
DIRECT ) ;
219
220
221
222
void setup () {
223
224
225
226
227
228
229
230
231
232
233
234
Wire . begin () ;
// Sample Rate 1 kHz , Filter Bandwidth 42 Hz , Gyro Range 2000
d/s
writeTo ( IMU3K , 0 x16 , 0 x1B ) ;
// set accel register data address
writeTo ( IMU3K , 0 x18 , 0 x32 ) ;
// set accel i2c slave address
writeTo ( IMU3K , 0 x14 , ACCEL ) ;
235
236
237
238
239
240
241
242
243
244
245
246
247
248
delay (1000) ;
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
106
delay (3000) ;
271
272
273
274
275
delay (3000) ;
276
277
278
279
PWM_Offset = 50;
280
281
void loop () {
282
283
284
285
286
287
288
ReadAccelGyro () ;
289
290
291
292
293
294
// IMU
MatrixUpdate () ;
Normalize () ;
D ri ft C or r ec ti o n () ;
Euler () ;
295
296
297
// Kalman
Kalman () ;
298
299
300
301
302
// PID
rollPD . Compute () ;
pitchPD . Compute () ;
yawPD . Compute () ;
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
/* Print output
Serial . print ( toDeg ( roll ) ) ;
Serial . print (" ,") ;
Serial . print ( toDeg ( pitch ) ) ;
107
329
330
331
332
333
334
335
11.2.
1
float op [3];
3
4
5
6
7
8
9
10
11
12
13
14
15
16
float op [2];
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
108
42
43
44
45
46
47
48
49
50
51
11.3.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
11.4.
1
2
3
109
Wire . b e g i n T r a n s m i s s i o n ( IMU3K ) ;
Wire . write ( REG_GYRO_X ) ; // Register Address GYRO_XOUT_H
Wire . en d Tr a ns mi s si on () ;
5
6
7
8
Wire . b e g i n T r a n s m i s s i o n ( IMU3K ) ;
Wire . requestFrom ( IMU3K ,12) ; // Read 12 bytes
while ( Wire . available () )
{
buffer [ i ] = Wire . read () ;
i ++;
}
Wire . en d Tr a ns mi s si on () ;
9
10
11
12
13
14
15
16
17
18
19
20
21
es MSB primero
<< 8 | byte ( buffer [1]) ) - giroOffset
<< 8 | byte ( buffer [3]) ) - giroOffset
<< 8 | byte ( buffer [5]) ) - giroOffset
22
23
24
25
26
27
28
29
30
31
32
33
34
int i ;
35
36
Wire . b e g i n T r a n s m i s s i o n ( IMU3K ) ;
Wire . write ( REG_GYRO_X ) ; // Register Address GYRO_XOUT_H
Wire . en d Tr a ns mi s si on () ;
37
38
39
40
Wire . b e g i n T r a n s m i s s i o n ( IMU3K ) ;
Wire . requestFrom ( IMU3K ,12) ; // Read 12 bytes
while ( Wire . available () ) {
buffer [ i ] = Wire . read () ;
i ++;
}
Wire . en d Tr a ns mi s si on () ;
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
// Actualizo matriz R
void MatrixUpdate () {
56
57
110
Vector_Suma (& Omega [0] , & giro [0] , & Omega_I [0]) ;
Vector_Suma (& Omega [0] , & Omega [0] , & Omega_P [0]) ;
58
59
60
UpdateMatrix [0][0]
UpdateMatrix [0][1]
UpdateMatrix [0][2]
UpdateMatrix [1][0]
UpdateMatrix [1][1]
UpdateMatrix [1][2]
UpdateMatrix [2][0]
UpdateMatrix [2][1]
UpdateMatrix [2][2]
61
62
63
64
65
66
67
68
69
= 1;
= - dt * Omega [2];
= dt * Omega [1];
= dt * Omega [2];
= 1;
= - dt * Omega [0];
= - dt * Omega [1];
= dt * Omega [0];
= 1;
// -Z
// Y
// Z
// -X
// -Y
// x
70
71
72
73
74
75
76
77
78
79
80
81
82
float error = 0;
float temp [3][3] = {{0 ,0 ,0} ,{0 ,0 ,0} ,{0 ,0 ,0}};
float renorm = 0;
83
84
85
86
// Calculo el error
error = - Vector_Dot (& DCM [0][0] , & DCM [1][0]) *0.5;
87
88
89
90
91
92
93
94
95
96
97
98
99
100
// Normalizo el vector X
renorm = .5 * sqrt ( sq ( temp [0][0]) + sq ( temp [0][1]) + sq (
temp [0][2]) ) ;
V e c t o r _ P o r E s c a l a r (& DCM [0][0] , 1/ renorm , & temp [0][0]) ;
101
102
103
104
// Normalizo el vector Y
renorm = .5 * sqrt ( sq ( temp [1][0]) + sq ( temp [1][1]) + sq (
temp [1][2]) ) ;
V e c t o r _ P o r E s c a l a r (& DCM [1][0] , 1/ renorm , & temp [1][0]) ;
105
106
107
108
109
110
111
112
// Normalizo el vector Z
renorm = .5 * sqrt ( sq ( temp [2][0]) + sq ( temp [2][1]) + sq (
temp [2][2]) ) ;
V e c t o r _ P o r E s c a l a r (& DCM [2][0] , 1/ renorm , & temp [2][0]) ;
111
113
114
void D ri ft C or re c ti on ( void ) {
115
// Compensacion de la deriva
static float S caled_ Omega_ I [3];
float acelModulo ;
float acelPeso ;
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
// Paso la aceleracion a m / s
V e c t o r _ P o r E s c a l a r (& acel [0] , .03832 , & acel [0]) ;
131
132
133
134
135
136
// Control proporcional
V e c t o r _ P o r E s c a l a r (& Omega_P [0] , Kp_ROLLPITCH * acelPeso , &
err orRoll Pitch [0]) ;
137
138
139
140
141
142
143
// Control integral
V e c t o r _ P o r E s c a l a r (& S caled_ Omega_ I [0] , Ki_ROLLPITCH * acelPeso ,
& er rorRol lPitch [0]) ;
Vector_Suma ( Omega_I , Omega_I , Scale d_Omeg a_I ) ;
144
145
146
147
148
149
150
151
11.5.
1
Filtro de Kalman
2
3
4
5
6
7
8
9
// Matrices
float x [3] = { roll , pitch , yaw };
float x_dot [3] = { roll , pitch , yaw };
float u_input [3] = { PWM_roll , PWM_pitch , PWM_yaw };
float z [3] = { roll , pitch , yaw };
float Temp2x2 [2][2][2];
float Cov [2][2];
10
112
11
12
13
14
15
// Estimacion a priori P
M at ri z 2x 2 _T ra n s ( Temp2x2 [1] , A ) ;
M a t r i z 2 x 2 _ P r o d u c t o ( Temp2x2 [0] , A , P [ i ]) ;
M a t r i z 2 x 2 _ P r o d u c t o ( P [ i ] , Temp2x2 [0] , Temp2x2 [1])
;
Matr iz2x2_ Suma ( P [ i ] , P [ i ] , Q [ i ]) ;
16
17
18
19
20
21
// Ganancia de Kalman
Matr iz2x2_ Suma ( Temp2x2 [0] , H , R [ i ]) ;
Matriz2x2_Inv ( Cov , Temp2x2 [0]) ;
M a t r i z 2 x 2 _ P r o d u c t o ( K [ i ] , P [ i ] , Cov ) ;
22
23
24
25
26
27
28
29
// Estimacion P a posteriori
Temp2x2 [0][0][0] = 1 - K [ i ][0][0];
Temp2x2 [0][0][1] = -K [ i ][0][1];
Temp2x2 [0][1][0] = -K [ i ][1][0];
Temp2x2 [0][1][1] = 1 - K [ i ][1][1];
30
31
32
33
34
35
Temp2x2 [1][0][0]
Temp2x2 [1][0][1]
Temp2x2 [1][1][0]
Temp2x2 [1][1][1]
36
37
38
39
=
=
=
=
P [ i ][0][0];
P [ i ][0][1];
P [ i ][1][0];
P [ i ][1][1];
40
41
42
43
44
45
46
47
// roll = x [0];
// pitch = x [1];
// yaw = x [2];
113