universidad nacional autonoma de m exico facultad de ingenier … · 2018-08-29 · universidad...
TRANSCRIPT
Universidad Nacional Autonoma de Mexico
Facultad de Ingenierıa
Proyecto Final
Control en Cuadricopetro
Integrantes:
Felix Tejeda Fernando
Torres Rodrıguez Ivan
Supervisor:
Dr. Marcos Angel
Gonzalez Olvera
Reporte de proyecto final en el
diseno de un controlador elaborado
en la materia de
Control Avanzado
Division de Ingenierıa Electrica - Control y Robotica
20 de mayo de 2015
“El secreto del exito es ser dueno de nada, pero el control de todo”
Nelson Rockefeller
UNIVERSIDAD NACIONAL AUTONOMA DE MEXICO
Resumen
Facultad de Ingenierıa
Division de Ingenierıa Electrica - Control y Robotica
Proyecto Final
Control en Cuadricopetro
por Felix Tejeda Fernando
Torres Rodrıguez Ivan
Se modela, disena e implementa un controlador y observador tal que se logre estabilizar
un cuadricoptero. Durante el desarrollo del proyecto se analiza e implementa la teorıa
del espacio de estados para sistemas dinamicos linealizados. Ademas, se elaboran ex-
perimentos para estimar, comprender y predecir distintos comportamientos fısicos del
sistema.
Indice general
Resumen II
Indice General III
1. Modelado en Variables de Estado 1
1.1. Introduccion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2. Fısica involucrada . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2.1. Cinematica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2.2. Motores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.2.3. Fuerzas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.2.4. Torques . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.3. Ecuaciones de movimiento . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.4. Modelo no lineal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.4.1. Variables de estado . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.4.2. Puntos de equilibrio . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.5. Modelo lineal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
1.6. Parametros del modelo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
1.7. Simplificaciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
1.8. Analisis de estabilidad . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
1.9. Simulaciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2. Controlador y Observador 18
2.1. Introduccion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
2.2. Controlador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.2.1. Controlabilidad . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
2.2.2. Control optimo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
2.2.3. Estabilidad en el sentido de Lyapunov . . . . . . . . . . . . . . . . 22
2.3. Observador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
2.3.1. Observabilidad . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
2.3.2. Observador de Luenberger . . . . . . . . . . . . . . . . . . . . . . . 23
2.4. Control por estados observados . . . . . . . . . . . . . . . . . . . . . . . . 24
2.5. Simulaciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
3. Implementacion 29
3.1. Introduccion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.2. Sistema discreto . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
3.2.1. Discretizacion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
iii
Indice General iv
3.2.2. Controlador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
3.2.3. Observador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
3.3. Simulaciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
3.4. Puesta a punto . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
3.5. Conlusiones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
A. Codigo en Matlab 42
Capıtulo 1
Modelado en Variables de Estado
1.1. Introduccion
Un cuadricoptero es una aeronave que tiene cuatro rotores distribuidos equidistante-
mente, usualmente colocados en las esquinas de un cuerpo cuadrado. Con cuatro rotores
independientes no son necesarios complejos mecanismos para llegar a manipularlo co-
rrectamente.
El desarrollo en los cuadricopteros habıa estado estancado por un largo periodo debido
a la complejidad en controlar cuatro motores independientes sin electronica comple-
ja y finos mecanismos. El actual decremento en los microcontroladores ha facilitado
la electronica e incluso ha sido posible el desarrolo de cuadricoperos completamente
autonomos para uso militar, comercial e incluso para fines recreativos.
El contro sobre un cuadicoptero es un problema bastante interesante. Con seis grados de
libertad (tres traslacional y tres rotacional) y solo cuatro entradas independientes (roto-
res), los cuadrcopteros estan realmente subactuados. Con el fin de lograr seis grados de
libertad, el movimiento rotacional y traslacional esta acoplado. La dinamica resultante
es altamente no lineal, especialmente despues de tomar en cuenta los efectos aerodinami-
cos. Finalmente, a diferencia de los vehıculos terrestres, las aeronaves tienen muy poca
friccion que prevengan su movimiento, por lo que deben proveer su propio frenado para
deterse y manterse estable. Se incluira un modelo muy simplificado del modelo dinamico
del cuadricoptero para lograr llevarlo a un punto de equilibrio. Ademas, se simula e
implementa un controlador que permita mantenerlo en vuelo.
1
Modelado en Variables de Estado 2
1.2. Fısica involucrada
El desarrollo en la dinamica del cuadricopero se empieza introduciendo las dos estructu-
ras en los que va a operar. La estructura inercial esta definido por la Tierra, cuya fuerza
gravitatoria apunta en direccion negativa al eje z. La estructura corporal esta definida
por la orientacion del cuadricoptero, cuyos rotores apuntan en direccion positiva al eje
z y sus brazos apuntan en direccion x y y.
Figura 1.1: Estructura inercial y corporal
1.2.1. Cinematica
Para formalizar la cinematica de la estructura inercial se define la posicion y velocidad
del cuadricopero como p = (x, y, z)T y p = (x, y, z)T respectivamente. Similarmente, se
definen los angulos roll, pitch y yaw para la estructura corporal como α = (φ, θ, ψ)T ,
cuyas correspondientes velocidades angulares son iguales a α = (φ, θ, ψ)T . Sin embargo,
notese que el vector de velocidad angular ω 6= α. La velocidad angular es un vector que
apunta a lo largo del eje de rotacion, mientras que α es solo la derivada con respecto del
tiempo de los angulos roll, pitch y yaw. Para convertir estas velocidades angulares a un
vector de velocidad angular, se utiliza la siguiente transformacion
ω =
1 0 −Sθ0 Cφ CθSφ
0 −Sφ CθCφ
αSe puede relacionar la estructura corporal e inercial mediante la matriz de rotacion
R. El desarrollo de esta matriz se hace mediante de los angulos de Euler y utilizando
Modelado en Variables de Estado 3
correctamente identidades trigonometricas en los angulos roll, pitch, yaw.
R =
CψCθ CψSθSφ − CφSψ SψSφ + CψCφSθ
CθSψ CψCφ + SψSθSφ CφSψSθ − CψSφ−Sθ CθSφ CθCφ
Para un vector ~v dado en la estructura corporal, el vector correspondiente en la estruc-
tura inercial esta dado por R~v.
1.2.2. Motores
Se utilizan motores de corriente directa para propulsar el cuadricoptero. Para estos
motores, el torque que producen estan dados por
τ = KtI
donde τ es el torque del motor, I es la corriente en el motor y Kt es la constante
proporcional de torque. Por otro lado, tomando en cuenta la resistencia del embobinado,
el voltaje sobre el motor esta dado por las leyes de Kirchoff
V = IRm +Kvω
donde V es el voltaje aplicado al motor, Rm su resistencia, ω la velocidad angular del
motor y Kv es una constante de proporcionalidad. Dados estos parametros y hacien-
do despreciable la pequena resistencia del motor, se obtiene la funcion de su potencia
consumida
P = V I =τ(τRm +KtKvω)
K2t
≈ Kv
Ktτω
1.2.3. Fuerzas
La potencia es utilizada para mantener el cuadricoptero en vuelo. Por la conversacion de
la energıa, la energıa utilizada por los motores en un cierto periodo de tiempo es igual a
la fuerza empleada por las helices para desplazar el aire a su alrededor y lograr elevarse
(P dt = F dx). Equivalentemente, la potencia es igual a un numero de veces la velocidad
del viento P = F dxdt .
P = Tvh
Se asume que la aeronave se movera relativamente lento, por lo que vh sera la velocidad
del viento desplazado cuando el cuadricoptero se encuentre flotando. Tambien se asumen
despreciables los efectos de rizo en el aire provocados por las helices. Dadas las ecuaciones
Modelado en Variables de Estado 4
de momentum para un objeto que se mantiene flotando
vh =
√T
2ρA
donde ρ es la densidad del aire y A es el area total que ocupan las aspas. Utilizando la
anterior ecuacion de potencia se tiene que
P =Kv
Ktτω =
KvKτ
KtTω =
T 3/2
√2ρA
Notese que en el caso general, τ = ~r× ~F ; en este caso, el torque es proporcional al empuje
T por una constante Kτ determinada por los parametros y configuracion de las aspas.
Resolviendo para la magnitud del empuje T se obtiene que el empuje es proporcional al
cuadrado de la velocidad angular del motor
T =
(KvKτ
√2ρA
Ktω
)2
= kω2
donde k se puede decir que es constante. Sumando sobre los 4 motores se tiene que el
empuje total sobre el cuadricoptero es
TB =
4∑i=1
Ti = k
4∑i=1
ω2i
Dado que la velocidad angular de cada rotor es proporcional al voltaje aplicado, con cm
igual a una constante y V al voltaje aplicado al motor, tenemos que
w2i = cmV
2i
En adicion a la fuerza de empuje, se modela la friccion como una fuerza proporcional a
la velocidad lineal. Esta es una vista simplificada de la friccion viscoza pero es suficiente
para el modelado. Esta fuerza esta definida por la ecuacion
FD = −kd
x
y
z
1.2.4. Torques
Cada rotor contribuye con un torque alrededor del eje z. Este torque es el necesario para
que las helices giren y proveer empuje, esto crea instanteneamente la aceleracion angular
Modelado en Variables de Estado 5
y provoca las fuerzas de friccion de arrastre. La ecuacion de arrastre en la dinamica de
los fluidos proporciona la fuerza de friccion
FD =1
2ρCDAv
2
donde ρ es la densidad del fluido, A el area ocupada por las aspas y CD es una constante
adimensional. Esto implica que el torque debido a la friccion esta dada por
τD =1
2RρCDAv
2 =1
2RρCDA(ωR)2 = bω2
donde ω es la velocidad angular de las helices y R es la longitud de la mitad de un aspa.
Finalmente se puede escribir el torque completo sobre el eje z para el iesimo motor.
τz = bω2 + IM ω
donde IM es el momento de inercia sobre el eje z, ω es la aceleracion angular de la helice
y b es el mismo coeficiente de friccion anterior. Notese que en estado estacionario ω ≈ 0,
dado que la mayor parte del tiempo las helices se mantendran a una velocidad constante.
Por lo que se simplifica la expresion a
τz = (−1)i+1bω2i
donde el termino (−1)i+1 es positivo para la iesima helice si es que esta gira en sentido
de las manecillas del reloj y negativo si gira levogiramente. El torque total sobre el eje
z esta dado por la suma de todos los torques de cada helice
τψ = b(ω21 − ω2
2 + ω23 − ω2
4)
Los torques roll y pitch se derivan de la mecanica clasica. Se puede escoger arbitriara-
mente el motor 1 o 3 para torque roll
τφ =∑
r × T = Lk(ω21 − ω2
3)
Correspondientemente, el torque pitch esta dado por una expresion similar
τθ = Lk(ω22 − ω2
4)
Modelado en Variables de Estado 6
donde L es la distancia desde el centro del cuadricopero a cualquiera de los rotores.
Juntando todo, se encuentra que los torques en la estructrura corporal son
τB =
Lk(ω2
1 − ω23)
Lk(ω22 − ω2
4)
b(ω21 − ω2
2 + ω23 − ω2
4)
El modelo obtenido hasta ahora esta altamente simplificado. Se han ignorado multiples
efectos avanzados que contribuyen en la dinamica no lineal del cuadricoptero. Se ignora
las fuerzas de friccion rotacionales (las velocidades rotacionales son practicamente nulas),
el aleteo de las helices (deformacion de las aspas debidas a las altas velocidades y los
materiales flexibles), las velocidades en el fluido del entorno (viento), etc.
1.3. Ecuaciones de movimiento
En la estructrura inercial, la aceleracion del cuadricopero esta regida por el empuje, la
gravedad y la friccion lineal. Se puede obtenter el vector de empuje en la estructura
inercial mediante la matriz de rotacion R para mapear el vector de empuje desde la
estructrura corporal a la inercial. Por lo tanto, el movimiento lineal puede ser resumido
en
mp =
0
0
−mg
+RTB + FD
donde ~p es la posicion del cuadricoptero, g es la aceleracion debida a la gravedad, FD la
fuerza de friccion y TB es el vector de empuje sobre la estructura corporal.
Figura 1.2: Dinamica involucrada
Si bien es conveniente tener las ecuaciones lineales de movimiento sobre la estructura
inercial, las ecuaciones rotacionales de movimiento son utiles en la estructura corporal,
por lo que se puede expresar las rotaciones alrededor del centro del cuadricoptero en
lugar de estar alrededor del centro de inercia. Las ecuaciones de movimiento rotacional
se derivan de las ecuaciones dinamicas de Euler para cuerpos rıgidos. Expresada en forma
Modelado en Variables de Estado 7
vectorial, las ecuaciones de Euler se expresan como
Iω + ω × (Iω) = τ
donde ω es el vector de velocidad angular, I es la matriz inercial y τ es un vector de
torques externos. Se puede reescribir como
ω =
ωx
ωy
ωz
= I−1(τ − ω × (Iω))
Se puede modelar el cuadricopero como dos barras delgadas uniformes que se cruzan en
el origen con un masa puntual (motor) en cada fin de la barra. Con esto en mente, su
simetria resulta en una matriz diagonal de inercias de la forma
I =
Ixx 0 0
0 Iyy 0
0 0 Izz
Ademas, se obtiene el resultado final para las ecuaciones de movimiento rotacional en la
estructura corporal
ω =
τφI−1xx
τθI−1yy
τψI−1zz
−Iyy−IzzIxx
ωyωzIzz−IxxIyy
ωxωzIxx−IyyIzz
ωxωy
1.4. Modelo no lineal
Finalmente, dada la ecuacion de movimiento lineal, la matriz de inercia y la relacion de
velocidad angular de lo angulos roll, pitch, yaw con el vector de velocidad angular del
cuadricoptero se obtienen las ecuaciones de posicion y orientacion
x = −kdm x+ kcm
m (sinψ sinφ+ cosψ cosφ sin θ)(V 21 + V 2
2 + V 23 + V 2
4
)y = −kd
m y + kcmm (cosφ sinψ sin θ − cosψ sinφ)
(V 21 + V 2
2 + V 23 + V 2
4
)z = −kd
m z − g + kcmm (cos θ cosφ)
(V 21 + V 2
2 + V 23 + V 2
4
)φ = ωx + ωy (sinφ tan θ) + ωz (cosφ tan θ)
θ = ωy cos θ − ωz sinφ
ψ = sinφcos θωy + cosφ
cos θωz
ωx = LkcmIxx
(V 21 − V 2
3 )− Iyy−IzzIxx
ωyωz
ωy = LkcmIyy
(V 22 − V 2
4 )− Izz−IxxIyy
ωxωz
ωz = bcmIzz
(V 21 − V 2
2 + V 23 − V 2
4 )− Ixx−IyyIzz
ωxωy
Modelado en Variables de Estado 8
1.4.1. Variables de estado
El problema se divide en dos partes, por un lado se definen las variables de estado xp εR6
y salida yp εR3 para la posicion del sistema
xp =[x y z x y z
]Typ =
[x y z
]TPor otro lado, para el problema de orientacion, se tienen los estados xo εR6 y salida
yo εR3
xo =[φ θ ψ ωx ωy ωz
]Tyo =
[φ θ ψ
]TPara ambas partes del sistema se tiene al voltaje como un vector de entradas u εR4
u =[V 21 V 2
2 V 23 V 2
4
]TNotese qe el vector de entradas esta en terminos del cuadro del voltaje en los rotores
y por lo tanto el problema de control sera en el cuadrado de los voltajes y no en los
voltajes. Tomando esto en cuenta, se obtiene el modelo de variables de estado x = f(x, u),
x0 = x(t0) y y = h(x, u)
xp =
x4
x5
x6
−kdmx4 + kcm
m (sinx9 sinx7 + cosx9 cosx7 sinx8) (u1 + u2 + u3 + u4)
−kdmx5 + kcm
m (cosx7 sinx9 sinx8 − cosx9 sinx7) (u1 + u2 + u3 + u4)
−kdmx6 − g + kcm
m (cosx8 cosx7) (u1 + u2 + u3 + u4)
xo =
x10 + x11 (sinx7 tanx8) + x12 (cosx7 tanx8)
x11 cosx8 − x12 sinx7sinx7cosx8
x11 + cosx7cosx8
x12LkcmIxx
(u1 − u3)− Iyy−IzzIxx
x11x12LkcmIyy
(u2 − u4)− Izz−IxxIyy
x10x12bcmIzz
(u1 − u2 + u3 − u4)− Ixx−IyyIzz
x10x11
Modelado en Variables de Estado 9
1.4.2. Puntos de equilibrio
Se define al punto de operacion o punto de equilibrio de un sistema con modelo de
variables de estado
Σ =
x = f(x, u), x0 = x(t0)
y = h(x, u)
como aquellos calores x = X, u = U, y = Y constantes para los cuales se cumple que si
x0 = X y u(t) = U ∀ t ≥ t0 entonces x(t) = X, y(t) = Y ∀ t ≥ t0. Es decir, el punto de
equilibrio sera aquel valor (X,U,Y) que satisfaga
f(X,U) = 0 h(X,U) = Y
Para determinar el punto de equilibrio del cuadricopero se iguala x = 0 por lo tanto
debe cumplirse que
X4 = 0
X5 = 0
X6 = 0
−kdmX4 + kcm
m (sinX9 sinX7 + cosX9 cosX7 sinX8) (U1 + U2 + U3 + U4) = 0
−kdmX5 + kcm
m (cosX7 sinX9 sinX8 − cosX9 sinX7) (U1 + U2 + U3 + U4) = 0
−kdmX6 − g + kcm
m (cosX8 cosX7) (U1 + U2 + U3 + U4) = 0
X10 +X11 (sinX7 tanX8) +X12 (cosX7 tanX8) = 0
X11 cosX8 −X12 sinX7 = 0sinX7cosX8
X11 + cosX7cosX8
X12 = 0LkcmIxx
(U1 − U3)− Iyy−IzzIxx
X11X12 = 0LkcmIyy
(U2 − U4)− Izz−IxxIyy
X10X12 = 0
bcmIzz
(U1 − U2 + U3 − U4)− Ixx−IyyIzz
X10X11 = 0
Es posible distinguir que el sistema (cuadricoptero) para mantenerse estable debe estar
en una posicion horizontal de vuelo, por lo que la salida esta definida por φ = θ = ψ = 0
y x = y = z = 0. Con esto en cuenta y la condicion dinamica anterior se tiene que
P.E. =
X1 = 0 X2 = 0 X3 = 0 X4 = 0
X5 = 0 X6 = 0 X7 = 0 X8 = 0
X9 = 0 X10 = 0 X11 = 0 X12 = 0
U1 = U1 U2 = U1 U3 = U1, U4 = U1
Y1 = 0 Y2 = 0 Y3 = 0
Y4 = 0 Y5 = 0 Y6 = 0
Modelado en Variables de Estado 10
1.5. Modelo lineal
Con el fin de analizar y disenar un control tal que mantenga al cuadricopero en un
vuelo estacionario es necesario linealizar el modelo en torno a un punto de equilibrio
donde, dadas las condiciones deseadas, mantengan estable el sistema. Esta linealizacion
busca aproximar el comportamiento del sistema lineal en torno de un punto de equilibrio
mediante la dinamica de un sistema lineal.
Esto se lleva a cabo mediante la expancion en series de Taylor de las funciones f(x, u) y
h(x, u) en torno del punto de equilibrio. Por lo que el sistema lineal (o linealizado) que
aproxima al sistema no lineal en torno de un punto de equilibrio (X,U,Y) es
˙x = Ax+Bu
y = Cx+Du
x = x−Xu = u− Uy = y − Y
A = ∂f∂x
∣∣∣X,U
B = ∂f∂u
∣∣∣X,U
C = ∂h∂x
∣∣∣X,U
D = ∂h∂u
∣∣∣X,U
La traslacion del sistema dados los puntos de equilibrio obtenidos anteriormente dan
como resultado un sistema lineal con
x = x =
[xp
xo
]y = y =
[yp
yo
]u =
u1 − U1
u2 − U1
u3 − U1
u4 − U1
A =
0 0 0 1 0 0 0 0 0 0 0 0
0 0 0 0 1 0 0 0 0 0 0 0
0 0 0 0 0 1 0 0 0 0 0 0
0 0 0 −kdm 0 0 0 4kcmU1
m 0 0 0 0
0 0 0 0 −kdm 0 −4kcmU1
m 0 0 0 0 0
0 0 0 0 0 −kdm 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 0 0 1 0
0 0 0 0 0 0 0 0 0 0 0 1
0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0
Modelado en Variables de Estado 11
B =
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0kcmm
kcmm
kcmm
kcmm
0 0 0 0
0 0 0 0
0 0 0 0LkcmIxx
0 −LkcmIxx
0
0 LkcmIyy
0 −LkcmIyy
bcmIzz
− bcmIzz
bcmIzz
− bcmIzz
C =
1 0 0 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0 0 0
0 0 1 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 0 0 1 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0 0
1.6. Parametros del modelo
Antes que cualquier controlador pueda ser desarrollado es necesario que los parametros
fısicos del cuadricoptero seas medidos o amenos estimados. Estos parametros se dividen
en las siguientes categorıas: masa y dimensiones de la aeronave, dinamica de los rotores,
motores y aerodinamica de las helices.
Para un analisis completo de la dinamica de los motores en los cuadricoptero es necesario
obtener un modelo matematico en las ecuaciones de movimiento del motor. Sin embargo,
ya que no hay informacion disponible relacionadas con las constantes del motor ademas
que es complicado modelar todos los elementos del motor de DC, se utiliza una aproxi-
macion alternativa. Esta aproximacion consiste en obtener la relacion voltaje-velocidad
de cada motor donde la pendiente del modelo lineal esperado nos dara la constante√cm.
El experimento consiste en un motor y sus helices (estas estan acopladas al motor por
un juego de engranes pero ya que la entrada del sistema es el voltaje y las salidas son
velocidad angular, los engranes se ven como una caja negra) que interrumpen el haz de
luz de un optointerruptor y, con ayuda del osciloscopio, se mide la frecuencia de la senal
cuadrada generada. La frecuencia se divide entre el numero de helices y eso resulta en la
velocidad angular del motor. Como excitacion se tiene una senal PWM que es controlada
mediante un microcontrolador, se hace variar el ancho de pulso y se analiza la respuesta.
Teoricamente, la relacion voltaje-velocidad de un motor DC es lineal sin embargo existen
sobrepasos de corriente ademas de errores en la medicion. Los resultados obtenidos en
el experimento, se muestran en la Fig. 1.3.
Modelado en Variables de Estado 12
0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.6100
150
200
250
300
350PWM vs Omega
PWM
Vel
ocid
ad a
ngul
ar
Student Version of MATLAB
Figura 1.3: Relacion voltaje-velocidad
Dado que la dinamica del cuadricoptero es altamente dependiente de sus momentos de
inercia, un experimento de pendulo es utilizado para determinar los momentos de inercia
de toda la aeronave. Ya que el centro de masa y los ejes principales (eje de coordenadas)
de la aeronave son conocidos, los momentos de inercia respecto a cada eje principal
son hallados directamente al desarrollar un pendulo torsional con la rotacion pasando a
traves de su centro de masa alineado a su eje de rotacion. Es por eso que el momento
de inercia puede ser calculado usando la ecuacion de una oscilacion armonica simple
(linealizado alrededor de su punto de equilibrio θ = 0).
I =mg(l + d)t2
(2π)2−m(l + d)2
donde t es el periodo de oscilacion en segundos. El montaje experimental es mostrado
en la Fig 1.4.
Figura 1.4: Experimento para medir los momentos de inercia
Modelado en Variables de Estado 13
Dado que el empuje es proporcional al cuadrado de la velocidad angular por una cons-
tante k, se realiza un experimento que estime este parametro. Para el desarrollo de esta
etapa se fija con un tensor inextensible en el suelo al rotor y se le aplica un voltaje, la
pendiente de la relacion voltaje-empuje despues se manipula para obtener la relacion
empuje-velocidad angular. La grafica obtenida durante el experimento se muestran en
la Fig. 1.5.
0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10.45
0.5
0.55
0.6
0.65
0.7
0.75
0.8
0.85
0.9PWM2 vs Empuje
PWM2
Em
puje
Student Version of MATLAB
Figura 1.5: Relacion voltaje-empuje
Para estimar el factor de friccion b es necesario describir el torque que actua sobre
la barra de cada motor en relacion con su velocidad angular. Para el experimento, el
cuadricoptero es colocado en una bascula y alimentado con una fuente de 5V . Para
estimar el coeficiente de friccion, se induce una corriente tal que el empuje sea igual
al peso de la aeronave. Al medir la potencia disipada, el torque inducido por la helice
puede calcularse con
τloss =Plossωhover
Figura 1.6: Desarrollo del experimento
Modelado en Variables de Estado 14
Para obtener el coeficiente, este es la proporcion de la friccion al cuadrado de la velocidad
angular. Finalmente, los resultados obtenidos durare los experimentos se muestran en la
siguiente tabla
Parametros Valores numericos
g 9.78 Nms2
m 730× 10−3 kg
L 115× 10−3 m
cm 573.832× 103 rad2
s2V 2
Ixx 960× 10−6 kgm2
rad
Iyy 1070× 10−6 kgm2
rad
Izz 2298× 10−6 kgm2
rad
k 2.376× 10−6 Ns2
rad2
kd 200× 10−3 kgs
b 63.141× 10−9 Ns2
rad2
1.7. Simplificaciones
Se vuelve bastante complicado trabajar con una matriz de 12× 12 y ya que el problema
principal de control es estabilizar al cuadricoptero, se simplifica el sistema a otro donde
la posicion es despreciable. Por lo tanto, la dinamica a controlar es
x =
x10 + x11 (sinx7 tanx8) + x12 (cosx7 tanx8)
x11 cosx8 − x12 sinx7sinx7cosx8
x11 + cosx7cosx8
x12LkcmIxx
(u1 − u3)− Iyy−IzzIxx
x11x12LkcmIyy
(u2 − u4)− Izz−IxxIyy
x10x12bcmIzz
(u1 − u2 + u3 − u4)− Ixx−IyyIzz
x10x11
Donde su modelo lineal dado por sus puntos de equilibrio dan como resultado el sistema
x =
0 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
x+
0 0 0 0
0 0 0 0
0 0 0 0LkcmIxx
0 −LkcmIxx
0
0 LkcmIyy
0 −LkcmIyy
bcmIzz
− bcmIzz
bcmIzz
− bcmIzz
u
Modelado en Variables de Estado 15
y =
1 0 0 0 0 0
0 1 0 0 0 0
0 0 1 0 0 0
xTomando la orientacion y velocidad angular como variables de estado, como entrada el
cuadrado del voltaje sobre el actuador y como salida la orientacion del cuadricoptero.
x =[φ θ ψ ωx ωy ωz
]Ty =
[φ θ ψ
]Tu =
[V 21 V 2
2 V 23 V 2
4
]T
1.8. Analisis de estabilidad
Se tiene un sistema de multiples entradas con multiples salidas (MIMO por sus siglas
en ingles). Con el fin de analizar la estabilidad del sistema para sus diferentes salidas se
obtienen las funciones de trasferencia con sus respectivos polos
y1(t) =163.3
s2 + (1.526× 10−15)s− (15.99× 10−18)u1(t) p =
−3.999× 10−9
3.999× 10−9
y3(t) =15.77
s2 + (15.06× 10−15)s+ (50.02× 10−18)u1(t) p =
−j7.072× 10−9
j7.072× 10−9
y2(t) =146.5
s2 + (8.328× 10−15)s+ (20.98× 10−18)u2(t) p =
−j4.580× 10−9
j4.580× 10−9
y3(t) =−15.77
s2 + (15.06× 10−15)s+ (50.02× 10−18)u2(t) p =
−j7.072× 10−9
j7.072× 10−9
y1(t) =−163.3
s2 + (1.526× 10−15)s− (15.99× 10−18)u3(t) p =
−3.999× 10−9
3.999× 10−9
y3(t) =15.77
s2 + (15.06× 10−15)s+ (50.02× 10−18)u3(t) p =
−j7.072× 10−9
j7.072× 10−9
y2(t) =−146.5
s2 + (8.328× 10−15)s+ (20.98× 10−18)u4(t) p =
−j4.580× 10−9
j4.580× 10−9
Modelado en Variables de Estado 16
y3(t) =−15.77
s2 + (15.06× 10−15)s+ (50.02× 10−18)u4(t) p =
−j7.072× 10−9
j7.072× 10−9
De las funciones de transferencia se concluye que la salida y1(t) es inestable para toda
entrada. Por su parte, las salidas y2(t) y y3(t) son crıticamente estables; esto indica que
la salida se comportara oscilatoriamente o asintoticamente estable.
1.9. Simulaciones
Se simulan los estados del sistema tanto no lineal como el modelo linealizado para una
condicion inicial de 0 para los angulos de orientacion y 1[rad/s] para las velocidades
angulares. Se utiliza la funcion de Matlab ode45 para simular a los sistema por 10s y un
paso de 0.01.
0 1 2 3 4 5 6 7 8 9 100
5
10Roll
0 1 2 3 4 5 6 7 8 9 10−5
0
5
10Pitch
0 1 2 3 4 5 6 7 8 9 10−20
−10
0
10Yaw
NL
L
Student Version of MATLAB
Figura 1.7: Modelo lineal vs no lineal
Modelado en Variables de Estado 17
0 1 2 3 4 5 6 7 8 9 10−20
−10
0
10Omega
1
0 1 2 3 4 5 6 7 8 9 10−2
−1
0
1
2Omega
2
0 1 2 3 4 5 6 7 8 9 10−2
−1
0
1
2Omega
3
NL
L
Student Version of MATLAB
Figura 1.8: Modelo lineal vs no lineal
De las simulaciones se concluye que mientras se mantenga el analisis en torno a un punto
cercano al de equilibrio, la linealizacion es efectiva para el diseno e implementacion de
controladores. Fısicamente el cuadricoptero, si es que es autonomo, se mantendra en un
comportamiento erratico y oscilatorio para algunos estados. Se puede decir que se tiene
un sistema inestable, por lo que el controlador debera ser capaz de mantener en una
posicion al cuadricoptero dado su modelo lineal.
Capıtulo 2
Controlador y Observador
2.1. Introduccion
Se dice que un sistema lineal es controlable o completamente controlable si para cualquier
estado inicial x0 = x(0) y cualquier estado final x(tf ) existe un control u(t) definido en
tε[0, tf ] tal que lleva el sistema de x(0) a x(tf ) en un tiempo finito; en caso contrario, se
que el sistema es no controlable. Por su parte, un observador sera una estructura que,
operando en paralelo con el sistema fısico, estime, reconstruya u observe el valor de los
estados de un sistema empleando unicamente la informacion de la entrada y la salida.
La controlabilidad es la propiedad que indica si el comportamiento de un sistema puede
ser controlado por medio de sus entradas, mientras que la observabilidad es la propiedad
que indica si el comportamiento interno del sistema puede detectarse en sus salıdas.
Estos conceptos son esenciales para analizar la estructura interna de los sistemas lineales.
Tambien son necesarios para el estudio de controladores y filtros.
Se dice que las propiedades de observabilidad y controlabilidad son dos nociones duales,
puesto que sus propiedades son intercambiables para sistemas duales adjuntos. Esto per-
mite analizar la observabilidad con las mismas herramientas empleadas para determinar
la controlabilidad o viceversa.
Dado un sistema lineal controlable y observable controlado mediante una realimentacion
de estados observados, el diseno del controlador y el observador pueden realizarse de
forma independiente, y si ambos son asintoticamente estables en el sentido de Lyapunov,
el sistema controlado seguira siendo asintoticamente estable.
18
Controlador y Observador 19
2.2. Controlador
Sea el sistema controlable x = Ax + Bu el cual se desea controlar de un estado inicial
x0 a un estado final dado por un cierto punto de equilibrio deseado (X,U). El objetivo
sera lograr disenar un controlador por realimentacion de estados tal que se logre
lımt→∞
x(t) = X
Donde el sistema realimentado resultante es
x = Ax+B(−kx+ r) ⇒ x = (A−Bk)x+Br
Donde, para lograr el objetivo de control es necesario garantizar que los polos o valores
caracterısticos de (A−Bk) sean de parte real negativa, con lo cual se logra que el origen
sea asintoticamente estable en el sentido de Lyapunov.
Figura 2.1: Contol por realimentacion de estados
Donde d es una perturbacion en la entrada del sistema y si consideramos el problema de
control de regulacion (control que lleva a un estado a un punto de equilibrio) al punto de
equilibrio (X,U) y trasladamos el sistema a este punto, el cual si (A,B) es controlable
seguira siendo de esta forma. Ahora en el sistema trasladado sera lograr
lımt→∞
x(t) = 0 ⇒ lımt→∞
x(t) = X
Donde para lograr el objetivo de control en el sistema trasladado u = −kx(t). Con lo
que el sistema realimentado es
˙x = Ax+B(−kx(t)) ⇒ ˙x = (A−Bk)x
Controlador y Observador 20
El diseno por variables de estado permite definir en forma arbitraria la dinamica del
sistema realimentado. De esta forma, si se determina dicha matriz k que asigna los polos
de (A−Bk) de modo que x = 0 sea asintoticamente estable en el sentido de Lyapunov
tenemos que
u = u− U = −kx = −k(x−X)
u(t) = −kx(t) + kX + U
Donde kX + U sera la referencia para el controlador por realimentacion de estado.
2.2.1. Controlabilidad
Dado el sistema de dimension n con p entradas en la ecuacion de estados
x = Ax+Bu
donde A y B son, respectivamente, matrices reales constantes de (n×n) y (n×p) se dice
que el par (A,B) es controlable si la matriz de controlabilidad C | ε Rn×np es de rango
completo, es decir P (C) = n.
C =[B AB A2B · · · An−1B
]Para el cuadricoptero, la matriz de controlabilidad resulta de 6× 24 y parte de esta es
C =
0 0 0 0 · · · 0 0
0 0 0 0 · · · 0 0
0 0 0 0 · · · 0 0
163.3269 0 −163.3269 0 · · · 0 0
0 146.5363 0 −146.5363 · · · 0 0
15.7669 −15.7669 15.7669 −15.7669 · · · 0 0
El rango de la matrix C es igual al numero de estado por lo que se puede concluir que
el par (A,B) es controlable.
2.2.2. Control optimo
El objetivo principal de la teorıa de control optimo es determinar las senales de control
que causan a un proceso el satisfacer las restricciones fısicas que se tengan y asimismo
minimizar o maximizar segun sea el caso cierto criterio de desempeno deseado.
Controlador y Observador 21
En el caso donde la dinamica del sistema es descrita por un conjunto de ecuaciones
diferenciales y el costo esta descrito por una funcion cuadratica, se le conoce como
problema LQ. Uno de los principales resultados en la solucion de este problema dieron
como resultado el Regulador Cuadratico Lineal (LQR), un control por realimentacion
de estados cuyas ecuaciones se describen a continuacion.
El control LQR asume un sistema dinamico lineal
x = Ax+Bu
Este control tiene la forma u = −kx con k = R−1BTP , el cual estabiliza al sistema y
minimiza la medida de desempeno
V =
∫ ∞t
(xTQx+ uTRu) dτ
con Q = I3×3 y R = 1 en este caso, ya que solo se busca resolver el problema de control
de una manera optima, mas no se requiere que los estados o la energıa tenga mas peso
una que la otra. P debe satisfacer la ecuacion algebraica de Riccati
ATP + PA− PBR−1BTP +Q = 0
Dadas estas condiciones y dada la linealizacion del cuadricoptero sobre los puntos de
equilibrio, se obtienen los vectores de ganancia del controlador
k =
0.7071 0.0000 0.5000 0.7102 0.0000 0.5156
0.0000 0.7071 −0.5000 0.0000 0.7105 −0.5156
−0.7071 0.0000 0.5000 −0.7102 0.0000 0.5156
0.0000 −0.7071 −0.5000 0.0000 −0.7105 −0.5156
Con los valores caracterısticos en lazo cerrado A−Bk en
λ1 λ2 λ3 λ4 λ5 λ6
−1.000 −230.977 −207.231 −31.518 −1.000 −1.000
El control resultante tendra como referencia una entrada de equilibrio U la cual se
interpreta como la velocidad necesaria de los motores para generar un empuje igual al
peso del cuadricoptero
r(t) = kX + U =[0.8 0.8 0.8 0.8
]
Controlador y Observador 22
2.2.3. Estabilidad en el sentido de Lyapunov
El origen del sistema autonomo
x = (A−Bk)x xεRn×1 y AεRn×n
es asintoticamente estable si la ecuacion de Lyapunov
ATP + PA = −Q
tiene solucion unica P = P T εRn×n , P > 0 ∀ Q = QT > 0 , QεRn×n. Si dada Q > 0
y la solucion resulta en P > 0 el sistema sobre el origen es asintoticamente estable en el
sentido de Lyapunov.
Para el cuadricoptero, se propone Q = I6×6 cuya solucion de P dada la ecuacion de
Lyapunov esta dada por
P =
0.5043 0 0 −0.5000 0 0
0 0.5048 0.0000 0 −0.5000 0.0000
0 0.0000 0.5315 0 0.0000 −0.5000
−0.5000 0 0 0.5000 0 0
0 −0.5000 0.0000 0 0.5000 −0.0000
0 0.0000 −0.5000 0 −0.0000 0.5002
M11(P ) = 0.5043 M22(P ) = 0.2546 M33(P ) = 0.1353
M44(P ) = 580.79× 10−6 M55(P ) = 2.7759× 10−6 M66(P ) = 82.85× 10−9
Todos los valores caracterısticos de la matriz P son positivos, indicando que P es positiva
definida. Por lo tanto se puede concluir que (A−Bk) es estable en el sentido de Lyapunov.
2.3. Observador
Dado el sistema observable x = Ax+Bu, y = Cx el observador asintotico
˙x = Ax+Bu+ L(y − y)
y = Cx x(0) = x0
Donde L es una matriz de diseno. Si se analiza la dinamica del error de observacion
˙x = (A−LC)x, con x = x− x y se garantiza que los valores caracterısticos de (A−LC)
Controlador y Observador 23
sean parte real negativa, entonces
lımt→∞
x(t) = 0 ⇒ lımt→∞
x(t) = x(t)
2.3.1. Observabilidad
Dado un sistema
x = Ax+Bu y = Cx
se dice que el par (A,C) es observable si es posible reconstruir la condicion inicial x0
en un tiempo finito tf a partir del conomiento de la entrada u(t) y la salida y(t) en
el intervalo tε[0, tf ]. De otro modo se dice que el par (A,C) es no (completamente)
observable.
Dado el principio de dualidad, si el par (AT , CT ) es controlable, entonces el par (A,C)
es observable, se analiza la observabilidad del cuadricoptero al obtener la matriz de
observabilidad O = CT(AT ,CT )
O =
1 0 0 0 0 0
0 1 0 0 0 0
0 0 1 0 0 0
0 0 0 1 0 0...
......
......
...
0 0 0 0 0 0
0 0 0 0 0 0
El rango de la matriz de observabilidad es igual al numero de estados, indicando que no
hay dependencia lineal entre las columnas de la matriz. Por lo tanto, el par (A,C) es
observable.
2.3.2. Observador de Luenberger
El problema de diseno consiste en los valores caracterısticos de la matriz (A− LC), los
cuales dictan la dinamica del error de observacion. En vista de que los valores carac-
terısticos de una matriz no se alteran al transponerla, el problema de diseno de la matriz
L es el mismo que en el caso de diseno de controladores. Por lo tanto, si se disena para
la matriz (AT − CTLT ), el problema es numericamente igual al diseno de la matriz k.
Controlador y Observador 24
De modo que los polos deseados para la matriz (A − LC) seran 5 veces mas rapidos a
los de la matriz (A−Bk) con el fin de asegurar un correcto funcionamiento del sistema.
L =
0 0 0
0 2200 0
0 0 2200
0 0 0
0 1196600 0
0 0 800
Se tiene un termino demasiado grande, provocando que la velocidad angular del segundo
motor ω2 opere en condiciones irreales dada la gran diferencia entre las constantes de
tiempo. Se modifica este termino y se tiene, por lo tanto
L =
0 0 0
0 2200 0
0 0 2200
0 0 0
0 11966 0
0 0 800
Con los valores caracterısticos en el observador asintotico A− LC
λ1 λ2 λ3 λ4 λ5 λ6
−5.000 −5.000 −157.600 −5.000 −5.500 −2185.6
2.4. Control por estados observados
Se considera el sistema controlable y observable el cual sera controlado mediante el
control por realimentacion de estados observados (o control por realimentacion de la
salida)
u(t) = −kx+ r(t) ˙x(t) = (A− LC)x(t) + bu(t) + Ly(t)
El controlador tiene como funcion de transferencia
C(s) = k[sI −A+Bk + LC]−1L
Dado que el controlador y el observador pueden realizarse de forma independiente gracias
al principio de separacion, las matrices k y L previamente desarrolladas seran utilizadas
para resolver el problema de control sobre el cuadricoptero.
Controlador y Observador 25
Figura 2.2: Contol por realimentacion de estados observados
2.5. Simulaciones
Inicialmente, se simula el control por estados realimentados (Fig. 2.3) sin el observador
para asegurar que se tenga un comportamiento deseado. Se dice que la planta tiene una
condicion inicial en la posicion de aproximadamente π radianes. Las salidas de interes
son los angulos Roll, Pitch y Y aw, la velocidad angular (Fig. 2.4) y la senal de control
(Fig. 2.5). Esta ultima no puede tomar valores mayores a uno, ya que se trata de una
senal PWM donde unicamente esta definido un cliclo de trabajo de 0 % a 100 %.
Figura 2.3: Control por realimentacion de estados
Controlador y Observador 26
0 1 2 3 4 5 6 7 8 9 100
0.5
1
1.5
2
2.5
3
0
0.5
1
1.5
2
2.5
3
0
0.5
1
1.5
2
2.5
3
Time offset: 0 Student Version of MATLAB
0 1 2 3 4 5 6 7 8 9 10−3
2.5
−2
1.5
−1
0.5
0
−3
2.5
−2
1.5
−1
0.5
0
−3
2.5
−2
1.5
−1
0.5
0
Time offset: 0 Student Version of MATLAB
Figura 2.4: Angulos (izquierda) y velocidades angulares
0 1 2 3 4 5 6 7 8 9 10.75
0.8
.85
0.9
.95
1
0
0.2
0.4
0.6
0.8
1
0
0.2
0.4
0.6
0.8
1
0
0.2
0.4
0.6
0.8
1
Time offset: 0 Student Version of MATLABFigura 2.5: Senal de control
Controlador y Observador 27
Una vez que se comprueba que el sistema es estable y que se logra el objetivo de control,
se simula el control realimentado por estados observados (Fig. 2.6). Las condiciones
iniciales del observador se definen como nulas. Las salidas de interes son iguales a la
simulacion anterior, sin embargo en este caso se compara el estado observado con el
estado del sistema (Fig. 2.7).
Figura 2.6: Control por realimentacion de estados observados
0 1 2 3 4 5 6 7 8 9 100
0.5
1
1.5
2
2.5
3
3.5
0
0.5
1
1.5
2
2.5
3
3.5
0.5
0
0.5
1
1.5
2
2.5
3
Time offset: 0 Student Version of MATLAB
0 1 2 3 4 5 6 7 8 9 100.1
0
0.1
0.2
0.3
0.4
0.5
0.6
−15
−10
−5
0
5
10
15
20
−8
−7
−6
−5
−4
−3
−2
−1
0
1
2
Time offset: 0 Student Version of MATLAB
Figura 2.7: Angulos (izquierda) y velocidades angulares con observador
Controlador y Observador 28
0 1 2 3 4 5 6 7 8 9 100.5
0.6
0.7
0.8
0.9
1
0.7
.75
0.8
.85
0.9
.95
1
0
0.2
0.4
0.6
0.8
1
0.4
0.5
0.6
0.7
0.8
0.9
Time offset: 0 Student Version of MATLABFigura 2.8: Senal de control con observador
Las simulaciones demuestran que es posible alcanzar el objetivo de control en el cua-
dricoptero. Los angulos Roll, Pitch, Y aw convergeran a su punto de equilibrio, de igual
forma tanto la dinamica del error como la del error en la estimacion convergen a cero. La
salida tiene un comportamiento amortiguado y la senal de control es relativamente rapi-
da pero al tratarse de motores de corriente directa y ya que no se invierte la polaridad
se puede decir que es ’soportable’ por el actuador.
Capıtulo 3
Implementacion
3.1. Introduccion
Actualmente, gracias al avance tecnologico en la electronica digital es posible construir
sofisticados sistemas a un precio razonablemente bajo, ademas de ser confiables y fle-
xibles. Uno de estos grandes avance son los microcontroladores. Un microcontolador es
un circuito integrado que incluye en su interior tres unidades funcionales de una compu-
tadora: CPU, Memoria y Unidades de E/S, es decir, se trata de un computador completo
en un solo circuito integrado.
La arquitectura tradicional de los microcontroladores se basa en el esquema conocido
como Harvard, en el que el CPU esta conectado a dos memorias por intermedio de dos
buses separados. Una de las memorias contiene solamente las instrucciones del programa,
y es llamada Memoria de Programa. La otra memoria solo almacena los datos y es
llamada Memoria de Datos. La principal ventaja en esta arquitectura es que el tiempo
de acceso a las instrucciones puede superponerse con el de los datos, logrando una mayor
velocidad de operacion.
Dado que la gran mayorıa de los sistemas de control se implementan en forma digital, es
necesario en alguna etapa del diseno discretizar; bien el controlador en tiempo continuo
disenado en base a un modelo en tiempo continuo de la planta, o bien la planta en tiempo
continuo misma a fin de realizar el diseno del controlador discreto en forma directa.
Un convertidor analogico-digital convierte la senal medida en tiempo continuo x(t) en
una secuencia de valores discretos x[k], aptos para ser procesados en un computador
digital. Mientras que un digital-analogico convierte la secuencia discreta x[k] en una senal
analogica x(t) que puede ser interpretada por la mayorıa de los dispositivos electronicos.
29
Implementacion 30
3.2. Sistema discreto
Se considera ahora un sistema controlado mediante un esquema de control digital
Figura 3.1: Sistema digital
En este caso el analisis del sistema se debe efectuar a partir de las senales muestreadas,
que resultan en senales de tiempo discreto. Se considera a la senal x(t), tεR la senal
muestreada x[k] se define mediante
x[k] = x(t) t = kTs
Donde Ts es el tiempo de muestreo. Por otro lado, al no existir el concepto de derivada
en una senal en tiempo discreto, la representacion o modelo de un sistema se realiza
mediante ecuaciones en diferencias. En el caso de un sistema representado mediante
variables de estado, estas toman la forma
x[k + 1] = fd(x[k], u[k]) y[k] = hd(x[k], u[k])
Si las ecuaciones son lineales (o linealizadas)
x[k + 1] = Adx[k] +Bdu[k] y[k] = Cdx[k] +Ddu[k]
donde para su solucion es necesario conocer la condicion inicial x[0] y la entrada u[k].
3.2.1. Discretizacion
Para obtener la representacion en tiempo discreto de un sistema continuo se utiliza la
discretizacion exacta con reten de orden cero (ZOH por sus siglas en ingles) a la salida
del DAC el cual retiene y mantiene constante una senal en tiempo discreto entre los
tiempos de muestreo. Para estas condiciones, se evalua el estado del sistema continuo en
Implementacion 31
el instante de muestreo t = (k + 1)Ts
x[k + 1] , x((k + 1)Ts) = eA(k+1)Tsx(0) +
∫ (k+1)T
0eA((k+1)Ts−τ)Bu(τ) dτ =
= eATsx[k] +
(∫ Ts
0eAσ dσ
)Bu[k]
donde σ = (k + 1)T − τ . Ası se llega al modelo discreto lineal donde
Ad , eATs Bd ,∫ Ts0 eAτ dτB
Cd , C Dd , D
Para obtener el model discreto del cuadricoptero se elabora un experimento para calcular
el tiempo de muestreo del microcontrolador. Solo se hace uso de la placa y el codigo que
se muestra a continuacion
void setup(){
Serial.begin(9600);
muestreo();
}
void loop(){
}
void muestreo(){
unsigned long time1=0;
unsigned long time=0;
for(byte i =0; i<4; i++){
time1=micros();
int A=analogRead(A0);
time=micros()-time1;
Serial.print(" Muestra: ");
Serial.print(i+1);
Serial.print(" Tiempo: ");
Serial.println(time);
}
}
En el monitor serial se muestra el tiempo que se tarda el microcontrolador en ejecutar el
conjunto de instrucciones. Se tiene una media de 112µs lo que equivale a una frecuencia
de muestreo de 8.928Hz, un dato aceptado ya que se utiliza el Arduino UNO. Este
Implementacion 32
intervalo de tiempo es utilizado para discretizar al sistema obteniendo, finalmente el
sistema discreto linealizado sobre el punto de equilibrio
Ad =
1.0000 0 0 0.0001 0 0
0 1.0000 0 0 0.0001 0
0 0 1.0000 0 0 0.0001
0 0 0 1.0000 0 0
0 0 0 0 1.0000 0
0 0 0 0 0 1.0000
Bd =
0.0000 0 0.0000 0
0 0.0000 0 0.0000
0.0000 0.0000 0.0000 0.0000
0.0183 0 −0.0183 0
0 0.0164 0 −0.0164
0.0018 −0.0018 0.0018 −0.0018
Cd =
1 0 0 0 0 0
0 1 0 0 0 0
0 0 1 0 0 0
3.2.2. Controlador
El analisis adecuado es el mismo que se emplea en tiempo continuo sobre las matrices de
controlabilidad. Por lo tanto, el problema de asignacion de polos de la matriz (Ad−Bdkd)es el mismo que en el caso continuo. La unica diferencia radica en la ubicacion de los
polos de diseno. Para disenar los polos en tiempo discreto conviene primero establecer
los polos en tiempo continuo deseados de la matriz (A−Bk) y despues disenar kd para
asignar los polos de (Ad −Bdkd).
Dadas estas observaciones sobre el diseno de la ganancia, se tienen los vectores
kd =
0.6980 0.0000 0.4991 0.7011 0.0000 0.5147
0.0000 0.6989 −0.4991 0.0000 0.7023 −0.5147
−0.6980 −0.0000 0.4991 −0.7011 0.0000 0.5147
−0.0000 −0.6989 −0.4991 −0.0000 −0.7023 −0.5147
Implementacion 33
Con los valores caracterısticos en lazo cerrado Ad −Bdkd en
λ1 λ2 λ3 λ4 λ5 λ6
0.9745 0.9771 0.9965 0.9999 0.9999 0.9999
Un sistema discreto, lineal, invariante en el tiempo es asintoticamente estable si todos
los valores caracterısticos de la matriz en lazo cerrado estan dentro de la unidad. Por
lo que se confirma que el sistema en tiempo continuo y discreto se comportaran de la
misma manera.
3.2.3. Observador
Si el sistema es observable y se disena los polos de (Ad−LdCd) de modo que se encuen-
tren dentro del cırculo unitario entonces se asegura que el error de observacion tiende
asintoticamente a cero. La matriz de ganancias del observador en tiempo discreto resulta
Ld =
0.2177 0 0
0 0.0181 0
0 0 0.0011
1.1884 0 0
0 0.0875 0
0 0 0.0028
Con los valores caracterısticos en el observador asintotico Ad − LdCd
λ1 λ2 λ3 λ4 λ5 λ6
0.9994 0.9994 0.7829 0.9825 0.9994 0.9994
3.3. Simulaciones
Se simula el sistema en tiempo discreto (Fig. 3.2) con el fin de analizar su respuesta y
verificar que las condiciones de estabilidad y observabilidad se cumplan. De igual forma
que en el sistema continuo, las salidas de interes son los angulos Roll, Pitch y Y aw,
la velocidad angular (Fig. 3.3) y la senal de control (Fig. 3.4). El mismo bloque de
saturacion es colocado en cascada con la senal de error para simular el ciclo de trabajo
del actuador. Ademas, de igual manera, las condiciones iniciales son π radianes en los
angulos.
Implementacion 34
Figura 3.2: Control discreto por realimentacion de estados
0 1 2 3 4 5 6 70
0.5
1
1.5
2
2.5
3
0
0.5
1
1.5
2
2.5
3
0
0.5
1
1.5
2
2.5
3
Time offset: 0 Student Version of MATLAB
0 1 2 3 4 5 6 7−3
2.5
−2
1.5
−1
0.5
0
−3
2.5
−2
1.5
−1
0.5
0
−3
2.5
−2
1.5
−1
0.5
0
Time offset: 0 Student Version of MATLAB
Figura 3.3: Angulos (izquierda) y velocidades angulares
Una vez que se comprueba que el sistema es estable y que se logra el objetivo de control,
se simula el control realimentado por estados observados (Fig. 3.5). Las condiciones
iniciales del observador se definen como nulas. Las salidas de interes son iguales a la
simulacion anterior, sin embargo en este caso se compara el estado observado con el
estado del sistema (Fig. 3.6).
De las simulaciones se concluye que el tiempo de muestreo del microcontrolador es lo
suficientemente rapido para emular un sistema continuo. Solo existen unas pequenas
diferencias en las senales, sin embargo no son lo suficientemente relevantes para llevar
a cabo alguna otra accion de control. El controlador y observador final a implementar
sobre el microcontrolador sera el descrito en este capıtulo.
Implementacion 35
0 1 2 3 4 5 6 7.75
0.8
.85
0.9
.95
1
0
0.2
0.4
0.6
0.8
1
0.2
0.4
0.6
0.8
1
0
0.2
0.4
0.6
0.8
1
Time offset: 0 Student Version of MATLABFigura 3.4: Senal de control discreto
Figura 3.5: Control discreto por realimentacion de estados observados
Implementacion 36
0 1 2 3 4 5 6 70.5
0
0.5
1
1.5
2
2.5
3
0.5
0
0.5
1
1.5
2
2.5
3
0.5
0
0.5
1
1.5
2
2.5
3
3.5
Time offset: 0 Student Version of MATLAB
0 1 2 3 4 5 6 7−8
−6
−4
−2
0
2
4
−10
−5
0
5
10
15
−15
−10
−5
0
5
10
15
20
Time offset: 0 Student Version of MATLAB
Figura 3.6: Angulos (izquierda) y velocidades angulares con observador discreto
0 1 2 3 4 5 6 70.4
0.5
0.6
0.7
0.8
0.9
1
0
0.2
0.4
0.6
0.8
1
0
0.2
0.4
0.6
0.8
1
0
0.2
0.4
0.6
0.8
1
Time offset: 0 Student Version of MATLABFigura 3.7: Senal de control con observador discreto
Implementacion 37
3.4. Puesta a punto
Se implementa el controlador por realimentacion de estados observados en el microcon-
trolador Arduino UNO. El algoritmo que seguira el microcontrolador es
1. Configurar el microcontrolador y las constantes (Ad, Bd, Cc, kd, Ld)
2. Inicializar x[k] = 0
3. Escribir a puerto de salida x[k]
4. Leer el ADC r[k], y[k]
5. Calcular u[k] = −kdx[k] + r[k]
6. Calcular x[k + 1] = (Ad − LdCd)x[k] +Bdu[k] + Ldy[k]
7. Esperar al siguiente Ts
8. Reasignar x[k]← x[k + 1]
9. Regresar al paso 3
Por otro lado, los dispositivos electronicos empleados para lograr estabilizar el cua-
dricoptero son
Frame para cuadricoptero Syma X5C
Microcontrolador ATMega328.
Giroscopio-Acelerometro de 3 ejes MPU6050.
Transistores de potencia NPN modelo BD135.
Resistencias de distintos valores.
Tableta para prototipos.
Las conexiones electricas al microcontrolador y a los distintos dispositivos se muestran
a continuacion en la Fig. 3.8. Se utilizan dos fuentes de poder, una para polarizar el mi-
crocontrolador que bien podrıa ser la computadora y otra mas para la etapa de potencia
en los motores.
Implementacion 38
)))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))
C))x)x)DD)
C)))
)C))))
)xF)))
00))4
00)))
00))6
00)))
004)8
00)))
006)
00)))
DD)2)8
DD)))
0)) )
0)) )
0)) 4
0)) )
0)4 6
0)) ))
0)6 ))
0)) ))
0)) ))
0)) )4
0)) ))
0)) )6
0)4 ))
0)) )8
0)6 )
))
x)
x)
x)
x4
)x8)))))8
80)))))
0))))
8) 8)
8) 84
Figura 3.8: Diagrama de conexiones
Finalmente, el codigo dentro del ambiente Arduino IDE desarrollado para lograr el
objetivo de control es el siguiente
#include <Wire.h>
//Direccion I2C de la IMU
#define MPU 0x68
//Ratios de conversion
#define A_R 16384.0
#define G_R 131.0
//Conversion de radianes a grados 180/PI
#define RAD_A_DEG = 57.295779
//MPU-6050 da los valores en enteros de 16 bits
//Valores sin refinar
int16_t AcX, AcY, AcZ, GyX, GyY, GyZ;
float Acc[2];
float Gy[2];
float Angle[4];
float y[4],x[]={0,0,0,0,0,0,0},u[5],xs[7],r=0.8;
int out[]={6,9,10,11};
Implementacion 39
void setup() {
Wire.begin();
Wire.beginTransmission(MPU);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
Serial.begin(9600);
pinMode(out[0], OUTPUT);
pinMode(out[1], OUTPUT);
pinMode(out[2], OUTPUT);
pinMode(out[3], OUTPUT);
}
void loop() {
//Leer los valores del Acelerometro de la IMU
Wire.beginTransmission(MPU);
Wire.write(0x3B); //Pedir el registro 0x3B - corresponde al AcX
Wire.endTransmission(false);
Wire.requestFrom(MPU,6,true); //A partir del 0x3B, se piden 6 registros
AcX=Wire.read()<<8|Wire.read(); //Cada valor ocupa 2 registros
AcY=Wire.read()<<8|Wire.read();
AcZ=Wire.read()<<8|Wire.read();
//Se calculan los angulos Y, X respectivamente.
Acc[1] = atan(-1*(AcX/A_R)/sqrt(pow((AcY/A_R),2)+pow((AcZ/A_R),2)))*RAD_TO_DEG;
Acc[0] = atan((AcY/A_R)/sqrt(pow((AcX/A_R),2)+pow((AcZ/A_R),2)))*RAD_TO_DEG;
//Leer los valores del Giroscopio
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU,4,true);
GyX=Wire.read()<<8|Wire.read();
GyY=Wire.read()<<8|Wire.read();
//Calculo del angulo del Giroscopio
Gy[0] = GyX/G_R;
Gy[1] = GyY/G_R;
//Aplicar el Filtro Complementario
Angle[0] = 0.98 *(Angle[0]+Gy[0]*0.010) + 0.02*Acc[0];
Implementacion 40
Angle[2] =Angle[0]-4;
Angle[1] = 0.98 *(Angle[1]+Gy[1]*0.010) + 0.02*Acc[1];
Angle[3] =Angle[1]+3.38;
y[1]=Angle[2];
y[2]=Angle[3];
y[3]=0;
u[1]=-(0.698*x[1]+0.4991*x[3]+0.7011*x[4]+0.5147*x[6])+r;
u[2]=-(0.6989*x[2]-0.4991*x[3]+0.7023*x[5]-0.5147*x[6])+r;
u[3]=-(-0.698*x[1]+0.4991*x[3]-0.7011*x[4]+0.5147*x[6])+r;
u[4]=-(-0.6989*x[2]-0.4991*x[3]-0.7023*x[5]-0.5147*x[6])+r;
//Se~nal de control a los actuadores
analogWrite(out[0],255*u[1]);
analogWrite(out[1],255*u[2]);
analogWrite(out[2],255*u[3]);
analogWrite(out[3],255*u[4]);
xs[1]=0.7823*x[1]+0.0001*x[4]+0.2177*y[1];
xs[2]=0.9819*x[2]+0.0001*x[5]+0.0181*y[2];
xs[3]=0.9989*x[3]+0.0001*x[6]+0.0011*y[3];
xs[4]=-1.1884*x[1]+1*x[4]+0.0183*u[1]-0.0183*u[3]+1.1884*y[1];
xs[5]=-0.0875*x[2]+1*x[5]+0.0164*u[2]-0.0164*u[4]+0.0875*y[2];
xs[6]=-0.0028*x[3]+1*x[6]+0.0018*u[1]-0.0018*u[2]+0.0018*u[3]
-0.0018*u[4]+0.0028*y[3];
x[1]=xs[1];
x[2]=xs[2];
x[3]=xs[3];
x[4]=xs[4];
x[5]=xs[5];
x[6]=xs[6];
}
3.5. Conlusiones
La implementacion del algoritmo de control es relativamente sencillo de programar,
sin embargo se tiene una considerable diferencia entre las simulaciones y los resultados
obtenidos. Estas diferencias se le atanen principalmente a la estimacion de parametros
en el cuadricoptero, se cree que no son lo suficientemente cercanas a la realidad por lo
que el observador tiene problemas para lidiar con este error. Ademas, otro problema es
el modelado ideal de los motores, la constante que relaciona al voltaje con la velocidad
Implementacion 41
angular es diferente para cada motor, en otras palabras las helices del cuadricoptero
giran a velocidades diferentes aun aplicando el mismo voltaje en los cuatro motores.
El desarrollo del proyecto resulto bastante util para comprender en mayor manera el
modelado y control por variables de estado. Se analizan y repasan todos los temas vistos
en el curso, por lo que se tiene un mejor entendimiento de la teorıa de variables de estado.
Ademas, se nota la gran diferencia entre las simulaciones y la teorıa y la respuesta del
sistema fısico.
FELIX TEJEDA Fernando
TORRES RODRIGUEZ Ivan
Apendice A
Codigo en Matlab
clc;
clear all;
syms x1 x2 x3 x4 x5 x6 x7 x8 x9 x10 x11 x12;
syms u1 u2 u3 u4;
syms kd m k cm L b Ix Iy Iz g;
%Sistema resultante
eq1=’(L*k*cm)/Ix*(u1-u3)=0’;
eq2=’(L*k*cm)/Iy*(u2-u4)=0’;
eq3=’(b*cm)/Iz*(u1-u2+u3-u4)=0’;
%Punto de equilibrio
[U2 U3 U4]=solve(eq1,eq2,eq3,u2,u3,u4);
%Funciones
f4=-kd/m*x4+k*cm/m*(sin(x9)*sin(x7)+cos(x9)*cos(x7)*sin(x8))*(u1+u2+u3+u4);
f5=-kd/m*x5+k*cm/m*(cos(x7)*sin(x9)*sin(x8)-cos(x9)*sin(x7))*(u1+u2+u3+u4);
f6=-kd/m*x6-g+k*cm/m*(cos(x8)*cos(x7))*(u1+u2+u3+u4);
%Orientacion
f7=x10+x11*sin(x7)*tan(x8)+x12*cos(x7)*tan(x8);
f8=x11*cos(x8)-x12*sin(x7);
f9=sin(x7)/cos(x8)*x11+cos(x7)/cos(x8)*x12;
f10=L*k*cm/Ix*(u1-u3)-(Iy-Iz)/Ix*x11*x12;
f11=L*k*cm/Iy*(u2-u4)-(Iz-Ix)/Iy*x10*x12;
f12=b*cm/Iz*(u1-u2+u3-u4)-(Ix-Iy)/Iz*x10*x11;
a1=diff(f7,x7,1);
42
Appendix A. Codigo en Matlab 43
a2=diff(f7,x8,1);
a3=diff(f7,x9,1);
a4=diff(f7,x10,1);
a5=diff(f7,x11,1);
a6=diff(f7,x12,1);
display(’a1’)
pretty(a1)
display(’a2’)
pretty(a2)
display(’a3’)
pretty(a3)
display(’a4’)
pretty(a4)
display(’a5’)
pretty(a5)
display(’a6’)
pretty(a6)
clc;
clear all;
m=0.73; l=0.115; cm=573832;
Ix=960e-6; Iy=1070e-6; Iz=2298e-6;
k=2.376e-6; kd=200e-3; b=63.141e-9;
U=0.85;
t=0:0.01:10;
ci=[0 0 0 1 1 1];
%No lineal
f=@(t,x)[x(4)+x(5)*sin(x(1))*tan(x(2))+x(6)*cos(x(1))*tan(x(2));...
x(5)*cos(x(2))-x(6)*sin(x(1));...
sin(x(1))/cos(x(2))*x(5)+cos(x(1))/cos(x(2))*x(6);...
-(Iy-Iz)/Ix*x(5)*x(6);...
-(Iz-Ix)/Iy*x(4)*x(6);...
-(Ix-Iy)/Iz*x(4)*x(5)];
[t,x]=ode45(f,t,ci);
%Lineal
f=@(t,xl)[xl(4);xl(5);xl(6);0;0;0];
Appendix A. Codigo en Matlab 44
[t,xl]=ode45(f,t,ci);
figure
subplot(3,1,1)
plot(t,x(:,1),’b-’)
title(’Roll’);
hold on
plot(t,xl(:,1),’k--’)
subplot(3,1,2)
plot(t,x(:,2),’b-’)
title(’Pitch’);
hold on
plot(t,xl(:,2),’k--’)
subplot(3,1,3)
plot(t,x(:,3),’b-’)
title(’Yaw’);
hold on
plot(t,xl(:,1),’k--’)
legend(’NL’,’L’)
figure
subplot(3,1,1)
plot(t,x(:,3),’b-’)
title(’Omega_1’);
hold on
plot(t,xl(:,3),’k--’)
subplot(3,1,2)
plot(t,x(:,4),’b-’)
title(’Omega_2’);
hold on
plot(t,xl(:,4),’k--’)
subplot(3,1,3)
plot(t,x(:,5),’b-’)
title(’Omega_3’);
hold on
plot(t,xl(:,5),’k--’)
legend(’NL’,’L’)
%Matrices
A=[0 0 0 1 0 0;...
0 0 0 0 1 0;...
0 0 0 0 0 1;...
Appendix A. Codigo en Matlab 45
0 0 0 0 0 0;...
0 0 0 0 0 0;...
0 0 0 0 0 0];
B=[0 0 0 0;...
0 0 0 0;...
0 0 0 0;...
l*k*cm/Ix 0 -l*k*cm/Ix 0;...
0 l*k*cm/Iy 0 -l*k*cm/Iy;...
b*cm/Iz -b*cm/Iz b*cm/Iz -b*cm/Iz];
C=[eye(3) zeros(3)];
D=[zeros(3) zeros(3,1);...
zeros(3) zeros(3,1)];
%Funciones de tranferencia
s=tf(’s’);
G=C*inv(s*eye(6)-A)*B;
roots([1 8.328e-15 2.098e-17]);
roots([1 1.506e-14 5.002e-17]);
%Controlador
Ctrl=ctrb(A,B);
RangoCtrl=rank(Ctrl);
Q=eye(6);
R=eye(4);
[k,P,E]=lqr(A,B,Q,R);
%Estabilidad
P=lyap((A-B*k),eye(6));
det(P(1:2,1:2));
det(P(1:3,1:3));
det(P(1:4,1:4));
det(P(1:5,1:5));
det(P(1:6,1:6));
%Observador
Appendix A. Codigo en Matlab 46
Obs=ctrb(A’,C’)’;
RangoObs=rank(Obs);
L=place(A’,C’,5*E)’;
L(5,2)=L(5,2)/100;
eig(A-L*C);
%Discreto
Ts=112e-6;
[kd,Sd,Ed] = lqrd(A,B,Q,R,Ts);
[Ad,Bd] = c2d(A,B,Ts);
Ld=place(Ad’,C’,exp(Ts*eig(A-L*C)))’
eig(Ad-Ld*C);
exp(Ts*eig(A-L*C));