2. descripción del robotbibing.us.es/proyectos/abreproy/70435/fichero/02.pdf · navegación y...

Trabajo Fin de Máster

Navegación y control de un robot móvil tolerante fallos en sensores -12-

2. Descripción del robot A continuación se describe el robot utilizado durante las pruebas. Éste está

compuesto por una parte mecánica, una parte electrónica y una parte software.

2.1.Parte mecánica del robot

La parte mecánica está compuesta por una base comercial que consta de cuatro ruedas en una configuración llamada Skid Steer. Esta configuración es similar a la diferencial, pero al constar de cuatro ruedas se produce deslizamiento transversal, cosa que no ocurre con la diferencial. A efectos del control puede manejarse de manera similar si no se tiene en cuenta el deslizamiento.

Figura3. Modelo 3D de la plataforma móvil.

Esta base consta de un motor para cada rueda y un chasis de aluminio de dimensiones 280x297x126 mm. Esta configuración está preparada para maniobrar sobre terrenos no preparados. Además, este kit incluye la electrónica de potencia necesaria para manejar los motores de cada lado por separado y un microcontrolador Atmel ATMega168 para realizar el control a bajo nivel de los motores.

2.2.Parte electrónica del robot

La parte electrónica del robot está compuesta por una placa controladora Wild Thumper, que realiza un control a bajo nivel de los motores, y una placa Ardupilot Mega, con los sensores necesarios para el control a alto nivel del robot.



2.2.1. Placa controladora Wild Thumper

Esta placa controladora combina una placa de desarrollo Arduino con un doble puente H para controlar los motores, con su correspondiente disipador térmico de aluminio. El corazón de la placa es un microcontrolador AVR ATMega168, que viene cargado con el bootloader de Arduino. El código puede ser fácilmente accedido y modificado en el IDE de Arduino con el chip en la placa FTDI y conector mini-USB. Todas las cabeceras de la placa están acompañadas por las líneas de tierra y de voltaje para permitir la conexión de servomotores. En la figura 4 se muestra esta placa, indicando sus conexiones y principales elementos.

Figura4. Placa controladora Wild Thumper.

Las principales características de esta placa son las siguientes:

- Doble puente H FET de hasta 15A continuos con protección individual por fusibles.

- Sensor de corriente para cada puente H. - Indicador LED de fusible fundido. - 7 pines de E/S digitales terminados con un terminal de tres pines para permitir

conectar servos directamente. - 5 entradas analógicas terminados con un terminal de tres pines para alimentar

sensores. - Regulador de 5A para alimentar la parte lógica, servo y sensores. - Comunicación por USB, TTL serie y I2C. También puede trabajar con RC y

entradas analógicas. - Microcontrolador Atmega 168 con 16K de memoria flash. - Compatibilidad con Arduino/AVR. Programable vía USB o interfaz ISP.



La programación del microcontrolador de esta placa se realiza mediante la interfaz de Arduino. Esta placa dispone de un chip FTDI que proporciona un enlace entre el puerto USB y el puerto serie (USART) del microcontrolador. La placa se conecta al ordenador mediante un cable USB y, gracias al chip FDTI, el ordenador la reconoce como un puerto serie estándar. Para programarla, se selecciona en Arduino la tarjeta “Arduino BT w/Atmega168” y el puerto COM correspondiente a la placa.

2.2.2. Ardupilot mega

Ardupilot Mega es un autopiloto diseñado para el control de todo tipo de UAVs. Está basado en el microcontrolador ATmega2560 e incorpora el software necesario para usarlo sin necesidad de programarlo, pero al ser de código abierto deja la puerta abierta a modificaciones del código y utilizarlo para cualquier otra función. Además, incorpora los elementos adecuados para la navegación de robots móviles, y por tanto es adecuada para este trabajo.

Para programar esta placa mediante la interfaz Arduino, se selecciona la tarjeta “Arduino Mega 2560 or Mega ADK” y se selecciona el puerto COM que corresponde a esta placa.

Figura5. Ardupilot mega.

2.2.2.1.Descripción detallada del Ardupilot Mega

El Ardupilot mega se encuentra dividido en dos placas. Una con el microcontrolador, en el que se implementa el control del robot, y otra con los sensores que proporcionarán información al robot. Estos sensores son los siguientes:



- Acelerómetro de tres ejes. - Giróscopo de tres ejes. - Magnetómetro de tres ejes. - Sensor de presión absoluta. - GPS: módulo Mediatek MT3329 con firmware 1.6.

El microcontrolador del Ardupilot mega es compatible con el entorno de programación Arduino, donde se puede modificar libremente el código fuente proporcionado por el desarrollador. En este caso, se usa una versión modificada para controlar el robot móvil utilizando los sensores de que dispone la placa. En esta versión, se mantienen las librerías necesarias para controlar los componentes hardware de la placa.

Al igual que la placa controladora de los motores del robot, la placa del Ardupilot incluye un chip FTDI para comunicar el microcontrolador con un ordenador a través de USB.

2.2.2.2.Funcionamiento de la IMU

La IMU que incorpora el Ardupilot Mega posee nueve grados de libertad (tres del acelerómetro, tres del giroscopio y tres del magnetómetro), es decir nueve medidas de los sensores que hay que convertir en los tres grados de libertad de rotación de un cuerpo en el espacio. Para estimar estos tres ángulos de orientación es necesario programar un filtro que unifique toda la información para corregir los errores de los sensores.

Los sensores inerciales (acelerómetro y giróscopo) dan información precisa a corto plazo, pero tienden a ser poco fiables con el tiempo. En cambio, el GPS y el magnetómetro dan información absoluta a lo largo del tiempo, por lo que no acumulan error. Combinando estos sensores se obtiene una información mucho más fiable y precisa de la actitud de la que tendrían cada uno por separado.

Los giróscopos dan información de la velocidad de giro en cada eje, por lo que se podría estimar la rotación del robot con solo integrar la velocidad de giro. Sin embargo, esta información no es suficiente, pues a lo largo del tiempo se acumulan errores.

Con un acelerómetro de tres ejes se puede determinar la orientación del robot fijo respecto a la superficie terrestre. En cambio, si el robot se mueve con cierta aceleración se medirá la combinación de la aceleración con la gravedad y ambas serán indistinguibles. Por tanto, un acelerómetro por sí solo no puede utilizarse para obtener la orientación del robot.



Se suele usar un filtro de Kalman para integrar las medidas de los sensores y corregir sus errores. Este filtro funciona bastante bien en la práctica pero es computacionalmente costoso, por eso el Ardupilot Mega utiliza otro método para identificar la orientación llamado "Premerlani-Bizard robust direction cosine matrix estimator" [2], basado en el trabajo de Mahony et al. [3]. El esquema del proceso es el siguiente:

Figura6. Esquema del proceso de estimación de orientación.

Los ángulos obtenidos como resultado se almacenan en variables de 32 bits en punto fijo con dos cifras decimales. El hecho de utilizar un mayor número de bits respecto a los 8 bits con los que trabaja el microcontrolador es para evitar un aumento de los errores numéricos al realizar operaciones. Por otra parte, trabajar con 32 bits en un procesador de 8 bits hace más lentas las operaciones. El microcontrolador del que consta el Ardupilot mega es lo suficientemente potente como para realizar estos cálculos en tiempo real.

2.2.3. Módulos ZigBee

Para comunicar el robot con el ordenador a distancia es necesario utilizar algún modo de comunicación inalámbrica. Así, esta comunicación se puede utilizar tanto para guiar al robot y ordenarle movimientos, como para obtener datos de telemetría del robot, como pueden ser las coordenadas gps o el ángulo de guiñada del robot entre otros. Para llevar a cabo esta comunicación se emplean un par de módulos Xbee que proporcionan una comunicación serie bidireccional entre el ordenador y el Ardupilot Mega.

Zigbee es un protocolo estandarizado para redes inalámbricas. Dicho estándar es abierto y fue desarrollado por la ZigBee Alliance, compuesta por un conjunto de empresas internacionales, muchas de ellas fabricantes de semiconductores.



Este estándar fue desarrollado para cumplir ciertas especificaciones como son: bajo coste, bajo consumo, crear redes de manera flexible y fácil de usar. Además, utiliza tres posibles bandas de frecuencia de uso público, entre ellas la de 2.4GHz utilizada en este trabajo. ZigBee consigue unos consumos insignificantes que permiten que funcionen con baterías durante años, con una tasa de transferencia de hasta 250 kbps.

Ya que ZigBee se ha creado como un estándar abierto, las distintas empresas han creado sus productos interpretando la definición de las normas. Esto hace que módulos de distintos fabricantes no tengan por qué ser compatibles entre sí.

El rango de transmisión con un dispositivo estándar suele ser de unos 200m en exterior y 30m en interior, aunque depende de la potencia de los módulos utilizados y las antenas. También se puede aumentar el rango de transmisión colocando nodos intermedios que unan los nodos lejanos, utilizando las topologías de red en malla o árbol.

Una ventaja más de ZigBee es la fiabilidad de la red. Emplea técnicas que aseguran que los datos lleguen correctamente a su destino, y en caso de no llegar reenviar de nuevo los datos. Todo ello de manera transparente al usuario. Todos los datos se envían encriptados en 128 bits, con lo que también logramos seguridad ante intrusiones en nuestra red.

El estándar ZigBee permite crear una red de hasta 216 nodos, distribuidos en subredes de 255 nodos. Estos nodos organizados en topología de árbol o mallada se comunican entre sí para hacer llegar los mensajes al nodo destinatario y son capaces de encontrar la ruta óptima entre los distintos nodos.

Los módulos utilizados en este trabajo son unos módulos Xbee Pro de la compañía Digi con antena integrada tipo “cable” y potencia de transmisión de 100mW. Se ha demostrado que la antena tipo cable tiene un rango de transmisión mucho mayor que la antena integrada en un chip, aunque para aplicaciones en interiores ambas antenas son equivalentes. La banda de frecuencia en la que operan es 2.4GHz que comprende desde los 2.4 hasta los 2.4835 GHz y se divide en 16 canales espaciados 5 MHz uno de otro.

Estos módulos Xbee funcionan con un mínimo de conexiones: alimentación a 3.3V y entrada y salida serie (UART). En este caso se han acoplado los módulos a un zócalo que convierte los niveles de 3.3V a los 5V con los que funciona el Ardupilot Mega. Para transmitir datos a un ordenador, el zócalo donde se inserta el Xbee incorpora un convertidor de puerto serie a USB basado en un chip FTDI.

2.2.3.1.Configuración de los módulos Xbee

Para poder utilizar los módulos Xbee hay que configurarlos previamente para que se encuentren emparejados y transmitan a la tasa de datos adecuada. En el caso que nos ocupa se van a utilizar dos módulos. Para configurarlos se utiliza una herramienta



software de la misma compañía Digi llamada X-CTU, que se comunica con los módulos enviando comandos AT, que son instrucciones de configuración. Para conectar los módulos al ordenador se utiliza un zócalo que contiene un convertidor USB-serie basado en FTDI, el cual se puede conectar directamente al puerto USB.

La configuración básica a modificar es el PANID y el Baudrate. El PANID es el número que identifica la red, que debe ser idéntico en todos los módulos que se quieran interconectar y el Baudrate es la velocidad del puerto serie que se crea hacia afuera de los Xbee. Por defecto, la velocidad de este puerto es de 9600 bps, pero en este trabajo se configura a 57600 bps para utilizarlo con el Ardupilot Mega. Es posible utilizar velocidades diferentes en ambos módulos pero no es recomendable pues se puede desbordar el buffer interno de los Xbee y perder bytes en la comunicación.

Además de la configuración básica, se han configurado otros parámetros de los módulos como son el Node Identifier (NI) que es un nombre que se asigna a cada módulo, el Packetization Timeout (RO) que define el número de bytes que se transmitirá en un mismo paquete de datos, y el Destination Address (DH y DL) que indica la dirección del módulo con el que se va a comunicar. También se ha definido el módulo conectado al PC como un nodo coordinador, aunque este parámetro no es necesario modificarlo para el funcionamiento.

Figura7. Interfaz X-CTU con la configuración del Xbee.

Para introducir esta configuración en el programa primero se elige el puerto COM asociado al Xbee y, a continuación, se selecciona la velocidad a la que se han



configurado (si es la primera vez será 9600bps). Luego hay que ir a la pestaña Modem Configuration para introducir los parámetros definidos anteriormente. Pulsando el botón Read se puede adquirir la configuración actual del módulo, y, así, modificar luego los parámetros deseados. Por último, pulsando el botón Write se carga la configuración al Xbee. Desde esta interfaz, que se muestra en la figura 7 con la configuración utilizada, también se puede modificar la versión del firmware, por ejemplo, para cargar la última versión.

2.3.Parte software del robot

La parte software del robot consta del controlador a bajo nivel de los motores en la placa controladora Wild Thumper, y del control a alto nivel.

El controlador a bajo nivel se encarga de recibir la información que le envía el Ardupilot Mega a través de señales PWM y controlar los motores del robot en consecuencia.

El controlador a alto nivel del robot se ha implementado tanto en el hardware del robot, en la placa Ardupilot Mega, programado a través del entorno de programación Arduino, como en el ordenador programado en Matlab. Así, es posible guiar al robot desde el ordenador con la información obtenida por la cámara, o bien enviarle un punto GPS para que lo alcance.

El robot tiene tres modos de funcionamiento: parado, guiado por cámara, y guiado por GPS. En un funcionamiento normal, el robot funcionará guiado por GPS. Así, el robot recibe a través del módulo Xbee el punto GPS al que debe ir y el controlador de a bordo se encarga de generar la referencia y las actuaciones necesarias para alcanzar dicho punto. De esta forma, es posible hacer que el robot siga una trayectoria, enviándole un nuevo punto destino cuando alcanza el anterior.

Cuando el GPS falle, indicando un valor erróneo de posición, ya sea por fallo hardware o por perdida de cobertura, la diferencia entre la posición obtenida por gps y la obtenida por la cámara se hará mucho mayor. Así, comparando en el ordenador la posición del GPS con la obtenida con la cámara, se detecta una diferencia superior al límite establecido como funcionamiento normal, se le envía al robot la orden de pasar al modo guiado por cámara. En este modo, se puede enviar al robot al punto deseado con la información de posición generada por la cámara, o bien hacer que vuelva a la base del UAV.

El microcontrolador del Ardupilot mega, el cual lleva el controlador a bordo del robot, se programa en lenguaje C a través del entorno de programación Arduino. En cuando a la parte del controlador que se ejecuta en el ordenador, se ha realizado en el entorno de programación Matlab.



El controlador programado en el hardware a bordo del robot se comporta como un esclavo del controlador implementado en Matlab, puesto que recibe las instrucciones de funcionamiento que éste envía a través de los módulos Xbee para realizar la acción deseada.

Además, el software de a bordo del robot se encarga de obtener los datos de los distintos sensores utilizados y de enviar al ordenador, a través del Xbee, los datos de sensores internos necesarios para el seguimiento y control.

El objetivo de este software es generar la actuación adecuada en los motores para que el robot llegue al punto de destino indicado, o para que el robot siga la trayectoria indicada desde el controlador en el ordenador.

Así, se definen tres modos de funcionamiento del robot, que será posible cambiar entre ellos enviando el comando adecuado desde el ordenador. Estos modos de funcionamiento son los siguientes:

- Parado: Es el modo por defecto. Cada vez que se inicia o resetea el controlador, se inicia en este modo. El robot permanece a la espera de recibir el comando adecuado para cambiar a otro modo de funcionamiento.

- Funcionamiento por GPS: El controlador del robot recibe las coordenadas GPS del punto siguiente al que debe dirigirse. Siempre que el número de satélites GPS disponibles sea suficiente para obtener la posición, el controlador del robot se encargará de calcular en qué dirección debe moverse el robot para alcanzar el punto objetivo en línea recta y de mantener dicha trayectoria. Una vez que el robot alcanza un punto alrededor del punto destino, con un margen de error configurable, el robot se detiene si no recibe un nuevo punto de destino.

- Guiado por cámara: En el caso de que la señal GPS no sea correcta, el

controlador ejecutado en el ordenador enviará un comando al robot para que cambie a modo guiado por cámara. Una vez en este modo, es el controlador del ordenador el que se encarga de, a partir de la información de la posición del robot obtenida con la cámara, generar la orientación adecuada para que el robot alcance el punto deseado y enviársela a este. Así, el controlador a bordo del robot se encargará de controlar los actuadores del robot para mantener la trayectoria con la orientación que recibe a través del Xbee.

Tanto en el modo de funcionamiento por cámara, como en el modo de funcionamiento por GPS, no se conoce la dirección real del robot, sino la medida proporcionada por la IMU, que está basada en las medidas del giróscopo y el magnetómetro.



Para mantener la dirección de referencia deseada, se ha implementado un controlador tipo PI con el esquema mostrado en la figura 8. Este controlador incluye una acción proporcional al error, que reacciona frente al error actual, y una acción integral, que consigue que el error sea nulo a partir de un instante de tiempo determinado. De esta forma, la acción de control puede expresarse como

( ) = ( ) + ( ) ( ) +

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, Ki es la ganancia de la acción integral y u0 es un offset de la señal de control.

Figura8. Esquema del controlador del robot.

Para comunicar la placa Ardupilot Mega con la placa de control a bajo nivel de los motores, se utilizan dos señales PWM, que varían el ancho de pulso en función de la velocidad y giro deseados. Usando este método de comunicación es posible utilizar los puertos serie de la placa para otras comunicaciones.

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 el caso que nos ocupa hay dos señales PWM, una para la velocidad y otra para el 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 el error a la salida de la acción de control es nulo, la señal PWM debe tener un ancho de pulso de 1.5 ms, por ello ha sido necesario añadir el offset u0 que se suma a la salida del controlador.

La acción de control descrita, es la forma continua del controlador, pero al implementarla digitalmente en un microcontrolador no es posible evaluarla de forma continua, sino a intervalos de tiempo regulares. Por eso, se aproxima la integral como un sumatorio, dando como resultado la siguiente expresión:



= + ( ) +

Esta expresión se evalúa en el microcontrolador a una frecuencia de 10 Hz, por lo que el término t toma el valor 0.1s.

Debido a que el término integral puede acumularse y saturar el controlador cuando éste no es capaz de corregir el error, se ha limitado la acción integral para que no pueda crecer indefinidamente.

Los valores de Kp y Ki de este controlador se han ajustado manualmente para que el tiempo de respuesta sea lo más rápido posible sin que se produzca sobreoscilación.

Como la configuración móvil del robot permite realizar giros sobre este mismo, se ha optado por limitar el ángulo de acción del controlador a 30º hacia cada lado, de forma que cuando el ángulo de referencia es mayor, el robot gira sobre sí mismo en la dirección con menor ángulo de giro.

2.3.1. Cálculo de la referencia en funcionamiento por GPS

Cuando el robot funciona guiado por GPS, recibe desde el ordenador, a través del Xbee, el punto al que se debe dirigir en forma de Latitud y Longitud.

Tomando como (x0, y0) la posición en la que se encuentra el robot y como(x1, y1) el punto de destino expresado como (latitud, longitud) se puede obtener la dirección que debe seguir el robot para alcanzar el punto objetivo. A continuación se detalla la forma de obtener la dirección adecuada, a partir de los datos mostrados en la figura 9.

Figura9. Esquema para el cálculo de la referencia en modo guiado por GPS.

La referencia en cuanto al ángulo de guiñada se ha tomado como 0º cuando el robot apunta al norte, 90º cuando el robot apunta al este, 180º cuando apunta hacia el sur, y,



270º cuando apunta hacia el oeste. De esta forma, el ángulo de guiñada que el robot debe tomar se calcula como

=

Aunque la función arco tangente no abarca toda la circunferencia y habría que tener en cuenta los signos del numerador y denominador, existe una función tanto en Matlab como en Arduino que tiene en cuenta los signos y devuelve un ángulo entre -180º y 180º. Esta función es atan2(numerador, denominador).

2.3.2. Cálculo de la referencia en el modo guiado por cámara

En este caso, el cálculo se realiza de la misma forma que en el caso anterior, con la diferencia de que se realiza en el ordenador, con coordenadas sobre el suelo (metros) y no con coordenadas geográficas, y luego se envía la dirección calculada al robot para que la siga.

Como el UAV también tiene sensores que le indican el ángulo de giro respecto al norte, se conoce previamente el ángulo que está girada la cámara respecto al norte. Se ha optado por girar el sistema de referencia de la cámara en ángulo adecuado para que quede orientado hacia el norte mediante una matriz de rotación. Como se verá posteriormente, este giro engloba también el ángulo que gira la cámara debido al pan & tilt. De esta forma, además de tener el mismo sistema de referencia para la posición basada en GPS que para la posición basada en la cámara, se puede enviar directamente la dirección calculada para que el robot la siga.

2.3.3. Puesta en marcha una vez programado

Cuando se programa el microcontrolador se borra la memoria eeprom que tiene almacenada la configuración de los sensores de la IMU, que incluye los offsets del acelerómetro y el giróscopo. Por ello, después de programar el Ardupilot mega, es necesario realizar la calibración de estos sensores.

El proceso para la calibración de estos sensores consiste en colocar el interruptor del Ardupilot en la posición de calibración (interruptor hacia el lado de los pines de entrada/salida), pulsar el botón de reset, esperar a que los leds dejen de parpadear y queden encendidos, volver a colocar el interruptor en el modo normal y, por último, resetear de nuevo el Ardupilot.

Con el Ardupilot conectado por cable USB al ordenador y mediante la herramienta de visualización de datos del puerto serie que incorpora el entorno de programación Arduino, o bien un terminal de puerto serie, es posible ver el progreso de la calibración.



2.3.4. Entorno de programación Arduino

El controlador en el Ardupilot Mega se programa a través de Arduino, que es una plataforma electrónica de software y hardware libre 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 micro controladores de la marca Atmel de 8 bits, aunque existen proyectos para extenderlo a diferentes 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.

La programación es similar al lenguaje C++ con algunas simplificaciones y modificaciones. Incorpora funciones básicas de micro controladores y librerías que facilitan bastante el uso de periféricos y las comunicaciones. 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. La figura 10 muestra este entorno de programación.

Figura10. Entorno de programación Arduino.



2.3.5. 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 se utiliza en muchas ocasiones para simulación, mediante la implementando de algoritmos y la visualización de datos en gráficas en 2 o 3 dimensiones, ya que posee un potente módulo que permite realizar gráficas fácilmente.

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 se utiliza para este trabajo es la capacidad para poder ejecutar funciones y subrutinas escritas en lenguaje C/C++. Para que las funciones creadas en C/C++ puedan ser utilizadas por Matlab como si de cualquier otra función se tratase, 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. Por ello, el algoritmo de seguimiento se usa implementado en este lenguaje, y el resto del programa se realiza en lenguaje Matlab, pera poder utilizar librerías que éste incluye y facilitan bastante el desarrollo.

2. descripción del robotbibing.us.es/proyectos/abreproy/70435/fichero/02.pdf · navegación y...

Documents