Loading Video...

[Tutorial] Rotationsmatrix und Quaternion einfach erklärt in DIN70000 ZYX Konvention

by Paul Balzer on 8. Juli 2014

11 Comments

Play Video Hide Video

Die Rotation eines Körpers im Raum ist ein Thema, welches einen Ingenieur in vielen Einsatzbereichen tangiert. Es gibt auch schon unzählige Webseiten dazu und auch die Wikipedia lässt sich zum Thema Drehmatrix oder Eulersche-Winkel ausführlich aus.

Doch so richtig gepasst hat bisher keine Beschreibung. Deshalb an dieser Stelle noch einmal eine ausführliche und einfache Beschreibung der 3D Rotation eines Körpers/Vektors mit Euler-Winkeln nach ZYX-Konvention im DIN70000 Koordinatensystem des Fahrzeugs.

Das Koordinatensystem ist folgendermaßen definiert:

  • x-Achse in Fahrzeuglängsrichtung positiv
  • y-Achse nach links positiv
  • z-Achse nach oben positiv
  • Drehungen um die Achsen mit rechter Hand Regel positiv (Daumen zeigt in Richtung der Achse, Finger zeigen positive Drehrichtung)
Rotation nach ZYX Konvention: 1. Gieren (Yaw) 2. Nicken (Pitch) 3. Rollen (Roll)

Rotation nach ZYX Konvention:
1. Gieren (Yaw)
2. Nicken (Pitch)
3. Rollen (Roll)

Die Drehung ist nach ZYX Konvention folgendermaßen definiert:

  • um Z-Achse, welche die Gierachse ([engl.: Yaw]) ist, bei Linkskurve positiv
  • um Y-Achse, welche die Nickachse ([engl.: Pitch]) ist, beim Eintauchen der Fahrzeugfront positiv
  • um X-Achse, welche die Rollachse ([engl.: Roll]) ist, beim Eintauchen der Beifahrerseite positiv

Natürlich kann man das beliebig anders definieren, aber dann muss man sich eben über die Rotationsmatrix und die Quaternion wieder selbst Gedanken machen. Behält man die Definition bei, so kann die Drehung einfach mit nachfolgenden Berechnungen vollführt werden.

Rotationsmatrix für ZYX Konvention berechnen

Die Rotationsmatrix setzt sich zusammen aus 3 einzelnen Matrizen, jeweils für Z-Rotation, Y-Rotation und X-Rotation:

$$\begin{align}
R & =
\begin{pmatrix}
1 & 0 & 0 \\
0 & \cos \phi & -\sin \phi \\
0 & \sin \phi & \cos \phi
\end{pmatrix}
\begin{pmatrix}
\cos \theta & 0 & \sin \theta \\
0 & 1 & 0 \\
-\sin \theta & 0 & \cos \theta
\end{pmatrix}
\begin{pmatrix}
\cos \psi & -\sin \psi & 0 \\
\sin \psi & \cos \psi & 0 \\
0 & 0 & 1
\end{pmatrix}
\end{align}$$

mit \(\psi = \textrm{Gieren}\), \(\theta = \textrm{Nicken}\), \(\phi = \textrm{Wanken}\) (von rechts multiplizieren)

Die ausmultiplizierte Version wird hier nicht angegeben.

Python Implementierung für Rotationsmatrix nach ZYX Konvention

def Rypr(y, p, r):
    '''
    Rotationsmatrix für y=yaw, p=pitch, r=roll in degrees
    '''
    # from Degree to Radians
    y = y*np.pi/180.0
    p = p*np.pi/180.0
    r = r*np.pi/180.0

    Rr = np.matrix([[1.0, 0.0, 0.0],[0.0, np.cos(r), -np.sin(r)],[0.0, np.sin(r), np.cos(r)]])
    Rp = np.matrix([[np.cos(p), 0.0, np.sin(p)],[0.0, 1.0, 0.0],[-np.sin(p), 0.0, np.cos(p)]])
    Ry = np.matrix([[np.cos(y), -np.sin(y), 0.0],[np.sin(y), np.cos(y), 0.0],[0.0, 0.0, 1.0]])

    return Ry*Rp*Rr

Die Rotationsmatrix erhält man durch Aufrufen der Funktion mit den 3 Rotationswinkeln als Argument:

R = Rypr(gieren, nicken, wanken)

Anschließend kann man einen beliebigen Vektor (oder Koordinatensystem oder Körper) mit der Rotationsmatrix multiplizieren und erhält das gedrehte Pendant. Sie ist z.B. geeignet, um ein erdfestes Koordinatensystem in das Fahrzeug-Koordinatensytem zu transformieren.

Beispielhaft ein Vektor \(g=\begin{bmatrix}1 & 0 & 0\end{bmatrix}^T\), welcher in positive x-Richtung zeigt, um 60 Grad um die Z-Achse gedreht und 45 Grad um die Y-Achse geneigt:

Original Vektor (g) und um 60Grad um Z sowie 45Grad um Y gedrehter Vektor in 3 Ansichten (links: Draufsicht, mitte: 3D, rechts: Seitenansicht)

Original Vektor (g) und um 60Grad um Z sowie 45Grad um Y gedrehter Vektor in 3 Ansichten (links: Draufsicht, mitte: 3D, rechts: Seitenansicht)

Die Rücktransformation des gedrehten Vektors kann mit der transponierten Rotationsmatrix erfolgen.

Problem des Gimbal-Lock bei Euler Drehung

Das Problem an der Drehung mit 3 Winkeln besteht dann, wenn eine solche Konfiguration entsteht, bei welcher 2 Drehachsen übereinander liegen.

Bei der ZYX-Konvention, wird der Gimbal-Lock erreicht, wenn der Nickwinkel \(\theta=90^\circ\) eingestellt wird. Dann ist sowohl Rollen als auch Gieren die gleiche Bewegung, welche allerdings keine Änderung der Lage mehr hervorruft. Dieser Zustand ist das so genannte „Gimbal-Lock“ und muss verhindert werden.

Aus diesem Grund wird häufig die Rotation mit der Quaternion vorgezogen.

Rotation mit Quaternion

Die Quaternion ist eine Erweiterung der komplexen Zahlen. Im Grunde genommen nur ein theoretisches Konstrukt, welches es ermöglicht, solche Berechnungen anzustellen.

Ähnlich wie bei den komplexen Zahlen, die als Summe aus Real- und Imaginärteil beschrieben werden (\(Z = a\cdot 1 + b \cdot \mathrm{i}\)), wird die Quaternion als Linearkombination aus 3 Imaginärteilen und einem Realteil konstruiert:

\(q = \underbrace{a\cdot 1}_{\textrm{real}} + \underbrace{b \cdot \mathrm{i} + c \cdot \mathrm{j} + d \cdot \mathrm{k}}_{\textrm{imag}}\)
missing
Source: http://www.opengl-tutorial.org/

Bei der Beschreibung der 3D-Drehung mit der Quaternion, wird mit den imaginären Elementen b, c und d ein Vektor definiert, um den (mit a) gedreht wird.

Dabei berechnet sich der Drehwinkel

\(w=2\arccos(a)\)

und die 3 Vektorkoordinaten der Rotationsachse:

\(n_R=\left[\begin{matrix}\cfrac{b}{\sin(\frac{w}{2})} \\ \cfrac{c}{\sin(\frac{w}{2})} \\ \cfrac{d}{\sin(\frac{w}{2})}\end{matrix}\right]\)

Dabei ist die Rotation nur mit der Einheitsquaternion möglich. Die folgende Funktion normiert die Quaternion auf die Einheitsquaternion:

def normQ(q):
    '''Calculates the normalized Quaternion
    a is the real part
    b, c, d are the complex elements'''
    # Source: Buchholz, J. J. (2013). Vorlesungsmanuskript Regelungstechnik und Flugregler.
    # GRIN Verlag. Retrieved from http://www.grin.com/de/e-book/82818/regelungstechnik-und-flugregler
    a, b, c, d = q

    # Betrag
    Z = np.sqrt(a**2+b**2+c**2+d**2)

    return np.array([a/Z,b/Z,c/Z,d/Z])

Rotationsmatrix aus Quaternion

Die Rotationsmatrix kann aus der Quaternion berechnet werden, falls sie benötigt wird.

$$R = \left[\begin{matrix}(a^2+b^2-c^2-d^2) & 2(bc-ad) & 2(bd+ac) \\ 2(bc+ad) & (a^2-b^2+c^2-d^2) & 2(cd-ab) \\ 2(bd-ac) & 2(cd+ab) & (a^2-b^2-c^2+d^2)\end{matrix}\right]$$

Python Implementierung zur Berechnung der Rotationsmatrix aus der Quaternion

def QtoR(q):
    '''Calculates the Rotation Matrix from Quaternion
    a is the real part
    b, c, d are the complex elements'''
    # Source: Buchholz, J. J. (2013). Vorlesungsmanuskript Regelungstechnik und Flugregler.
    # GRIN Verlag. Retrieved from http://www.grin.com/de/e-book/82818/regelungstechnik-und-flugregler
    q = normQ(q)

    a, b, c, d = q

    R11 = (a**2+b**2-c**2-d**2)
    R12 = 2.0*(b*c-a*d)
    R13 = 2.0*(b*d+a*c)

    R21 = 2.0*(b*c+a*d)
    R22 = a**2-b**2+c**2-d**2
    R23 = 2.0*(c*d-a*b)

    R31 = 2.0*(b*d-a*c)
    R32 = 2.0*(c*d+a*b)
    R33 = a**2-b**2-c**2+d**2

    return np.matrix([[R11, R12, R13],[R21, R22, R23],[R31, R32, R33]])

Euler Winkel aus der Quaternion

Natürlich können auch die Euler-Winkel aus der Quaternion berechnet werden, falls benötigt.

$$\psi = \arctan\left(\cfrac{2(bc+ad)}{a^2+b^2-c^2-d^2}\right) \\
\theta = \arcsin(2(ac-bd)) \\
\phi = -\arctan\left(\cfrac{2(cd+ab)}{-(a^2-b^2-c^2+d^2)}\right)$$

Python Implementierung zur Berechnung der Euler Winkel aus der Quaternion

def Q2Eul(q):
    '''Calculates the Euler Angles from Quaternion
    a is the real part
    b, c, d are the complex elements'''
    # Source: Buchholz, J. J. (2013). Vorlesungsmanuskript Regelungstechnik und Flugregler.
    # GRIN Verlag. Retrieved from http://www.grin.com/de/e-book/82818/regelungstechnik-und-flugregler
    q = normQ(q)

    a, b, c, d = q

    gieren = np.arctan2(2.0*(b*c+a*d),(a**2+b**2-c**2-d**2)) * 180.0/np.pi
    nicken = np.arcsin(2.0*(a*c-b*d)) * 180.0/np.pi
    wanken = -np.arctan2(2.0*(c*d+a*b),-(a**2-b**2-c**2+d**2)) * 180.0/np.pi

    return np.array([gieren, nicken, wanken])

Die Rotation erfolgt nun mit der Quaternion genauso, wie mit Euler Winkeln. Einfach aus den Elementen a, b, c, d die Rotationsmatrix berechnen und den Vektor/Objekt rotieren.

Das nachfolgende Video zeigt dies anschaulich:

Video Tutorial zur Rotation mit Quaternion und Euler Winkel

Rotation einer Punktwolke

Natürlich kann auch eine Punktwolke rotiert werden und nicht nur ein Vektor. Das folgende Video zeigt das äquivalente Vorgehen. Die Software zur Simulation des Laserscanners (oder einer Kinect) ist BlenSor.

Das komplette IPython Notebook gibt es auf Github.

11 Comments

  1. Hey, hätte mal eine Frage zu deiner Erklärung. Ich bin bei dem Gimbal-Lock zum Stocken gekommen. Dieser kann doch nur eintreten, wenn es raumfeste Axel gibt und andere mit gedreht werden. Das ist hier doch gar nicht der Fall. Außerdem hab ich noch eine Frage: Ist in der DIN70000 eine Rotation nach Eulerwinkeln, also Mitdrehen des Koordinatensystems beschrieben oder um ein Ortsfestes Koordinatensystem? Die Berechnungen hier, drehen das Auto um ein Ortsfestes System, auf Wikipedia wird jede Drehung um das neue Koordinatensystem ausgeführt.
    Grüße, Jan
    siehe: http://de.wikipedia.org/wiki/Eulersche_Winkel
    http://de.wikipedia.org/wiki/Gimbal_Lock (Animation dreht eine Achse, anderen bleiben Ortsfest, weil fixiert). Ich glaube Quaternionen werden im KFZ Bereich vor allem wegen der Mehrdeutigkeit von Eulerwinkeln benutzt.

  2. Sehr gute Erklärung, vielen Dank!
    Die Implementierung als IPython notebook ist eine super Idee.
    Eine Anmerkung: Beim Link auf GitHub fehlt ein „s“ beim „httpS://“

  3. Habe die oben angegebenen Gleichungen Quaternion -> Euler programmiert. Die Quaterniondaten kommen aus dem BNO055. Solange ROLL und PTICH nicht gleichzeitig geändert wird, stimmen die Erg.
    Sonst nicht.

    VG Blödow

    1. Die Gleichungen stimmen nur, wenn die Quaternion genauso definiert ist, wie sie aus dem Tinkerforge Sensor kommen.
      * a is the real part
      * b, c, d are the complex elements

      Manchmal ist auch
      * a, b, c complex
      * d real

      oder noch anders. Da muss man bisschen rum probieren. :)

  4. Super erklärung! Und wieder ein gutes Beispiel
    Ich glaube die DIN 70000 ist inzwischen von der DIN 8855 abgelößt worden. Hat sich aber, glaub ich, nichts geändert im Automobilbereich.

  5. Hi,

    „Bei der Beschreibung der 3D-Drehung mit der Quaternion, wird mit den imaginären Elementen b, c und d ein Vektor definiert, um den (mit a) gedreht wird.“

    Super, genau nach so einer Erklärung hab ich gesucht :)

  6. Sehr schönes Tutorial!
    Mit welcher Python und iPython Version ist das Notebook erstellt?
    Mit Anaconda Version 2.7 bekomme ich es nicht zum laufen, weil es in eine Notebook-Version 4 konvertiert wird.
    Auch die Schieberegler aus dem Video fehlen dann. Bin noch neu in der Materie, fällt mir aktuell schwer die Fehlermeldungen zu interpretieren und zu beheben…

Leave a Reply

Deine E-Mail-Adresse wird nicht veröffentlicht. Erforderliche Felder sind mit * markiert.