MIRO es una plataforma abierta de robot de interior. Parte 5 - Componente de software: ARDUINO (AVR), subimos "bajo el capó"

Esta vez, veamos un poco más a fondo la implementación de algunos métodos de biblioteca clave para ARDUINO (AVR), que son responsables de mover el robot MIRO. Esta parte será interesante para todos los que se preguntaron cómo controlar la velocidad lineal y angular del robot en el ARDUINO, equipado con motores con los codificadores más simples.

Tabla de contenido: Parte 1 , Parte 2 , Parte 3 , Parte 4 , Parte 5 .

Los métodos responsables de conducir con odometría siguen siendo difíciles de explicar cómo, qué y por qué. Lo primero que debe saber sobre el control del movimiento del robot es el hecho simple y obvio de que los motores colectores del robot nunca giran a la misma velocidad sin un ajuste adicional. Embrague diferente, características de salida diferentes de los canales del conductor, motores eléctricos ligeramente diferentes y lubricación en la caja de cambios.

El segundo hecho que debe comprender y conocer es la presencia de inercia en el motor, incluso con una relación de transmisión suficientemente grande. Es decir Al eliminar el voltaje de los terminales del motor, la rueda, incluso sin carga, se mueve unos pocos grados más. La magnitud de esta rotación adicional depende de la fuerza de carga sobre la rueda, de la velocidad de rotación antes de aliviar el estrés y de los mismos factores invisibles como el tipo y la cantidad de lubricante en la caja de engranajes.

Estos hechos determinan la implementación de un grupo de métodos relacionados con el movimiento de un chasis equipado con sensores de odómetro (en el caso de MIRO, codificadores digitales de cada rueda).

Como descubrimos en la cuarta parte, en el modelo de software existe la clase Chassis , que implementa el control de rotación de motores de chasis individuales. Quiero enfatizar, no el control del movimiento del chasis, el trole, sino el control de los motores del trole. El control directo del carro se implementa en las clases Robot y Miro .

Comencemos desde arriba. A continuación se muestra un método de la clase Miro que implementa el movimiento de un robot a cierta distancia ( dist , metros) con una velocidad lineal ( lin_speed , m / s) y angular ( ang_speed , deg / s). Todavía no estamos prestando atención al parámetro en_break .

int Miro::moveDist(float lin_speed, float ang_speed, float dist, bool en_break) { float _wheelSetAngSpeed[WHEEL_COUNT]; _wheelSetAngSpeed[LEFT] = MIRO_PI2ANG * (lin_speed - (ROBOT_DIAMETER * ang_speed / (2 * MIRO_PI2ANG))) / WHEEL_RADIUS; _wheelSetAngSpeed[RIGHT] = MIRO_PI2ANG * (lin_speed + (ROBOT_DIAMETER * ang_speed / (2 * MIRO_PI2ANG))) / WHEEL_RADIUS; float _wheelSetAng[WHEEL_COUNT]; _wheelSetAng[RIGHT] = _wheelSetAngSpeed[RIGHT] * dist / lin_speed; _wheelSetAng[LEFT] = _wheelSetAngSpeed[LEFT] * dist / lin_speed; return this->chassis.wheelRotateAng(_wheelSetAngSpeed, _wheelSetAng, en_break); } 

En este método, primero se calculan las velocidades angulares REQUERIDAS para los motores izquierdo y derecho. Según fórmulas bastante obvias, lo que no es un problema para deducir. Solo es necesario tener en cuenta que la velocidad lineal en el método se especifica en metros por segundo, y la velocidad angular en grados por segundo (no en radianes). Por lo tanto, calculamos previamente la constante MIRO_PI2ANG = 57.29 = 180 / pi. ROBOT_DIAMETER : distancia entre las ruedas izquierda y derecha del robot (en metros), WHEEL_RADIUS : radio de la rueda (también en metros). Todas las constantes numéricas para tales casos están contenidas en el archivo defs.h, y los parámetros personalizados del robot y el chasis están en el archivo config.h.

Después de eso, se calcula el ángulo mediante el cual se debe girar cada rueda para que el robot recorra la distancia dist (también en metros).

Por lo tanto, en esta etapa obtenemos a qué velocidad y en qué ángulo necesita girar cada rueda del chasis del robot. Y luego se llama al método wheelRotateAng () del objeto del chasis .

El método wheelRotateAng (float * speed, float * ang, bool en_break) se usa para rotar las ruedas del robot con las velocidades angulares especificadas por la matriz speed [] (en m / s) según los ángulos especificados por la matriz ang [] (en grados). El último parámetro en_break (que ya cumplimos anteriormente) establece el requisito de una parada brusca de las ruedas después de hacer un giro al aplicarles voltaje inverso a corto plazo. Esto es necesario para suprimir la inercia del robot, evitando que se mueva más allá de la distancia requerida después de eliminar el voltaje de control de los motores. Para una satisfacción completa, por supuesto, existe el método wheelRotateAngRad () , similar a wheelRotateAng () con la diferencia de que toma los valores de los ángulos de rotación y las velocidades angulares en radianes y radianes por segundo como parámetros.

El algoritmo del método wheelRotateAng () es el siguiente.

1. Primero, se verifica la correspondencia de los valores de velocidad [] y ang [] con algunas condiciones de contorno. Obviamente, el chasis tiene limitaciones físicas tanto en la velocidad angular máxima de rotación de las ruedas como en la mínima (velocidad mínima de alejamiento). Además, los ángulos en ang [] no pueden ser inferiores al ángulo de rotación fijo mínimo, determinado por la precisión de los codificadores.

2. A continuación, se calcula la dirección de rotación de cada rueda. Obviamente a través del signo del producto ang [i] * velocidad [i] ;

3. Se calcula la "distancia de rotación" Dw [i] para cada rueda: el número de muestras de codificador que se deben hacer para rotar en el ángulo dado [i] .
Este valor está determinado por la fórmula:

Dw [i] = ang [i] * WHEEL_SEGMENTS / 360 ,
donde WHEEL_SEGMENTS es el número de segmentos de la rueda del codificador (revolución completa).

4. Se registra el valor de voltaje en el controlador del motor.


5. De acuerdo con la tabla de calibración para cada rueda, se determina el valor inicial de la señal PWM, lo que garantiza la rotación de la rueda con la velocidad de velocidad requerida [i] . ¿Qué tipo de tabla de calibración y de dónde vino? Analizaremos más a fondo.

6. La rotación de los motores se inicia de acuerdo con los valores calculados de velocidad y dirección de rotación. En el texto de la implementación de la clase, el método privado _wheel_rotate_sync () es responsable de esto.

Vamos aún más profundo. El método _wheel_rotate_sync () funciona de acuerdo con el siguiente algoritmo:

1. En un bucle infinito, se realiza una comprobación para lograr el contador de las respuestas del codificador de la distancia de giro Dw [i] para cada rueda. Si se alcanza CUALQUIERA de los contadores Dw [i] , todas las ruedas se detienen y salen del ciclo y luego salen de la función (paso 5). Esto se hace por las siguientes razones. Debido a la discreción de medir el ángulo de rotación, es una situación muy común cuando la distancia calculada Dw [i] de una rueda se obtiene al redondear un valor no entero a un lado más pequeño y Dw [j] de la segunda rueda a una más grande. Esto lleva al hecho de que después de detener una de las ruedas, la segunda rueda continúa girando. Para un chasis con un accionamiento diferencial (y para muchos otros), esto lleva a un "giro" no planificado del robot al final de la tarea. Por lo tanto, en el caso de organizar el movimiento espacial de todo el chasis, es necesario detener todos los motores a la vez.

2. Si no se alcanza Dw [i] , entonces en el bucle se verifica el hecho de la siguiente operación del codificador (la variable _syncloop [w] , actualizada desde la interrupción del codificador y restablecida en este bucle infinito). Cuando se produce la próxima intersección, el programa calcula el módulo de la velocidad angular actual de cada rueda (grados / s), de acuerdo con la fórmula obvia:

W [i] = (360 * tau [i]) / WHEEL_SEGMENTS ,
donde:
tau [i] : el valor promedio del tiempo entre las dos últimas respuestas de los codificadores. La "profundidad" del filtro de promedio está determinada por MEAN_DEPTH y su valor predeterminado es 8.

3. En función de las velocidades calculadas de las ruedas, los errores absolutos se calculan como las diferencias entre las velocidades angulares establecidas y reales.

4. En función de los errores calculados, la acción de control (valor de señal PWM) se corrige para cada motor.

5. Después de alcanzar Dw [i] , en el caso de un en_break activo, se aplica voltaje inverso a corto plazo a los motores. La duración de este efecto se determina a partir de la tabla de calibración (ver más abajo) y generalmente varía de 15 a 40 ms.

6. Hay una liberación completa de tensión de los motores y sale de _wheel_rotate_sync () .

Ya he mencionado una cierta tabla de calibración dos veces. Entonces, en la biblioteca hay una tabla especial de valores almacenados en la EEPROM de la memoria del robot y que contiene registros de tres valores relacionados:

1. Voltaje en los terminales del motor. Se calcula traduciendo el valor de la señal PWM en el voltaje real. Para esto, en el paso 4 del método wheelRotateAng () , se registra el voltaje real en el controlador del motor.

2. La velocidad angular de rotación de la rueda (sin carga) correspondiente a un voltaje dado.

3. La duración de la señal de parada dura correspondiente a esta velocidad angular.
Por defecto, el tamaño de la tabla de calibración es de 10 registros (determinado por la constante WHEEL_TABLE_SIZE en el archivo config.h ) - 10 triples de los valores "voltaje - velocidad angular - duración de la señal de parada".

Para determinar los valores de 2 y 3 entradas en esta tabla, se utiliza un método especial: wheelCalibrate (rueda de bytes) .

Echemos un vistazo a esto. Este método implementa una secuencia de acciones para determinar los valores faltantes en la tabla de calibración del motor / rueda, así como para determinar la velocidad angular mínima de arranque y la velocidad angular máxima de la rueda.

Para realizar la calibración, el robot se monta en un soporte; toda la rotación de las ruedas durante la calibración se realiza sin carga.

1. Primero debe determinar la velocidad mínima de arranque. Esto se hace de manera muy simple. En un ciclo, el PWM de control se alimenta al motor, comenzando desde 0, con un incremento de 1. En cada paso, el programa espera un tiempo, determinado por la constante WHEEL_TIME_MAX ( retraso normal () ). Una vez transcurrido el tiempo de espera, verifica si se ha completado el inicio (cambiando el valor del contador del codificador). Si se completa la polea, entonces se calcula la velocidad angular de rotación de la rueda. Para mayor certeza, el valor de 10 se agrega al valor de PWM correspondiente a esta velocidad de arranque, lo que da el primer par de valores "voltaje en el motor" - "velocidad angular".

2. Después de encontrar la velocidad de inicio, se calcula el paso PWM para llenar uniformemente la tabla de calibración.

3. En el ciclo, para cada nuevo valor de PWM, la rueda gira 2 vueltas completas y la velocidad angular se mide de acuerdo con un algoritmo similar al método _wheel_rotate_sync () . En el mismo ciclo, también por aproximación sucesiva, se mide el valor óptimo de la duración de la señal de parada dura. Inicialmente, se toma un valor obviamente grande. Y luego se prueba en el modo "giro-parada". Como el óptimo, se selecciona el valor máximo de la duración de la señal de parada, en el que no se supera la "distancia de giro" establecida. En otras palabras, dicho valor de la duración de la señal, una vez que se suministra al motor, por un lado, se suprime la inercia y, por otro lado, no hay movimiento inverso a corto plazo (que se fija con el mismo codificador).

4. Una vez completada la calibración, el voltaje de control al motor calibrado deja de aplicarse y la tabla de calibración de esta rueda se registra en la EEPROM.

Omití todo tipo de trivialidades de implementación y traté de expresar la esencia. Puede observar que los métodos wheelRotateAng () y wheelRotateAngRad () son funciones de bloqueo. Este es el precio por la precisión del movimiento y una integración bastante simple en los bocetos de los usuarios. Sería posible hacer un pequeño administrador de tareas con un tiempo fijo, pero esto requeriría que el usuario integrara su funcionalidad estrictamente en la cuota de tiempo asignada.

Y para una aplicación sin bloqueo, la API tiene una función wheelRotate (flotante * velocidad) . Como se puede ver en la lista de parámetros, simplemente realiza la rotación de las ruedas con las velocidades establecidas. Y la velocidad de rotación se ajusta en el método Sync () del chasis del robot, que se llama en el método Sync () del objeto de clase Miro del mismo nombre. Y de acuerdo con los requisitos para la estructura del boceto del usuario, este método debería llamarse cada iteración del bucle principal () del boceto ARDUINO.

En el paso 4, en la descripción del método _wheel_rotate_sync () , mencioné la "corrección de control" del motor. ¿Cómo lo adivinaste? Este es el controlador PID). Bueno, más precisamente el controlador PD. Como saben (de hecho, no siempre), la mejor manera de determinar los coeficientes del regulador es la selección). Hay una definición en el archivo de configuración config.h:

 #define DEBUG_WHEEL_PID 

Si lo descomenta, cuando llame al método moveDist () de la clase Miro, el siguiente gráfico invertido del error relativo en el control de la velocidad angular de una de las ruedas del robot (izquierda) se mostrará en la consola del robot.



¿No se parece a nada)? Abajo está el tiempo (cada barra es un paso del ciclo de control), y el valor del error se guarda a la derecha (con el signo preservado). Aquí hay dos pares de gráficos en la misma escala con diferentes coeficientes del controlador PD. Los "jorobados" son solo las "olas" de sobreimpulso. Los números en las barras horizontales son un error relativo (con la preservación del signo). Visualización simple del regulador, ayudando a ajustar manualmente los coeficientes. Con el tiempo, espero hacer una configuración automática, pero por ahora.

Aquí hay un adok :-)

Bueno, por último, veamos un ejemplo. Directamente desde la biblioteca API_Miro_moveDist:

 #include <Miro.h> using namespace miro; byte PWM_pins[2] = { 5, 6 }; byte DIR_pins[2] = { 4, 7 }; byte ENCODER_pins[2] = { 2, 3 }; Miro robot(PWM_pins, DIR_pins, ENCODER_pins); int laps = 0; void setup() { Serial.begin(115200); } void loop() { for (unsigned char i = 0; i < 4; i++) { robot.moveDist(robot.getOptLinSpeed(), 0, 1, true); delay(500); robot.rotateAng(0.5*robot.getOptAngSpeed(), -90, true); delay(500); } Serial.print("Laps: "); Serial.println(laps); laps++; } 

Del texto del programa todo debe quedar claro. Cómo funciona: en el video.


Baldosas de 600 x 600 mm y espacios de baldosas de 5 mm. En teoría, el robot debe rodear un cuadrado con un lado de 1 metro. Por supuesto, la trayectoria "flota". Pero para ser justos, vale la pena decir que en la versión del robot que me queda para las pruebas, hay motores bastante giratorios que son difíciles de manejar lentamente. Pero a alta velocidad y deslizamiento hay un lugar para estar, y la inercia no es fácil de manejar. Los motores con una relación de transmisión más alta (como incluso en nuestros robots MIRO, que no estaban disponibles durante la prueba) deberían comportarse algo mejor.

Si hay momentos incomprensibles, me complace aclarar, debatir y mejorar. La retroalimentación es generalmente interesante.