Koordinatensysteme und Rotationen

Jede Zahl, die ein Roboter für Position oder Orientierung verwendet, bezieht sich auf ein Koordinatensystem (Frame). Diese Seite behandelt Frames, Rotationsmatrizen, Roll-Nick-Gier-Winkel und Gimbal Lock, Quaternionen und homogene Transformationen und zeigt dann, wie sie im Transformationsbaum (tf) von Strido zusammenspielen.

In Strido

• Basis-Frame: REP-103 (x vorwärts, y links, z oben), mit dem Ursprung im Mittelpunkt der vier Schnittpunkte der Hüftachsen. Er ist in hardware/kinematics.yaml (base_frame) definiert, zusammen mit der Rotation aus dem CAD-Frame (STEP).
• Frame-Baum: ros2/strido_description/urdf/strido.urdf.xacro definiert base_link, imu_link, lidar_link, camera_link, camera_optical_frame und für jedes Bein *_hip, *_thigh, *_calf und *_foot.
• Orientierung: Die Firmware führt einen Mahony-Filter aus und gibt eine Quaternion aus (firmware/strido_esp32s3/src/main.cpp). mcu_bridge veröffentlicht sie als /imu/data in imu_link.
• Beintransformationen im Code: foot_position in packages/strido-core/src/strido/leg.py.

Frames

Ein Frame ist ein Ursprung plus drei orthogonale Einheitsachsen. Ein Punkt $\mathbf{p}$ hat für sich allein keine Koordinaten; er hat nur Koordinaten in einem Frame. Wir schreiben ${}^A\mathbf{p}$ für „die Koordinaten von $\mathbf{p}$ im Frame $A$“. Alle Frames hier sind rechtshändig: $\hat{\mathbf{x}}\times \hat{\mathbf{y}}=\hat{\mathbf{z}}$.

ROS legt die Konventionen in REP-103 fest. Körper-Frames verwenden $x$ vorwärts, $y$ links und $z$ oben. Einheiten sind Meter und Radiant. Strido folgt dem. Sein base_link-Ursprung ist der Mittelpunkt der vier Schnittpunkte der Hüftachsen, daher liegen die Hüften bei $(\pm 0.120,\pm 0.039,0)\mathrm{m}$ (legs_base in hardware/kinematics.yaml).

Das CAD-Modell verwendet einen anderen Frame. hardware/kinematics.yaml speichert die Rotation, die STEP-Koordinaten in Basiskoordinaten abbildet:

$${\mathbf{R}}_{\text{base}\leftarrow \text{STEP}}=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]$$

Das ist eine Drehung um $+{90}^{\circ }$ um $z$: Die STEP-$y$-Achse zeigt nach hinten, daher wird STEP $-y$ zu Basis $+x$.

Rotationsmatrizen

Die Orientierung von Frame $B$ relativ zu Frame $A$ ist eine $3\times 3$-Matrix ${}^A{\mathbf{R}}_B$. Ihre Spalten sind die Einheitsachsen von $B$, ausgedrückt in $A$-Koordinaten. Ein Vektor wird umgerechnet als ${}^A\mathbf{v}={}^A{\mathbf{R}}_B{}^B\mathbf{v}$. Da die Spalten orthonormal sind:

$${\mathbf{R}}^{\mathrm{\top }}\mathbf{R}=\mathbf{I},det\mathbf{R}=+1,{\mathbf{R}}^{-1}={\mathbf{R}}^{\mathrm{\top }}$$

Rotationsmatrizen mit diesen Eigenschaften bilden die Gruppe $SO(3)$. Die drei elementaren Rotationen sind:

$${\mathbf{R}}_x(\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right],{\mathbf{R}}_y(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right],{\mathbf{R}}_z(\theta )=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$$

Rotationen werden durch Multiplikation verknüpft, ${}^A{\mathbf{R}}_C={}^A{\mathbf{R}}_B{}^B{\mathbf{R}}_C$. Die Reihenfolge zählt, weil Rotationen nicht kommutieren: im Allgemeinen gilt ${\mathbf{R}}_x{\mathbf{R}}_y\ne {\mathbf{R}}_y{\mathbf{R}}_x$.

Rollen, Nicken, Gieren und Gimbal Lock

Eine Rotation hat drei Freiheitsgrade, ihre Matrix aber neun Zahlen. Euler-Winkel beschreiben sie mit drei aufeinanderfolgenden elementaren Rotationen. ROS und URDF verwenden Roll-Nick-Gier (RPY): Rollen $\varphi$ um $x$, Nicken $\theta$ um $y$, Gieren $\psi$ um $z$, in dieser Reihenfolge um die festen Achsen angewendet. Das ergibt:

$$\mathbf{R}={\mathbf{R}}_z(\psi ){\mathbf{R}}_y(\theta ){\mathbf{R}}_x(\varphi )$$

RPY ist leicht zu lesen. Die Kamera von Strido ist mit rpy="0 0.261799 0" montiert, einer Neigung von ${15}^{\circ }$ nach unten. Ein um $5^{\circ }$ „gerollter“ Körper ist leicht vorstellbar.

RPY-Winkel haben eine Singularität. Bei $\theta =\pm {90}^{\circ }$ fallen Roll- und Gierachse zusammen, und nur $\varphi \pm \psi$ ist definiert. Ein Freiheitsgrad ist verloren. Das ist der Gimbal Lock. In seiner Nähe kann eine kleine Orientierungsänderung einen großen Sprung der Winkel verursachen, und die Winkelraten

$$\dot{\varphi },\dot{\theta },\dot{\psi }$$

gehen gegen unendlich. Das liegt daran, dass die Matrix, die die Winkelgeschwindigkeit des Körpers auf $(\dot{\varphi },\dot{\theta },\dot{\psi })$ abbildet, $1/\cos \theta$ enthält. Kein Satz aus drei Winkeln vermeidet das überall; es ist eine topologische Eigenschaft von $SO(3)$. Ein gehender Roboter nickt selten um ${90}^{\circ }$, ein Sturz kann es aber. Das ist ein Grund, warum Schätzer die Orientierung als Quaternion speichern.

Quaternionen

Eine Einheitsquaternion $\mathbf{q}=(w,x,y,z)$ mit $w^2+x^2+y^2+z^2=1$ kodiert eine Rotation um den Winkel $\alpha$ um die Einheitsachse $\hat{\mathbf{n}}$:

$$\mathbf{q}=(\cos \frac{\alpha }{2},\hat{\mathbf{n}}\sin \frac{\alpha }{2})$$

Rotationen werden durch das Quaternionenprodukt (Hamilton-Produkt) verknüpft. $\mathbf{q}$ und $-\mathbf{q}$ ergeben dieselbe Rotation. Die Darstellung hat keine Singularität, braucht vier statt neun Zahlen und lässt sich nach numerischer Drift günstig renormieren. Die äquivalente Rotationsmatrix ist:

$$\mathbf{R}(\mathbf{q})=\left[\begin{array}{ccc}1-2(y^2+z^2)& 2(xy-wz)& 2(xz+wy)\\ 2(xy+wz)& 1-2(x^2+z^2)& 2(yz-wx)\\ 2(xz-wy)& 2(yz+wx)& 1-2(x^2+y^2)\end{array}\right]$$

Die Firmware von Strido erhält $\mathbf{q}$ aus orientation.getQuaternion und sendet sie an den Pi. Die gelernte Policy verwendet nicht die rohen Winkel. Sie verwendet die Richtung der Schwerkraft aus Sicht des Körpers, ${}^B\mathbf{g}=\mathbf{R}(\mathbf{q}{)}^{\mathrm{\top }}(0,0,-1{)}^{\mathrm{\top }}$ (siehe packages/strido-sim/src/strido_sim/rl/env.py). Dieser Vektor ist für jede Orientierung glatt und hat keinen Gimbal Lock. Siehe State Estimation, wie der Filter $\mathbf{q}$ erzeugt.

Homogene Transformationen

Eine Pose kombiniert eine Rotation und eine Translation. Eine homogene $4\times 4$-Transformation fasst beide zusammen:

$${}^A{\mathbf{T}}_B=\left[\begin{array}{cc}{}^A{\mathbf{R}}_B& {}^A{\mathbf{t}}_B\\ {\mathbf{0}}^{\mathrm{\top }}& 1\end{array}\right],\left[\begin{array}{c}{}^A\mathbf{p}\\ 1\end{array}\right]={}^A{\mathbf{T}}_B\left[\begin{array}{c}{}^B\mathbf{p}\\ 1\end{array}\right]$$

Hier ist ${}^A{\mathbf{t}}_B$ der Ursprung von $B$, ausgedrückt in $A$. Transformationen werden durch Multiplikation verkettet, ${}^A{\mathbf{T}}_C={}^A{\mathbf{T}}_B{}^B{\mathbf{T}}_C$. Die Inverse ist:

$${}^B{\mathbf{T}}_A={\left({}^A{\mathbf{T}}_B\right)}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^{\mathrm{\top }}& -{\mathbf{R}}^{\mathrm{\top }}\mathbf{t}\\ {\mathbf{0}}^{\mathrm{\top }}& 1\end{array}\right]$$

Jedes URDF-Gelenk ist genau eine solche Transformation. Zuerst kommt der feste <origin xyz rpy>. Bei einem Drehgelenk folgt eine Rotation um <axis> um den Gelenkwinkel.

Der tf-Baum von Strido

Diagramm wird gezeichnet…

robot_state_publisher berechnet die Beintransformationen aus den Gelenkwinkeln. Die Sensor-Frames sind fest. Zum Beispiel liegt imu_link bei $(0.042,-0.004,-0.011)\mathrm{m}$, mit Achsen parallel zu base_link. camera_optical_frame verwendet rpy="-π/2 0 -π/2", damit seine $z$-Achse entlang der Blickrichtung zeigt, was die optische Konvention in REP-103 ist. odom → base_link kommt von gait_node, das die kommandierte Körpergeschwindigkeit integriert. Es ist keine gemessene Odometrie (siehe den Docstring von ros2/strido_control/strido_control/gait_node.py).

Für das vordere linke Bein lautet die URDF-Kette:

$${}^{\text{base}}{\mathbf{T}}_{\text{foot}}=\underset{\text{FL\_hip\_joint}}{\underbrace{\mathrm{Trans}(0.1197,0.039,0){\mathrm{Rot}}_x(q_1)}}\underset{\text{FL\_thigh\_joint}}{\underbrace{\mathrm{Trans}(0,0.0383,0){\mathrm{Rot}}_y(q_2)}}\underset{\text{FL\_calf\_joint}}{\underbrace{\mathrm{Trans}(0,0,-0.130){\mathrm{Rot}}_y(q_3)}}\underset{\text{FL\_foot\_joint}}{\underbrace{\mathrm{Trans}(0,0,-0.124)}}$$

Rechenbeispiel: Abduktion des vorderen linken Beins

In der Standpose (stand_pose() in strido/gait.py, Körperhöhe $0.17\mathrm{m}$) sind die Gelenkwinkel vorne links $q_1=0$, $q_2={46.5}^{\circ }$ und $q_3=-{96.0}^{\circ }$. Die Rotationen von Oberschenkel und Unterschenkel platzieren den Fuß bei ${\mathbf{p}}_{\text{hip}}=(0,0.0383,-0.170)\mathrm{m}$ relativ zum Ursprung des Hüftgelenks. Die $0.0383\mathrm{m}$ sind der seitliche Versatz zur Beinebene, und der Fuß steht senkrecht unter der Hüfte.

Nun Abduktion um $q_1={10}^{\circ }$ ($\cos =0.9848$, $\sin =0.1736$):

$${\mathbf{R}}_x({10}^{\circ }){\mathbf{p}}_{\text{hip}}=\left[\begin{array}{c}0\\ 0.9848\cdot 0.0383-0.1736\cdot (-0.170)\\ 0.1736\cdot 0.0383+0.9848\cdot (-0.170)\end{array}\right]=\left[\begin{array}{c}0\\ 0.0672\\ -0.1608\end{array}\right]\mathrm{m}$$

Addiert man den Hüftursprung $(0.1197,0.039,0)$, liegt der Fuß bei $(0.1197,0.1062,-0.1608)\mathrm{m}$ in base_link. foot_position("FL", (0.1745, 0.8121, -1.6761)) liefert denselben Punkt. Dieselbe Rotation als Quaternion ist $(\cos 5^{\circ },\sin 5^{\circ },0,0)=(0.9962,0.0872,0,0)$.

Prüfen Sie sich selbst

1. Warum ist ${}^A{\mathbf{R}}_B^{-1}$ einfach die Transponierte?
2. Der IMU-Frame ist parallel zu base_link, aber um einige Zentimeter versetzt. Ändert der Versatz die gemessene Winkelgeschwindigkeit? Ändert er die gemessene Beschleunigung?
3. Warum beobachtet die Policy den Schwerkraftvektor statt Rollen, Nicken und Gieren?
4. Schreiben Sie ${}^{\text{camera\_optical}}{\mathbf{T}}_{\text{base}}$ mithilfe der URDF-Transformationen.

Antworten

1. Die Spalten sind orthonormal, daher gilt ${\mathbf{R}}^{\mathrm{\top }}\mathbf{R}=\mathbf{I}$.
2. Die Winkelgeschwindigkeit ist überall auf einem starren Körper gleich, daher ändert der Versatz sie nicht. Die lineare Beschleunigung ändert sich: Ein versetzter Punkt sieht zusätzliche Terme $\mathit{\omega }\times (\mathit{\omega }\times \mathbf{r})+\dot{\mathit{\omega }}\times \mathbf{r}$.
3. Er ist stetig und hat keine Singularität. Außerdem ist das Gieren mit einer IMU ohne Magnetometer nicht beobachtbar, und das Gehen braucht es nicht.
4. ${\left({}^{\text{base}}{\mathbf{T}}_{\text{camera}}{}^{\text{camera}}{\mathbf{T}}_{\text{optical}}\right)}^{-1}$, wobei jeder Faktor aus xyz und rpy des Gelenks gebildet wird.

Weiterführende Literatur

• K. M. Lynch und F. C. Park, Modern Robotics: Mechanics, Planning, and Control, Cambridge University Press, 2017, Kap. 3 (Starrkörperbewegungen). Kostenloses PDF
• B. Siciliano, L. Sciavicco, L. Villani und G. Oriolo, Robotics: Modelling, Planning and Control, Springer, 2009, Kap. 2.
• REP-103: Standard Units of Measure and Coordinate Conventions und REP-105: Coordinate Frames for Mobile Platforms.
• J. Solà, „Quaternion kinematics for the error-state Kalman filter“, arXiv:1711.02508, 2017.
• ROS 2 und die Roboterbeschreibung in diesem Wiki.