Embed Size (px)
Citation preview
Capítulo 3 – Descripción del software 26
Capítulo 3. Descripción del software
En este capítulo se hará una introducción a los entornos de programación
utilizados que son Matlab y Arduino, luego se presentarán las principales librerías
de software utilizadas que proporcionan una base para implementar el
controlador del robot tanto en el PC como en el robot. Por último se detallará la
programación del controlador del robot y el PC.
3.1. CONTROLADOR DEL ROBOT
El controlador del robot se ha implementado tanto a bordo del robot como
desde el ordenador con el que se encuentra comunicado inalámbricamente. El
robot puede funcionar en tres modos: manual, guiado por GPS y guiado por
cámara. En un funcionamiento normal el robot se guiará por el GPS siguiendo un
camino de puntos ya establecidos (waypoints). En caso de que exista una pérdida
de la señal de GPS el robot verá afectada su capacidad de orientarse, en ese
instante el ordenador recibirá la pérdida de señal y cambiará el modo de
funcionamiento a guiado por cámara. Una vez en este modo se manejará al robot
mediante un control visual con la cámara para llevarlo de vuelta al UAV.
Los microcontroladores se programan en lenguaje C utilizando el entorno
de programación Arduino. Para el control en el ordenador se ha hecho uso del
entorno Matlab.
3.1.a. Arduino
Arduino es una plataforma electrónica abierta para desarrollar prototipos.
Se programa a través de una interfaz también de código abierto y libre distribución
que permite programar, compilar y cargar el programa en el microcontrolador. No
admite cualquier microcontrolador, sino determinados microcontroladores de la
marca Atmel de 8 bits, aunque existen proyectos para extenderlo a diferentes
Capítulo 3 – Descripción del software 27
marcas y arquitecturas. Antes de comenzar a programarlos es necesario cargar en
ellos un pequeño programa llamado bootloader que permite comunicarse con el
ordenador a través de un puerto serie, recibir el programa y grabarlo en la
memoria flash del micro.
Figura 12. Interfaz Arduino de programación
La programación es similar al lenguaje C++ con algunas simplificaciones y
modificaciones. Incorpora funciones típicas de los microcontroladores y librerías
que facilitan el uso de periféricos como servos o pantallas LCD. Arduino presenta
un alto nivel de abstracción, por lo que no es necesario entender cómo funciona el
hardware a bajo nivel, sino que el funcionamiento viene enmascarado dentro de
las funciones. Por estos motivos presenta una curva de aprendizaje bastante
rápida.
3.1.b. MATLAB
Matlab es la abreviación de Matrix Laboratory y se trata de un software de
carácter matemático con un lenguaje de programación propio (lenguaje M) de alto
nivel. Esta herramienta es escogida en muchas ocasiones para implementar
algoritmos y visualización de datos en gráficas en 2 o 3 dimensiones, ya que posee
un potente módulo que permite personalizar su apariencia.
Capítulo 3 – Descripción del software 28
Las funcionalidades de Matlab se pueden ampliar con paquetes de funciones
específicas llamados Toolboxes. Existen toolboxes de diferentes categorías como
procesamiento de imágenes, estadística, control…
Una característica de Matlab que es utilizada en este proyecto es la
capacidad para poder llamar a funciones y subrutinas escritas en lenguaje C/C++.
Para que las funciones creadas en C/C++ puedan ser cargadas en Matlab
como lo haría cualquier función hay que crear un archivo llamado MEX que sirve
de enlace. La principal razón para utilizar código en C/C++ es obtener un código
mucho más eficiente. Sin embargo, no se realiza toda la programación en un
lenguaje a bajo nivel como C/C++ debido a que requeriría mucho tiempo en
implementarlo.
Este programa no es libre ni gratuito. Existe un software alternativo a
Matlab que es Octave, aunque no llega a ser compatible por completo.
3.1.c. Programación del Ardupilot Mega
El controlador del robot programado en el Ardupilot Mega se comporta
como un esclavo del controlador programado en Matlab, de manera que recibe
instrucciones de la acción que debe llevar a cabo mediante comandos enviados por
el puerto serie a través del Zigbee. Para controlar el movimiento del robot se han
definido cuatro estados de funcionamiento que se seleccionan según la forma en
que se desee manejar el robot. El objetivo del controlador del robot será adquirir
los datos de los sensores internos, que proporcionan la información de posición y
orientación, e implementar los controladores de los motores para que el robot se
dirija a la posición deseada con la velocidad adecuada.
Los estados de funcionamiento del robot que se han definido son los cuatro
siguientes:
1. Modo de parada. En este modo el robot permanecerá quieto hasta
cambiar a otro modo de funcionamiento. Puede ser utilizado como
parada de emergencia también.
2. Modo de funcionamiento normal. El robot es guiado hacia un
punto de destino definido por la latitud y la longitud usando para
ello la posición del robot proporcionada por el GPS y siempre que la
señal de la posición sea suficientemente exacta. El punto de destino
vendrá dado por el controlador en Matlab que será transmitido al
Ardupilot Mega, luego el robot mantendrá una trayectoria en línea
recta hacia el punto marcado. Existe un margen alrededor del punto
donde se considerará que el robot se encuentra en dicho punto,
siendo este margen configurable. También es posible establecer una
trayectoria basada en varios puntos GPS.
Capítulo 3 – Descripción del software 29
3. Modo de funcionamiento con cámara. En caso de perder la señal
del GPS y estar el robot en línea de visión de la cámara del UGAV se
calcula la posición del robot usando la información de la imagen de la
cámara para hacer volver al robot a su base de acoplamiento. En este
caso el controlador en Matlab indicará al robot hacia donde debe
girar y el robot será el encargado de realizar el giro ordenado.
4. Modo manual. En este caso se utilizará el robot como un vehículo
por radiocontrol y el movimiento es controlado por una persona.
Este modo de funcionamiento podría utilizarse para tareas críticas
como el acoplamiento al UGAV o en caso de fallo de los modos 2 y 3.
Es un modo que ha de seleccionarse de forma manual para utilizarlo.
Figura 13. Esquema de transiciones entre modos
Tanto en el modo 2 como el modo 3 el robot es el encargado de mantener la
dirección que le sea asignada. En el modo 2 el robot calcula la dirección de
referencia a partir de la posición actual y el destino en forma de latitud y longitud.
En el modo 3 es el controlador en Matlab el que proporciona cada cierto tiempo la
posición que debe seguir. En ambos casos no conocemos la dirección real del robot,
sino la medida que obtenemos de la IMU, que está basada en las medidas de los
giroscopios y magnetómetros. Para que el efectivamente pueda seguir la dirección
de referencia se ha implementado un controlador tipo PI con el siguiente esquema:
MODO
DE
PARADA
MODO
NORMAL
CON GPS
GUIADO
CON
CÁMARA
Cambio de
modo desde
Matlab
Fallo en la
posición GPS
No hay visión
del robot
desde la
cámara
Capítulo 3 – Descripción del software 30
Figura 14. Esquema del controlador PI
El controlador PI (proporcional + integral) incluye dos tipos de acciones:
una proporcional al error y otra acumulativa del error. La primera acción
reacciona frente al error actual y la segunda acción garantiza que con el tiempo el
error llegue a anularse. La acción de control queda de la forma siguiente
expresada en el dominio temporal:
∫
Donde u(t) es la acción de control, e(t) es el error en el instante t, Kp es la
ganancia de la acción proporcional y Ki es la ganancia de la acción integral.
El Ardupilot Mega se conecta a la placa de control de motores mediante dos
señales PWM (Puse Width Modulation, traducido Modulación de Ancho de Pulso)
que varían su ancho de pulso para indicar la velocidad y el giro del robot. Se ha
elegido este tipo de conexión debido a que se están utilizando los puertos serie de
la placa para otras conexiones y esta conexión permite su uso directamente para el
modo radiocontrolado.
La señal PWM es una señal digital estándar en los equipos de radiocontrol
que funciona a 50Hz y utiliza un ancho de pulso que varía entre 1 y 2 ms. La
posición cero corresponde a 1.5 ms. En este caso hay dos señales, una
correspondiente a la velocidad y otra al giro. Un ancho de pulso de 2 ms indica la
mayor velocidad hacia delante y 1 ms la mayor velocidad hacia atrás, y en el caso
del giro 1 ms indica el mayor giro a la izquierda y 2 ms el mayor giro a la derecha.
Cuando tenemos un error nulo la salida de la acción de control debe ser en
régimen permanente 1.5 ms, por eso se ha añadido el término u0 que se suma a la
salida del controlador.
La acción de control anterior es la forma continua del controlador, pero al
implementarla en un microcontrolador no se puede evaluar de manera continua la
función, sino a intervalos de tiempo. Por eso se va a utilizar una forma discretizada
del controlador PI que aproxima el término integral mediante un sumatorio:
Robot
P
I
+
+ + -
referencia de
dirección dirección del
robot
error
actuación
+
u0
+
Capítulo 3 – Descripción del software 31
( ) ( ) ∑
La expresión anterior es evaluada por el microcontrolador más de 10 veces
por segundo, por lo que .
El implementar un controlador con acción integral conlleva un efecto
indeseable que hace que el término integral se acumule y sature el controlador
cuando el error no es capaz de llegar a la referencia. Este efecto se llama windup y
se puede corregir mediante algún mecanismo anti-windup. El mayor perjuicio de
este efecto ocurre cuando varía la referencia y el controlador tarda demasiado en
reaccionar para llegar a la nueva referencia. En este caso se ha optado por limitar
la acción integral para que no crezca indefinidamente.
Este controlador requiere el ajuste de dos parámetros: Kp y Ki que se han
ajustado manualmente hasta obtener una sobreoscilación lo más pequeña posible
y un tiempo de respuesta suficientemente rápido.
Cuando el error supera los 30° se ha optado por hacer que el robot gire
sobre sí mismo, ya que lo permite la configuración diferencial. De esta forma el
controlador PI sólo se utiliza para corregir la trayectoria en un margen de 60°.
Figura 15. Acción según el error en la trayectoria
3.1.d. Obtención de la dirección de referencia en modo guiado por
GPS
Como se ha mencionado anteriormente en el modo 2 el robot se guía a
partir de suposición actual y la posición destino, dados por su latitud y longitud.
Si llamamos a la posición de inicio (x0, y0) y la posición final (x1, y1) (latitud,
longitud) podemos obtener la dirección que debe seguir el robot para llegar a la
posición final de la siguiente manera:
30° -30°
0°
Gira a la
izquierda
Gira a la
izquierda
actuación
del PID
Capítulo 3 – Descripción del software 32
Figura 16. Dirección de referencia en coordenadas geográficas
La IMU del Ardupilot Mega proporciona el ángulo de guiñada (yaw), que
corresponde con la dirección a la que apunta el robot, medido como el ángulo que
forma con respecto a la dirección norte. Devuelve un ángulo comprendido entre 0°
y 360°, donde 90° corresponde con la dirección este y 270° la dirección oeste.
Por tanto, se calcula la dirección deseada de la siguiente manera:
(
)
Para que la función arcotangente devuelva un ángulo que abarque toda la
circunferencia habrá que tener en cuenta los signos del numerador y denominador.
Esta función se halla implementada en la mayoría de compiladores, en este caso
está disponible en Matlab y Arduino y se denomina atan2(numerador,
denominador).
3.1.e. Obtención de la dirección de referencia en modo guiado por
cámara
De manera parecida al modo anterior, en el modo 3 se utilizan las
coordenadas sobre el suelo en lugar de las coordenadas geográficas latitud y
longitud. En este caso la posición se mide en X e Y respecto a la posición de la
cámara. En este problema es conocida la orientación de la cámara con respecto al
norte (α), ya que el UAV también dispone de sensores que le indican la orientación.
Este ángulo también es necesario para realizar comparaciones entre la posición
adquirida por el GPS y la posición que devuelve la cámara.
(x0, y0)
(x1, y
1)
latitud
longitud
θ
dirección
norte
dirección
deseada
Capítulo 3 – Descripción del software 33
Figura 17. Coordenadas obtenidas por la cámara (vista en planta)
Por tanto, la dirección deseada se puede obtener de manera similar a la
anterior, siendo el punto inicial donde se encuentra el robot (x0, y0) y el punto final
de destino (x1, y1). En este caso el punto de destino coincidirá con el origen (0,0)
que es la posición donde se encuentra la cámara y por tanto el UAV al que debe
retornar el robot.
Figura 18. Dirección de referencia en coordenadas de la cámara
La dirección de referencia (θ) se calcula de la siguiente forma:
(
)
Esta dirección es calculada en Matlab y enviada al Ardupilot Mega para
realizar el seguimiento.
x
y
dirección
norte
cámara
α
(x0, y0)
(x1, y
1) = (0, 0)
X
Y
θ
dirección
norte
dirección
deseada
α
β
Capítulo 3 – Descripción del software 34
3.1.f. Consideraciones al programar el microcontrolador
Al programar el microcontrolador se borra la configuración almacenada en
la memoria eeprom. Esta configuración está compuesta por los offsets de los
acelerómetros y giroscopios, necesarios para conocer la posición “cero” de los
ángulos de giro del robot. Por tanto, es necesario llevar a cabo al menos una vez la
calibración de los sensores. El proceso es sencillo y lo realiza automáticamente el
programa del Ardupilot Mega de la siguiente manera:
1. Colocar el interruptor de modo a la posición de calibración
(interruptor hacia el lado de los pines de entrada/salida)
2. Presionar el botón de reset
3. Observar que los leds A y C parpadeen.
4. Esperar hasta que los tres leds permanezcan encendidos
permanentemente.
5. Volver a colocar el interruptor al modo normal y resetear para
iniciar el funcionamiento normal.
Si se conecta el USB al ordenador se podrá ver el progreso de la calibración
a través de un terminal de puerto serie.
El programa resultante ocupa 44576 bytes de un total disponible de 126976
bytes.
3.2. LIBRERÍA DE SEGUIMIENTO CON CÁMARA
Para realizar el seguimiento del robot a través de la cámara se ha utilizado
la librería de licencia libre OpenTLD para Matlab. TLD (Tracks, Learns, Detect) es
un algoritmo de seguimiento de un objeto en una secuencia de video. Se selecciona
el objeto de interés al comienzo de la secuencia y el algoritmo los sigue, aprende su
apariencia y detecta su aparición en el video.
Capítulo 3 – Descripción del software 35
Figura 19. Selección del robot como objetivo a seguir
TLD está escrito en C y Matlab y ha sido desarrollado para Windows, Mac o
Linux. Está basado en la librería libre de visión artificial OpenCV 2.2 escrita en
C/C++. Actualmente se está desarrollando una versión para portar toda la librería
a C/C++ y de esa manera hacerlo más rápido que Matlab.
Este algoritmo es capaz de seguir casi cualquier objeto desde distintos
ángulos e incluso variando las condiciones de luz. El seguimiento es invariante a
los cambios de tamaño del objeto, no así a la rotación del objeto, aunque permite
cierta adaptación a la rotación, ya que continuamente está comprobando las
variaciones del objeto para aprenderlas.
Esta librería requiere una captura inicial del objeto a seguir, luego se
extraen características de este objeto y posteriormente se busca en cada frame
similitudes con las características del objeto. Si se encuentra el objeto deseado en
el frame se añade a la base de datos, de manera que el algoritmo va aprendiendo
analizando cada similitud clasificándola como positiva o negativa. Las clasificadas
como negativas sirven para no equivocarse en la identificación del objeto, y las
clasificadas como positivas son muestras del objeto deseado. Por esto, el algoritmo
mejora su eficacia con el tiempo al tener un mayor número de muestras del objeto.
Capítulo 3 – Descripción del software 36
En el estado actual de la librería sólo puede seguir un objeto a la vez y el
programa corre en un núcleo aunque el ordenador disponga de más. No dispone de
aprendizaje previo, sino que debe ir aprendiendo conforme se ejecuta.
3.2.a. Modificación de la librería
Para las necesidades de este proyecto se ha necesitado modificar ciertos
aspectos de la librería TLD. En primer lugar se requiere hacer un seguimiento del
robot desde posiciones cercanas hasta distancias intermedias, donde el robot
ocupa un tamaño reducido en la imagen de la cámara. Por eso se ha aumentado la
resolución que usa el programa sacrificando velocidad de procesamiento. En la
práctica no es efectivo alejar el robot más de 20 metros pues su tamaño resulta
demasiado pequeño.
Para mejorar el seguimiento se puede seguir aumentando la resolución
utilizada, pero sería necesario un ordenador de mayor potencia al utilizado en este
proyecto, o bien trasladar el programa a C++ cuando éste se halle disponible, ya
que el código de Matlab es más ineficiente. Por ese motivo la librería de
seguimiento utiliza parte del código previamente compilado.
El programa trabaja con imágenes de 640x480 píxeles en escala de grises,
pero se ha modificado para que muestre por pantalla al usuario una imagen de
800x600 a color. Esto requiere modificar la posición en que muestra el objeto
escalándolo según la proporción de resoluciones.
Para tratar los datos captados por el programa a posteriori se guardan los
resultados en un fichero. El fichero se guarda en una subcarpeta llamada “_output”
y almacena la posición de seguimiento en cada instante y la secuencia de video
original. Como no se utiliza ninguna compresión para el video, los ficheros
generados pueden ocupar alrededor de 1Gb para unos minutos de video.
3.2.b. Modificación del método de aprendizaje de la librería
Una de las pruebas que se han realizado en cuanto a modificación de la
librería se trata del aprendizaje previo de las distintas perspectivas del robot. De
esa manera no se depende del aprendizaje en línea que realiza el programa.
La manera de implementar este aprendizaje se ha realizado seleccionando
manualmente el recuadro que contiene al robot variando la perspectiva del mismo.
Los resultados más que mejorar la respuesta del programa la han empeorado
incluso llegando a no detectar al robot en ninguna posición. Por eso se han
desechado estos cambios realizados.
Capítulo 3 – Descripción del software 37
3.3. PROGRAMACIÓN EN MATLAB
En Matlab se ha implementado además del seguimiento con la cámara el
controlador del robot y la telemetría. Para agrupar todas estas funciones se ha
aprovechado la estructura original del programa de seguimiento con cámara. La
estructura general se basa en un programa principal llamado “run_TLD.m”, éste
crea las estructuras básicas de funcionamiento de la librería de seguimiento y
llama a la función “programa.m” que es el encargado de inicializar la cámara, la
comunicación serie y el objeto a seguir por el programa; e incluye el bucle de
funcionamiento normal del programa.
Dentro del bucle del programa se adquiere un fotograma nuevo de la
cámara y se procesa para seguir al robot. La telemetría funciona de manera
asíncrona, procesando los datos según van llegando, aunque el envío de comandos
al robot se realiza en el mismo bucle. Por último, realiza un log de todos los
fotogramas e información de posición del robot desde la cámara y el GPS, además
de orientación del robot. Al pulsar cualquier tecla el programa se detiene y se
vuelca a un fichero todos los datos recogidos en el log, que se guardan en una
carpeta especificada en el archivo “run_TLD.m” en la variable opt.output.
3.3.a. Conversión de coordenadas geográficas a coordenadas UTM
Los datos adquiridos del GPS son coordenadas geográficas (latitud y
longitud) dadas en grados y es necesario convertirlas a unidades de metros para
comparar la posición del GPS con la posición obtenida de la cámara.
Para un punto de la superficie terrestre la longitud mide la distancia con el
meridiano de Greenwich (meridiano cero) y la latitud mide la distancia con el
ecuador, ambos expresados en grados sexagesimales.
El sistema de coordenadas UTM (Universal Transversa Mercator) ideado en
1569 y desarrollado en la década de 1940 por los EEUU es un sistema de
proyección de la superficie terrestre para representarla sobre un plano. La
representación de la superficie de la Tierra sobre un plano supone un problema, ya
que no existe ningún modo de hacerlo sin deformarla y en cualquier caso se
cometen errores en distancias, superficies y ángulos. La proyección UTM conserva
los ángulos pero distorsiona las superficies y distancias originales.
La proyección UTM divide la Tierra en 60 husos de 6° longitud cada uno y
20 bandas de 8° de latitud (representadas por letras desde C hasta X excluyendo I y
O por su similitud con los números 1 y 0). Esta rejilla sirve para disminuir la
distorsión lineal a un máximo de un 1% a costa de perder la continuidad.
Capítulo 3 – Descripción del software 38
Figura 20. Husos y zonas de la rejilla del sistema UTM
Un punto de la superficie quedará localizado con los siguientes datos:
X e Y en metros, kilómetros, millas, etc
Huso
Zona
El origen de coordenadas del sistema UTM es diferente para cada huso,
situándose en la intersección del meridiano central del huso (a 3° de los límites del
huso) con el ecuador. Sin embargo, el origen de coordenadas no tiene valor X=0 e
Y=0, sino que X=500 kilómetros y para el hemisferio sur Y=10.000 kilómetros.
Para convertir la posición de latitud y longitud a metros UTM se ha utilizado
una función llamada deg2utm disponible en el repositorio de funciones Matlab
Central. La forma de utilizar esta función es la siguiente: [X,Y,zona] =
deg2utm(Latitud,Longitud), con latitud y longitud en unidades de grados
sexagesimales y X e Y en metros.
3.3.b. Telemetría desde el puerto serie
Como se ha explicado en el capítulo anterior se utiliza una comunicación
entre el robot y el ordenador a través de unos módulos inalámbricos Zigbee, que
proporcionan una comunicación serie. En Matlab es posible comunicarse a través
de un puerto serie con funciones que incorpora Matlab. Para abrir un puerto serie
en Matlab se deben seguir los siguientes pasos:
1. Abrir el puerto de comunicaciones deseado y asignarlo a un
manejador
Capítulo 3 – Descripción del software 39
handle=serial(‘COM4’,'Name','Nombre');
Hay que seleccionar el puerto utilizado, en este ejemplo es COM4
pero es necesario consultar en cada caso cual se está utilizando.
Opcionalmente se le puede asignar un nombre a dicho puerto.
2. Seleccionar la tasa de transferencia deseada, en este caso hemos
configurado el Zigbee a 57600bps
set(handle,'Baudrate',57600);
3. Configurar el bit de parada, la paridad y los bits de datos. Esta es una
configuración estándar y no suele hacer falta modificarla.
set(handle,'StopBits',1);
set(handle,'DataBits',8);
set(handle,'Parity','none');
4. Opcionalmente se pueden leer los datos de entrada desde una
función externa al programa principal y que se ejecuta cuando se han
alcanzado un número de bytes de entrada o bien cuando se ha
recibido una cadena de texto.
Set(handle,’ BytesAvailableFcn’, {@mycallback,argumento1});
Opcionalmente se pueden añadir argumentos a la función llamada en
este caso “mycallback” colocándolos seguidos en una lista de
corchetes.
Si queremos que se ejecute cada vez que lleguen X bytes desde el
puerto serie añadiremos las líneas:
Set(handle,’BytesAvailableFcnCount’,X);
Set(handle, ‘BytesAvailableFcnMode’,’ byte’);
Si queremos que se ejecute cuando se ha escrito una cadena
terminada en un terminador (retorno de carro o nueva línea)
añadiremos la línea:
set(tld.serial,'Terminator',' LF/CR');
Set(handle, ‘BytesAvailableFcnMode’,’ terminator’);
En este caso el terminador es una línea nueva seguida de un retorno
de carro pero puede ser cualquier combinación de estos o cualquier
otro byte que corresponda a un carácter ASCII (0-127).
Capítulo 3 – Descripción del software 40
5. Se abre el puerto para comenzar a recibir y enviar datos.
fopen(handle);
6. Matlab proporciona distintos modos de comunicarse por el puerto
serie. En este caso se utilizan las funciones fread y fwrite.
7. Una vez no se vaya a seguir utilizando el puerto serie hay que
cerrarlo.
fclose(handle);
delete(handle);
3.3.c. Protocolo de comunicación serie
En la comunicación entre Matlab y el Ardupilot Mega es necesario enviar
información de distinto tipo según el modo en que esté funcionando el robot. Para
diferenciar los tipos de datos que se envían desde Matlab al Ardupilot Mega se ha
creado un sencillo protocolo que envía un byte de cabecera para clasificar los datos
enviados. De esta forma se definen 4 tipos de cabecera, con su valor en formato
hexadecimal:
0xAB: cambio de modo de funcionamiento + variable de 1 byte
correspondiente al modo.
0xA9: Latitud del punto de destino en modo de guiado por GPS +
variable de 32 bits correspondiente a la latitud en formato de punto
fijo con 7 decimales.
0xAA: Longitud del punto de destino en modo de guiado por GPS +
variable de 32 bits correspondiente a la longitud en formato de
punto fijo con 7 decimales.
0xA8: Dirección de referencia para el modo de guiado por cámara +
variable de 16 bits correspondiente a la dirección de referencia en
formato de punto fijo con 2 decimales.
Para enviar los datos desde el Ardupilot Mega a Matlab se ha optado por
otro modo de comunicación, enviando toda la información codificada en ASCII y
separando las variables con comas. Cada paquete enviado se termina con un
carácter especial llamado “retorno de carro”, que corresponde en decimal al
número 13. Las variables que se envían en cada paquete son: latitud y longitud
actual del robot, número de satélites que observa el GPS, 3 ángulos de orientación
de la IMU y distancia al punto de destino. En el caso de las variables de latitud y
longitud se envían
Capítulo 3 – Descripción del software 41
3.4. CONVERSIÓN DE POSICIÓN EN LA IMAGEN A POSICIÓN
EN EL TERRENO
La librería de seguimiento proporciona el recuadro en el que se halla el
objeto, en este caso el robot. Se tomará como posición del robot el centro del
recuadro. El tamaño del recuadro puede dar cierta información de distancia a la
cámara pero se ha visto en la práctica que resulta poco fiable y no se ajusta
siempre perfectamente al tamaño del robot.
La calibración de la cámara consiste en determinar la relación entre la
posición de lo que aparece en el plano de la imagen y su localización en el mundo
real.
La imagen observada por la cámara ve la proyección sobre el sensor de la
posición del robot. Para obtener una medida de distancia es necesario deshacer la
perspectiva de la imagen. Si consideramos que el suelo proyectado en la cámara
guarda una perspectiva frontal, lo cual se asemeja bastante a la realidad, se puede
aplicar una transformación del tipo proyectiva.
Una transformación afín se define como una transformación lineal no
singular seguida de una traslación, si se hace más generalizada añadiendo una
transformación de perspectiva obtendremos una transformación proyectiva. La
representación matricial de esta transformación es
(
) (
)( )
O en forma compacta
[
]
Donde A es una matriz no singular, es decir, con determinante distinto de
cero; t es el término de traslación, p es el término de perspectiva y e es el término
del escalado. En total hay 9 variables aunque sólo 8 son independientes. Con esto
queda que una proyectividad puede ser calculada a partir de la correspondencia
entre cuatro puntos.
Esta conversión será válida en terrenos relativamente llanos, y debido a las
limitaciones indicadas en el punto 3.2b sería para distancias cercanas-intermedias.
Si se tiene en cuenta que el robot se encuentra en un principio acoplado al UAV se
tendrá que el terreno de alrededor no debe ser muy inclinado ni accidentado, pues
no podría aterrizar el avión. Por ese motivo se puede aplicar la transformación con
un error aceptable en el cálculo de la posición del robot.
Capítulo 3 – Descripción del software 42
3.4.a. Transformación proyectiva en Matlab
Matlab proporciona un conjunto de funciones que resuelven el sistema de
ecuaciones de una transformación proyectiva a partir de cuatro o más
equivalencias entre puntos. Estas equivalencias se encuentran comprobando la
posición real en que se encuentra un punto visto en la imagen captada por la
cámara.
La función de Matlab que resuelve la transformación se llama cp2tform, y
requiere los siguientes argumentos:
t = cp2tform(input_points, base_points,'projective');
El argumento input_points es una matriz Nx2 que guarda la posición de los
píxel de la imagen, base_points es otra matriz de la misma dimensión que identifica
cada pixel con su posición en la realidad. El número de puntos N debe ser mayor
que 4 para este tipo de transformación.
Esta función también se puede utilizar para otros tipos de transformaciones
indicándolo con el tercer argumento, el cual puede ser 'affine', 'similarity',
'polynomial', 'piecewise linear' o 'lwm'.
Una vez se ha creado la variable que guarda la transformación, en este caso
t, se puede realizar la transformación directa (coordenadas de la imagen a
coordenadas en la realidad) con la función tformfwd, y la transformación inversa
con la función tforminv. La forma de utilizarlo es como sigue:
[X, Y] = tformfwd(t, u, v);
Donde (u, v) es la posición del objeto en la imagen y (X, Y) es la
correspondencia de esos puntos en la realidad.
3.4.b. Calibración de la cámara
Las funciones anteriormente descritas requieren al menos 4
correspondencias entre puntos de la imagen y en la realidad, por lo que es
necesaria la calibración de estos puntos debidamente para un buen
funcionamiento del programa. En Matlab se ha desarrollado una interfaz para
calibrar la cámara. Primero se marca un punto en la imagen, luego hay que medir
Capítulo 3 – Descripción del software 43
la posición respecto a la cámara, se toman al menos 4 puntos y después el
programa guarda la configuración en un archivo llamado calibracion.m para abrirlo
posteriormente por el programa de seguimiento en Matlab. Un mayor número de
puntos reducirá errores en la medida.
Figura 21. Interfaz en Matlab para calibrar la cámara y puntos introducidos
Aunque esta calibración puede ser válida para tomar medidas con la cámara
es necesario comprobar a posteriori con el robot que las distancias medidas sean
correctas. Normalmente habrá un pequeño desajuste entre la medida de la cámara
y la real que puede eliminarse realizando manualmente un ajuste más fino de la
transformación.
Los valores de correspondencia de los 13 puntos medidos en la calibración
de la cámara son los siguientes, teniendo en cuenta que la resolución de la imagen
utilizada para calibrar es de 800x600 píxeles y posteriormente se reescalarán los
resultados a 480x640 con que funciona el programa.
Puntos en la imagen (u, v) (píxeles)
Posición en la realidad (X, Y) (metros)
(400, 221) ( 10.5, 0)
(400, 236) ( 8, 0)
Capítulo 3 – Descripción del software 44
(400, 279) ( 5, 0)
(400, 204) ( 14, 0)
(311, 229) ( 9, -1.2)
(211, 230) ( 9, -2.4)
(133, 230) ( 9, -3.6)
(249, 278) ( 5, -1.2)
(93, 277) ( 5, -2.4)
(476, 17) ( 11, 1.2)
(548, 214) ( 11, 2.4)
(581, 229) ( 9, 2.4)
(643, 346) ( 3, 1.2)
Es conveniente indicar que para que la calibración sea válida es necesario
que de un experimento al siguiente la cámara no modifique su altura ni inclinación
respecto al suelo. Para solucionar esto se ha colocado la cámara en un trípode y se
ha marcado la posición para no variarla. La posición que se ha adoptado para la
cámara es una altura desde el suelo al centro de la cámara de 1.2 metros y una
inclinación de 20 grados respecto la vertical.
Capítulo 3 – Descripción del software 45
Figura 22. Trípode utilizado para mantener la posición de la cámara
1.2 m
20°
