Control PID con TurtleSim

Resumen

Move Turtle es un simple script de Python de ROS que mueve la tortuga en TurtleSim de ROS a coordenadas (puntos) especificadas. Este proyecto comenzó como un proyecto de clase para CSCI473 en la Escuela de Minas de Colorado (CSOM) y luego evolucionó a un proyecto que utilicé para ayudarme a aprender cómo se pueden crear sistemas de lazo cerrado, conocimiento que se utilizó para ayudarme en mi trabajo en el Laboratorio de Robótica Centrada en el Humano (HCR) de CSOM.

Contexto

Para un poco de contexto, ROS significa Sistemas Operativos de Robots. ROS es un middleware de robótica de código abierto, generalmente emparejado con el sistema operativo Ubuntu, que se utiliza para gestionar mejor los componentes/clústeres de software de un sistema robótico. Puedes aprender más sobre cómo usar ROS a través de la wiki oficial de ROS. El código que escribes en ROS es en C++ o Python.

TurtleSim es un simple simulador de robots 2D utilizado como una introducción a ROS y a los paquetes de ROS para nuevos usuarios de ROS. Todo lo que hace TurtleSim es abrir una simple pantalla 2D con una tortuga. Esa tortuga actúa como el “robot” y puedes enviar mensajes a ese “robot” para moverlo y/o girarlo. Al mover, TurtleSim genera algún error aleatorio para simular la física del mundo real de una manera simple.

Sabiendo qué es ROS y TurtleSim, ¿por qué los estaba utilizando? Bueno, en la primavera de 2020 estaba tomando una clase llamada “Robótica Centrada en el Humano” en la Escuela de Minas de Colorado como parte de mi carga de clases de informática de pregrado. La clase fue impartida por el Dr. Hao Zhang. La clase consistió en tres proyectos y el primer proyecto consistió en configurar ROS, aprender a usar ROS e implementar un algoritmo de lazo abierto o cerrado que hiciera que la tortuga en TurtleSim dibujara una M. La descripción original del proyecto se puede ver AQUÍ.

Cuando estaba tomando la clase, completé este proyecto implementando un algoritmo de lazo abierto. Esta solución funcionó, pero realmente fue mala y dibujó una A que era “suficientemente buena” pero no estaba cerca de ser “genial”. Después de enviar el proyecto alrededor de mediados de febrero de 2020, me olvidé de él hasta un año después, alrededor de febrero de 2021.

Desafío

En febrero de 2021, fui contratado como Asistente de Investigación en el Laboratorio de Robótica Centrada en el Humano (HCR) en la Escuela de Minas de Colorado, donde trabajé bajo el Dr. Hao Zhang. El objetivo del laboratorio es investigar sobre “autonomía colaborativa a lo largo de la vida, con el objetivo de permitir que los robots operen y se adapten durante largos períodos de tiempo”. A partir del 1-9-2022 (1 de septiembre de 2022), el laboratorio se trasladó de la Escuela de Minas de Colorado a la Universidad de Massachusetts Amherst. Pero, cuando trabajé allí, todavía estaban ubicados en Colorado.

En el Laboratorio HCR, se me encargó trabajar en el proyecto Triton. El proyecto Triton consistió en múltiples, decenas, de robots llamados Tritons. Los Tritons eran robots terrestres triangulares rueda omni que se veían así:

Junto con los Tritons, había ocho Cámaras IR de OptiTrack dispuestas en un “orden octagonal” a unos pies sobre el suelo. Usando esas cámaras, el software Motive de Optitrack, una PC con Windows 10 y ROS; se crea un espacio de 2 metros por 2 metros por 2 metros (x, y, z) donde se puede determinar la posición exacta en el mundo real de objetos con marcadores de captura de movimiento.

Mi primera tarea con el proyecto Triton fue hacer scripts que hicieran que el robot Triton se moviera a una posición específica en el mundo real. En ese momento, no estaba seguro de cómo lograr esto, pero recordé el primer proyecto de CSCI473 donde se me introdujo la idea de un sistema de lazo cerrado, así que comencé a investigar realmente sobre sistemas de lazo cerrado y, a su vez, sistemas de retroalimentación.

Investigación

Durante mi investigación, descubrí este gran video de AerospaceControlsLab:

En este video, se mostró y explicó la idea de un sistema PID. Un sistema PID es un sistema de control de lazo cerrado que produce salidas específicas basadas en datos del mundo real. En términos básicos, es un sistema de retroalimentación que ajusta su salida en función de la diferencia entre el valor deseado y el valor medido. Esta es la fórmula para un controlador PID:

$$ u(t) = K_p e(t) + K_i \int e(t) dt + K_d \frac{de(t)}{dt} $$

$u(t)$ es la señal de control.
$K_p$, $K_i$, $K_d$ son las ganancias proporcional, integral y derivativa respectivamente.
$e(t)$ es la señal de error (la diferencia entre la salida deseada y la real).
$\int e(t) dt$ es la integral del error a lo largo del tiempo.
$\frac{de(t)}{dt}$ es la derivada del error.

Para aprender más sobre esto, consulta el video de AerospaceControlsLab o este increíble artículo de Wikipedia del cual proviene esta fórmula.

Después de hacer mi investigación y obtener ayuda de mis amigos y profesores inteligentes, determiné que usar un Controlador Proporcional, Controlador P, sería lo mejor para ayudar al Triton a alcanzar una coordenada específica.

Dado que el Triton es un robot terrestre, solo necesito preocuparme por las coordenadas X e Y. Sabiendo esto, luego tuve que determinar qué valores deseados y valores medidos medir y procesar dentro del(los) Controlador(es) P. En mi investigación encontré esta increíble publicación en la wiki de ROS llamada Ir a la Meta donde se enfocaron en los siguientes errores:

Error de Distancia: La diferencia en distancia entre la ubicación deseada (X, Y) y la ubicación actual (X, Y).
Error de Theta: La diferencia entre la orientación deseada y la orientación actual.

Solución Propuesta

Sabiendo todo esto, la solución para hacer que el Triton se mueva a una coordenada específica en el mundo real es usar un Controlador Proporcional para el error de distancia del Triton y el error de theta con respecto a la coordenada objetivo y la coordenada actual del Triton. Pero antes de implementar esto en un sistema físico, quería probar la idea en una simulación y, en ese momento, pensé que usar TurtleSim como la simulación era la mejor opción.

Pruebas en Simulación

Con la solución teórica delineada, opté por validarla inicialmente a través de simulación, en lugar de un escenario del mundo real. El mundo real a menudo está lleno de ruido que podría enturbiar la efectividad de la solución y llevar a lidiar con problemas irrelevantes. Reconociendo esto, volví a visitar mi proyecto de CSCI473 y lo adapté como un campo de pruebas para esta solución teórica.

Después de actualizar el antiguo código de usar ROS Melodic a usar ROS Noetic y con algunos refinamientos iterativos, logré que la solución propuesta funcionara en TurtleSim. Las demostraciones de su funcionalidad, con algunos caminos de coordenadas objetivo geniales, se pueden encontrar al final de esta publicación del blog. El código para todo esto se puede ver AQUÍ.

Conclusión

En resumen, a través de este proyecto, la tortuga de TurtleSim navega efectivamente a coordenadas establecidas utilizando el sistema de lazo cerrado, superando significativamente mi sistema de lazo abierto inicial de 2020. Probado con éxito, implementé esta solución con los robots Triton. Pero el ruido del mundo real introdujo problemas inesperados que requirieron semanas de depuración y pruebas en el mundo real. Después de esas semanas, finalmente logré que el Triton alcanzara coordenadas específicas en el mundo real utilizando la solución que probé en TurtleSim. Aquí hay una demostración del Triton moviéndose a coordenadas específicas en el mundo real utilizando el método probado en TurtleSim:

En retrospectiva, desearía haber utilizado una simulación que simulara mejor la física del mundo real. TurtleSim es genial para aprender, pero no es bueno para probar ideas de robótica del mundo real. En ese momento, pensé que sería lo suficientemente bueno para mi caso de uso simple, pero eso luego se demostró que no era el caso.

El objetivo principal de esta publicación del blog era delinear la creación de un sistema de lazo cerrado que permitiera a la tortuga en TurtleSim, y posteriormente a los robots Triton, navegar con precisión a posiciones objetivo especificadas (X, Y). Creo que este objetivo se logró, pero podría haber generado algún interés en mi trabajo en el Laboratorio HCR. Sabiendo esto, tengo planes de hacer más publicaciones en el blog sobre mi experiencia laboral en el Laboratorio HCR…

Demos de Turtle en Movimiento (TurtleSim)

Este video contiene las siguientes demostraciones:

Cayley Nodal: Un simple diseño de Cayley Nodal
Círculos: Un diseño similar a una telaraña
Círculos Internos: Un diseño de “círculos dentro de círculos”
M: Un diseño de M muy simple