CAPÍTULO 5: DISEÑO DE...
22
49 CAPÍTULO 5: DISEÑO DE CONTROLADORES Una vez obtenido el modelo del quadrotor pasamos a la fase de diseño del control en posición. Pero el modelo que tenemos es un sistema no lineal, así pues para aplicar las conocidas técnicas de control en primer lugar hay que linealizar el modelo [5]. Aplicando la aproximación de ángulos pequeños a la matriz de rotación que aparece en el modelo: [ ] [ ] [ ] - = ⋅ - = ⋅ = → ≈ ≈ ≈ g U m z U m y U m x x con x x sen x 1 1 1 1 ) ( 1 ( 1 0 ) ( ) ( 0 & & & & & & φ θ Finalmente suponiendo que la potencia total aproximadamente contrarresta la fuerza gravitatoria, g m U U ⋅ = ≈ 1 1 , excepto en el eje z. [ ] [ ] [ ] ) 15 ( 1 ) ( 1 ( 1 1 1 1 - = ⋅ - = ⋅ = g U m z U m y U m x & & & & & & φ θ Ya tenemos el modelo lineal de la aceleración que necesitábamos para poder diseñar los controladores en posición. El modelo lineal resultante es una aproximación a la realidad y por lo tanto en los experimentos comprobaremos cuanto se aproxima al modelo real de nuestro quadrotor. 5.1. Diseño del controlador por el lugar de las raíces Para diseñar los controladores nos basaremos en la herramienta del lugar de las raíces, útil para analizar sistemas lineales dinámicos tipo SISO (single input single output) y su estabilidad (BIBO) [6].
Transcript of CAPÍTULO 5: DISEÑO DE...
49
CAPÍTULO 5: DISEÑO DE CONTROLADORES
Una vez obtenido el modelo del quadrotor pasamos a la fase de diseño del
control en posición. Pero el modelo que tenemos es un sistema no lineal, así pues para
aplicar las conocidas técnicas de control en primer lugar hay que linealizar el modelo
[5].
Aplicando la aproximación de ángulos pequeños a la matriz de rotación que
aparece en el modelo:
[ ]
[ ]
[ ]
−=
⋅−=
⋅=
→
≈≈
≈
gUm
z
Um
y
Um
x
xcon
xxsen
x
1
1
1
1
)(1
(1
0)(
)(
0
&&
&&
&&
φ
θ
Finalmente suponiendo que la potencia total aproximadamente contrarresta la
fuerza gravitatoria, gmUU ⋅=≈ 11 , excepto en el eje z.
[ ]
[ ]
[ ]
)15(
1
)(1
(1
1
1
1
−=
⋅−=
⋅=
gUm
z
Um
y
Um
x
&&
&&
&&
φ
θ
Ya tenemos el modelo lineal de la aceleración que necesitábamos para poder
diseñar los controladores en posición. El modelo lineal resultante es una aproximación a
la realidad y por lo tanto en los experimentos comprobaremos cuanto se aproxima al
modelo real de nuestro quadrotor.
5.1. Diseño del controlador por el lugar de las raíces
Para diseñar los controladores nos basaremos en la herramienta del lugar de las
raíces, útil para analizar sistemas lineales dinámicos tipo SISO (single input single
output) y su estabilidad (BIBO) [6].
50
El lugar de las raíces permite determinar la posición de los polos de la función
de transferencia en lazo cerrado para un determinado valor de ganancia a partir de la
función de transferencia en lazo abierto G(s).
Para facilitar los cálculos usamos la función de matlab “sisotool” (figura 32),
una herramienta para el diseño de sistemas lineales mediante el lugar de las raíces. A
esta función debemos pasarle sólo la función de transferencia en lazo abierto. Una vez
ejecutada la aplicación podemos añadir polos y ceros para formar el controlador y
comprobar como sería la respuesta del sistema a un escalón como entrada.
Figura 32 : Sisotool
5.1.1. Controlador en X-Y
El control en X-Y será igual debido a la simetría del quadrotor y que sale a
relucir en el modelo. Por lo tanto diseñaremos un controlador para X y este mismo
controlador valdrá para el control en Y.
La función de transferencia en lazo abierto para el control de X será:
22)()()(
s
gsGs
s
gsxgx Integrando =→⋅= →⋅= θθ&&
Como se puede ver la G(s) no es más que dos integradores en el origen por una
constante con lo cual, si esta función de transferencia es correcta no debemos de tener
errores en régimen permanente al tener ya integradores en la G(s).
51
Como primera aproximación decidimos usar un controlador proporcional-
derivativo (PD) debido a que deseamos que el movimiento del quadrotor sea suave y sin
sobreoscilaciones.
5.0101.0
14.1 ⋅+⋅
+⋅=s
sCxPD
Situamos un cero real en -0.7142 y ajustamos la ganancia para que los polos
sean reales. Además añadimos al controlador un polo de alta frecuencia para que sea
una función propia.
En el lugar de las raíces (figura 33) comprobamos que los polos en lazo cerrado
se situaran en las posiciones:
64.92
54.6
8085.0
3
2
1
−=−=
−=
p
p
p
Figura 33: Lugar de las raíces PD
En los resultados experimentales con el quadrotor comprobamos que el
controlador PD no es válido porque presenta un error en régimen permanente. Esto nos
indica que el modelo, como ya esperábamos, sólo es una aproximación.
52
Para solventar este error diseñamos un control proporcional-integral-derivativo,
PID.
2.185.20285.0
185.2365.22
2
⋅⋅+⋅+⋅+⋅=ss
ssCxPID
Situamos un cero complejo en -0.6024±0.2446 y ajustamos la ganancia para que
la respuesta ante un escalón sea parecida a un sistema de primer orden y añadimos al
controlador un polo de alta frecuencia para que se una función propia.
En el lugar de las raíces (figura 34) vemos que los polos en lazo cerrado se
situaran en las posiciones:
193.89
471.9
206.06682.0
3
2
1
−=−=
±−=
p
p
p
Figura 34: Lugar de las raíces PID
Al final de este apartado se muestran los resultados obtenidos en Simulink con
los distintos controladores y valoraremos estos resultados.
5.1.2. Controlador en Z
El control en Z será totalmente distinto al control X-Y. La función de
transferencia en lazo abierto para el control de Z será:
53
22
1)()(
1)(
smsGsT
smszg
m
Tz Integrando
⋅=→⋅
⋅= →−=&&
Como se puede ver la G(s) no es más que dos integradores en el origen por una
constante, la masa del quadrotor. Deseamos tener una respuesta rápida en Z para evitar
que el quadrotor caiga al suelo cuando hay un cambio brusco en X-Y, ya que están
acoplados. Para ello diseñamos un control proporcional-integral-derivativo, PID.
501.0
15.02
2
⋅+⋅
++⋅=ss
ssCxPID
Situamos un cero complejo en -1±1 y ajustamos la ganancia para que la
respuesta ante un escalón tenga un tiempo de subida pequeño y también sobreoscile lo
menos posible. Además añadimos al controlador un polo de alta frecuencia para que se
una función propia.
En el lugar de las raíces (figura 35) comprobamos que los polos en lazo cerrado
se situaran en las posiciones:
26.97
5054.1
8008.1606.0
3
2
1
−=−=
±−=
p
p
p
Figura 35: Lugar de las raíces PID
Al final de este apartado se muestran y valoran los resultados obtenidos en
Simulink.
54
5.2. Integración en Simulink
Una vez diseñado el controlador para cada eje de coordenadas pasamos a la
simulación para ver el comportamiento de dichos controladores. Para esta labor usamos
la herramienta de matlab, “Simulink”, muy buena para la simulación de todo tipo de
sistemas de control continuos.
El esquema general de nuestro bucle de control será el que se muestra en la
siguiente figura 36:
Figura 36: Esquema general del bucle de control
Como ya hemos comentado, Simulink es un simulador para sistemas de control
continuos pero en nuestro caso requerimos que el sistema de control sea discreto porque
cuando tengamos que programar en C++ todo el control será como es lógico en discreto.
Por lo tanto nos vemos obligado a transformar cada uno de los bloques a su homólogo
discreto. La frecuencia de muestreo deseada será de 100 Hz (T = 0.01) ya que el tiempo
de ejecución del bucle de control es de unos 10 ms.
5.2.1. Modelo
En el subsistema “Modelo No Lineal” se encuentra el modelo calculado en el
apartado anterior. En la figura 37 que se muestra a continuación podemos ver los
bloques que forman el subsistema.
55
Figura 37: Modelo
Este conjunto de bloques representa el sistema formado por las ecuaciones (3).
En primer lugar tenemos las entradas al sistema que son los ángulos de navegación y la
potencia dada a los rotores. En el bloque “Modelo” se encuentra la función matlab que
realiza el cálculo del sistema de ecuaciones (13) y da como salida las aceleraciones
angulares. Para tener la posición a partir de la aceleración angular debemos integrar dos
veces que es lo que hace el bloque “1/S^2 discreto”:
1222
1
22
1−−−
−=
⋅+−⋅= → kkKkT
ZS
UUETUS
5.2.2. Prefiltro
Para suavizar la respuesta del sistema a cambios de referencia se introduce un
prefiltro en la referencia. Como consecuencia se introduce un retraso en el seguimiento
de la referencia.
El prefiltro tendrá una función de transferencia de primer orden, donde el polo
de la función de transferencia debe ser suficiente para suavizar la respuesta del sistema
modificando lo menos posible las propiedades del control.
1
1
)1( −
−=
⋅⋅−+⋅⋅= →+ kkk
T
ZS
UTaETaUSa
a
En la figura 38 que se muestra a continuación podemos ver los bloques que
forman el subsistema.
56
Figura 38: Prefiltro de la referencia
5.2.3. Rotación
El bloque de rotación es de vital importancia para poder controlar el quadrotor,
pues es el encargado de relacionar el sistema de referencia tierra con el sistema de
referencia cuerpo. En este cambio de sistemas de referencia se ha hecho la
simplificación de que tanto el plano X-Y de tierra y como el de cuerpo son planos
paralelos. Por lo tanto ambos sistemas de referencia podrán diferir en una rotación
respecto al eje Z que viene dada por el valor de yaw.
−=
)cos()(
)()cos()(
xxsen
xsenxxR
Con esta rotación el quadrotor se moverá siempre en su sistema de referencia
pero en función a medidas del sistema de referencia de tierra. En la figura 39 podemos
ver los bloques que forman el subsistema.
Figura 39: Rotación en Yaw
57
5.2.4. Cálculo del error
El error entre la medida y la referencia deseada se calcula de una forma tan
sencilla como muestra la figura 40, haciendo la resta entre la referencia y el valor
medido.
Figura 40: Calculo del error
5.2.5. Controladores
En el subsistema “Controladores” se encuentra el controlador para cada eje de
coordenada calculado anteriormente. En la figura 41 que se muestra a continuación
podemos ver los bloques que forman el subsistema.
Figura 41: Controladores
Los controladores han sido discretizados mediante la transformación de Tustin
para obtener las constantes que forman un controlador discreto.
58
T
TdKpq
T
Td
Ti
TKpq
T
Td
Ti
TKpq
⋅=
⋅−⋅
+−⋅=
+⋅
+⋅=
2
1
0
)2
21(
)2
1(
El controlador discreto tendrá una forma tal y como se muestra a continuación
(figura 42):
221101 ++− ⋅+⋅+⋅+= kkkkk eqeqeqUU
Figura 42: Controlador discreto
5.2.6. Saturación
El bloque de saturación es algo necesario para evitar valores de cabeceo y alabeo
demasiado grandes que provoquen inestabilidad en el quadrotor. En un primer lugar se
optó por una saturación de 30º pero tras diferentes pruebas se decidió por un valor más
conservador como es 20º.
Figura 43: Saturación
En estos dos bloques (figura 43) realizan la saturación para valores de pitch y
roll mayores de ±20º. Y como es lógico si no se satura la señal se deja pasar con el
mismo valor de entrada.
59
5.2.7. Resultados de la simulación
En este apartado mostraremos los resultados obtenidos en Simulink para cada
uno de los controladores.
5.2.7.1 Controlador X-Y
Como ya comentamos antes, el control para el eje Y es igual que para X y por lo
tanto sólo mostraremos los resultados para uno de ellos. La respuesta cuando hay un
cambio de referencia de 1 metro en X y el controlador es un PD es la que se muestra en
la figura 44.
Figura 44: Resultado controlador PD
La respuesta obtenida es la esperada con un tiempo de subida de 1,4 segundos,
sin embargo cuando la implementamos en el control del quadrotor nos aparece un error
en régimen permanente que hace que no sea útil para el control de la posición.
La solución a este problema es utilizar un controlador PID, con el cual
obtenemos la respuesta que se muestra en la figura 45. Comprobamos que ahora
tenemos una pequeña sobreoscilación y un tiempo de subida de 0,9 segundos. El
resultado no es muy recomendable para el control de un quadrotor porque no es
aconsejable que sobreoscile.
60
Figura 45: Resultado controlador PID
Para intentar suavizar la respuesta ante un cambio de referencia se introduce el
uso de un prefiltro. Se coge la constante de tiempo del prefiltro para que la respuesta sea
la deseada. Si observamos el resultado obtenido (figura 46) vemos que hemos eliminado
la sobreoscilación a costa de aumentar el tiempo de subida a 2,1 segundos.
Figura 46: Resultado controlador PID con prefiltro
61
5.2.7.2 Controlador Z
El controlador en Z también es un PID con una respuesta como la que se muestra
en la figura 47. En este caso también tenemos una sobreoscilación de un 21% y un
tiempo de subida de 0.18 segundos.
Figura 47: Resultado controlador PID
Al igual que en el control en X-Y también introducimos un prefiltro en la
referencia para suavizar la respuesta (figura 48).
Figura 48: Resultado controlador PID con prefiltro
62
5.3. Integración software de los controladores
La integración software del control es un paso importante puesto que el buen
funcionamiento de toda la integración se basa principalmente en él. En esta integración
se trasladará a C++ los mismos bloques usados en Simulink por lo tanto no plantea
demasiada dificultad. A continuación se muestra el diagrama de flujo de control.
Figura 49: Diagrama de flujo del controlador
63
Ahora mostramos como se ha implementado cada uno de los bloques de
Simulink en C++.
5.3.1. Prefiltro
El prefiltro discretizado es 1)1( −⋅⋅−+⋅⋅= kkk UTaETaU y por lo tanto la
implementación en C++ no es más que expresar la ecuación con las variables
adecuadas.
// referencia X
double Rx;
Rx = 0.004*Xd1 + 0.996*Rx1;
Rx1 = Rx;
Xd1 = Xd;
// referencia Y
double Ry;
Ry = 0.004*Yd1 + 0.996*Ry1;
Ry1 = Ry;
Yd1 = Yd;
// referencia Z
double Rz;
Rz = 0.004*Zd1 + 0.996*Rz1;
Rz1 = Rz;
Zd1 = Zd;
5.3.2. Rotación
La rotación respecto al eje Z se integra como se muestra a continuación:
//Referencia
64
if (Play == 0)
{
//Con prefiltro
rx = Rx*cos(Ya) + sin(Ya)*Ry;
ry = Rx*(-sin(Ya)) + cos(Ya)*Ry;
rz = Rz;
}
else
{
//Sin prefiltro
rx = Xd*cos(Ya) + sin(Ya)*Yd;
ry = Xd*(-sin(Ya)) + cos(Ya)*Yd;
rz = Zd;
}
//Posición
x = Xmed*cos(Ya) + sin(Ya)*Ymed;
y = Xmed*(-sin(Ya)) + cos(Ya)*Ymed;
z = Zmed;
5.3.3. Controladores
// CONTROL EN EL EJE X
control.X_Uk = X_Uk1 + q0x*Ekx + q1x*X_Ek1 + q2x*X_Ek2;
//Actualización de las variables de control
X_Uk1 = control.X_Uk;
X_Ek2 = X_Ek1;
X_Ek1 = Ekx;
// CONTROL EN EL EJE Y
control.Y_Uk = Y_Uk1 + q0y*Eky + q1y*Y_Ek1 + q2y*Y_Ek2;
//Actualización de las variables de control
Y_Uk1 = control.Y_Uk;
Y_Ek2 = Y_Ek1;
Y_Ek1 = Eky;
// CONTROL EN EL EJE Z
65
control.Z_Uk = Z_Uk1 + q0z*Ekz + q1z*Z_Ek1 + q2z*Z_Ek2;
//Actualización de las variables de control
Z_Uk1 = control.Z_Uk;
Z_Ek2 = Z_Ek1;
Z_Ek1 = Ekz;
5.3.4. Saturación
Para la saturación de 20º debemos saturar la salida del controlador en el caso que
supere dicho valor.
//Saturacion X
if(control.X_Uk > 0.349065) control.X_Uk = 0.349065;
if(control.X_Uk < -0.349065) control.X_Uk = -0.349065;
//Saturacion Y
if(control.Y_Uk > 0.349065) control.Y_Uk = 0.349065;
if(control.Y_Uk < -0.349065) control.Y_Uk = -0.349065;
5.3.5. Adaptación de lo valores
La adaptación es necesaria para convertir la salida del controlador (un ángulo) a
los valores de entrada al quadrotor con un rango de [0,4095].
// Pitch
if (control.X_Uk >= 0)
{
control.X_Uk = floor(control.X_Uk*1303.1606 + 2048);
}
else
{
control.X_Uk = floor(2048 + control.X_Uk*(1303.1606));
}
// Roll
66
if (control.Y_Uk >= 0)
{
control.Y_Uk = floor(2048 + control.Y_Uk*1303.1606);
}
else
{
control.Y_Uk = floor(2048 + control.Y_Uk*(1303.1606));
}
// Thrust
control.Z_Uk = floor(control.Z_Uk*348.5049);
if(control.Z_Uk < 0)
{
control.Z_Uk = 0;
}
5.3.6. Resultados
En este apartado se muestran los resultados obtenidos en vuelos realizados con
el quadrotor.
5.3.6.1 Controladores X-Y
En primer lugar realizamos pruebas con el controlador PD con el cual vemos
(figura 50) que el movimiento del quadrotor es suave y adecuado para nuestros
experimentos.
Figura 50: Resultado controlador PD eje X
67
A pesar de parecer un control bueno, no es válido porque presenta un error en
régimen permanente en el eje Y, lo que indica que el quadrotor no es totalmente
simétrico. Como comprobamos en la siguiente figura 51, el quadrotor no alcanza la
referencia debido a este error.
Figura 51: Resultado controlador PD eje Y
En la siguiente figura 52 se muestra el error en el eje Y donde podemos ver
como no alcanzar el valor cero sino que se mantiene por encima.
Figura 52: Resultado controlador PD error
Como ya hemos comentado este error nos lleva a usar un controlador PID con
un prefiltro para suaviza su respuesta ante cambios de referencias. En la figura 53
comprobamos el resultado experimental con el controlador PID, donde vemos cual es el
68
comportamiento real. A pesar de tener una pequeña sobreoscilación antes de alcanzar la
referencia los resultados obtenidos experimentalmente son bastantes buenos.
Figura 53: Resultado controlador PID
5.3.6.2 Controlador Z
El control en Z se puede considerar el control que mejores resultados hemos
obtenido (figura 54). Ante un cambio en referencia se desplaza con suavidad hasta
alcanzar la referencia y una vez alcanzada tiene una respuesta rápida debida a la
dinámica del PID.
Figura 54: Resultado controlador PID
69
5.4. Control de yaw
El objetivo de este proyecto es controlar el quadrotor en posición pero nos
vemos obligados a controlar también la guiñada para evitar que el quadrotor se desplace
girando sobre sí mismo.
El controlador de yaw será un simple proporcional, puesto que con este tipo de
control obtenemos buenos resultados. En la figura 55 comprobamos el resultado de
cambiar la referencia de yaw de cero grados a noventa. Se ha introducido también un
prefiltro en la referencia para que se alcance la referencia suavemente.
Figura 55: Resultado controlador yaw
70
