Kinematik und Gaits

Kinematik ist die Geometrie der Bewegung: Wo ist der Fuß bei gegebenen Gelenkwinkeln (Vorwärtskinematik), und welche Gelenkwinkel bringen ihn an eine gegebene Fußposition (inverse Kinematik)? Ein Gait ist der Rhythmus, in dem die vier Füße abheben, schwingen und abstoßen. Diese Seite arbeitet beides für das Strido-Bein durch, mit dem Code, der den Roboter tatsächlich steuert.

In Strido

Beinkinematik und Kniekoppelgetriebe: packages/strido-core/src/strido/leg.py (Python) und sein C++-Gegenstück firmware/strido_esp32s3/lib/strido_core/src/kinematics.cpp + servo_map.cpp.
Gait: packages/strido-core/src/strido/gait.py (TrotGait, GaitConfig, stand_pose, clamp_leg), unverändert verwendet von gait_node (ROS 2), strido_sim.walk (MuJoCo) und als Standpose für die gelernte Policy.
Maße und Grenzen: aus dem CAD in hardware/kinematics.yaml und hardware/joint_limits.yaml gemessen, dann in packages/strido-core/src/strido/geometry.py und geometry_generated.h generiert (CAD als Code).
Servos: 12 × SPT5435LV-180W Positionsservos (Servos).

Das Bein

Jedes Bein hat drei Gelenke, benannt und mit Vorzeichen wie beim Unitree Go2, damit Werkzeuge und Policies übertragbar sind:

Gelenk	Achse	Null	Positive Richtung	Angetrieben von
Hüfte (q1)	+x (vorwärts)	Bein senkrecht	Fuß bewegt sich nach +y (links)	Hüftservo, direkt
Oberschenkel (q2)	+y	Femur gerade nach unten	Knie bewegt sich nach hinten	Oberschenkelservo, direkt (koaxial zum Femur-Drehpunkt)
Unterschenkel (q3)	+y, relativ zum Femur	Unterschenkel in Linie mit Femur	negativ = Knie gebeugt	Knieservo, über zwei Koppelgetriebe

Gemessene Maße (Bein vorne links, generierte Werte gerundet):

Größe	Wert
Position der Hüftachse in `base_link`	x = ±0,120 m, y = ±0,039 m
Seitlicher Versatz Hüftachse → Beinebene (`thigh_offset_y`)	0,038 m vorne, 0,040 m hinten
Länge Oberschenkel (Femur) l1	0,130 m
Länge Unterschenkel (Tibia) l2, bis Fußmitte	0,124 m
Radius der Fußkugel	0,012 m

Die volle Streckung beträgt $l_{1} + l_{2} \approx 0.254 m$ ; der Roboter steht mit den Hüftachsen 0,17 m über den Fußmitten.

Das Kniekoppelgetriebe

Der Knieservo sitzt nicht am Knie, sondern oben im Beinmodul. Er dreht einen Arm; ein Parallelogramm (Arm 24 mm, kurze Koppel 28,3 mm, Kurbel) überträgt den Armwinkel auf eine Kurbel koaxial zum Femur-Drehpunkt; ein Viergelenk (Kurbel 26,9 mm, lange Koppel 131 mm, Tibiakurbel 24 mm, Femur) dreht die Tibia. Weil die Servomasse nahe am Körper bleibt, lässt sich das Bein leichter schwingen.

Der Preis: Der Kniewinkel $q_{3}$ hängt sowohl vom Kurbelwinkel $ψ$ als auch vom Oberschenkelwinkel $q_{2}$ ab. Bewegt man nur den Oberschenkelservo, ändert sich auch der Kniewinkel. Die Software muss daher das Koppelgetriebe lösen:

crank_from_calf(leg, q2, q3) liefert den Kurbelwinkel, den der Knieservo halten muss (inverses Koppelgetriebe), durch Schnitt zweier Kreise: einer um den Femur-Drehpunkt mit dem Kurbelradius, einer um den Tibia-Bolzen mit der Länge der langen Koppel.
calf_from_crank(leg, q2, ψ) ist das Vorwärts-Koppelgetriebe.

Zwei Kreise schneiden sich in zwei Punkten; der Code wählt den Zweig, in dem sich die CAD-Baugruppe befindet (_branches), damit der Löser nie in die gespiegelte Konfiguration umspringt.

Der Übertragungswinkel ist der Winkel zwischen der langen Koppel und der Kurbel (bzw. Tibiakurbel). Nahe 0° oder 180° ist das Koppelgetriebe nahe einer Singularität: Eine kleine Servobewegung erzeugt eine riesige Kniebewegung, und der Servo verliert Hebelwirkung. Die Bewegungsbereichs-Analyse hält ihn bei mindestens 25°; in der Standpose beträgt er etwa 84–86°, nahe am Ideal von 90°.

Vorwärtskinematik

foot_position(name, q) in leg.py:

In der Beinebene ( $u$ vorwärts, $v$ nach oben, Ursprung im Oberschenkel-Drehpunkt):
$p_{knee} = (\begin{matrix} - l_{1} \sin q_{2} \\ - l_{1} \cos q_{2} \end{matrix}), p_{foot} = p_{knee} + (\begin{matrix} - l_{2} \sin (q_{2} + q_{3}) \\ - l_{2} \cos (q_{2} + q_{3}) \end{matrix})$
Der absolute Winkel des Unterschenkels ist $q_{2} + q_{3}$ , weil $q_{3}$ relativ zum Femur gemessen wird: Die Winkel einer seriellen Kette addieren sich.
Setzen Sie den Ebenenpunkt mit dem seitlichen Versatz thigh_offset_y an, drehen Sie ihn um den Hüftwinkel $q_{1}$ um $x$ und addieren Sie dann die Hüftposition in base_link: $p = p_{hip} + R_{x} (q_{1}) (u, d_{y}, v)^{⊤}$ . Die allgemeine Methode (homogene Transformationen) steht in Kinematik und Koordinatensysteme und Rotationen.

Beispiel aus dem Code: Die Standpose für das Bein vorne links ist $q = (0, 0.812, - 1.676) rad = (0^{\circ}, {46.5}^{\circ}, - {96.0}^{\circ})$ , und die Vorwärtskinematik liefert den Fuß bei $(0.120, 0.077, - 0.170) m$ : direkt unter der Hüfte, 0,170 m tiefer. Prüfen Sie die Höhe in der Ebene: $v = - 0.130 \cos (0.812) - 0.124 \cos (0.812 - 1.676) \approx - 0.089 - 0.081 = - 0.170 m$ . Der $y$ -Wert 0,077 m setzt sich aus den 0,039 m der Hüfte plus dem Beinebenen-Versatz von 0,038 m zusammen.

Inverse Kinematik

inverse_kinematics(name, foot) löst die Umkehrung analytisch (ohne Iteration):

Hüfte. Betrachten Sie das Bein von vorne. Der Fuß muss auf einem Kreis mit Radius $d_{y}$ (thigh_offset_y) um die Hüftachse liegen, plus einer „Tiefe“ $d$ entlang des Beins, nach Pythagoras: $d = \sqrt{y^{2} + z^{2} - d_{y}^{2}}$ . Der Hüftwinkel ist die Drehung, die die Beinebene auf den Fuß ausrichtet: $q_{1} = atan 2 (z, y) - atan 2 (- d, d_{y})$ .
Knie aus dem Kosinussatz. In der Beinebene liegt der Fuß bei $(u, v) = (x, - d)$ . Das Dreieck Oberschenkel–Unterschenkel–(Drehpunkt zu Fuß) hat die Seiten $l_{1}$ , $l_{2}$ und $r = \sqrt{u^{2} + v^{2}}$ , also $r^{2} = l_{1}^{2} + l_{2}^{2} + 2 l_{1} l_{2} \cos q_{3}$ , woraus folgt
$\cos q_{3} = \frac{u^{2} + v^{2} - l_{1}^{2} - l_{2}^{2}}{2 l_{1} l_{2}}, q_{3} = - \arccos (\cos q_{3})$
(negativ: Das Knie beugt sich nach hinten). Ist $| \cos q_{3} | > 1$ , liegt das Ziel außer Reichweite und die Funktion gibt None zurück.
Oberschenkel. Die Richtung zum Fuß minus den Winkel, den der gebeugte Unterschenkel hinzufügt:
$q_{2} = atan 2 (- u, - v) - atan 2 (l_{2} \sin q_{3}, l_{1} + l_{2} \cos q_{3})$

Danach wendet clamp_leg die Gelenkgrenzen und das Kniekurbelfenster an. Liegt die benötigte Kurbelstellung außerhalb ihres Fensters, wird sie begrenzt und der Unterschenkelwinkel aus der begrenzten Kurbel neu berechnet: Die Firmware tut im Aktorraum genau dasselbe, sodass Simulation und Roboter übereinstimmen.

Gelenkgrenzen

Generiert aus der CAD-Bewegungsbereichs-Analyse (Kollisionen mit allen Teilen, ein gespiegeltes Partnerbein mit 0,3 mm Abstand, Übertragungswinkel ≥ 25° und ±85° Servoweg um die Montage-Neutralstellung):

Gelenk	FL	FR	RL	RR
Hüfte	−17,5…50°	−50…17,5°	−17,5…45°	−45…17,5°
Oberschenkel	−40…125°	−40…125°	−15…125°	−15…125°
Unterschenkel	−150…−45°	−150…−45°	−150…−45°	−150…−45°
Kniekurbel	−2,5…75°	−2,5…75°	−2,5…75°	−2,5…75°

Die inneren Hüftgrenzen (±17,5°) sind klein, weil sich sonst beide Vorderfüße (und beide Hinterfüße) unter dem Körper treffen könnten (ADR-005 in Entscheidungen). Die Analyse erlaubte für den vorderen Oberschenkel −45°, doch der Servo erreicht innerhalb von ±85° um seine 45°-Neutralstellung nur −40°. Die Grenzen werden an drei Stellen (Gait, Policy-Umgebung, Firmware) aus denselben generierten Zahlen angewendet.

Von Winkeln zu Servopulsen

Gelenkraum und Servoraum unterscheiden sich nur beim Knie: Hüft- und Oberschenkelservo halten $q_{1}$ und $q_{2}$ direkt, der Knieservo hält den Kurbelwinkel $ψ$ . Die Firmware bildet dann jeden Aktorwinkel $θ$ auf eine Pulsbreite $t_{p}$ ab, mit Kalibriervorzeichen $s$ (direction), neutral_us $t_{neutral}$ und Referenzwinkel $θ_{0}$ (zero_rad):

t_{p} = t_{neutral} + s \cdot \frac{2000 μ s}{π rad} \cdot (θ - θ_{0})

mit gelenkweiser Kalibrierung aus dem Flash (Firmware). In der Montagereferenz (alle Servos bei 1500 µs) stehen die Beine in der Abduktion senkrecht, der Femur ist 45° nach hinten geneigt und die Kniekurbel steht bei 35°.

Gaits: Stand, Schwung und der Gangzyklus

Ein Gait wiederholt sich mit einer Periode $T$ . Jedes Bein verbringt einen Teil davon am Boden (Standphase, schiebt den Körper) und den Rest in der Luft (Schwungphase, bewegt sich zum nächsten Auftrittspunkt vor). Der Standanteil ist der Belastungsfaktor $β = T_{stance} / T$ . Phasenversätze geben an, wann jedes Bein seinen Zyklus beginnt. Die Theorie zu Gaits, Stützpolygonen und Raibert-Fußplatzierung steht in Laufende Fortbewegung.

Gait	Belastungsfaktor	Phasenmuster	Charakter
Schritt	≥ 0,75	ein Bein nach dem anderen (z. B. FL, RR, FR, RL)	mindestens drei Füße am Boden: statisch stabil, langsam
Trot	0,5	diagonale Paare gemeinsam (FL+RR, dann FR+RL)	zwei Füße am Boden: braucht Schwung und eine kurze Periode; robust und einfach

Statische Stabilität: Ein stillstehender Roboter fällt nicht um, wenn sein Schwerpunkt innerhalb des Stützpolygons liegt, das die Füße am Boden bilden. Mit drei Füßen am Boden (Schritt) lässt sich dieses Dreieck unter dem Körper halten. Mit zwei diagonalen Füßen (Trot) ist das Polygon eine Linie, sodass der Körper in Rollen und Nicken kurz ungestützt ist; ein Zyklus von 0,4 s wechselt die Paare, bevor er merklich kippt.

Strido implementiert nur einen Trot (TrotGait, PHASE_OFFSET = {"FL": 0.0, "RR": 0.0, "FR": 0.5, "RL": 0.5}). Ein Schritt bräuchte eine neue Phasentabelle und einen Belastungsfaktor ≥ 0,75; nichts im Repository stellt das bereit.

Der Trot in Zahlen

Standardwerte von GaitConfig:

Parameter	Wert
`body_height`	0.17 m
`period`	0,40 s (2,5 Schritte pro Sekunde je Beinpaar)
`duty`	0,5 (0,2 s Stand, 0,2 s Schwung)
`step_height`	0,035 m Scheitel der Schwungphase
`max_linear`	0,30 m/s vorwärts, 0,15 m/s seitwärts
`max_yaw_rate`	1,0 rad/s

Wie foot_targets() jeden Fuß platziert:

Geschwindigkeit pro Bein. Für den Körperbefehl $(v_{x}, v_{y}, ω_{z})$ muss sich ein Fuß bei $(x, y)$ mit $v_{foot} = (v_{x} - ω_{z} y, v_{y} + ω_{z} x)$ bewegen, der ebenen Form von $v + ω \times r$ : Beim Drehen auf der Stelle bewegen sich die linken Füße nach hinten und die rechten nach vorne.
Schrittlänge = Geschwindigkeit × Standzeit, $L = v β T$ . Bei 0,15 m/s: $0.15 m / s \times 0.2 s = 0.03 m$ . Am Limit von 0,30 m/s: 0,06 m.
Standphase (Phase $< β$ ): Der Fuß gleitet relativ zu seiner Nennposition mit Körpergeschwindigkeit von $+ L / 2$ nach $- L / 2$ , sodass sich der Körper über ihn hinweg nach vorne bewegt.
Schwungphase: Der Fuß kehrt von $- L / 2$ nach $+ L / 2$ zurück und hebt sich auf einer Halbsinuskurve, $h (s) = 0.035 m \cdot \sin (π s)$ mit Schwungfortschritt $s \in [0, 1]$ . Eine halbe Schrittlänge vor der Hüfte aufzusetzen ist die Fußplatzierung nach Raibert: Der Aufsetzpunkt liegt symmetrisch zur Hüfte, sodass die Standphase den Körper im Mittel weder beschleunigt noch bremst.
Inverse Kinematik wandelt die vier Fußziele in 12 Winkel um, danach wendet clamp_leg die Grenzen an.

Bei einem Nullbefehl bleiben die Füße an ihren Nennpositionen: Der Roboter steht still, statt auf der Stelle zu traben.

gait_node ergänzt eine Rampe (Geschwindigkeitsänderungen höchstens 0,5 m/s² und 0,5 rad/s²) und ein sanftes Aufstehen in 3 s von der Ruhepose (0,07 m: Oberschenkel 69,4°, Unterschenkel −148,1°) in die Standpose.

Dieser Gait ist gesteuert (Open Loop): Sie liest die IMU nicht und nimmt keine Balancekorrekturen vor. Sie funktioniert, weil der Trot eine kurze Periode hat und der Roboter leicht ist (3,01 kg). Die gelernte Policy (Simulation und RL) ist die geregelte Alternative: Sie beobachtet die IMU.

PD-Regelung: Was ein Gelenk an seinem Ziel hält

Ein Positionsservo ist ein kleiner Regelkreis. Das übliche Modell ist ein PD-Gesetz (proportional-differenzierend):

τ = k_{p} (q_{target} - q) - k_{d} \dot{q}

Der P-Anteil zieht wie eine Feder zum Ziel; der D-Anteil dämpft die Bewegung wie ein Stoßdämpfer. Ein großes $k_{p}$ ergibt steife, genaue Gelenke, kann aber schwingen; $k_{d}$ unterdrückt die Schwingung. Für ein Gelenk mit Trägheit $I$ ist der geschlossene Kreis ein Masse-Feder-Dämpfer-System, $I \ddot{q} + k_{d} \dot{q} + k_{p} q = k_{p} q_{target}$ , mit Eigenfrequenz $ω_{n} = \sqrt{k_{p} / I}$ und Dämpfungsgrad $ζ = k_{d} / (2 \sqrt{k_{p} I})$ ; siehe Regelung.

In Strido:

Am Roboter sitzt der PD-Regelkreis in jedem SPT5435LV: Seine Elektronik vergleicht den befohlenen Puls mit einem internen Potentiometer und treibt den Motor an. Der ESP32 sendet nur Sollwerte; außerhalb des Servos kennt nichts den tatsächlichen Winkel oder das Drehmoment.
In MuJoCo ist jeder Servo ein Positionsaktor mit $k_{p} = 25 N m / rad$ , $k_{v} = 0.4 N m s / rad$ und einer Drehmomentgrenze von $\pm 3.43 N m$ , dem Blockiermoment des SPT5435LV bei 6 V. 0,1 rad Fehler ergeben also $25 \times 0.1 = 2.5 N m$ ; jenseits von $3.43 / 25 \approx 0.14 rad$ geht der Servo in die Sättigung, wie ein echter es täte.
In Gazebo stellt gz_ros2_control pro Gelenk eine position-Befehlsschnittstelle bereit, angesteuert von leg_controller (position_controllers/JointGroupPositionController, 200 Hz).

Diese Verstärkungen sind Modellschätzungen; Simulation merkt an, dass sie an das gemessene Servoverhalten angepasst werden sollten, sobald ein Roboter existiert.

Selbsttest

Warum ändert sich der Kniewinkel, wenn man nur den Oberschenkelservo bewegt?
Was ergibt der Kosinussatz für $\cos q_{3}$ bei einem Fuß in voller Streckung (Abstand $l_{1} + l_{2}$ )?
Wie groß ist die Schrittlänge bei 0,20 m/s mit dem Standard-Trot?
Warum ist ein Trot nicht statisch stabil, und warum funktioniert er trotzdem?
Welches Drehmoment erzeugt ein Gelenk in MuJoCo bei 0,05 rad Fehler und Geschwindigkeit null?

Antworten

Der Knieservo treibt die Tibia über ein Parallelogramm und ein Viergelenk an, die am Femur befestigt sind; der Kniewinkel hängt vom Kurbelwinkel und vom Oberschenkelwinkel ab.
$(l_{1} + l_{2})^{2} - l_{1}^{2} - l_{2}^{2} = 2 l_{1} l_{2}$ , also $\cos q_{3} = 1$ und $q_{3} = 0$ : Das Bein ist gestreckt (und die Unterschenkelgrenze $- 45^{\circ}$ verbietet das).
$0.20 m / s \times 0.2 s = 0.04 m$ .
Nur zwei diagonale Füße stützen den Körper, daher liegt der Schwerpunkt nicht innerhalb eines Stützpolygons; die kurze Periode von 0,4 s wechselt die Paare, bevor der Körper weit kippt.
$25 N m / rad \times 0.05 rad = 1.25 N m$ (unter der Grenze von 3,43 N·m).

Kinematik und Gaits ​

Das Bein ​

Das Kniekoppelgetriebe ​

Vorwärtskinematik ​

Inverse Kinematik ​

Gelenkgrenzen ​

Von Winkeln zu Servopulsen ​

Gaits: Stand, Schwung und der Gangzyklus ​

Der Trot in Zahlen ​

PD-Regelung: Was ein Gelenk an seinem Ziel hält ​

Selbsttest ​

Weiterführende Literatur ​