Robot de autoequilibrio Arduino

¡Hola a todos!

En este instructable, te mostraré cómo construir un pequeño robot de auto-equilibrio que pueda moverse evitando obstáculos. Este es un pequeño robot que mide 4 pulgadas de ancho y 4 pulgadas de alto y se basa en la placa de desarrollo Arduino Pro Mini y el módulo acelerómetro-giroscopio MPU6050.

En los siguientes pasos, veremos cómo conectar el MPU6050 con Arduino, cómo medir el ángulo de inclinación del robot, cómo usar PID para que el robot se mantenga equilibrado. También se agrega un telémetro ultrasónico al robot que evita que se golpee contra obstáculos mientras deambula.

Lista de partes

Compré la mayoría de estas piezas de aliexpress, pero también puedes encontrarlas en cualquier otra tienda de electrónica.

1. Arduino Pro Mini

2. Módulo GY-521 con MPU-6050

3. DRV8833 Pololu motor driver

4. 2, convertidor de impulso de 5V

5. Sensor de distancia ultrasónico US-020

6. Batería y soporte NCR18650

7. Par de motores de engranajes de micro metal (N20, 6V, 200 rpm) y soportes

8. Par de ruedas de 42x19 mm.

9. 3, PCB prototipo de doble cara (4 cm x 6 cm)

10. 8 espaciadores de nylon de 25 cm y 4 tuercas de nylon

Además de lo anterior, necesitará algunos cables, conectores tipo berg y un interruptor de encendido / apagado.

Paso 1: un poco de teoría

Comencemos con algunos fundamentos antes de ensuciarnos las manos.

El robot de equilibrio automático es similar a un péndulo invertido. A diferencia de un péndulo normal que sigue balanceándose una vez que se le da un empujón, este péndulo invertido no puede mantenerse equilibrado por sí solo. Simplemente se caerá. Entonces, ¿cómo lo equilibramos? Considere equilibrar una escoba en nuestro dedo índice, que es un ejemplo clásico de equilibrar un péndulo invertido. Movimos nuestro dedo en la dirección en que cae el palo. Similar es el caso con un robot de equilibrio automático, solo que el robot caerá hacia adelante o hacia atrás. Así como equilibramos un palo en nuestro dedo, equilibramos el robot moviendo sus ruedas en la dirección en la que está cayendo. Lo que estamos tratando de hacer aquí es mantener el centro de gravedad del robot exactamente por encima del punto de pivote.

Para conducir los motores necesitamos cierta información sobre el estado del robot. Necesitamos saber en qué dirección está cayendo el robot, cuánto se ha inclinado el robot y la velocidad con la que está cayendo. Toda esta información se puede deducir de las lecturas obtenidas de MPU6050. Combinamos todas estas entradas y generamos una señal que impulsa los motores y mantiene el robot equilibrado.

Paso 2: Comencemos a construir

Primero completaremos los circuitos y la estructura del robot. El robot está construido sobre tres capas de perfboards que están separados 25 mm con separadores de nylon. La capa inferior contiene los dos motores y el controlador del motor. La capa intermedia tiene el controlador, la IMU y los módulos reguladores de refuerzo de 5V. La capa superior tiene la batería, un interruptor de encendido / apagado y el sensor de distancia ultrasónico (lo instalaremos hacia el final una vez que equilibremos el robot).

Antes de comenzar a crear un prototipo en una tabla de perfilar, debemos tener una idea clara de dónde se debe colocar cada parte. Para facilitar la creación de prototipos, siempre es mejor dibujar el diseño físico de todos los componentes y utilizarlo como referencia para colocar los componentes y enrutar los puentes en el panel de perfilado. Una vez que todas las partes están colocadas y soldadas, interconecte las tres tablas con espaciadores de nylon.

Es posible que haya notado que he usado dos módulos reguladores de voltaje separados para conducir los motores y el controlador a pesar de que ambos requieren una fuente de 5V. Esto es muy importante. En mi primer diseño, utilicé un solo regulador de refuerzo de 5V para encender el controlador y los motores. Cuando encendí el robot, el programa se congela intermitentemente. Esto se debió al ruido generado por el circuito del motor que actúa sobre el controlador y la IMU. Esto se eliminó efectivamente separando el regulador de voltaje del controlador y el motor y agregando un condensador de 10uF en los terminales de la fuente de alimentación del motor.

Paso 3: medición del ángulo de inclinación con el acelerómetro

El MPU6050 tiene un acelerómetro de 3 ejes y un giroscopio de 3 ejes. El acelerómetro mide la aceleración a lo largo de los tres ejes y el giroscopio mide la velocidad angular sobre los tres ejes. Para medir el ángulo de inclinación del robot, necesitamos valores de aceleración a lo largo de los ejes y y z. La función atan2 (y, z) da el ángulo en radianes entre el eje z positivo de un plano y el punto dado por las coordenadas (z, y) en ese plano, con signo positivo para ángulos en sentido antihorario (mitad derecha plano, y> 0) y signo negativo para ángulos en el sentido de las agujas del reloj (semiplano izquierdo, y <0). Utilizamos esta biblioteca escrita por Jeff Rowberg para leer los datos de MPU6050. Cargue el código que figura a continuación y vea cómo varía el ángulo de inclinación.

#incluye "Wire.h"
#include "I2Cdev.h" #include "MPU6050.h" #include "math.h"

MPU6050 mpu;

int16_t accY, accZ; flotar en ángulo;

configuración nula () {mpu.initialize (); Serial.begin (9600); }

bucle vacío () {accZ = mpu.getAccelerationZ (); accY = mpu.getAccelerationY (); accAngle = atan2 (accY, accZ) * RAD_TO_DEG; if (isnan (accAngle)); más Serial.println (accAngle); }

Intente mover el robot hacia adelante y hacia atrás mientras lo mantiene inclinado en un ángulo fijo. Observará que el ángulo que se muestra en su monitor en serie cambia repentinamente. Esto se debe a que el componente horizontal de la aceleración interfiere con los valores de aceleración de los ejes y y z.

Paso 4: medición del ángulo de inclinación con el giroscopio

El giroscopio de 3 ejes de MPU6050 mide la velocidad angular (velocidad de rotación) a lo largo de los tres ejes. Para nuestro robot de equilibrio automático, la velocidad angular a lo largo del eje x solo es suficiente para medir la velocidad de caída del robot.

En el código que figura a continuación, leemos el valor del giroscopio sobre el eje x, lo convertimos a grados por segundo y luego lo multiplicamos por el tiempo del bucle para obtener el cambio de ángulo. Agregamos esto al ángulo anterior para obtener el ángulo actual.

#incluye "Wire.h"
#include "I2Cdev.h" #include "MPU6050.h"

MPU6050 mpu;

int16_t gyroX, gyroRate; flotar gyroAngle = 0; unsigned long currTime, prevTime = 0, loopTime;

configuración nula () {mpu.initialize (); Serial.begin (9600); }

bucle vacío () {currTime = millis (); loopTime = currTime - prevTime; prevTime = currTime; gyroX = mpu.getRotationX (); gyroRate = mapa (gyroX, -32768, 32767, -250, 250); gyroAngle = gyroAngle + (float) gyroRate * loopTime / 1000; Serial.println (gyroAngle); }

La posición del MPU6050 cuando el programa comienza a ejecutarse es el punto de inclinación cero. El ángulo de inclinación se medirá con respecto a este punto.

Mantenga el robot estable en un ángulo fijo y observará que el ángulo aumentará o disminuirá gradualmente. No se mantendrá estable. Esto se debe a la deriva inherente al giroscopio.

En el código anterior, el tiempo de bucle se calcula utilizando la función millis () que está integrada en el IDE de Arduino. En pasos posteriores, utilizaremos interrupciones de temporizador para crear intervalos de muestreo precisos. Este período de muestreo también se utilizará para generar la salida utilizando un controlador PID.

Paso 5: combinar los resultados con un filtro complementario

Google define complementario como " combinar de manera tal que mejore o enfatice las cualidades de cada uno u otro ".

Tenemos dos medidas del ángulo de dos fuentes diferentes. La medición del acelerómetro se ve afectada por movimientos horizontales repentinos y la medición del giroscopio se aleja gradualmente del valor real. En otras palabras, la lectura del acelerómetro se ve afectada por señales de corta duración y la lectura del giroscopio por señales de larga duración. Estas lecturas son, en cierto modo, complementarias entre sí. Combínelos con un filtro complementario y obtendremos una medición estable y precisa del ángulo. El filtro complementario es esencialmente un filtro de paso alto que actúa sobre el giroscopio y un filtro de paso bajo que actúa sobre el acelerómetro para filtrar la deriva y el ruido de la medición.

currentAngle = 0.9934 * (anteriorAngle + gyroAngle) + 0.0066 * (accAngle)

0.9934 y 0.0066 son coeficientes de filtro para una constante de tiempo de filtro de 0.75s. El filtro de paso bajo permite que pase cualquier señal más larga que esta duración y el filtro de paso alto permite que pase cualquier señal más corta que esta duración. La respuesta del filtro se puede ajustar seleccionando la constante de tiempo correcta. Bajar la constante de tiempo permitirá que pase más aceleración horizontal.

Eliminando los errores de compensación del acelerómetro y giroscopio
Descargue y ejecute el código proporcionado en esta página para calibrar las compensaciones del MPU6050. Cualquier error debido a la compensación puede eliminarse definiendo los valores de compensación en la rutina setup () como se muestra a continuación.

mpu.setYAccelOffset (1593);
mpu.setZAccelOffset (963); mpu.setXGyroOffset (40);

Paso 6: Control PID para generar salida

PID significa Proporcional, Integral y Derivado. Cada uno de estos términos proporciona una respuesta única a nuestro robot de equilibrio automático.

El término proporcional, como su nombre lo indica, genera una respuesta que es proporcional al error. Para nuestro sistema, el error es el ángulo de inclinación del robot.

El término integral genera una respuesta basada en el error acumulado. Esto es esencialmente la suma de todos los errores multiplicados por el período de muestreo. Esta es una respuesta basada en el comportamiento del sistema en el pasado.

El término derivado es proporcional a la derivada del error. Esta es la diferencia entre el error actual y el error anterior dividido por el período de muestreo. Esto actúa como un término predictivo que responde a cómo podría comportarse el robot en el próximo ciclo de muestreo.

Multiplicando cada uno de estos términos por sus constantes correspondientes (es decir, Kp, Ki y Kd) y sumando el resultado, generamos la salida que luego se envía como comando para conducir el motor.

Paso 7: Ajuste de las constantes PID

1. Establezca Ki y Kd en cero y aumente gradualmente Kp para que el robot comience a oscilar alrededor de la posición cero.

2. Aumente Ki para que la respuesta del robot sea más rápida cuando está fuera de balance. Ki debe ser lo suficientemente grande como para que el ángulo de inclinación no aumente. El robot debe volver a la posición cero si está inclinado.

3. Aumente Kd para reducir las oscilaciones. Los excesos también deberían reducirse ahora.

4. Repita los pasos anteriores ajustando cada parámetro para lograr el mejor resultado.

Paso 8: Agregar el sensor de distancia

El sensor de distancia ultrasónico que he usado es el US-020. Tiene cuatro pines: Vcc, Trig, Echo y Gnd. Está alimentado por una fuente de 5V. Los pines de activación y eco están conectados respectivamente a los pines digitales 9 y 8 de Arduino. Utilizaremos la biblioteca NewPing para obtener el valor de distancia del sensor. Leeremos la distancia una vez cada 100 milisegundos y si el valor está entre 0 y 20 cm, le ordenaremos al robot que realice una rotación. Esto debería ser suficiente para alejar el robot del obstáculo.

Paso 9: el código completo

 #incluye "Wire.h" 

#include "I2Cdev.h" #include "MPU6050.h" #include "math.h" #include

#define leftMotorPWMPin 6 #define leftMotorDirPin 7 #define rightMotorPWMPin 5 #define rightMotorDirPin 4

#define TRIGGER_PIN 9 #define ECHO_PIN 8 #define MAX_DISTANCE 75

#define Kp 40 #define Kd 0.05 #define Ki 40 #define sampleTime 0.005 #define targetAngle -2.5

MPU6050 mpu; Sonda NewPing (TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

int16_t accY, accZ, gyroX; motor int volátil Power, gyroRate; flotación volátil, ángulo, girocompás, ángulo actual, ángulo anterior = 0, error, error anterior = 0, suma de error = 0; recuento de bytes volátiles = 0; int distanceCm;

void setMotors (int leftMotorSpeed, int rightMotorSpeed) {if (leftMotorSpeed> = 0) {analogWrite (leftMotorPWMPin, leftMotorSpeed); digitalWrite (leftMotorDirPin, LOW); } else {analogWrite (leftMotorPWMPin, 255 + leftMotorSpeed); digitalWrite (leftMotorDirPin, HIGH); } if (rightMotorSpeed> = 0) {analogWrite (rightMotorPWMPin, rightMotorSpeed); digitalWrite (rightMotorDirPin, LOW); } else {analogWrite (rightMotorPWMPin, 255 + rightMotorSpeed); digitalWrite (rightMotorDirPin, HIGH); }}

void init_PID () // inicializar Timer1 cli (); // deshabilitar interrupciones globales TCCR1A = 0; // establece el registro TCCR1A completo en 0 TCCR1B = 0; // lo mismo para TCCR1B // establece el registro de comparación de coincidencias para establecer el tiempo de muestra 5ms OCR1A = 9999; // activa el modo CTC TCCR1B

void setup () {// establece el control del motor y los pines PWM en el modo de salida pinMode (leftMotorPWMPin, OUTPUT); pinMode (leftMotorDirPin, OUTPUT); pinMode (rightMotorPWMPin, OUTPUT); pinMode (rightMotorDirPin, OUTPUT); // establece el LED de estado en modo de salida pinMode (13, OUTPUT); // inicializa el MPU6050 y establece los valores de desplazamiento mpu.initialize (); mpu.setYAccelOffset (1593); mpu.setZAccelOffset (963); mpu.setXGyroOffset (40); // inicializa el bucle de muestreo PID init_PID (); }

void loop () {// lee los valores de aceleración y giroscopio accY = mpu.getAccelerationY (); accZ = mpu.getAccelerationZ (); gyroX = mpu.getRotationX (); // establece la potencia del motor después de restringirlo motorPower = restrictin (motorPower, -255, 255); setMotors (motorPower, motorPower); // mide la distancia cada 100 milisegundos if ((count% 20) == 0) {distanceCm = sonar.ping_cm (); } if ((distanceCm <20) && (distanceCm! = 0)) {setMotors (-motorPower, motorPower); }} // El ISR se llamará cada 5 milisegundos ISR (TIMER1_COMPA_vect) {// calcular el ángulo de inclinación accAngle = atan2 (accY, accZ) * RAD_TO_DEG; gyroRate = mapa (gyroX, -32768, 32767, -250, 250); gyroAngle = (float) gyroRate * sampleTime; currentAngle = 0.9934 * (anteriorAngle + gyroAngle) + 0.0066 * (accAngle); error = currentAngle - targetAngle; errorSum = errorSum + error; errorSum = restricción (errorSum, -300, 300); // calcula la salida de los valores P, I y D motorPower = Kp * (error) + Ki * (errorSum) * sampleTime - Kd * (currentAngle-prevAngle) / sampleTime; prevAngle = currentAngle; // alterna el led en pin13 cada segundo cuenta ++; if (cuenta == 200) {cuenta = 0; digitalWrite (13, ! digitalRead (13)); }}

Paso 10: pensamientos finales

Pasar un poco más de tiempo ajustando las constantes PID nos daría un mejor resultado. El tamaño de nuestro robot también limita el nivel de estabilidad que podemos lograr. Es más fácil construir un robot de equilibrio de tamaño completo que construir uno pequeño como el nuestro. Aún así, supongo que nuestro robot hace un trabajo bastante decente al equilibrarse en varias superficies como se muestra en el video.

Eso es todo por ahora.

Gracias por tu tiempo. No olvides dejar tus pensamientos en la sección de comentarios.

Artículos Relacionados