Olá a todos! Como resultado da transição para o trabalho remoto, tive mais tempo livre para desenvolver um hexapod (+2 horas por dia devido à economia na estrada). Eu finalmente fui capaz de criar um algoritmo universal para construir um caminho de movimento em tempo real. A nova matemática tornou possível implementar movimentos básicos alterando apenas dois parâmetros. Este é outro passo para a implementação do "piloto automático". Neste artigo, tentarei falar em detalhes sobre a nova matemática e como ela geralmente funciona. Haverá muitas fotos e gifs.

Estágios de desenvolvimento:

Parte 1 - projeto
Parte 2 - montagem
Parte 3 - cinemática
Parte 4 - matemática de trajetórias e sequências
Parte 5 - eletrônica
Parte 6 - transição para impressão 3D
Parte 7 - novas habitações, software aplicativo e protocolos de comunicação
Parte 8 - matemática avançada do movimento

De volta no tempo

Até esse ponto, todos os movimentos básicos (frente, trás, rotação) eram definidos na forma de "mover a perna da posição atual para o ponto (X, Y, Z) usando movimentos lineares / sinusoidais / qualquer". Isso funciona muito bem e de forma confiável, mas limita severamente o movimento. Por exemplo, para implementar o movimento ao longo de um arco com um raio de R1, você precisa calcular antecipadamente as coordenadas do início e do fim do movimento para cada membro, adicione um botão adicional ao aplicativo para poder selecionar esse movimento. Portanto, para adicionar movimento ao longo de um arco com um raio R2 diferente, é necessário calcular as coordenadas novamente e adicionar outro botão. Extremamente desconfortável.

Agora, para a implementação de movimentos básicos, é utilizada uma unidade matemática, que pode ser adaptada a várias situações com dois parâmetros. O principal bônus da nova matemática foi o apoio ao movimento ao longo de um arco, com uma mudança de raio durante o movimento!

Idéia do algoritmo

Primeiro, vale a pena explicar como funciona nos dedos. O que acontece se você selecionar uma janela de tamanho fixo em um círculo e começar a aumentar seu raio? Isso é o que:

Tudo é verdade? Alterando o raio do círculo, podemos obter diferentes trajetórias de movimento e, com alguma precisão, uma linha reta. Foi possível parar com isso, mas nem tudo é tão róseo. Você não pode simplesmente implementar e implementar isso apenas com base no raio - existem nuances.

As posições iniciais dos membros do hexápode podem estar em círculos diferentes, respectivamente, os parâmetros da equação já serão diferentes. No meu caso, os membros são organizados da seguinte forma (aproximadamente):

O valor da distância que um membro deve percorrer depende do raio do círculo em que está localizado. Isso nos força a mudar para um cálculo individual da trajetória de cada membro. Para resolver esse problema, você precisa encontrar um círculo com um raio máximo, calcular os ângulos inicial e final do arco. Além disso, em relação ao arco resultante, existem arcos para outros membros. Fiz uma animação desse pedaço de algoritmo:

Há uma animação para ilustrar o trabalho de toda essa mágica (como estou feliz que, em nosso tempo, exista o Excel). No início, o valor da distância que o hexápode deve percorrer por ciclo (semelhante à velocidade) muda, depois o valor da curvatura da trajetória (semelhante à rotação do volante) muda.

Em geral, essa é a idéia da nova matemática, espero que seja acessível para explicar. Agora você pode analisar o algoritmo com mais detalhes e tentar contar tudo manualmente na prática.

Matemática

Parâmetros de entrada

Parâmetros de entrada variáveis são distância (distância) e curvatura do caminho do movimento (curvatura). O valor da curvatura da trajetória deve estar nos intervalos [-1,999; -0,001] e [0,001; 1.999], enquanto o valor da distância máxima depende das características físicas do hexapod. No meu caso, a distância máxima por ciclo é de 110 mm, com valores grandes, os membros começam a se encostar. Para um exemplo de cálculo, tomamos os valores curvatura = 1,5 e distância = 20.

Além disso, para que o algoritmo funcione, é necessário conhecer as posições iniciais dos membros. Estes são os pontos em que os membros estão localizados quando o hexápode está de pé. Por exemplo, usaremos os seguintes pontos (a origem de cada membro está localizada no ponto de conexão da COXA):

Nota: Eu trabalho no plano XZ e você pode ignorar a coordenada Y. Você pode se familiarizar com o sistema de coordenadas hexapod na terceira parte do ciclo e,

como resultado, temos o seguinte:

Fórmulas e cálculos

Começamos calculando as coordenadas do centro de movimento do hexápode, dependendo do valor da curvatura e da distância:

R = t g (\frac{(2 - c u r v a t u r e) * Π}{4}) * d i s t a n c e

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

R = t g (\frac{(2 - 1.5) * Π}{4}) * 20 = 8.28

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

Como resultado, chegamos ao ponto [R; 0] e a trajetória do corpo do hexápode. Em relação a isso, as trajetórias para cada membro serão calculadas.

Em seguida, é necessário calcular os raios das trajetórias de cada membro em relação ao centro de movimento (ponto [R; 0]), levando em consideração a posição inicial do membro [x0; z0]. Uma linguagem mais compreensível é encontrar o comprimento do vetor desenhado a partir do ponto [R; 0] para o ponto [x0; z0]:

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

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

R_{0} = \sqrt{(8.28 - (- 20))^{2} + 20^{2}} = 34.64

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

R_{1} = \sqrt{(8.28 - (- 35))^{2} + 0^{2}} = 43.28

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

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

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

R_{3} = \sqrt{(8.28 - 20)^{2} + (- 20)^{2}} = 23.17

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

R_{4} = \sqrt{(8.28 - 35)^{2} + 0^{2}} = 26.71

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

R_{5} = \sqrt{(8.28 - 20)^{2} + 20^{2}} = 23.17

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

Imagem para maior clareza. Azul mostra os vetores desejados.

A partir dos valores obtidos, encontramos o máximo:

R_{m a x} = m a x i m u m (R_{0 - 5})

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

R_{m a x} = 43.28

$R_{max} = 43.28$

Em seguida, você precisa encontrar o ângulo para cada vetor que tentamos na etapa anterior.

α_{0 i} = a t a n 2 (z_{0 i}; - (R - x_{0 i}))

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

α_{00} = a t a n 2 (20; - (8.28 - (- 20))) = 2.52 (144.7 °)

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

α_{01} = a t a n 2 (0; - (8.28 - (- 35))) = 3.14 (180 °)

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

α_{02} = a t a n 2 (- 20; - (8.28 - (- 20))) = - 2.52 (- 144.7 °)

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

α_{03} = a t a n 2 (- 20; - (8.28 - 20)) = - 1.04 (- 59.6 °)

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

α_{04} = a t a n 2 (0; - (8.28 - 35)) = 0 (0 °)

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

α_{05} = a t a n 2 (20; - (8.28 - 20)) = 1.04 (59.6 °)

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

Agora encontramos o ângulo do arco no maior círculo de raio R_max (o mais distante do centro de movimento da trajetória), cujo comprimento deve ser igual ao valor da distância. Esse ângulo determina os ângulos inicial e final de outros arcos ao longo dos quais os membros se moverão. Acho que a imagem abaixo ajudará a entender isso.

O ângulo é calculado da seguinte forma:

a r c M a x = s i g n (R) * \frac{d i s t a n c e}{R_{m a x}}

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

a r c M a x = \frac{20}{43.28} = 0.462 (26 °)

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

Além disso, usando esse ângulo, podemos calcular os ângulos inicial e final dos arcos para outros membros. Algo assim deve acontecer:

Uma pequena digressão. Para implementar esse algoritmo, é necessário introduzir o conceito de tempo, cujo valor está no intervalo [0; 1] Também é necessário que cada membro com um valor de tempo 0,5 retorne ao seu ponto de partida. Esta regra é uma verificação da exatidão dos cálculos - todos os círculos devem passar pelos pontos de partida de cada membro.

A seguir, começa o cálculo dos pontos pelos parâmetros obtidos dos arcos, usando as seguintes fórmulas:

a r c A n g l e_{i} = (t i m e - 0.5) * α_{0 i} + a r c M a x

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

x_{i} = R + R_{i} * c o s (a r c A n g l e_{i})

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

z_{i} = R_{i} * s i n (a r c A n g l e_{i})

$z_i = R_i * sin(arcAngle_i)$

Código fonte da função

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;
}

Decidi também mostrar o cálculo da coordenada Y (o cálculo Y aponta por tempo). O cálculo depende da trajetória escolhida, que é definida rigidamente no código e é necessária para realizar o movimento do membro no solo e no ar.

Há também uma peça para implementar a direção inversa do movimento (tempo de movimento de inversão, se necessário). É necessário que durante o movimento de três membros no chão, os outros três membros se movam na direção oposta pelo ar.

Desenvolvendo um hexapod a partir do zero (parte 8) - movimento matemático aprimorado