¡Hola a todos! Como resultado de la transición al trabajo remoto, obtuve más tiempo libre para desarrollar un hexápodo (+2 horas al día debido a los ahorros en el camino). Finalmente pude hacer un algoritmo universal para construir una ruta de movimiento en tiempo real. Las nuevas matemáticas permitieron implementar movimientos básicos cambiando solo dos parámetros. Este es otro paso hacia la implementación del "piloto automático". En este artículo trataré de hablar en detalle sobre las nuevas matemáticas y cómo funciona en general. Habrá muchas fotos y gifs.

Etapas de desarrollo:

Parte 1 - diseño
Parte 2 - montaje
Parte 3 - cinemática
Parte 4 - matemática de trayectorias y secuencias
Parte 5 - electrónica
Parte 6 - transición a la impresión 3D
Parte 7: nuevas viviendas, software de aplicación y protocolos de comunicación
Parte 8: matemáticas avanzadas de movimiento

Atrás en el tiempo

Hasta este punto, todos los movimientos básicos (hacia adelante, hacia atrás, rotación) se configuraron en forma de "mover la pierna desde la posición actual al punto (X, Y, Z) usando movimiento lineal / sinusoidal / cualquier movimiento". Esto funciona bastante bien y de manera confiable, pero limita severamente el movimiento. Por ejemplo, para implementar el movimiento a lo largo de un arco con un radio de R1, debe calcular de antemano las coordenadas del comienzo y el final del movimiento para cada miembro, agregar un botón adicional a la aplicación para que pueda seleccionar este movimiento. En consecuencia, para agregar movimiento a lo largo de un arco con un radio R2 diferente, debe calcular las coordenadas nuevamente y agregar otro botón. Extremadamente incomodo.

Ahora, para la implementación de movimientos básicos, se utiliza una unidad matemática, que se puede adaptar a diversas situaciones con dos parámetros. ¡La principal ventaja de las nuevas matemáticas fue el apoyo al movimiento a lo largo de un arco con un cambio en su radio justo durante el movimiento!

Idea del algoritmo

Primero, vale la pena explicar cómo funciona en los dedos. ¿Qué sucede si selecciona una ventana de tamaño fijo en un círculo y comienza a aumentar su radio? Eso es lo que:


¿Todo es verdad? Al cambiar el radio del círculo, podemos obtener diferentes trayectorias de movimiento y con cierta precisión una línea recta. Era posible detenerse en esto, pero no todo es tan color de rosa. No puede simplemente tomar e implementar esto solo en función del radio: hay matices.

Las posiciones iniciales de las extremidades del hexápodo pueden estar en diferentes círculos, respectivamente, los parámetros de la ecuación ya serán diferentes. En mi caso, las extremidades están dispuestas de la siguiente manera (aproximadamente):


El valor de la distancia que debe recorrer una extremidad depende del radio del círculo en el que se encuentra. Esto nos obliga a cambiar a un cálculo individual de la trayectoria de cada miembro. Para resolver este problema, necesita encontrar un círculo con un radio máximo, calcular los ángulos inicial y final del arco. Más relativo al arco resultante hay arcos para otras extremidades. Hice una animación de este algoritmo:


Hay animación para ilustrar el trabajo de toda esta magia (qué contento estoy de que en nuestro tiempo haya Excel). Al principio, el valor de la distancia que debe recorrer el hexapod por ciclo (similar a la velocidad) cambia, luego cambia el valor de la curvatura de la trayectoria (similar a girar el volante).


En general, esta es la idea de las nuevas matemáticas, espero que haya sido accesible para explicar. Ahora puede analizar el algoritmo con más detalle e intentar contar todo a mano en la práctica.

Matemáticas

Parámetros de entrada

Los parámetros de entrada variables son la distancia (distancia) y la curvatura de la ruta de movimiento (curvatura). El valor de la curvatura de la trayectoria debe estar en los rangos [-1.999; -0,001] y [0,001; 1.999], mientras que el valor de la distancia máxima depende de las características físicas del hexapod. En mi caso, la distancia máxima por ciclo es de 110 mm, con valores grandes, las extremidades comienzan a apoyarse entre sí. Para un ejemplo de cálculo, tomamos los valores de curvatura = 1.5 y distancia = 20.

Además, para que el algoritmo funcione, es necesario conocer las posiciones iniciales de las extremidades. Estos son los puntos donde se encuentran las extremidades cuando el hexápodo está de pie. Por ejemplo, utilizaremos los siguientes puntos (el origen de cada extremidad se encuentra en el punto de unión de COXA):


Nota: Trabajo en el plano XZ y puedes ignorar la coordenada Y. Puede familiarizarse con el sistema de coordenadas hexapod en la tercera parte del ciclo.

Como resultado, tenemos lo siguiente:

Fórmulas y cálculos.

Comenzamos calculando las coordenadas del centro de movimiento del hexapod dependiendo del valor de curvatura y distancia:

$$

$$R = tg(\frac{(2 - curvature) * \Pi}{4}) * distance$$

$$

$$R = tg(\frac{(2 - 1.5) * \Pi}{4}) * 20 = 8.28$$

Como resultado, obtuvimos el punto [R; 0] y la trayectoria del cuerpo del hexápodo. En relación con él, se calcularán las trayectorias para cada miembro.


A continuación, es necesario calcular los radios de las trayectorias para cada miembro en relación con el centro de movimiento (punto [R; 0]) teniendo en cuenta la posición inicial del miembro [x0; z0]. Un lenguaje más comprensible es encontrar la longitud del vector dibujado desde el punto [R; 0] hasta el punto [x0; z0]:

$$

$$R_i = \sqrt{(R - x_{0i})^2 + z_{0i}^2}$$

$$

$$R_0 = \sqrt{(8.28 - (-20))^2 + 20^2} = 34.64$$

$$

$$R_1 = \sqrt{(8.28 - (-35))^2 + 0^2} = 43.28$$

$$

$$R_2 = \sqrt{(8.28 - (-20))^2 + (-20)^2} = 34.64$$

$$

$$R_3 = \sqrt{(8.28 - 20)^2 + (-20)^2} = 23.17$$

$$

$$R_4 = \sqrt{(8.28 - 35)^2 + 0^2} = 26.71$$

$$

$$R_5 = \sqrt{(8.28 - 20)^2 + 20^2} = 23.17$$

Imagen para mayor claridad. El azul muestra los vectores deseados.


De los valores obtenidos encontramos el máximo:

$$

$$R_{max} = maximum(R_{0-5})$$

$$

$$R_{max} = 43.28$$

Luego, debes encontrar el ángulo para cada vector que probamos en el paso anterior.

$$

$$\alpha_{0i} = atan2(z_{0i}; -(R - x_{0i}))$$

$$

$$\alpha_{00} = atan2(20; -(8.28 - (-20))) = 2.52 (144.7°)$$

$$

$$\alpha_{01} = atan2(0; -(8.28 - (-35))) = 3.14 (180°)$$

$$

$$\alpha_{02} = atan2(-20; -(8.28 - (-20))) = -2.52 (-144.7°)$$

$$

$$\alpha_{03} = atan2(-20; -(8.28 - 20)) = -1.04 (-59.6°)$$

$$

$$\alpha_{04} = atan2(0; -(8.28 - 35)) = 0 (0°)$$

$$

$$\alpha_{05} = atan2(20; -(8.28 -20)) = 1.04 (59.6°)$$

Ahora encontramos el ángulo del arco en el círculo de radio más grande R_max (el más distante del centro de movimiento de la trayectoria), cuya longitud debe ser igual al valor de la distancia. Este ángulo determina los ángulos inicial y final de otros arcos a lo largo de los cuales se moverán las extremidades. Creo que la siguiente imagen ayudará a entender esto.


El ángulo se calcula de la siguiente manera:

$$

$$arcMax = sign(R) * \frac{distance}{R_{max}}$$

$$

$$arcMax = \frac{20}{43.28} = 0.462 (26°)$$

Además de usar este ángulo, podemos calcular los ángulos inicial y final de los arcos para otras extremidades. Algo como esto debería resultar:


Una pequeña digresión. Para implementar este algoritmo, es necesario introducir el concepto de tiempo, cuyo valor se encuentra en el rango [0; 1] También es necesario que cada miembro con un valor de tiempo 0.5 regrese a su punto de partida. Esta regla es una verificación de la exactitud de los cálculos: todos los círculos deben pasar por los puntos de inicio de cada miembro.

A continuación, comienza el cálculo de puntos por los parámetros obtenidos de los arcos, utilizando las siguientes fórmulas:

$$

$$arcAngle_i = (time - 0.5) * \alpha_{0i} + arcMax$$

$$

$$x_i = R + R_i * cos(arcAngle_i)$$

$$

$$z_i = R_i * sin(arcAngle_i)$$

Código fuente de la función

static bool process_advanced_trajectory(float motion_time) {

    // Check curvature value
    float curvature = (float)g_current_trajectory_config.curvature / 1000.0f;
    if (g_current_trajectory_config.curvature == 0)    curvature = +0.001f;
    if (g_current_trajectory_config.curvature > 1999)  curvature = +1.999f;
    if (g_current_trajectory_config.curvature < -1999) curvature = -1.999f;
    
    //
    // Calculate XZ
    //
    float distance = (float)g_current_trajectory_config.distance;

    // Calculation radius of curvature
    float curvature_radius = tanf((2.0f - curvature) * M_PI / 4.0f) * distance;

    // Common calculations
    float trajectory_radius[SUPPORT_LIMBS_COUNT] = {0};
    float start_angle_rad[SUPPORT_LIMBS_COUNT] = {0};
    float max_trajectory_radius = 0;
    for (uint32_t i = 0; i < SUPPORT_LIMBS_COUNT; ++i) {
        
        float x0 = g_motion_config.start_positions[i].x;
        float z0 = g_motion_config.start_positions[i].z;

        // Calculation trajectory radius
        trajectory_radius[i] = sqrtf((curvature_radius - x0) * (curvature_radius - x0) + z0 * z0);

        // Search max trajectory radius
        if (trajectory_radius[i] > max_trajectory_radius) {
            max_trajectory_radius = trajectory_radius[i];
        }

        // Calculation limb start angle
        start_angle_rad[i] = atan2f(z0, -(curvature_radius - x0));
    }
    if (max_trajectory_radius == 0) {
        return false; // Avoid division by zero
    }

    // Calculation max angle of arc
    int32_t curvature_radius_sign = (curvature_radius >= 0) ? 1 : -1;
    float max_arc_angle = curvature_radius_sign * distance / max_trajectory_radius;

    // Calculation points by time
    for (uint32_t i = 0; i < SUPPORT_LIMBS_COUNT; ++i) {
        
        // Inversion motion time if need
        float relative_motion_time = motion_time;
        if (g_motion_config.time_directions[i] == TIME_DIR_REVERSE) {
            relative_motion_time = 1.0f - relative_motion_time;
        }

        // Calculation arc angle for current time
        float arc_angle_rad = (relative_motion_time - 0.5f) * max_arc_angle + start_angle_rad[i];

        // Calculation XZ points by time
        g_limbs_list[i].position.x = curvature_radius + trajectory_radius[i] * cosf(arc_angle_rad);
        g_limbs_list[i].position.z = trajectory_radius[i] * sinf(arc_angle_rad);
        
        // Calculation Y points by time
        if (g_motion_config.trajectories[i] == TRAJECTORY_XZ_ADV_Y_CONST) {
            g_limbs_list[i].position.y = g_motion_config.start_positions[i].y;
        }
        else if (g_motion_config.trajectories[i] == TRAJECTORY_XZ_ADV_Y_SINUS) {
            g_limbs_list[i].position.y = g_motion_config.start_positions[i].y;
            g_limbs_list[i].position.y += LIMB_STEP_HEIGHT * sinf(relative_motion_time * M_PI);  
        }
    }
    
    return true;
}

Luego decidí mostrar también el cálculo de la coordenada Y (Cálculo de los puntos Y por tiempo). El cálculo depende de la trayectoria elegida, que se establece rígidamente en el código y es necesaria para la implementación del movimiento de las extremidades en el suelo y en el aire.

También hay una pieza para implementar la dirección de movimiento inversa (tiempo de movimiento de inversión si es necesario). Es necesario que durante el movimiento de tres extremidades en el suelo, las otras tres extremidades se muevan en la dirección opuesta a través del aire.

resultados