Kinematik

Kinematik ist die Geometrie der Bewegung, ohne Kräfte. Die Vorwärtskinematik bildet Gelenkwinkel auf die Fußposition ab. Die inverse Kinematik bildet eine Fußposition zurück auf Gelenkwinkel ab. Die Jacobi-Matrix verknüpft Gelenkgeschwindigkeiten mit Fußgeschwindigkeiten. Diese Seite leitet alle drei für das Strido-Bein her, einschließlich seines Kniekoppelgetriebes.

In Strido

• Beinmodell: packages/strido-core/src/strido/leg.py (foot_position, inverse_kinematics, calf_from_crank, crank_from_calf, transmission_angles), in C++ gespiegelt in firmware/strido_esp32s3/lib/strido_core/.
• Maße: hardware/kinematics.yaml (legs_linkage, legs_base), aus dem CAD gemessen. Grenzen: hardware/joint_limits.yaml.
• Praxisleitfaden: Kinematik und Gaits zeigt, wie der Gait diese Funktionen nutzt.

Das Bein als kinematische Kette

Jedes Bein hat drei Drehgelenke (Docstring von leg.py; die Konventionen entsprechen dem Unitree Go2):

Gelenk	Achse	Null	Positiv
Hüfte $q_1$	$+x$	Bein senkrecht	Fuß Richtung $+y$
Oberschenkel $q_2$	$+y$	Femur gerade nach unten	Knie nach hinten
Unterschenkel $q_3$	$+y$, relativ zum Femur	Unterschenkel in Linie mit Femur	negativ = Knie gebeugt

Aus hardware/kinematics.yaml (Bein vorne links, gerundet): Die Hüfte liegt bei $\mathbf{h}=(0.1197,0.039,0)\mathrm{m}$ in base_link, die Beinebene liegt $d=38.3\mathrm{mm}$ außerhalb der Hüfte ($40.2\mathrm{mm}$ bei den Hinterbeinen), der Oberschenkel misst $l_1=130.0\mathrm{mm}$ und der Unterschenkel $l_2=124.1\mathrm{mm}$ bis zur Fußmitte.

Oberschenkel und Unterschenkel bewegen sich in einer senkrechten Beinebene. Das Hüftgelenk kippt diese Ebene um $x$. Das Problem zerfällt also in zwei einfachere: einen ebenen Zweigelenkarm und eine Drehung.

Vorwärtskinematik

In der Beinebene zeige $u$ nach vorne und $v$ nach oben, beide vom Oberschenkel-Drehpunkt aus gemessen. Schreiben Sie $s_2=\sin q_2$ und $s_{23}=\sin (q_2+q_3)$, analog für Kosinus. Knie und Fuß liegen bei:

$$\mathbf{k}=\left[\begin{array}{c}-l_1{s}_2\\ -l_1{c}_2\end{array}\right],\left[\begin{array}{c}u\\ v\end{array}\right]=\left[\begin{array}{c}-l_1{s}_2-l_2{s}_{23}\\ -l_1{c}_2-l_2{c}_{23}\end{array}\right]$$

Die Winkel addieren sich, weil $q_3$ relativ zum Femur gemessen wird; der absolute Winkel des Unterschenkels ist also $q_2+q_3$. Die Vorzeichen sind negativ, weil Null „nach unten zeigend“ bedeutet und ein positives $q_2$ den Fuß nach hinten schwingt (Richtung $-u$).

Als Nächstes die Hüftdrehung. Der Fuß im Hüft-Koordinatensystem vor der Abduktion ist $(u,d,v)$. Drehung um $x$ um $q_1$ und Addition der Hüftposition ergibt:

$$\mathbf{p}=\mathbf{h}+{\mathbf{R}}_x(q_1)\left[\begin{array}{c}u\\ d\\ v\end{array}\right]=\mathbf{h}+\left[\begin{array}{c}u\\ d\cos q_1-v\sin q_1\\ d\sin q_1+v\cos q_1\end{array}\right]$$

Das ist foot_position Zeile für Zeile. Zu den Konventionen der Koordinatensysteme siehe Koordinatensysteme und Rotationen.

Inverse Kinematik

Gegeben ist ein Fußziel $\mathbf{p}$, gesucht ist $(q_1,q_2,q_3)$. Sei $\mathbf{r}=\mathbf{p}-\mathbf{h}=(r_x,r_y,r_z)$.

Hüftabduktion

Eine Drehung um $x$ lässt die $x$-Komponente unverändert und erhält die Länge in der $y$–$z$-Ebene:

$$r_y^2+r_z^2=d^2+v^2$$

Der Fuß liegt unter der Hüfte, also nehmen wir die negative Wurzel:

$$v=-\sqrt{r_y^2+r_z^2-d^2}$$

Ist der Term unter der Wurzel nicht positiv, liegt das Ziel innerhalb des Zylinders mit Radius $d$ um die Hüftachse und ist nicht erreichbar. Die Richtung von $(r_y,r_z)$ ist die um $q_1$ gedrehte Richtung von $(d,v)$, also:

$$q_1=\mathrm{atan}2(r_z,r_y)-\mathrm{atan}2(v,d)$$

Das ebene Zweigelenkproblem: Kosinussatz

Setzen Sie nun $u=r_x$ und lösen Sie den Zweigelenkarm. Oberschenkel, Unterschenkel und die Linie vom Drehpunkt zum Fuß bilden ein Dreieck mit den Seiten $l_1$, $l_2$ und $\rho =\sqrt{u^2+v^2}$. Der Kosinussatz am Knie liefert den Kniewinkel. Mit unserer Konvention (der Knie-Innenwinkel ist $\pi +q_3$):

$${\rho }^2=l_1^2+l_2^2+2l_1{l}_2\cos q_3⟹\cos q_3=\frac{u^2+v^2-l_1^2-l_2^2}{2l_1{l}_2}$$

Es gibt zwei Lösungen, $\pm \arccos$, für Knie nach vorne und Knie nach hinten. Strido verwendet $q_3=-\arccos (\cdot )$, ein gebeugtes Knie, das nach hinten zeigt. Ist $|\cos q_3|>1$, liegt das Ziel außer Reichweite.

Der Oberschenkelwinkel ist die Richtung zum Fuß minus den Winkel, den der Unterschenkel im Dreieck hinzufügt:

$$q_2=\mathrm{atan}2(-u,-v)-\mathrm{atan}2(l_2\sin q_3,l_1+l_2\cos q_3)$$

Die Minuszeichen im ersten Term stammen aus der Konvention „Null = gerade nach unten, positiv = nach hinten“. Verwenden Sie atan2, nie atan, damit der Quadrant stimmt.

Das Knie-Viergelenk

Der Knieservo treibt $q_3$ nicht direkt an. Ein Parallelogramm (Arm $24.0\mathrm{mm}$, kurze Koppel $28.3\mathrm{mm}$, Kurbel $24.0\mathrm{mm}$) überträgt den Servowinkel auf eine Kurbel $\psi$ am Oberschenkel-Drehpunkt. Ein Viergelenk (Kurbel $26.9\mathrm{mm}$, lange Koppel $131\mathrm{mm}$, Tibiakurbel $24.0\mathrm{mm}$ und der Femur $130.0\mathrm{mm}$ als Steg) dreht dann die Tibia. Weil der Steg des Viergelenks der Femur selbst ist:

$$q_3=f(q_2,\psi )$$

Bewegt man nur den Oberschenkel, bewegt sich auch das Knie. crank_from_calf invertiert das geometrisch. Das freie Ende der Kurbel liegt auf einem Kreis mit Radius $26.9\mathrm{mm}$ um den Drehpunkt und zugleich auf einem Kreis mit Radius $131\mathrm{mm}$ (die lange Koppel) um den Tibia-Bolzen. Der Schnitt der beiden Kreise liefert zwei Kandidaten, und der Montagezweig wählt einen aus. circle_intersection gibt None zurück, wenn sich die Kreise nicht treffen. Dann lässt sich das Koppelgetriebe in dieser Pose nicht zusammensetzen.

Diagramm wird gezeichnet…

Die Jacobi-Matrix

Die Jacobi-Matrix bildet Gelenkgeschwindigkeiten auf die Fußgeschwindigkeit ab, $\dot{\mathbf{p}}=\mathbf{J}(\mathbf{q})\dot{\mathbf{q}}$. Ableiten der ebenen Vorwärtskinematik ergibt:

$${\mathbf{J}}_{\text{plane}}=\frac{\partial (u,v)}{\partial (q_2,q_3)}=\left[\begin{array}{cc}-l_1{c}_2-l_2{c}_{23}& -l_2{c}_{23}\\ \phantom{-}{l}_1{s}_2+l_2{s}_{23}& \phantom{-}{l}_2{s}_{23}\end{array}\right],det{\mathbf{J}}_{\text{plane}}=-l_1{l}_2\sin q_3$$

Die vollständige $3\times 3$-Jacobi-Matrix ergänzt die Hüftspalte, $\partial \mathbf{p}/\partial q_1=\hat{\mathbf{x}}\times (\mathbf{p}-\mathbf{h})$, und dreht die ebenen Spalten mit ${\mathbf{R}}_x(q_1)$. Die Jacobi-Matrix bestimmt auch die Kräfte über $\mathit{\tau }={\mathbf{J}}^{\mathrm{\top }}\mathbf{F}$ (siehe Statik und Dynamik).

Für den Antrieb fügt die Kettenregel einen weiteren Faktor hinzu. Die Geschwindigkeit des Knieservos ist $\dot{\psi }$, und

$${\dot{q}}_3=\frac{\partial f}{\partial q_2}{\dot{q}}_2+\frac{\partial f}{\partial \psi }\dot{\psi }$$

Arbeitsraum und Singularitäten

Ohne Gelenkgrenzen erreicht der ebene Fuß einen Kreisring:

$$|l_1-l_2|\le \rho \le l_1+l_2\Rightarrow 5.9\mathrm{mm}\le \rho \le 254.1\mathrm{mm}$$

Gelenkgrenzen und Eigenkollisionen schränken das ein. hardware/joint_limits.yaml begrenzt $q_3$ auf $[-{150}^{\circ },-{45}^{\circ }]$ und das vordere $q_2$ auf $[-{45}^{\circ },{125}^{\circ }]$, ermittelt durch Abfahren des gestellten CAD auf Kollisionen in einem $5^{\circ }$-Raster.

Eine Singularität ist eine Pose mit $det\mathbf{J}=0$. Dort kann sich der Fuß in eine Richtung nicht bewegen, egal wie schnell sich die Gelenke drehen, und die inverse Kinematik wird schlecht konditioniert. Beim ebenen Bein geschieht das bei $\sin q_3=0$: Bein vollständig gestreckt ($q_3=0$, am äußeren Rand des Arbeitsraums) oder vollständig eingeklappt. Die Unterschenkelgrenze hält $|\sin q_3|\ge \sin {30}^{\circ }=0.5$ (am Ende bei $-{150}^{\circ }$; mindestens $0.71$ am Ende bei $-{45}^{\circ }$), sodass das Bein nie in die Nähe kommt. Das ist ein Grund, warum die Standpose das Knie deutlich gebeugt hält.

Das Viergelenk hat eigene Singularitäten. Wenn die lange Koppel mit der Kurbel oder mit der Tibiakurbel fluchtet (Übertragungswinkel nahe $0$ oder ${180}^{\circ }$), wird $\partial f/\partial \psi$ unendlich oder verschwindet. transmission_angles misst beide Winkel, und joint_limits.yaml verlangt mindestens ${25}^{\circ }$ (transmission_min_deg).

Durchgerechnetes Beispiel: die Standpose

Die Standpose setzt jeden Fuß senkrecht unter seine Hüfte, $0.17\mathrm{m}$ tiefer (stand_pose(height=0.17) in strido/gait.py). Für das Bein vorne links gilt $\mathbf{r}=(0,0.0383,-0.170)\mathrm{m}$.

Hüfte. $v=-\sqrt{{0.0383}^2+{0.170}^2-{0.0383}^2}=-0.170$, also $q_1=\mathrm{atan}2(-0.170,0.0383)-\mathrm{atan}2(-0.170,0.0383)=0$.

Knie. Mit $u=0$:

$$\cos q_3=\frac{{0.170}^2-{0.1300}^2-{0.1241}^2}{2\cdot 0.1300\cdot 0.1241}=\frac{-0.00339}{0.03226}=-0.105\Rightarrow q_3=-{96.0}^{\circ }$$

Oberschenkel. $l_2\sin q_3=-0.1234$ und $l_1+l_2\cos q_3=0.1170$, also

$$q_2=\mathrm{atan}2(0,0.170)-\mathrm{atan}2(-0.1234,0.1170)=0+{46.5}^{\circ }={46.5}^{\circ }$$

Das stimmt mit stand_pose() überein, das für jedes Bein $(0^{\circ },{46.53}^{\circ },-{96.03}^{\circ })$ liefert. Der absolute Winkel des Unterschenkels ist $q_2+q_3=-{49.5}^{\circ }$: Das Knie liegt hinter und über dem Fuß, und der Unterschenkel neigt sich nach vorne, um ihn zu erreichen.

Jacobi-Matrix. In dieser Pose gilt

$${\mathbf{J}}_{\text{plane}}=\left[\begin{array}{cc}-0.170& -0.0806\\ 0& -0.0943\end{array}\right]\mathrm{m}/\mathrm{rad},det=0.0160{\mathrm{m}}^2$$

Eine Oberschenkelgeschwindigkeit von $1\mathrm{rad}/\mathrm{s}$ bewegt den Fuß mit $0.17\mathrm{m}/\mathrm{s}$ nach hinten und überhaupt nicht senkrecht. Um den Fuß anzuheben, braucht man vor allem das Knie.

Koppelgetriebe. leg.py liefert einen Kurbelwinkel von $\psi ={38.9}^{\circ }$ und Übertragungswinkel von ${84}^{\circ }$ und ${86}^{\circ }$, nahe am Ideal von ${90}^{\circ }$. Die Empfindlichkeit ist $\partial q_3/\partial \psi =-1.12$: Das Knie dreht sich um ${1.12}^{\circ }$ pro Grad Kurbel.

Selbsttest

1. Warum lässt sich der Hüftwinkel vor und getrennt von Oberschenkel und Knie lösen?
2. Was bedeutet $|\cos q_3|>1$ physikalisch?
3. Welches Gelenk bewegt den Fuß in der Standpose hauptsächlich nach vorne, und welches hauptsächlich senkrecht?
4. Warum sind die Übertragungswinkel wichtig, selbst wenn $det{\mathbf{J}}_{\text{plane}}$ weit von null entfernt ist?

Antworten

1. Die Hüftachse ist die $x$-Achse, und $q_2$, $q_3$ ändern den Abstand des Fußes von ihr nie. Daher hängt $r_y^2+r_z^2$ nur von der ebenen Tiefe $v$ und dem festen Versatz $d$ ab.
2. Das Ziel ist weiter entfernt als $l_1+l_2$ oder näher als $|l_1-l_2|$, also existiert kein Dreieck.
3. Der Oberschenkel bewegt ihn nach vorne und hinten ($J_{11}=-0.170$, $J_{21}=0$). Nur die Kniespalte hat eine senkrechte Komponente.
4. Sie beschreiben das Antriebs-Koppelgetriebe, nicht das Bein. Nahe $0^{\circ }$ oder ${180}^{\circ }$ verliert der Knieservo Hebelwirkung und die Abbildung Kurbel–Knie wird schlecht konditioniert, egal was die Bein-Jacobi-Matrix sagt.

Weiterführende Literatur

• K. M. Lynch und F. C. Park, Modern Robotics, Cambridge University Press, 2017, Kap. 4–6 (Vorwärtskinematik, Geschwindigkeitskinematik, inverse Kinematik).
• B. Siciliano et al., Robotics: Modelling, Planning and Control, Springer, 2009, Kap. 2–3.
• R. L. Norton, Design of Machinery, McGraw-Hill, Kap. 4 (Lageanalyse von Viergelenken und Übertragungswinkel).
• Kinematik und Gaits in diesem Wiki.