I quaternioni di Hamilton e la rotazione in 3D con Python

Post Views: 15.465

Con gli angoli di Eulero si sono fondate le basi per il calcolo della rotazione di corpi in spazi tridimensionali. Successivamente si è scoperto però che i quaternioni di Hamilton risultano uno strumento più efficiente per lo studio del modo di rotazione dei corpi. In questo articolo vedremo cosa sono i quaternioni, come si calcolano e come si applicano alla rotazione di un corpo, aiutandoci anche in questo caso con del codice Python.

I quaternioni e la rotazione in 3D con Python

Leggi articolo precedente: Le rotazioni in 3D e gli angoli di Eulero

I Quaternioni di Hamilton

I quaternioni furono ideati dal matematico da Hamilton (1805-1865) per estendere le funzionalità dei numeri complessi in un sistema a quattro dimensioni.

I numeri complessi vengono generalmente indicati con la notazione

$z = a + bi$

dove a è la parte reale, b è la componente immaginaria mentre i è l’unità immaginaria definita come la radice di -1

$i = \sqrt{-1}$

I quaternioni estendono il concetto dei numeri complessi, e quindi si aggiungono altri due simboli:

$q=w+xi+yj+zk$

dove w è la parte reale, e i,j,k denotano le tre unità immaginarie, e x,y,z denotano le tre componenti immaginarie. Per rendere le cose più semplici si può scrivere semplicemente:

$q=w+v$

dove w è la componente scalare e v=(x,y,z) è il vettore delle componenti immaginarie.

L’algebra dei quaternioni

Alla base dell’algebra dei quaternioni si hanno le seguenti regole base:

$i^2=j^2=k^2=ijk= -1$

$ij = k = -ji$

$jk = i = -kj$

$ki = j = -ik$

Inoltre dati due quaternioni q₁ e q₂:

$q_1=w_1+x_1i+y_1j+z_1k$

$q_2=w_2+x_2i+y_2j+z_2k$

la somma dei due quaternioni si definisce nel seguente modo:

$q_1+q_2=(w_1+w_2)+(x_1+x_2)i+(y_1+y_2)j+(z_1+z_2)k$

mentre il prodotto dei due quaternioni si svolge in questo modo:

$q_1q_2=(w_1w_2-x_1x_2−y_1y_2-z_1z_2)+(w_1x_2+x_1w_2+y_1z_2-z_1y_2)i+(w_1y_2-x_1z_2+y_1w_2+z_1x_2)j+(w_1z_2+x_1y_2-y_1x_2+z_1w_2)k$

Nella notazione con il vettore v si ha un’espressione più sintetica

$q_1q_2=w_1w_2 - v_1\cdot v_2 + w_1v_2+w_2v_1 + v_1 \times v_2$

dove v1∙v2 è il prodotto scalare e v1×v2 è il prodotto vettoriale.

Altre definizioni importante per l’algebra dei quaternioni sono il coniugato, la norma e l’inverso di un quaternione.

Il coniugato di un quaternione si indica con q^* e si definisce:

$q^*=w-xi-yj-zk$

Se moltiplichiamo un quaternione con il suo coniugato abbiamo:

$q q^* = w^2 + x^2 + y^2 + z^2$

La lunghezza o norma di un quaternione viene invece definita come:

$N(q) = \mid q \mid = \sqrt{ q q^* } = \sqrt{w^2 + x^2 + y^2 + z^2}$

Infine per ogni quaternione, eccetto q=0, esiste un inverso definito come:

$q^{-1} = \frac{q^*}{\mid q \mid ^2}$

Ultima operazione algebrica è la divisione tra due quaternioni, che si può ottenere moltiplicando il primo quaternione per l’inverso del secondo

$q_1 q_2^{-1}$

Ultima considerazione al riguardo è che mentre i numeri complessi costituiscono un campo algebrico, i quaternioni costituiscono invece un corpo algebrico. Questo perchè per i quaternioni non vale la proprietà commutativa del prodotto e quindi q₁ per q₂ è diverso da q₂ x q₁.

I quaternioni come strumento per il calcolo delle rotazioni di un corpo intorno ad un asse arbitrario

Secondo il teorema di Eulero, qualsiasi rotazione o sequenza di rotazioni di un corpo rigido intorno ad un punto fisso è equivalente ad una singola rotazione di un angolo θ intorno ad un determinato asse (chiamato asse di Eulero) che passa attraverso questo punto fisso.

Angoli di Eulero - due sistemi di riferimento

L’asse di Eulero è normalmente rappresentato da un versore (vettore unità) $\hat{u}$ . Perciò qualsiasi rotazione in 3 dimensioni puoi essere rappresentata come una combinazione di un versore $\hat{u}$ ed uno scalare θ.

Una rotazione di un angolo θ attorno ad un asse definito dal versore

$\hat{u} = (u_x, u_y, u_z) = u_x \hat{i} + u_y \hat{j} + u_z \hat{k}$

In maniera simile a come abbiamo fatto con gli angoli di Eulero e la matrice di rotazione R, che si possono applicare ad un vettore ordinario p:

$\vec{p} = (p_x, p_y, p_z)$

Possiamo quindi indicare con R(θ,p) la rotazione di un vettore p di un angolo θ intorno all’asse indicato dal versore u, come abbiamo già visto nell’articolo precedente sugli angoli di Eulero.

$p_1 = R p$

La stessa rotazione può essere rappresentata con un quaternione di Hamilton:

$q = q_w + q_x \hat{i} + q_y \hat{j} + q_z \hat{k}$

rappresentabile anche nella forma

$q = \begin{bmatrix} q_w & q_x & q_y & q_z \end{bmatrix}^T$

con

$\mid q \mid ^2 = q_w^2 + q_x^2 + q_y^2 + q_z^2$

associando i vari termini agli angoli di rotazione

$q_w = cos(\theta /2)$

$q_x = sin(\theta /2) cos(\beta_x)$

$q_y = sin(\theta /2) cos(\beta_y)$

$q_z = sin(\theta /2) cos(\beta_z)$

dove theta è l’angolo di rotazione e $cos(\beta_x), cos(\beta_y), cos(\beta_z)$ sono i coseni direttori dell’asse di rotazione indicato dal versore u.

Inoltre si può ottenere un risultato alla matrice di Rotazione utilizzata con gli angoli di Eulero, utilizzando il quaternione q, calcolando la coniugazione del vettore p₀ con q. Si ottiene così il nuovo vettore p₁ che è il risultato della rotazione.

$p_1 = q p_0 q^*$

con

$q^* = e^{-\frac{1}{2} \theta ( u_x \hat{i} + u_y \hat{j} + u_z \hat{k} ) }$

$= cos \frac{\theta}{2} - ( u_x \hat{i} + u_y \hat{j} + u_z \hat{k} ) sin\frac{\theta}{2}$

Applicando diversi teoremi e definendo un quaternione puro:

$P = (0, v)$

e un quaternione unitario

$\vert \vert q \vert \vert = 1$

$q = (cos\theta, u sin\theta)$

si trova che

$R(\theta,v) = q P q^*$

Inoltre espandendo il secondo termine si ottiene una matrice

$R = \begin{pmatrix} 1 - 2y^2 -2z^2 & 2xy - 2wz & 2xz +2wy \\ 2xy + 2wz & 1 - 2x^2 - 2z^2 & 2yz - 2wz \\ 2xz - 2wy & 2yz + 2wx & 1 - 2x^2 - 2y^2 \end{pmatrix}$

Data la matrice di rotazione sopra descritta, è possibile risalire al quaternione corrispondente con pochi passaggi. Si calcola la traccia della matrice R (somma degli elementi diagonali):

$Tr(R) = 3 - 4x^2 - 4y^2 - 4z^2 = 4w^2 - 1$

questo poiché il quaternione è unitario

$x^2 + y^2 + z^2 +w^2 = 1$

quindi

$w = \sqrt{\frac{Tr(R)+1}{4}}$

In modo simile si calcolano le altri componenti x,y,z.

Rotazione con i quaternioni in Python

Adesso che abbiamo visto che è possibile effettuare calcoli di rotazione con i quaternioni e quali sono le espressioni matematiche da utilizzare, cominciamo ad implementare il tutto in Python.

Per prima cosa abbiamo bisogno di calcolarci il coniugato del quaternione.

def q_conjugate(q):
    w, x, y, z = q
    return (w, -x, -y, -z)

Adesso abbiamo tutti gli elementi per effettuare la moltiplicazione

$P_1 = q P_0 q^*$

Infatti P non è altro che il quaternione puro ricavato, utilizzando il vettore v da ruotare per i tre termini immaginari e la parte reale w uguale a zero.

$P = (0, v)$

Quindi in Python possiamo implementare in questo modo:

def qv_mult(q1, v1):
    q2 = (0.0,) + v1
    return q_mult(q_mult(q1, q2), q_conjugate(q1))[1:]

Come possiamo vedere nella funzione, è necessario utilizzare la moltiplicazione tra quaternioni, e quindi dobbiamo implementarla in Python utilizzando le equazioni prima svolte matematicamente.

def q_mult(q1, q2):
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
    y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
    z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
    return w, x, y, z

Inoltre, se abbiamo una sequenza delle rotazioni multiple, come era possibile con Eulero moltiplicare tra di loro le matrici di rotazione R, allo stesso modo si moltiplicano tra di loro i quaternioni corrispondenti alle singole rotazioni. Il quaternione risultante sarà equivalente alla rotazione totale.

Conversione dagli angoli di Eulero a quaternioni e viceversa

Altra operazione molto utile e comune è quella di passare dalla formulazione di Eulero a quella dei quaternioni. E’ possibile quindi, conoscendo gli angoli di Eulero ( $\phi, \theta, \psi$ ), ricavare il quaternione corrispondente q.

Per esempio, per gli angoli di Eulero in una convenzione XYZ, si ottiene il quaternione q corrispondente a tale rotazione con la seguente espressione:

$q = \begin{bmatrix} q_w \\ q_x \\ q_y \\ q_z \end{bmatrix} = \begin{bmatrix} cos(\phi /2)cos(\theta /2)cos(\psi /2) + sin(\phi /2)sin(\theta /2)sin(\psi /2) \\ sin(\phi /2)cos(\theta /2)cos(\psi /2) - cos(\phi /2)sin(\theta /2)sin(\psi /2) \\ cos(\phi /2)sin(\theta /2)cos(\psi /2) + sin(\phi /2)cos(\theta /2)sin(\psi /2) \\ cos(\phi /2)cos(\theta /2)sin(\psi /2) + sin(\phi /2)sin(\theta /2)cos(\psi /2) \end{bmatrix}$

Che in Python si può esprimere nella seguente funzione:

def euler_to_quaternion(phi, theta, psi):

        qw = m.cos(phi/2) * m.cos(theta/2) * m.cos(psi/2) + m.sin(phi/2) * m.sin(theta/2) * m.sin(psi/2)
        qx = m.sin(phi/2) * m.cos(theta/2) * m.cos(psi/2) - m.cos(phi/2) * m.sin(theta/2) * m.sin(psi/2)
        qy = m.cos(phi/2) * m.sin(theta/2) * m.cos(psi/2) + m.sin(phi/2) * m.cos(theta/2) * m.sin(psi/2)
        qz = m.cos(phi/2) * m.cos(theta/2) * m.sin(psi/2) - m.sin(phi/2) * m.sin(theta/2) * m.cos(psi/2)

        return [qw, qx, qy, qz]

Stessa cosa possiamo fare all’inverso. Conoscendo i termini del quaternione ricavarci gli angoli di Eulero in una convenzione XYZ.

$\begin{bmatrix} \phi \\ \theta \\ \psi \end{bmatrix} = \begin{bmatrix} arctan \frac{2(q_0 q_1 + q_2 q_3)}{1-2(q_1^2 q_2^2)} \\ arcsin (2(q_0 q_2 - q_3 q_1)) \\ arctan \frac{2(q_0 q_3 + q_1 q_2)}{1-2(q_2^2 q_3^2)} \end{bmatrix}$

Che si può implementare in Python nella seguente funzione:

def quaternion_to_euler(w, x, y, z):

        t0 = 2 * (w * x + y * z)
        t1 = 1 - 2 * (x * x + y * y)
        X = m.atan2(t0, t1)

        t2 = 2 * (w * y - z * x)
        t2 = 1 if t2 > 1 else t2
        t2 = -1 if t2 < -1 else t2
        Y = m.asin(t2)
        
        t3 = 2 * (w * z + x * y)
        t4 = 1 - 2 * (y * y + z * z)
        Z = m.atan2(t3, t4)

        return X, Y, Z

Esempio – la rotazione di un punto nello spazio

Adesso faremo lo stesso esempio che abbiamo fatto con le trasformazioni di Eulero (vedi articolo), ma questa volta utilizzando i quaternioni invece della matrice di rotazione R.

L’esempio più semplice di applicazione di quello che abbiamo già visto nell’articolo è la rotazione di un punto situato in uno spazio di coordinate (X,Y,Z). Un punto nello spazio può essere rappresentato mediante un vettore a 3 elementi che ne caratterizza i valori sui tre assi delle coordinate.

$v = \begin{bmatrix} x \\ y \\ z \end{bmatrix}$

Quindi per il nostro esempio, partiremo da un punto semplice sull’asse X descritto dal seguente vettore. Che poi è lo stesso dell’esempio con gli angoli di Eulero.

$v = \begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix}$

Esprimiamo questa volta il vettore come un tupla:

v1 = (1,0,0)

Adesso per ottenere il quaternione che equivalga alla stessa rotazione dell’esempio con gli angoli di Eulero, utilizzeremo proprio quest’ultimi e li convertiremo nel quaternione corrispondente mediante la funzione che abbiamo poco fa definito. Definiamo quindi prima i tre angoli di Eulero e poi convertiamoli nel quaternione di cui andremo a visualizzare i termini.

phi = m.pi/2
theta = m.pi/4
psi = m.pi/2
q = euler_to_quaternion(phi, theta, psi)
print("w =", q[0])
print("x =", q[1])
print("y =", q[2])
print("z =", q[3])

Eseguendo si ottiene il seguente quaternione

w = 0.6532814824381883
x = 0.27059805007309845
y = 0.6532814824381882
z = 0.2705980500730985

Adesso effettuiamo la moltiplicazione tra vettore e quaternione

v2 = qv_mult(q,v1)
print(np.round(v2, decimals=2))

Ed otterremo lo stesso vettore che avevamo ottenuto nell’esempio degli angoli di Eulero.

[ 0. 0.71 -0.71]

Vediamo il risultato della nostra rotazione riportando su grafico con assi cartesiani la posizione del vettore (che descrive il punto) prima e dopo la rotazione.

import matplotlib.pyplot as plt
 
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# Cartesian axes
ax.quiver(-1, 0, 0, 3, 0, 0, color='#aaaaaa',linestyle='dashed')
ax.quiver(0, -1, 0, 0,3, 0, color='#aaaaaa',linestyle='dashed')
ax.quiver(0, 0, -1, 0, 0, 3, color='#aaaaaa',linestyle='dashed')
# Vector before rotation
ax.quiver(0, 0, 0, 1, 0, 0, color='b')
# Vector after rotation
ax.quiver(0, 0, 0, 0, 0.71, -0.71, color='r')
ax.set_xlim([-1.5, 1.5])
ax.set_ylim([-1.5, 1.5])
ax.set_zlim([-1.5, 1.5])
plt.show()

Eseguendo il codice si otterrà la rappresentazione grafica.

Angoli di Eulero - verso delle rotazioni elementari — Visualizzazione del vettore prima (blu) e dopo la rotazione (rosso)

Da qui poi si può passare alla rotazione di linee, figure geometriche e oggetti tridimensionali. Tutto alla base dei motori 3D con cui sono sviluppati moltissimi videogiochi.

Vantaggi dell’uso dei quaternioni rispetto agli angoli di Eulero

L’uso dei quaternioni al posto degli angoli di Eulero per quanto riguarda il calcolo della rotazione in uno spazio tridimensionale porta ad alcuni vantaggi. Vediamo in dettaglio quali sono.

Per il calcolo di una rotazione con gli angoli di Eulero sono necessari ben 9 parametri, mentre per i quaternioni sono necessari solo 4 parametri.

La rotazione con gli angoli di Eulero consiste da una sequenza di rotazioni elementari attorno ad uno dei tre assi cartesiani con tante possibili combinazioni. Questo richiede diverse equazioni e risoluzioni per ogni caso. Mentre la rotazione tramite quaternioni è univoca.

La rotazione mediante quaternioni non è soggetta al gimbal lock.