Modelamiento Dinámico y control LQR de un Quadrotor

Modelamiento Dinámico y control LQR de un Quadrotor Modeling and LQR control of a Quadrotor Mauricio Vladimir Peña Giraldo1*, Edilberto Carlos Vivas Gonzales2, Carol Ivonn Rodríguez Feliciano3 1 MSc. Automatización Industrial. Universidad Libre. *[email protected] MSc. Automatización Industrial. Universidad Pedagógica Nacional. [email protected] 3 MSc. Automatización Industrial. Universidad Nacional de Colombia. [email protected] 2 Fecha de recepción del artículo: 10/09/2010: Fecha de aceptación del artículo: 10/12/2010 Resumen Este trabajo deduce el modelo dinámico de un Quadrotor, que consiste en una estructura central donde se encuentran las baterías y la aviónica del dispositivo unida a cuatro largueros con un conjunto propulsor (motor-hélice) en el extremo de cada larguero, formando una cruz perfecta brindando la posibilidad de sustentarse en el aire controlando su orientación y traslación. Múltiples artículos hablan de este modelo dinámico en los cuales se hacen suposiciones para vehículos bajo techo, simplificando considerablemente la complejidad del modelo. Por esta razón se modela físicamente la dinámica del vehículo como un sistema no lineal tomando en cuenta fenómenos aerodinámicos de las hélices. Luego se realiza una linealización del modelo y una comparación entre los modelos “real” y “linealizado” usando un control LQR estabilizante. Palabras clave central body formed by a box with the batteries and on-board computers for control and avionic functions. The body is joined by four beams which have a motor with two rotating wings at the far extreme of each of them. This gives sustentation to the vehicle and gives the possibility of controlling the orientation and translation of the system. Although several theoretical papers can give account of the quad-rotors dynamic model, many of these attempts make several assumptions that are only true for small indoor type vehicles. For this reason, the first stage of this work is to model physically the dynamics of the vehicle as a totally non-linear system, where the complex dynamics of the rotating wings are taken into account. This is followed by a linearization and a comparison between the “real” and linear systems and then design a LQR control for it stabilization. Keywords Dynamic model, linear system, non-linear system, quadrotor, LQR control. Modelo dinámico, sistema lineal, sistema no lineal, quadrotor, control LQR. Introducción Abstract This work shows the study of the dynamic model for a quad-rotor helicopter, which consists of a En el presente artículo se concibe un modelo dinámico para un vehículo aéreo de despegue vertical comúnmente conocido como Quadrotor, AVANCES Investigación en Ingeniería 13 (2010) 71 el cual consta de cuatro propulsores dispuestos al final de los cuatro extremos de una estructura en cruz y ubicados perpendicularmente a cada uno de los largueros. Para el estudio dinámico de esta aeronave es necesario incluir conceptos de mecánica de cuerpos rígidos, dinámica de vuelo, aerodinámica, sistemas dinámicos y control, entre los más representativos. Cantidad de movimiento lineal y angular de un cuerpo rígido La cantidad de movimiento lineal para un cuerpo rígido es un vector que se define como el producto de la masa por la velocidad instantánea del centro de masa con que dicha masa se desplaza, así: → → (1) p = m v c g En el presente artículo se mostrará el procedimiento y análisis que hace posible el modelamiento en general para una aeronave como si fuera un cuerpo rígido, y específicamente para el Quadrotor, de acuerdo a su principio de funcionamiento que es similar al de los propulsores de un helicóptero convencional, y especialmente similar en su aerodinámica. Diversos artículos [1 - 10] parten de un modelo dinámico para un Quadrotor ya existente usado en artículos anteriores donde no se conoce la fuente del modelo dinámico. Se ha hecho entonces la revisión de un trabajo doctoral [11] como el punto más alto de comparación y el cual es el modelo más completo dentro de la bibliografía, pero aun así es deficiente en la explicación de cada uno de los términos. Usando los conceptos y principios del rotor de un helicóptero mostrados en [13, 14], el modelo dinámico aquí analizado y desarrollado se emplea para conocer aspectos como la estructura del modelo, cómo influyen las perturbaciones del entorno sobre el dispositivo, y qué información podría perderse al linealizar el modelo dinámico obtenido para diseñar y simular un control LQR. → Donde p es el vector de cantidad de movimiento lineal, cuya unidad en el sistema internacional de unidades es k g . m / s . De la misma manera, la cantidad de movimiento angular para un sólido rígido se define como el resultado de la matriz de inercias con respecto al centro de giro y que pasa por el centro de gravedad, multiplicada por la velocidad angular, así: → → (2) H = I c g w → Donde H es la cantidad de movimiento angular, que en el sistema internacional tiene unidades de k g .m2 / s o N . m . s . Segunda ley de movimiento de Newton para la traslación y la rotación Al derivar las cantidades de movimiento de traslación y rotación con n respecto al tiempo, y suponiendo una masa constante, se obtiene la segunda ley de movimiento para la traslación y la rotación, así: Metodología (3) De la literatura revisada se hace evidente que el vehículo aéreo a estudiar es representado por un modelo no lineal, inestable y complejo, dada su naturaleza e interacción con el entorno. La investigación teórica de este artículo logra obtener un modelo linealizado en un punto de equilibrio inestable pero estabilizable modificando las entradas del sistema (velocidades de los motores). 72 AVANCES Investigación en Ingeniería 13 (2010) (4) Para i =1, 2, 3, ... n y las cuales se enuncian respectivamente como: “La sumatoria de fuerzas externas que actúan en un cuerpo rígido producen en él una aceleración proporcional a su valor e inversamente a su masa” y “La sumatoria de momentos externos que actúan en un cuerpo rígido producen en él una aceleración angular proporcional a su valor e inversamente proporcional a su momento de inercia”. (9) (10) Dinámica básica de una aeronave para i =1, 2, 3, ... n . La dinámica de un cuerpo rígido queda completamente definida por las siguientes ecuaciones, de acuerdo a las secciones anteriores: A continuación se muestra el sistema de ecuaciones, conocido con el nombre de ecuación de NewtonEuler: (5) y (6) Donde el superíndice indica que estas ecuaciones son referidas a un marco de referencia inercial (I). (11) →   i ∑i = 0 Fi  = m I 3 x 3 0  →  0  i I c d g  ∑i = 0 Fi   •   •→   •→ → Bυ 1  B ω 1 × m Bυ 1    • + • • →   →   →  B ω 1   B ω 1 × I c d g B ω 1  La ecuación 11 es no-lineal y de mucha complejidad. Se llega a simplificar si se asume que el marco de referencia (B) es solidario al marco de referencia (I), tal que los movimientos de traslación de B con respecto a l sean nulos pero con los tres grados de libertad en la rotación, como se muestra en la Figura 2, y además se supone que el desplazamiento que puede ocurrir es dado en el marco de referencia (I). Figura 1. Aeronave y marcos de referencia en I y B. Ahora supóngase dos marcos de referencia como se muestra en la Figura 1: un primer marco fijo a un sistema de coordenadas inercial (I) y un segundo marco (B) fijo al centro de gravedad de la aeronave. Entonces, las ecuaciones de movimiento quedan definidas de la siguiente forma: (7) (8) Figura 2. Marcos de referencia en el quadrotor. → Lo anterior significa que B υ 1 = 0 y por consiguiente se reduce el término: → → B ω1 × Bυ 1 = 0 AVANCES Investigación en Ingeniería 13 (2010) 73 De esta manera, se puede representar la dinámica del sistema con las siguientes ecuaciones: (12) →   ••   i F = ∑  i 0 x i  m x  ••  →    i ∑i = 0 Fy i  = m y    ••   →  i = F  m z  ∑ zi   i 0   •• → •      i •  ∑i = 0 Tx i   I x x φ  0 − ψ θ   I x x 0 0  φ  (13)  i →   ••   •   •  ∑i = 0 T y i  =  I y y θ  + ψ 0 − φ  0 I y y 0  θ    • •    •  •• →  i = T   I ψ  − θ φ 0  0 0 I z z  ψ  ∑ z z z i    i 0      → Donde ω por definición se puede escribir en la forma de matriz antisimétrica (ver [12]) así: • φ  → •  ω = θ  • ψ    •   ψ θ 0 −   •  ψ 0 − φ    • • − θ φ 0    Por la propiedad de una matriz antisimétrica: a × b = S (a) b Donde S (a ) es la matriz antisimétrica de a . Se puede llegar a: ••    i  ∑i = 0 τ x i   I x x φ    •• (14)  i = τ  =  I θ  + ∑ y y y i i 0     •• i  = τ  I ψ  ∑i 0 z i   z z   • • φ ψ ( I z z − I y y )   • • φ ψ ( I x x − I z z )   • • φ θ ( I − I ) yy xx   Donde el segundo término de la derecha de la Ec. 14 es el efecto giroscópico de la aeronave, en este caso un quadrotor, en términos de las ratas de giro de los ángulos de Euler (balanceo, cabeceo y rotación, de ahora en adelante llamados en el documento Roll, Pitch y Yaw por su uso general en la aeronáutica mundial). En las anteriores ecuaciones quedó planteado el sistema de ecuaciones para el quadrotor sin tener 74 AVANCES Investigación en Ingeniería 13 (2010) en cuenta las fuerzas externas que lo afectan. A continuación se presentan los fenómenos más representativos, los cuales se reflejan en fuerzas y momentos que afectan la dinámica, que permite el vuelo en estado estable o inestable y que depende de las condiciones de velocidad de las hélices y del entorno. Dichas fuerzas y momentos que componen el conjunto de fuerzas y momentos externos pueden estudiar en A continuación se explican los términos presentes en la Ecuación 15: • (sen φ sen ψ + co sψ sen θ co s φ ) ∑i4=1Ti (− co sψ 4 , sen ψ + senψ co s φ sen θ ) ∑i =1 Ti y c o s φ c o sψ ∑ 4 T i =1 i Los empujes Ti de los cuatro propulsores siempre son paralelos al eje de Yaw, por ende giran su posición con respecto a los ejes X, Y y Z en los cuales se estudia el movimiento de traslación del quadrotor, por lo que se tiene la transformación en términos de los ángulos de Roll, Pitch y Yaw. Los empujes Ti son las fuerzas perpendiculares al plano de cada una de las hélices que en forma general para un ala rotatoria viene dada por ([13, 14]): T = C T r A (Ω R ) 2 Donde: r es la densidad del aire, A es el área barrida por la hélice, Ω es la velocidad angular de la hélice, R es el radio de la hélice y CT es un coeficiente (factor de corrección debido a la hélice) hallado de manera experimental o teórica mediante: 1 1  CT = σ a  + µ 2  6 4   θτ ϖ 1  2 − λ  θ 0 − (1 − µ ) 4 4   En la cual σ es la relación de solidez (relación entre el área de las hélices y el área del disco que las contiene), a es la pendiente de sustentación (que indica el cambio en el ángulo de ataque a lo largo de la hélice), µ es la relación de avance del rotor (relación entre la velocidad horizontal del centro  ∑i =  i ∑i =   i= ∑i  i ∑i =  ∑i =  i  i ∑i = i (15) • •  1 4 4 ( ) sen sen c o s sen c o s T H C A x x φ ψ ψ θ φ r + − − ∑ ∑ i x i x c   i =1 i =1 2 F 0 xi   • • →   1 4 4 ( ) c o s sen sen c o s sen T H C A y y ψ ψ ψ φ θ r − + − − F  ∑ ∑ i y i y c  i =1 i =1 0 yi 2   → • • 1 4 4 F   0 zi   m g − co s φ co sψ ∑i =1 Ti − ∑i =1 H x i − C z Ac r z z = 2   τ •  4 4 0 xi  l1 (T4 − T2 ) + (−1) i + 1 ∑i =1 Rm x i − l 2 ∑i =1 H y i + J r θ Ω r   τ   • 4 4 0 yi   l1 (T1 − T3 ) + (−1) i + 1 ∑i =1 Rm y i − l 2 ∑i =1 H x i − J r φ Ω r   τ   • 4 0 zi  i l H H H H Q J ( ) ( ) ( 1 ) − + − + − + Ω ∑i =1 i r r x2 x4 y3 y1 1  → ( ( [ ) ) ]                 Donde i =1, 2, 3, 4. de gravedad del quadrotor y la velocidad horizontal al extremo de la hélice debido a la rotación de la hélice), θ 0 es el ángulo de incidencia del perfil en la raíz de la hélice, y θτ ω es la variación del ángulo de incidencia del perfil al final de la hélice. • H x i y H y i : Las fuerzas H i son la resultante de fuerzas distribuidas a lo largo de la hélice en la dirección y sentido opuesto del vuelo. Aparecen como consecuencia del desbalanceo de velocidades a lado y lado de la hélice debido a la traslación del quadrotor que se refleja en una fuerza de arrastre mayor en la semihélice de mayor velocidad como se muestra en la Figura 3. Esta fuerza está dada en cada hélice por: H = C H r A (Ω Rr2a d Donde: → 1 θτ ϖ    1   C H = σ  µ 2 C d + λ µ θ 0 − 4  4 2    Ver [14] y [13] (ver Figura 3). • Cx • • • • • • 1 1 1 Ac r x x , C y Ac r y y y C z Ac r z z : 2 2 2 Estos términos son las fuerzas de fricción que aparecen en todo cuerpo que se traslada en presencia de algún fluido, en este caso el aire se opone al movimiento del quadrotor, donde Pérdidas t(R-V) H X Pérdidas t(R+V) Figura 3. Fuerzas H i . C x , C y y C z son coeficientes experimentales y/o teóricos que depende de la forma del cuerpo, Ac es el área del centro del quadrotor y r es la densidad del aire. • l1 (T4 − T2 ) : Debido a una diferencia en velocidad y/o aceleración entre los propulsores 4 y 2, se genera un momento con respecto al eje de roll de esta magnitud debido a los empujes en las hélices 4 y 2, en la cual L1 es la distancia perpendicular que existe entre el eje de giro de las hélices y el centro de gravedad del quadrotor, como se observa en la Figura 4. AVANCES Investigación en Ingeniería 13 (2010) 75 (a) Momentos l1 (T4 - T2) y l1 (T3 – T1) (b) Debido al desbalanceo de empujes Figura 4. Momentos. • l1 (T3 − T1 ) : Debido a una diferencia en velocidad y/o aceleración entre los propulsores 1 y 3, se genera un momento con respecto al eje de pitch de esta magnitud como se explicó en el anterior ítem (véase Figura 4). • (−1) i + 1 ∑i =1 Rm x i : Este término aparece como una consecuencia del movimiento de traslación en del quadrotor. Obsérvese de la Figura 5 que cada hélice está sometida a una velocidad de entrada ω R pero en dirección contraria a las cuales se suma la velocidad de traslación de su centro de gravedad V , y como es de esperarse una semihélice tiene más sustentación que la opuesta debido a una mayor velocidad de entrada, de esta manera se genera el momento Rm x i (momento con respecto al eje x de la hélice i ). Empuje menor 4 Este momento para un par de semihélices está dado por la siguiente ecuación: Rm = C R m r A (Ω R) 2 R Donde: 1 1  1 CR m = µ σ α  θ 0 − θτ ϖ − λ  8 8  6 i +1 • (−1) ∑i =1 Rm y i : Es un momento similar al anterior, la diferencia es que el movimiento 4 76 Empuje menor R-V AVANCES Investigación en Ingeniería 13 (2010) Rmxy Y X R+V Figura 5. Momento con respecto a x: debido a la traslación en x de un par hélices. está referido al eje y, tanto en el movimiento de traslación como el de rotación. ( ) • l 2 ∑i =1 H y i : Así como en los dos ítems anteriores se generó un empuje extra por la traslación en x debido a velocidades diferentes en las dos hélices, también se produce una fuerza debida a estas velocidades pero por las pérdidas en las hélices y que también son diferentes debido a la traslación en el eje y. Esta fuerza genera un momento alrededor de x al existir una distancia l 2 . 4 ( ) • l 2 ∑i =1 H x i : Es un momento igual al anterior producido por H pero con respecto al eje y cuando el cuerpo se traslada en x. 4 i • (−1) ∑i =1 Qi : Sucede cuando hay desbalance en los movimientos de las hélices que giran a derechas con los que giran a izquierdas. 4 • J r θ Ω r : Este momento se genera debido al giro de las hélices con velocidad angular relativa marco de referencia con Ω respecto a otro • velocidad angular θ y se conoce como el efecto giroscópico del propulsor con respecto al eje x. • Donde: Q = C Q r A (Ω R) 2 R Donde: • • J r φ Ω r : Este momento se genera debido al giro de las hélices con velocidad angular relativa marco de referencia con Ω , respecto a otro • velocidad angular φ y se conoce como el efecto giroscópico del propulsor con respecto al eje y. Pérdidas t(R-V) CH = σ → (1 + µ )C 8 2 d θτ ϖ  + λ θ 0 − 2  [ 1 1 1   θ 0 − θτ ϖ − λ  8 4  6 ] • l1 ( H x 2 − H x 4 ) + ( H y 3 − H y 1 ) : Este momento lo producen las fuerzas de desbalance de fricción H causadas por la traslación en x o y. • • J r Ω r : Efecto giroscópico de la hélice con respecto al eje r (ver Figura 7). H En las ecuaciones 16 a 18 (pág. 78) se puede observar el resultado de reacomodar las ecuaciones. Y l2 X Pérdidas t(R+V) Figura 6. Momento con respecto a x debido a la traslación en y de un par hélices. Control LQR (Linear Quadratic Regulator) El método de control LQR, es la solución óptima a un problema de minimización con lo cual asegura la estabilidad del sistema en lazo cerrado, además su cómputo es fácil. Se puede comenzar por definir el problema más general que da solución a este problema. (a) Velocidad en los motores. (b) Momentos de arrastre. Figura 7. Desplazamientos. AVANCES Investigación en Ingeniería 13 (2010) 77 (16)  i ∑i =  i ∑i =   i= ∑i  i ∑i =  ∑i =  i  i ∑i = • •  1 4 4 ( ) sen sen c o s sen c o s T H C A x φ ψ ψ θ φ r + − − ∑i =1 i ∑i =1 xi x 2 c x   F 0 xi   • • →   1 4 4 ( ) c o s sen sen c o s sen T H C A y y ψ ψ ψ φ θ r − + − − F  ∑ ∑ i y i y c  i =1 i =1 0 yi 2   → • • 1 4 4 F   0 zi   m g − c o s φ c o sψ ∑i =1 Ti − ∑i =1 H x i − C z Ac r z z = 2   τ •   x i 4 4 0 l1 (T4 − T2 ) + (−1) i + 1 ∑i =1 Rm x i − l 2 ∑i =1 H y i + J r θ Ω r   τ   • 4 4 0 yi   l1 (T1 − T3 ) + (−1) i + 1 ∑i =1 Rm y i − l 2 ∑i =1 H x i − J r φ Ω r   τ   • 4 0 zi  l1 ( H x 2 − H x 4 ) + ( H y 3 − H y 1 ) + (−1) i ∑i =1 Qi + J r Ω r  → ( ( [ ) ) ]                 Por lo tanto: (17) • •  1 4 4 ( ) sen sen c o s sen c o s T H C A x x φ ψ ψ θ φ r + − − ∑ ∑ xi x c •• i =1 i i =1    2 m x    • • ••  (− c o sψ sen ψ + senψ c o s φ senθ ) 4 T − 4 H − C 1 A r y y   ∑ ∑ i y i y c my i =1 i =1   2 ••    • •  1 4 4 mz   m g − c o s φ co sψ ∑i =1Ti − ∑i =1 H x i − C z Ac r z z  =  •• • • 2  I x x φ + φ ψ ( I z z − I y y )  • 4 4    l1 (T4 − T2 ) + (−1) i + 1 ∑i =1 Rm x i − l 2 ∑i =1 H y i + J r θ Ω r    I θ•• φ• ψ• ( I x x − Izz )   •  yy + 4 4 l1 (T1 − T3 ) + (−1) i + 1 ∑i =1 Rm y i − l 2 ∑i =1 H x i − J r φ Ω r •• • •     I z z ψ + φ θ ( I y y − I x x )   •  4 l1 ( H x 2 − H x 4 ) + ( H y 3 − H y 1 ) + (−1) i ∑i =1 Qi + J r Ω r  ( ( [ ) ) ] Ahora al reacomodar el sistema en variables de estado, resulta: (18) • •   1 4 4  ••   1    x   m (sen φ sen ψ + co sψ sen θ c o s φ ) ∑i =1 Ti − ∑i =1 H x i − C x 2 Ac r x x     ••     • • y  1   1 4 4   ( ) c o s sen sen c o s sen T H C A y y ψ ψ ψ φ θ r − + − −   ∑i=1 i ∑i=1 y i y 2 c      ••   m   z   • •  1  1 4 4     m g c o s c o s T H C A z z φ ψ r − − − ∑i=1 i ∑i=1 xi z 2 c      m     ••  =  • • • 4  φ   1 θ ψ ( I − I ) + l (T − T ) + (−1) i + 1 4 R − l ∑i=1 m x i 2 ∑i=1 H y i + J r θ Ω r   yy zz 1 4 2    I x x      •   ••   1  • • 4 4  i +1 θ   I ψ θ ( I z z − I x x ) + l1 (T1 − T3 ) + (−1) ∑i =1 Rm y i − l 2 ∑i =1 H x i − J r φ Ω r       yy   •    1 • • 4   i φ θ ( I I ) l ( H H ) ( H H ) ( 1 ) Q J  ••   − + − + − + − + Ω r ∑ xx yy x2 x4 y3 y1 i r 1    i =1 ψ   I z z    [ 78 AVANCES Investigación en Ingeniería 13 (2010) ] ( ) ( )                 Dado el sistema dinámico (20) u (t ) = − K s s x (t ) • x (t ) = A x(t ) + Bu (t ); x(t o ) = x0 Y se puede encontrar desde MATLAB encontrando K s s con la sintaxis: K s s = l q r ( A, B, C T RZ Z C , Ru u ). Con x(t )∈ R n y u (t )∈ R m , z (t ) = C x (t ) U Con z (t )∈ R . Se define una función de costo cuadrática: X p J= Planta K * 1 1 z T (t ) R z z z (t ) + u T (t ) d t + x T (t f ) Pt f x (t f ) ∫ 2 2 [ -K u ] Con Rz z > 0 y R y y > 0 , Pt f ≥ 0 , A (t ) es una función continua del tiempo y B (t ) , C (t ) , Rz z y R y y son funciones continuas en el tiempo y acotadas constituyendo así un sistema LTV (Linear Time Variant). Entonces el problema general del LQR es encontrar una entrada u (t ) óptima en un lapso. Dicha entrada está dada por la expresión: u o p (t ) = − Ru−u1 B T P (t ) x (t ) = − K (t ) x (t ) Donde es la solución a la ecuación diferencial de Ricatti: − P (t ) = A P (t ) + P (t ) A + C T Rz z C − P (t ) B R −1 uu B T P (t ) Pero en la mayoría de los casos la solución que se busca es para sistemas lineales invariantes en el tiempo (LTI), entonces la ecuación diferencial de Ricatti alcanza un valor en estado estable. Es decir P (t ) = 0 . En este caso la solución al problema LQR está dada por: (19) AT P + R x x − PBR −1 B T P = 0 Donde Rx x = C T R z z C . Denominada la ecuación algebraica de Ricatti para el control (CARE por sus siglas en inglés: Control Algebraic Ricatti Equation) encuentra el valor óptimo de P y la entrada óptima: Figura 8. Control estabilizante LQR. Resultados En el control de sistemas es de vital importancia obtener un modelo sencillo que contenga la información más preponderante para facilitar el diseño de un controlador. A continuación se presentará un modelo dinámico simplificado esperando que la dinámica no se afecte de manera ostensible. Según [11] la afectación se puede despreciar sin cambios importantes (ver Ecuación 21, pág. 80). Como se ve en los planteamientos anteriores el modelo no es lineal y complejo para efectos de control, se harán entonces dos suposiciones que se presumen no cambian mucho el modelo pero si lo simplifican de manera considerable. Las fuerzas debidas a la fricción con el aire se pueden plantear como perturbaciones conocidas tanto en el modelo no-lineal como en el lineal. Para el caso específico en que las velocidades y desplazamientos lineales y angulares son pequeños se tiene que los coeficientes: C H x 3 , C H y 3 , C R x , C R y tienen valores muy cercanos a 0. Véase, entonces, en la Ecuación 22 (pág. 80) el modelo simplificado. Si se analiza el sistema anterior se encuentra que existen dos sistemas, el sistema que describe la AVANCES Investigación en Ingeniería 13 (2010) 79 1     (sen φ sen ψ + c o sψ sen θ c o s φ ) C t a U (1) + C H x a U (1) − C x bυ x υ x   υ   m     x    υ x •    x   1    •   (− c o sψ sen ψ + senψ c o s φ sen θ )C t a U (1) + C H y a U (1) − C y bυ y υ y   υ y   m   •    υ y y    •    1   υ z   m g − (c o s φ c o sψ ) C t α U (1) + C H z a U (1) − C z bυ z υ z   m  •     z    υ z  =  •    1  ω ι   I ωθ ωφ ( I y y − I z z ) + C t l a U (3) − C H y h α U (1) + C R x a RU (2) + J r ωθ U (5)  x x •     φ   ωφ  •    ω θ   1  ( ) ( 4 ) ( 1 ) ( 2 ) ( 3 ) ω ω ω I I C l a U C h a U C R a U J U − + + + − φ φ θ z z x x t H x R x r  •  I  yy θ    •   ωθ  ω ψ    • 1       ( ) ( 2 ) ( 3 ) ( 4 ) ( 5 ) ω ω I I C a R U C l a U C l a U J U − + + + + • φ θ x x y y q H x H y r   ψ   I z z      ωψ   • (21) [ ] [ ] Con: U (1) = (Ω1 + Ω 2 + Ω 3 + Ω 4 ) 2 2 2 2 U (2) = (Ω1 + Ω 2 − (Ω 3 + Ω 4 ) ) 2 2 U (3) = (Ω 4 − Ω 2 ) 2 2 U (4) = (Ω1 − Ω 3 ) U (5) = (Ω1 + Ω 3 − (Ω 2 + Ω 4 ) ) 2 a = r AR 2 b= (22) 80 1 Ac r 2  1 υ•   m [(sen φ sen ψ + c o sψ sen θ c o s φ ) C t a U (1)]  x     υx •    x  1   •   [(− c o sψ sen ψ + senψ c o s φ sen θ )C t a U (1)]  υ y   m  •    υy y    1 •    [ ] m g c o s c o s C U φ ψ α ( ) ( 1 ) − t υ z    m •    υz z     = 1  ωθ ωφ ( I y y − I z z ) + C t l a U (3) + J r ωθ U (5)  •   ω  ι  I xx  •    ω φ φ    •   1  ωφ ωφ ( I z z − I x x ) + C t l a U (4) − J r ωθ U (3)  ω θ    •  I yy  θ    ωθ •    • ω ψ   1      ωφ ωθ ( I x x − I y y ) + C q a R U (2) + + J r U (5)  ψ•   I z z       ωψ   [ ] [ ] AVANCES Investigación en Ingeniería 13 (2010) 2 2 2 traslación de la aeronave, y el otro que describe la rotación con respecto a su centro de gravedad (de ahora en adelante actitud de la aeronave). Nótese que las segundas seis ecuaciones son dependientes de los tres referentes a las • •estado • velocidades angulares φ , θ y ψ y de las entradas U (2) , U (3) , U (4) y U (5) , y es independiente de las posiciones y velocidades de traslación de la aeronave y de la entrada U (1) ; pero las seis primeras ecuaciones dependen de φ , θ y ψ y de la entrada U (1) . La cual se puede poner en diagramas de bloque como: w U(2...5) Rotación Doble integrador Traslación U(1) Figura 9. División del sistema en traslación y rotación. Las ecuaciones de variables de estado son de la siguiente forma: (23)  •   1 [(sen φ sen ψ + co sψ sen θ c o s φ ) C a U (1)]  t  υ x   m  •   υ x x      •   1 υ y   [(− c o sψ sen ψ + senψ c o s φ sen θ )C t a U (1)]    •  = m  y   υy  •   1  υ z   [ ] φ ψ α m g ( c o s c o s ) C U ( 1 ) − t  •   m  z   υz       1 ωθ ωφ ( I y y − I z z ) + C t l a U (3) + J r ωθ U (5)  •   ω  ι   I xx  •    ωφ φ     •   1  ω ω ω I I C l a U J U ( ) ( 4 ) ( 3 ) − + − (24) ω θ   I φ φ zz xx t r θ  =  •   yy  θ    ωθ •    • ω ψ   1   ω ω I I C a R U J U ( ) ( 2 ) ( 5 ) − + + + x x y y q r φ θ  •   I    ψ   z z     ωψ   [ [ la aeronave. Después, se realizará la linealización de la actitud en los puntos de equilibrio. Entonces al igualar este sistema a cero se obtienen los puntos de equilibrio, así:   1 ωθ ωφ ( I y y − I z z ) + C t l a U (3) + J r ωθ U (5)     0   I x x      ω φ 0        1  ω ω ω ( ) ( 4 ) ( 3 ) I I C l a U J U − + −   0 zz xx t r θ φ φ (25) =  I  y y     0    ωθ     •    1   ω ω ( ) ( 2 ) ( 5 ) I I C a R U J U − + + + x x y y q r φ θ 0        I zz   0 ωψ   [ ] [ ] Del sistema anterior, se deduce que los puntos de equilibrio para la actitud son independientes de los ángulos de rotación de Euler φ , θ y ψ y se observa, también, que para que esta igualdad se cumpla las velocidades angulares y las entradas deben ser cero, lo que indica que las velocidades de rotación de las hélices deben ser todas iguales. Del sistema de traslación:    1  0   m [(sen φ sen ψ + c o sψ sen θ c o s φ ) C t a U (1)]      υx  0     1  (26) 0  =  m [(− co sψ sen ψ + senψ co s φ sen θ )C t a U (1)]  0    υy     1     0 [ ] m g ( c o s c o s ) C U ( 1 ) φ ψ α − t     m     υ 0 z     ] ] Se puede pensar que para controlar la traslación de la aeronave, se debe controlar primero la actitud de Se observa que υ y y υ z deben ser cero, y para que esto se cumpla se debe asegurar que los ángulos de roll y de pitch sean cero y que la fuerza de sustentación de la aeronave sea de la misma magnitud e igual dirección y en sentido opuesto al peso del Quadrotor. Luego se puede deducir que los múltiples puntos de equilibrio de todo el sistema están dados por: (27) X 0 = [0 0 0 0 0 0 0 0 0 0 0 ψ ]T AVANCES Investigación en Ingeniería 13 (2010) 81 Cualquier punto de equilibrio significaría Hover. Al linealizar el sistema en el punto de equilibrio se obtiene con ayuda de Matlab simbólico: (28) (A)  1 υ•   m C t a U (1) ∗θ  x     υx •    x   1   •  − C t a U (1) ∗φ  υ y   m  •    υy y    •   1  C a U ( 1 ) − υ z   m t  •    υz z     = 1  •    C t l a U (3)  ω  ι  I xx  •    ω φ φ     •   1  C t l a U ( 4) ω θ    I  •   yy  θ    ω θ •    • ω ψ   1     I C q a R U (2) + J r U (5) • ψ   z z      ωψ  AL N 0 0  0  0 = 1  0 0   0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 Diseño de control LQR y simulación En esta sección se reemplazaron los parámetros hallados a partir del quadrotor real y del virtual bajo el software Solidedge en el modelo linealizado • de la forma X (t ) = AL N X (t ) + BL N U (t ) , con Y (t ) = C L N X (t ) + DL N U (t ) y aclarando que de aquí en adelante Q = R x x y Q = R y y por unificación en la notación que Matlab usa, como se puede observar en las matrices (A), (B) y (C). Con las matrices AL N , BL N y las matrices de peso Q (Valorando la importancia de los estados) y R (valorando la amplitud de las señales de control), se puede solucionar el problema LQR LTI computacionalmente con ayuda del Matlab mediante el comando lqr, se obtiene de esta manera la Matriz constante del control: 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0  0  0 0  0 0  0  BL N 0 127 .4725 0 0    0 0 127.4725 0    0 0 0 278.0868    − 7.5188 0 0 0   =  0 0 0 0    0 0 0 0    0 0 0 0    0 0 0 0   (B) AL N 82 0 0 = 0  0 0 0 0 1 0 0 0 0 0 0 0 1 0 0  0 0 0 0 0 1 0  0 0 0 0 0 0 1 (C) DL N 0 0  0  0 = 0  0 0   0 AVANCES Investigación en Ingeniería 13 (2010) 0 0 0 0 0 0  0 0 0  0 0 0 0 0 0  0 0 0 0 0 0  0 0 0   10 0  0  0 Q= 1  0 0   0 0 0 0 0 0 10 0 0 0 0 0 10 0 0 0 0 0 10 0 0 0 0 0 10 0 1 0 0 0 10 0 0 1 0 0 1 0 0 0 0 0 0  0 0  0 0   0 0  0 0   0 0  10 0   0 10  1 0 R=  0  0 0 0 0 1 0 0  0 1 0  0 0 1 0.0000 − 3.2926 0.0000 0.0000 0.0000 − 3.1623   0.0000 − 0.0000  3.1701 − 0.0000 − 0.0000 − 0.0000 3.1623 − 0.0000 0.0000 − 0.0000  Q=  − 0.0000 3.1701 − 0.0000 0.0000 − 0.0000 3.1623 − 0.0000 0.0000    3.1659 − 0.0000 0.0000 − 0.0000 3.1623 − 0.0000   − 0.0000 − 0.0000 Para el sistema linealizado se tiene como premisa que el sistema es óptimo para dichos valores de Q y R , entonces los objetivos de desempeño del sistema se logran al modificar estas matrices de peso y al verificar que las señales de control no saturen los actuadores. El sistema linealizado no tiene restricción alguna ya que se ha obviado la dinámica de los actuadores lo cual no ocurre en el sistema no lineal que aparte de tener en cuenta las no linealidades del sistema modelado, también se ha incluido la dinámica de los motores para identificar que controlador es el apropiado para no saturarlos, esto se logra al observar los voltajes de entrada al circuito de potencia de los motores. Una característica importante del modelo es que las entradas del sistema son funciones del cuadrado de las velocidades de los motores que para diseño del control en simulación no es necesario conocer; pero si se trata de incluir la dinámica de los motores o ejercer una señal de control real dada en voltios, es necesario conocer las velocidades de los motores que satisfacen un sistema de ecuaciones que a continuación se describen:  U1  U   2= U 3    U 4  1 1  1  0 −1 0   1 0 −1  1 −1  −1 1 1 0  1  Ω12  2  Ω2  Ω2  3  Ω 24       Luego se puede escribir: Ω 2 = A −1 U Y obtener el valor de las velocidades angulares de las hélices de los motores al asumir que siempre son positivas por lo que las hélices siempre giran en un mismo sentido (el de diseño).  Ω12   Ω 2  2   2  Ω3   2  Ω 4      =      1 4  1 4 1  4 1  4 − 0 1 2 1 2 0 0 1 2 − 1 2 0 1 4  1 4 1 −  4 1  4 −  U1      U 2      U 3      U   4 Lo siguiente es comparar el funcionamiento en ambos sistemas al observar sus respuestas ante las mismas condiciones iniciales (ver Figura 10). Cuando se hace la linealización de un sistema los puntos de equilibrio se trasladan a cero. El control LQR sin efecto integral genera estabilización, es decir, las variables de estado tienden al punto de equilibrio en el sistema lineal. En el sistema no lineal los puntos de equilibrio de las variables de estado Roll, Pitch y Yaw son cero, y para la altura es diferente de cero, por esto la Figura 10 existe diferencia en el valor de estado estacionario de la altura entre el sistema lineal y el sistema no lineal (Figura 11). Conclusiones En el desarrollo teórico del modelo de este vehículo aéreo, se obtuvo un modelo no lineal e inestable dependiente de los parámetros geométricos y aerodinámicos del dispositivo los cuales dependiendo del parámetro pueden ser una función no lineal multiparamétrica o una constante hallada por medición indirecta o directa según el caso. Por lo tanto, el modelo no lineal puede tener AVANCES Investigación en Ingeniería 13 (2010) 83 (a) Sistema lineal (b) Sistema no lineal Figura 10. Respuesta a la condición inicial. (a) Velocidades angulares. (b) Voltajes. Figura 11. Señales de control para los actuadores. varias imprecisiones; sin embargo, su estructura es útil en un proceso de identificación del modelo del sistema. La linealización del modelo no lineal restringe su validez a puntos cercanos al punto de equilibrio y particularmente lo desacopla, porque diseñar un control capaz de mantener el dispositivo alrededor del punto de equilibrio es relativamente sencillo con la técnica LQR. Particularmente el modelo linealizado permite observar el comportamiento del vehículo en función de las entradas del sistema 84 AVANCES Investigación en Ingeniería 13 (2010) las cuales son las velocidades de los propulsores que resulta bastante lógico. El modelo linealizado no tiene en cuenta los efectos que produce el entorno sobre éste tales como las fricciones con el aire, y tampoco tiene en cuenta los cambios de posición de las hélices debido al cambio en la actitud del vehículo, porque en el movimiento de las hélices se producen fuerzas que afectan la geometría de las mismas y por consiguiente las nuevas fuerzas de sustentación y arrastre cambian el comportamiento del sistema. Debido a lo anterior y al mismo proceso de linealización este modelo puede ser válido sólo en una región llamada en el vocabulario aeronáutico internacional Hover. Este modelo linealizado es útil en el diseño de un control estabilizante; pero no es lo suficientemente bueno para hacer un control con cambio de referencias y en diferentes actitudes como se pretende; luego el modelo lineal será importante para estabilizar el vehículo, esto a su vez permite hacer la identificación del sistema basada en una estructura de modelo dinámico, como la del modelo no lineal obtenido para el diseño de un control total del vehículo. Se concluye de la Figura 10 que aunque el tiempo de estabilización se puede hacer más pequeño en ambos sistemas, resulta conveniente dejarlo en este tiempo; si se observa la Figura 11 se alcanzan los límites de saturación de los actuadores aunque sólo sea en períodos muy cortos de tiempo. Cabe aclarar que los actuadores tienen ganancia de 13 y la señal de control máxima es 9 voltios. Para realizar tareas de navegación con este dispositivo no es suficiente estabilizar el sistema, pues no se quiere sólo suspender en el aire y mantenerlo a cierta altura, se desea brindarle la capacidad de seguir trayectorias al diseñar e implementar un control capaz de seguir ciertas referencias dadas por el usuario, llámese a este último algoritmo de navegación o instrucciones desde un control remoto. Dadas las anteriores conclusiones y los fines experimentales y de investigación del vehículo aéreo se hace imprescindible construir un modelo no lineal basado en su estructura y en datos experimentales de funcionamiento para poder ampliar la gama de controles aplicables al sistema y poder mejorar su comportamiento de acuerdo al tipo de misión. 2. Bouabdallah, S.; Pierpaolo, M. and Roland, S. (2004). Design and control of a micro indoor Quadrotor. IEEE: International conference on robotic and automation, 4393 - 4398. 3. Abdellah, M. and Benallegue, A. (2004). Dynamic feedback controller of Euler angles and wind parameters estimation for a Quadrotor unmanned aerial vehicle. IEEE: International conference on robotic and automation, 2359 - 2366. 4. Bouabdallah, S.; Noth, A, M. and Roland, S. (2004). PID vs LQ control techniques applied to an indoor micro Quadrotor. IEEE\RSJ: International conference on intelligent robots and systems, 2451 - 2456. 5. Tayebi, A. and McGilvray, S. (2004). Attitude estabilization of a four rotor aerial robot. 43rd IEEE: Conference on decision and control, 1216 - 1221. 6. Moktari, A.; Benallegue, A. and Daachi, B. (2005). Robust feedback linearization and GH for a Quadrotor unmanmned aerial vehicle. IEEE/ RSJ: International conference on intelligent robots and systems, 1009 - 1014. 7. Bouabdallah, S. and Roland, S. (2005). Backstepping and sliding mode techniques applied to an indoor micro Quadrotor. IEEE: International conference on robotic and automation, 2247 2252. 8. Tayebi, A. and McGilvray, S. (2006). Attitude estabilization of a VTOL Quadrotor aircraft. IEEE: Transactions on control systems technology, 14, 562 - 571. Referencias bibliográficas 9. Besnard, L., Yuri, B. (2007). Control of a Quadrotor vehicle using slide mode disturbances observer. American control conference, 5230 - 5235. 1. Altug, E. and James, P. (2003). Quadrotor control using dual camera visual feedback. IEEE: International conference on robotic and automation, 4294 - 4299. 10. Mehmet, O. (2007). Robust low altitude behavior control of a Quadrotor rotorcraft throught slides modes. Mediterranean conference on control and automation. 23 - 35. AVANCES Investigación en Ingeniería 13 (2010) 85 86 11. Boouabdallah, S. (2004). Design and control of quadrotors with application to autonomous flying. Ecole Polytechnique Federale de Lausanne. 13. Bramwell, A. R. S. (1985). Bramwell’s helicopter dynamics. Butterworth-Heinemann Ltd. 12. Spong, M. W.; Hutchinson, S. and Vidyasagar, M. (1999). Robot modeling and control. Wiley. 14. Seddon, L. (1990). Basic aerodynamics helicopters. BSP Professional Books. AVANCES Investigación en Ingeniería 13 (2010)