Trasformazioni di coordinate robotiche: matrici, quaternioni e TF

Contents — find the section you need

Change parameters and verify

Open the panel, then press Run to load Python. You can stop execution and reset parameters. Results are computed on this device. No Python installation is required.

Local execution steps below are optional for reproducing the source results; they are not required for the browser experiment.

The experiment controls are in English.

Open experiment panel in a new tab

Download reproduction source

Inizia una trasformazione di coordinate specificando quale sistema di riferimento descrive il punto di input e quale dovrebbe descrivere l'output. Anche con i calcoli matriciali corretti e la direzione invertita, gli oggetti vengono comunque posizionati in modo errato.

Interpretare gli indici come direzione

Sia R_AB la matrice che mappa le coordinate di B in coordinate di A, e t_AB l'origine di B espressa in A. Utilizziamo vettori colonna:

p_A=R_{AB}p_B+t_{AB}

Le colonne della matrice di rotazione rappresentano gli assi di B espressi in A. Anche la traslazione è espressa in A. I punti subiscono una traslazione; i vettori liberi, come le direzioni, ruotano soltanto. Applicare lo stesso trattamento a ogni array di tre elementi confonde questi significati.

Un sensore ruotato di novanta gradi

In 2D, posizioniamo l'origine di B in (1, 2) m in A e orientiamo l'asse x di B lungo l'asse y di A. Il punto (2, 0) m in B diventa:

R_{AB}=\begin{bmatrix}0&-1\\1&0\end{bmatrix},\qquad p_A=\begin{bmatrix}0&-1\\1&0\end{bmatrix}\begin{bmatrix}2\\0\end{bmatrix}+\begin{bmatrix}1\\2\end{bmatrix}=\begin{bmatrix}1\\4\end{bmatrix}

Sistema di riferimento traslato in (1,2) m e ruotato di 90 gradi; il punto trasformato è (1,4) m.

La funzione transforms nello script Python verifica questo risultato e il suo inverso. È stata eseguita con Python 3.12.3, NumPy 1.26.4 e Matplotlib 3.6.3. Eseguire python3 engineering_labs.py per rigenerare la figura e i risultati.

Per rigenerare, salvare lo script collegato come engineering_labs.py in una nuova directory ed eseguirlo lì. I file di output vengono salvati nella directory di lavoro: questa figura è transforms.svg e le coordinate verificate si trovano sotto transform in engineering-results.json. Lo stesso script genera anche figure di Kalman, ICP, risposta in frequenza e aliasing, oltre a file CSV. Utilizzare una directory di lavoro vuota perché i file di output esistenti verranno sovrascritti.

L'inversione richiede più della semplice negazione della traslazione

L'inversa di una matrice di rotazione è la sua trasposta:

p_B=R_{AB}^T(p_A-t_{AB}),\qquad R_{BA}=R_{AB}^T,\qquad t_{BA}=-R_{AB}^Tt_{AB}

Sottraendo (1, 2) da (1, 4), quindi ruotando di -90° si ottiene (2, 0). La traslazione inversa è (-2, 1), non semplicemente (-1, -2). Eseguire un'operazione di andata e ritorno su punti arbitrari per rilevare eventuali errori di direzione.

Composizione di trasformazioni omogenee

In 3D, una matrice 4×4 combina rotazione e traslazione:

T_{AB}=\begin{bmatrix}R_{AB}&t_{AB}\\0&1\end{bmatrix},\qquad T_{AC}=T_{AB}T_{BC}

La trasformazione più a destra agisce per prima. Con la mappa A, il corpo B e il sensore C, un punto si sposta sensore→corpo→mappa. La moltiplicazione di matrici generalmente non può essere riordinata. L'abbinamento della matrice B intermedia facilita anche la denominazione delle variabili.

I quaternioni rappresentano la stessa rotazione

Per l'asse unitario u e l'angolo θ, l'ordine dei componenti ROS (x, y, z, w) fornisce:

q=(u_x\sin(\theta/2),u_y\sin(\theta/2),u_z\sin(\theta/2),\cos(\theta/2))

Una rotazione di 90° attorno all'asse z è (0, 0, 0.7071, 0.7071). Alcune librerie utilizzano (w, x, y, z), quindi verificare l'ordine prima di copiare gli array. q e −q codificano la stessa rotazione; la sola sottrazione dei componenti non è una metrica per l'errore di rotazione. Verificare anche la normalizzazione. REP-103 definisce le convenzioni SI destrorse di ROS, corpo x-avanti/y-sinistra/z-alto e ottica z-avanti/x-destra/y-basso. Verificare prima le convenzioni quando la geometria della telecamera appare ruotata.

La trasformazione TF dipende anche dal tempo

Leggere una trasformazione padre/figlio come la posa del figlio espressa nel suo genitore, corrispondente a T_AB sopra. Le API di trasformazione dei punti richiedono un'attenta selezione del frame di destinazione/sorgente. Una trasformazione di montaggio fissa differisce da una posa del veicolo variabile nel tempo.

REP-105 distingue la mappa corretta globalmente, che può presentare salti, dall'odom continuo ma con deriva. Le matrici corrette al timestamp errato continuano a posizionare in modo errato i punti del sensore in movimento. Continuare con il debug della fusione dei sensori per verificare contemporaneamente direzione e temporizzazione.