Carro Robot Guiado por GPS: Arduino UNO en Acción

Escrito por Raúl Alvarez.

Por Raul Alvarez-Torrico

Esta es una traducción automática (pueden haber algunos errores de traducción) de mi artículo original en inglés publicado por la revista “Circuit Cellar” (#347 de Junio, 2019). Puedes ver un extracto del artículo original en este enlace. El código de programa y diagramas aún están en Inglés.

Introducción

En este artículo presento un carro robot básico de accionamiento diferencial para la navegación autónoma de puntos de referencia mediante GPS. El carro robot recibe una lista de coordenadas GPS y navega a cada punto de referencia en su orden dado. Para comprender cómo funciona, analizaré conceptos sobre el Sistema de Posicionamiento Global (GPS), presentaré un enfoque simple para implementar la navegación autónoma mediante GPS, el hardware necesario para la tarea, cómo calcular los vectores de navegación mediante la "fórmula de Haversine" y la "fórmula del azimut delantero”, y una implementación simple de un filtro de promedio móvil para filtrar las lecturas de coordenadas GPS. También analizo un enfoque muy simple para el control de navegación minimizando la distancia del carro robot y el error de orientación con respecto al objetivo.

Este proyecto está dirigido a principiantes con experiencia básica en carros robot (por ejemplo, seguidores de línea, evasores de obstáculos, etc.) que ahora quieran probar algo un poco más complejo, o simplemente cualquier persona interesada en el tema. El enfoque que elegí para la navegación y el control no es necesariamente el más robusto, pero espero que sea lo suficientemente claro y simple, apropiado para principiantes.

Diagrama de Bloques y Descripción Funcional

La Figura 1 muestra los componentes principales del sistema. El receptor GPS nos ayudará a calcular la distancia desde el carro robot hasta el objetivo. Con la ayuda de una brújula digital, el GPS también nos ayudará a determinar en qué dirección se encuentra el objetivo. Esos dos parámetros (distancia y dirección) nos darán el vector de navegación requerido para controlar el carro robot hacia la meta. Utilicé una configuración de transmisión diferencial de cuatro ruedas para el carro robot, que se comporta casi exactamente como una transmisión diferencial de dos ruedas. El código proporcionado con el proyecto debería funcionar bien con ambas configuraciones. Para calcular la distancia al objetivo utilicé la "fórmula de Haversine", y la "fórmula del azimut delantero" para calcular la dirección o rumbo. Ambos parámetros pueden calcularse utilizando los siguientes datos conocidos: las coordenadas GPS del objetivo, las coordenadas del carro robot obtenidas desde receptor GPS y la dirección del carro robot con respecto al Norte, obtenida de la brújula digital.

El carro robot recalculará constantemente el vector de navegación y usará la distancia y dirección obtenidas para controlar los motores y acercarse a la meta. También puse un zumbador (buzzer) en el carro robot para proveer una señal audible cuando el éste llegue a los puntos de navegación.

Figura 1: Diagrama de bloques

Hardware

Como se puede ver en el diagrama de bloques, estoy usando una placa Arduino UNO como controlador principal. Estoy usando Arduino porque es increíblemente intuitivo para principiantes y tiene una enorme constelación de bibliotecas que facilita la realización de proyectos razonablemente avanzados sin entrar en detalles acerca de los drivers de hardware y software para sensores y actuadores.

El receptor GPS que elegí para la tarea es el módulo GY-GPS6MV2 basado en el chip Ublox NEO-6M y la brújula digital es el módulo GY-271 basado en el chip Honeywell HMC5883L. Ambos son de bajo costo y ubicuos, con bibliotecas Arduino fácilmente disponibles. El Ublox NEO-6M tiene una interfaz de comunicación serial UART y el HMC5883L funciona con el protocolo serial I2C. Se recomienda que la brújula se coloque al menos 15 centímetros por encima del resto de la electrónica para evitar interferencias.

Los motores DC funcionan con el popular módulo L298N basado en el controlador puente 'H' de doble canal STMicroelectronics L298N. Este puede manejar dos motores DC con una corriente máxima de 2 amperios por canal. También puede manejar dos motores DC por canal, cuatro en total (si no se supera la especificación de corriente máxima), que es lo que estoy haciendo con el chasis de cuatro ruedas usado para este prototipo. Mi chasis tiene una plataforma de aluminio con un tamaño de 30 por 20 centímetros, cuatro motores genéricos DC de 12V, 85 revoluciones por minuto y ruedas de 130 milímetros de diámetro, pero se puede usar casi cualquier chasis genérico de dos o cuatro ruedas.

Para suministrar energía al carro robot, estoy usando una batería de Polímero de Litio (LiPo) de 11.1 voltios y 2200 miliamperios-hora con una tasa de descarga de 25C; pero para mi tipo de chasis, una batería de la mitad del tamaño también hubiera funcionado bien. La Figura 2 muestra el diagrama del circuito para este proyecto, y la Foto 1 muestra el carro terminado.

Figura 2: Diagrama de circuito

Foto 1: Carro robot GPS terminado

Sistema de Posicionamiento Global

El Sistema de Posicionamiento Global (GPS) es un sistema global de navegación por satélite, propiedad del gobierno de los Estados Unidos, que proporciona información de geolocalización y tiempo a cualquier receptor GPS en la superficie de la Tierra, siempre que tenga una línea de visión sin obstrucciones hacia al menos cuatro satélites GPS (cuanto más, mejor) [1]. Los receptores GPS generalmente pueden proporcionar coordenadas de latitud y longitud con una precisión de aproximadamente 2.5 a 5 metros en condiciones ideales (es decir, buena visibilidad del cielo y muchos satélites en línea de vista). Programaremos el carro robot con uno o más puntos de navegación definidos por sus coordenadas de latitud y longitud; el receptor GPS del carro robot nos dará su posición real en el mismo tipo de coordenadas.

Algoritmo Básico Para Navegación Autónoma GPS

Figura 3: Diagrama de flujo

La Figura 3 muestra un diagrama de flujo del algoritmo que corre en el carro robot. Creo que la mayor parte se explica por sí sola; por lo tanto, me enfocaré solo en el cálculo del vector de navegación y el control de navegación; que son posiblemente las partes más complejas del algoritmo. Como dije anteriormente, estoy usando la "fórmula Haversine" para calcular la distancia al objetivo y la "fórmula del azimut delantero" para calcular la orientación del objetivo con respecto al Norte magnético. Si la distancia al objetivo es mayor que un error de tolerancia de distancia dado (1 metro en nuestro caso), el algoritmo procederá a mover el carro robot hacia adelante. Por otra parte, si el ángulo de dirección del carro robot con respecto al objetivo actual está fuera de un error de tolerancia de dirección dado (+/- 5 grados en nuestro caso), el algoritmo también rotará el carro robot hasta que la nariz apunte hacia el objetivo. El algoritmo conducirá el carro robot hasta que alcance el objetivo actual y luego repetirá los mismos pasos si hay otros puntos de navegación disponibles; si no, el carro robot se detendrá.

El carro robot debe repetir constantemente este ciclo de calcular el vector de navegación y controlar su desplazamiento, porque con cada movimiento que realiza, se introducen más errores de distancia y dirección. Por otra parte, tanto el receptor GPS como la brújula digital tienen sus propias tolerancias de error, que también se sumarán a las magnitudes de error anteriores (sin mencionar otras tolerancias de error existentes en los actuadores y las perturbaciones externas).

Cálculo del Vector de Navegación

La Figura 4 muestra gráficamente cómo se define el vector de navegación: distancia y dirección. Estos dos componentes se calcularán a partir de tres parámetros conocidos: las coordenadas del objetivo, las coordenadas del carro robot y la dirección del carro robot con respecto al Norte magnético. Esos dos componentes serán también los errores que intentaremos minimizar con nuestro algoritmo de control. El error de distancia será reducido dando al carro robot una velocidad lineal y el error de dirección (ángulo) dándole una velocidad angular correspondiente. La "fórmula de Haversine" nos permitirá calcular el error de distancia "d" y la "fórmula del azimut delantero" nos permitirá calcular el azimut (waypoint_angle), con el que podemos calcular el error de dirección "α".

Figura 4: Cálculo del vector de navegación

La "fórmula de Haversine" es una fórmula general en trigonometría esférica que permite el cálculo de la distancia entre dos puntos en la superficie de la Tierra (o cualquier otra esfera). Para calcular la distancia entre dos puntos en la superficie de la Tierra, debemos tener las coordenadas (latitud y longitud) de los dos puntos. Las ecuaciones (1) y (2) nos dan respectivamente la fórmula general de Haversine y la identidad trigonométrica de Haversine [2]. Sin embargo, las ecuaciones (3), (4) y (5) son las que implementé en el código; las cuales son solo un desglose de (1) y (2) que expresan los cálculos de forma más conveniente [3].

El azimut se define como el ángulo "horizontal" medido en el sentido de las agujas del reloj desde una línea de base Norte o meridiano hasta una línea de dirección definida por dos puntos sobre la superficie de la Tierra. Si tomamos el Norte verdadero como referencia, obtendremos el "azimut verdadero" y si tomamos el Norte magnético, obtendremos el "azimut magnético" correspondiente. Por lo tanto, la fórmula del azimut delantero nos permitirá calcular el ángulo entre estas dos líneas: la línea entre la posición del carro robot y el Norte, y la línea entre la posición del carro robot y el objetivo (ver Figura 4). La ecuación (6) muestra esta fórmula [3]. Para calcular el azimut, solo necesitamos las coordenadas del objetivo y el carro robot.

Una vez que tenemos el azimut o el ángulo del punto de navegación, debemos calcular el error de dirección, que es solo la diferencia entre el azimut y la dirección del carro robot con respecto al Norte (que obtenemos de la brújula digital): heading_error = waypoint_angle - robot_car_heading.

Control de Navegación

El algoritmo de control de navegación es muy simple: primero, tratamos de reducir el error de dirección girando el carro robot hacia la derecha, si el error de ángulo es positivo, y hacia la izquierda si el error es negativo. Debido a que es difícil reducir el error exactamente a cero (y mantenerlo en cero), estoy definiendo un rango de tolerancia de +/- 5 grados; por lo tanto, estará bien en principio si reducimos el error a cualquier magnitud entre ese rango. Al mismo tiempo, trataremos de reducir el error de distancia comandando al carro robot para que avance en la dirección aproximada al objetivo; pero al hacerlo, normalmente el error de ángulo tenderá a aumentar en magnitud de manera positiva o negativa, esto debido a errores de tolerancia no solo en los sensores (receptor GPS y brújula digital), sino también en los actuadores y también por las perturbaciones externas. Por lo tanto, debemos iterar permanentemente sobre el cálculo de un nuevo vector de navegación y la minimización de ambos errores hasta que finalmente lleguemos al objetivo.

Si el valor absoluto del error de dirección está entre 5 grados y 45 grados (25 por ciento de 180 grados), el carro robot girará lentamente hacia la derecha o hacia la izquierda dependiendo del signo del error (como se indicó anteriormente). Si el error está entre 45 grados y 180 grados, el carro robot girará más rápido para minimizar el error más rápidamente también. El límite de 45 grados lo elegí empíricamente, funcionó bien en las pruebas pero se puede cambiar en el código Arduino al valor que uno desee. Por lo que puedo entender, esta estrategia de control es una especie de "controlador proporcional escalonado" porque divide el espacio de error en cinco sectores (-180 a -45, -45 a -5, -5 a +5, +5 a +45, +45 a +180); y el algoritmo utiliza una señal de control de corrección proporcional que está "codificada" en las velocidades de cinco maniobras correspondientes que el carro robot tiene disponibles para corregir el error y llegar al objetivo (que son: girar a la derecha rápido, girar a la derecha lento, avanzar, girar a la izquierda lento, girar a la izquierda rápido).

Precisión del GPS y la Brújula Digital

La precisión y la exactitud son dos conceptos no siempre bien entendidos incluso por personas con mentalidad técnica (incluyéndome a mí). Ambos conceptos generalmente se asocian en el mundo de la ingeniería con la toma de mediciones de sensores y, a veces, se usan indistintamente de manera incorrecta. En términos simples, la exactitud es qué tan cerca está un valor medido del valor real real, y la precisión se refiere a la repetibilidad de la medición; en otras palabras, qué tan cerca están dos o más mediciones entre sí. La Figura 5 muestra la diferencia entre estos dos conceptos. Para comprender más fácilmente la diferencia, podemos usar la siguiente ilustración: si al lanzar canastas en baloncesto, siempre golpeas al lado izquierdo del tablero y casi nunca al aro, eres preciso, pero no exacto.

La hoja de datos del Ublox NEO-6M ofrece una precisión de posición horizontal de 2.5 metros, CEP, 50%. Esto básicamente significa que solo el 50% de todas las mediciones tomadas en un período de tiempo determinado tendrán esa precisión de 2.5 metros. Además, esto es en condiciones ideales, porque en la mayoría de las condiciones normales, la precisión (CEP 50%) se puede reducir a 10, 20 o más metros.

Entonces, la precisión con la navegación GPS en general no es tan alta. En nuestro caso, el carro robot alcanzará los puntos de navegación, más o menos con ese mismo nivel de precisión, lo que no parece realmente impresionante al principio, pero ver cómo el carro robot navega automáticamente a todos los puntos (dentro de un margen de error) todavía sigue siendo muy divertido. En términos generales, obtendremos un comportamiento más confiable y repetible del carro robot en espacios muy abiertos, con menos edificios y otros obstáculos alrededor. En ese tipo de espacios comprobé que el carro robot llegaba al objetivo en un radio de entre 1 y 3 metros.

Como ya sabrás, hay varias formas de mejorar la precisión de la navegación; por ejemplo, usar un GPS mucho más sofisticado y costoso (GPS diferencial, por ejemplo) o agregar otro tipo de sensores al carro robot; por ejemplo, un sensor de odometría o una unidad de medición inercial (IMU). En aplicaciones más sofisticadas, también se puede utilizar una cámara o un sensor LIDAR. En cuanto a la brújula digital, la precisión no es un gran problema. La hoja de datos del Honeywell HMC5883L establece una precisión de 1-2 grados, que es más que suficiente para alcanzar la meta, por lo que la inferior precisión del receptor GPS siempre será el factor dominante. Sin embargo, las brújulas digitales están sujetas a interferencias magnéticas por circuitos electrónicos, materiales magnéticos y objetos metálicos cercanos, como estructuras metálicas, cercas metálicas, mallas metálicas, etc.

Figura 5: Exactitud y precisión

Filtro de Promedio Móvil

Para mitigar de alguna manera el abundante ruido presente en los datos del GPS (es decir, el ruido heredado de la baja exactitud y precisión del receptor GPS), implementé un filtro de promedio móvil (que técnicamente es un filtro paso bajo de respuesta de impulso finito o FIR). En principio, un filtro de promedio móvil es solo una calculadora de promedios, que toma "N" lecturas, por ejemplo In₀ a In_N-1 y proporciona una salida filtrada Out₀, como promedio de las N entradas. En la siguiente iteración, tomará las entradas In₁ a In_N y proporcionará una salida filtrada Out₁; y así sucesivamente. Ve las Ecuaciones 7 y 8 para una descripción matemática de este filtro [4].

En el código, implementé el filtro como dos arrays para 'N' números de punto flotante, uno para almacenar latitudes y el otro para almacenar longitudes. Los dos arrays funcionan como búferes "First In First Out" (FIFO); es decir, cada vez que se debe almacenar una nueva lectura, la lectura más antigua se elimina primero en la cola del búfer (si el búfer ya está lleno), luego todos los valores restantes se mueven hacia la cola, y finalmente se empuja el nuevo valor en la cabeza del búfer. Por lo tanto, cada vez que haya disponible una nueva lectura de GPS, los valores de latitud y longitud se almacenan en los arrays, para luego obtener valores de latitud y longitud filtrados, que a su vez se utilizarán para calcular el nuevo vector de navegación.

El código Arduino disponible para el proyecto imprime en el Monitor Serial datos de las lecturas de latitud originales y filtradas de forma predeterminada, por lo que, por ejemplo, puedes abrir el Plotter en el IDE de Arduino para ver gráficamente el efecto del filtro de promedio móvil sobre los datos de latitud. En la función Print_Data(), puedes comentar / descomentar otros tipos de datos que se enviarán al Plotter; para que esto funcione, por supuesto, debes tener la placa Arduino del carro robot conectada a la computadora. La figura 6 muestra una captura de pantalla del Plotter con el filtro de promedio móvil aplicado a la latitud.

Figura 6: Filtro de promedio móvil aplicado a datos de latitud

Implementación del Código

Los waypoints (puntos denavegación) se almacenan en código estáticamente en un array de tipo struct t_waypoint. Este tipo estructura almacena valores de latitud y longitud para un punto de navegación dado. Al principio, el primer punto de navegación en el array es establecido como el objetivo actual a alcanzar; una vez alcanzado, el segundo punto de navegación se establecerá como el nuevo objetivo a seguir, y así sucesivamente, hasta el último. Hay una función Get_Waypoint_With_Index (int index), la cual retorna un waypoint desde el array para un índice dado.

En la función loop() (la función principal en Arduino), el valor del sensor de la brújula digital se lee primero con la función Get_Compass_Heading() para obtener la dirección del carro robot; luego, la función Query_Gps() se ejecuta para ver si hay datos disponibles desde el receptor GPS. Si ese fuera el caso, se llama a la función Gps_Dump(gps) para obtener las coordenadas de latitud y longitud actuales del carro robot. A continuación, la función Store_Gps_Reading() se ejecuta para guardar esas coordenadas en los búferes del filtro, y luego, la función Compute_Filtered_Gps() se llama para obtener los valores de latitud y longitud filtrados. Una vez que tenemos estos valores filtrados, la función Compute_Navigation_Vector() se ejecuta para obtener los parámetros de distancia y dirección para el vector de navegación.

Luego, la función Control_Navigation() se llama para mover el carro robot hacia el objetivo. Esta misma función verifica si se ha alcanzado el objetivo actual; si es así, una variable waypoint_index que apunta a las coordenadas de objetivo actual en el array de puntos de navegación se incrementará para establecer el siguiente waypoint como el nuevo objetivo. Si se han alcanzado todos los puntos de navegación el carro robot se detendrá. En el código, las lecturas de la brújula digital están siendo compensadas por la declinación magnética; que es el ángulo en el plano horizontal entre el Norte magnético (la dirección del Norte devuelta por la brújula digital, que corresponde a la dirección de las líneas del campo magnético de la Tierra) y el Norte verdadero, que es la dirección a lo largo de un meridiano hacia el Polo Norte geográfico. Este ángulo llamado "declinación" varía no solo según la posición en la superficie de la Tierra, sino que también cambia con el tiempo. Hay mapas de declinación en Internet donde puedes encontrar el valor correspondiente para tu ubicación y cambiarlo en el código; o puedes ignorarlo por completo y ponerlo a cero; el algoritmo aún debería funcionar, aunque quizás con un poco menos de eficiencia. El zumbador (buzzer) emite dos pitidos cada vez que se alcanza un punto de navegación y tres veces cuando se alcanza el último punto del array.

Prueba del Carro Robot

El GPS siempre funciona mejor en espacios abiertos, por lo que se recomienda probar el carro robot en un área abierta, como un parque, un campo de fútbol, un estacionamiento abierto o incluso una cancha de baloncesto, siempre que no haya demasiados árboles cercanos, edificios y otras estructuras que podrían interferir con la recepción del GPS. Las estructuras metálicas, como cercas o mallas de alambre similares a las que se usan para rodear las canchas de baloncesto, también pueden interferir en cierta medida con la brújula digital. Utilicé Google Maps para definir coordenadas de puntos de navegación para mis rutas de prueba. Puedes marcar algunos puntos de navegación en Google Maps y luego copiar sus coordenadas en el array de puntos de navegación en el código y listo. ¡No olvides cambiar los puntos de navegación que yo puse como ejemplo en el código por los tuyos, o tendrás a tu carro robot intentando alcanzar algunas coordenadas en un parque ubicado cerca de mi casa!

Conclusiones y Mejoras Futuras

La Foto 2 muestra el carro robot en acción. Probé un par de mapas diferentes en diferentes lugares y el carro robot siempre completaba las rutas con todos los puntos de navegación; aunque me di cuenta que divagaba un poco al pasar entre algunos puntos, a veces describiendo una parábola. Cuando el carro robot llega al área cercana al objetivo, rodea un poco el área hasta que finalmente llega al mismo. Esto se notó más especialmente cuando algunas estructuras metálicas cercanas estaban presentes. El filtro de promedio móvil para las lecturas de GPS realmente hace una diferencia. Ejecuté el filtro con 15 puntos (es decir, un promedio de 15 lecturas) y comparé el rendimiento sin el filtro. Con el filtro activado, noté que el carro robot deambula un poco menos que sin el filtro, llegando al punto de navegación un poco más rápido en general. La estrategia de control "proporcional escalonado" funciona bastante bien, pero a veces el carro robot tiende a oscilar un poco al intentar corregir el error de dirección o al intentar seguir caminos rectos. Especialmente cuando el error está cerca de uno de los límites de "paso de error"; es decir, al pasar de un rango de error a otro (recuerda que el espacio de error se divide en cinco pasos de error).

Foto 2: Carro robot GPS en acción

Algunas mejoras que podría agregar al carro robot en el futuro son las siguientes: Primero, me gustaría implementar un controlador PID para la navegación, porque creo que podría hacer que la navegación sea más fluida y efectiva. En segundo lugar, me gustaría agregar encoders a las ruedas para odometría y luego implementar un filtro Kalman para fusionar la odometría con las lecturas del GPS. Tercero, sería genial escribir una aplicación Python para recibir datos de telemetría en mi computadora a través de un transceptor inalámbrico y visualizar puntos de navegación, lecturas de sensores, magnitudes de error, filtrajes, trayectorias, etc. Y finalmente, sería genial construir un bote robot GPS para navegación autónoma sobre el agua, utilizando como inicio parte del hardware y el código de este proyecto. ¡Sí, creo que espero más días soleados con mucho bloqueador solar sobre mi piel, y con la esperanza de esa sonrisa final en mi rostro!

REFERENCIAS

[1] https://en.wikipedia.org/wiki/Global_Positioning_System

[2] https://en.wikipedia.org/wiki/Haversine_formula

[2] https://www.movable-type.co.uk/scripts/latlong.html

[3] https://dspguide.com/ch15.htm

RECURSOS

Fundamentals of a GPS guided vehicle

https://www.robotshop.com/community/forum/t/fundamentals-of-a-gps-guided-vehicle/12955

Calculating the Distance Between Two GPS Coordinates with Python (Haversine Formula)

https://nathanrooy.github.io/posts/2016-09-07/haversine-with-python/

-----------------------------