estimación de la posición relativa de aeronaves a partir...

Equation Chapter 1 Section 1

Trabajo Fin de Grado

Grado en Ingeniería de las Tecnologías

Industriales

Estimación de la posición relativa de aeronaves a

partir de imágenes de la pista de aterrizaje.

Autor: Pablo de la Mata Santaella

Tutor: Manuel Ruiz Arahal

Dep. Ingeniería de Sistemas y Automática

Escuela Técnica Superior de Ingeniería

Universidad de Sevilla

Sevilla, 2017

Trabajo Fin de Grado

Grado en Ingeniería de las Tecnologías Industriales

Estimación de la posición relativa de aeronaves a

partir de imágenes de la pista de aterrizaje.

Autor:

Pablo de la Mata Santaella

Tutor:

Manuel Ruiz Arahal

Profesor Catedrático

Dep. de Ingeniería de Sistemas y Automática

Escuela Técnica Superior de Ingeniería

Universidad de Sevilla

Sevilla, 2017

4

Trabajo Fin de Grado: Estimación de la posición relativa de aeronaves a partir de imágenes de la pista de

aterrizaje.

Autor: Pablo de la Mata Santaella

Tutor: Manuel Ruiz Arahal

El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes miembros:

Presidente:

Vocales:

Secretario:

Acuerdan otorgarle la calificación de:

Sevilla, 2017

El Secretario del Tribunal

6

A mi familia

A mis maestros

8

Resumen

La proliferación en los últimos años de las pequeñas aeronaves no tripuladas con fines tanto recreativos como

comerciales ha necesitado de un apoyo constante de sistemas de control automáticos, capaces de mantener la

aeronave en vuelo con la mínima intervención humana posible. Sin embargo en muchas ocasiones estos

sistemas de control dependen de medidas de sensores que pueden ser ruidosos y poco fiables por un lado o

muy costosos por el otro.

Para tratar de paliar este problema surje este trabajo, el cual pretende apoyar a estos sensores y sistemas de

control, aportando información de una manera sencilla y de bajo coste. El rápido avance en el campo de la

visión artificial y sus crecientes capacidades son las que han motivado la creación de este proyecto, el cual

pretende obtener una estimación fiable y rápida de la actitud de la aeronave como única referencia la pista de

aterrizaje, de la misma manera que podría hacerlo un piloto humano.

En el caso de alcanzar este objetivo se abriría un amplio abanico de posibilidades dentro del campo del vuelo

autónomo, permitiendo en última instancia la operación completamente automática de pequeñas aeronaves no

tripuladas.

El logro fundamental de este trabajo será la creación de una herramienta modular que permita extraer

información de actitud a partir de una única imagen con pequeño coste computacional, empleando para ello el

software comercial MatLab©.

Índice

Resumen 8

Índice 9

Índice de Tablas 11

Índice de Figuras 12

1 Introducción 15 1.1 Contexto 16

2 Introducción a la formación de imágenes 17 2.1. Transformación de perspectiva 17

3 Introducción a la estimación de orientación 23 3.1 Parámetros cámara 24 3.2 Algoritmo de estimación de orientación 27

3.2.1 Definición de variables del problema de optimización 27 3.2.2 Definición del problema de optimización 29

4 Implementación del algoritmo 31 4.1 Descripción entorno de trabajo 31

4.1.1 Entorno de programación MatLab© 31 4.1.2 Pista de aterrizaje 31 4.1.3 Aeronave 32

4.2 Puesta en práctica del algoritmo mediante MatLab© 33 4.2.1 Prueba con 1 ángulo: Pitch 34 4.2.2 Prueba con 2 ángulos: Pitch y Roll 36 4.2.3 Prueba con 3 ángulos: Pitch, Roll y Yaw 38 4.2.4 Prueba de orientación completa: Pitch, Roll, Yaw y vector x, y, z 39 4.2.5 Aplicación a una imagen real 40

5 Puesta en práctica y generación de resultados 44 5.1 Ángulo de Pitch 45 5.2 Ángulos de Pitch y Roll 49

5.2.1 Fuerza bruta 49 5.2.2 Función MatLab fminunc 50

5.3 3 ángulos: Roll Pitch y Yaw 53 5.4 Orientación completa (ROLL, PITCH, YAW y posición espacial) 56

5.4.1 Resolución mediante fminunc 57 5.4.2 Resolución mediante lsqnonlin 60

5.5 Aplicación a imágenes reales 63 5.5.1 Preparación de las pruebas 66 5.5.2 Prueba frame 2537 67

10

5.5.3 Prueba frame 4091 69 5.5.4 Prueba frame 7882 71 5.5.5 Prueba caso límite 1: fallo de la detección de pista 73 5.5.6 Prueba caso límite 2: pista fuera de los límites de recuadro 76

6 Conclusiones y discusión final 79 6.1 Objetivos 79 6.2 Valoración del trabajo 79 6.3 Trabajo futuro 80

Referencias 81

Anexo: Código .m utilizado 82

ÍNDICE DE TABLAS

Tabla 1: prueba de validación de algoritmo para un ángulo mediante fuerza bruta 45

Tabla 2: Resultados del algoritmo fuerza bruta para ángulo de PITCH 49

Tabla 3: Resultados del algoritmo fuerza bruta para ángulo de ROLL 50

Tabla 4: Resultados obtenidos en la estimación de ángulo ROLL para valores iniciales de roll =10º pitch = 15º

50

Tabla 5: Resultados obtenidos en la estimación de ángulo PITCH para valores iniciales de roll =10º pitch = 15º

51

Tabla 6: Resultados para estimación de ángulo ROLL para valores iniciales de 51

roll =100º pitch = 150º 51

Tabla 7: Resultados para estimación de ángulo PITCH para valores iniciales de roll =100º pitch = 150º 52

Tabla 8: Resultados para estimación de ángulo ROLL para valores iniciales de roll = 0º pitch = 0º yaw = 0º

53

Tabla 9: Resultados para estimación de ángulo YAW para valores iniciales de roll = 0º pitch = 0º yaw = 0º

54

Tabla 10: Resultados para estimación de ángulo ROLL para valores iniciales de roll =0º pitch = -10º yaw =

30º 55

Tabla 11: Resultados para estimación de ángulo PITCH para valores iniciales de roll =0º pitch = -10º yaw =

30º 55

Tabla 12: Resultados para estimación de ángulo YAW para valores iniciales de roll =0º pitch = -10º yaw =

30º 55

Tabla 13: Tabla de valores iniciales para estimación de orientación completa 57

Tabla 14: Resultados obtenidos en la estimación de ángulo ROLL en orientación completa fminunc 57

Tabla 15: Resultados obtenidos en la estimación de ángulo PITCH en orientación completa fminunc 58

Tabla 16: Resultados obtenidos en la estimación de ángulo YAW en orientación completa fminunc 58

Tabla 17: Resultados obtenidos en la estimación de posición X en orientación completa fminunc 59

Tabla 18: Resultados obtenidos en la estimación de posición Y en orientación completa fminunc 59

Tabla 19: Resultados obtenidos en la estimación de posición Z en orientación completa fminunc 60

Tabla 20: Resultados obtenidos en la estimación de ángulo ROLL en orientación completa lsqnonlin 60

Tabla 21: Resultados obtenidos en la estimación de ángulo PITCH en orientación completa lsqnonlin 61

Tabla 22: Resultados obtenidos en la estimación de ángulo YAW en orientación completa lsqnonlin 61

Tabla 23: Resultados obtenidos en la estimación de posición X en orientación completa lsqnonlin 62

Tabla 24: Resultados obtenidos en la estimación de posición Y en orientación completa lsqnonlin 62

Tabla 25: Resultados obtenidos en la estimación de posición Z en orientación completa lsqnonlin 63

Índice de Figuras 12

ÍNDICE DE FIGURAS

Figura 1: Geometría de formación de imagen con lente fina (tomado de Corke 2011) 18

Figura 2: Modelo de perspectiva central (tomado de Corke 2011) 19

Figura 3: Representación ejes mundo-cámara (tomada de Corke 2011) 20

Figura 4: Estructura de sensor de imagen en ejes pixeles (tomada de Corke 2011) 22

Figura 6: Datasheet del sensor OV5647 25

Figura 6: Proceso de formación de imágenes digitales 28

Figura 7: Diagrama del algoritmo a utilizar 30

Figura 8: Pista de aterrizaje con puntos superpuestos 32

Figura 9: Skywalker X-8 en vuelo 33

Figura 10: sistemas embarcados 33

Figura 11: Creación de puntos sintéticos (izquierda) Creación de matriz de Transformación (derecha) 34

Figura 12: imagen utilizada en prueba con puntos superpuestos 41

Figura 13: Procedimiento de estimación de orientación para imágenes reales 42

Figura 14: Ejemplo de resultado de estimación 44

Figura 15: evolución de la función de error para 150,2º 46

Figura 16: ampliación del error en el entorno del mínimo para 150,2º 46





Figura 21: diagrama algoritmo completo estimación de orientación 65

Figura 22: Diagrama algoritmo de preprocesado de vídeo 65

Figura 23: Algoritmo de identificación y clasificación de las esquinas de la pista 66

Figura 24: frame 2537 67

Figura 25: Resultado final para frame 2537 68


Figura 27: Resultado final para frame 4091 70


Figura 28: resultado final para frame 7882 72

Figura 29: frame con fallo de detección (frame 8878) 73

Figura 29: superposición de esquinas en imagen con detección fallida 74

Figura 29: resultado final con superposición 75

Figura 30: frame donde ocurre el fallo (frame 9005) 76

Figura 31: detección de esquinas fallida 77

Figura 32: Corrección de detección de esquinas 78

Figura 33: imagen superpuesta final para caso límite 2 78

1 INTRODUCCIÓN

l objetivo de este trabajo es la estimación de la actitud de aeronaves mediante el empleo de imágenes de

una pista de aterrizaje, pudiendo servir de apoyo a los instrumentos tradicionales para medir la actitud

(como el horizonte artificial). Utilizando estas imágenes se estima la orientación de la cámara con

respecto a la pista y con ello se extrae información de la actitud de la aeronave. La solución al problema de la

detección de dicha pista en la imagen se basa en el trabajo previo realizado por el alumno de ESCUELA

TÉCNICA SUPERIOR DE INGENIERÍA INFORMÁTICA Juan Manuel Benítez Domínguez en el PFC

“Detección automática de pistas de aterrizaje mediante técnicas de procesado de imágenes”1.

EL objetivo último de este método es la asistencia y posible guiado de la aeronave en la maniobra de

aterrizaje, utilizando esta información para asegurar la precisión en dicha maniobra (seguimiento de senda de

planeo, rumbo real de la pista y evitar las oscilaciones en el ángulo balance). Además ofrece una alternativa

fiable al resto de sensores embarcados, ya que sensores como los presentes en una IMU2 (Inertial

Measurement Unit, unidad de medición inercial por sus siglas en inglés) presentan problemas de precisión al

ser sensores ruidosos y proclives a perder exactitud al ir acumulando deriva.

Usando esta información se podría mejorar en gran medida la seguridad en la maniobra de aterrizaje y, en

última instancia, podría permitir el aterrizaje autónomo de la aeronave sin necesidad de grandes y costosas

instalaciones como el ILS (Instrumental Landing System)3, GPS diferencial o MLS (Microwave Landing

System)4. Presenta además la ventaja de necesitar muy poco equipamiento (cámara y equipo para procesar la

imagen), siendo este de bajo coste, de reducidas dimensiones y de escaso peso, permitiendo la implementación

casi inmediata en aeronaves ya existentes.

La escalabilidad es otra de sus ventajas, ya que siempre hay margen de utilizar imágenes de mayor resolución

e incluso imágenes infrarrojas (al depender únicamente del análisis de imágenes digitales) para condiciones de

baja visibilidad. Esto podría requerir de más capacidad de procesamiento, pero gracias al estado de la técnica

actual no supondría un problema ni de coste ni de peso del equipo

.

1 Referencia en bibliografía 2 Paquete de sensores integrado, mediciones de velocidad y orientación mediante aceleróemtros y giroscopios. Referencia en bibliografía para más información. 3 Sistema aplicado en aviación que permite el aterrizaje en condiciones de nula visibilidad. Formando por dos sistemas independientes, uno para guiado lateral y otro para guiado vertical. Es un sistema eficaz y fiable aunque muy costoso. Vease bibliografía para más información. 4 Sistema de aterrizaje de precisión mediante haces de radar, con la principal ventaja con respecto al sistema ILS de poder definir sendas de planeo curvas. Más información en bibliografía.

E

16

1.1 Contexto

En primer lugar deberíamos definir en qué consiste la orientación de una cámara y como se podría estimar.

La orientación corresponde a la parte geométrica de la transformación de perspectiva entre puntos en el

espacio tridimensional y esos mismos puntos en el espacio bidimensional del plano de la imagen. Con esto

caracterizamos los parámetros extrínsecos del modelo de una cámara, los cuales se componen de la

orientación y de la posición espacial de la cámara con respecto a un sistema de referencia.

Definiremos la orientación mediante 3 ángulos: Roll, Pitch y Yaw (ángulos de balance, cabeceo y guiñada) y

3 coordenadas X, Y, Z que determinan la posición de la cámara en el espacio. El conocimiento de la

orientación es necesario para caracterizar la transformación proyectiva entre los puntos en el mundo real y los

puntos en el plano de imagen de la cámara. Esta transformación también dependerá de una serie de parámetros

dependientes de la cámara en sí (parámetros intrínsecos). El cálculo de estas componentes que caracterizan la

orientación se basará en el conocimiento de las posiciones de 4 puntos del espacio y su transformación al

plano de la imagen. Estos puntos pertenecen a las esquinas de la pista de aterrizaje y son conocidos., mientras

que los puntos en el plan de la cámara corresponden a la posición en la imagen (pixeles) de dichos puntos.

La estimación de orientación u orientación de una cámara es un problema recurrente dentro de la comunidad

de la visión artificial y ha sido abordado desde gran cantidad de formas. La manera quizá más utilizada de

atacar el problema es mediante variaciones del algoritmo DLT (Direct Lineal Transformation)5.

El problema lo afrontaremos como si fuese un problema de optimización, es decir buscando los mínimos de

una función de error que defina la similitud la transformación aplicada frente a la imagen real. Para ello se

comparará la distancia cuadrática entre el mismo punto en la imagen y el punto resultante de la transformación

de proyección mediante la orientación estimada. Se evaluarán distintas maneras de resolver esta estimación,

empezando con un solo parámetro usando puntos sintéticos, para ir subiendo la complejidad al aumentar el

número de parámetros, para finalmente resolver la orientación completa en imágenes reales.

5 Abdel-Aziz, Y. I., & Karara, H. M. (1971). Direct linear transformation from comparator coordinates into object space coordinates in closerange photogrammetry.

2 INTRODUCCIÓN A LA FORMACIÓN DE

IMÁGENES6

Conocer la manera en que se captura y forma una imagen en una cámara es fundamental para entender la

manera en la que una fotografía o un video recogen y plasman la información del mundo exterior. El modelo

más simple que se aplica es el modelo de la cámara estenopéica, el cual es perfectamente aplicable en el caso

de una cámara digital. La cámara estenopeica forma la imagen en un plano tras el estenopo (pequeña cavidad

por la que entra la luz), esta imagen está invertida, ya que los rayos de luz que van desde el punto hasta la

imagen deben pasar necesariamente por el estenopo. Esta condición se llama colinealidad y es la causante de la

inversión de la imagen.

En el proceso de formación de una imagen en una cámara interviene la proyección del mundo tridimensional

en una superficie bidimensional, perdiéndose la información de profundidad.

2.1. Transformación de perspectiva

Un inconveniente de la cámara estenopeica es que las imágenes que produce son relativamente oscuras, ya que

el estenopo deja pasar muy poca luz debido a su reducido tamaño. Para conseguir imágenes más brillantes se

añade una lente, la cual deja pasar una mayor cantidad de luz gracias a su mayor diámetro.

El plano de la imagen lo notaremos como X-Y y el eje perpendicular a ambos en dirección a la lente lo

llamaremos Z. Esta coordenada Z se relaciona con las anteriores mediante la siguiente ecuación:

1

𝑍0+1

𝑍𝑖=1

𝑓 𝐸𝑐𝑢𝑎𝑐𝑖ó𝑛 (2.1)

Donde Z0 es la distancia al objeto, Zi la distancia a la imagen y f es la distancia focal de la lente.

Cuando Z0 > f la imagen se forma invertida en el plano de la imagen en z< -f. La desventaja de añadir una

lente radica en la necesidad de enfocar la imagen, moviendo la lente hasta que se encuentre a una distancia Zi

del plano de la imagen. Esto es así ya que el plano de la imagen se encuentra fijo en la superficie del sensor.

En una cámara estenopeica no es necesario enfocar, pero es un pequeño precio a pagar por el aumento de

luminosidad que se consigue al colocar la lente.

6 La totalidad de este apartado esta basado en el libro de texto Corke 2011, referencia completa en bibliografía.

18

Figura 1: Geometría de formación de imagen con lente fina (tomado de Corke 2011)

El modelo más utilizado es la formación de imagen por perspectiva central, donde los rayos de luz convergen

en el origen del sistema de referencia de la cámara {C} y proyectan una imagen no invertida en el plano de

imagen en Z=f. Mediante semejanza de triángulos se puede demostrar que un punto en coordenadas absolutas

P=(X, Y, Z) es proyectada en el plano de la imagen p=(x, y) de la forma:

𝑥 = 𝑓𝑋

𝑍 , 𝑦 =

𝑌

𝑍 𝐸𝑐𝑢𝑎𝑐𝑖ó𝑛 (2.2)

Figura 2: Modelo de perspectiva central (tomado de Corke 2011)

La ecuación (2.2) caracteriza una trasformación de proyectiva, concretamente la proyección de un objeto en

real en las coordenadas en el plano de la imagen. Presenta las siguientes cualidades:

1. Es una transformación desde el espacio tridimensional al plano bidimensional: R3 -> R

2

2. Las líneas rectas se proyectan como líneas rectas en el plano de la imagen.

3. Las líneas paralelas dejan de ser paralelas y pasan a ser líneas que intersectan en un punto de fuga, en

el caso que estas líneas sean también paralelas al plano de la imagen este fenómeno no ocurrirá.

4. Las curvas cónicas (intersección de un plano con un cono) como las circunferencias o las parábolas

seguirán siendo cónicas. Ej. Una circunferencia se proyectará como una elipse.

5. Esta transformación no es 1:1 y por ello no existe una única transformación inversa, ya que realmente

no se puede conocer la distancia Z entre el punto real y la cámara, únicamente se puede conocer la

recta que une ambos pasando por el centro de la cámara. Esto tiene importantes consecuencias como

la necesidad de tomar dos imágenes distintas del mismo punto para poder conocer la distancia que

separa al punto de la cámara.

6. La transformación de perspectiva no conserva la forma de los objetos al no mantenerse los ángulos

internos del mismo.

Como puede comprobarse, la ecuación (2.2) no es lineal con lo cual será recomendable reescribir los puntos en

el plano de la imagen en coordenadas homogéneas p’= (x’, y’, z’), donde:

𝑥′ = 𝑓𝑋

𝑧′ , 𝑦 =

𝑌

𝑧′, 𝑧′ = 𝑍

20

O en forma matricial compacta

𝑝′ = (𝑓 0 00 𝑓 00 0 1

) (𝑋𝑌𝑍)

Siendo las coordenadas no-homogéneas en el plano de la imagen las siguientes

𝑥 =𝑥′

𝑧′ , 𝑦 =

𝑦′

𝑧′

Siendo CP=(X, Y, Z, 1) T el punto origen del sistema de coordenadas absoluto “mundo”, la transformación

proyectiva puede escribirse en forma lineal de la siguiente manera:

𝑝 = (𝑓 0 00 𝑓 00 0 1

000)C

P o p=CCP

En este caso C es una matriz 3x4 que define los parámetros de la cámara y CP representa el punto con respecto

al sistema de ejes cámara. Es común descomponer la matriz C en 2 factores para distinguir entre parámetros

intrínsecos (cámara en sí) y extrínsecos (caracterizan la transformación entre los ejes mundo y los ejes cámara)

�̃�′ = (𝑓 0 00 𝑓 00 0 1

)(1 0 00 1 00 0 1

000 )

C𝑃 ̃ 𝐸𝑐𝑢𝑎𝑐𝑖ó𝑛 (2.3)

Figura 3: Representación ejes mundo-cámara (tomada de Corke 2011)

El cambio de referencia entre ejes mundo y ejes cámara se hace mediante una matriz de trasformación

homogénea CTO (mundo a cámara) y (

CTO)

-1 (cámara a mundo), quedando la transformación de puntos como

sigue:

CP= 𝑇𝐶−1𝑃 𝐸𝑐𝑢𝑎𝑐𝑖ó𝑛 (2.3)

Donde TC es de la forma 𝑇𝐶 = (𝑅3𝑥3 𝑝3𝑥10 1

) siendo R una matriz de rotación 3x3 entre los ejes de

coordenadas y p un vector de desplazamiento 3x1 entre los orígenes de los sistemas de referencia.

Todo lo anterior se corresponde con los parámetros extrínsecos de la trasformación, ahora pasaremos a los

parámetros dependientes de la cámara en sí: los parámetros intrínsecos.

Una cámara digital se compone de un sensor que está formado por un conjunto de elementos foto-sensibles

(CCD, dispositivo acoplado por carga), que agrupados de 3 en 3 forman los pixeles. La imagen la forman un

número Alto X Ancho de pixeles, agrupados en forma de matriz 2x2. Las coordenadas en pixeles son un

vector de enteros (u,v) donde, por convención, el origen (coordenadas (1,1)) se encuentra en la esquina

superior izquierda de la imagen. Aprovechando que los pixeles son de dimensiones idénticas entre sí y que

están centrados en una matriz regular, podemos relacionar las coordenadas cámara con las coordenadas en

pixeles de la siguiente manera:

𝑢 =𝑥

𝜌𝐻+ 𝑢0, 𝑣 =

𝑦

𝜌𝑤+ 𝑣0 𝐸𝑐𝑢𝑎𝑐𝑖ó𝑛 (2.4)

Donde 𝜌𝐻 y 𝜌𝑤 son la altura y anchura de cada pixel respectivamente y el vector (𝑢0 , 𝑣0) es el punto

principal (coordenadas donde el eje óptico Z de los ejes cámara intersecta con el plano de la imagen.

22

Figura 4: Estructura de sensor de imagen en ejes pixeles (tomada de Corke 2011)

Una vez conozcamos el cambio de ejes cámara en unidades naturales a pixeles, podemos reescribir las

ecuaciones de transformación para que se trabaje en pixeles.

𝑝 = (

1

𝜌𝐻0 𝑢0

01

𝜌𝑤𝑣0

0 0 1

) ∙ (𝑓 0 00 𝑓 00 0 1

000 ) ∙ CP

Donde p= (u’, v’ w’) es el punto en las coordenadas mundo P, transformado a coordenadas pixel. Las

coordenadas no-homogéneas en pixeles son como sigue:

𝑢 =𝑢′

𝑤′ , 𝑣 =

𝑣′

𝑤′

Finalmente obtenemos las ecuaciones de la transformación entre un punto en coordenadas absolutas mundo y

coordenadas en el plano de la imagen en pixeles:

�̃� = (𝑓 𝜌𝐻⁄ 0 𝑢00 𝑓 𝜌𝑤⁄ 𝑣00 0 1

) ∙ (1 0 00 1 00 0 1

000 ) (0TC)-1 C�̃�

Podemos compactar más esta ecuación si nombramos las 2 primeras matrices, las cuales se corresponden con

los parámetros intrínsecos de la cámara, como K y P0.

�̃�= K P0 (0TC)-1 �̃� 𝐸𝑐𝑢𝑎𝑐𝑖ó𝑛 (2.5)

Para finalmente expresarla de manera compacta como: �̃�=C�̃�. Siendo C una matriz 3x4 conocida como

matriz de la cámara.

Queda por definir un parámetro más que afectaría a como la imagen se forma: la distorsión por la lente. La

distorsión geométrica es un efecto realmente problemático al desplazar los puntos en la imagen más allá de lo

que cabría esperar según las ecuaciones de transformación.

Esta distorsión vendrá caracterizada por 2 factores: distorsión radial y tangencial.

Distorsión radial: Se aproxima mediante el polinomio

𝛿𝑟 = 𝑘1𝑟3 + 𝑘2𝑟

5 + 𝑘·3𝑟7 +⋯

Con r siendo la distancia del punto de la imagen al punto principal (𝑢0 , 𝑣0). La distorsión radial se presenta

mayoritariamente de 2 formas: cóncava y biconvexa. La distorsión cóncava ocurre principalmente cuando la

distorsión aumenta con la distancia al punto principal, causando que las líneas rectas cerca del borde de la

imagen se curven hacía el centro. Por otra parte la distorsión biconvexa provoca el mismo efecto, pero

invertido: la distorsión decrece con la distancia y las líneas se curvan hacía fuera.

La distorsión tangencial tiene un efecto menor en la imagen final y ocurre de forma tangencial al radio con el

centro de la imagen.

La coordenada final del punto tras la distorsión queda como sigue:

𝑢𝑑 = 𝑢 + 𝛿𝑢, 𝑣𝑑 = 𝑣 + 𝛿𝑣

Donde los parámetros 𝛿𝑢 y 𝛿𝑣 siguen las ecuaciones que se muestran a continuación:

(𝛿𝑢𝛿𝑣) = (

𝑢(𝑘1𝑟3 + 𝑘2𝑟

5 + 𝑘·3𝑟7 +⋯)

𝑣(𝑘1𝑟2 + 𝑘2𝑟

4 + 𝑘·3𝑟6 +⋯)

)+(2𝑝1𝑢𝑣 + 𝑝2(𝑟

2 + 2𝑢2)

𝑝1(𝑟2 + 2𝑢2) + 2𝑝2𝑢𝑣

) 𝐸𝑐𝑢𝑎𝑐𝑖ó𝑛 (2.6)

Siendo el primer elemento del segundo miembro el responsable de la distorsión radial y el segundo de la

distorsión tangencial.

3 INTRODUCCIÓN A LA ESTIMACIÓN DE

ORIENTACIÓN

La estimación de orientación trata de determinar la transformación homogénea entre el sistema de referencia

de los ejes de la cámara y el sistema de referencia en ejes mundo. En este caso los ejes mundo los colocaremos

en una esquina de la pista de aterrizaje. Para poder estimar la orientación necesitaremos conocer las

dimensiones de esta pista, colocando sobre ella 4 puntos de posición conocida. Los puntos estarán colocados

en las esquinas por simplicidad, ya que serían los más sencillos de determinar de forma automática una vez

esté detectada la pista en la imagen.

El problema lo enfocaremos desde la perspectiva de la optimización, es decir trataremos de obtener la

24

orientación que menor “error” provoque. Se compararán las posiciones de la imagen tomada con el resultado

de la transformación aplicada a los puntos reales medidos, esta comparación se hará mediante la diferencia de

las posiciones de los puntos al cuadrado. Esto será nuestra función a minimizar y una vez se ha encontrado este

mínimo extraeremos la orientación que ha conseguido la menor separación con el punto real.

3.1 Parámetros cámara

Como paso previo antes de poder abordar la estimación de orientación es necesario conocer de la mejor forma

posible la matriz de calibración de la cámara. El modelo utilizado para nuestras pruebas fue la cámara

OV5647, fabricada por la empresa OmniVision Technologies. Esta cámara es especialmente útil para la tarea,

ya que está especialmente diseñada para la integración con Raspberry Pi. El sensor CMOS es de 1/4" y es

capaz de tomar imágenes de hasta 5Mpx. Además la grabación de vídeo alcanza resoluciones de 1920x1080

pixeles a 30 fotogramas por segundo y 640x480 a 90 fotogramas por segundo.

Como procesar imágenes a resoluciones tan altas a esas tasas de fotogramas podrían saturar la capacidad de

procesamiento embarcada, se optó por grabar el vídeo a una resolución de 640x480 a 25 imágenes por

segundo.

EL objetivo fundamental de la calibración es determinar los parámetros intrínsecos que definimos en la

sección anterior. Como disponemos de acceso a la información del fabricante de la cámara por medio de su

datasheet, podremos simplemente extraerlos del mismo.

Los parámetros necesarios son:

1. Distancia focal 𝑓

2. Altura y anchura de cada pixel, 𝜌𝐻 y 𝜌𝑤 respectivamente

3. Posición del punto principal en pixeles (𝑢0 , 𝑣0)

La distancia focal 𝑓 será de 3.6mm y los pixeles serán cuadrados con unas dimensiones de 1.4 x 1.4 μm.

Para determinar la posición del punto principal es necesario conocer la distribución y tamaño del sensor

CMOS. Si recurrimos a la descripción del datasheet obtenemos:

Figura 6: Datasheet del sensor OV5647

Podemos ver que el área de la imagen tiene unas dimensiones de 3670 x 2740 μm y que el punto principal se

encuentra en el centro de este rectángulo. Como las coordenadas las necesitamos en pixeles, solo tendremos

que dividir esta distancia entre el tamaño de cada pixel para tener las coordenadas de la manera deseada. El

punto quedaría de la siguiente manera:

𝑢0 = 3670 2⁄

1.4 = 1310.7 pixeles 𝑣0 =

2740 2⁄

1.4 = 978.57 pixeles

Colocando los parámetros en forma de matriz obtenemos:

𝐾′ = ((3.6)10−3 (1.4)10−6⁄ 0 1310.7

0 (3.6)10−3 (1.4)10−4⁄ 978.570 0 1

)

Sin embargo debemos hacer una pequeña modificación, ya que las imágenes tomadas no son a la resolución

del sensor (2592 x 1944) sino a 640 x 480 pixeles. Este escalado supone una reducción de 4.05 veces con

respecto de la situación original. El cambio de resolución es una transformación homogénea que afecta a los

ejes del plano, sin afectar a la tercera componente. La transformación se representa como:

26

𝑇 = (

6402592⁄ 0 0

0 4801944⁄ 0

0 0 1

)

Esto, aplicado a la matriz original, nos da la matriz de parámetros intrínsecos final, la cual usaremos en

nuestros cálculos.

𝐾 = 𝑇 ∙ 𝐾′

𝐾 =

(

640

25920 0

0480

19440

0 0 1)

∙

(

(3.6)10−3

(1.4)10−40 1310.7

0(3.6)10−3

(1.4)10−4978.57

0 0 1 )

𝐾 = (634.9206 0 323.6332

0 634.9206 241.62260 0 1

)

3.2 Algoritmo de estimación de orientación

Una vez se conoce la matriz de parámetros intrínsecos de la cámara, estamos en situación de acometer el

cálculo de la matriz de parámetros extrínsecos. A continuación explicaremos de qué manera se va a afrontar

este problema.

3.2.1 Definición de variables del problema de optimización

Como se mencionó en apartados anteriores, este problema se va a abordar desde la perspectiva de la

optimización, tratando de minimizar una función de error que describiremos a lo largo de este subapartado.

Nuestro objetivo es obtener la orientación de la cámara, es decir, la matriz de parámetros extrínsecos. Una vez

sea conocida el cálculo de la actitud de la aeronave es sencillo, ya que simplemente habría que transformar

entre los ejes de la cámara y los ejes cuerpo de la aeronave. La matriz de transformación homogénea la

podemos descomponer en 3 matrices de rotación consecutivas (una para cada giro en cada eje de coordenadas)

y un vector de traslación, añadiéndole después la fila extra necesaria para que esté representada de forma

homogénea.

Las matrices de rotación las definiremos individualmente, cada matriz representa un giro en un único eje y la

multiplicación de las 3 define cualquier giro arbitrario.

Matriz de giro en ángulo de balance (Roll, Φ, eje Z de cámara):

𝑅(Φ)=(cosΦ −sinΦ 0sinΦ cosΦ 00 0 1

)

Matriz de giro en ángulo de cabeceo (Pitch, θ, eje X de cámara):

𝑅(θ)=(1 0 00 cosθ − sinθ0 sinθ cos θ

)

Matriz de giro en ángulo de guiñada (Yaw, Ψ, eje Y de cámara):

𝑅(Ψ)=(cosΨ 0 sinΨ0 1 0

− sinΨ 0 cosΨ)

Para obtener la matriz de giro final se multiplican de forma consecutiva en orden XYZ de cámara:

𝑅 = 𝑅(θ) ∙ 𝑅(Ψ) ∙ 𝑅(Φ)

28

28

Por otra parte tenemos el vector de translación entre el origen del sistema de referencia cámara y el origen de

las coordenadas mundo:

𝑝 = (𝑥𝑦𝑧)

Añadiendo la última fila obtendremos la matriz de transformación homogénea final:

𝑇 = (𝑅3𝑥3 𝑝3𝑥10 1

)

Los parámetros que definen la orientación serán los 3 ángulos de rotación y las 3 componentes del vector

translación:

𝑂𝑟𝑖𝑒𝑛𝑡𝑎𝑐𝑖ó𝑛 = 𝑂𝑟𝑖𝑒𝑛𝑡𝑎𝑐𝑖ó𝑛(Φ, θ, Ψ, x , y, z)

Este es nuestro objetivo final y las variables que definen el problema de optimización.

Nuestro objetivo: estimar parámetros que caracterizan esta transformación (θ, φ, Ψ, x, y, z)

Punto proyectado final en coordenadas pixel

Cambio de marco de referencia

mundo-cámaraOw � OC

Punto real P en sistema de referencia natural mundo

Ow

Proyección al plano de la imagen

f

Cambio a coordenadas pixel

αx αy u0 v0

Figura 6: Proceso de formación de imágenes digitales

3.2.2 Definición del problema de optimización

Sea x las posiciones reales de los puntos de la pista en coordenadas del plano de la imagen en pixeles y �̃� la

posición estimada en las mismas coordenadas tras aplicar la transformación proyectiva usando la orientación

resultante de la optimización. Desde este punto en adelante toda coordenada estará expresada en forma de

coordenadas homogéneas.

La función a minimizar será la siguiente:

min𝑂𝑟𝑖𝑒𝑛𝑡𝑎𝑐𝑖ó𝑛

||𝑥 − �̃�(𝑂𝑟𝑖𝑒𝑛𝑡𝑎𝑐𝑖ó𝑛)||2 𝐸𝑐𝑢𝑎𝑐𝑖ó𝑛 (3.1)

Donde �̃�(𝑂𝑟𝑖𝑒𝑛𝑡𝑎𝑐𝑖ó𝑛) se obtiene aplicando la transformación proyectiva entre los ejes pista y los ejes del

plano de la imagen a los puntos reales de la pista. X corresponde a los puntos reales de la pista en coordenadas

homogéneas. x son las coordenadas de los puntos en el plano de la imagen en pixeles.

�̃�(𝑂𝑟𝑖𝑒𝑛𝑡𝑎𝑐𝑖ó𝑛) = 𝐾 ∙ 𝐼3𝑥3 ∙ 𝑇(𝑂𝑟𝑖𝑒𝑛𝑡𝑎𝑐𝑖ó𝑛) ∙ 𝑋 𝐸𝑐𝑢𝑎𝑐𝑖ó𝑛 (3.2)

La orientación �̃� que consiga minimizar este problema será la mejor estimación y la que tomaremos como

orientación de la cámara. Esta orientación luego se traslada a los ejes cuerpo de la aeronave y tendremos tanto

como la posición y orientación (rumbo) de la aeronave con respecto a la pista, como la actitud en forma de

ángulos de Roll y Pitch.

En la página siguiente mostraremos un diagrama del funcionamiento del algoritmo

30

30

Puntos Reales esquinas de la pista en coordenadas homogéneas

Creación de matriz de parámetros intrínsecos

K (f αx αy u0 v0 )

Creación de matriz de parámetros extrínsecos

T (θ, φ, Ψ, x, y, z)

Aplicación de la transformación a los

puntos

Conversión de los puntos obtenidos a coordenadas

homogéneas

Cálculo de la función de error para los puntos

anteriores

¿Se ha obtenido el mínimo de la función

de error?

Variar los parámetros extrínsecos

θ, φ, Ψ, x, y, z

Parámetros extrínsecos representan la orientación de

la cámara

SI

NO

Figura 7: Diagrama del algoritmo a utilizar

4 IMPLEMENTACIÓN DEL ALGORITMO

Ahora que tenemos bien definido y delimitado el algoritmo podemos pasar a implementarlo. Se tomó un

enfoque incremental, comenzando con la búsqueda de un solo ángulo partiendo de imágenes sintéticas, para ir

aumentando la complejidad de forma gradual. Al ir consiguiendo resultados satisfactorios con 1, 2 y 3 ángulos

respectivamente, se pasó a tratar de estimar también la posición. Tras alcanzar soluciones aceptables en con las

imágenes sintéticas se abordó finalmente la estimación de orientación mediante el uso de imágenes reales.

4.1 Descripción entorno de trabajo

4.1.1 Entorno de programación MatLab©

MATLAB© (MATtrix LABoratory) es una herramienta de desarrollo creada por The MathWorks, lanzada

por primera vez en 1984 y ha permanecido en continuo desarrollo desde entonces. En el momento de

redacción de este trabajo la última versión estable es la R2017a.

Esta herramienta es particularmente útil para el desarrollo de nuestra aplicación, ya que dispone de funciones

especialmente diseñadas para el procesamiento de imágenes y el tratamiento de vectores y matrices está

altamente optimizado.

4.1.2 Pista de aterrizaje

La pista de aterrizaje se encuentra a unos 3km al sur de la localidad sevillana de Utrera, separado 1.5km del

aeródromo de la misma, coordenadas GPS L/L 37°09'36.0"N 5°47'13.2"W. Es un pequeño aeródromo

recreativo, con unas medidas reducidas, no siendo apto para el uso de aeronaves tripuladas.

La pista tiene una orientación suroeste, noreste, siendo las posibles pistas activas la pista 06 (orientación

noreste) y la pista 27 (orientación sureste). Las dimensiones son de 150 metros de largo por 12 metros de

ancho.

Sobre la esquina izquierda de la pista 06 colocaremos el sistema de ejes mundo, y con esa referencia

colocaremos los puntos reales de la pista.

32

32

Figura 8: Pista de aterrizaje con puntos superpuestos

Como se puede ver en la figura, los puntos a utilizar serán:

Esquina superior izquierda: A = (0,150 ,0 ,1)

Esquina superior derecha: B = (12,150, 0, 1)

Esquina inferior izquierda: C = (12, 0, 0, 1)

Esquina inferior derecha y origen de coordenadas: D = (0, 0, 0 ,1)

Todos los puntos están expresados en metros y en coordenadas homogéneas

4.1.3 Aeronave

La aeronave utilizada en las pruebas fue la que ensambló el alumno de Ingeniería Aeronáutica Benito

Fernández Rojas en su proyecto final de carrera7, desarrollando un pequeño avión no tripulado utilizando el

modelo Skywalker X-8 como base.

7 Fernández Rojas, D. Benito (2014). Proyecto de fin de carrera, Ingeniería Aeronáutica en la Escuela Técnica Superior de Ingenieros de la Universidad de Sevilla: "Desarrollo de un avión no tripulado: construcción, integración de sistemas y ensayos en vuelo"

Figura 9: Skywalker X-8 en vuelo

La aeronave se ensambló usando los siguientes componentes:

1. Batería: batería tipo LiPo de 5 celdas (18,5V y 5800 mAh) alimentación del motor 2 celdas

(7,4V 2500 mAh) alimentación de sistemas electrónicos.

2. Servomotores para superficies de control y rueda de morro.

3. Controlador electrónico de velocidad ESC.

4. Motor brushless para la hélice.

5. Hélice fija para propulsión.

6. Antenas y receptores para control y telemetría.

7. Ardupilot para el control y monitorización del avión.

8. GPS/magnetómetro para para posicionamiento y rumbo magnético.

9. Tubo de pitot, medida de velocidad mediante presión dinámica del aire.

10. Raspberry Pi, ejecución del algoritmo de detección de la pista.

11. Cámara OV5647 descrita anteriormente

Figura 10: sistemas embarcados

4.2 Puesta en práctica del algoritmo mediante MatLab©

A continuación detallaremos el proceso que hemos seguido para resolver el problema de la estimación de

orientación. Gracias al uso de la herramienta MatLab se ha podido implementar el algoritmo de forma sencilla

y eficaz.

Por claridad de presentación vamos a definir ahora una serie de bloques que se utilizarán varias veces a lo

largo de los siguientes apartados:

34

34

Punto Real de pista coordenadas

homogéneas con referencias ejes mundo

Ow

Creación de matriz de parámetros intrínsecos

K (f αx αy u0 v0 )

Creación de matriz transformación

T(α, β, γ, u, v, w)

Punto transformado a plano de imagen

en coordenadas INHOMOGÉNEAS

Multiplicación de vector de punto por matriz de

transformación T

Parámetros elegidos arbitrariamente por

nosotros

Obtenidos mediante la calibración de la

cámara

Representación de resultado en coordenadas homogéneas

Dividiendo cada vector obtenido entre su tercera

componente

Punto sintético final

Para cada punto de pista A B C D

Leer parámetros α, β, γ, u, v, w

Creación de matrices de Rotación R(α) R(β) R(γ)

Multiplicación de las 3 matrices conscutivamente

T�=(R�|t)Añadir 4º columna con parámetros de vector

t=(u,v,w)T

R�(α,β,γ) =R(α) R(β) R(γ)

Definidas en ecuaciones

Añadir 4º fila con vector (0,0,0,1) a T�

T=K*I*T�

Matriz de Transformación T

final

Multiplicar por la izquierda por matriz calibración K y

matriz identidad 3x3 I

Figura 11: Creación de puntos sintéticos (izquierda) Creación de matriz de Transformación (derecha)

4.2.1 Prueba con 1 ángulo: Pitch

En esta prueba se tratará de implementar el algoritmo únicamente para estimar el ángulo de Pitch, usando para

ello puntos sintéticos. Este enfoque es necesario para poder valorar la efectividad y precisión del algoritmo en

un entorno más controlado. Una imagen real no permitiría aislar el efecto de los demás parámetros, ya que esta

se encuentra en una orientación arbitraria desconocida. Los puntos sintéticos se crearán aplicando una

transformación homogénea a los puntos reales en coordenadas homogéneas mundo. Esta transformación

tendrá una traslación fija (50m hacia arriba en el eje Z) y un giro arbitrariamente elegido en torno al eje X, sin

perjuicio de los demás ejes haciendo los cambios pertinentes. El algoritmo funcionará independientemente del

eje elegido, aunque solo estimará un ángulo de giro.

La transformación también incluirá el término dependiente de los parámetros intrínsecos y se incluirá tanto en

la creación de los puntos sintéticos como a la hora de estimar el ángulo girado. α es el ángulo arbitrario

elegido por nosotros para la transformación.

Matriz de giro: 𝑅(α)=(1 0 00 cos α −sin α0 sinα cos α

) Vector de traslación 𝑡 = (0050)

Matriz de transformación homogénea: 𝑇(α)=(

1 0 00 cos α − sin α00

sinα0

cos α0

0 0

50 1

)

Creación de los puntos sintéticos en coordenadas imagen en pixeles:

𝑝𝑖(α) = 𝐾 ∙ 𝐼3𝑥3 ∙ 𝑇(α) ∙ 𝑃𝑖 𝐸𝑐𝑢𝑎𝑐𝑖ó𝑛 (4.1)

siendo 𝑃𝑖 i : 1,2,3,4 los puntos A,B,C,D de la pista.

Una vez se tienen esos puntos se dividen cada vector entre su tercera componente para que el vector resultante

vuelva a estar representado en coordenadas homogéneas.

Para la búsqueda del mínimo se va abordar el problema mediante fuerza bruta, es decir, se probarán todos los

ángulos posibles entre 0º y 360 º en incrementos de 0.1º. La elección de este tipo de aproximación tiene 2

razones principales. Una de ellas es el testeo del algoritmo utilizado, poder valorar su precisión conociendo en

profundidad todo código, sin margen a resultados inesperados por el uso de funciones preexistentes de

MatLab, las cuales no podemos conocer en profundidad al tratarse de software propietario. La otra es obtener

un resultado predecible al probar todos los valores posibles (dentro de las limitaciones del paso elegido) desde

0º a 360º y poder obtener una representación gráfica de la variación del error frente al ángulo.

La variable elegida será θ. La ecuación que determina la transformación estimada es equivalente a la ecuación

(4.1) con la sustitución de α por θ. El error de cada estimación (cada valor de θ) se calcula mediante la norma

al cuadrado de la diferencia de los puntos:

𝐸𝑟𝑟𝑜𝑟 = ∑||𝑝𝑖(α) − 𝑝𝑖(θ)||2

4

𝑖=1

𝐸𝑐𝑢𝑎𝑐𝑖ó𝑛 (4.2)

El error se almacenará en un vector de longitud (360 − 0)

0.1 ⁄ = 3600 y luego se buscará el mínimo valor

dentro del mismo, una vez se tenga se extrae su posición dentro del vector y se multiplica por el paso (mínimo

incremento del ángulo estimado θ ,0.1 en este caso) y se obtiene el valor de θ estimada. El código utilizado se

encuentra en el ANEXO.

36

36

4.2.2 Prueba con 2 ángulos: Pitch y Roll

Tras la implementación satisfactoria del algoritmo para la estimación de 1 solo ángulo, el paso natural será

extenderlo a 2: Roll y Pitch. En este caso se abordó el problema de la minimización mediante fuerza bruta y

mediante la función preexistente en MatLab fminunc (búsqueda de mínimos sin restricciones).

La razón de esta doble aproximación es: por una parte asegurar el funcionamiento del algoritmo y por otra

verificar que la solución obtenida es la misma en ambos casos. Utilizar funciones preexistentes supone una

gran ventaja en cuanto a la reducción del tiempo de ejecución, ya que las funciones de MatLab están altamente

optimizadas.

El enfoque de fuerza bruta ya se presentaba demasiado lento y engorroso, al requerir de bucles for anidados,

los cuales son muy costosos de ejecutar. Una vez nos hayamos asegurado del correcto funcionamiento de la

función fminunc en nuestro problema, pasaremos a emplearla en el resto de estimaciones. En todo momento se

seguirán empleando los puntos sintéticos.

4.2.2.1 Caso 1: fuerza bruta

En esta prueba añadiremos otro elemento a la transformación homogénea: una matriz de rotación extra. Esta

matriz caracterizará el giro con respecto del ángulo de Roll (con respecto a eje Z de cámara). La matriz de

transformación final concatenará ambas rotaciones y una traslación fija y conocida. Para la creación de los

puntos sintéticos emplearemos los parámetros (elegidos por nosotros) α y β.

La extensión de 1 a 2 variables requerirá de realizar 2 barridos completos en los ángulos, será necesario

recorrer todos los valores de un ángulo para cada valor del otro, resultando en una estructura anidada.

Matrices de giro: 𝑅(α)=(1 0 00 cos α −sin α0 sinα cos α

) 𝑅(β)=(cosβ − sin β 0sinβ cosβ 00 0 0

)

Vector de traslación 𝑡 = (0050)

Matriz de rotación: 𝑅′(α, β)= 𝑅(α) ∙ 𝑅(β) = (1 0 00 cos α − sinα0 sin α cos α

) ∙ (cos β − sinβ 0sinβ cos β 00 0 0

)

= (

cos β −sin β 0sinβ cos α cos β cosα −sin αsinβ sin α cosβ sinα cos α

)

Matriz de transformación homogénea:

𝑇(α, β)=(

cos β − sinβ 0sinβ cos α cos β cos α − sin αsinβ sinα

0cos β sinα

0

cos α0

0 0

50 1

)


𝑝𝑖(α, β) = 𝐾 ∙ 𝐼3𝑥3 ∙ 𝑇(α, β) ∙ 𝑃𝑖 𝐸𝑐𝑢𝑎𝑐𝑖ó𝑛(4.3)


Como en el apartado anterior, pasaremos los puntos a coordenadas homogéneas al dividir cada vector entre su

tercera componente. El barrido en ambos ángulos será entre 0º y 360º con un paso de 0.1º. Se usará la

ecuación (4.3) para estimar los ángulos de Pitch (θ) y Roll (Φ), sin más que sustituir α y β por θ y Φ

respectivamente. Una vez se obtengan se aplica la ecuación siguiente para obtener el error de estimación para

esos valores de θ y Φ.

𝐸𝑟𝑟𝑜𝑟 = ∑||𝑝𝑖(α, β) − 𝑝𝑖(θ,Φ)||2

4

𝑖=1

𝐸𝑐𝑢𝑎𝑐𝑖ó𝑛(4.4)

El error se almacenará en una matriz de dimensiones 3600x3600 para buscar mínimo valor dentro esta matriz,

una vez se tenga se extrae su posición dentro del vector y se multiplica por el paso (mínimo incremento del

ángulo estimado 0.1 en este caso) en cada variable y se obtienen los valores estimados de los ángulos θ y Φ. El código utilizado se encuentra en el ANEXO.

4.2.2.2 Caso 2: función de MatLab fminunc

El enfoque empleado en el apartado anterior ha resultado en un enorme incremento del número de operaciones

necesarias. Esto ha dado lugar a un código lento de ejecutarse, muy dependiente de los límites de búsqueda

establecidos, por ello se decidió alterar el algoritmo para que pudiese ser abordado mediante la función de

MatLab fminunc.

La función fminunc nos permite obtener el mínimo sin restricciones de una función matemática de varias

variables, haciendo el nombre alusión a su funcionamiento (Find minimum of unconstrained multivariable

function → f (ind)min (imum)unc (constrained)). Esta función intenta obtener el mínimo de una función

escalar partiendo de una estimación inicial, problema comúnmente llamado optimización no lineal sin

38

38

restricciones.

En esencia son las mismas ecuaciones y la misma manera de trabajar que en el apartado anterior, con lo cual

no nos extenderemos aquí en exceso. Se adecuó el código para poder emplear la función de MatLab fminunc

y se probaron sus resultados, resultando en una ejecución mucho más rápida y no dependiente de los límites de

búsqueda elegidos. Además se obtuvieron buenos resultados, validando así su uso futuro.

4.2.3 Prueba con 3 ángulos: Pitch, Roll y Yaw

Tras evaluar la validez y precisión del algoritmo para 2 ángulos, nos embarcamos en la tarea de tratar de

resolver la orientación angular completa con los 3 ángulos: Roll, Pitch y Yaw.

Cómo ya hemos validado la correcta funcionalidad de la función fminunc, podremos evitar el uso de fuerza

bruta. Debido al enorme volumen de datos a tratar en el caso de los 3 ángulos este enfoque sería altamente

ineficiente, ya que tendríamos una estructura triplemente anidada. Sería necesario que para cada valor de uno

de los ángulos se probaran todos y cada uno de los valores de los otros dos, resultando en un número de

iteraciones realmente elevado, del orden de 36003.

Una vez más, se utilizarán puntos sintéticos para estimar la orientación de la cámara mediante nuestro

algoritmo. Estos puntos se crearán usando el mismo método que en los apartados anteriores, con la salvedad de

tener que añadir una tercera matriz de rotación.

Matrices de giro:

𝑅(α)=(1 0 00 cos α − sinα0 sin α cos α

) 𝑅(β)=(cos β −sin β 0sinβ cos β 00 0 0

) 𝑅(γ)=(cos γ 0 sin γ0 1 0

− sin γ 0 cos γ)

Vector de traslación 𝑡 = (0050)

Matriz de rotación:

𝑅′(α, β, γ)= 𝑅(α) ∙ 𝑅(β) ∙ 𝑅(γ) =

(1 0 00 cos α − sinα0 sin α cos α

) ∙ (cos β − sinβ 0sin β cos β 00 0 0

) ∙ (cos γ 0 sin γ0 1 0

− sin γ 0 cos γ) =

(

cos γ cos β − sinβ sin γ cos βsin γ sin β + cos γ sin β cos α cos β cos α sin γ sin β cos α − cos γ sinαcos γ sinα sin β − sin γ cos α cos β sin α cos γ cos α + sin γ sinβ sinα

)


𝑇(α, β, γ)=

(

cosγ cos β − sinβ 0 sin γ cos βsin γ sinβ + cos γ sin β cosα cos β cos α sin γ sinβ cos α − cos γ sin αcos γ sin α sinβ − sin γ cosα

0cos β sinα

0cos γ cos α + sin γ sin β sinα

0

0 0

50 1

)


𝑝𝑖(α, β, γ) = 𝐾 ∙ 𝐼3𝑥3 ∙ 𝑇(α, β, γ) ∙ 𝑃𝑖 𝐸𝑐𝑢𝑎𝑐𝑖ó𝑛(4.5)


Cómo en anteriores ocasiones, los puntos deben estar representados en coordenadas homogéneas para que el

algoritmo funcione correctamente. Esto se consigue sin más que dividir cada vector entre su tercera

componente. Una vez estén definidas todas las transformaciones, podremos “poner a trabajar” a fminunc

obteniendo el resultado de la minimización.

𝐸𝑟𝑟𝑜𝑟 = ∑||𝑝𝑖(α, β, γ) − 𝑝𝑖(θ,Φ,Ψ))||2

4

𝑖=1


4.2.4 Prueba de orientación completa: Pitch, Roll, Yaw y vector x, y, z

Nos encontramos finalmente ante el problema de estimar la orientación completa, es decir los 6 parámetros

extrínsecos que definen la transformación proyectiva que ocurre al formarse una imagen en una cámara.

Será la última ocasión en la que se emplearán puntos sintéticos, siendo necesarios ahora para poder valorar la

eficacia del algoritmo sin tener que afrontar problemas de ruido o de imprecisión a la hora de extraer los

puntos de la imagen.

El enfoque se mantendrá como en los apartados anteriores: se transforman los puntos usando parámetros

arbitrarios conocidos, para luego estimar esa transformación efectuada mediante el uso de nuestro algoritmo.

También será necesario evaluar la estabilidad y convergencia del mismo, ya que hemos doblado el número de

variables a estimar (de 3 ángulos a 3 ángulos y 3 componentes del vector desplazamiento).

La función de MatLab fminunc nos ha dado muy buenos resultados en los apartados anteriores, pero ahora

necesita de bastantes más iteraciones para converger y es más susceptible a los valores iniciales de los

parámetros. Añadiremos otra prueba extra utilizando una función más adecuada a nuestro caso: la función

lsqnonlin.

40

40

lsqnonlin está especialmente diseñada para minimizar funciones consistentes en la suma de cuadrados, es decir

problemas de identificación no lineales mediante el uso de mínimos cuadrados. Demostrará ser más rápida y

precisa que fminunc, con lo cual será la función que usaremos a la hora de resolver el problema de la

estimación de orientación utilizando imágenes reales.

La creación de puntos sintéticos utilizará las mismas ecuaciones que el apartado anterior para las matrices de

rotación, y simplemente añadirá las variables u, v, w para caracterizar el desplazamiento de los ejes en la

transformación.


𝑇(α, β, γ, u, v, w)=

(

cos γ cos β − sinβ sin γ cos βsin γ sinβ + cos γ sinβ cos α cos β cos α sin γ sin β cos α − cos γ sin αcos γ sin α sinβ − sin γ cosα

0cos β sin α

0cos γ cos α + sin γ sin β sinα

0

𝑢 𝑣

𝑤 1

)


𝑝𝑖(α, β, γ, u, v, w) = 𝐾 ∙ 𝐼3𝑥3 ∙ 𝑇(α, β, γ, u, v, w) ∙ 𝑃𝑖 𝐸𝑐𝑢𝑎𝑐𝑖ó𝑛(4.7)


Se representan los puntos obtenidos en coordenadas homogéneas como se ha hecho anteriormente. La función

a minimizar por fminunc y lsqnonlin será la siguiente:

𝐸𝑟𝑟𝑜𝑟 = ∑||𝑝𝑖(α, β, γ, u, v, w) − 𝑝𝑖(θ,Φ,Ψ, x, y, z))||2

4

𝑖=1


4.2.5 Aplicación a una imagen real

Las pruebas demostraron el correcto funcionamiento y la estabilidad del algoritmo8, con lo cual podemos

pasar a emplearlo en una imagen real. Aunque la idea fundamental es que todo el proceso se ejecute de forma

automática, es decir extrayendo los puntos de las esquinas directamente desde el video, en este caso

utilizaremos una imagen extraída de forma manual. La razón es simple, evitar cualquier incertidumbre

causada por la posible imprecisión en la extracción de los puntos, asegurándonos de la exactitud de la posición.

8 Vease apartado posterior: Puesta en práctica y generación de resultados.

Al tratarse de una imagen real, el método para resolver la estimación de la orientación es algo diferente. En

este caso no se crearán puntos sintéticos para el cálculo de la función del error, sino que se emplearán los

puntos (en pixeles) extraídos de la imagen para compararlos con los obtenidos tras aplicar la transformación

proyectiva.

Figura 12: imagen utilizada en prueba con puntos superpuestos

Con la posición de los puntos ya conocida, tendremos una referencia para comprobar el resultado de la

transformación y obtener el error de la orientación utilizada frente a la real. El concepto se mantiene con

respecto a las anteriores pruebas, encontrar unos valores de los parámetros extrínsecos que minimicen el error

de posición.

Una vez se obtenga la orientación se comprobará el resultado mediante otra función que se encargará de

aplicar la transformación proyectiva usando esa orientación, para luego superponerla sobre la imagen real y

valorar el resultado obtenido. Esta función se llamará PruebaPose9 y la definiremos más adelante.

9 Código en anexo

42

42

En el siguiente diagrama se ilustra la manera de proceder.

Creación de matriz de puntos de esquinas reales

Creación de matriz de parámetros extrínsecos

T (θ, φ, Ψ, x, y, z)

Aplicación de transformación a

puntos reales

Calculo de error entre resultado de

transformación y punto de imagen

¿Es un mínimo?

Orientación de la cámara

obtenida

Variar los parámetros extrínsecos

θ, φ, Ψ, x, y, z

Añadiendo a la matriz de transformación T la matriz de parámetros

de cámara K

Puntos en coordenadas mundo, dimensiones extraídas mediante

foto satélite

Aplicando función de error eq:

Extracción de puntos A B C D desde imagen en coordenadas pixel

homogéneas

NO

SI

Figura 13: Procedimiento de estimación de orientación para imágenes reales

Como en anteriores ocasiones, el vector de parámetros que define la orientación vendrá caracterizado por las

variables θ,Φ,Ψ, x, y, z. θ representa el ángulo de Pitch (cabeceo), Φ el ángulo de Roll (balance) Ψ el

ángulo de Yaw (guiñada) y x, y, z el desplazamiento.

Los parámetros representarán el cambio que hay que realizar para pasar desde el sistema de ejes cámara OC al

sistema de ejes mundo OW.

Matriz de transformación homogénea que modela este cambio de ejes de referencia:

𝑇(θ,Φ,Ψ, x, y, z)=

(

cosΨ cosΦ −sinΦ sinΨ cosΦsinΨ sinΦ + cosΨ sinΦ cos θ cosΦ cosα sinΨ sinΦ cosθ − cosΨ sinθcosΨ sinθ sinΦ − sinΨ cos α

0cosΦ sinθ

0cosΨ cos θ + sinΨ sinΦ sin θ

0

𝑥 𝑦

𝑧1

)

Las coordenadas de puntos reales de la pista obtenidos mediante imagen satélite se refirieron a un sistema de

coordenadas ubicado sobre la propia pista10

. Es necesario referirlos al sistema de ejes cámara OC

para luego

aplicar la transformación de coordenadas cámara a coordenadas pixel. La conversión entre sistemas se realiza

mediante la siguiente ecuación:

CP= 𝑇−1𝑃 𝐸𝑐𝑢𝑎𝑐𝑖ó𝑛 (4.9)

Dónde 𝑇−1 es la inversa de la matriz de transformación anterior T, P el punto referido a los ejes mundo y CP referido a los ejes cámara. Tras esta necesaria conversión ya estamos en condiciones de aplicar la matriz

intrínseca de la cámara K y obtener los puntos transformados en ejes cámara en pixeles. Por último, se divide

cada vector resultante entre su tercera componente para que estén representados mediante coordenadas

homogéneas.

Es importante añadir que este cambio de marco de referencia se debe realizar para cada valor de

θ,Φ,Ψ, x, y, z.

Ahora sí estamos en situación de aplicar la función de error.

Ecuación que define el paso de unidades naturales a pixeles:

�̃�𝑖 = 𝐾 ∙ 𝐼3𝑥3 ∙ 𝐶 𝑃𝑖 𝐸𝑐𝑢𝑎𝑐𝑖ó𝑛 (4.10)

Siendo C𝑃𝑖 i : 1,2,3,4 los puntos A,B,C,D de la pista en referencia a los ejes cámara y �̃�𝑖 los puntos

estimados en coordenadas pixel para esos valores de θ,Φ,Ψ, x, y, z

En este caso solo utilizaremos la función de MatLab lsqnonlin para minimizar la función de error.

𝐸𝑟𝑟𝑜𝑟 = ∑||𝑝𝑖𝑚𝑎𝑔𝑒𝑛 𝑖 − �̃�𝑖(θ,Φ, Ψ, x, y, z)||2

4

𝑖=1

𝐸𝑐𝑢𝑎𝑐𝑖ó𝑛 (4.11)

En los anteriores apartados para validar el resultado era suficiente con comparar el valor de aplicado en la

creación de los puntos sintéticos con el resultado del programa. En este caso será necesario validar mediante

10 Se estableció un sistema de referencia cartesiano OXYZ, donde el eje X es paralelo al eje transversal, creciente hacia la derecha, el eje Y creciente con el eje longitudinal de la pista y el eje Z perpendicular a ambos. Se centró en el punto C mostrado en la Figura 8

44

44

un algoritmo extra, el cuál superpondrá el resultado de la transformación a la imagen inicial.

Figura 14: Ejemplo de resultado de estimación

En la figura superior podemos ver el resultado final del algoritmo; las cruces rojas son las esquinas detectadas

en la imagen, círculos azules la posición esperada de la pista tras transformar usando la orientación estimada.

La construcción de los puntos azules se realizó mediante las ecuaciones de transformación proyectiva,

ecuación (4.7).

5 PUESTA EN PRÁCTICA Y GENERACIÓN DE

RESULTADOS

continuación se realizarán una serie de ejecuciones del algoritmo en cada una de las situaciones descritas

anteriormente, tratando de validar el correcto funcionamiento y precisión del mismo. Mantendremos el

mismo orden que en el bloque anterior, comenzando por la estimación de 1 solo ángulo y cerrando con la

orientación completa en una imagen real.

5.1 Ángulo de Pitch

Para probar el funcionamiento para este caso simple, se realizará una ejecución en bucle del algoritmo,

variando el giro en cada iteración, viendo cómo responde el algoritmo a los distintos valores del ángulo

introducido. Por simplicidad mostraremos todos los ángulos probados en una tabla y gráficas de la evolución

del error en algunos de ellos.

Como parámetros la función recibe los límites inferior y superior de búsqueda (recordemos que funciona

mediante fuerza bruta) y el ángulo arbitrario para la creación de los puntos sintéticos. Estos ángulos se

eligieron de forma aleatoria mediante el uso de la función de MatLab rand, la cual devuelve valores aleatorios

entre 0 y 1, multiplicando por 360 tendremos valores aleatorios del ángulo. A continuación se muestra una

tabla con los resultados de la prueba:

Ángulo introducido (º) Ángulo estimado (º) Ángulo introducido (º) 2Ángulo estimado (º)

27,348 27,3 145,408 145,4

86,370 86,4 34,724 34,7

44,395 44,4 47,510 47,5

66,207 66,2 339,138 339,1

86,383 86,4 344,208 344,2

150,216 150,2 207,075 207,1

17,876 17,9 21,521 21,5

324,978 325 84,521 84,5

340,123 340,1 127,137 127,1

176,711 176,7 295,630 295,6

176,131 176,1 5,545 5,5

121,579 121,6 15,489 15,5

324,019 324 60,836 60,8

132,929 132,9 233,682 233,7

40,033 40 263,420 263,4

280,891 280,9 233,189 233,2

140,306 140,3 162,333 162,3

Tabla 1: prueba de validación de algoritmo para un ángulo mediante fuerza bruta

A la vista de los resultados podemos concluir que el algoritmo hace muy buen trabajo, no alcanzando la

precisión exacta por la elección del paso. Un paso más pequeño nos permitiría aumentar la precisión a costa de

un mayor coste computacional. No obstante esto no será necesario porque hemos considerado que tenemos

precisión suficiente al calcular hasta la décima de grado.

Mostraremos además una gráfica de la evolución de la función de error frente al ángulo en 2 casos concretos:

para un ángulo de 150,2º , para 15,5º y para un ángulo de 199,1º ya que es un punto de particular interés al

sufrir un cambio de pendiente muy significativo

A

46

46

Figura 15: evolución de la función de error para 150,2º

Figura 16: ampliación del error en el entorno del mínimo para 150,2º

48

48



Es importante destacar que en ambos casos hay que restar un entero al valor de iteración para el ángulo ya que,

los vectores en MatLab tienen su primera componente en el índice 1, en vez de en el 0. Además hay que

dividir entre 10, al ser el paso 0.1.

Podemos observar la tendencia decreciente en torno a los mínimos del error, demostrando la convergencia del

algoritmo a los mínimos de la función incluso en el entorno de “inestabilidad numérica” de 199,1º.

5.2 Ángulos de Pitch y Roll

A continuación mostraremos los resultados del algoritmo para el cálculo de 2 ángulos tanto para el método de

fuerza bruta como con la función de MatLab fminunc.

5.2.1 Fuerza bruta

Se procederá de una manera similar al apartado anterior, probando valores aleatorios de los ángulos de entrada.

Será en esta prueba donde quede patente la lentitud de este enfoque ya que requirió de varios minutos poder

generar los datos del funcionamiento del mismo. Para generar los datos se creó un bucle y se probaron los

valores aleatorios nombrados.

Será tras esta validación cuando abandonemos definitivamente el concepto de fuerza bruta, ya que resultará

enormemente costos computacionalmente de solucionar. Para poder obtener todos los valores ha sido

necesaria un barrido desde 0º a 360º en ambos ángulos, además de 36 repeticiones para valorar la eficacia del

algoritmo. Todo esto ha resultado en un número de operaciones muy grandes, del orden de 36002

x 36 =

466560000, necesitando de varias horas para poder realizar todo el barrido.

Ángulo PITCH Ángulo PITCH Ángulo PITCH Ángulo PITCH Ángulo PITCH Ángulo PITCH

introducido (º) estimado (º) introducido (º)estimado (º) introducido (º)estimado (º)

293,14 293,10 330,19 330,20 204,78 204,80

87,67 87,70 102,90 102,90 168,98 169,00

334,53 334,50 272,59 272,60 4,28 4,30

125,99 126,00 271,34 271,30 121,36 121,40

70,77 70,80 136,96 137,00 58,39 58,40

90,39 90,40 204,42 204,40 285,94 285,90

221,78 221,80 27,31 27,30 112,04 112,00

170,38 170,40 19,42 19,40 190,27 190,30

126,60 126,60 191,09 191,10 59,63 59,60

299,10 299,10 280,50 280,50 216,71 216,70

210,70 210,70 336,24 336,20 94,67 94,70

197,90 197,90 46,77 46,80 235,47 235,50

Tabla 2: Resultados del algoritmo fuerza bruta para ángulo de PITCH

50

50

Ángulo ROLL Ángulo ROLL Ángulo ROLL Ángulo ROLL Ángulo ROLL Ángulo ROLL

introducido (º) estimado (º) introducido (º) estimado (º) introducido (º) estimado (º)

248,12 248,20 38,39 38,50 65,46 66,20

269,33 269,30 346,28 346,30 94,97 94,90

162,19 162,60 1,67 1,70 52,39 52,60

30,18 30,20 278,97 279,00 48,98 49,00

82,43 82,40 294,23 294,20 312,95 313,00

328,80 328,80 312,73 312,50 208,69 208,80

54,86 55,20 30,40 30,40 197,95 198,00

297,29 297,40 143,92 144,10 52,18 52,70

193,80 193,80 93,55 93,40 307,09 307,10

358,61 358,60 288,02 288,00 223,94 224,10

28,14 28,20 155,31 155,90 126,34 126,30

159,36 159,40 327,83 327,90 184,77 184,50

Tabla 3: Resultados del algoritmo fuerza bruta para ángulo de ROLL

Los resultados demuestran un buen resultado, obteniendo un valor realmente próximo al valor introducido, con

variaciones menores en casi todos los casos a 0,1º, el cual es el valor del paso elegido. Si se hubiera optado por

un paso menor los resultados hubieran sido marginalmente menores, a costa de aún más tiempo de

computación, con lo cual se optó por mantener ese valor para el paso. Una vez más se valida el

funcionamiento del algoritmo, quedando la tarea de tratar de optimizar su funcionamiento. Para ello

introducimos la función fminunc en el apartado siguiente.

5.2.2 Función MatLab fminunc

El método de la fuerza bruta demostró ser extremadamente ineficiente, necesitando de varios minutos para

resolver 1 de las 36 iteraciones que se precisaron para obtener los datos. Gracias a la función fminunc pudimos

resolver todas las iteraciones en menos de 2 segundos. Los valores de la transformación para la creación de

los puntos sintéticos se obtuvieron de la misma forma que antes, consiguiendo valores aleatorios entre 0º y

360º mediante la función rand. Ahora mostraremos una tabla con los valores obtenidos de los ángulos.



42,92 42,92 38,09 38,09 49,99 49,99

338,34 197,25 39,49 39,49 250,66 58,22

232,40 27,91 22,89 22,89 33,78 33,78

172,61 172,61 145,65 145,65 189,15 189,15

230,15 16,84 161,41 161,41 190,92 332,34

196,10 196,10 131,69 131,69 310,01 161,40

233,03 26,07 274,86 94,91 174,55 174,55

195,80 -14,29 226,04 12,18 141,64 141,64

259,58 71,50 277,91 103,47 241,72 38,77

188,10 188,35 335,83 914,63 266,85 88,81

357,73 -2,27 350,19 -9,81 187,22 187,22

78,72 78,72 69,13 69,13 125,18 125,18

Tabla 4: Resultados obtenidos en la estimación de ángulo ROLL para valores iniciales de roll =10º pitch = 15º



330,27 -29,73 244,45 -115,55 126,08 126,08

96,86 88,95 228,88 -131,12 238,32 116,86

275,58 85,65 340,26 -19,74 149,82 149,82

67,92 67,92 75,22 75,22 303,09 -56,91

103,50 94,72 255,34 -104,66 299,85 -123,15

32,80 32,80 85,04 85,04 92,32 91,84

207,44 -223,62 42,98 -45,87 220,85 -139,15

246,01 84,99 218,63 -116,12 209,61 209,61

196,77 160,78 162,05 200,12 194,67 153,86

153,26 41,44 165,14 16,05 313,18 49,68

232,00 -128,00 238,30 -121,70 95,32 95,32

233,14 -126,86 277,30 -82,70 114,51 114,51

Tabla 5: Resultados obtenidos en la estimación de ángulo PITCH para valores iniciales de roll =10º pitch = 15º



42,92 42,92 38,09 38,09 49,99 49,99

338,34 197,25 39,49 39,49 250,66 58,22

232,40 27,91 22,89 22,89 33,78 33,78

172,61 172,61 145,65 145,65 189,15 189,15

230,15 16,84 161,41 161,41 190,92 190,92

196,10 556,10 131,69 131,69 310,01 161,40

233,03 26,07 274,86 94,91 174,55 174,55

195,80 -734,29 226,04 12,18 141,64 141,64

259,58 71,50 277,91 103,47 241,72 38,77

188,10 188,10 335,83 194,64 266,85 88,81

357,73 -362,27 350,19 -729,81 187,22 187,22

78,72 78,72 69,13 69,13 125,18 125,18

Tabla 6: Resultados para estimación de ángulo ROLL para valores iniciales de roll =100º pitch = 150º

52

52



330,27 -29,73 244,45 244,45 126,08 126,08

96,86 88,95 228,88 228,88 238,32 116,86

275,58 85,64 340,26 -19,74 149,82 149,82

67,92 67,92 75,22 75,22 303,09 663,09

103,50 94,72 255,34 255,34 299,85 299,85

32,80 145,19 85,04 85,04 92,32 91,84

207,44 136,38 42,98 314,14 220,85 220,85

246,01 85,00 218,63 243,88 209,61 209,61

196,77 160,78 162,05 200,12 194,67 153,86

153,26 153,26 165,14 162,22 313,18 49,68

232,00 232,00 238,30 238,30 95,32 95,32

233,14 233,14 277,30 277,30 114,51 114,51

Tabla 7: Resultados para estimación de ángulo PITCH para valores iniciales de roll =100º pitch = 150º

A la vista de los datos podemos decir que los resultados del algoritmo son consistentes, alcanzando el valor

introducido en una gran cantidad de casos. En más de una situación ha estimado correctamente el valor pero en

la dirección contraria, es decir, el algoritmo devuelve un valor de -29,73º cuando se le había introducido

330,27º. Esto es correcto ya que 360-29,73=330,27 º.

Bien es cierto que demos puntualizar los casos en los que no ha devuelto el valor correcto, siendo siempre en

valores muy alejados de los valores iniciales. Esto se debe fundamentalmente a la manera de funcionar de

fminunc: la función ha encontrado un mínimo local en esa posición y como es el mínimo más cercano a los

valores iniciales lo acepta como válido. Esto podría solucionarse de 2 maneras:

La primera sería tratar de estimar un valor inicial mediante la misma imagen de la pista, contabilizando la

relación entre el número de pixeles de cielo con respecto a la imagen para el valor de Pitch. El valor de Roll se

podría estimar mediante el ángulo que forma la línea del horizonte. Para el ángulo de Yaw, se podría estimar

una orientación con respecto de la pista, viendo la relación entre los momentos horizontales y verticales de la

pista en la imagen

Otra solución es situar los valores iniciales en torno a los valores normales de vuelo, como 0º para roll y pitch,

ya que, en circunstancias normales no vamos a salir del entorno de ± 20º para Pitch y ± 40º para Roll. Paliando

casi completamente el problema de estimar valores alejados de las condiciones iniciales.

5.3 3 ángulos: Roll Pitch y Yaw

Abordaremos en este apartado por primera vez la orientación angular completa: Roll Pitch y Yaw. Se abordará

únicamente mediante la función fminunc, generando los resultados y mostrándolos en una tabla. Volveremos a

encontrarnos con el problema de la imprecisión para valores alejados de las condiciones iniciales de búsqueda.

Esta prueba se hará mediante la utilización de puntos sintéticos, creándolos de la forma mostrada en el bloque

anterior (ecuación (4.7)) Fminunc demuestra su optimización al no necesitar más que 2,4 segundos para

resolver las 36 iteraciones de la prueba.

Se realizarán 2 pruebas con valores iniciales diferentes para poder cerciorarnos de que los valores no

acertados se deban a la lejanía con estos en vez de a un fallo del algoritmo. Además limitaremos los datos

introducidos a estados coherentes con la actitud de vuelo: ± 45º para PITCH, ± 60º para ROLL y ± 100º para

YAW. Es importante remarcar que hemos llamado YAW al ángulo que forman la dirección de vuelo del avión

y el eje longitudinal de la pista, siendo más cercano a una orientación que al ángulo de guiñada de una

aeronave.

Los resultados extraídos son los siguientes:



-11,78 -11,78 -19,47 -14,75 9,03 9,03

-50,88 32,98 48,01 48,01 -52,83 21,80

-31,21 -5,89 -15,69 -15,69 -31,83 -6,56

-45,20 5,96 -46,66 21,04 -17,62 -17,62

-37,93 -0,37 33,63 33,63 38,54 38,54

-31,21 -3,71 -13,23 -13,23 -58,15 52,97

-9,93 -9,93 -31,00 -7,55 -54,84 41,03

-54,04 46,73 -11,53 -11,53 -39,72 0,70

48,33 48,33 -48,43 12,89 17,89 17,89

53,37 53,37 -44,16 14,81 27,81 27,81

-1,10 -1,10 53,05 53,05 17,73 17,73

-1,29 -1,29 54,74 54,74 -5,89 -5,89

Tabla 8: Resultados para estimación de ángulo ROLL para valores iniciales de roll = 0º pitch = 0º yaw = 0º

54

54

Ángulo YAW Ángulo YAW Ángulo YAW Ángulo YAW Ángulo YAW Ángulo YAW


4,70 4,70 -1,32 -177,39 -14,93 -14,93

-20,37 168,23 -6,41 -6,41 43,90 -126,16

24,47 -153,63 -5,32 -5,32 37,59 -139,30

-31,10 158,19 -19,37 166,45 5,02 5,02

18,68 -156,04 0,85 0,85 12,25 12,25

-31,65 149,66 1,08 1,08 8,70 -158,93

-13,15 -13,15 31,76 -145,33 -29,23 160,89

12,56 -154,97 29,48 29,48 -19,88 166,69

28,02 28,02 14,43 -156,98 -2,91 -2,91

-41,89 -41,89 -12,14 349,09 -26,95 -26,95

42,94 42,94 31,16 31,16 34,43 34,43

27,57 27,57 3,28 3,28 -30,52 -30,52

Tabla 9: Resultados para estimación de ángulo YAW para valores iniciales de roll = 0º pitch = 0º yaw = 0º

A la vista de los resultados se puede ver una fuerte tendencia hacia la imprecisión de los puntos más alejados

de los valores iniciales, teniendo las mejores estimaciones en el ángulo más acotado: PITCH.

En ROLL se puede observar la misma tendencia, perdiendo exactitud las mediciones según se aleja el valor

introducido de las condiciones iniciales. Queda patente el aumento de la dificultad de encontrar mínimos

absolutos según aumenta el número de variables, esto es realmente un resultado esperado ya que la cantidad de

mínimos locales crece de forma positiva con el número de variables. El ángulo de YAW exhibe el mismo

comportamiento que ROLL y PITCH.



2,54 2,54 -48,15 12,43 46,91 46,91

-32,21 -6,11 -28,58 -8,61 -19,90 -18,94

-1,33 -1,33 -19,76 -17,11 23,85 23,85

14,89 14,89 21,57 21,57 -36,26 -29,76

21,50 21,50 -43,61 10,35 -56,34 56,98

-12,54 -12,54 26,55 26,55 29,29 29,29

-15,91 -15,91 -47,19 10,50 0,00 0,00

58,56 58,56 18,45 18,45 -2,41 -2,41

-55,47 53,52 -0,70 -0,70 48,57 48,57

46,22 46,22 33,49 33,49 13,18 13,18

49,59 49,59 25,80 25,80 14,12 14,12

35,54 35,54 48,45 48,45 43,13 43,13

Tabla 10: Resultados para estimación de ángulo ROLL para valores iniciales de roll =0º pitch = -10º yaw = 30º



-24,67 -24,67 -21,77 14,79 -37,30 -37,30

-29,64 20,14 -8,22 181,40 -21,38 -9,48

-24,51 -24,51 8,54 -8,55 27,09 27,09

-5,79 -5,79 -21,40 -21,40 -42,37 186,74

-17,00 -17,00 9,26 -23,07 38,60 -49,05

38,10 38,10 19,01 19,01 20,73 20,73

-6,28 -6,28 -25,04 17,62 -1,03 -1,03

-28,37 -28,37 -34,43 -34,43 7,07 7,07

36,44 -49,15 -18,30 -18,30 -23,64 -23,64

43,18 43,18 -16,31 -16,31 -3,70 -3,70

-5,50 -5,50 -6,82 -6,82 41,68 41,68

-35,00 -35,00 0,71 0,71 4,21 4,21

Tabla 11: Resultados para estimación de ángulo PITCH para valores iniciales de roll =0º pitch = -10º yaw = 30º



30,55 30,55 -44,04 144,48 47,30 47,30

7,67 190,87 18,20 384,35 14,90 28,69

-31,71 -31,71 -45,76 134,24 30,03 30,03

-26,01 -26,01 -42,86 -42,86 -4,62 549,66

38,65 38,65 2,16 186,72 -6,76 189,73

-47,13 -47,13 -40,33 -40,33 32,53 32,53

-1,01 -1,01 31,81 -139,62 -41,65 -41,65

-33,21 -33,21 31,75 31,75 -36,68 -36,68

47,87 -117,12 22,24 22,24 -32,66 -32,66

21,27 21,27 -35,01 -35,01 -10,91 -10,91

0,05 0,05 15,96 15,96 33,14 33,14

-2,89 -2,89 1,86 1,86 30,34 30,34

Tabla 12: Resultados para estimación de ángulo YAW para valores iniciales de roll =0º pitch = -10º yaw = 30º

Tras realizar la segunda prueba queda patente que la desviación de la estimación frente a los datos de entrada

se debe a la elección de las condiciones iniciales. El algoritmo se muestra muy capaz de encontrar el valor

exacto en el caso que esté lo suficientemente cerca de las mismas. Dado la importancia de elegir un valor

inicial lo suficientemente próximo, será muy recomendable la utilización de los datos de los sensores

embarcados para obtener los puntos iniciales de búsqueda de mínimos.

56

56

La aplicación de algoritmos que permitan aumentar la robustez también sería una buena manera de hacer más

fiable la estimación, tomando varias medidas cada vez y desechando las que se alejen del modelo. Un ejemplo

de esto podría ser la aplicación de filtros de Kalman.

5.4 Orientación completa (ROLL, PITCH, YAW y posición espacial)

Nos enfrentamos finalmente a la mejor aproximación al problema real, la estimación de la orientación

completa. Seguimos utilizando puntos sintéticos, que se crearán con los métodos antes descritos. En esta

ocasión doblamos el número de variables a minimizar (3 coordenadas angulares y 3 coordenadas espaciales),

aumentando de forma significativa la complejidad matemática del problema.

Vamos a suponer a partir de ahora que se dispone de datos de telemetría y que se conoce de forma aproximada

la actitud completa de la aeronave. Estos datos se utilizarán para obtener una aproximación de los valores

iniciales, de forma que podamos “ayudar” al algoritmo a encontrar los mínimos que buscamos. Esta

suposición no está nada alejada de la realidad, ya que si se puede montar el equipo necesario para el

procesamiento de imágenes en tiempo real, la adición de sensores para medir la actitud no supone ningún

problema.

En bastantes ocasiones las mediciones de estos sensores pueden ser ruidosas y poco fiables, pero no es

necesariamente un problema ya que solo se necesita para poder ofrecer una primera aproximación.

Esta suposición se traducirá en la elección de forma manual de los valores iniciales, los cuales estarán cerca

del valor real introducido, pero con variaciones que lo alejen lo suficiente del valor exacto, simulando así la

imprecisión de los sensores de a bordo. Se establecerá un intervalo en torno al valor exacto y se escogerán los

valores de forma aleatoria dentro de ese intervalo.

Aunque de forma general los resultados eran satisfactorios con la función fminunc, es necesario puntualizar

que existe cierto margen de mejora. La cantidad de puntos errados era lo suficientemente alta como para

plantear un nuevo enfoque, que obtuviera unos resultados más consistentes, no tan dependientes de los valores

iniciales introducidos. Para ello introduciremos la función de MatLab lsqnonlin explicada anteriormente.

θ : ROLL 𝜃0 = �̃� ± 15 (⁰)

Φ: PITCH 𝛷0 = �̃� ± 10 (⁰)

Ψ: YAW 𝛹0 = �̃� ± 20 (⁰)

x : separación transversal con pista 𝑥0 = �̃� ± 20 (𝑚)

y : separación longitudinal con pista 𝑦0 = �̃� ± 20(𝑚)

z : altura sobre pista 𝑧0 = �̃� ± 10(𝑚)

Tabla 13: Tabla de valores iniciales para estimación de orientación completa

A continuación mostramos los resultados de esta serie de pruebas:

5.4.1 Resolución mediante fminunc



2,54 2,54 -48,15 12,43 46,91 46,91

-32,21 -6,11 -28,58 -8,61 -19,90 -18,94

-1,33 -1,33 -19,76 -17,11 23,85 23,85

14,89 14,89 21,57 21,57 -36,26 -29,76

21,50 21,50 -43,61 10,35 -56,34 56,98

-12,54 -12,54 26,55 26,55 29,29 29,29

-15,91 -15,91 -47,19 10,50 0,00 0,00

58,56 58,56 18,45 18,45 -2,41 -2,41

-55,47 53,52 -0,70 -0,70 48,57 48,57

46,22 46,22 33,49 33,49 13,18 13,18

49,59 49,59 25,80 25,80 14,12 14,12

35,54 35,54 48,45 48,45 43,13 43,13

Tabla 14: Resultados obtenidos en la estimación de ángulo ROLL en orientación completa fminunc

58

58



15,41 15,49 -8,37 -137,63 -0,45 -0,45

-5,52 -5,52 22,38 6,87 -31,72 -31,71

30,02 176,47 29,30 29,30 -40,05 -40,05

24,20 24,20 26,10 26,10 31,56 31,56

-29,95 -29,15 -16,33 -16,33 5,45 5,45

32,58 32,58 3,07 3,07 38,66 38,66

44,09 44,09 -36,90 -45,65 17,70 17,70

1,30 1,30 -34,95 -34,94 7,45 7,45

34,59 34,58 -32,73 -32,73 28,39 28,39

7,92 7,92 16,08 16,08 34,11 34,11

-31,07 -30,67 -0,43 -0,43 44,00 44,00

-27,01 -27,01 -27,93 -27,12 -44,95 -44,81

Tabla 15: Resultados obtenidos en la estimación de ángulo PITCH en orientación completa fminunc



-45,05 -46,55 -0,09 -66,04 -29,40 -29,40

-1,04 -1,04 3,58 -5,32 44,79 44,79

-30,75 -6,72 -5,48 -5,48 -41,79 -41,79

-37,69 -37,69 -37,61 -37,61 -39,43 -39,43

-29,45 -30,54 -0,96 -0,96 -35,80 -35,80

-35,35 -35,35 35,30 35,30 -33,35 -33,35

-31,09 -31,09 37,39 44,22 12,10 12,09

-45,73 -45,73 -22,97 -22,97 7,37 7,37

13,52 13,52 -29,15 -29,15 -44,79 -44,79

-21,81 -21,81 6,50 6,50 43,12 43,12

3,86 3,84 14,03 14,03 22,87 22,87

19,52 19,52 -8,30 -6,55 23,78 23,63

Tabla 16: Resultados obtenidos en la estimación de ángulo YAW en orientación completa fminunc

Posición X Posición X Posición X Posición X Posición X Posición X

introducida (m)estimada (m) introducida (m)estimada (m) introducido (m)estimada (m)

-174,64 -181,61 -79,48 -15,39 -129,16 -129,16

144,18 144,15 -81,79 40,35 65,12 65,12

173,76 35,32 -66,83 -66,83 -67,67 -67,67

193,76 193,76 -13,17 -13,17 159,39 159,39

143,58 145,99 59,28 59,27 -152,74 -152,74

114,22 114,22 -189,91 -189,90 195,37 195,36

5,35 5,35 136,88 105,32 15,99 15,99

-128,96 -128,96 23,61 23,61 82,77 82,77

-40,56 -40,56 141,64 141,64 199,80 199,80

-146,43 -146,43 -60,85 -60,85 -84,86 -84,86

-187,64 -185,08 -21,59 -21,59 -34,19 -34,19

175,66 175,65 -178,30 -174,49 -14,06 -14,03

Tabla 17: Resultados obtenidos en la estimación de posición X en orientación completa fminunc

Posición Y Posición Y Posición Y Posición Y Posición Y Posición Y


105,58 109,82 -12,61 -42,57 -171,42 -171,42

127,28 127,26 164,85 -104,49 -103,01 -103,00

-159,91 -29,78 -158,40 -158,39 -178,50 -178,50

-128,75 -128,75 98,22 98,22 -23,31 -23,31

-56,15 -57,11 94,51 94,49 -194,69 -194,69

-177,32 -177,32 24,74 24,74 158,88 158,87

8,75 8,75 -126,32 -82,29 -121,34 -121,33

-65,66 -65,66 38,88 38,87 -162,65 -162,65

-129,73 -129,73 -80,03 -80,03 -77,05 -77,05

-116,42 -116,42 -146,35 -146,35 -17,58 -17,58

162,06 159,71 -114,96 -114,96 -159,33 -159,33

70,16 70,15 157,98 146,30 198,16 197,06

Tabla 18: Resultados obtenidos en la estimación de posición Y en orientación completa fminunc

60

60

Posición Z Posición Z Posición Z Posición Z Posición Z Posición Z


29,89 31,13 48,04 134,25 63,65 63,65

26,76 26,76 9,82 -5,99 70,32 70,32

5,58 1,10 74,32 74,32 25,92 25,92

26,84 26,84 30,43 30,43 62,33 62,33

4,17 4,24 26,46 26,45 50,10 50,10

45,49 45,49 67,17 67,17 35,69 35,69

68,53 68,53 0,93 -2170,18 5,54 5,54

56,80 56,80 4,36 4,36 70,22 70,22

8,09 8,09 60,11 60,11 30,38 30,38

7,28 7,28 54,31 54,31 54,71 54,71

69,95 68,99 47,35 47,35 66,71 66,71

81,46 81,46 65,67 62,79 9,43 9,38

Tabla 19: Resultados obtenidos en la estimación de posición Z en orientación completa fminunc

A la vista de las pruebas se puede observar que los resultados solo son parcialmente satisfactorios, existiendo

un número de casos en los que la precisión alcanzada no es suficiente. Como en el anterior apartado, estas

imprecisiones son causadas principalmente por la lejanía del valor con respecto del punto inicial introducido.

Para paliar, o al menos, limitar este fenómeno vamos a realizar otra serie de pruebas con la función lsqnonlin,

mejor optimizada para problemas de mínimos cuadrados como el nuestro.

5.4.2 Resolución mediante lsqnonlin



23,38 23,38 25,12 25,12 30,15 30,15

-21,95 -21,95 30,56 30,56 -29,39 -29,39

54,03 54,03 -26,88 -26,88 0,71 0,71

-55,87 -55,87 21,56 21,55 23,89 23,89

-7,35 -7,35 18,61 18,61 46,91 46,91

-14,21 -14,21 -40,49 -40,49 55,11 55,11

31,86 31,86 -45,72 -45,72 5,67 5,67

35,42 35,42 -0,20 -0,20 -43,37 -43,37

-37,58 -37,58 55,17 55,17 -42,08 -42,08

-1,23 -1,23 -19,15 -19,15 -29,10 -29,10

-6,53 -6,53 10,23 10,23 40,89 40,89

17,56 17,56 -33,14 -33,14 -29,49 -29,49

Tabla 20: Resultados obtenidos en la estimación de ángulo ROLL en orientación completa lsqnonlin



28,33 28,33 41,15 41,15 16,09 16,09

36,52 36,52 -1,32 -1,32 23,20 23,20

-33,57 -33,57 27,03 27,03 21,88 21,88

37,20 37,20 -32,23 -32,23 -9,70 -9,70

11,91 11,91 -7,04 -7,04 13,99 13,99

-36,22 -36,22 37,42 37,42 -29,59 -29,59

-19,94 -101,14 26,30 26,30 18,54 18,54

4,22 4,22 41,35 41,35 -42,14 -42,14

41,18 41,18 14,02 14,02 -20,08 -20,08

41,84 41,84 -41,79 -41,79 -40,84 -40,84

-30,81 -30,81 31,42 31,42 -36,26 -36,26

42,35 42,35 39,06 39,06 29,11 29,11

Tabla 21: Resultados obtenidos en la estimación de ángulo PITCH en orientación completa lsqnonlin



31,43 31,43 41,72 41,72 6,88 6,88

-25,65 -25,65 -21,42 -21,42 -3,06 -3,06

42,93 42,93 25,72 25,72 -48,81 -48,81

-15,00 -15,00 25,37 25,37 -16,29 -16,29

-30,34 -30,34 -11,96 -11,96 -33,78 -33,78

-24,89 -24,89 6,78 6,78 29,43 29,43

11,60 11,60 -42,41 -42,41 -18,88 -18,88

-2,67 -2,67 -44,60 -44,60 2,85 2,85

-14,83 -14,83 3,08 3,08 -33,44 -33,44

33,08 33,08 27,92 27,92 10,20 10,20

8,53 8,53 43,40 43,40 -23,70 -23,70

4,97 4,97 -37,01 -37,01 15,41 15,41

Tabla 22: Resultados obtenidos en la estimación de ángulo YAW en orientación completa lsqnonlin

62

62

Posición X Posición X Posición X Posición X Posición X Posición X


75,69 75,69 -157,34 -157,34 -127,26 -127,26

99,26 99,26 184,76 184,76 -94,48 -94,48

-19,78 -19,78 -198,15 -198,15 -141,78 -141,78

-166,47 -166,47 109,96 109,96 -145,57 -145,57

-108,41 -108,41 126,92 126,92 147,72 147,72

165,33 165,33 147,48 147,48 31,88 31,88

-139,05 -139,05 -166,23 -166,23 19,94 19,94

130,33 130,33 -40,09 -40,09 -142,02 -142,02

15,34 15,34 -96,05 -96,05 141,21 141,21

198,45 198,45 120,03 120,03 48,82 48,82

-168,73 -168,73 -27,43 -27,43 -59,62 -59,62

-22,93 -22,93 164,26 164,26 5,30 5,30

Tabla 23: Resultados obtenidos en la estimación de posición X en orientación completa lsqnonlin

Posición Y Posición Y Posición Y Posición Y Posición Y Posición Y


-39,28 -39,28 -64,91 -64,91 30,08 30,08

-169,61 -169,61 160,02 160,02 -176,09 -176,09

-104,03 -104,03 -52,30 -52,30 -106,09 -106,09

-150,67 -150,67 -155,52 -155,52 -58,74 -58,74

-126,44 -126,44 112,10 112,10 128,48 128,48

-104,02 -104,02 -44,10 -44,10 -193,84 -193,84

-33,09 -33,09 -103,32 -103,32 -182,79 -182,79

-180,14 -180,14 -38,44 -38,44 -132,40 -132,40

161,09 161,09 -161,42 -161,42 59,65 59,65

177,91 177,91 -147,21 -147,21 92,69 92,69

-3,65 -3,65 176,82 176,82 59,10 59,10

-4,30 -4,30 182,45 182,45 -19,63 -19,63

Tabla 24: Resultados obtenidos en la estimación de posición Y en orientación completa lsqnonlin

Posición Z Posición Z Posición Z Posición Z Posición Z Posición Z


49,23 49,23 43,81 43,81 31,57 31,57

26,67 26,67 39,23 39,23 84,51 84,51

67,02 67,02 40,21 40,21 78,83 78,83

17,01 17,01 27,57 27,57 49,51 49,51

61,81 61,81 45,77 45,77 56,02 56,02

16,52 16,52 45,97 45,97 52,83 52,83

33,16 33,16 73,59 73,59 18,70 18,70

56,31 56,31 71,53 71,53 27,11 27,11

70,22 70,22 57,99 57,99 42,38 42,38

7,30 7,30 34,07 34,07 20,74 20,74

83,64 83,64 73,04 73,04 75,99 75,99

69,81 69,81 47,95 47,95 17,53 17,53

Tabla 25: Resultados obtenidos en la estimación de posición Z en orientación completa lsqnonlin

El cambio de función para resolver la optimización resultó ser muy acertado, ya que se consiguió una

estimación precisa, fiable y rápida, que necesitó de menos de 2.5s para realizar las 36 iteraciones que

comprendían esta prueba.

La función de MatLab lsqnonlin ha demostrado su validez para nuestro problema, con lo cual será la elección

obvia de cara a acometer el problema final: la estimación de orientación completa para imágenes reales.

5.5 Aplicación a imágenes reales

Tras demostrar la validez y robustez del algoritmo en las pruebas anteriores, momento de aplicarlo en

imágenes reales. Estas imágenes son extraídas del vídeo procesado fruto del trabajo del antiguo alumno de

Ingeniería Informática Juan Manuel Benítez en su Proyecto Final de Carrera “Detección automática de pistas

de aterrizaje mediante técnicas de procesado de imágenes”. El algoritmo que desarrolló era más que capaz de

localizar una pista de aterrizaje en una imagen a color, tomada de un vídeo grabado desde una pequeña

aeronave no tripulada.

Una vez que el vídeo era procesado, se obtenía como producto otro vídeo donde se superponía sobre la imagen

el resultado de esta detección, remarcando en color verde el contorno de la pista de aterrizaje.

Es gracias a este contorno marcado como extraeremos la información de los puntos de las esquinas de la pista,

los cuales son necesarios para poder extraer la información de la orientación de la aeronave. Los puntos se

buscaron de manera automática haciendo uso de las propiedades de las imágenes del vídeo procesado.

64

64

Con la intención de reducir sustancialmente el coste computacional de realizar la detección de esquinas en

todos y cada uno de los frames del vídeo, se realizará un preprocesado con la finalidad de determinar los

frames en los que está presente la pista de aterrizaje.

Se desarrolló un algoritmo que realizara esta búsqueda; comenzaba extrayendo la información de color de la

imagen, filtrando todos los píxeles que no fueran verdes11

. Este filtrado daba como resultado una imagen

binaria12

, en la que solo aparecían los pixeles pertenecientes al contorno de la pista, llevando además la cuenta

del número de pixeles verdes que aparecen en la imagen. Si se superaba una cierta cantidad, se determinaba

que esa imagen en concreto contenía a la pista de aterrizaje.

Una vez se conoce la posición en el vídeo de las imágenes que incluyen a la pista de aterrizaje se pasa a

realizar una búsqueda de las esquinas de ese contorno. Con la posición de las esquinas conocida, solo restaría

clasificarlas, esto es, asignar a cada esquina la etiqueta que le corresponda (A, B, C o D). Realizar

correctamente este paso es de capital importancia, ya que la correspondencia entre los puntos exactos de la

pista en la imagen y su homólogo real es la base del algoritmo de estimación de la orientación.

Cuando se tengan los puntos en coordenadas pixeles de las esquinas correctamente asignados, se está en

situación de comenzar a ejecutar el algoritmo de estimación de orientación. La orientación obtenida se validará

posteriormente gracias a otra función que superpondrá el resultado de la transformación proyectiva de las

esquinas de la pista a la imagen real.

Por claridad de explicación mostraremos un diagrama del proceso a seguir.

11 Una imagen a color RGB se compone de 3 matrices de niveles de color de 0 a 255. Cada matriz representa a un color siendo estos Rojo, Verde, Azul. El color verde presentaría un valor de intensidad alto en la matriz de verde y muy bajo en las matrices de rojo y azul. 12 Una imagen binaria solo presenta dos valores de intensidad, máximo (blanco) y mínimo (negro)

Estimación de

orientación completa mediante

imagen real

Lectura del archivo de vídeo

procesado

Preprocesado inicial

Búsqueda de frames con pista, indizado de los

mismos

Búsqueda de

esquinasEstimación de

valores iniciales

Mediante telemetría o

inspección de la imagen

Aplicación del algoritmo de

estimación de orientación

Validación de

orientación

Resultado de la estimación

Información de imagen y posición de

las esquinas de la pista

Superposición de resultado de transformación en imagen real

Figura 21: diagrama algoritmo completo estimación de orientación

El algoritmo que se usó para el preprocesado es el siguiente:

Lectura de video

Procesado de video

Clasificación de esquinas

obtenidas de pista (A,B,C,D)

Extracción de frames que

incluyen a pista

Aplicación de algoritmo detección esquinas

Posición de esquinas en imagen

Nº de frames con pista y localización

en vídeo

Estructura array con información de esquinas clasificadas e imagen donde

se encuentra

Búsqueda de pixeles verdes

en imagen

Figura 22: Diagrama algoritmo de preprocesado de vídeo

Además mostraremos el diagrama para la identificación de esquinas

66

66

Algoritmo localización e identificación esquinas

Búsqueda y clasificación de

esquinas

Binarización de imagen

Almacenar coordenadas de

puntos resultantes

Erosión de la imágen

Plantilla [ 1 1 ] [ -1 1 ] [ 1 -1 ] [ -1 1 ]

Posición de esquinas en matriz

Esquinas localizadas, clasificación de las mismas

Selección de 2 candidatos a

esquinas superiores

Selección de 2 candidatos a

esquinas inferiores

Puntos con menor

coordenada j

Puntos con mayor

coordenada j

Punto A más cercano a origen, B más alejado

Punto C más a la izquierda, B más a la

derecha

Esquinas clasificadas y localizadas

Eliminación de pixeles que no sean

verdes

Figura 23: Algoritmo de identificación y clasificación de las esquinas de la pista

Definidos los métodos necesarios para realizar la prueba, pasaremos a comentar el desarrollo y metodología de

la misma.

Vamos a emplear 5 imágenes distintas para demostrar el funcionamiento del algoritmo completo, partiendo de

la lectura del vídeo de forma automática, pasando por la detección y clasificación de esquinas y finalizando

con la superposición del resultado final en la imagen inicial extraída del video

5.5.1 Preparación de las pruebas

La ejecución del programa de lectura de video, extracción de imágenes con pista e identificación y

clasificación de esquinas de pista requirió de un par de minutos para completarse. Como resultado se obtuvo el

número de escenas que incluyen a la pista, como el identificador de la misma dentro del vídeo. Este

identificador será muy útil con vistas a obtener las esquinas pertenecientes a esa imagen, saber su lugar dentro

de la estructura donde se almacenan las imágenes con pista y para poder mostrar la imagen de forma

individual.

Una vez se tengan localizadas las imágenes, su identificador y la posición de las esquinas clasificadas

podemos comenzar con la prueba.

5.5.2 Prueba frame 2537

Esta imagen se corresponde con la imagen con pista nº 200, hemos elegido esta en concreto porque cumple

todos los requisitos para la detección de pista. El algoritmo que se usó para obtener el video procesado limitaba

la búsqueda a un recuadro de menor tamaño dentro de la imagen.

Si la pista no se encontraba de forma íntegra dentro de este recuadro, el algoritmo no debería buscar la pista en

esa imagen ya que los resultados podrían no ser fiables. Haciendo uso de esta peculiaridad, nosotros tratamos

de reducir el coste computacional del programa de detección e identificación de esquinas al limitar el proceso

de búsqueda a dentro de este recuadro.

Sin embargo en el vídeo final sí que se detecta la pista en las imágenes, pero al limitar la búsqueda al recuadro,

los resultados de la posición de las esquinas no serán correctos. Más adelante mostraremos este caso.

A continuación se muestra la imagen a procesar, extraída directamente del vídeo gracias a los algoritmos

anteriormente citados.

Figura 24: frame 2537

Comenzamos la detección e identificación de esquinas mediante el programa Esquinas.m, se extrae la posición

de los puntos y se almacena para su uso posterior.

Mediante inspección de la imagen y simulando los datos de telemetría, se introdujeron los siguientes valores

iniciales. Después se aplicó el programa PoseTotalRealLSQAUTO.m con la información anterior y se obtuvo

un valor de la orientación

68

68

Valor inicial Resultados de estimación

𝜃0 = 10 ⁰ �̃� = 2,9582 ⁰

𝛷0 = −10 ⁰ �̃� = 79.6320 ⁰

𝛹0 = 15 ⁰ �̃� = −3.7780 ⁰

𝑥0 = 200 𝑚 �̃� = 1.4759 𝑚

𝑦0 = 100 𝑚 �̃� = −76.3223 𝑚

𝑧0 = 60 𝑚 �̃� = 17.5564 𝑚

Es importante remarcar que el valor de PITCH fruto de la estimación, requiere de una pequeña modificación

para su interpretación como valor de actitud de la aeronave. Como los puntos reales se tomaron mediante una

vista desde arriba, la transformación de un rectángulo perpendicular al movimiento del avión, (es decir en el

plano de la imagen) a otro rectángulo paralelo al plano horizontal “mundo” implica de un giro de -90º para

alinear el PITCH estimado con el PITCH actitud del avión. El valor real de PITCH (aeronave) sería:

�̃� = 79.6320 − 90 = −10.3680 ⁰

Ahora restaría validar esa orientación, para ello haremos uso del programa PruebaPose.m, e cual nos dará una

imagen del resultado de la transformación superpuesta con la imagen inicial. Además se superpondrá las

esquinas calculadas mediante Esquinas.m

Figura 25: Resultado final para frame 2537

Podemos observar un buen resultado, ligeramente descentrado quizá, pero satisfactorio en general. Estas

desviaciones se deben a la imprecisión con la que se han tomado los puntos, mejorando seguramente la

exactitud si se dispusiese de un algoritmo de búsqueda más sofisticado.


Vamos a realizar ahora una segunda prueba, en la línea de la anterior. La manera de proceder será equivalente,

extraer los puntos, clasificarlos, obtener la estimación y validarlo.



𝜃0 = 10⁰ �̃� = −6.7323⁰

𝛷0 = −10 ⁰ �̃� = 80.8967 ⁰

𝛹0 = 0 ⁰ �̃� = −2.8811 ⁰

𝑥0 = 40 𝑚 �̃� = 8.2259 𝑚

𝑦0 = −10 𝑚 �̃� = −108.2152 𝑚

𝑧0 = 60 𝑚 �̃� = 35.5633 𝑚

70

70

Como en el frame anterior, se debe aplicar la transformación del ángulo de PITCH para obtener el valor de la

aeronave.

El valor real de PITCH (aeronave) sería:

�̃� = 80.8967 − 90 = −9.1033 ⁰

Valoramos el resultado mediante PruebaPose.m

Figura 27: Resultado final para frame 4091

La estimación vuelve a ser satisfactoria, ajustando bien la transformación con los parámetros estimados con la

realidad.


Esta será la última prueba que mostremos en los rangos normales de funcionamiento, es decir frames donde la

pista se encuentra dentro del recuadro limitador y el algoritmo de búsqueda de la pista funciona

adecuadamente.

En pruebas futuras analizaremos el funcionamiento de nuestro algoritmo cuando la detección de la pista falla.

El procedimiento seguido se mantiene con respecto a las demás pruebas.



𝜃0 = 10⁰ �̃� = 2.1194⁰

𝛷0 = −10 ⁰ �̃� = 81.1808 ⁰

𝛹0 = 0 ⁰ �̃� = 1.5072 ⁰

𝑥0 = 40 𝑚 �̃� = 4.1686 𝑚

𝑦0 = −10 𝑚 �̃� = −40.4597 𝑚

𝑧0 = 60 𝑚 �̃� = 11.3043 𝑚

72

72

Corrección para el valor de PITCH:

�̃� = 81.1808 − 90 = −8.8192⁰

Figura 28: resultado final para frame 7882

Los resultados obtenidos tras estas 3 primeras pruebas se muestran muy prometedoras, demostrando un

funcionamiento sólido del algoritmo en condiciones normales. Además podemos ver que los valores de la

estimación se muestran consistentes aunque las condiciones iniciales se muestren alejadas de los valores

reales.

A continuación se realizarán una serie de pruebas en casos límite, dónde el algoritmo podría fallar por unas

causas u otras.

5.5.5 Prueba caso límite 1: fallo de la detección de pista

En el caso que fallase el algoritmo de detección de la pista, no podrían ejecutarse los algoritmos de detección y

clasificación de esquinas, sería necesario seleccionar los puntos de manera manual o desarrollar un algoritmo

secundario que entrara en funcionamiento en el momento en que se detecte un fallo.

Como la detección de pista se encuentra fuera del objeto de este trabajo, nos limitaremos a realizar la prueba

mediante selección manual de los puntos.

Figura 29: frame con fallo de detección (frame 8878)

Para la búsqueda manual de los puntos se utilizará la herramienta de visionado de imágenes de MatLab

imshow, la cual nos permitirá conocer la posición de los píxeles de las esquinas. Tras realizar esta búsqueda

manual las posiciones de los puntos (en pixeles) fueron las siguientes:

A = [ 208, 240, 1 ] B = [235, 240, 1 ]

C = [ 175, 309, 1 ] D = [227, 308 ,1 ]

Se añadió un 1 como tercera componente a cada vector para que estuvieran representados en coordenadas

homogéneas, lo cual es una necesidad de cara a resolver la estimación de la orientación. Después se añaden a

un vector y se les da el formato necesario para introducirlo en el algoritmo de estimación de orientación.

74

74

Figura 29: superposición de esquinas en imagen con detección fallida

Con la posición de las esquinas conocida y formateada se procede a ejecutar el algoritmo de estimación de la

manera habitual.


𝜃0 = 10⁰ �̃� = 1.0190⁰

𝛷0 = −10 ⁰ �̃� = 80.6231 ⁰

𝛹0 = 0 ⁰ �̃� = 6.2230 ⁰

𝑥0 = 40 𝑚 �̃� = −12.8680 𝑚

𝑦0 = −10 𝑚 �̃� = −217.5336 𝑚

𝑧0 = 60 𝑚 �̃� = 58.7108 𝑚

.


�̃� = 81.1808 − 90 = −9.3765⁰

Figura 29: resultado final con superposición

En este caso se pone de manifiesto una ligera imprecisión en la estimación de la orientación, aunque los

ángulos de la actitud son correctos, el algoritmo parece estimar una distancia mayor a la real, por eso vemos la

pista de un tamaño menor al que realmente tiene.

Esto es esperado ya que el algoritmo de detección ha considerado que la imagen es demasiado lejana como

para resolver la detección. Al aumentar la separación, la distancia entre los puntos de las esquinas decrece,

siendo entonces más sensible el algoritmo al ruido. Este ruido genera incertidumbre e inexactitud en la

posición de las esquinas, provocando en última instancia la ligera imprecisión de la orientación.

76

76

5.5.6 Prueba caso límite 2: pista fuera de los límites de recuadro

Esta situación es relativamente común, ocurre de forma más significativa que el caso límite 1. Nos

encontraremos con este problema en las situaciones en las que se ha detectado de forma exitosa la pista para

luego sobrevolarla. El algoritmo de estimación continúa mostrando la posición de la pista aunque la misma ya

no se encuentre dentro del recuadro limitador.

Le programa mantendrá el contenedor verde hasta que la pista se salga del campo de visión de la cámara.

La razón de tomar esta situación como caso límite se debe al fallo sistemático de nuestro algoritmo de

detección de esquinas, ya que el mismo solo actúa dentro del recuadro limitador, los puntos fuera de este son

simplemente ignorados. Recordamos que esto se hizo para reducir el coste computacional del mismo.

Figura 30: frame donde ocurre el fallo (frame 9005)

Si se aplicase el algoritmo de detección y clasificación de esquinas en esta situación se obtendrían unas

esquinas erróneas, las cuales provocarían el fallo a su vez de todo el algoritmo de estimación. Será importante

ampliar la búsqueda a toda la imagen para obtener resultados coherentes.

Por suerte, la simple ampliación a la imagen completa resuelve este problema, con lo cual será importante

reconocer las situaciones en la que esto ocurrirá.

Una vez se hayan obtenido las esquinas, aunque antes de clasificarlas, será importante comprobar que estas

esquinas no tienen ninguna componente sobre el recuadro. En caso de que se tenga, se realizará una nueva

búsqueda con la imagen completa.

Figura 31: detección de esquinas fallida

Aplicamos la solución y obtenemos la imagen con las esquinas correctamente detectadas.

Con este problema resuelto se aplica el algoritmo de estimación de orientación y posteriormente el de

validación.


𝜃0 = 10⁰ �̃� = 14.5060⁰

𝛷0 = −10 ⁰ �̃� = 91.5419 ⁰

𝛹0 = 0 ⁰ �̃� = 6.2230 ⁰

𝑥0 = 40 𝑚 �̃� = 0.4487 𝑚

𝑦0 = −10 𝑚 �̃� = −7.4767 𝑚

𝑧0 = 60 𝑚 �̃� = 41.5747 𝑚

.


�̃� = 91.5419 − 90 = 1.5419⁰

78

78

Figura 32: Corrección de detección de esquinas

Figura 33: imagen superpuesta final para caso límite 2

Este caso límite también presenta una precisión por debajo de lo habitual, causada probablemente tanto por la

posición un tanto extrema de la pista, como por la ligera inexactitud de la pista detectada por el programa de

detección de pista.

Debemos recordar que se trata de un caso límite, que aunque se presenta con mayor frecuencia que el caso 1,

no representa una porción muy significativa de los regímenes de vuelo.

6 CONCLUSIONES Y DISCUSIÓN FINAL

6.1 Objetivos

En este trabajo se consiguieron alcanzar los siguientes objetivos:

1. Desarrollo de una herramienta modular para la estimación de orientación en una aeronave no

tripulada.

2. Robustez, fiabilidad y velocidad de la herramienta para condiciones normales de operación.

3. Coordinación con trabajos anteriores, permitiendo la posterior ampliación y optimización de la

herramienta para más situaciones de las inicialmente planteadas.

4. Creación de un punto de partida para un sistema futuro de aterrizaje automático con reglas VFR13

en

aeronaves no tripuladas.

5. Empleo de software comercial, de gran expansión, muy utilizado en la comunidad de la ingeniería.

6.2 Valoración del trabajo

El enfoque incremental seguido ha sido de capital importancia para el correcto desarrollo de la herramienta. Al

comenzar con tareas más sencillas se ha conseguido un conocimiento más profundo del problema a resolver,

poniéndose de manifiesto de forma inmediata las dificultades emergentes de cada escalón de desarrollo.

Además permitió resolver las complicaciones inherentes a cada nivel, pudiendo centrarnos casi

exclusivamente en resolver ese problema en el momento en que se presente. Esto simplificó el desarrollo de

las herramientas siguientes ya que, al haber resuelto ya ese problema, no aparecía en el siguiente nivel.

Con las primeras etapas de desarrollo se pudo validar el funcionamiento de la resolución de la estimación de

orientación mediante la optimización y minimización de la función de error. Se utilizó un enfoque por fuerza

bruta, que aunque lento, permitía un control absoluto de la ejecución, no dejando espacio a la incertidumbre

que presenta la resolución de un problema mediante la “caja negra” que supone utilizar una herramienta de

terceros.

También se pudo observar que la fuerza bruta, aunque eficaz y precisa era demasiado lenta y costosa con lo

cual se tuvo que abandonar para dar paso a funciones más óptimas. Sólo se pudo cambiar la manera de

resolución una vez estuvimos seguros de que la estrategia de resolución era eficaz.

Se continuó aumentando la complejidad al ir añadiendo variables, siendo necesario cambiar de fuerza bruta a

la función fminunc, y cuando esta demostró ser muy sensible a la elección de los valores iniciales se pudo

pasar a la función óptima lsqnonlin.

13 Visual Flight Rules. Reglas de vuelo visual, normativa que regula las condiciones para vuelo con única referencia de observación visual.

80

80

La utilización de puntos sintéticos también demostró ser especialmente útil; al dar un control completo sobre

la transformación a realizar y saber de antemano que resultados esperar. Esto provocaba que cualquier error

de implementación saliese a flote con relativa facilidad, permitiendo la rápida solución de los mismos.

El salto cualitativo más importante fue el cambio de puntos sintéticos a imágenes reales, aumentando de forma

significativa la dificultad de obtener estimaciones correctas.

Fue en este momento cuando se necesitó del trabajo realizado por el algoritmo de detección de pista que se

desarrolló en el proyecto “Detección automática de pistas de aterrizaje mediante técnicas de procesado de

imágenes” el cual nos permitió poner en práctica nuestro algoritmo sobre los puntos de las esquinas ya

detectados.

Podemos concluir que este trabajo ha obtenido un buen resultado, no solo por solucionar en buena medida el

problema de la estimación de orientación de una cámara sino también por la oportunidad de desarrollo

profesional y personal que demostró ser para el autor de este trabajo.

6.3 Trabajo futuro

La ampliación más inmediata sería la migración a algún tipo de lenguaje que permitiese embarcarlo

directamente en una aeronave. Todo el trabajo realizado ha sido fuera de línea, procesando un vídeo mediante

MatLab en un ordenador personal.

El algoritmo se ha mostrado lo suficientemente rápido como para realizar varias medidas cada segundo, con lo

cual podría ser refinado y optimizado para su uso en tiempo real.

Por otra parte la herramienta desarrollada depende de trabajo externo para poder realizar la estimación, si este

falla, provoca el fallo también de la estimación. Se podría desarrollar un algoritmo secundario que actuara en

caso de fallo, proveyendo de un sistema redundante de apoyo.

Si se consiguiese la coordinación completa y fiable entre esta herramienta y el algoritmo de detección, se

podría utilizar como punto de partida para un futuro trabajo de aterrizaje automático de aeronaves no

tripuladas. Con las estrategias de control adecuadas y el uso de estas herramientas conseguiríamos un sistema

de aterrizaje automático sencillo y de mucho menor coste que los existentes en la actualidad como el ILS o el

MLS.

Referencias

[1]. Benítez Domínguez, J. Manuel (2015). Proyecto de fin de carrera, Ingeniería Informática en la Escuela

Técnica Superior de Ingeniería Informática de la Universidad de Sevilla: “Detección automática de pistas de

aterrizaje mediante técnicas de procesado de imágenes”

[2] « Inertial measurement unit» [En línea] Available:

https://en.wikipedia.org/wiki/Inertial_measurement_unit [Último acceso: junio 2017]

[3] « Instrument landing system» [En línea] Available:

https://en.wikipedia.org/wiki/Instrument_landing_systeml [Último acceso: junio 2017]

[4] « Microwave landing system» [En línea] Available:

https://en.wikipedia.org/wiki/Microwave_landing_systeml [Último acceso: junio 2017]

[5] P. Corke (2011) Robotics, Vision and Control. Fundamental Algorithms in MATLAB©

Springer, Alemania

https://en.wikipedia.org/wiki/Inertial_measurement_unit

https://es.wikipedia.org/wiki/Unidad_de_medici%C3%B3n_inercial

https://es.wikipedia.org/wiki/Unidad_de_medici%C3%B3n_inercial

82

82

Anexo: Código .m utilizado

1 Ángulo Pitch

function angPITCH = AnguloPitch(angx,liminf,limsup)

%Los argumentos son: el ángulo REAL girado, el límite inferior %de busqueda y el límite superior

%DATOS DE CAMARA CON SENSOR OV5647 (INSTALADA) %Area imagen 3.6736 mm x 2.7384 mm, ¿es area de imagen efectiva?

los 640x480? o %los 2592x1944 %pixel 1.4 x 1.4 um %dist_focal 3.6 mm % array center en 320,175 um

u0=(3670/2)/1.4; v0=(2740/2)/1.4; dist_foc=3.6*10^(-3); mx=1.4*10^(-6); my=1.4*10^(-6); alfax=dist_foc/mx; alfay=dist_foc/my;

A1=[alfax/4.05,0,u0/4.05; 0,alfay/4.05,v0/4.05; 0, 0 ,1;]; I=eye(3); I(:,4)=zeros(3,1); A=A1*I;

% Datos de posición real de pista, posición GPS extraida de % datos de telemetría y dimensiones de la pista medidas con % Google Maps % 37°08'51.0"N 5°47'13.2"W (Coordenadas L/L de pista) % Esquinas

ancho=12; largo=150; D=0;

B=0; a=[B,largo+D,0,1]; b=[ancho-B,largo+D,0,1]; c=[ancho,0,D,1]; d=[B,0,0,1];

%Prueba a altura H y girado 45º en X y con varios puntos

puntos=[a',b',c',d']; puntosg=zeros(3,4);

angy=0; angz=0;

% Matrices de rotación para creación de puntos sintéticos

Rotx=[1,0,0; 0,cosd(angx),-sind(angx); 0,sind(angx),cosd(angx);];

Roty=[cosd(angy),0,sind(angy); 0,1,0; -sind(angy),0,cosd(angy);];

Rotz=[cosd(angz),-sind(angz),0; sind(angz),cosd(angz),0; 0,0,1;];

Trasl=[0,0,50]'; HZ=zeros(4); HZ(1:3,1:3)=Rotx*Roty*Rotz; HZ(1:3,4)=Trasl; HZ(4,4)=1;

% Aplicación de transformación proyectiva para creación de puntos

sintéticos k=1:4; puntoscam=HZ*puntos(:,k);

paso=0.1; puntosmod=zeros(3,4); puntosHOM=zeros(3,4); puntosHOM2=zeros(3,4); aux=0; modulo=zeros(1,(limsup-liminf)/paso); conti=1;

% Paso a coordenadas pixel, y despues a coordenadas homogéneas

for k=1:4 puntosg(:,k)=A*puntoscam(:,k); puntosHOM(:,k)=puntosg(:,k)./puntosg(3,k); end

% Barrido de ángulo PITCH

84

84

tic for angtheta=liminf:paso:limsup

for k=1:4

rota=[1,0,0,0; 0,cosd(angtheta),-sind(angtheta),0; 0,sind(angtheta),cosd(angtheta),50; 0,0,0,1];

puntosmod(:,k)=A*rota*puntos(:,k); puntosHOM2(:,k)=puntosmod(:,k)./puntosmod(3,k); aux=aux+norm(puntosHOM(:,k)-puntosHOM2(:,k))^2;

%Cálculo de función de error

if k==4 modulo(conti)=aux; %Aplicación de función de

error para las 4 esquinas aux=0; end end conti=conti+1; end

mini=min(modulo(1:(limsup-liminf)/paso)); posi=find(abs(modulo-mini)<0.0001,1); angPITCH=liminf+paso*(posi-1); %Posición -1 para

tener en cuenta que el primer elemento es modulo(1) [~,col]=size(modulo); %en vez de modulo(0) toc figure plot(1:col,modulo) title('Función de error frente a iteración'); xlabel('Nº de iteración, dividir por 10 para ángulo') ylabel('Valor de función de error') grid on

2 ángulos Roll y Pitch:

2.1 Fuerza bruta

function [angPITCH,angROLL] =

dosANGULOS(angx,liminfpitch,limsuppitch,angy,liminfroll,limsuproll)


ancho=12; largo=150; D=0; B=0; a=[B,largo+D,0,1]; b=[ancho-B,largo+D,0,1]; c=[ancho,0,D,1]; d=[B,0,0,1];





%Prueba a altura H y girado 45º en X y con varios puntos

puntos=[a',b',c',d']; puntosg=zeros(3,4);

% PRUEBA RESOLUCION H INVERSA CON PUNTOS SINTETICOS

angz=0;




Trasl=[0,0,50]';

86

86

HZ=zeros(4); HZ(1:3,1:3)=Rotx*Roty*Rotz; HZ(1:3,4)=Trasl; HZ(4,4)=1; P=A*HZ; T=zeros(4); T(1:3,4)=Trasl; T(4,4)=1;

paso=0.1; puntosmod=zeros(3,4); puntosHOM=zeros(3,4); puntosHOM2=zeros(3,4); aux=0; modulo=zeros((limsuppitch-liminfpitch)/paso,(limsuproll-

liminfroll)/paso); conti=1; [~,num_pitch]=size(liminfpitch:paso:limsuppitch); [~,num_roll]=size(liminfroll:paso:limsuproll);

for k=1:4

puntosg(:,k)=P*puntos(:,k); puntosHOM(:,k)=puntosg(:,k)./puntosg(3,k);

end

tic for fila=1:num_pitch

angtheta=liminfpitch+paso*fila; rotap=[1,0,0; 0,cosd(angtheta),-sind(angtheta); 0,sind(angtheta),cosd(angtheta);];

for colu=1:num_roll

angphi=liminfroll+paso*colu; rotar=[cosd(angphi),0,sind(angphi); 0,1,0; -sind(angphi),0,cosd(angphi);];

T(1:3,1:3)=rotap*rotar;

for k=1:4

puntosmod(:,k)=A*T*puntos(:,k); puntosHOM2(:,k)=puntosmod(:,k)./puntosmod(3,k); aux=aux+norm(puntosHOM(:,k)-puntosHOM2(:,k))^2;

if k==4 modulo(fila,colu)=aux; aux=0; end end

conti=conti+1;

end

end toc

[~,posi]=min(modulo(:)); [posi_fila,posi_colu]=ind2sub(size(modulo),posi); angROLL=liminfroll+paso*posi_colu; angPITCH=liminfpitch+paso*posi_fila; surf(modulo)

2.2 fminunc

function modulo = Modulo2Ang(angulo,angx,angy,angz)

% ASIGNAN LOS VALORES DE LOS PARAMETROS % angulo,angx,angy,angz,modo % PREPARA FUNCION PARA EVALUAR CON PARAMETROS % f=@(angulo)Modulo2Ang(angulo,angx,angy,angz); % SOLUCIONA % x0=[10,15]; % [angulo,fval] = fminunc(f,x0)


ancho=12; largo=150; D=0; B=0; a=[B,largo+D,0,1]; b=[ancho-B,largo+D,0,1]; c=[ancho,0,D,1]; d=[B,0,0,1]; puntos=[a',b',c',d'];



u0=(3670/2)/1.4;

88

88

v0=(2740/2)/1.4; dist_foc=3.6*10^(-3); mx=1.4*10^(-6); my=1.4*10^(-6); alfax=dist_foc/mx; alfay=dist_foc/my;


GIRO=zeros(4); puntosg=zeros(3,4); puntosmod=zeros(3,4); puntosHOM=zeros(3,4); puntosHOM2=zeros(3,4); aux=0;

rotap=[1,0,0; 0,cosd(angulo(1)),-sind(angulo(1)); 0,sind(angulo(1)),cosd(angulo(1));];

rotar=[cosd(angulo(2)),0,sind(angulo(2)); 0,1,0; -sind(angulo(2)),0,cosd(angulo(2));];

GIRO(1:3,1:3)=rotap*rotar;




Trasl=[0,0,50]'; HZ=zeros(4); HZ(1:3,1:3)=Rotx*Roty*Rotz; HZ(1:3,4)=Trasl; HZ(4,4)=1; P=A*HZ; GIRO(1:3,4)=Trasl; GIRO(4,4)=1;

for k=1:4

puntosg(:,k)=P*puntos(:,k); puntosHOM(:,k)=puntosg(:,k)./puntosg(3,k);

end

for k=1:4

puntosmod(:,k)=A*GIRO*puntos(:,k); puntosHOM2(:,k)=puntosmod(:,k)./puntosmod(3,k); aux=aux+norm(puntosHOM(:,k)-puntosHOM2(:,k))^2;

if k==4 modulo=aux; aux=0; end

end

3 3 Ángulos ROLL, PITCH y YAW

function modulo = Modulo3Ang(angulo,angR,angP,angY)

% ASIGNAN LOS VALORES DE LOS PARAMETROS % angulo,angx,angy,angz,modo % PREPARA FUNCION PARA EVALUAR CON PARAMETROS % f2=@(angulo)Modulo3Ang(angulo,angR,angP,angY); % SOLUCIONA % x0_2=[-10,5,30]; % [pose,fval] = fminunc(f2,x0_2)


ancho=12; largo=150; D=0; B=0; a=[B,largo+D,0,1]; b=[ancho-B,largo+D,0,1]; c=[ancho,0,D,1]; d=[B,0,0,1]; puntos=[a',b',c',d']; puntosg=zeros(3,4); puntosmod=zeros(3,4); puntosHOM=zeros(3,4); puntosHOM2=zeros(3,4); aux=0;

%DATOS DE CAMARA CON SENSOR OV5647 (INSTALADA)

90

90

%Area imagen 3.6736 mm x 2.7384 mm, ¿es area de imagen efectiva?




GIRO=zeros(4); rotar=[1,0,0; 0,cosd(angulo(1)),-sind(angulo(1)); 0,sind(angulo(1)),cosd(angulo(1));];

rotap=[cosd(angulo(2)),0,sind(angulo(2)); 0,1,0; -sind(angulo(2)),0,cosd(angulo(2));];

rotay=[cosd(angulo(3)),-sind(angulo(3)),0; sind(angulo(3)),cosd(angulo(3)),0; 0,0,1;];

GIRO(1:3,1:3)=rotar*rotap*rotay;

RotR=[1,0,0; 0,cosd(angR),-sind(angR); 0,sind(angR),cosd(angR);];

RotP=[cosd(angP),0,sind(angP); 0,1,0; -sind(angP),0,cosd(angP);];

RotY=[cosd(angY),-sind(angY),0; sind(angY),cosd(angY),0; 0,0,1;];

Trasl=[0,0,50]'; HZ=zeros(4); HZ(1:3,1:3)=RotR*RotP*RotY; HZ(1:3,4)=Trasl; HZ(4,4)=1; GIRO(1:3,4)=Trasl; GIRO(4,4)=1; k=1:4; puntoscam=HZ\puntos(:,k); %Necesario para pasar los puntos en puntoscam2=GIRO\puntos(:,k);

%ejes mundo a ejes cámara

for k=1:4 puntosg(:,k)=A*puntoscam2(:,k); puntosHOM(:,k)=puntosg(:,k)./puntosg(3,k); end

for k=1:4

puntosmod(:,k)=A*puntoscam(:,k); puntosHOM2(:,k)=puntosmod(:,k)./puntosmod(3,k); aux=aux+norm(puntosHOM(:,k)-puntosHOM2(:,k))^2;


end

4 Orientación completa (Roll, Pitch, Yaw X, Y, Z)

4.1 mediante fminunc

unction modulo = PoseTotal(pose,angR,angP,angY,x,y,z)

% ASIGNAN LOS VALORES DE LOS PARAMETROS % angulo,angx,angy,angz,x,y,z % PREPARA FUNCION PARA EVALUAR CON PARAMETROS % f2=@(pose)PoseTotal(pose,angR,angP,angY,x,y,z); % SOLUCIONA % x0_2=[10,-20,50,50,-20,60]; % options = optimset('fminunc'); % options = optimset('LargeScale','off'); % options.MaxFunEvals = 10000 % % [pose,fval] = fminunc(f2,x0_2,options)

% angR=30; % angP=10; % angY=60; % x=20; % y=-30; % z=60;

%DATOS DE CAMARA CON SENSOR OV5647 (INSTALADA) %Area imagen 3.6736 mm x 2.7384 mm, ¿es area de imagen efectiva? los

92

92

640x480? o %los 2592x1944 %pixel 1.4 x 1.4 um %dist_focal 3.6 mm % array center en -320,-175 um (del centro), de la imagen 2740/2,3670/2

um



ancho=12; largo=150; D=0; B=0; a=[B,largo+D,0,1]; b=[ancho-B,largo+D,0,1]; c=[ancho,0,D,1]; d=[B,0,0,1]; puntos=[a',b',c',d'];

puntosg=zeros(3,4); puntosmod=zeros(3,4); puntosHOM=zeros(3,4); puntosHOM2=zeros(3,4); aux=0;

rotap=[1,0,0; 0,cosd(pose(2)),-sind(pose(2)); 0,sind(pose(2)),cosd(pose(2));];

rotay=[cosd(pose(3)),0,sind(pose(3)); 0,1,0; -sind(pose(3)),0,cosd(pose(3));];

rotar=[cosd(pose(1)),-sind(pose(1)),0; sind(pose(1)),cosd(pose(1)),0; 0,0,1;];

Trasl=[pose(4),pose(5),pose(6)]'; HZ=zeros(4); HZ(1:3,1:3)=rotap*rotay*rotar; HZ(1:3,4)=Trasl; HZ(4,4)=1;

Rotx=[1,0,0; 0,cosd(angP),-sind(angP); 0,sind(angP),cosd(angP);];

Roty=[cosd(angY),0,sind(angY); 0,1,0; -sind(angY),0,cosd(angY);];

Rotz=[cosd(angR),-sind(angR),0; sind(angR),cosd(angR),0; 0,0,1;];

Trasla=[x,y,z]'; H2=zeros(4); H2(1:3,1:3)=Rotx*Roty*Rotz; H2(1:3,4)=Trasla; H2(4,4)=1;

for k=1:4 puntosg(:,k)=A*H2*puntos(:,k); puntosHOM(:,k)=puntosg(:,k)./puntosg(3,k); end

for k=1:4

puntosmod(:,k)=A*HZ*puntos(1:4,k); puntosHOM2(:,k)=puntosmod(:,k)./puntosmod(3,k); aux=aux+norm(puntosHOM(:,k)-puntosHOM2(:,k))^2;


end

4.2 mediante lsqnonlin

function modulo = PoseTotalLSQ(pose,angR,angP,angY,x,y,z)

% ASIGNAN LOS VALORES DE LOS PARAMETROS % angulo,angx,angy,angz,x,y,z

94

94

% PREPARA FUNCION PARA EVALUAR CON PARAMETROS % f2=@(pose)PoseTotalLSQ(pose,angR,angP,angY,x,y,z); % SOLUCIONA % x0_2=[40,-30,20,-150,-20,50]; % [pose2,fval2] = lsqnonlin (f2,x0_2)

% angR=21.55; % angP=-32.23; % angY=25.37; % x=109.96; % y=-155.52; % z=27.57;

%DATOS DE CAMARA CON SENSOR OV5647 (INSTALADA) %Area imagen 3.6736 mm x 2.7384 mm, ¿es area de imagen efectiva? los

640x480? o %los 2592x1944 %pixel 1.4 x 1.4 um %dist_focal 3.6 mm % array center en -320,-175 um (del centro), de la imagen 2740/2,3670/2

um



ancho=12; largo=150; D=0; B=0; a=[B,largo+D,0,1]; b=[ancho-B,largo+D,0,1]; c=[ancho,0,D,1]; d=[B,0,0,1]; puntos=[a',b',c',d']; puntosg=zeros(3,4); puntosmod=zeros(3,4); puntosHOM=zeros(3,4); puntosHOM2=zeros(3,4);


rotay=[cosd(pose(3)),0,sind(pose(3)); 0,1,0;

-sind(pose(3)),0,cosd(pose(3));];


Trasl=[pose(4),pose(5),pose(6)]'; HZ=zeros(4); HZ(1:3,1:3)=rotap*rotay*rotar; HZ(1:3,4)=Trasl; HZ(4,4)=1;

Rotx=[1,0,0; 0,cosd(angP),-sind(angP); 0,sind(angP),cosd(angP);];

Roty=[cosd(angY),0,sind(angY); 0,1,0; -sind(angY),0,cosd(angY);];

Rotz=[cosd(angR),-sind(angR),0; sind(angR),cosd(angR),0; 0,0,1;];

Trasla=[x,y,z]'; H2=zeros(4); H2(1:3,1:3)=Rotx*Roty*Rotz; H2(1:3,4)=Trasla; H2(4,4)=1;

for k=1:4 puntosg(:,k)=A*H2*puntos(:,k); puntosHOM(:,k)=puntosg(:,k)./puntosg(3,k); puntosmod(:,k)=A*HZ*puntos(:,k); puntosHOM2(:,k)=puntosmod(:,k)./puntosmod(3,k); end

k=1:4; modulo=puntosHOM(:,k)-puntosHOM2(:,k);

5 Orientación complete imagen real

5.1 Con puntos seleccionados manualmente

function modulo = PoseTotalRealLSQ(pose)

96

96

% PREPARA FUNCION PARA EVALUAR CON PARAMETROS % f=@(pose)PoseTotalRealLSQ(pose); % VALORES INICIALES % x0=[10,-80,-50,-200,100,-60]; % SOLUCION % [pose,fval] = lsqnonlin (f,x0)





% IMAGEN 7351 % ap=[428,130,1]; % bp=[456,123,1]; % cp=[449,235,1]; % dp=[519,219,1]; % ep=[361,208,1]; % fp=[366,212,1]; % gp=[326,254,1]; % hp=[321,250,1]; % IMAGEN 3170 % ap=[145,370,1]; % bp=[123,320,1]; % cp=[349,377,1]; % dp=[336,353,1]; % % FRAME 6102 % ap=[357,201,1]; % bp=[375,214,1]; % cp=[301,246,1]; % dp=[334,271,1];

%FRAME 10000 % ap=[502,457,1]; % bp=[152,452,1]; % cp=[372,226,1]; % dp=[329,226,1];

%FRAME 7835 % ap=[312,180,1]; % bp=[345,176,1]; % cp=[272,289,1]; % dp=[365,287,1];

%FRAME 4091 ap=[348,232,1]; bp=[376,235,1]; cp=[315,340,1]; dp=[392,349,1];

% puntosIMAGEN=[ap',bp',cp',dp'];



puntosREAL=[a',b',c',d']; puntosmod=zeros(3,4); puntosHOM=zeros(3,4);




Trasl=[pose(4),pose(5),pose(6)]'; HZ=zeros(4); HZ(1:3,1:3)=rotap*rotay*rotar; HZ(1:3,4)=Trasl; HZ(4,4)=1; k=1:4; puntoscam=HZ\puntosREAL(:,k); %Necesario para pasar los puntos

en %ejes mundo a ejes cámara

98

98

for k=1:4 puntosmod(:,k)=A*puntoscam(:,k);

%Conversión de unidades naturales a pixeles puntosHOM(:,k)=puntosmod(:,k)./puntosmod(3,k);

%Conversión del resultado a coord. homogéneas end

k=1:4; modulo=puntosIMAGEN(:,k)-puntosHOM(:,k);

5.2 Con puntos calculados automáticamente

function modulo = PoseTotalRealLSQAUTO(pose,puntosIMAGEN)

% PREPARA FUNCION PARA EVALUAR CON PARAMETROS % f=@(pose)PoseTotalRealLSQAUTO(pose,puntosIMAGEN); % VALORES INICIALES % x0=[10,-10,50,-10,100,20]; % SOLUCION % [pose,fval] = lsqnonlin (f,x0)





% IMAGEN 7351 % ap=[428,130,1]; % bp=[456,123,1]; % cp=[449,235,1]; % dp=[519,219,1]; % ep=[361,208,1];

% fp=[366,212,1]; % gp=[326,254,1]; % hp=[321,250,1]; % IMAGEN 3170 % ap=[145,370,1]; % bp=[123,320,1]; % cp=[349,377,1]; % dp=[336,353,1]; % % FRAME 6102 % ap=[357,201,1]; % bp=[375,214,1]; % cp=[301,246,1]; % dp=[334,271,1];

%FRAME 10000 % ap=[502,457,1]; % bp=[152,452,1]; % cp=[372,226,1]; % dp=[329,226,1];

%FRAME 7835 % ap=[312,180,1]; % bp=[345,176,1]; % cp=[272,289,1]; % dp=[365,287,1];

% % puntosIMAGEN=[ap',bp',cp',dp'];



puntosREAL=[a',b',c',d']; puntosmod=zeros(3,4); puntosHOM=zeros(3,4);



rotar=[cosd(pose(1)),-sind(pose(1)),0;

100

100

sind(pose(1)),cosd(pose(1)),0; 0,0,1;];

Trasl=[pose(4),pose(5),pose(6)]'; HZ=zeros(4); HZ(1:3,1:3)=rotap*rotay*rotar; HZ(1:3,4)=Trasl; HZ(4,4)=1; k=1:4; puntoscam=HZ\puntosREAL(:,k); %Necesario para pasar los puntos

en %ejes mundo a ejes cámara

for k=1:4 puntosmod(:,k)=A*puntoscam(:,k);

%Conversión de unidades naturales a pixeles puntosHOM(:,k)=puntosmod(:,k)./puntosmod(3,k);

%Conversión del resultado a coord. homogéneas end

k=1:4; modulo=puntosIMAGEN(:,k)-puntosHOM(:,k);

6 Detección de esquinas

vuelo=VideoReader('vuelo4.mp4'); vueloproc=VideoReader('video procesado.AVI'); imagen=zeros((396-86),(520-122)); contadorppix=0; contador=0; h=1; esquinas=zeros(4,2,9470); posicion=zeros(1,9470); a=zeros(9470,2); b=zeros(9470,2); c=zeros(9470,2); d=zeros(9470,2);

tic for k=1:9470

actual=read(vueloproc,k); frameproc=actual(86:395,122:519,:); [fil,col]=size(frameproc(:,:,2));

for i=1:fil; for j=1:col;

if frameproc(i,j,2) > 200 && frameproc(i,j,1) < 50 &&

frameproc(i,j,3) < 50 imagen(i,j)=255; contadorppix=contadorppix+1;

else imagen(i,j)=0; end end end if contadorppix > 570 [I,J]=find(imagen > 200); %Posicion de los puntos de la

pista IJ=[I,J]; [~,idx]=min(IJ*[1 1; -1 -1; 1 -1; -1 1].'); esquinas(:,:,k)=IJ(idx,:); posicion(h)=k; h=h+1; esquinas(1:4,2,k)=esquinas(1:4,2,k)+122; esquinas(1:4,1,k)=esquinas(1:4,1,k)+86; aux=esquinas(1:4,1,k); %Intercambio de

columnas esquinas(1:4,1,k)=esquinas(1:4,2,k); esquinas(1:4,2,k)=aux;

%BUSQUEDA DE PUNTOS A Y B ordena= sort(esquinas(1:4,2,k)); ordena2=ordena(1:2);

canda1(1,2)=esquinas(find(esquinas(1:4,2,k) ==

ordena2(1),1),2 ... ,k); %#ok<*FNDSB>


ordena2(1),1),1 ... ,k);


ordena2(2),1),2 ... ,k);


ordena2(2),1),1 ... ,k);

if canda1(1) < canda2(1) a(k,1:2)=canda1; b(k,1:2)=canda2; else a(k,1:2)=canda2; b(k,1:2)=canda1; end

%BUSQUEDA DE PUNTOS C Y D ordena4=ordena(3:4);

candc1(1,2)=esquinas(find(esquinas(1:4,2,k) ==

ordena4(1),1),2 ... ,k);


ordena4(1),1),1 ... ,k);


ordena4(2),1),2 ...

102

102

,k);


ordena4(2),1),1 ... ,k);

if candc1(1) < candc2(1) c(k,1:2)=candc1; d(k,1:2)=candc2; else c(k,1:2)=candc2; d(k,1:2)=candc1; end end contadorppix=0; end toc

% posi=find(abs(posicion - 7385)<0.001); actual=read(vueloproc,posicion(posi)); %#ok<*VIDREAD> % imtool(actual) imshow(actual) hold on; scatter puntosIMAGEN=[[a(posicion(posi),1:2),1]',[b(posicion(posi),1:2),1]' ... ,[c(posicion(posi),1:2),1]',[d(posicion(posi),1:2),1]'];

imshow(actual) hold on % scatter(puntosIMAGEN(1,1:4),puntosIMAGEN(2,1:4),'+','r')

plot([puntosIMAGEN(1,1:2),puntosIMAGEN(1,4),puntosIMAGEN(1,3),... puntosIMAGEN(1,1)],[puntosIMAGEN(2,1:2),puntosIMAGEN(2,4),... puntosIMAGEN(2,3),puntosIMAGEN(2,1)],'-ro')

estimación de la posición relativa de aeronaves a partir...

Documents