Robot autobalanceado
- Introducción
Nuestro decisión de proyecto ha sido un robot autobalanceado, el objetivo es que la máquina, sea capaz de mantener su posición erguida en equilibrio y estable sobre sus 2 ruedas. Para conseguir este objetivo hemos utilizado una IMU (Inertial Measurement Unit), un sensor que incorpora un giroscopio y un acelerómetro para medir los cambios de posición en el espacio del sistema y por tanto las rotaciones en los tres ejes que se puedan dar; el segundo componente, es un par de motores, para realizar los movimientos de ajuste en la vertical en caso de que el sistema empiece a “caer” y mantener así su equilibrio.
Es una implementación de un sistema de control con retroalimentación, como los sistemas distribuidores de carga o estabilizadores. Siguen como principio de funcionamiento, el uso de los valores de entrada, para variar el valor de salida para alcanzar el objetivo.
- Implementación
La implementación requiere de varias partes esenciales para el funcionamiento:
- PID (Proportional Integral Derivative): Es una de las partes fundamentales para el sistema, se encarga de calcular el valor de salida usando el error entre la posición deseada y la medida como entrada. Para ello, el sistema de control PID, usa 3 variables: la Proporcional, que se multiplica al error, la Integrativa, que se multiplica al acumulado los errores anteriores y la Derivativa que se multiplica a la variación del error. Todas ellas, se combinan para obtener la salida.
La primera, es la respuesta inmediata al error, la segunda es una reacción a la acumulacion del error y la tercera, contrarresta la velocidad del cambio, evita que las 2 anteriores sobre reaccionen a la entrada y en vez de corregir el error, creen otro opuesto.
- Filtro complementario: Este módulo se va a encargar de filtrar los datos que genera la IMU para calcular el ángulo; el objetivo es el procesado de los datos de la IMU para generar el ángulo actual del sistema de la manera más precisa. Aprovecha la falta de ruido del giroscopio y la falta de deriva del acelerómetro y, contrarresta, la deriva a largo plazo del giroscopio y el alto ruido del acelerómetro.
- Interrupciones: Las interrupciones son las encargadas de generar las salidas en el proyecto, en nuestro caso utilizaremos un par de motores. La idea ha sido generar interrupciones en intervalos variables dependiendo del ángulo del sistema y la velocidad de rotación, de esta forma se hace mover los motores paso a paso el número de veces necesario para contrarrestar el movimiento y tratar de estabilizar el sistema.
Todas estas partes se juntan de forma que leeremos los datos de la IMU, los procesamos para sacar el peridodo de interrupciones y lo seteamos de forma que comiencen a generarse según sean necesarias y realizaremos espera activa para filtrar los datos no necesarios.
Hardware:
| Pieza | Cantidad | Precio (€/unidad) | Descripción (si se precisa) |
| Motor paso a paso | 2 | 5.12 | |
| Controlador Drv8825 | 2 | 2.40 | (controladores de los motores) |
| Arduino NANO | 1 | 4.66 | |
| rueda LEGO | 2 | 2.99 | |
| buzzer | 1 | 2.99 | |
| IMU (MPU 6050) | 1 | 8.67 | |
| soporte | 1 | 1.00 | hemos usado una regla |
| bateria | 1 | 8 pilas por 3.66 | hemos usado 8 pilas con aluminio para hacer de batería |
| breadboard | 1 | 3.50 | para el circuito |
| cables | 120 cables con pines 6.99 |
- Reparto de tareas
El reparto de tareas ha sido el siguiente:
- Montura de hardware: Conjunto, organizamos días para quedar físicamente y realizar el proyecto.
- Archivo principal(Self_Balancing_Robot_Project_Code.ino): Conjunto, exactamente igual que el punto anterior.
- PID: Santiago Serrano Marco
- Filtro complementario: Rodrigo Martínez Ruiz
- Extracción datos MPU: Rodrigo Martínez Ruiz
- Interrupciones: Alejandro Valtierra Baños
- Memoria: Alejandro Valtierra Baños
- Vídeo: Santiago Serrano Marco
- Problemas y soluciones encontradas
- Interrupciones: Las interrupciones en Arduino base son mediante hardware, de modo que seleccionas uno de los pines y lo preparas de forma que si llega un flanco de subida o bajada (decisión tuya) genere una interrupción controlada por un handler. Como nuestro objetivo es generar interrupciones continuas se buscó una forma de evitar tener que comer el tiempo de ejecución poniendo salidas en un pin mediante el reloj; se encontró una librería que genera interrupciones según el reloj y te permite establecer el periodo entre interrupciones.
- Interrupciones: Otro de los problemas era la cantidad de tiempo gastado en interrupciones, lo que hacía que casi se gastase todo el tiempo en ellas en vez de programa, por tanto la solución fue encontrar instrucciones atómicas que en el peor de los casos hacen que las interrupciones ocupen menos de un tercio del total de las instrucciones en arduino, y dado que se usa espera activa no se pierde prácticamente nada.
- IMU: Al principio se pensó que la IMU daría los valores correctos en cada momento, pero resulta que comete errores, lo cual hizo la necesidad de crear la clase filtroComplementario con el objetivo de ajustar los errores y que no salieran ángulos incorrectos para generar las interrupciones.
- Código
Para no llenar el documento de código se adjunta en el zip y se hará referencia a las funciones y su explicación.
- filtroComplementario.h:
filtroComplementario(float escGyro, float escAcel): constructor.
void setFiltroAcel (float x): set para el filtro de la aceleración.
void setFiltroPasa (float ratio): set para el filtro de el paso alto y el bajo.
void filtraAcel (): se usa para evitar los valores del acelerometro inútiles dado que son muy sensibles
void calcularAngulo(int8_t i): calculamos el nuevo ángulo.
void calcularAngulos(): simplemente llama a los tres calcularAngulo.
void calcularPeriodo(): calcula el periodo al que deben producirse las interrupciones.
- pid.h:
pid (): es el constructor de la clase
float calcularPID (float error): esta función la utilizamos para calcular el valor exacto en base al error
void calcularPeriodo(): Nos sirve para calcular el periodo al que se deben generar las interrupciones.
void setPeriodo (uint32_t periodo): Función set para el tPeriodo.
void setVariables (float p, float i, float d): Función para hacer set de las variables de los tres errores del PID ya explicados.
float calcularP (float error): Calcular el error proporcional.
float calcularI (float error): Calcular el error integral.
float calcularD (float error): Calcular el error derivativo
- I2Cdev.h: Es una librería adicional requerida por la siguiente.
- MPU6050.h: Usaremos esta librería para sacar los valores de la IMU.
- Self_Balancing_Robot_Project_Code.ino:
setup(): Inicializamos las variables, ponemos el modo de los pines, ponemos unos tiempos de espera para la preparación indicándose con un buzzer, se comprueba que funcione la IMU e inicia el periodo para las interrupciones.
loop(): Recogemos los valores de la IMU, los procesamos y generamos el nuevo periodo de interrupciones habiendo dicho la dirección de los motores, después hacemos una espera activa para filtrar los valores antes de volver a leer los valores.
interrupcion(): Es el handler de la interrupción, mantiene la salida de los motores durante 32 ciclos en alto y luego la para, lo suficiente para que den un paso.
pitidos(): Sirve solo para dar tres pitidos cuando se la llame.
- Casos de uso
El robot se basa en la implementación de un sistema de control retroalimentado, es una demostración del mismo.
La teoría dice que a diferencia de un sistema de control abierto, la actuación del sistema se calcula respecto a lo solicitado y a la posición actual, en vez de solamente calcularla con respecto a lo solicitado.
Nuestra implementación de esta demostración, es un robot autobalanceado, similar a un SEGWAY.
Su funcionamiento comienza con los datos de la IMU que una vez son procesadas por el filtro complementario, nos dan el ángulo de inclinación actual del robot.
Con este ángulo se calcula la diferencia o error respecto al ángulo deseado, que es para nosotros la posición de equilibrio. Si este error es mayor de 20º se considera que el robot no se puede recuperar y se apaga.
Este error, es la entrada del pid que calcula la actuación, Actúa proporcionalmente respecto al error, si el error no mejora, aumenta el valor de la salida y si la velocidad de cambio del error es muy alta, actúa contrariamente para conseguir que una vez llegado a su objetivo (error = 0) se mantenga allí y no cree un error opuesto. Ajustando el peso de todas estas correcciones, se consigue llegar a error 0 de la manera más rápida posible.
