Estrategia de control optimo no lineal para el ánalisis de trayectoria de un hexacóptero

(1)

Estrategia de Control Optimo no Lineal para el An´

alisis de

Trayector´ıa de un Hexac´

optero

Andr´

es Camilo Morales Palacios 200326001

[email protected]

Facultad de Ingenier´ıa

Departamento de Ingenier´ıa El´ectrica y Electr´onica

Bogot´a D.C.

(2)

(3)

Estrategia de Control ´

Optimo no Lineal para el An´

alisis de

Trayector´ıa de un Hexac´

optero

Andr´

es Camilo Morales Palacios 200326001

[email protected]

Trabajo de grado para optar al t´ıtulo de:

Magister en Ingenier´ıa El´

ectrica y Electr´

onica

Director:

Alain Gauthier Sellier. PhD.

L´ınea de Investigaci´

on:

Grupo de Investigaci´

on:

Universidad de los Andes

Facultad de Ingenier´ıa

Departamento de Ingenier´ıa Eléctrica y Electrónica Bogotá D.C.

(4)

(5)

La preocupación por el hombre y su destino siempre debe ser el interés primordial de todo esfuerzo técnico. Nunca olvides esto entre tus diagramas y ecuaciones.

(6)

(7)

Agradecimientos

Le agradezco a mi familia por tenerme toda la pasciencia del caso en este largo periodo de la Maestr´ıa.

Al profesor Fernando Jimenez quien me recib´ıo inicialmente en su grupo de trabajo dandome la primer orientaci´on en este proyecto de grado.

Al Profesor Alain Goutierr quien me dio los lineamientos finales y el impulso para llevar esta tesis a fel´ız t´ermino.

Al profesor Daniel P´aez por su apoyo incondicional durante todo el proceso y su soporte enconomico en los desarrollos de proyectos que ayudaron a obtener recursos para las asistencias de investigaci´on de la tesis.

Al profesor Yamil por darme un espacio en su grupo de trabajo durante todo un semestre.

Al profesor Luis Francisco Combita estudiante de Doctorado en control de la Univer-sidad de los Andes por todos los apoyos y explicaciones incondicionales.

A la Profesora Phd Diana Ovalle de la universidad Distrital quien en su caracter de guia me llevo de la mano para entender buena parte del proceso del desarrollo de la tesis y fue un apoyo incondicional durante todo el proceso.

(8)

(9)

Resumen

En la(s) útima(s) decada(s) los veh´ıculos autónomos y en especial los veh´ıculos aéreos no tripulados han sugido como resultado del incremento en la velocidad de proceso de los microprocesadores y micro-controladores y, de igualmanera, el avance en la mi-niaturización de los dispositivos electrónicos. En particular, los veh´ıculos Aéreos No tripulados están disponibles a precios más económicos y han sido usados en múltiples aplicaciones. Por lo anterior, diferentes clases de controladores han sido diseñados e implementados satisfactoriamente para hacer que el veh´ıculo se mueva como se desea. En el mismo sentido, el modelo del hexacóptero ha sido desarrollado desde la perspec-tiva newtoniana y lagrangiana e incluso usando cuaterniones.

Sin embargo el modelo No Lineal no es muy utilizado, el común de los trabajos rea-lizados sobre los hexacópteros linealizan los modelos para utilizar algunas técnicas de control lineal y as´ı lograr el objetivo del control del mismo.

Sin embargo, se podr´ıa sospechar un alto acoplamiento entre los movimientos longi-tudinales y laterales con los verticales del mismo, debido a la ausencia de actuadores en los ejes x y y. Por lo tanto decidimos explorar el modelo No lineal del hexac´ opte-ro, explorar elmanifold de equilibrio y resolver su problema de maniobrabilidad como un control ´optimo, a fin concluir acerca de las trayectorias reales que un hexac´optero podr´ıa seguir.

(10)

(11)

Contenido

Resumen IX

Lista de Figuras XIII

Lista de Tablas XV

Lista de S´ımbolos XV

Introducci´on 1

1. Antecedentes y Objetivos 3

1.1. Antecedentes . . . 3

1.1.1. Evoluci´on Hist´orica . . . 3

1.2. Clasificaci´on . . . 4

1.3. Objetivos . . . 5

1.3.1. Objetivo General . . . 5

1.3.2. Objetivos espec´ıficos . . . 5

2. Análisis Dinámico del Hexacóptero 7 2.1. Modelamiento del Hexacóptero . . . 7

2.2. Análisis Cinemático del Hexacóptero . . . 8

2.3. Ecuaciones de Movimiento . . . 11

3. Manifold de Equilibrio del Hexac´optero 15 3.1. Introducci´on . . . 15

3.2. Parametrizaci´on del Manifold de Equilibrio . . . 15

3.3. Resultados Num´ericos del Manifold de Equilibrio del Hexac´optero . . . 18

4. Problema de Maniobrabilidad 21 4.1. Introducci´on . . . 21

4.2. Problema de Maniobrabilidad . . . 21

4.3. Una Aproximaci´on de control ´Optimo . . . 22

(12)

xii CONTENIDO

4.4.1. C´alculo del Gradiente de la Funci´on de Costo . . . 24

4.4.2. Abordando las restricciones . . . 24

4.4.3. Simulaci´on Num´erica . . . 25

4.4.4. Resultados Num´ericos . . . 25

5. Conclusiones y observaciones 30 6. Anexos 31 6.1. C´odigos del Matlab . . . 31

6.1.1. C´odigo General . . . 31

6.1.2. Din´amica del hexac´optero . . . 33

6.1.3. Din´amica del coestado . . . 35

(13)

Lista de Figuras

1-1. Clasificaci´on por Tipo de Despegue [3]. . . 5

2-1. Sistema Coordinado fijo del Hexac´optero . . . 9

3-1. Movimiento constante a lo largo del eje z. Izquierda: vista en el eje z. Derecha: vista tridimensional. . . 18

3-2. Movimiento circular en el plano xy, para variaciones de w y p. L´ınea continua p = 1, l´ınea discontinua p= −1. Rojo w = −1, negro w = 0, azul w= 1. . . 19

3-3. Movimiento circular en el plano xy, para variaciones de w y q. L´ınea continua q = 1, l´ınea discontinua q = −1. Rojo w = −1, negro w = 0, azul w= 1. . . 19

4-1. Comportamiento de las variables de estado m´as relevantes en el movi-meinto. (a) desplazamiento verticalz(t), (b) velocidad de descensow(t), (c) controles uuu(t), y (d) funci´on de coste. . . 26

4-2. Comportamiento de las variables de estado m´as relevantes en el movi-meinto. (a) desplazamiento lateraly(t), (b) desplazamiento verticalz(t), (c) velocidad lateral v(t), (d) velocidad de descenso w(t), (e) ´angulo roll

φ(t), y (f) controlesuuu(t). . . 28

4-3. Comportamiento de las variables de estado m´as relevantes en el movi-meinto. (a) desplazamiento longitudinalx(t), (b) desplazamiento vertical

z(t), (c) velocidad longitudinal u(t), (d) velocidad de descenso w(t), (e) ´

(14)

(15)

Lista de Tablas

2-1. Tabla de Componentes de Movimiento. . . 8

(16)

(17)

LISTA DE S´IMBOLOS xvii

Lista de s´ımbolos

A Matrix que relaciona las velocidades angulares de las h´elices con las fuerzas y los movimientos que producen sobre el hexac´optero.

Eh Manifold de equilibrio del hexac´optero.

Vector de funciones no lineales asociadas a la cinemática y dinámica del he-xacóptero.

k Vector de funciones no lineales asociadas a la cinemática del hexacóptero. d Vector de funciones no lineales asociadas a la dinámica del hexacóptero.

fZ Fuerza generada por las h´elices en el ejez del eje a bordo del hexac´optero.

h Función de coste del problema de control óptimo asociado a la maniobrabilidad del hexacóptero.

I Momento de inercia del hexac´optero

Ix Momento de inercia del hexac´optero alrededor del ejex.

Iy Momento de inercia del hexac´optero alrededor del ejey.

Iz Momento de inercia del hexac´optero alrededor del ejez.

K Conjunto acotado que se refiere a las limitantes f´ısicas de las variables de control u.

l Porci´on incremental del coste.

L Longitud de cada brazo del hexac´optero.

m Masa del hexac´optero.

O Origen del plano fijo en el hexac´optero.

P1 Matriz de ponderaci´on para el estado final deseado.

p vector de coestado del hexac´optero.

p1i i−´esimo elemento de P1. ˙

p Vector de primeras derivadas de las variables de coestado del hexac´optero.

p Velocidad angular de rotaci´on alrededor del eje x, medida desde el eje a bordo del hex´acopeto.

Q Matriz de ponderaci´on para el estado deseado.

q Velocidad angular de rotaci´on alrededor del ejey, medida desde el eje a bordo del hex´acopeto.

R Matriz de ponderaci´on para las variables de control.

r Velocidad angular de rotaci´on alrededor del ejez, medida desde el eje a bordo del hex´acopeto.

(18)

xviii LISTA DE S´IMBOLOS

τττ Duración máxima de ejecución de una maniobra.

ue Vector de variables de control en un punto de equilibrio.

u Velocidad de avance longitudinal, medida desde el eje a bordo del hex´acopeto.

v Velocidad de avance lateral, medida desde el eje a bordo del hex´acopeto.

w Velocidad de descenso, medida desde el eje a bordo del hex´acopeto.

x Vector de variables de estado del hexac´optero. ˙

x Vector de primeras derivadas de las variables de estado del hexac´optero.

x0 Vector de condiciones iniciales de las variables de estado.

xd(t) Vector de comportamiento deseado de las variables de estado.

xe Vector de variables de estado en un punto de equilibrio.

xT Vector de estado final deseado.

x Eje longitudinal, dirigido hacia la h´elice #1.

y Eje transversal, dirigido al punto medio entre las h´elices #2 y #3.

z Eje normal, dirigido hacia abajo.

Tolerancia para el criterio de parada del algoritmo de gradiente.

λ Paso fijo del algoritmo de gradiente.

φ Angulo de rotaci´on alrededor del ejex.

θ Angulo de rotaci´on alrededor del ejey.

ψ Angulo de rotación alrededor del ejez. ∆ Gradiente de la función que lo acompaña.

∆u Gradiente de la funci´on que lo acompa˜na, respecto a las variables de control.

ηηη Vector de posiciones lineales y angulares del hexac´optero, medidas en el marco de referencia inercial.

ηηη1 Vector de posiciones lineales del hexac´optero, medidas en el marco de referencia inercial.

ηηη2 Vector de posiciones angulares del hexac´optero, medidas en el marco de refe-rencia inercial.

ννν Vector de velocidades lineales y angulares del hexac´optero, medidas en el marco de referencia abordo del mismo.

ννν1 Vector de velocidades lineales del hexac´optero, medidas en el marco de refe-rencia abordo del mismo.

ννν2 Vector de velocidades angulares del hexac´optero, medidas en el marco de re-ferencia abordo del mismo.

∇ Gradiente de la funci´on que lo acompa˜na.

∇x Gradiente de la funci´on que lo acompa˜na, respecto a las variables de estado.

∇u Gradiente de la funci´on que lo acompa˜na, respecto a las variables de control.

ΩΩΩ Vector de las velocidades angulares de las h´elices del hexac´optero.

Ω Conjunto acotado que se refiere a las limitantes f´ısicas de las variables de estado

(19)

LISTA DE S´IMBOLOS xix

Ωi Velocidad angular de rotaci´on de la h´elice #i.

T Vector de fuerzas y momentos que act´uan sobre el hexac´optero.

τ1 Vector de fuerzas que act´uan sobre el hexac´optero.

τ2 Vector de momentos que act´uan sobre el hexac´optero.

τK momento angular alrededor del eje x.

τM momento angular alrededor del eje y.

(20)

(21)

Introducci´

on

El presente proyecto de Maestr´ıa en Ingenier´ıa Electrónica con enfasis en control, con-siste en el desarrollo de una simulación y control no lineal del vuelo de un veh´ıculo aéreo no tripulado (UAV)1. En particular, se trata de un veh´ıculo de seis rotores o hexacóptero, habitualmente designado por su nombre en inglés hexarotor.

El trabajo desarrollado consta de las siguientes etapas:

Estudio y modelizaci´on aerodin´amica.

Establecimiento de un modelo din´amico que se asemeje a la realidad.

La simulaci´on en tiempo real del modelo establecido.

An´alisis de los puntos de equilibrio.

Planteamiento y solución numérica del control no lineal del hexacóptero.

(22)

(23)

Cap´ıtulo 1

Antecedentes y Objetivos

1.1. Antecedentes

Los UAV1 son aeronaves no tripuladas utilizadas en el campo militar y civil para aplica-ciones destinadas a la vigilancia o la seguridad, principalmente. Para ello, se integran con cámaras y diferentes tipos de sensores que permiten observar desde las alturas, sustituyendo as´ı al ojo humano y reduciendo costos de la utilización de aeronaves tri-puladas. De esta manera, entre las ventajas que se les atribuyen respecto a las aeronaves tripuladas es que tienen una mayor capacidad de maniobra para acceder a sitios donde los aviones tripulados, por su tamaño no pueden acceder. Además, tienen un menor costo.

1.1.1. Evoluci´

on Hist´

orica

Como ocurre en muchas disciplinas del desarrollo tecnológico, no existe consenso a la hora de fijar la aparición de los UAV. Por lo tanto, uno de los tantos recuentos hist´ ori-cos del origen de estas máquinas se remonta a mediados del siglo XIX, cuando el 22 de agosto de 1849 un primitivo UAV formado por un globo cargado de bombas se utilizó en el ataque austriaco a la ciudad de Venecia. Durante la guerra de secesión americana (1861-1865) también fueron utilizados por ambos bandos, al igual que en Venecia, en forma de globos cargados. Sin embargo, no tuvieron mucho éxito ya que la fiabilidad y exactitud de movimientos de los UAV era pequeña.

Fue a partir de la Primera y la Segunda Guerra Mundial cuando comenzó decidida-mente el desarrollo de los UAV para aplicaciones militares. As´ı, los tiradores británicos antiaéreos fueron entrenados con aviones radiocontrolados. En esta época, Estados Uni-dos creó un prototipo de UAV llamado Operación Afrodita, eran aviones B-17 cargados de explosivos y con control remoto que se estrellaban contra objetivos estratégicos. Pos-teriormente, durante la Guerra Fr´ıa, Estados Unidos desarrolló un UAV para espionaje

(24)

4 1 Antecedentes y Objetivos

y reconocimiento. Al primero de ellos se le design´o como Firebee y fue utilizado sobre la Rep´ublica Comunista de China.

En la Guerra de Vietnam, los UAV contaban ya con reconocimiento de cámaras de d´ıa. Más tarde, se mejoraron con cámaras de fotos nocturnas, as´ı como con avances en comunicación y electrónica. En los conflictos del Golfo Pérsico y Bosnia, los UAV consolidaron su empleo como aplicación militar. Prueba de ello fue el Predator, capaz de volar 700Km con una autonom´ıa de vuelo de 24 horas.

En los últimos años, ha habido un avance muy importante en cuanto a las aplicaciones civiles de los veh´ıculos aéreos no tripulados. Cada vez son más los proyectos de inves-tigación y las empresas que muestran interés en esta disciplina. Posterior a todas estas evoluciones, actualmente las importantes universidades del mundo han volcado sus ojos en el estudio de nuevos controles y funciones especificas para mejorar los rendimientos y aplicaciones de los sistemas no tripulados a pequeña escala, especialmente los de ala rotatoria.

1.2. Clasificaci´

on

A través de la historia, y en los diferentes paises, existen muchas maneras de clasificar los UAV’s. Pero una de las formas más básicas de clasificar los veh´ıculos aéreos no tripulados es de acuerdo a su peso. Las distintas categor´ıas establecidas por el gobierno de Estados Unidos América son las siguientes:

1. Micro: Menos de un 1 Kg.

2. Mini: 1 – 10 Kg.

3. Peque˜no: 10 – 50 Kg.

4. Mediano: 50 – 100 Kg.

5. Grande: M´as de 100 Kg.

Algunos otros ejemplos de clasificación, puede ser según el sistema de propulsión. Las distintas categor´ıas son:

1. Motores de h´elice: Gasolina, diesel y otros derivados.

2. Turbina de gas comprimido.

3. Motores el´ectricos: Bater´ıas precargadas o energ´ıa solar.

(25)

1.3 Objetivos 5

Habitualmente, es posible diferenciar dos grupos en función del despegue de UAVs, pu-diendo ser éste vertical como el de los helicópteros, o no vertical como el del parapente. La Figura1-1 muestra una clasificación en función de este criterio.

Figura 1-1: Clasificaci´on por Tipo de Despegue [3].

1.3. Objetivos

1.3.1. Objetivo General

El desarrollo de este control óptimo no lineal para un hexacóptero no incluye su imple-mentación sobre un prototipo de hexacóptero, ni el diseño aerodinámico, ni mecánico del mismo. Lo que se busca con el proyecto es la simulación del control para verifi-car el comportamiento y el rendimiento del sistema. La simulación debe considerar el modelo dinámico del hexacóptero lo más cercano a la realidad, basado en los con-ceptos aerodinámicos pertinentes y las caracter´ısitcas f´ısicas reales de un hexacóptero determinado.

1.3.2. Objetivos espec´ıficos

Establecer las variables caracter´ısticas que definen la modelización de un he-xacóptero. Estas variables incluyen el dimensionamiento del veh´ıculo, su peso, la configuración de la geometr´ıa, los motores y sus caracter´ısticas.

Establecer un modelo dinámico para el vuelo del hexacóptero que represente de forma adecuada el comportamiento real del hexacóptero.

(26)

6 1 Antecedentes y Objetivos

Realizar un estudio de los puntos de equilibrio del hexacoptero, para tener un punto de partida del conocimiento del comportamiento din´amico del mismo.

Plantear el problema de control ´optimo no lineal, que permita definir maniobras a realizar por el hexac´optero y que genere los controles para lograrlo.

(27)

Cap´ıtulo 2

An´

alisis Din´

amico del Hexac´

optero

Lo primero que se debe tener en cuenta al momento de la modelización aerodinámica es que el hexacóptero será considerado como un sólido r´ıgido. Se pretende estudiar cuáles son los parámetros que hacen óptimo el funcionamiento del desplazamiento del drone, al igual que se presenta un modelo del hexacóptero configurado a través de una serie de campos o parametros que debe introducirse tomados de la literatura y de la captura de datos de campo de un hexacoptero real modelo F555 de la marca DJI a través del software de vuelo autonomo llamado mission planer.

Las ecuaciones del modelo dinámico deben representar fielmente el vuelo del Hexac´ opte-ro. La optimización de esta parte del código de programación resulta esencial. La si-mulación en matlab exige que el tiempo de cálculo y representación del modelo no sean elevados. La mayor parte de los trabajos y estudios desarrollados relativos al quadrotor ponen especial énfasis en la aplicación de diversos sistemas de control as´ı como en la implementación de componentes electrónicos y sensores.

2.1. Modelamiento del Hexac´

optero

Es fácil notar que el hexacóptero puede describir libremente movimientos rotacionales y traslacionales a lo largo de las tres dimensiones, es por esta razón que decimos que un hexacóptero tiene seis grados de libertad. Por conveniencia, las variables que tomaremos en consideración para el análisis dinámico del hexacóptero serán las posiciones lineales y angulares del mismo y sus derivadas, como se muestra en la Tabla2-1. Teniendo en cuenta el número de variables, es mucho más conveniente utilizar una notación vectorial como se representa en la Tabla2-2. Dondeηηη es el vector de posición y orientacion del Hexacóptero,νννes el vector de velocidades lineales y angulares yτττ es el vector de fuerzas y momentos actuando sobre el hexacóptero.

(28)

8 2 Análisis Dinámico del Hexacóptero

Tabla 2-1: Tabla de Componentes de Movimiento. Hexac´optero

Componentes del Movimiento

Fuerzas y Momentos

Velocidades Lineales y Angulares

Posiciones Lineales y Angulares

Movimiento en el eje x - u x

Movimiento en el eje y - v y

Movimiento en el eje z fZ w z

Rotaciones al rededor dex τK p φ

Rotaciones al rededor dey τM q θ

Rotaciones al rededor dez τN r ψ

Tabla 2-2: Tabla de Matrices de Movimiento.

η

ηη1 = [x y z]T ηηη2 = [φ θ ψ]T ηηη= [ηηη1 ηηη2]T

ν

νν1 = [u v w]T ννν2 = [p q r]T ννν= [ννν1 ννν2]T

τττ1 = [X Y Z]T τττ2 = [K M N]T τττ = [τττ1 τττ2]T

2.2. An´

alisis Cinem´

atico del Hexac´

optero

Para poder entender el análisis cinemático del hexacóptero, iniciamos definiendo un sistema coordinado inercial Xe , Ye, Ze en la convención NED (North, East, Down)

también denominado marco de referencia espacial. Los ángulos de rotación alrededor de los ejes de este marco de referncia se definen como:

Roll: Rotaci´on sobre el eje Xe, positiva a la derecha.

Pitch: Rotaci´on sobre el eje Ye, positiva hacia abajo.

Yaw: Rotaci´on sobre el eje Ze , positiva desde el Norte al Este.

Tambi´en debemos definir un sistema coordenado fijo centrado en el centro de gravedad del hexac´opteroX0 , Y0, Z0, como el mostrado en la Figura2-1.

Con el fin de hacer un análisis cinemático adecuado para el hexacóptero, se hacen las siguientes suposiciones:

La dinámica de los motores es mucho más rápida que la del cuerpo r´ıgido y por ende se desprecia.

(29)

2.2 Análisis Cinemático del Hexacóptero 9

Figura 2-1: Sistema Coordinado fijo del Hexac´optero

las fuerzas y momentos de propulsi´on dentro del plano del motor son peque˜nos y son perpendiculares al plano del rotor.

A partir de las definiciones vectoriales en la tabla2-2y la definición de los dos sistemas coordenados, sabemos que los ángulosηηη2 = [φ θ ψ]T representa la orientación del marco de referencia fijo al hexacóptero respecto al marco de referencia inercial (espacial). Por lo tanto, podemos relacionar la velocidad lineal espacial ˙ηηη₁ = [ ˙x y˙ z˙]T y el vector de velocidades angulares del hexacópteroννν1 = [u v w]T a través de una matriz de rotación, as´ı

˙

η η

η₁ =R(ηηη2)ννν1, (2-1) dondeR(ηηη2) representa la rotaci´on alrededor de los tres ejes, definida como

R(ηηη2) = R0z(ψ)R

0

y(θ)R

0_x₍_φ_),

siendo R0x(φ) la traspuesta de la matriz de rotaci´on respecto al eje x en el ´angulo φ, que puede ser escrita como

Rx(φ) =





1 0 0

0 cφ sφ

0 −sφ cφ



,

con cφ = cos(φ) y sφ= sin(φ). De manera similar,

Ry(θ) =





cθ 0 sθ

0 1 0

−sθ 0 cθ



 y Rz(ψ) =





cψ −sψ 0

sψ cψ 0

0 0 1



.

Por lo que R(ηηη2) puede ser escrito como

R(ηηη2) = 



cθcψ −cθsψ +sφsθcψ sφsψ+cφsθcψ

cθsψ cφcψ +sφsθsψ −sφcψ +cφsθsψ

−sθ sφcθ cφcθ



(30)

El orden de las rotaciones se hace siempre considerando que primero ocurre la rotaci´on respecto a x, luego a y y, finalmente, con respecto az.

En este momento es importante precisar que para el modelo del hexac´optero conside-ramos

−π

2 ≤φ≤

π

2,

−π

2 ≤θ≤

π

2 y 0≤ψ ≤2π.

En este punto, buscamos una transformaci´on entre las velocidades angulares en el marco de referencia abordo del hexac´optero y las variaciones de los angulos de Euler definidas en el marco de referencia inercial (espacial). Para lo anterior, consideramos

ννν2 = [p, q, r]T como las velocidades angulares respecto al marco de referencia del he-xac´optero, tal que:

˙

R(ηηη2) =R(ηηη2)ˆννν2 y νννˆ2 =R0(ηηη2) ˙R(ηηη2), donde

ν νν2 =





0 −r q r 0 −p

−q p 0 



es una representaci´on matricial (anti-sim´etrica) del mapeo lineal

ν

νν1 →ννν2×ννν1 = ˆννν2ννν1.

Un c´alculo directo muestra que la transformaci´on J(.) de ( ˙ηηη˙˙2) aννν2 satisface

ννν2 =J(ηηη2) ˙(ηηη2) = 



1 0 −sθ

0 cφ sφcθ

0 −sφ cφcθ

    ˙ φ ˙ θ ˙ ψ  .

J(ηηη2) es invertible siempre que cos(θ)6= 0, entonces en la regi´on de inter´es, ˙

η

ηη2 =J−1(ηηη2)ννν2 (2-2) con

J−1(ηηη2) = 



1 sφtθ cφtθ

0 cφ −sφ

0 −sφ/cθ cφ/cθ



.

El conjunto de tranformaciones (2-1) y (2-2) constituyen lasecuaciones cinem´aticas del hexac´optero, haciendo uso de estas, podemos definir el mapeo deννν a ˙(ηηη) como:

T(ηηη) =

R(ηηη2) 03x3 03x3 J−1(ηηη2)

,

para expresar las ecuaciones cinem´aticas como

˙

η η

(31)

2.3 Ecuaciones de Movimiento 11

2.3. Ecuaciones de Movimiento

El movimiento de un cuerpo r´ıgido puede ser descompuesto en sus componentes rota-cionales y traslarota-cionales. Por lo tanto, con el fin de describir la dinámica de hexacóptero asumido como un cuerpo r´ıgido, utilizaremos las ecuaciones de Newton-Euler, que go-biernan el movimiento linear y angular. Primero que todo, la fuerza que actúa sobre el hexacóptero está dada por

τττ1 =maaa =m( ˙ννν1+ννν2×ννν1), (2-4) dondem es la masa del hexacópteroy es asumida como constante y ,aaa la aceleración del hexacóptero.

Para determinar las fuerzas que generan las hélices sobre el hexacóptero, es importante considerar que cada rotor itiene una velocidad angular Ωi, y están ubicados paralelos

al ejeZ0. Por lo que la rotaci´on de los rotores genera una fuerza de empuje,F, paralela al eje Z0, con componentes iguales a cero en los otros ejes, es decir

Fi =

0 0 FZ

T

,=

0 0 kΩi2

T

,

siendok la constante de elevaci´on.

El empuje total junto con la fuerza gravitacional representa la fuerza total que actúa sobre el hexacóptero. Dado que la fuerza de empuje está expresada en el marco de referencia del hexacóptero, al igual que la fuerza total actuando sobre éste y la gravedad actúa en el marco de referencia inercial, podemos escribir la fuerza total actuando sobre el hexacóptero como:

τττ1 =F −m g R0(ηηη2)~eee3, con~eee₃ =0 0 Fi

0

. Reemplazando la expresi´on deτττ1 en (2-4) y despejando para ˙ννν1, tendremos

˙

ννν1 = 1

mF −g R

0

(ηηη2)~eee3−ννν2×ννν1. (2-5) Con respecto al movimiento rotacional del hexacóptero, iniciaremos llamamos III a la matriz de inercia. El hexacóptero tiene una estructura simétrica respecto a todos los ejes del marco de referencia centrado en el centro de masa del hexacóptero, por lo tanto la matriz de inercia se puede escribir como

I= 



Ix −Ixy −Ixz

−Ixy Iy Iyz

−Ixz −Iyz Iz



= 



Ix 0 0

0 Iy 0

0 0 Iz





Lo anterior se deriba de las simetr´ıas con respecto a los ejes X0, Y0 y Z0, aunque la simetr´ıa con respecto a este ´ultimo no es completa, asumir que si, no cambiar´a mucho

(32)

el comportamiento del modelo. Podemos escribir el momento de inercia total que actua sobre el hexac´optero como:

τττ2 =III( ˙ννν2+ννν2 ×ννν2)

De la estructura geométrica del hexacóptero, es posible obtener información respecto a los momentos que causan roll, pitch y yaw, as´ı

τττ2 =   τK τM τN  =      3

4k l(Ω2 2_{+ Ω}

32−Ω52−Ω62)

k l(−Ω12− Ω22

4 + Ω32

4 + Ω4

2₊Ω52 4 −

Ω62 4 )

b(−Ω12 + Ω22−Ω32+ Ω42−Ω52+ Ω62)      ,

donde l es la distancia entre el eje del rotor y el centro de gravedad del hexac´optero y

b es la constante de arrastre de los motores. Por lo que para el movimiento rotacional tendremos

Iννν˙2+ννν2×(Iννν2) =τττ2, entonces,

˙

ν ν

ν2 =I−1(τττ2−ννν2×(Iννν2)). (2-6) Por lo que, el modelo del hexac´optero se puede escribir de forma expl´ıcita como:

˙

x=ucosψcosθ−v(cosφsinψ−sinφcosψsinθ) +w(sinφsinψ+ cosψcosφsinθ),

˙

y=ucosθsinψ+v(cosφcosψ+ sinφsinψsinθ)−w(cosψsinφ−cosφsinψsinθ),

˙

z =−usinθ+vcosθsinφ+wcosφcosθ,

˙

φ=p+qsinφtanθ+rcosφtanθ

˙

θ =qcosφ−rsinφ,

˙

ψ =rcosφ

cosθ +q

sinφ

cosθ,

˙

u=rv−qw−gsinθ,

˙

v =pw−ru+gcosθsinφ,

˙

w=qu−pv− fZ

m +gcosφcosθ,

˙

p=Iy −Iz

Ix

qr+τK

Ix

,

˙

q=Iz−Ix

Iy

pr− τM

Iy

,

˙

r=Ix−Iy

Iz

pq− τN

Iz

.

(33)

2.3 Ecuaciones de Movimiento 13

Los parámetros del hexacóptero están definidos como:

m= 0,468,

g = 9,81,

l = 0,225,

k = 0,00000298,

b= 0,000000114,

Ix =Iy = 0,004856,

Iz = 0,008801.

Tambi´en podr´ıamos escribir el modelo de una forma m´as compacta, como:

˙

x

xx=fff(xxx, uuu),

dondexxx=ηηη0 ννν00ser´a el vector de variables de estado del hexac´optero yuuu=Ω1 Ω2 Ω3 Ω4 Ω5 Ω6

ser´a el vector de variables de control, que se puede obtener como funci´on de τττ. Note queτττ =0 0 fZ τK τM τN

y que             1

kfZ

2

√

3k lτK

2

k lτM

1

b τN

           

| {z }

¯

τττ

=     

1 1 1 1 1 1

0 1 1 0 −1 −1

−2 −1 1 2 1 −1

−1 1 −1 1 −1 1 

   

| {z }

A AA          Ω2 1 Ω2 2 Ω2 3 Ω2₄ Ω2₅ Ω2 6         

| {z } ΩΩΩ2

.

Por lo que,

ΩΩ =Ω AAA000(((A AA AA A000)))−−−111τττ ,¯ o, lo que es equivalente

        

Ω2₁ Ω2₂ Ω2 3 Ω2 4 Ω2 5 Ω2 6          =                     1

6 0 −

1 6 − 1 6 1 6 1 4 − 1 12 1 6 1 6 1 4 1 12 − 1 6 1 6 0 1 6 1 6 1 6 − 1 4 1 12 − 1 6 1 6 − 1 4 − 1 12 1 6                                 1

kfZ

2

√

3k l τK

2

k lτM

1

bτN

            .

(34)

Lo que nos limitar´ıa el problema a buscar los valores de las 4 componentes deτττ para encontrar las 6 velocidades ΩΩΩ de los rotores.

(35)

Cap´ıtulo 3

Manifold de Equilibrio del Hexac´

optero

3.1. Introducci´

on

El entendimiento de la dinámica del sistema comienza con un conjunto de condiciones de funcionamiento constantes, es decir, puntos de equilibrio y trayectorias de trima-do. Los puntos de equilibrio representan de forma trivial los movimiento primitivos más simples posibles. Matemáticamente, son expresados como el conjunto de valores constantes de (x,u) tal que 0 = ˙x= f(x,u). Para el hexacóptero, y cualquier tipo de veh´ıculo, en general, ya que las variables de estado incluyen generalmente la posición del veh´ıculo y la orientación, los puntos de equilibrio significar´ıan que el veh´ıculo se detiene en una posición particular, que no dice mucho acerca de su dinámica.

Por lo tanto, un acercamiento relativo y m´as interesante se considera para los veh´ıcu-los:las trayectorias de trimado. Estas pueden ser vistas como aquellas trayectorias a lo largo de las cuales las velocidades en el eje del veh´ıculo y los controles son constantes. En condiciones adecuadas en la din´amica del sistema, el conjunto de trayectorias de trimado proporcionan una hipersuperficie definida impl´ıcitamente o, mejor, una varie-dad diferenciable a la que nos referimos como el manifold de equilibrio, ya que es un conjunto de puntos de equilibrio relativos o trayectorias de trimado.

En este cap´ıtulo, nos planteamos una caracterización de las trayectorias de trimado del hexacóptero, que es en s´ı mismo de alguna novedad. Se muestran las trayectorias de trimado del hexacóptero, los cálculos y algunos resultados de la simulación.

3.2. Parametrizaci´

on del Manifold de Equilibrio

Consideremos un sistema No Lineal,

˙

(36)

16 3 Manifold de Equilibrio del Hexac´optero

como establecimos en el cap´ıtulo anterior, para el hexac´optero tenemos xT _{= [η, ν].}

Podemos escribir el modelo del hexac´optero en (2-7) como:

˙ η ˙ ν =

fk(η)

fd(η, ν,u)

, (3-1)

donde ˙η =fk(η) representa la porción cinética equivalente al modelo del hexacóptero

para (2-3), y ˙ν =fd(η, ν,u) representa la porción cinética equivalente a una expresión

compacta para un conjunto de ecuaciones de movimiento (2-5) y (2-6). Si se inspeccio-nan de cerca las ecuaciones de movimiento, se puede ver que sólo la porción de η que está involucrado en fd es [ϕ, θ]. Por lo que se puede escribir la porción dinámica del

modelo del hexac´optero como: ˙ν =fd(ϕ, θ,ν,u), vamos a definir un espacio de estados

reducido xr = (ϕ, θ,ν). Por lo tanto, se pueden definir las trayectorias de trimado del

hexac´optero como un conjunto trayectorias constantes (xr e,ue) tal que:

0=fd(xr e,ue), (3-2)

donde el sub´ındicee corresponde a los valores de constantes de equilibrio. De la misma manera, la familia de trayectorias de trimado puede ser definida como el conjunto:

Eh ={(η,ν,u)∈R12×R4 :0=fd(xr,u),η˙ =fk(η)}.

Llamamos Eh al manifold de equibrio del hexac´optero.

Debido a que (3-2) tiene más incógnitas que ecuaciones, es necesario definir algunas de las incógnitas para encontrar una única solución para una trajectoria de trimado en particular. El proceso de configuración de las variables es denominado parametrización de las trayectorias de trimado, o más generalmente, la parametrización del manifold de equilibrio. Se ha demostrado queEh puede ser parametrizado por n variables, donde n

es el n´umero de variables de control del sistema, ver [14].

En el caso del hexac´optero, el conjunto de ecuaciones que definen las trayectorias de trimado son f´aciles de abordar anal´ıticamente, de la siguiente manera:

0 =rv−qω+gsin(θ)

0 =pω−ru+gcos(θ) sin(φ) 0 =qu−pv+ fZ

m −gcos(φ) cos(θ)

0 =(Iy −Iz)qr+τK

Ix

0 =− τM + (Ix−Iz)pr

Iy

0 =− τN + (Iy−Ix)pq

Iz

.

(37)

3.2 Parametrizaci´on del Manifold de Equilibrio 17

Sin embargo, si tomamos (3-3) y no garantizamos que la dinámica deφ y θ no esté en equilibrio, las trayectorias de trimado no van a satisfacer su definición. Por lo tanto es importante también considerar:

0 =p+qsin(φ) tan(θ) +rcos(φ) tan(θ)

0 =qcos(φ)−rsin(φ). (3-4)

Para la parametrización del manifold de equilibrio del hexacóptero se deben considerar las 8 ecuaciones definidas en (3-3) y (3-4), que incluyen 12 incógnitas, y determinar cuáles 4 variables tiene sentido fijar, para encontrar las otras 8. Incialmente, conside-rando (3-4), se puede evidenciar que se tiene una ecuación de φ en función de q y r y otra deθ en función de p, q y r.

Al considerar (3-3), se puede evienciar que las tres ´ultimas ecuaciones tienen a τK, τM

yτN nuevamente como funci´on de p, q y r. Teniendo en cuenta que podr´ıamos, o bien

definir p, q y r o definir τK, τM y τN, y dado que estos ´ultimos tres son parte de los

controles del hexacóptero, cuya influencia no es fácilmente evidenciable en la dinámica del veh´ıculo, se considera mucho mejor definirp, q y r. Sólo nos hace falta definir una variable por fijar.

Ahora, considerando las dos primeras (3-3) vemos que ambas incluyen a w y cono-ciéndola, podr´ıamos conocer av (de la primera ecuación), au(de la segunda ecuación) y afZ (de la tercera ecuación). Por lo que fijar w se ve como una opción razonable.

As´ı las cosas, el problema de las trayectorias de trimado se reduce a fijar w, p, q y r, y encontrarφ,θ, u,v,fZ, τK, τM y τN utiizando las expresiones a continuaci´on:

φ= tan−1q

r

θ = tan−1

₋

p

qsin(φ) +rsin(φ)

u=pw+gcos(θ) sin(φ)

r

v =qw−gsin(θ)

r

fZ =m(pv−qu+gcos(θ) cos(φ))

τK =(Iz−Iy)qr

τM =(Iz−Ix)pr

τN =(Ix−Iy)pq= 0.

(3-5)

Con lo anterior, ya es posible hacer un barrido por las variables fijas y evidenciar qu´e puede hacer el hexac´optero.

(38)

3.3. Resultados Num´

ericos del Manifold de

Equili-brio del Hexac´

optero

En esta sección, algunos resultados de simulación obtenidos son presentados resolvien-do las expresiones de (3-5). Una de las caracter´ısticas más interesantes del hexacóptero es que puede mantener las condiciones iniciales indefinidamente siempre queφyθ sean iguales a cero. Lo anterior sucede para cualquier valor dewyr, siempre quep=q = 0, y genera movimientos con velocidad constante en z y; Si r = 0, se pueden fijar u y v

en cualquier valor para lograr movimientos con velocidad constante en x y y.

De esta forma, se puede pensar en que el hexac´optero describa ascenso/descenso y avance/retroceso a velocidad constante tanto a lo largo del ejesx, como del eje y.

El movimiento en equilibrio más conocido de un sistema de ala rotatoria, helicóptero ó multirotor es el conocido como estacionario o hoover, que se define como la maniobra en la que el hexacóptero es capaz de mantener su posición en (x, y, z) por un tiempo indefinido, se presenta cuandofZ/m se equipara con la aceleración de la gravedad. En

la Figura 3-1, se muestra, en negro, el comportamiento en hoover del hexac´optero, en azul, un ascenso para w=−1, y, en rojo, un descenso para w= 1, con p=r =q= 0 y u = v = 0. (Es importante recordar que el eje z positivo est´a en la parte de abajo del plano.)

Figura 3-1: Movimiento constante a lo largo del eje z. Izquierda: vista en el eje z. Derecha: vista tridimensional.

Dada la simetr´ıa del hexacóptero, este es capaz de describir trayectorias circulares al combinar valores de p y r o de q y r. Esto se puede evidenciar en la Figura 3-2, all´ı todas las combinaciones de las variables de estado describen una trayectoria circular en el plano xy, de diámetro 1.75 m. Para todas las gráficas, r = 4 rad/s y q = 0. Las

(39)

3.3 Resultados Num´ericos del Manifold de Equilibrio del Hexac´optero 19

l´ıneas continuas se generan con p = 0,1 rad/s y las discontinuas con p = −0,1 rad/s, lo cual genera giros a la derecha o a la izquierda, respectivamente.

Las gráficas de color negro se generan con w = 0 m/s, las azules con w = 1 m/s y las rojas con w = −1 m/s. La relación entre p y r generan el valor del ángulo de inclinación, en este caso θ =±1,4321o_.

Figura 3-2: Movimiento circular en el plano xy, para variaciones de w y p. L´ınea continua p= 1, l´ınea discontinuap=−1. Rojo w=−1, negro w= 0, azul w= 1.

Figura 3-3: Movimiento circular en el plano xy, para variaciones de w y q. L´ınea continua q= 1, l´ınea discontinua q=−1. Rojo w=−1, negro w= 0, azul w= 1.

Las gr´aficas de la Figura 3-2 se pueden obtener, no variando p, sino q. Esto se puede evidenciar en la Figura 3-3, all´ı todas las combinaciones de las variables de estado describen una trayectoria circular en el plano xy, de di´ametro 1.75 m. Para todas las

(40)

gr´aficas, r = 4 rad/s y p = 0. Las l´ıneas cont´ınuas se generan con q = −0,1 rad/s y las discont´ınuas con p = 0,1 rad/s, lo cual genera giros a la derecha o a la izquierda, respectivamente. Las gr´aficas de color negro se generan con w= 0 m/s, las azules con

w = 1 m/s y las rojas con w = −1 m/s. La relaci´on entre q y r generan el valor del ´

angulo de inclinaci´on, en este caso φ =±1,4321o_.

De la comparaci´on entre las Figuras3-2 y3-3resulta evidente que las simetr´ıas en los planosx y yen el cuerpo del hexac´optero son absolutamente equivalentes, de hecho se puede evidenciar que el comportamiento para valores positivos de p es equivalente al comportamiento para valores negativos deq y viceversa.

De las pruebas previas, se evidencia que el valor del diámetro del c´ırculo depende di-rectamente de la relación |p/r|, o |q/r|, según sea el caso. En el caso en que tanto p

comoq sean distintas de cero, el di´ametro del c´ırculo ser´a directamente proporcional a p

p2₊_q2_/_|_r_|_.

Con el conocimiento que proporcionan los puntos de equilibrio del hexac´optero, se pueden tener mejores percepciones del comportamiento del veh´ıculo para definir un problema de maniobrabilidad que le permita definir diferentes trayectorias, tratando de terminar siempre en un punto de equilibrio.

(41)

Cap´ıtulo 4

Problema de Maniobrabilidad

4.1. Introducci´

on

En este cap´ıtulo, primero definimos el problema de maniobrabilidad del hexacóptero. Luego en orden de aproximarnos al problema de maniobrabilidad del hexacóptero, for-mularemos un problema de control óptimo no lineal con restricciones. Para la solución numérica del problema de control óptimo usaremos el método del descenso del gradien-te. Al final del cap´ıtulo mostraremos algunos resultados de simulación para diferentes maniobras.

4.2. Problema de Maniobrabilidad

F´ısicamente, la trayectoria de un hexacóptero está compuesta particularmente por (xxx(t), uuu(t)) tal quexxx(˙t) =fff(xxx(t), uuu(t)) yxxx(0) =xxx0, nosotros podemos llamar maniobra a la porción de trayectoriaxxx(t). Para hexacópteros en general, las principales maniobras de interés son:

Movimiento en l´ınea recta: Manteniendo el rumbo y la altura, avanzando tanto sobre el eje x como sobre el eje y.

Movimiento de cambio de Altura: Manteniendo el rumbo y ascendiendo o des-cendiendo a un punto determinado, o sencillamente manteniendo la posici´on en

x y/o y.

Movimiento de cambio de Rumbo: Manteniendo la altura y desplaz´andose de una direcci´on a otra.

Movimiento Circular: Manteniendo la altura y desplaz´andose en forma circular sobre el plano xy.

Movimiento de Zig – Zag sobre el plano horizontal: Manteniendo la altura, descri-biendo un zig-zag, usualmente fijando el rudder en un ´angulo de desplazamiento

(42)

22 4 Problema de Maniobrabilidad

hasta que el ´angulo yaw llegue al valor deseado, y el desplazamiento del rudder a la posici´on opuesta.

Movimiento de Zig – Zag sobre el plano Vertical: Manteniendo el rumbo, des-cribiendo un zig-zag, usualmente fijando el plano interno a un ángulo de des-plazamiento en particular hasta que el ángulo pitch llegue al valor deseado, y el desplazamiento del plano interno a la posición opuesta.

Movimiento de una maniobra compuesta, que puede ser cualquier composici´on de las anteriores.

El problema de maniobrabilidad para el hexac´optero se puede abordar de la siguiente manera:

Encontrar(si es posible ) un función de (xxx(t), uuu(t)) o trayectoria, que representa la ley de estados del hexacóptero comoxxx(˙t) =fff(xxx(t), uuu(t)), tal quexxx(t) defina una maniobra especifica de interés, la cual es fijada comenzando. Adicionalmente, podemos requerir algún comportamiento óptimo para alguna variable de estado o variable de control durante la maniobra.

4.3. Una Aproximaci´

on de control ´

Optimo

En términos matemáticos, el problema de maniobrabilidad puede ser formulado como se presenta a continuación:

Dado un estado inicialxxx(0) =xxx0 ∈Ω, y un estado final deseadoxxxT ∈Ω, buscamos un

controluuu(t) tal que resuelve:

(PT)

            

Minimizar uuu: h(uuu) = m(xxx(T)) +RT

0 l(xxx(τ), uuu(τ), τ)δτ Sujeto a:

˙

xxx=fff(xxx(t), uuu(t));

xxx(0) =xxx0 ∈Ω;

xxx(t)∈Ω yuuu(t)∈ K, 0≤t ≤T.

(4-1)

Donde:

h(uuu) es la funci´on de coste,

l(xxx(τ), uuu(τ), τ) es el coste incremental, y

m(xxx(T)) es el coste terminal, todas estas son funciones escalares.

T´ıpicamente,

m(xxx(T)) = ||xxx(T)−xxxT(t)|| 2

P PP1

(43)

4.4 Soluci´on Num´erica del Problema (PPPt) 23

y

l(xxx(t), uuu(t), t) = ||xxx(t)−xxxd(t)|| 2

QQQ

2 +

||uuu(t)||2

RRR

2 (4-3)

conPPP1,QQQ,RRR, matrices diagonales reales, yxxxdel estado deseado.PPP1,QQQson consideradas matrices positivas semi definidas, mientras queRRRes considerada extrictamente positiva definida.

||aaa||2

K K

K =aaaTKaKaKa

Ajustando los valores dePPP1 y QQQ nosotros podemos pesar la importancia relativa de la desviación de cada estado de su valor deseado en el tiempo finalTTT, o durante todo el tiempo de la maniobra. Para el momento en que se incrementan los elementos de la diagonal dePPP1,p1i nosotros le damos más significancia que la desviación dexxxi(T) para el valor deseado, por que haciendo P1i cero indicamos que el valor final de xxxi no es importante para cualquier valor. De igual manera, podemos darle un peso de relativa importancia a la desviacion de las variables de estado para algun valor en particular a lo largo de toda la maniobra y el esfuerzo del control, ajustando los elementos de la diagonal deQQQ yRRR respectivamente.

4.4. Soluci´

on Num´

erica del Problema

(

P

_t

)

Hay varios métodos de optimización que pueden ser aplicados para solucionar (4-1). Debido a la complejidad de la ley de estado y el gran número de variables complejas del problema, es bastante razonable usar el método del desenso del gradiente. Brevemente, el esquema de este método consiste en seguir los siguientes pasos:

Calcular el gradiente de la funci´on de costo 5h(uuu).

Escoger un tama˜no de paso peque˜no pero justo con tj _>_0.

Actualizar para j ≥0∈_Z

uuu(j+1) =u(j)−t(j)5h(u(j))

Hasta la convergencia. El criterio de parada puede ser escogio como:

|h(uuu(j+1))−h(uuu(j))| ≤ε|h(uuu(0))|

(44)

4.4.1. C´

alculo del Gradiente de la Funci´

on de Costo

El paso crucial del cómputo del gradiente 5h(u(k)_{). Esto puede ser obtenido usando el} método adjoint que es descrito a continuación:

1. Dado el control u(j)_{, j} _≥_{0, soluciona la ecuaci´}_{on de estado:} (

˙

x

xx(t) =fff(xxx(t), uuu(j)(t))

x

xx(0) =xxx(j)₍_t₎ para obtener el estadoxxx(j+1)₍_t_).

2. Con el par (uuu(j)₍_t₎_{, x}_x_x(j+1)₍_t_{)), resolvemos la ecuaci´}_{on del sistema de ecuaciones} diferenciales para el estado adjuntoppp(t), hacia atr´as en el tiempo,

˙

ppp(t) =− 5xl(xxx(j+1)(t), uuu(j)(t))−[5xfff(xxx(j+1)(t), uuu(j)(t))]Tppp(t),

ppp(T) = − 5xm(xxx(j+1)(T)xxxT),

donde5x es el gradiente con respecto a las variablesxxx. As´ı, obtenemosppp(j+1)(t).

3. Finalmente, el gradiente de la funci´on de coste puede ser encontrado como:

5h(uuu(j)) = 5ul(xxx(j+1)(t), uuu(j)(t)) + [5ufff(xxx(j+1)(t), uuu(j)(t))]Tppp(j+1)(t),

Aqu´ıAT _{representa la traspuesta de}_A_.

4.4.2. Abordando las restricciones

Como es visto en la formulaci´on (PT) hay l´ımites tanto en el estado como en las variables

de control. En este trabajo, tratamos con estos l´ımites a seguir:

Si durante el proceso de optimización, el comportamiento de variables de estado como son los ángulos de rollo ψ(t) o elevación θ(t) estan fuera de las fronteras real´ısticas (−45o _{< φ <} ₄₅o _,₋₄₅o _{< θ <} ₄₅o _,₀o _{< ψ <} ₃₆₀o_{), nosotros}

tratamos de mantener los l´ımites agregando o incrementando los par´ametros qqq4 y qqq5 respectivamente. Lo mismo es aplicable a las variables de control (K = [0,1000r.p.s.]×[0,1000r.p.s.]×[0,1000r.p.s.]×[0,1000r.p.s.]×[0,1000r.p.s.]×

[0,1000r.p.s.]) para las variables de control en las fronteras f´ısicas). No obstante, incluso si después del aumento de los valores de los parámetros de peso de la variable(s) de interés, estamos fuera de los l´ımites, lo que hacemos es incrementar el tiempo final, ya que la razón de tal comportamiento de algoritmo sea que el sistema no es f´ısicamente capaz de alcanzar lo que uno lo pide hacer. Obviamente, a nivel matemático, la forma correcta de tratar con l´ımites tanto en el estado

(45)

como en las variables de control debe introducir funciones de barrera asociadas con las restricciones en la función de costo; Aunque no seguimos este acercamiento porque nos beneficiamos del f´ısico del problema que hace innecesario el poner en práctica un algoritmo más sofisticado.

4.4.3. Simulaci´

on Num´

erica

El programa para implementar la solución de (4-1) fue hecho usando Matlab® interac-tuando con el código C mediante funciones-S (el lector es referido a la documentación de Matlab para más detalles de implementación de funciones-S). Los ODEs son resuel-tos usando la función de Matlab ode45, que es un solucionador de un paso basado en un método Runge-Kutta expl´ıcito (Shampine 1997). Una constante del step size que fue usado. Como era esperado, los resultados dependen de la inicialización del algorit-mo. Incluso cuando los resultados relacionados con este asunto no son incluidos aqu´ı, el lector puede remitirse a [12] para ver este aspecto con el conjunto de coeficientes ligeramente diferentes de los considerados en la Tabla 2.3, y además para ver algunas otras simulaciones numéricas con un modelo diferente de Helices.

Para ilustrar la aproximaci´on de la propuesta en este cap´ıtulo nosotros seleccionamos una maniobra en particular. La t´ıpica maniobra de prueba para este tipo de sistemas no tripulados de este trabajo es llamada estacionario.

4.4.4. Resultados Num´

ericos

Movimiento sobre el eje z

Consideramos los siguientes parametros para el problema de maniobrabilidad del he-xac´optero                 

x(0) = [0 0 −10 0 0 0 0 0 0 0 0 0]T

x(T) = [0 0 −10 0 0 0 0 0 0 0 0 0]T

α = [0 0 1 0 0 0 0 0 0 0 0 0]

xd(t) = [0 0 −5 0 0 0 0 0 0 0 0 0]T

γ = [0 0 0,3 0 0 0 0 0 1 0 0 0]

(46)

(a) (b)

(c) (d)

Figura 4-1: Comportamiento de las variables de estado m´as relevantes en el movi-meinto. (a) desplazamiento vertical z(t), (b) velocidad de descenso w(t), (c) controles

(47)

Movimiento sobre el eje y

x(0) = [0 0 −10 0 0 0 0 0 0 0 0 0]T

x(T) = [0 1 0 0 0 0 0 0 0 0 0 0]T

α = [0 10 10 100 0 0 0 100 0 0 0 0]

xd(t) = [0 1 0 0 0 0 0 0 0 0 0 0]T

γ = [0 1 5 0 0 0 0 0 0 0 0 0]

λ = 1×10−8_.

Movimiento sobre el eje x

x(0) = [0 0 −10 0 0 0 0 0 0 0 0 0]T

x(T) = [1 0 0 0 0 0 0 0 0 0 0 0]T

α = [10 0 10 0 0 0 10 0 10 0 0 0]

xd(t) = [1 0 0 0 0 0 0 0 0 0 0 0]T

γ = [5 0 5 0 0 0 0 0 0 0 0 0]

λ= 1×10−8.

(48)

(a) (b)

(c) (d)

(e) (f)

Figura 4-2: Comportamiento de las variables de estado m´as relevantes en el movi-meinto. (a) desplazamiento lateraly(t), (b) desplazamiento vertical z(t), (c) velocidad lateral v(t), (d) velocidad de descenso w(t), (e) ´angulo roll φ(t), y (f) controlesuuu(t).

(49)

(a) (b)

(c) (d)

(e) (f)

Figura 4-3: Comportamiento de las variables de estado m´as relevantes en el movimein-to. (a) desplazamiento longitudinalx(t), (b) desplazamiento verticalz(t), (c) velocidad longitudinalu(t), (d) velocidad de descenso w(t), (e) ´angulo pitchθ(t), y (f) controles

u uu(t).

(50)

Cap´ıtulo 5

Conclusiones y observaciones

- Los resultados muestran lo fácil que es que el hexacóptero se mueva verticalmente, porque los rotores están actuando directamente sobre el eje z.

- Sin embargo, los movimientos longitudinales y laterales son m´as dif´ıciles de con-seguir debido a la falta de actuadores en los ejes x y y.

De hecho como se puede analizar en el comportamiento de los resultados, es evidente que, aunque es peque˜no el desplazamiento en el plano vertical, hay una necesidad de moverse verticalmente para poder realizar un movimiento hacia delante o hacia atras y laretalmente.

Los resultados numéricos también muestran la simetr´ıa evidente a lo largo de los ejes x y y, lo que hace que el hexacóptero tenga la disponibilidad para realizar la misma acción o actuar con la misma eficacia ya sea seleccionando parámetros para los movimientos longitudinal y parámetros para los movimientos laterales.

(51)

Cap´ıtulo 6

Anexos

6.1. C´

odigos del Matlab

6.1.1. C´

odigo General

1 %function costegeneralf 2

3 clear all 4 close all 5 clc

6

7 t=cputime; 8

9 global x t1 alpha xf fz TK TM TN p ci h

10

11 load hexacopter params 12

13 Tf=10; % tiempo final

14 h=Tf/100; % paso de tiempo

15

16 % El coste esta definido como

17 % J = int (F(x,u,t)) dt + Phi(x(f)) 18 % donde

19 % F = gamma*(x(t)−xd)ˆ2 + beta*u(t)ˆ2 20 % y

21 % Phi = alpha*(x(Tf)−xf)ˆ2 22

23 alpha = [10 10 10 0 0 0 100 100 100 0 0 0]; % estado ...

final

24 beta = 0*[0 1 1 0]; % control

25 lambda = 1e−9; % paso de las ...

iteraciones

(52)

32 6 Anexos

27 xd = [1 1 1 0 0 0 0 0 0 0 0 0]; % valor ...

estacionario deseado de x(t)

28 x0 = [0 0 0 0 0 0 0 0 0 0 0 0]; % estado inicial 29 xf = [1 1 1 0 0 0 0 0 0 0 0 0]; % estado final 30

31 t1=0:h:Tf; % vector de tiempo

32 33 fz=mass*gg+0*t1'; 34 TK=0+0*t1'; 35 TM=0+0*t1'; 36 TN=0+0*t1'; 37 38

39 % ...

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

40 % NONLINEAR SYSTEM resolucion con ODE 41

42 tspan = [0,Tf]; 43

44 [T,x] = ode45(@(t,x) hex state(t,x,t1,fz,TK,TM,TN),tspan,x0'); 45

46 x = interp1(T,x,t1); 47

48

49 % ...

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

50

51 % c?lculo del primer coste 52

53 a = length(x);

54 F = beta*[(fz−gg*mass).ˆ2 (TK).ˆ2 (TM).ˆ2 (TN).ˆ2]' + ... gamma*(((x−ones(a,1)*xd)').ˆ2);

55 q = simps v(F,h);

56 ci(2) = q + alpha*(((x(a,:)−xf)').ˆ2) 57 ci(1) = 1.1*ci(2)

58 59 j=2;

60 %% iteraciones

61 while (ci(j−1)−ci(j))>(1e−06*ci(2))

62 % 63 j=j+1; 64

65 % solucion ecuacion coestado a partir de x y u 66

67 pt2 = 2*alpha.*(x(length(x),:)−xf); 68 tspan = [Tf,0];

69

(53)

6.1 C´odigos del Matlab 33

71

72 p = interp1(t2,p,t1); 73

74 % calculo del gradiente a partir de x u y p 75

76 gradJ = gradientefinalp2(fz,TK,TM,TN,p,t1,beta); 77

78 % calculo del nuevo control 79

80 uk = [fz TK TM TN]; 81

82 ukp1 = uk − lambda*gradJ; 83

84 fz = ukp1(:,1); 85 TK = ukp1(:,2); 86 TM = ukp1(:,3); 87 TN = ukp1(:,4); 88

89 % volvemos a resolver la dinamica para conocer el nuevo coste y ...

decidir si seguimos iterando o encontramos la solucion

90

91 tspan = [0,Tf]; 92

93 [T,x] = ode45(@(t,x) hex state(t,x,t1,fz,TK,TM,TN),tspan,x0'); 94

95 x = interp1(T,x,t1); 96

97 % calculo del coste con el nuevo control debido a lambda 98

99 a = length(x);

100 F = beta*[(fz−mass*gg).ˆ2 (TK).ˆ2 (TM).ˆ2 (TN).ˆ2]' + ... gamma*(((x−ones(a,1)*xd)').ˆ2);

101 q1 = simps v(F,h);

102 ci(j) = q1 + alpha*(((x(a,:)−xf)').ˆ2) 103

104 end

105 cputime−t

106 %%−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

6.1.2. Din´

amica del hexac´

optero

1 function out = hex state(t,x,t1,fz,TK,TM,TN)

2

3 % aqu? se define la funcion f(x(t),u(t)), que es el segundo ...

(54)

34 6 Anexos

4 % la EDO dx(t)=f(x(t),u(t)) 5

6 fz = interp1(t1,fz,t); 7 TK = interp1(t1,TK,t); 8 TM = interp1(t1,TM,t); 9 TN = interp1(t1,TN,t); 10

11 phi = pi/180*x(4); 12 th = pi/180*x(5); 13 psi = pi/180*x(6); 14 uu = x(7);

15 vv = x(8); 16 ww = x(9);

17 pp = pi/180*x(10); 18 qq = pi/180*x(11); 19 rr = pi/180*x(12); 20

23 out = [ %f1

24 uu*cos(th)*cos(psi) + vv*sin(phi)*sin(th)*cos(psi) + ... 25 − vv*cos(phi)*sin(psi) + ww*sin(phi)*sin(psi) + ... 26 ww*cos(phi)*sin(th)*cos(psi)

27 %f2

28 uu*cos(th)*sin(psi) + vv*cos(phi)*cos(psi) + ...

29 vv*sin(phi)*sin(th)*sin(psi) − ww*sin(phi)*cos(psi) + ... 30 ww*cos(phi)*sin(th)*sin(psi)

31 %f3

32 − uu*sin(th) + vv*sin(phi)*cos(th) + ww*cos(phi)*cos(th)

33 %f4

34 pp + qq*sin(phi)*tan(th) + rr*cos(phi)*tan(th)

35 %f5

36 qq*cos(phi) − rr*sin(phi)

37 %f6

38 qq*sin(phi)/cos(th) + rr*cos(phi)/cos(th)

39 %f7

40 vv*rr − ww*qq − gg*sin(th)

41 %f8

42 ww*pp − uu*rr + gg*sin(phi)*cos(th)

43 %f9

44 uu*qq − vv*pp + gg*cos(phi)*cos(th) + ... 45 − fz/mass

46 %f10

47 ((Iy−Iz)*qq*rr + TK)/Ix;

48 %f11

49 ((Iz−Ix)*pp*rr − TM)/Iy;

50 %f12

(55)

52

53 % xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx

6.1.3. Din´

amica del coestado

1 function out1 = hex costate(t2,p,t1,x,gamma,xd)

2

3 % g1 define el segundo miembro de la ecuacion del coestado: 4 % dp(x)=−A'*p(x)−2*0.001*b;

5 % donde A esta formada por las derivadas respecto de x(t) de cada ...

funcion del segundo miembro de la ecuacion de estado.

6

7 x = interp1(t1,x,t2); 8

11 f1x1 = 0; 12 f1x2 = 0; 13 f1x3 = 0; 14 f1x4 = ...

pi/180*(((x(:,8).*cos(1/180*pi*x(:,4))).*sin(pi/180*x(:,5)))....

15 _{*cos(pi/180*x(:,6)))+}...

16 pi/180*((x(:,8).*sin(pi/180*x(:,4))).*sin(pi/180*x(:,6)))+... 17 pi/180*((x(:,9).*cos(pi/180*x(:,4))).*sin(pi/180*x(:,6)))+... 18 −pi/180*(((x(:,9).*sin(pi/180*x(:,4))).*sin(pi/180*x(:,5))).... 19 _{*cos(pi/180*x(:,6)));}

20 %plot(T,f1x4)

21 f1x5 = −(pi/180*x(:,7).*sin(pi/180*x(:,5))).*cos(pi/180*x(:,6))+... 22 ((pi/180*x(:,8).*sin(pi/180*x(:,4))).*cos(pi/180*x(:,5))).... 23 _{*cos(pi/180*x(:,6))+}...

24 ((pi/180*x(:,9).*cos(pi/180*x(:,4))).*cos(pi/180*x(:,5))).... 25 _{*cos(pi/180*x(:,6));}

26 %plot(T,f1x5)

27 f1x6 = −(pi/180*x(:,7).*cos(pi/180*x(:,5))).*sin(pi/180*x(:,6))+... 28 −((pi/180*x(:,8).*sin(pi/180*x(:,4))).*sin(pi/180*x(:,5))).... 29 _{*sin(pi/180*x(:,6))+}...

30 −(pi/180*x(:,8).*cos(pi/180*x(:,4))).*cos(pi/180*x(:,6))+... 31 (pi/180*x(:,9).*sin(pi/180*x(:,4))).*cos(pi/180*x(:,6))+... 32 −((pi/180*x(:,9).*cos(pi/180*x(:,4))).*sin(pi/180*x(:,5))).... 33 _{*sin(pi/180*x(:,6));}

34 %plot(T,f1x6)

35 f1x7 = cos(pi/180*x(:,5)).*cos(pi/180*x(:,6)); 36 %plot(T,f1x7)

37 f1x8 = ...

(56)

36 6 Anexos

38 (cos(pi/180*x(:,4))).*sin(pi/180*x(:,6)); 39 %%plot(T,f1x8)

40 f1x9 = ...

(sin(pi/180*x(:,4))).*sin(pi/180*x(:,6))+((cos(pi/180*x(:,4)))....

41 _{*sin(pi/180*x(:,5))).*cos(pi/180*x(:,6));} 42 %plot(T,f1x9)

43 f1x10 = 0; 44 f1x11 = 0; 45 f1x12 = 0; 46

47 f2x1 = 0; 48 f2x2 = 0; 49 f2x3 = 0;

50 f2x4 = −(pi/180*x(:,8).*sin(pi/180*x(:,4))).*cos(pi/180*x(:,6))+... 51 ((pi/180*x(:,8).*cos(pi/180*x(:,4))).*sin(pi/180*x(:,5))).... 52 _{*sin(pi/180*x(:,6))+}...

53 −((pi/180*x(:,9).*sin(pi/180*x(:,4))).*sin(pi/180*x(:,5))).... 54 _{*sin(pi/180*x(:,6))+}...

55 −(pi/180*x(:,9).*cos(pi/180*x(:,4))).*cos(pi/180*x(:,6)); 56 %plot(T,f2x4)

57 f2x5 = −(pi/180*x(:,7).*sin(pi/180*x(:,5))).*sin(pi/180*x(:,6))+... 58 ((pi/180*x(:,8).*sin(pi/180*x(:,4))).*cos(pi/180*x(:,5))).... 59 _{*sin(pi/180*x(:,6))+}...

60 ((pi/180*x(:,9).*cos(pi/180*x(:,4))).*cos(pi/180*x(:,5))).... 61 _{*sin(pi/180*x(:,6));}

62 %plot(T,f2x5)

63 f2x6 = (pi/180*x(:,7).*cos(pi/180*x(:,5))).*cos(pi/180*x(:,6))+... 64 −(pi/180*x(:,8).*cos(pi/180*x(:,4))).*sin(pi/180*x(:,6))+... 65 ((pi/180*x(:,8).*sin(pi/180*x(:,4))).*sin(pi/180*x(:,5))).... 66 _{*cos(pi/180*x(:,6))+}...

67 ((pi/180*x(:,9).*cos(pi/180*x(:,4))).*sin(pi/180*x(:,5))).... 68 _{*cos(pi/180*x(:,6))+}...

69 (pi/180*x(:,9).*sin(pi/180*x(:,4))).*sin(pi/180*x(:,6)); 70 %plot(T,f2x6)

71 f2x7 = (cos(pi/180*x(:,5))).*sin(pi/180*x(:,6)); 72 %plot(T,f2x7)

73 f2x8 = ...

(cos(pi/180*x(:,4))).*cos(pi/180*x(:,6))+((sin(pi/180*x(:,4)))....

74 _{*sin(pi/180*x(:,5))).*sin(pi/180*x(:,6));} 75 %plot(T,f2x8)

76 f2x9 = ...

((cos(pi/180*x(:,4))).*sin(pi/180*x(:,5))).*sin(pi/180*x(:,6))...

77 −(sin(pi/180*x(:,4))).*cos(pi/180*x(:,6)); 78 %plot(T,f2x9)

79 f2x10 = 0; 80 f2x11 = 0; 81 f2x12 = 0; 82

(57)

83 f3x1 = 0; 84 f3x2 = 0; 85 f3x3 = 0;

86 f3x4 = (pi/180*x(:,8).*cos(pi/180*x(:,4))).*cos(pi/180*x(:,5))+... 87 −(pi/180*x(:,9).*sin(pi/180*x(:,4))).*cos(pi/180*x(:,5)); 88 %plot(T,f3x4)

89 f3x5 = −pi/180*x(:,7).*cos(pi/180*x(:,5))+...

90 −(pi/180*x(:,8).*sin(pi/180*x(:,4))).*sin(pi/180*x(:,5))+... 91 −(pi/180*x(:,9).*cos(pi/180*x(:,4))).*sin(pi/180*x(:,5)); 92 %plot(T,f3x5)

93 f3x6 = 0;

94 f3x7 = −sin(pi/180*x(:,5)); 95 %plot(T,f3x7)

96 f3x8 = (sin(pi/180*x(:,4))).*cos(pi/180*x(:,5)); 97 %plot(T,f3x8)

98 f3x9 = (cos(pi/180*x(:,4))).*cos(pi/180*x(:,5)); 99 %plot(T,f3x9)

100 f3x10 = 0; 101 f3x11 = 0; 102 f3x12 = 0; 103

104 f4x1 = 0; 105 f4x2 = 0; 106 f4x3 = 0;

107 f4x4 = (pi/180*x(:,11).*cos(pi/180*x(:,4))).*tan(pi/180*x(:,5))+... 108 −(pi/180*x(:,12).*sin(pi/180*x(:,4))).*tan(pi/180*x(:,5)); 109 %plot(T,f4x4)

110 f4x5 = ...

(pi/180*x(:,11).*sin(pi/180*x(:,4))).*(1+(tan(pi/180*x(:,5))).ˆ2)+...

111 (pi/180*x(:,12).*cos(pi/180*x(:,4))).*(1+(tan(pi/180*x(:,5))).ˆ2); 112 %plot(T,f4x5)

113 f4x6 = 0; 114 f4x7 = 0; 115 f4x8 = 0; 116 f4x9 = 0; 117 f4x10 = 1;

118 f4x11 = (sin(pi/180*x(:,4))).*tan(pi/180*x(:,5)); 119 %plot(T,f4x11)

120 f4x12 = (cos(pi/180*x(:,4))).*tan(pi/180*x(:,5)); 121 %plot(T,f4x12)

122

123 f5x1 = 0; 124 f5x2 = 0; 125 f5x3 = 0;

126 f5x4 = −pi/180*x(:,11).*sin(pi/180*x(:,4)) − ... pi/180*x(:,12).*cos(pi/180*x(:,4));

127 %plot(T,f5x4) 128 f5x5 = 0;

(58)

38 6 Anexos

129 f5x6 = 0; 130 f5x7 = 0; 131 f5x8 = 0; 132 f5x9 = 0; 133 f5x10 = 0;

134 f5x11 = cos(pi/180*x(:,4)); 135 %plot(T,f5x11)

136 f5x12 = −sin(pi/180*x(:,4)); 137 %plot(T,f5x12)

138

139 f6x1 = 0; 140 f6x2 = 0; 141 f6x3 = 0;

142 f6x4 = pi/180*x(:,11).*cos(pi/180*x(:,4))./cos(pi/180*x(:,5))+... 143 −pi/180*x(:,12).*sin(pi/180*x(:,4))./cos(pi/180*x(:,5)); 144 %plot(T,f6x4)

145 f6x5 = pi/180*x(:,11).*sin(pi/180*x(:,4))./(cos(pi/180*x(:,5))).ˆ2.... 146 _{*sin(pi/180*x(:,5))+}...

147 pi/180*x(:,12).*cos(pi/180*x(:,4))./(cos(pi/180*x(:,5))).ˆ2.... 148 _{*sin(pi/180*x(:,5));}

149 %plot(T,f6x5) 150 f6x6 = 0; 151 f6x7 = 0; 152 f6x8 = 0; 153 f6x9 = 0; 154 f6x10 = 0;

155 f6x11 = (sin(pi/180*x(:,4)))./cos(pi/180*x(:,5)); 156 %plot(T,f6x11)

157 f6x12 = (cos(pi/180*x(:,4)))./cos(pi/180*x(:,5)); 158 %plot(T,f6x12)

159

160 f7x1 = 0; 161 f7x2 = 0; 162 f7x3 = 0; 163 f7x4 = 0;

164 f7x5 = −pi/180*gg*cos(pi/180*x(:,5)); 165 f7x6 = 0;

166 f7x7 = 0;

167 f7x8 = (pi/180)*x(:,12); 168 f7x9 = −(pi/180)*x(:,11); 169 f7x10 = 0;

170 f7x11 = −(pi/180)*x(:,9); 171 f7x12 = (pi/180)*x(:,8); 172

173 f8x1 = 0; 174 f8x2 = 0; 175 f8x3 = 0;

(59)

177 f8x5 = −(pi/180*gg*sin(pi/180*x(:,4))).*sin(pi/180*x(:,5)); 178 f8x6 = 0;

179 f8x7 = −(pi/180)*x(:,12); 180 f8x8 = 0;

181 f8x9 = (pi/180)*x(:,10); 182 f8x10 = (pi/180)*x(:,9); 183 f8x11 = 0;

184 f8x12 = −(pi/180)*x(:,7); 185

186 f9x1 = 0; 187 f9x2 = 0; 188 f9x3 = 0;

189 f9x4 = −(pi/180*gg*sin(pi/180*x(:,4))).*cos(pi/180*x(:,5)); 190 f9x5 = −(pi/180*gg*cos(pi/180*x(:,4))).*sin(pi/180*x(:,5)); 191 f9x6 = 0;

192 f9x7 = (pi/180)*x(:,11); 193 f9x8 = −(pi/180)*x(:,10); 194 f9x9 = 0;

195 f9x10 = −(pi/180)*x(:,8); 196 f9x11 = (pi/180)*x(:,7); 197 f9x12 = 0;

198 199

200 f10x1 = 0; 201 f10x2 = 0; 202 f10x3 = 0; 203 f10x4 = 0; 204 f10x5 = 0; 205 f10x6 = 0; 206 f10x7 = 0; 207 f10x8 = 0; 208 f10x9 = 0; 209 f10x10 = 0;

210 f10x11 = (pi/180)ˆ2*x(:,12)*(Iy−Iz)/Ix; 211 f10x12 = (pi/180)ˆ2*x(:,11)*(Iy−Iz)/Ix; 212

213 f11x1 = 0; 214 f11x2 = 0; 215 f11x3 = 0; 216 f11x4 = 0; 217 f11x5 = 0; 218 f11x6 = 0; 219 f11x7 = 0; 220 f11x8 = 0; 221 f11x9 = 0;

222 f11x10 = (pi/180)ˆ2*x(:,12)*(Iz−Ix)/Iy; 223 f11x11 = 0;

(60)

40 6 Anexos

225

226 f12x1 = 0; 227 f12x2 = 0; 228 f12x3 = 0; 229 f12x4 = 0; 230 f12x5 = 0; 231 f12x6 = 0; 232 f12x7 = 0; 233 f12x8 = 0; 234 f12x9 = 0;

235 f12x10 = (pi/180)ˆ2*x(:,11)*(Ix−Iy)/Iz; 236 f12x11 = (pi/180)ˆ2*x(:,10)*(Ix−Iy)/Iz; 237 f12x12 = 0;

238 239

240 A = [f1x1 f1x2 f1x3 f1x4 f1x5 f1x6 f1x7 f1x8 f1x9 f1x10 ... f1x11 f1x12

241 f2x1 f2x2 f2x3 f2x4 f2x5 f2x6 f2x7 f2x8 f2x9 f2x10 ... f2x11 f2x12

251 f12x1 f12x2 f12x3 f12x4 f12x5 f12x6 f12x7 f12x8 f12x9 f12x10 ... f12x11 f12x12];

252

253 out1 = −A'*p−(2*diag(gamma)*(x−ones(length(t2),1)*xd)');%−2*0.01*b; 254

255 % xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx

(61)

1 function gradJ = gradientefinalp2(fz,TK,TM,TN,p,t1,beta)

2

5 gradJ = zeros(length(t1),4); 6 for i = 1:length(p);

7 A2 = [zeros(8,4)

8 −1/mass 0 0 0

9 0 1/Ix 0 0

10 0 0 −1/Iy 0 11 0 0 0 −1/Iz]; 12

13 %aux=inv(M)*(A2)

14 A3 = 2*beta*[fz(i)−mass*gg TK(i) TM(i) TN(i)]'; 15 gradJ(i,:) = A3 + p(i,:)*(A2);

16 end 17