2.Modelo matemático del sistema - bibing.us.esbibing.us.es/proyectos/abreproy/12063/fichero/Sección+2%2FParte+2.… · Alejandro Suárez Fernández – Miranda Proyecto fin de carrera,

Alejandro Suárez Fernández – Miranda Proyecto fin de carrera, 2011 – 2012

2. Modelo matemático del sistema

En esta sección se presenta un modelo matemático del Quadrotor de Motores Orientables que sirva tanto para la simulación como para el diseño del controlador. En primer lugar se indicará la notación utilizada a lo largo de esta sección. Seguidamente se expondrá el modelo geométrico adoptado en este trabajo para luego pasar a obtener el modelo dinámico que relaciona las variables de posición, orientación y velocidad con la fuerza ejercida por cada motor y con su ángulo de orien-tación. Las ecuaciones obtenidas se pasarán finalmente a un diagrama de bloques de Simulink para realizar la simulación del sistema, incluyendo un módulo de control y de Realidad Virtual.

2.1. Notación utilizada

Se seguirá aquí la notación habitual en el modelado de vehículos aéreos tipo quadrotor y utilizada en [1], [2] y [3].

– Notación para el modelo geométrico

E: sistema de coordenadas asociado a la tierra {XE , YE , ZE }: sistema de ejes cartesianos asociados al sistema de coordenadas E B: sistema de coordenadas asociado a la base del quadrotor {XB , YB , ZB }: sistema de ejes cartesianos asociados al sistema de coordenadas B x, y, z [m]: coordenadas cartesianas de un punto expresadas respecto de B X, Y, Z [m]: coordenadas cartesianas de un punto expresadas respecto de E vx , vy , vz [m·s-1]: componentes cartesianas de la velocidad en los ejes XE, YE, ZE. ϕ, θ, ψ [º ó radianes]: ángulos de Tait – Bryan (roll, pitch y yaw, respectivamente) M1: motor situado en el cuadrante XB+, YB+ M2: motor situado en el cuadrante XB –, YB+ M3: motor situado en el cuadrante XB+, YB – M4: motor situado en el cuadrante XB –, YB – l [m]: lado de la base, asumiéndose cuadrada s [m]: separación entre los dos perfiles laterales de la base del quadrotor w [m]: anchura del cuerpo central d [m]: profundidad del cuerpo central h [m]: altura del cuerpo central hm [m]: altura de los motores (supuestos cilíndricos) α [º ó radianes]: ángulo de orientación de los motores

– Notación para el modelo dinámico

IXX, IYY, IZZ [Kg·m2]: momentos de inercia en los ejes XE , YE , ZE

RBE(ϕ,θ,ψ): matriz de rotación entre los sistemas de coordenadas B y E

T1 , T2 , T3 , T4 [Kg]: fuerza de propulsión o empuje ejercido por los motores 1, 2, 3 y 4 Ω1 , Ω2 , Ω3 , Ω4 [rad·s-1]: velocidad de rotación de los motores 1, 2, 3 y 4 b [Kg·(rad/s)-2]: thrust factor, coeficiente que relaciona la fuerza de empuje de un motor con

la velocidad angular de rotación al cuadrado

2.1


d [Kg·(rad/s)-2]: drag factor, coeficiente que relaciona el par de rotación del quadrotor entor-no a su eje ZB debido al giro de cada motor

g: aceleración de la gravedad mk [Kg]: masa de cada motor (k = 1, 2, 3, 4), asumiéndose igual para los cuatro motores m0 [Kg]: masa del cuerpo central

– Notación para los términos aerodinámicos

Cx , Cy , Cz: drag coefficient, coeficiente de resistencia aerodinámica en cada eje ρ [Kg·m-3]: densidad del aire

2.2. Modelo geométrico

En los quadrotors convencionales, la estructura de la base tiene forma de X, de forma que los motores se sitúan sobre los mismos ejes de coordenadas del sistema B. En el caso del Quadrotor de Motores Orientables, la base tiene forma de H y los motores ya no se sitúan sobre los ejes, si no que la disposición es la mostrada en la figura 2.1:

Figura 2.1. Vistas en planta y de perfil del Quadrotor de Motores Orientables.

Se ha representado en rojo la unidad de medida inercial (IMU), asumiéndose centrada en el origen de coordenadas del sistema B, OB , que además coincidirá con el centro de gravedad del quadrotor (CoG). Esta simplificación evita tener que aplicar transformaciones entre ejes, ya que las ecuacio-nes dinámicas de la orientación estarán referidas al centro de masas del quadrotor.

El quadrotor es completamente simétrico respecto del eje XB , manteniendo la simetría respecto de YB si α = 0, lo que hace que la geometría sea igual a la original. Para valores de α > 0 parte de la fuerza de propulsión de los motores M2 y M4 se proyecta sobre el eje XB provocando una aceleración de traslación, además de una pérdida de empuje en la componente ZB de estos dos motores. Como se comentará en la parte del modelo dinámico, la diferencia de fuerza en las componentes verticales entre los motores M2 – M4 y M1 – M3 provocará un par de rotación al rededor del eje YB en el sentido de las agujas del reloj según la vista derecha de la figura 2.1. De la misma forma, la diferencia de

2.2

XB

YB

M1

M2

M4

M3

l2

l2

l2

l2

α

d

w m0

m1

m2m4

m3s

XB

ZB

M1,3

M2,4

α

d

m0h

CoG ≡ OBm1,3

m2,4

hmIMU


fuerza entre los motores M3 – M4 y M1 – M2 genera un par de rotación entorno al eje XB .Para evitar la generación de un par de rotación en torno al eje ZB , dos de los motores deberán girar en un sentido y otros dos en el sentido contrario, de forma que se compense el par. Se usarán hélices de giro normal y de giro inverso para que la dirección de la fuerza de empuje sea positiva, independientemente del sentido de giro de los motores.

Por la construcción del quadrotor, el centro de gravedad del cuerpo central y de los cuatro motores no se encuentran alineados sobre el mismo plano del eje ZB , si no que hay una cierta distancia verti-cal entre ellos, como se puede ver en la parte de la derecha de la figura 2.1. Esta estructura confiere mayor estabilidad al quadrotor, ya que al estar la masa del cuerpo central en un plano inferior al de los motores, el momento de inercia entorno a los ejes XB e YB aumenta, de forma que será necesario aplicar un par mayor para rotar el quadrotor.

2.3. Modelo dinámico

En esta sección se presentan las ecuaciones que describen matemáticamente el movimiento del quadrotor en cuanto a posición, velocidad y orientación a lo largo del tiempo, relacionándose con la fuerza ejercida por cada motor. No se trata de deducir desde cero el modelo dinámico, si no que se partirán de las ecuaciones utilizadas en el modelado de quadrotors convencionales, introduciéndose las modificaciones necesarias para contemplar la orientación de los motores traseros así como el cambio en la geometría. Los modelos utilizados son los indicados en [1], [2] y [3].El modelo dinámico se descompondrá en tres partes: ecuaciones dinámicas para la orientación, ecuaciones dinámicas para la trayectoria, y términos aerodinámicos [1], [5].La deducción de las ecuaciones puede realizarse a partir de la formulación de Newton-Euler, o bién por medio de la formulación de Lagrange-Euler, obteniéndose los mismos resultados

2.3.1. Ecuaciones dinámicas para la orientación

La orientación del quadrotor se define en base a la rotación sucesiva de los tres ángulos ϕ (roll), θ (pitch), y ψ (yaw) mostrados en la figura 2.2. Éstos son los denominados ángulos de Tait-Bryan que representan la rotación de un cuerpo alrededor de los tres ejes del sistema de coordenadas asociado al sistema, esto es, giro entorno a XB para ϕ, giro entorno a YB para θ, y giro entorno a ZB para ψ.

Fig. 2.2. Ángulos roll (izquierda), pitch (centro) y yaw (derecha).

La relación entre los sistemas de coordenadas E, fijo a la tierra, y B, asociado al quadrotor, viene dada por la matriz de rotación de B respecto de E, representada por RB

E, la cuál permite expresar los vectores referidos al sistema B en el sistema E.

2.3

YE

YB

ZB

ZE

ϕ < 0X

E

XB

ZB

ZE

θ < 0

XE

XBψ

YB

YE

Ánguloroll

Ángulopitch

Ánguloyaw


(2.1)

Como se dijo, tanto las ecuaciones dinámicas para la orientación como para la trayectoria se pueden calcular a partir de la formulación de Newton-Euler o de Lagrange-Euler. La diferencia entre las dos está en que la primera se obtiene a partir de las fuerzas y de los pares externos que se ejercen sobre el sistema, mientras que la segunda se deriva de la energía mecánica.

Formulación de Newton-Euler

La relación entre las fuerzas y los pares externos al sistema, representados por F (fuerza de trasla-ción), Fd (perturbación de traslación), τ (vector de pares aplicados) y τd (pares de perturbación), con la velocidad de traslación V y la velocidad angular ω viene dada por:

(2.2)

donde I3x3 es la matriz identidad de orden 3, J es la matriz de inercia y m es la masa total del cuerpo.

Formulación de Lagrange-Euler

Se define el Lagrangiano L como la diferencia entre la energía cinética T y la energía potencial V, debiéndose cumplir la ecuación generalizada para las fuerzas y los pares:

(2.3)

donde Γ es la generalización de las fuerzas y de los pares y q es el vector de coordenadas generali-zadas, que representa tanto la posición cartesiana como angular, es decir:

(2.4)

Antes de continuar desarrollando las ecuaciones dinámicas para los ángulos de orientación, se cal-culará primero la matriz de inercia del quadrotor para tratar de simplificar las expresiones, ya que debido a la simetría, los términos cruzados se anularán. Dicha matriz será simétrica y los coeficien-tes de su diagonal serán positivos para sistemas reales.

Si se consideran masas puntuales de valor mi en las posiciones ri = [xi , yi , zi]T , los coeficientes de la matriz serían:

(2.5)(2.6)

Para distribuciones continuas de masa, el sumatorio se sustituiría por la integral:

(2.7)

(2.8)

2.4

L = T − V ; Γ =ddt (

∂ L∂ q )−

∂ L∂q

I xx = Σi

mi( y i2+ zi

2) ; I yy = Σ

imi(x i

2+ zi

2) ; I zz = Σ

imi(x i

2+ y i

2)

I xy = Σi

mi xi yi ; I xz = Σi

mi xi z i ; I yz = Σi

mi yi z i

I xx =∫D

( y2+z 2

)dm ; I yy =∫D

( x2+ z2

)dm ; I zz =∫D

( x2+ y 2

)dm

I xy =∫D

x y dm ; I xz =∫D

x z dm ; I yz =∫D

y z dm

RBE(φ ,θ , ψ) = [

cos ψcosθ cos ψ sinθsin φ − sin ψcos φ cos ψsin θcosφ + sin ψsin φ

sin ψcosθ sin ψ sinθsin φ + cos ψcos φ sin ψsin θcos φ − sin φcos ψ−sinθ cos θsin φ cosθ cosφ ]

[m I 3×3 00 J ][Vω ] + [ω × mV

ω × J ω ] = [F + F d

τ + τ d]

Γ = [F , τ ]T

; F = [ F x , F y , F z ]T

; τ = [τ X , τY , τZ ]T

; q = [ x , y , z , φ , θ , ψ ]T


donde la integral se evaluará en el volumen, área o filamento D ocupado por la masa, siendo los respectivos diferenciales dm = ρv dV para el volumen, dm = ρAdA para las áreas y dm = ρl dl para filamentos, con ρ la densidad de masa. Hay que tener en cuenta que tanto el sumatorio como la integral se evalúan asumiendo que el origen de coordenadas está en el centro de masas del sistema.

En un sistema con varios componentes mecánicos como el quadrotor, que consta de cuatro motores, un cuerpo central y los brazos que unen a los primeros con la base, el momento de inercia en cual-quiera de los ejes se puede calcular sumando la contribución de cada elemento por separado al momento de inercia en ese eje, de forma que en los casos en los que el cálculo de la integral resulte complicado, se podrán considerar masas puntuales en su lugar, como ocurriría con los motores.

Como se comentó anteriormente, el quadrotor está construido de tal forma que las masas no están alineadas en el mismo plano, si no que hay cierta distancia en el eje ZB entre ellas, la cual se denota-rá por dCoG . El centro de gravedad (CoG) del sistema se situará sobre este eje en a una distancia a respecto de los cuatro motores y a una distancia b respecto del centro de gravedad del cuerpo cen-tral, debiéndose cumplir lo siguiente:

(2.9)

El centro de gravedad del quadrotor estará más próximo de los motores cuanto mayor sea la masa de éstos respecto de la del cuerpo central, y viceversa, la distancia del centro de gravedad respecto de la base será menor cuanto mayor sea la masa del cuerpo central respecto de los cuatro motores. La distancia vertical entre los centros de gravedad de los cuatro motores y del cuerpo central se cal-culará atendiendo al modelo geométrico de la figura 2.1, es decir:

(2.10)

Se va a proceder entonces al cálculo de los momentos de inercia. Los elementos considerados son:

Cuatro motores de masa puntual m1 situados en las posiciones dadas por:

(2.11)

Los momentos de inercia se calcularán aplicando (2.5) y (2.6):

(2.12)

(2.13)

(2.14)

Nótese que debido a la disposición simétrica de las masas de los cuatro motores, los momen-tos de inercia cruzados se anulan, quedando sólo los momentos sobre los ejes principales:

(2.15)

2.5

r k = [± l2

, ±l2

,m0 · dCoG

4 ·m1 + m0]

T

; k = 1, 2, 3 , 4

a =m0

4 ·m1 + m0

· d CoG ; b =4 ·m1

4 · m1 + m0

· d CoG

I XX , k = I YY ,k = m1·[( l2 )

2

+ ( m0· d CoG

4 · m1 + m0)

2

] ; I ZZ , k = m1 ·l 2

2; k = 1, 2, 3, 4

I XY ,1 ,2 ,3 ,4 = Σi = 1

4mi · xi · yi = m1 ·[ l

2·

l2

+l2

·(−1) ·l2

+ (−1)l2

·l2

+ (−1)l2

·(−1)l2 ] = 0

I XZ ,1 ,2 ,3,4 = I YZ ,1,2 ,3 ,4 = Σi = 1

4mi · x i · z i = m1 ·

m0 · d CoG

4 · m1 + m0[ l

2−

l2

+l2

−l2] = 0

I XXmotor

= I YYmotor

= 4 · m1· [( l2)

2

+ ( m0 · d CoG

4 · m1 + m0)

2

] ; I ZZmotor

= 2 · m1 ·l 2

d CoG =hm

2+

h2


El cuerpo central está formado por una caja rectangular hueca de altura h (sobre el eje ZB), largo d (sobre el eje XB) y anchura w (sobre el eje YB). Por tanto, habrá que calcular los momentos de inercia debido a distribuciones superficiales de masa perpendiculares a los tres ejes El cálculo se realizará en primer lugar considerando la caja de forma aislada. Posterior-mente se aplicará el Teorema de Steiner para referir los momentos de inercia al centro de masas del quadrotor.

Fig. 2.3. Disposición de las superficies que componen la caja hueca del cuerpo central.

1. Placa perpendicular al eje ZB a una distancia h/2 del centro y de dimensiones w x d

El cálculo se hará a partir de la integral de volumen, considerando la función delta de Dirarc para modelar la densidad superficial. La constante ρS se expresará en Kg·m-2. Sólo será nece-sario hacer los cálculos para una de las superficies, no para las dos, ya que están dispuestas simétricamente. La simetría hace además que los momentos cruzados se anulen.

(2.16)

(2.17)

(2.18)

2. Placa perpendicular al eje XB una distancia d/2 del centro y de dimensiones h x w

(2.19)

(2.20)

(2.21)

2.6

I xxZ

=∫V

( y2+ z2

)·ρ(x , y , z )dV = ρS ·∫h2

-

h2

+

∫−

w2

w2

∫−

d2

d2

( y2+ z2

)·δ(z − h/2)· dx dy dz = ρS ·[ w3 · d12

+h 2 · w ·d

4 ]

I yyZ

=∫V

(x2+ z2

)·ρ(x , y , z )dV = ρS ·∫h2

-

h2

+

∫−

w2

w2

∫−

d2

d2

(x2+ z2

) ·δ(z − h /2) · dx dydz =ρS ·[ w · d 3

12+

h2· w· d4 ]

I zzZ =∫

V

(x2 + y2)·ρ(x , y , z)dV =ρS ·∫r

h2

-

h2

+

∫−

w2

w2

∫−

d2

d2

(x2 + y2)·δ(z − h /2)· dx dy dz =ρS ·[ w 3· d12

+w ·d 3

12 ]

I xxX

=∫V

( y2+ z2

)·ρ(x , y , z )dV =ρS ·∫−

h2

h2

∫−

w2

w2

∫d2

-

d2

+

(y2+ z2

) ·δ(x − d /2) · dx dy dz = ρS ·[ w3· h12

+w · h3

12 ]

I yyX

=∫V

(x2+ z2

)·ρ(x , y , z )dV = ρS ·∫−

h2

h2

∫−

w2

w2

∫d2

-

d2

+

(x2+ z2

) ·δ(x − d /2) · dx dy dz = ρS ·[ d 2 · h · w4

+w· h3

12 ]

I zzX=∫

V

(x2+ y2

)·ρ(x , y , z)dV = ρS ·∫−

h2

h2

∫−

w2

w2

∫d2

-

d2

+

(x2+ y2

)·δ(x − d /2) · dx dy dz =ρS ·[ d 2 · h· w4

+w3· h

12 ]

ZB

XB

h2

ZB

YB

XB

d2

ZB

YB

XB

w2


3. Placa perpendicular al eje YB a una distancia w/2 del centro y de dimensiones h x w

(2.22)

(2.23)

(2.24)

Sumando el efecto de las tres parejas de placas en cada eje, se tiene el momento de inercia de la caja respecto de su centro geométrico:

(2.25)

(2.26)

(2.27)

Puesto que el centro de masas de la caja está desplazado una cantidad b calculada en (2.9) a lo largo del eje ZB respecto del CoG del quadrotor, habrá que sumar un término constante a las ecuaciones (2.25) y (2.26) en aplicación del Teorema de Steiner, de forma que los nuevos valores son:

(2.28)

Como se comentó antes, las ecuaciones dinámicas de los ángulos de orientación se pueden obtener a partir del Lagrangiano, de forma que el vector de coordenadas será:

(2.29)

La energía cinética de un cuerpo en rotación al rededor de un cierto eje es igual a un medio de su momento de inercia por la velocidad angular al cuadrado:

(2.30)

siendo I el momento de inercia en el eje de giro y ω la velocidad angular. En el caso del quadrotor, la energía cinética es debida a la rotación de los tres ángulos de giro, roll, pitch y yaw, de forma que el cálculo se hace más complicado. A continuación se explica este cálculo.Se denotará por rE(X, Y, Z) a cualquier punto de coordenadas [X, Y, Z]T del espacio expresado en base al sistema de coordenadas fijo a la tierra, E. De forma análoga, se denotará por rB(x, y, z) a cualquier punto de coordenadas [x, y, z]T del espacio expresado en base al sistema de coordenadas asociado al quadrotor, B. Asumiendo que el origen de coordenadas de ambos sistemas de referencia coincide, se puede aplicar la siguiente trasformación de coordenadas para expresar un punto del sis-tema B en el correspondiente punto del sistema E:

2.7

q = [φ , θ , ψ]T

T =12

· I ·ω2

I xxY

=∫V

(y2+ z2

) ·ρ(x , y , z)dV = ρS ·∫−

h2

h2

∫w2

-

w2

+

∫−

d2

d2

( y2+ z2

)·δ( y − w /2)· dx dy dz = ρS ·[w2 · h ·d4

+h3· d12 ]

I yyY

=∫V

(x2+ z2

)·ρ(x , y , z )dV = ρS ·∫−

h2

h2

∫w2

-

w2

+

∫−

d2

d2

( x2+ z2

) ·δ( y − w /2) · dx dy dz = ρS ·[ d 3 · h12

+h3 · d12 ]

I zzY

=∫V

(x2+ y2

)·ρ(x , y , z)dV = ρS ·∫−

h2

h2

∫w2

-

w2

+

∫−

d2

d2

(x2+ y2

)·δ( y − w /2)· dxdy dz = ρS ·[ d 3· h12

+w2 · h· d

4 ]

I xxbase

= 2 · I xxX

+ 2 · I xxY

+ 2 · I xxZ

=16

·ρS · [w3 · (h + d ) + h3 ·(w + d ) + 3 · d ·(w2 · h + h2 ·w)]

I yybase

= 2 · I yyX

+ 2 · I yyY

+ 2 · I yyZ

=16

·ρS ·[ h3·(w + d ) + d 3 · (h + w) + 3 · w· (h2 · d + d 2 · h)]

I zzbase

= 2 · I zzX

+ 2 · I zzY

+ 2 · I zzZ

=16

·ρS · [w3 · (h + d ) + d 3·(h + w) + 3 · h ·(d 2 · w + w2 · d )]

I xxbase

= I xxbase

+ m0 ·[ 4 ·m1

4 ·m1 + m0

· dCoG ]2

; I yybase

= I yybase

+ m0·[ 4 ·m1

4 · m1 + m0

· dCoG]2

; I zzbase

= I zzbase


(2.31)

donde ϕ, θ y ψ son los ángulos de orientación del sistema B respecto del E. Como el sistema B cam-biará su orientación respecto de E por el movimiento del quadrotor, estos tres ángulos dependerán del tiempo:

(2.32)

El interés de esta transformación está en que la posición de las masas del quadrotor que determinan el momento de inercia se expresa en base al sistema de referencia B, pero los ángulos de orientación están referidos al sistema E. La matriz RB

E(ϕ,θ,ψ) es la matriz de rotación que permite expresar los vectores de B en el sistema E.Por otra parte, para obtener la energía cinética se deberá calcular el cuadrado de la velocidad de las masas del quadrotor derivando (2.31) respecto al tiempo, teniendo en cuenta que la matriz de rota-ción depende de esta variable a través de los tres ángulos. El cálculo de la velocidad se demuestra en detalle en [1]. Al final se obtiene una expresión dependiente de las tres coordenadas de posición de la base, (x, y, z), y de los tres ángulos de orientación. Las primeras representan la posición de las masas del quadrotor referidas al sistema de coordenadas de la base, de tal forma que integrando en las regiones que contienen la masa se tiene el momento de inercia. Finalmente, asumiendo que la matriz de inercia es diagonal (los términos cruzados se anulan con la simetría), se tiene que:

(2.33)

Aplicando (2.3) sobre la expresión de la energía cinética, se llega a la siguiente relación entre los momentos de inercia y las variables angulares:

(2.34)

Si se comparan estas expresiones con la ecuación para los pares τ = I · θ( t ) , se observa que el término de la derecha en (2.34) se corresponde con un par de rotación. No obstante este no es el único par que afecta a los ángulos de orientación. Cada uno de los cuatro motores genera un par que se ve compensado en parte por su opuesto, de tal forma que las diferencias de empuje entre los cuatro motores afectan a la dinámica de la rotación del quadrotor. Teniendo en cuenta que el par es igual al producto de la fuerza por la distancia y que el empuje de un motor es proporcional al cuadrado de la velocidad de rotación, con constante de proporcionalidad b, se tienen las siguientes ecuaciones:

(2.35)

(2.36)

(2.37)

En (2.35) se ha tenido en cuenta que las parejas de motores {M1, M2} y {M3, M4} están en posiciones opuestas, como se puede ver en la figura 2.1, mientras que en (2.36) son las parejas {M1, M3} y {M2, M4} las que están enfrentadas, así como que los motores M2 y M4 pueden abatirse en un ángulo α. La constante b es el coeficiente de empuje (thrust factor) que relaciona la

2.8

r Ê= RB

E(φ ,θ ,ψ)· r B

φ = φ(t ) ; θ = θ(t ) ; ψ = ψ( t )

T =12

· I xx ·( φ − ψ· sin θ)2

+12

· I yy ·( θ ·cosφ + ψ· sinφ ·cosθ)2

+12

· I zz · (θ · sin φ − ψ· cosφ ·cosθ)2

I xx φ = θ ψ( I yy − I zz)

I yy θ = φ ψ( I zz − I xx)

I zz ψ = φθ( I xx − I yy)

I xx φ = θ ψ( I yy − I zz) +l2

· b · [Ω32− Ω1

2+ Ω4

2 ·cos(α) − Ω22 · cos(α)]

I yy θ = φ ψ( I zz − I xx) +l2

· b · [Ω22·cos(α) + Ω4

2 ·cos(α) − Ω12− Ω3

2 ]

I zz ψ = φθ( I xx − I yy) +l

√2d [Ω1

2− Ω3

2+ Ω2

2 ·cos(α) − Ω42 · cos(α)]


fuerza de empuje del motor con su velocidad de rotación. Por último, en (2.37) se ha añadido un término que tiene en cuenta que las diferencias en la velocidad de rotación al cuadrado de los cuatro motores provoca la rotación del quadrotor alrededor del eje ZB , siendo d la constante de proporcio-nalidad denominada drag factor.Aún hay que añadir un tercer término a la ecuación (2.37) que represente el par de rotación causado por la componente horizontal de la fuerza de propulsión de los dos motores traseros. Esto es lo que se ilustra en la figura 2.4:

Fig. 2.4. Componentes de la fuerza de propulsión de los dos motores traseros.

Cuando los motores traseros están rotados un ángulo α, parte de su fuerza de propulsión, T2,4 repre-sentada en rojo, se proyecta sobre el plano perpendicular al eje ZB (vector azul). Luego, parte de la componente horizontal es tangente a la circunferencia con centro en OXBYB y que pasa por los dos motores a la vez, siendo la responsable de generar un par de rotación alrededor del eje ZB . El valor de dicho par, asumiendo que el ángulo que forman los motores con los ejes XB e YB es de 45º, viene dado por:

(2.38)

De esta forma, la expresión final es:

(2.39)

2.3.2. Ecuaciones dinámicas para la trayectoria

Las ecuaciones para la trayectoria se pueden obtener igualmente del Lagrangiano, o de forma más inmediata, aplicando la ley de Newton:

(2.40)

En principio, las fuerzas que actúan sobre el quadrotor son la gravedad y la fuerza de propulsión de cada uno de los cuatro motores. Expresándolo en forma vectorial:

(2.41)

2.9

m· d 2 rdt2

= Σi

F i

g E = [00

−g] ; T 1B = [

00T 1

] ; T 2B = [

T 2· sin (α)

0T 2 ·cos(α)] ; T 3

B =[00

T 3] ; T 4

B = [T 4 · sin (α)

0T 4 · cos(α)]

M4

XB

YB

T 2,4

T 2,4 ·sin (α)

T 4 ·sin (α)

√22

T 4 · sin (α )

T 2 ·sin (α)

√22

T 2 · sin (α)

M1 M3

M2

τψ =√22

· l ·(T 2 − T 4) ·sin (α) =√22

· b · l ·(Ω22− Ω4

2) ·sin (α)

I zz ψ = φθ( I xx − I yy) +l

√2d [Ω1

2− Ω3

2+ Ω2

2 · cos(α) − Ω42 ·cos (α)] + √ 2

2· b · l · (Ω2

2− Ω4

2)· sin (α)


Se han utilizado los superíndices E y B para indicar en qué sistema de coordenadas está expresado cada vector. Puesto que las ecuaciones de movimiento están expresadas en E, habrá que aplicar un cambio de base a los vectores de fuerza de propulsión multiplicando por la matriz de rotación dada en (2.1). El vector de fuerza de los motores es entonces:

(2.42)

Ahora que todas las fuerzas están en el sistema de coordenadas asociado a la Tierra, se suman las fuerzas que actúan en cada eje:

(2.43)(2.44)(2.45)(2.46)

En el estado de hover (ϕ = 0 y θ = 0), el primer sumando de las ecuaciones (2.43) y (2.44) se anula-ría, quedando sólo una componente de fuerza proporcional al seno del ángulo de orientación α. La dirección de esta fuerza en el plano XEYE vendrá dada por el ángulo de orientación ψ. Por otra parte, se puede conseguir el movimiento del quadrotor con la inclinación en los ángulos roll y pitch. Supo-niendo que se mantiene la dirección fija en ψ = 0, el movimiento a lo largo del eje XE se conseguiría con una inclinación en el ángulo pitch, mientras que el movimiento en YE se producirá con valores de roll no nulos. El quadrotor podrá moverse en cualquier dirección considerando valores no nulos en los ángulos roll y pitch.

2.3.3. Términos aerodinámicos

Según las ecuaciones (2.43) y (2.44), si se mantiene el quadrotor en hover pero la componente de fuerza debido a la orientación de los motores traseros es no nula (es decir, α ≠ 0), entonces el cuerpo aceleraría indefinidamente hasta alcanzar una velocidad infinita. En la realidad, el movimiento se ve frenado por la fricción del aire, considerado como un fluido viscoso, sobre las superficies del quadrotor.

La fricción aerodinámica se modelará como una fuerza en la dirección del movimiento pero de sen-tido contrario. Según [4], la expresión de esta fuerza, bajo la condición de incidencia normal, viene dada por:

(2.47)

donde ρ es la densidad del aire, CN es el coeficiente de fuerza normalizada (o drag coefficient), S es la superficie de fricción y V es la velocidad del movimiento. No obstante, la consideración de esta fuerza implica estimar el coeficiente CN . En [4] se proporcionan algunas fórmulas para formas sen-cillas, como rectángulos, elipses o triángulos. Para un rectángulo de dimensiones b x c, se tiene que:

(2.48)

En [1] lo que se hace es descomponer la fuerza de fricción aerodinámica en cada eje. De esta forma, a la ecuación (2.40) habría que añadirle, además de la fuerza de propulsión y de la gravedad, un ter-cer término cuyas componentes son:

2.10

T kE= RB

E(φ ,θ ,ψ)· T k

B , k = 1, 2, 3, 4

(m0 + 4 · m1)· x = [cos ψsin θcos φ + sin ψ sinφ] · T norm + (T 2 + T 4) ·sin (α )·[cos ψ cosθ]

(m0 + 4 ·m1) · y = [sin ψ sinθ cosφ − sinφ cos ψ]· T norm + (T 2 + T 4) · sin (α)· [sin ψ cosθ]

(m0 + 4 ·m1) z =−g + [cosθ cosφ] · T norm − (T 2 + T 4)· sin (α) ·[sinθ]

T norm = T 1 + T 3 + (T 2 + T 4) · cos(α )

N =12

·ρ ·C N · S ·V 2

CN = 1.10 + 0.02 ·( bc

+cb ) ,

130

≤bc

≤ 30


(2.49)

2.4. Modelo de simulación con Matlab – Simulink

En la figura 2.5 se ha representado el diagrama de bloques de MATLAB – Simulink correspondiente al Quadrotor de Motores Orientables.Se han coloreado los bloques más importantes del diagrama, de forma que el bloque azul se corres-ponde con las ecuaciones de orientación, el verde con el de posición, el rojo es el controlador y el naranja modela diferentes aspectos de los motores. Las señales que se pretenden controlar son los dos ángulos de orientación roll y pitch. No se plantea el control en posición ni orientación en yaw porque se requerirían sensores adicionales. Un GPS podría proporcionar información sobre la posi-ción del quadrotor, así como su velocidad, aunque con una resolución más apropiada para exteriores (la resolución en posición es de unos 3 metros), mientras que para conocer la orientación sería nece-saria una brújula o magnetómetro. Por otra parte, el control de altura se puede conseguir utilizando sensores ultrasonidos o infrarrojos en la base del quadrotor, permitiendo en el caso del ultrasonido un alcance de unos 5 metros, con una resolución de pocos centímetros.

Fig. 2.5. Diagrama de bloques de Simulink del Quadrotor de Motores Orientables.

Por estos motivos, las señales de referencia de entrada al controlador serán los ángulos roll y pitch, así como la altura del quadrotor respecto del suelo. Otra referencia, aunque no es utilizada por el controlador, es el ángulo de orientación de los motores traseros.Se ha modelado el ruido debido a las vibraciones de los motores y que afectan a las medidas de orientación por medio de un bloque del tipo “Random Number”, especificándose unos valores máximos y mínimos obtenidos de las medidas experimentales, aunque podría realizarse un estudio frecuencial o estadístico para modelar en detalle el ruido de vibraciones. En cualquier caso, se ha supuesto un efecto aditivo sobre la medida real del ángulo.

2.11

F a =−12

·ρ ·[C x · x ·∣x∣· S x

C y · y ·∣y∣· S y

C z · z ·∣ z∣· S z]


Se han representado gráficamente con los bloques “Scope” las señales de orientación (ángulos roll, pitch y yaw), posición (ejes X, Y, Z del sistema de referencia asociado a la tierra) y velocidad de ro-tación de los cuatro motores. Se ha añadido además un bloque “VR Sink” para poder representar el movimiento que realizaría un modelo de Realidad Virtual del quadrotor, como el mostrado en la fi-gura 2.6:

Fig. 2.6. Vista del “VR Sink” del modelo de Realidad Virtual del quadrotor.

En la siguiente sección se describirá cómo se creó el modelo de VR del quadrotor. A continuación se pasa a describir cada uno de los bloques del modelo de Simulink.

Bloque de orientación El bloque de orientación es un subsistema (bloque “Subsystem”) cuyo contenido se muestra en la figura 2.7. El bloque implementa las ecuaciones (2.35) a (2.37). A su entrada tiene un vector “Ome-ga” con la velocidad angular de los cuatro motores y el ángulo “alfa” de orientación de los motores traseros. A su salida están los tres ángulos de orientación “roll”, “pitch” y “yaw”. El bloque contiene las constantes del modelo: los tres momentos de inercia, la longitud del quadrotor y los coeficientes drag y thrust. En rojo, verde y azul se representan los tres subsistemas que calculan la aceleración angular en los tres ejes, de forma que integrando dos veces (bloque “Integrator) se tienen los tres ángulos. En estos bloques se especificarán la posición y velocidad iniciales en la simulación.

Bloque de posición La figura 2.8 representa el diagrama de bloques del módulo de posición, el cual representa las ecuaciones (2.43) a (2.46). El diagrama es bastante similar al subsistema de orientación. Se tienen a la entrada los tres ángulos de orientación “roll”, “pitch” y “yaw”, el ángulo de orientación de los motores traseros “alfa”, y la velocidad de rotación de los motores. Las salidas son las variables de posición “x”, “y” y “z” referidas al sistema de coordenadas asociado a la Tierra. Los parámetros necesarios son el factor de empuje de los motores “trhust factor” y un vector de cinco componentes con las masas de los cuatro motores y del cuerpo central. Los subsistemas rojo, verde y azul calcu-lan la aceleración en los tres ejes a partir de las fuerzas de los motores, e integrando doblemente se obtiene la posición. A la aceleración obtenida por estos bloques se resta el término de fricción aerodinámica debida al rozamiento del cuerpo con el aire y que es proporcional al cuadrado de la velocidad.

2.12


Fig. 2.7. Subsistema que representa las ecuaciones dinámicas de los tres ángulos de orientación.

Fig. 2.8. Subsistema que representa las ecuaciones dinámicas de la posición en los tres ejes XE , YE , ZE .

2.13


Bloque de motores

En la figura 2.9 se representa el diagrama de bloques que modela los efectos de retraso, diferen-cia en las ganancias, dinámica de primer orden y saturación que afectan a los motores Brushless.

Fig. 2.9. Subsistema que modela los efectos de retraso, dinámica de primer orden y saturación en los motores.

Se ha asumido un retraso de 20 ms en la aplicación de la señal de control, puesto que la señal PWM que controla los variadores es de 50 Hz. Se ha supuesto una dinámica de primer orden con constante de tiempo de 0.1 segundos. Este valor es puramente estimativo, no se ha podido medir experimen-talmente, aunque es un valor razonable. Además, se ha tenido en cuenta las diferencias en los cuatro motores asignándoles a cada uno una ganancia distinta, aunque en torno a la unidad. Por último, se ha contemplado la saturación en la velocidad de rotación. Los motores utilizados se caracterizan por un valor de 1450 KV, es decir, 1450 revoluciones por voltio. Teniendo en cuenta que se alimentarán con 11 Voltios, y pasando de r.p.m a radianes por segundo, se puede obtener el límite superior de la saturación.

Bloque de controlador Dentro de este subsistema se implementarán los diferentes tipos de controladores descritos en la Sección 3 de este trabajo y cuyos resultados se comentarán en la Sección 6, contrastándolos con los datos obtenidos del sistema real.

2.5. Modelo de Realidad Virtual con VRML

El modelo de Realidad Virtual del quadrotor se construyó utilizando el software V-Realm Builder 2.0 incluido en la versión R2008b de Matlab. La aplicación permite crear mundos virtuales gráfica-mente sin necesidad de escribir código VRML (Virtual Reality Modeling Language). Estos mundos se componen de un conjunto de objetos organizados mediante una estructura en árbol en la que un objeto padre puede estar compuesto por varios objetos hijos, de forma que todas las transformacio-nes geométricas (traslaciones, escalados, rotaciones, etc) que se apliquen al nodo padre, afectará de la misma forma a todos los nodos hijos.

2.14


Para dar forma a los objetos se dispone de ocho tipos de elementos: cubos, conos, cilindros, esferas, texto, mayas de elevación, extrusiones y conjunto de puntos indexados. Aquí se han utilizado sólo los tres primeros, de forma que los motores se representaron mediante cuatro cilindros, los perfiles con un cubo (se pueden especificar las dimensiones de altura, anchura y profundidad), el cuerpo central es otro cubo, y para los ejes de coordenadas se usaron cilindros con un cono en la punta.

Cada uno de los objetos tiene un conjunto de propiedades que se puede editar. Algunas son de apa-riencia, como color, forma o textura, mientras que otras son las propiedades geométricas, como el centro de rotación, los ángulos de rotación, escala, traslación, etc. En la figura 2.10 se representa el conjunto de propiedades de un cubo. Algunas de las más importantes son:

– children → geometry → Box → size: especifica las dimensiones de la caja– translation: indica el desplazamiento del objeto respecto de su centro– center: centro de giro y de traslación del objeto– rotation: rotación del objeto en los tres ejes

Para crear un mundo virtual, primero se arrastrará un bloque “VR Sink” del Virtual Reality Toolbox al diagrama de bloques de Simulink. Luego, haciendo doble click sobre él, aparecerá un cuadro de configuración de los parámetros. Dentro del área marcada como “Source file” aparecerá un botón “Browse” para seleccionar el archivo .wrl si ya se creó el mundo virtual, así como el botón “Edit” para crear uno nuevo. Por último, se especificará el período de muestreo (Sample time) y las propie-dades que se deseen visualizar como entradas al bloque. Todo esto se representa en la figura 2.11.

Fig. 2.10. Propiedades de un cubo que se pueden editar con V-Realm Builder 2.0.

2.15


Fig. 2.11. Propiedades seleccionadas como entradas al bloque VR Sink.

2.6. Estimación numérica de los parámetros

En este último apartado se van a calcular los valores numéricos de los parámetros del modelo.

Masa del cuerpo central

El cuerpo central está formado por una caja de aluminio hueca sobre la que se montará la placa electrónica y el servo. No se ha considerado la presencia de una batería porque la alimentación se realiza a través de una fuente de alimentación. El peso estimado es m0 = 250 gramos. La caja tiene unas dimensiones de 20.5 x 12.25 x 6 cm, de forma que la densidad de masa superficial es igual a ρS = 0.27 [Kg·m-2].

Masa de los motores y variadores

Según las especificaciones de los motores y variadores (véase la Sección 4), el peso de cada motor es de 29 gramos, y el peso del variador de 18 gramos. Por la construcción del quadrotor, se asumirá que las dos masas están lo bastante próximas entre sí como para considerar un único cuerpo cuyo peso sea la suma de los dos más una corrección del 25% para considerar la contribución de las héli-ces, tornillos y otros elementos utilizados en la fijación a los perfiles. Es decir:

2.16

m1 = (0.029 + 0.018)· (1 + 0.25) = 0.05875 [ Kg ]


Momentos de inercia

Para el cálculo de los momentos de inercia se hará uso de las ecuaciones (2.15) y (2.28). El quad-rotor tiene una longitud de l = 0.4 [m], y la distancia vertical entre los centros de gravedad de los cuatro motores respecto del centro de gravedad del cuerpo central es dCoG = 0.04 [m]. Entones:

Los momentos de inercia del cuerpo central son Ixxbase = 1.5614·10-4, Iyy

base = 2.2175·10-4, Izzbase =

1.5929·10-4. Como se ve, su valor es dos órdenes de magnitud inferior a los momentos de inercia de los motores, aunque la masa de éstos es bastante inferior, debido a que la masa de la caja está menos distribuida.

Thrust factor, b

Esta constante se calculará dividiendo el empuje máximo de un motor entre su velocidad máxima. Los motores Brushless utilizados se caracterizan por un empuje máximo de 300 gramos y una velo-cidad de 1450 KV (rmp/V), alimentándose a 11,1 Voltios (véanse especificaciones en la Sección 4):

Drag factor, d

Como el valor de esta constante depende de las hélices utilizadas, sería necesario realizar la iden-tificación de forma experimental midiendo el par ejercido por el motor sobre el eje ZB , lo que reque-riría de un montaje complejo. Es por esto por lo que no se ha podido obtener su valor. No obstante, sí se puede decir que su orden de magnitud debe ser similar al del thrust factor.

2.17

Ωmáx =1450[rpm /V ]

60[ s/min ]·11.1[V ] = 268.25 [ Hz ] = 1685.46 [rad /s ]

b =T max

Ωmax2

=0.3[ Kg ]

(1685.46 [rad /s ])2

= 1.056 · 10−7 [ Kg

(rad /s)2 ]

I XXmotor

= I YYmotor

= 4 · 0.05875 [Kg ]·[(0.4 [m ]

2 )2

+ ( 0.25[ Kg ] ·0.04[m ]

4 ·0.05875[ Kg ] + 0.25[ Kg ] )] = 0.01 [Kg · m2]

I ZZmotor = 2 ·0.05875[ Kg ] · 0.42 = 0.0188 [Kg · m2]

Documents

2.Modelo matemático del sistema - bibing.us.esbibing.us.es/proyectos/abreproy/12063/fichero/Sección+2%2FParte+2.… · Alejandro Suárez Fernández – Miranda Proyecto fin de carrera,