Hallo alle zusammen! Durch den Übergang zur Fernarbeit hatte ich mehr Zeit, um einen Hexapod zu entwickeln (+2 Stunden pro Tag aufgrund von Einsparungen auf der Straße). Endlich konnte ich einen universellen Algorithmus zum Erstellen eines Bewegungspfads in Echtzeit erstellen. Neue Mathematik ermöglichte es, grundlegende Bewegungen zu implementieren, indem nur zwei Parameter geändert wurden. Dies ist ein weiterer Schritt zur Implementierung des "Autopiloten". In diesem Artikel werde ich versuchen, detailliert über die neue Mathematik und ihre allgemeine Funktionsweise zu sprechen. Es wird viele Bilder und Gifs geben.

Entwicklungsstadien:

Teil 1 - Entwurf
Teil 2 - Montage
Teil 3 - Kinematik
Teil 4 - Mathematik der Trajektorien und Sequenzen
Teil 5 - Elektronik
Teil 6 - Übergang zum 3D-Druck
Teil 7 - Neues Gehäuse, Anwendungssoftware und Kommunikationsprotokolle
Teil 8 - Fortgeschrittene Bewegungsmathematik

Zurück in der Zeit

Bis zu diesem Punkt wurden alle Grundbewegungen (vorwärts, rückwärts, Drehung) in Form von „Bewegen des Beins von der aktuellen Position zum Punkt (X, Y, Z) mit linearer / sinusförmiger / beliebiger Bewegung“ eingestellt. Dies funktioniert recht gut und zuverlässig, schränkt jedoch die Bewegung stark ein. Um beispielsweise eine Bewegung entlang eines Bogens mit einem Radius von R1 zu implementieren, müssen Sie die Koordinaten des Beginns und des Endes der Bewegung für jedes Glied im Voraus berechnen und der Anwendung eine zusätzliche Schaltfläche hinzufügen, damit Sie diese Bewegung auswählen können. Um eine Bewegung entlang eines Bogens mit einem anderen Radius R2 hinzuzufügen, müssen Sie die Koordinaten erneut berechnen und eine weitere Schaltfläche hinzufügen. Extrem unangenehm.

Für die Implementierung von Grundbewegungen wird nun eine mathematische Einheit verwendet, die mit zwei Parametern an verschiedene Situationen angepasst werden kann. Der Hauptbonus der neuen Mathematik war die Unterstützung der Bewegung entlang eines Bogens mit einer Änderung des Radius direkt während der Bewegung!

Idee des Algorithmus

Zunächst lohnt es sich zu erklären, wie es an den Fingern funktioniert. Was passiert, wenn Sie ein Fenster mit fester Größe in einem Kreis auswählen und dessen Radius vergrößern? Das ist, was:


Ist alles nur wahr? Durch Ändern des Radius des Kreises können wir verschiedene Bewegungsbahnen und mit einiger Genauigkeit eine gerade Linie erhalten. Es war möglich, damit aufzuhören, aber nicht alles ist so rosig. Sie können dies nicht nur anhand des Radius nehmen und umsetzen - es gibt Nuancen.

Die Anfangspositionen der Gliedmaßen des Hexapods können sich jeweils auf verschiedenen Kreisen befinden, die Parameter der Gleichung sind bereits unterschiedlich. In meinem Fall sind die Gliedmaßen (ungefähr) wie folgt angeordnet:


Der Wert der Entfernung, die ein Glied zurücklegen muss, hängt vom Radius des Kreises ab, auf dem es sich befindet. Dies zwingt uns, für jedes Glied zu einer individuellen Berechnung der Flugbahn zu wechseln. Um dieses Problem zu lösen, müssen Sie einen Kreis mit einem maximalen Radius finden und den Start- und Endwinkel des Bogens berechnen. Weiter relativ zum resultierenden Bogen sind Bögen für andere Gliedmaßen. Ich habe eine Animation dieses Algorithmus erstellt:


Es gibt Animationen, die die Arbeit all dieser Magie veranschaulichen (wie froh ich bin, dass es in unserer Zeit Excel gibt). Zu Beginn der Wert der Strecke, die der Hexapod pro Zyklus zurücklegen muss (ähnlich der Geschwindigkeit), dann der Wert der Krümmung der Flugbahn (ähnlich dem Drehen des Lenkrads).


Im Allgemeinen ist dies die Idee der neuen Mathematik, ich hoffe, sie hat sich als zugänglich erwiesen. Jetzt können Sie den Algorithmus genauer analysieren und versuchen, in der Praxis alles von Hand zu zählen.

Mathematik

Eingabeparameter

Variable Eingabeparameter sind Abstand (Abstand) und Krümmung des Bewegungspfades (Krümmung). Der Wert der Krümmung der Flugbahn sollte in den Bereichen [-1,999; -0,001] und [0,001; 1,999], während der maximale Abstandswert von den physikalischen Eigenschaften des Hexapods abhängt. In meinem Fall beträgt der maximale Abstand pro Zyklus 110 mm, bei großen Werten beginnen die Gliedmaßen aneinander zu stoßen. Für ein Berechnungsbeispiel nehmen wir die Werte Krümmung = 1,5 und Abstand = 20. Damit

der Algorithmus funktioniert, müssen auch die Anfangspositionen der Gliedmaßen bekannt sein. Dies sind die Punkte, an denen sich die Gliedmaßen befinden, wenn der Hexapod auf den Füßen steht. Zum Beispiel werden wir die folgenden Punkte verwenden (der Ursprung jedes Gliedes befindet sich am COXA-Befestigungspunkt):


Hinweis: Ich arbeite in der XZ-Ebene und Sie können die Y-Koordinate ignorieren. Sie können sich im dritten Teil des Zyklus mit dem Hexapod-Koordinatensystem vertraut machen.

Als Ergebnis haben wir Folgendes:

Formeln und Berechnungen

Wir beginnen mit der Berechnung der Koordinaten des Bewegungszentrums des Hexapods in Abhängigkeit vom Krümmungs- und Abstandswert:

$$

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

$$

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

Als Ergebnis haben wir den Punkt [R; 0] und die Flugbahn des Körpers des Hexapods. Relativ dazu werden Trajektorien für jedes Glied berechnet.


Als nächstes müssen die Radien der Trajektorien für jedes Glied relativ zum Bewegungszentrum (Punkt [R; 0]) unter Berücksichtigung der Anfangsposition des Gliedes [x0; z0]. Eine verständlichere Sprache besteht darin, die Länge des vom Punkt [R; 0] bis zum Punkt [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$$

Bild zur Verdeutlichung. Blau zeigt die gewünschten Vektoren.


Aus den erhaltenen Werten ergibt sich das Maximum:

$$

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

$$

$$R_{max} = 43.28$$

Als nächstes müssen Sie den Winkel für jeden Vektor ermitteln, den wir im vorherigen Schritt versucht haben.

$$

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

Nun finden wir den Winkel des Bogens auf dem größten Kreis mit dem Radius R_max (der am weitesten vom Bewegungszentrum entfernten Flugbahn), dessen Länge gleich dem Abstandswert sein sollte. Dieser Winkel bestimmt den Start- und Endwinkel anderer Bögen, entlang derer sich die Gliedmaßen bewegen. Ich denke, das Bild unten wird helfen, dies zu verstehen.


Der Winkel wird wie folgt berechnet:

$$

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

$$

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

Unter Verwendung dieses Winkels können wir den Start- und Endwinkel der Bögen für andere Gliedmaßen berechnen. So etwas sollte sich herausstellen:


Ein kleiner Exkurs. Um diesen Algorithmus zu implementieren, muss das Zeitkonzept eingeführt werden, dessen Wert im Bereich [0; 1]. Es ist auch notwendig, dass jedes Glied mit einem Zeitwert von 0,5 zu seinem Ausgangspunkt zurückkehrt. Diese Regel ist eine Überprüfung der Richtigkeit der Berechnungen - alle Kreise müssen durch die Startpunkte jedes Gliedes verlaufen.

Als nächstes beginnt die Berechnung der Punkte anhand der erhaltenen Parameter der Bögen unter Verwendung der folgenden Formeln:

$$

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

$$

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

$$

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

Funktionsquellcode

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

Dann habe ich beschlossen, auch die Berechnung der Y-Koordinate (Berechnung der Y-Punkte nach Zeit) anzuzeigen. Die Berechnung hängt von der gewählten Flugbahn ab, die im Code fest festgelegt ist und zur Realisierung der Bewegung der Extremität am Boden und in der Luft erforderlich ist.

Es gibt auch ein Stück zum Implementieren der umgekehrten Bewegungsrichtung (Inversionsbewegungszeit, falls erforderlich). Während der Bewegung von drei Gliedmaßen am Boden bewegen sich die anderen drei Gliedmaßen in die entgegengesetzte Richtung durch die Luft.

Ergebnisse