Bonjour à tous! À la suite de la transition vers le travail à distance, j'ai eu plus de temps libre pour développer un hexapode (+2 heures par jour grâce aux économies réalisées sur la route). J'ai finalement pu créer un algorithme universel pour construire une trajectoire de mouvement en temps réel. Les nouvelles mathématiques ont permis d'implémenter des mouvements de base en ne modifiant que deux paramètres. Il s'agit d'une autre étape vers la mise en œuvre du "pilote automatique". Dans cet article, je vais essayer de parler en détail des nouvelles mathématiques et de leur fonctionnement général. Il y aura de nombreuses photos et gifs.

Stades de développement:

Partie 1 - conception
Partie 2 - assemblage
Partie 3 - cinématique
Partie 4 - mathématiques des trajectoires et séquences
Partie 5 - électronique
Partie 6 - transition vers l'impression 3D
Partie 7 - nouveaux logements, logiciels d'application et protocoles de communication
Partie 8 - mathématiques avancées du mouvement

Dans le temps

Jusqu'à ce point, tous les mouvements de base (avant, arrière, rotation) étaient définis sous la forme de «déplacer la jambe de la position actuelle au point (X, Y, Z) en utilisant un mouvement linéaire / sinusoïdal / tout mouvement». Cela fonctionne assez bien et de manière fiable, mais limite considérablement les mouvements. Par exemple, pour implémenter un mouvement le long d'un arc avec un rayon de R1, vous devez calculer à l'avance les coordonnées du début et de la fin du mouvement pour chaque membre, ajouter un bouton supplémentaire à l'application afin de pouvoir sélectionner ce mouvement. Par conséquent, pour ajouter un mouvement le long d'un arc de rayon R2 différent, vous devez calculer à nouveau les coordonnées et ajouter un autre bouton. Extrêmement inconfortable.

Maintenant, pour la mise en œuvre des mouvements de base, une unité mathématique est utilisée, qui peut être adaptée à diverses situations avec deux paramètres. Le principal avantage des nouvelles mathématiques était le support du mouvement le long d'un arc avec un changement de son rayon juste pendant le mouvement!

Idée de l'algorithme

Tout d'abord, il convient d'expliquer comment cela fonctionne sur les doigts. Que se passe-t-il si vous sélectionnez une fenêtre de taille fixe sur un cercle et commencez à augmenter son rayon? C'est ce que:


Est-ce que tout est juste vrai? En changeant le rayon du cercle, nous pouvons obtenir différentes trajectoires de mouvement et avec une certaine précision une ligne droite. Il était possible de s'arrêter là-dessus, mais tout n'est pas si rose. Vous ne pouvez pas simplement prendre et implémenter cela sur la seule base du rayon - il y a des nuances.

Les positions initiales des membres de l'hexapode peuvent être sur différents cercles, respectivement, les paramètres de l'équation seront déjà différents. Dans mon cas, les membres sont disposés comme suit (environ):


La valeur de la distance que doit parcourir un membre dépend du rayon du cercle sur lequel il se trouve. Cela nous oblige à passer à un calcul individuel de la trajectoire pour chaque membre. Pour résoudre ce problème, vous devez trouver un cercle avec un rayon maximum, calculer les angles de début et de fin de l'arc. Par rapport à l'arc résultant, il y a des arcs pour d'autres membres. J'ai fait une animation de ce morceau d'algorithme:


Il y a une animation pour illustrer le travail de toute cette magie (comme je suis content qu'à notre époque il y ait Excel). Au début, la valeur de la distance que l'hexapode doit parcourir par cycle (similaire à la vitesse) change, puis la valeur de la courbure de la trajectoire (similaire à la rotation du volant) change.


En général, c'est l'idée de nouvelles mathématiques, j'espère qu'elle s'est avérée accessible à expliquer. Vous pouvez maintenant analyser l'algorithme plus en détail et essayer de tout compter à la main dans la pratique.

Mathématiques

Paramètres d'entrée

Les paramètres d'entrée variables sont la distance (distance) et la courbure de la trajectoire de mouvement (courbure). La valeur de la courbure de la trajectoire doit être comprise entre [-1,999; -0,001] et [0,001; 1,999], tandis que la valeur de distance maximale dépend des caractéristiques physiques de l'hexapode. Dans mon cas, la distance maximale par cycle est de 110 mm, avec de grandes valeurs, les membres commencent à se toucher. Pour un exemple de calcul, nous prenons les valeurs courbure = 1,5 et distance = 20.

Aussi, pour que l'algorithme fonctionne, il est nécessaire de connaître les positions initiales des membres. Ce sont les points où se trouvent les membres lorsque l'hexapode est sur ses pieds. Par exemple, nous utiliserons les points suivants (l'origine de chaque membre se situe au point d'attache COXA):


Remarque: je travaille dans le plan XZ et vous pouvez ignorer la coordonnée Y. Vous pouvez vous familiariser avec le système de coordonnées hexapodes dans la troisième partie du cycle.

Par conséquent, nous avons:

Formules et calculs

On commence par calculer les coordonnées du centre de mouvement de l'hexapode en fonction de la valeur de courbure et de la distance:

$$

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

$$

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

En conséquence, nous avons obtenu le point [R; 0] et la trajectoire du corps de l'hexapode. Par rapport à lui, les trajectoires de chaque membre seront calculées.


Ensuite, il est nécessaire de calculer les rayons des trajectoires pour chaque membre par rapport au centre de mouvement (point [R; 0]) en tenant compte de la position initiale du membre [x0; z0]. Un langage plus compréhensible consiste à trouver la longueur du vecteur tiré du point [R; 0] au point [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$$

Image pour plus de clarté. Le bleu montre les vecteurs souhaités.


A partir des valeurs obtenues on trouve le maximum:

$$

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

$$

$$R_{max} = 43.28$$

Ensuite, vous devez trouver l'angle pour chaque vecteur que nous avons essayé à l'étape précédente.

$$

$$\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°)$$

On retrouve maintenant l'angle de l'arc sur le plus grand cercle de rayon R_max (le plus éloigné du centre de mouvement de la trajectoire), dont la longueur doit être égale à la valeur de distance. Cet angle détermine les angles de début et de fin des autres arcs le long desquels les membres se déplaceront. Je pense que l'image ci-dessous aidera à comprendre cela.


L'angle est calculé comme suit:

$$

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

$$

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

En utilisant cet angle, nous pouvons calculer les angles de début et de fin des arcs pour d'autres membres. Quelque chose comme ça devrait se révéler:


Une petite digression. Pour mettre en œuvre cet algorithme, il est nécessaire d'introduire la notion de temps, dont la valeur se situe dans l'intervalle [0; 1]. Il faut aussi que chaque membre d'une valeur de temps 0,5 revienne à son point de départ. Cette règle est une vérification de l'exactitude des calculs - tous les cercles doivent passer par les points de départ de chaque membre.

Ensuite, le calcul des points par les paramètres obtenus des arcs commence, en utilisant les formules suivantes:

$$

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

$$

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

$$

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

Code source de la fonction

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

J'ai alors décidé de montrer également le calcul de la coordonnée Y (Calcul des points Y par le temps). Le calcul dépend de la trajectoire choisie, qui est fixée rigidement dans le code et est nécessaire à la mise en œuvre du mouvement des membres au sol et dans l'air.

Il y a aussi une pièce pour implémenter le sens inverse du mouvement (temps de mouvement d'inversion si besoin). Il est nécessaire que lors du mouvement de trois membres au sol, les trois autres membres se déplacent dans la direction opposée à travers l'air.

résultats