Captulo 3.

Descripcin del
software
En este captulo se har una introduccin a los entornos de programacin
utilizados que son Matlab y Arduino, luego se presentarn las principales libreras
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
programacin 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 inalmbricamente. El
robot puede funcionar en tres modos: manual, guiado por GPS y guiado por
cmara. 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 prdida
de la seal de GPS el robot ver afectada su capacidad de orientarse, en ese
instante el ordenador recibir la prdida de seal y cambiar el modo de
funcionamiento a guiado por cmara. Una vez en este modo se manejar al robot
mediante un control visual con la cmara para llevarlo de vuelta al UAV.
Los microcontroladores se programan en lenguaje C utilizando el entorno
de programacin Arduino. Para el control en el ordenador se ha hecho uso del
entorno Matlab.

3.1.a. Arduino
Arduino es una plataforma electrnica abierta para desarrollar prototipos.
Se programa a travs de una interfaz tambin de cdigo abierto y libre distribucin
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
Captulo 3 Descripcin del software

26

marcas y arquitecturas. Antes de comenzar a programarlos es necesario cargar en

ellos un pequeo programa llamado bootloader que permite comunicarse con el
ordenador a travs de un puerto serie, recibir el programa y grabarlo en la
memoria flash del micro.

Figura 12. Interfaz Arduino de programacin

La programacin es similar al lenguaje C++ con algunas simplificaciones y

modificaciones. Incorpora funciones tpicas de los microcontroladores y libreras
que facilitan el uso de perifricos como servos o pantallas LCD. Arduino presenta
un alto nivel de abstraccin, por lo que no es necesario entender cmo 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
rpida.

3.1.b. MATLAB
Matlab es la abreviacin de Matrix Laboratory y se trata de un software de
carcter matemtico con un lenguaje de programacin propio (lenguaje M) de alto
nivel. Esta herramienta es escogida en muchas ocasiones para implementar
algoritmos y visualizacin de datos en grficas en 2 o 3 dimensiones, ya que posee
un potente mdulo que permite personalizar su apariencia.

Captulo 3 Descripcin del software

27

Las funcionalidades de Matlab se pueden ampliar con paquetes de funciones

especficas llamados Toolboxes. Existen toolboxes de diferentes categoras como
procesamiento de imgenes, estadstica, control
Una caracterstica 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 hara cualquier funcin hay que crear un archivo llamado MEX que sirve
de enlace. La principal razn para utilizar cdigo en C/C++ es obtener un cdigo
mucho ms eficiente. Sin embargo, no se realiza toda la programacin en un
lenguaje a bajo nivel como C/C++ debido a que requerira 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. Programacin 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 accin que debe llevar a cabo mediante comandos enviados por
el puerto serie a travs del Zigbee. Para controlar el movimiento del robot se han
definido cuatro estados de funcionamiento que se seleccionan segn 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 informacin de posicin y
orientacin, e implementar los controladores de los motores para que el robot se
dirija a la posicin 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 tambin.
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 posicin del robot proporcionada por el GPS y siempre que la
seal de la posicin 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 lnea
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. Tambin es posible establecer una
trayectoria basada en varios puntos GPS.

Captulo 3 Descripcin del software

28

3. Modo de funcionamiento con cmara. En caso de perder la seal

del GPS y estar el robot en lnea de visin de la cmara del UGAV se
calcula la posicin del robot usando la informacin de la imagen de la
cmara 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 vehculo
por radiocontrol y el movimiento es controlado por una persona.
Este modo de funcionamiento podra utilizarse para tareas crticas
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.

Cambio
de
modo desde
Matlab

MODO
DE
PARADA

No hay visin
del robot
desde la
cmara

GUIADO
CON
CMARA

MODO
NORMAL
CON GPS

Fallo en la
posicin GPS

Figura 13. Esquema de transiciones entre modos

Tanto en el modo 2 como el modo 3 el robot es el encargado de mantener la

direccin que le sea asignada. En el modo 2 el robot calcula la direccin de
referencia a partir de la posicin 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
posicin que debe seguir. En ambos casos no conocemos la direccin real del robot,
sino la medida que obtenemos de la IMU, que est basada en las medidas de los
giroscopios y magnetmetros. Para que el efectivamente pueda seguir la direccin
de referencia se ha implementado un controlador tipo PI con el siguiente esquema:

Captulo 3 Descripcin del software

29

u0

error

referencia de
direccin

+
-

actuacin

Robot

direccin del
robot

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 accin
reacciona frente al error actual y la segunda accin garantiza que con el tiempo el
error llegue a anularse. La accin de control queda de la forma siguiente
expresada en el dominio temporal:

Donde u(t) es la accin de control, e(t) es el error en el instante t, Kp es la

ganancia de la accin proporcional y Ki es la ganancia de la accin integral.
El Ardupilot Mega se conecta a la placa de control de motores mediante dos
seales PWM (Puse Width Modulation, traducido Modulacin de Ancho de Pulso)
que varan su ancho de pulso para indicar la velocidad y el giro del robot. Se ha
elegido este tipo de conexin debido a que se estn utilizando los puertos serie de
la placa para otras conexiones y esta conexin permite su uso directamente para el
modo radiocontrolado.
La seal PWM es una seal digital estndar en los equipos de radiocontrol
que funciona a 50Hz y utiliza un ancho de pulso que vara entre 1 y 2 ms. La
posicin cero corresponde a 1.5 ms. En este caso hay dos seales, 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 atrs, 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 accin de control debe ser en
rgimen permanente 1.5 ms, por eso se ha aadido el trmino u0 que se suma a la
salida del controlador.
La accin de control anterior es la forma continua del controlador, pero al
implementarla en un microcontrolador no se puede evaluar de manera continua la
funcin, sino a intervalos de tiempo. Por eso se va a utilizar una forma discretizada
del controlador PI que aproxima el trmino integral mediante un sumatorio:

Captulo 3 Descripcin del software

30

( )

( )

La expresin anterior es evaluada por el microcontrolador ms de 10 veces

por segundo, por lo que
.
El implementar un controlador con accin integral conlleva un efecto
indeseable que hace que el trmino 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 algn mecanismo anti-windup. El mayor perjuicio de
este efecto ocurre cuando vara la referencia y el controlador tarda demasiado en
reaccionar para llegar a la nueva referencia. En este caso se ha optado por limitar
la accin integral para que no crezca indefinidamente.
Este controlador requiere el ajuste de dos parmetros: Kp y Ki que se han
ajustado manualmente hasta obtener una sobreoscilacin lo ms pequea posible
y un tiempo de respuesta suficientemente rpido.
Cuando el error supera los 30 se ha optado por hacer que el robot gire
sobre s mismo, ya que lo permite la configuracin diferencial. De esta forma el
controlador PI slo se utiliza para corregir la trayectoria en un margen de 60.
actuacin
del PID

0
-30

Gira a la
izquierda

30

Gira a la
izquierda

Figura 15. Accin segn el error en la trayectoria

3.1.d. Obtencin de la direccin de referencia en modo guiado por

GPS
Como se ha mencionado anteriormente en el modo 2 el robot se gua a
partir de suposicin actual y la posicin destino, dados por su latitud y longitud.
Si llamamos a la posicin de inicio (x0, y0) y la posicin final (x1, y1) (latitud,
longitud) podemos obtener la direccin que debe seguir el robot para llegar a la
posicin final de la siguiente manera:

Captulo 3 Descripcin del software

31

latitud

direccin
norte

(x1, y1)

direccin
deseada

(x0, y0)

longitud
Figura 16. Direccin de referencia en coordenadas geogrficas

La IMU del Ardupilot Mega proporciona el ngulo de guiada (yaw), que

corresponde con la direccin a la que apunta el robot, medido como el ngulo que
forma con respecto a la direccin norte. Devuelve un ngulo comprendido entre 0
y 360, donde 90 corresponde con la direccin este y 270 la direccin oeste.
Por tanto, se calcula la direccin deseada de la siguiente manera:
(

Para que la funcin arcotangente devuelva un ngulo que abarque toda la

circunferencia habr que tener en cuenta los signos del numerador y denominador.
Esta funcin se halla implementada en la mayora de compiladores, en este caso
est disponible en Matlab y Arduino y se denomina atan2(numerador,
denominador).

3.1.e. Obtencin de la direccin de referencia en modo guiado por

cmara
De manera parecida al modo anterior, en el modo 3 se utilizan las
coordenadas sobre el suelo en lugar de las coordenadas geogrficas latitud y
longitud. En este caso la posicin se mide en X e Y respecto a la posicin de la
cmara. En este problema es conocida la orientacin de la cmara con respecto al
norte (), ya que el UAV tambin dispone de sensores que le indican la orientacin.
Este ngulo tambin es necesario para realizar comparaciones entre la posicin
adquirida por el GPS y la posicin que devuelve la cmara.

Captulo 3 Descripcin del software

32

direccin
norte
y

cmara
Figura 17. Coordenadas obtenidas por la cmara (vista en planta)

Por tanto, la direccin 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 posicin donde se encuentra la cmara y por tanto el UAV al que debe
retornar el robot.

direccin
norte

(x0, y0)

direccin
deseada
(x1, y1) = (0, 0)

Figura 18. Direccin de referencia en coordenadas de la cmara

La direccin de referencia () se calcula de la siguiente forma:

( )

Esta direccin es calculada en Matlab y enviada al Ardupilot Mega para

realizar el seguimiento.

Captulo 3 Descripcin del software

33

3.1.f. Consideraciones al programar el microcontrolador

Al programar el microcontrolador se borra la configuracin almacenada en
la memoria eeprom. Esta configuracin est compuesta por los offsets de los
acelermetros y giroscopios, necesarios para conocer la posicin cero de los
ngulos de giro del robot. Por tanto, es necesario llevar a cabo al menos una vez la
calibracin de los sensores. El proceso es sencillo y lo realiza automticamente el
programa del Ardupilot Mega de la siguiente manera:
1. Colocar el interruptor de modo a la posicin de calibracin
(interruptor hacia el lado de los pines de entrada/salida)
2. Presionar el botn 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 calibracin
a travs de un terminal de puerto serie.
El programa resultante ocupa 44576 bytes de un total disponible de 126976
bytes.

3.2. LIBRERA DE SEGUIMIENTO CON CMARA

Para realizar el seguimiento del robot a travs de la cmara se ha utilizado
la librera 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 inters al comienzo de la secuencia y el algoritmo los sigue, aprende su
apariencia y detecta su aparicin en el video.

Captulo 3 Descripcin del software

34

Figura 19. Seleccin 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 librera libre de visin artificial OpenCV 2.2 escrita en
C/C++. Actualmente se est desarrollando una versin para portar toda la librera
a C/C++ y de esa manera hacerlo ms rpido 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 tamao del objeto, no as a la rotacin del objeto, aunque permite
cierta adaptacin a la rotacin, ya que continuamente est comprobando las
variaciones del objeto para aprenderlas.
Esta librera requiere una captura inicial del objeto a seguir, luego se
extraen caractersticas de este objeto y posteriormente se busca en cada frame
similitudes con las caractersticas del objeto. Si se encuentra el objeto deseado en
el frame se aade a la base de datos, de manera que el algoritmo va aprendiendo
analizando cada similitud clasificndola como positiva o negativa. Las clasificadas
como negativas sirven para no equivocarse en la identificacin 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 nmero de muestras del objeto.

Captulo 3 Descripcin del software

35

En el estado actual de la librera slo puede seguir un objeto a la vez y el

programa corre en un ncleo aunque el ordenador disponga de ms. No dispone de
aprendizaje previo, sino que debe ir aprendiendo conforme se ejecuta.

3.2.a. Modificacin de la librera

Para las necesidades de este proyecto se ha necesitado modificar ciertos
aspectos de la librera TLD. En primer lugar se requiere hacer un seguimiento del
robot desde posiciones cercanas hasta distancias intermedias, donde el robot
ocupa un tamao reducido en la imagen de la cmara. Por eso se ha aumentado la
resolucin que usa el programa sacrificando velocidad de procesamiento. En la
prctica no es efectivo alejar el robot ms de 20 metros pues su tamao resulta
demasiado pequeo.
Para mejorar el seguimiento se puede seguir aumentando la resolucin
utilizada, pero sera 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 cdigo de Matlab es ms ineficiente. Por ese motivo la librera de
seguimiento utiliza parte del cdigo previamente compilado.
El programa trabaja con imgenes de 640x480 pxeles 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 posicin en que muestra el objeto
escalndolo segn la proporcin 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 posicin de seguimiento en cada instante y la secuencia de video
original. Como no se utiliza ninguna compresin para el video, los ficheros
generados pueden ocupar alrededor de 1Gb para unos minutos de video.

3.2.b. Modificacin del mtodo de aprendizaje de la librera

Una de las pruebas que se han realizado en cuanto a modificacin de la
librera se trata del aprendizaje previo de las distintas perspectivas del robot. De
esa manera no se depende del aprendizaje en lnea 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 ms que mejorar la respuesta del programa la han empeorado
incluso llegando a no detectar al robot en ninguna posicin. Por eso se han
desechado estos cambios realizados.

Captulo 3 Descripcin del software

36

3.3. PROGRAMACIN EN MATLAB

En Matlab se ha implementado adems del seguimiento con la cmara el
controlador del robot y la telemetra. Para agrupar todas estas funciones se ha
aprovechado la estructura original del programa de seguimiento con cmara. La
estructura general se basa en un programa principal llamado run_TLD.m, ste
crea las estructuras bsicas de funcionamiento de la librera de seguimiento y
llama a la funcin programa.m que es el encargado de inicializar la cmara, la
comunicacin 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
cmara y se procesa para seguir al robot. La telemetra funciona de manera
asncrona, procesando los datos segn van llegando, aunque el envo de comandos
al robot se realiza en el mismo bucle. Por ltimo, realiza un log de todos los
fotogramas e informacin de posicin del robot desde la cmara y el GPS, adems
de orientacin 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. Conversin de coordenadas geogrficas a coordenadas UTM

Los datos adquiridos del GPS son coordenadas geogrficas (latitud y
longitud) dadas en grados y es necesario convertirlas a unidades de metros para
comparar la posicin del GPS con la posicin obtenida de la cmara.
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 dcada de 1940 por los EEUU es un sistema de
proyeccin de la superficie terrestre para representarla sobre un plano. La
representacin de la superficie de la Tierra sobre un plano supone un problema, ya
que no existe ningn modo de hacerlo sin deformarla y en cualquier caso se
cometen errores en distancias, superficies y ngulos. La proyeccin UTM conserva
los ngulos pero distorsiona las superficies y distancias originales.
La proyeccin 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 nmeros 1 y 0). Esta rejilla sirve para disminuir la
distorsin lineal a un mximo de un 1% a costa de perder la continuidad.

Captulo 3 Descripcin del software

37

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, kilmetros, millas, etc

Huso

Zona

El origen de coordenadas del sistema UTM es diferente para cada huso,

situndose en la interseccin del meridiano central del huso (a 3 de los lmites del
huso) con el ecuador. Sin embargo, el origen de coordenadas no tiene valor X=0 e
Y=0, sino que X=500 kilmetros y para el hemisferio sur Y=10.000 kilmetros.
Para convertir la posicin de latitud y longitud a metros UTM se ha utilizado
una funcin llamada deg2utm disponible en el repositorio de funciones Matlab
Central. La forma de utilizar esta funcin 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. Telemetra desde el puerto serie

Como se ha explicado en el captulo anterior se utiliza una comunicacin
entre el robot y el ordenador a travs de unos mdulos inalmbricos Zigbee, que
proporcionan una comunicacin serie. En Matlab es posible comunicarse a travs
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
Captulo 3 Descripcin del software

38

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
configuracin estndar 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
funcin externa al programa principal y que se ejecuta cuando se han
alcanzado un nmero de bytes de entrada o bien cuando se ha
recibido una cadena de texto.
Set(handle, BytesAvailableFcn, {@mycallback,argumento1});
Opcionalmente se pueden aadir argumentos a la funcin llamada en
este caso mycallback colocndolos seguidos en una lista de
corchetes.
Si queremos que se ejecute cada vez que lleguen X bytes desde el
puerto serie aadiremos las lneas:
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 lnea)
aadiremos la lnea:
set(tld.serial,'Terminator',' LF/CR');
Set(handle, BytesAvailableFcnMode, terminator);
En este caso el terminador es una lnea nueva seguida de un retorno
de carro pero puede ser cualquier combinacin de estos o cualquier
otro byte que corresponda a un carcter ASCII (0-127).
Captulo 3 Descripcin del software

39

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 comunicacin serie

En la comunicacin entre Matlab y el Ardupilot Mega es necesario enviar
informacin de distinto tipo segn el modo en que est funcionando el robot. Para
diferenciar los tipos de datos que se envan desde Matlab al Ardupilot Mega se ha
creado un sencillo protocolo que enva 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: Direccin de referencia para el modo de guiado por cmara +

variable de 16 bits correspondiente a la direccin 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 comunicacin, enviando toda la informacin codificada en ASCII y
separando las variables con comas. Cada paquete enviado se termina con un
carcter especial llamado retorno de carro, que corresponde en decimal al
nmero 13. Las variables que se envan en cada paquete son: latitud y longitud
actual del robot, nmero de satlites que observa el GPS, 3 ngulos de orientacin
de la IMU y distancia al punto de destino. En el caso de las variables de latitud y
longitud se envan

Captulo 3 Descripcin del software

40

3.4. CONVERSIN DE POSICIN EN LA IMAGEN A POSICIN

EN EL TERRENO
La librera de seguimiento proporciona el recuadro en el que se halla el
objeto, en este caso el robot. Se tomar como posicin del robot el centro del
recuadro. El tamao del recuadro puede dar cierta informacin de distancia a la
cmara pero se ha visto en la prctica que resulta poco fiable y no se ajusta
siempre perfectamente al tamao del robot.
La calibracin de la cmara consiste en determinar la relacin entre la
posicin de lo que aparece en el plano de la imagen y su localizacin en el mundo
real.
La imagen observada por la cmara ve la proyeccin sobre el sensor de la
posicin del robot. Para obtener una medida de distancia es necesario deshacer la
perspectiva de la imagen. Si consideramos que el suelo proyectado en la cmara
guarda una perspectiva frontal, lo cual se asemeja bastante a la realidad, se puede
aplicar una transformacin del tipo proyectiva.
Una transformacin afn se define como una transformacin lineal no
singular seguida de una traslacin, si se hace ms generalizada aadiendo una
transformacin de perspectiva obtendremos una transformacin proyectiva. La
representacin matricial de esta transformacin es
( )

)( )

O en forma compacta
[

Donde A es una matriz no singular, es decir, con determinante distinto de

cero; t es el trmino de traslacin, p es el trmino de perspectiva y e es el trmino
del escalado. En total hay 9 variables aunque slo 8 son independientes. Con esto
queda que una proyectividad puede ser calculada a partir de la correspondencia
entre cuatro puntos.
Esta conversin ser vlida en terrenos relativamente llanos, y debido a las
limitaciones indicadas en el punto 3.2b sera 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 podra aterrizar el avin. Por ese motivo se puede aplicar la transformacin con
un error aceptable en el clculo de la posicin del robot.

Captulo 3 Descripcin del software

41

3.4.a. Transformacin proyectiva en Matlab

Matlab proporciona un conjunto de funciones que resuelven el sistema de
ecuaciones de una transformacin proyectiva a partir de cuatro o ms
equivalencias entre puntos. Estas equivalencias se encuentran comprobando la
posicin real en que se encuentra un punto visto en la imagen captada por la
cmara.
La funcin de Matlab que resuelve la transformacin 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 posicin de los
pxel de la imagen, base_points es otra matriz de la misma dimensin que identifica
cada pixel con su posicin en la realidad. El nmero de puntos N debe ser mayor
que 4 para este tipo de transformacin.

Esta funcin tambin se puede utilizar para otros tipos de transformaciones

indicndolo 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 transformacin, en este caso
t, se puede realizar la transformacin directa (coordenadas de la imagen a
coordenadas en la realidad) con la funcin tformfwd, y la transformacin inversa
con la funcin tforminv. La forma de utilizarlo es como sigue:
[X, Y] = tformfwd(t, u, v);
Donde (u, v) es la posicin del objeto en la imagen y (X, Y) es la
correspondencia de esos puntos en la realidad.

3.4.b. Calibracin de la cmara

Las funciones anteriormente descritas requieren al menos 4
correspondencias entre puntos de la imagen y en la realidad, por lo que es
necesaria la calibracin de estos puntos debidamente para un buen
funcionamiento del programa. En Matlab se ha desarrollado una interfaz para
calibrar la cmara. Primero se marca un punto en la imagen, luego hay que medir
Captulo 3 Descripcin del software

42

la posicin respecto a la cmara, se toman al menos 4 puntos y despus el

programa guarda la configuracin en un archivo llamado calibracion.m para abrirlo
posteriormente por el programa de seguimiento en Matlab. Un mayor nmero de
puntos reducir errores en la medida.

Figura 21. Interfaz en Matlab para calibrar la cmara y puntos introducidos

Aunque esta calibracin puede ser vlida para tomar medidas con la cmara
es necesario comprobar a posteriori con el robot que las distancias medidas sean
correctas. Normalmente habr un pequeo desajuste entre la medida de la cmara
y la real que puede eliminarse realizando manualmente un ajuste ms fino de la
transformacin.
Los valores de correspondencia de los 13 puntos medidos en la calibracin
de la cmara son los siguientes, teniendo en cuenta que la resolucin de la imagen
utilizada para calibrar es de 800x600 pxeles y posteriormente se reescalarn los
resultados a 480x640 con que funciona el programa.
Puntos en la imagen
(u, v) (pxeles)

Posicin en la realidad
(X, Y) (metros)

(400, 221)

( 10.5, 0)

(400, 236)

( 8, 0)

Captulo 3 Descripcin del software

43

(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 calibracin sea vlida es necesario

que de un experimento al siguiente la cmara no modifique su altura ni inclinacin
respecto al suelo. Para solucionar esto se ha colocado la cmara en un trpode y se
ha marcado la posicin para no variarla. La posicin que se ha adoptado para la
cmara es una altura desde el suelo al centro de la cmara de 1.2 metros y una
inclinacin de 20 grados respecto la vertical.

Captulo 3 Descripcin del software

44

20

1.2 m

Figura 22. Trpode utilizado para mantener la posicin de la cmara

Captulo 3 Descripcin del software

45