Forward Kinematics – La cinematica diretta

Post Views: 12.760

Cinematica Diretta (Forward Kinematics)

La cinematica diretta (forward kinematics) ci pemette di determinare la posizione e l’orientamento dell’estremità efficiace (end-effector) in termini delle variabili dei giunti. Un modo semplice per spiegare la cinematica diretta è quella di prendere un caso molto semplice ed applicarci le metodologie base di questa tecnica.

Il manipolatore a 2 link e 2 giunti

Un caso molto semplice di robotica con cui applicare la cinematica diretta è il manipolatore a 2 link e 2 giunti. Ora il classico problema della cinematica diretta sarà quello, data la lunghezza dei link l1 ed l2 e gli angoli assunti dai due giunti θ₁ e θ₂, di ricavare la posizione dell’estremità efficace con le coordinate x e y.

Forward Kinematics - il problema della cinematica diretta

Nei casi pratici, spesso i manipolatori hanno al loro interno dei sensori che permettono di conoscere in tempo reale la posizione o l’angolo dei vari giunti. Quindi si darà per scontato la conoscenza dei valori dei due angoli θ₁ e θ₂ che si useranno come valori base per i nostri calcoli.

La prima cosa da fare sarà quello di stabilire un sistema fisso di coordinate, chiamato base frame. Nel caso dei manipolatori ci si riferisce alla base del manipolatore (estremità opposta all’estremità efficace) come base delle coordinate o₀x₀y₀.

Forward Kinematics - base frame e sistemi di coordinate cartesiane

Le coordinate (x,y) del tool vengono espresse come:

$x = x_2 = l_1 cos \theta_1 + l_2 cos (\theta_1 + \theta_2)$

$y = y_2 = l_1 sin \theta_1 + l_2 sin (\theta_1 + \theta_2)$

In cui l₁e l₂ sono le lunghezze dei due link. Anche l’orientamento del tool frame relativi al base frame viene dato dalla direzione dei coseni degli assi x₂ e y₂ rispetto agli assi x₀ e y₀.

$x_2 \cdot x_0 = cos(\theta_1 + \theta_2)$

$x_2 \cdot y_0 = -sin(\theta_1 + \theta_2)$

$y_2 \cdot x_0 = sin(\theta_1 + \theta_2)$

$y_2 \cdot y_0 = cos(\theta_1 + \theta_2)$

Queste quattro equazioni possono essere combinate per ottenere una matrice di orientamento.

$\begin{bmatrix} x_2 \cdot x_0 & y_2 \cdot x_0 \\ x_2 \cdot y_0 & y_2 \cdot y_0 \end{bmatrix} = \begin{bmatrix} cos(\theta_1 + \theta_2) & -sin(\theta_1 +\theta_2) \\ sin(\theta_1 + \theta_2) & cos(\theta_1 + \theta_2) \end{bmatrix}$

Le equazioni che abbiamo appena definito vengono chiamate le equazioni di cinematica diretta (forward kinematic equations) e sno specifiche del manipolatore preso in esame.

Casi più complessi – la procedura di Denavit-Hartenberg

Il caso del manipolatore a 2 link e 2 giunti è un esempio base, ma altri casi più avanzati posso portare ad equazioni troppo complesse e spesso non facilmente scrivibili.

In ogni caso, esiste una procedura generale che permette di stabilire un sistema di coordinate per ciascun giunto, per poi di passare da un sistema all’altro attraverso delle trasformazioni di matrici. Tale procedura viene chiamata procedura di Denavit-Hartenberg.

In tale procedura verranno usate delle coordinate omogenee e delle trasformazioni omogenee per semplificare la trasformazione tra i diversi sistemi di coordinate.

Esisono quindi delle convenzioni, dette appunto di Denavit-Hartenberg.

La numerazione dei link e dei giunti.
I link vengono numerati da 0 ad n partendo dalla base ed arrivando all’estremità efficace, e si indicano con $L_0, L_1, …, L_n$ . Si descrive così un manipolatore seriale con una catena di n+1 link. Questo manipolatore avrà di conseguenza n giunti, che si indicheranno a loro volta con $J_1, …, J_n$ .
Il link $L_i$ sarà quello che collega il giunto $J_i$ con quello $J_{i+1}$ .

Assegnazione degli assi z dei diversi sistemi di riferimento (frames).
Ad ogni link $L_i$ si associa un frame di coordinate solidale $S_i = (O_i,x_i,y_i,z_i)$ il cui asse $z_i$ coincide con l’asse del giunto $J_{i+1}$ , cioè del giunto a valle nella catena (quello più vicino all’estremità efficace). Essendo i giunti generalmente rotativi su di un piano, si indica con z, l’asse perpendicolare a questo piano.

L’elemento base: link-giunto

Quindi la procedura di Denavit-Hartenberg consiste principalmente di considerare come elemento base di un manipolatore, una coppia di giunti con il link che li unisce. Ad ogni elemento base così definito corrisponderà una matrice di trasformazione di coordinate

Si hanno quindi due giunti $J_i$ e $J_{i+1}$ ed il link $L_i$ interposto, con insieme definiti i due sistemi di riferimento $S_{i-1} = (O_{i-1}, x_{i-1}, y_{i-1}, z_{i-1})$ e $S_i = (O_i, x_i, y_i, z_i)$ .

Nella scelta dei sistemi di riferimento si procede nel seguente modo:

l’asse $z_{i-1}$ si sceglie coincidente con l’asse del giunto $J_{i-1}$
l’asse $z_i$ coincidente con l’asse del giunto $J_i$ ;
l’asse $x_{i-1}$ può essere scelto liberamente, ma è conveniente porlo in direzione del giunto successivo, e si interseca con $z_{i-1}$ in corrispondenza del centro del giunto $J_{i-1}$ (scelto come origine);
l’asse $x_i$ corre lungo la normale comune fra gli assi $z_{i-1}$ e $z_i$
gli assi $y_{n-1}$ e $y_n$ sono scelti in modo da completare le rispettive terne levogire.

La trasformazione è allora descritta da quattro parametri di Denavit-Hartenberg:

$d$ che è la distanza dell’asse $z_{i-1}$ dalla normale comune; nel caso vi siano infinite normali comuni (assi $z_i$ e e $z_{i-1}$ paralleli) si sceglierà il valore di $d$ più conveniente;
$\theta$ è l’angolo di rotazione intorno all’asse $z_{i-1}$ necessario per allineare $x_{i-1}$ con $x_i$ ;
$a$ (a volte indicato anche con $r$ è la distanza minima fra gli assi $z_{i-1}$ e $z_i$ ;
$\alpha$ è l’angolo di rotazione intorno alla normale comune (ovvero attorno a $x_i$ ) per allineare l’asse $z_{i-1}$ a $z_i$ .

Si può notare che l’asse $x_i$ è perpendicolare sia all’asse $z_{i-1}$ che all’asse $z_i$ e interseca entrambi.

Trasformazione di coordinate

Ogni coppia braccio-giunto si può descrivere come un’operazione di trasformazione di coordinate fra i due sistemi di riferimento associati ai giunti.

Se si sceglie di orientare l’asse $x_i$ lungo la normale comune fra gli assi $z_{i-1}$ e $z_i$ , la matrice di trasformazione è definita come una serie di due rototraslazioni consecutive:

${}^{i-1}T_i = Trans_{z_{i-1}} (d_i) \cdot Rot_{z_{i-1}} (\theta_i) \cdot Trans_{x_i}(r_i) \cdot Rot_{x_i} (\alpha_i)$

Dove

$Trans_{z_{i-1}} (d_i) = \begin{pmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d_n \\ 0 & 0 & 0 & 1 \end{pmatrix}$

$Rot_{z_{i-1}} (\theta_i) = \begin{pmatrix} \cos \theta_i & -\sin \theta_i & 0 & 0 \\ \sin \theta_i & \cos \theta_i & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{pmatrix}$

$Trans_{x_i} (r_i) = \begin{pmatrix} 1 & 0 & 0 & r_i \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{pmatrix}$

$Rot_{x_i} (\alpha_i) = \begin{pmatrix} 1 & 0 & 0 & 0 \\ 0 & \cos \alpha_i & -\sin \alpha_i & 0 \\ 0 & \sin \alpha_i & \cos \alpha_i & 0 \\ 0 & 0 & 0 & 1 \\ \end{pmatrix}$

Da qui si ricava la matrice di trasformazione completa:

${}^{i-1}T_i = \begin{pmatrix} \cos \theta_i & -\sin \theta_i \cos \alpha_i & \sin \theta_i \sin \alpha_i & r_i \cos \theta_i \\ \sin \theta_i & \cos \theta_i \cos \alpha_i & -\cos \theta_i \sin \alpha_i & r_i \sin \theta_i \\ 0 & \sin \alpha_i & \cos \alpha_i & d_i \\ 0 & 0 & 0 & 1 \\ \end{pmatrix}$

Adesso che conosciamo la matrice di trasformazione delle coordinate per ogni singolo elemento base link-giunto, è possibile applicare la dinamica diretta su qualsiasi tipo di manipolatore a catena. E’ sufficiente scomporre il manipolatore, per quanto complesso, nei suoi elementi base, calcolarsi le matrici di trasformazione per ciascun elemento, e poi moltiplicarli per ottenere una matrice generale.

Vediamo alcuni esempi di manipolatori più complessi.

Manipolatore planare a 3 bracci

Il manipolatore a tre bracci è formato da tre elementi base, i cui giunti ruotano tutti sullo stesso piano.

Per prima cosa quindi si definiscono i quattro parametri di Denavit-Hartenberg per ciascun elemento base e si riportano in una tabella.

Elemento base	$a_i$	$\alpha_i$	$d_i$	$\theta_i$
1	$a_1$	$0$	$0$	$\theta_1$
2	$a_2$	$0$	$0$	$\theta_2$
3	$a_3$	$0$	$0$	$\theta_3$

Dalla tabella degli elementi base si può vedere che le matrici di trasformazione saranno identiche:

$T_i^{i-1} = \begin{bmatrix} cos \theta_i & -sin \theta_i & 0 & a_i cos \theta_i \\ sin \theta_i & cos \theta_i & 0 & a_i sin \theta_i \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} i = 1,2,3$

e quindi la trasformazione generale sarà:

$T_3^0 = T_1^0 T_2^1 T_3^2$

$T_3^0 = \begin{bmatrix} c_{123} & -s_{123} & 0 & a_1 c_1 + a_2 c_{12} + a_3 c_{123} \\ s_{123} & c_{123} & 0 & a_1 s_1 + a_2 s_{12} + a_3 s_{123} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

Manipolatore sferico

Anche per il manipolatore sferico è possibile creare una tabella con i quattro parametri di Denavit-Hartenberg per ciascun elemento base. Anche qui abbiamo 3 elementi base, ma questa volta le matrici di trasformazione saranno diverse per ciascun elemento base.

Elemento base	$a_i$	$\alpha_i$	$d_i$	$\theta_i$
1	$0$	$-\pi/2$	$0$	$\theta_1$
2	$0$	$\pi/2$	$d_2$	$\theta_2$
3	$0$	$0$	$d_3$	$0$

Le tre matrici di trasformazione delle coordinate corrispondenti ai singoli elementi saranno quindi le seguenti:

$T_1^0 = \begin{bmatrix} c_1 & 0 & -s_1 & 0 \\ s_1 & 0 & c_1 & 0 \\ 0 & -1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

$T_2^1 = \begin{bmatrix} c_2 & 0 & s_2 & 0 \\ s_2 & 0 & -c_2 & 0 \\ 0 & 1 & 0 & d_2 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

$T_3^2 = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d_3 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

E quindi la matrice di trasformazione generale sarà:

$T_3^0 = T_1^0 T_2^1 T_3^2$

$T_3^0 = \begin{bmatrix} c_1 c_2 & -s_1 & c_1 s_2 & c_1 s_2 d_3 - s_1 d_2 \\ s_1 c_2 & c_1 & s_1 s_2 & s_1 s_2 d_3 + c_1 d_2 \\ -s_2 & 0 & c_2 & c_2 d_3 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

Manipolatore antropomorfo

Anche per manipolatori complessi come quello antropomorfo, l’approccio rimane pressochè lo stesso. Anche qui si definiscono tre elementi basi e la seguente tabella con le corrispondenti proprietà di Denavit-Hartenberg.

Elemento base	$a_i$	$\alpha_i$	$d_i$	$\theta_i$
1	$0$	$-\pi/2$	$0$	$\theta_1$
2	$a_2$	$0$	$0$	$\theta_2$
3	$a_3$	$0$	$0$	$\theta_3$

Dalla tabella si ricavano le tre matrici di trasformazione delle coordinate (due sono uguali).

$T_1^0 = \begin{bmatrix} c_1 & 0 & s_1 & 0 \\ s_1 & 0 & -c_1 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

$T_i^{i-1} = \begin{bmatrix} c_i & -s_i & 0 & a_i c_i \\ s_i & c_i & 0 & a_i s_i \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} i = 2,3$

Dalle tre matrici di trasformazione degli elementi base si ricava quella generale:

$T_3^0 = T_1^0 T_2^1 T_3^2$

$T_3^0 = \begin{bmatrix} c_1 c_{23} & -c_1 s_{23} & s_1 & c_1 ( a_2 c_2 - a_3 c_{23}) \\ s_1 c_{23} & -s_1 s_{23} & -c1 & s_1 ( a_2 c_2 + a_3 c_{23} \\ s_{23} & c_{23} & 0 & a_2 s_2 + a_3 s_{23} \\ 0 & 0 & 0 & 1 \end{bmatrix}$

Polso Sferico

Un altro manipolatore molto utilizzato è il polso sferico, spesso collegato come estremità efficace ad altri manipolatori.

Elemento base	$a_i$	$\alpha_i$	$d_i$	$\theta_i$
1	$0$	$-\pi/2$	$0$	$\theta_1$
2	$0$	$\pi/2$	$0$	$\theta_2$
3	$0$	$0$	$d_3/latex]</td><td>[latex]\theta_3$

Dalla tabella si ricavano le tre matrici di trasformazione delle coordinate (due sono uguali).

$T_1^0 = \begin{bmatrix} c_1 & 0 & -s_1 & 0 \\ s_1 & 0 & c_1 & 0 \\ 0 & -1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

$T_2^1 = \begin{bmatrix} c_2 & 0 & s_2 & 0 \\ s_2 & 0 & -c_2 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

$T_3^2 = \begin{bmatrix} c_3 & 0 & -s_3 & 0 \\ s_3 & 0 & c_3 & 0 \\ 0 & 0 & -1 & d_3 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

Dalle tre matrici di trasformazione degli elementi base si ricava quella generale:

$T_3^0 = T_1^0 T_2^1 T_3^2$

$T_3^0 = \begin{bmatrix} c_1 c_2 c_ - s_1 s_3 & -c_1 c_2 s_3 - s_1 c_3 & c_1 s_2 & c_2 s_2 d_3 \\ s_1 c_2 c_3 + c_1 s_3 & -s_1 c_2 s_3 + c_1 c_3 & s_1 s_2 & s_1 s_2 d_3 \\ - s_2 c_3 & s_2 s_3 & c_2 & c_2 d_3 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

Cinematica Diretta (Forward Kinematics)

Il manipolatore a 2 link e 2 giunti

Casi più complessi – la procedura di Denavit-Hartenberg

L’elemento base: link-giunto

Trasformazione di coordinate

Manipolatore planare a 3 bracci

Manipolatore sferico

Manipolatore antropomorfo

Polso Sferico

Lascia un commento Annulla risposta