Robotica - inversione cinematica e pianificazione della traiettoria di un manipolatore sferico

Indice

1. Introduzione
2. Cinematica Diretta
   1. Denavit-Hartenberg
   2. Rappresentazione minima
3. Inversione Cinematica
   1. Cinematica Differenziale
4. Pianificazione della Traiettoria

Capitolo 1

Introduzione

Figura 1.1. Il manipolatore sferico in esame

Il compito che ci è stato richiesto di svolgere è quello di implementare un algoritmo di inversione cinematica lungo il segmento congiungente i punti p_i (iniziale) e p_f (finale) di coordinate:

$$ p_i = \left[ \sqrt{\frac{3}{2}} \quad 1 \quad \sqrt{\frac{3}{4}} \right]^T m $$

$$ p_f = \left[ -\sqrt{\frac{5}{2}} \quad \sqrt{\frac{2}{2}} \quad 0.1 \right]^T m $$

assumendo una posa iniziale q(0) = [0 \quad \frac{\pi}{4} \quad 0.5]^T e un tempo di esecuzione della traiettoria t_f = 4 s, per un manipolatore sferico a tre gradi di mobilità come in figura 1.1.

In particolare l’algoritmo deve essere realizzato in Matlab, utilizzando la regola di integrazione di Eulero, considerando un intervallo di integrazione Δt = 2 ms.

Il manipolatore sferico, è costituito da 4 bracci (di cui uno che lo fissa a terra) connessi tramite 3 giunti: i primi due sono giunti di rotazione mentre l’ultimo è un giunto di traslazione. All’estremità di

Cinematica Diretta

dalle quali otteniamo la matrice di trasformazione che definirà la cinematica diretta del nostro manipolatore:

dove q = [ ∅₁ ∅₂ d₃ ]^T.

La matrice T può essere vista come una come composta da una parte rotazionale ed una posizionale, in altri termini:

dove la matrice R(q) rappresenta la parte rotazionale ed il vettore p(q) quella posizionale. Da un’analisi di T si nota che sul nostro manipolatore la variabile di giunto d₃, del giunto prismatico, non influenza (giustamente) la parte rotazionale dell’organo terminale.

2.2 Rappresentazione minima

La matrice di rotazione R(q) presenta un certo grado di ridondanza, dato che i suoi 9 elementi non sono indipendenti a causa dei vincoli di ortogonalità. Si può ottenere una rappresentazione minima dell’orientamento rispetto a 3 parametri indipendenti tramite le rappresentazioni di Eulero (ZYZ) o Roll-Pitch-Yaw (ZYX).

Tramite la rappresentazione di Eulero è possibile descrivere la posa del nostro organo terminale con un vettore:

dove p_e (3 x 1) descrive la posizione dell’organo terminale e ∅_e il suo orientamento.

Lo spazio in cui è definito il vettore x_e è detto spazio operativo, ed è quello spazio rispetto al quale viene tipicamente specificata l’operazione richiesta al manipolatore, che non deve essere confuso con lo spazio dei giunti che è lo spazio in cui è definito il vettore delle variabili di giunto.

4 - Pianificazione della Traiettoria

Figura 4.3. Posizione desiderata e calcolata dell’organo terminale.

e per legare q^˙ ad e è possibile sfruttare il Metodo di Newton, ipotizzando J quadra e non singolare si giunge a:

q^˙ = J^-1(q)(x_d + Ke)

dove K è una matrice definita positiva, tipicamente diagonale che fa tendere l’errore a zero con una velocità dipendente dagli autovalori della matrice (più grandi sono più è veloce la convergenza). Infine, ricordando che nel nostro caso lo Jacobiano analitico coincide con quello geometrico possiamo calcolare ogni q in questo modo:

q(t_k+1) = q(t_k) + [J^-1(q(t_k))(x_d + Ke)q(t_k) Δt

Eseguendo l’algoritmo ci rendiamo conto di avere un problema di singolarità. Ciò risultava evidente anche dopo aver risolto la cinematica inversa sul punto finale p_f = [−^√2₂√2₂0.1]^T

dato che q_f = [π₄ 0 0.1]^T, si nota che la posa finale del manipolatore è proprio in una condizione di singolarità (Θ₂ = 0). La fig. 4.2 che mette in evidenza la posizione dell’organo terminale calcolata (x, solida nel grafico) con la posizione desiderata (x_d tratteggiata nel grafico) ci mostra che ad un certo punto vi è un “salto” nel movimento dei giunti, che ci porta a pensare di essere vicini ad una condizione di singolarità. La conferma arriva analizzando il grafico della velocità ai giunti e il grafico del movimento dei giunti (fig. 4.3), infatti si nota che quando il secondo giunto (Θ₂) arriva ad un angolo di zero radianti (che è una condizione di singolarità del nostro Jacobiano) la velocità al giunto 2 ha un picco.

Per risolvere il problema bisogna evitare il calcolo diretto dell’inversa dello Jacobiano, adottando un’inversione più robusta, secondo il metodo dei minimi quadrati smorzati:

J^-1 = J^T(J J^T + k I) >^-1

dove il fattore k produce uno smorzamento che ammorbiddisce l’inversione e migliora il comportamento a livello dei giunti. Agendo in questo modo siamo stati in grado di evitare il passaggio per una condizione di singolarità del nostro manipolatore, al costo però di non seguire più il percorso in linea retta che ci è stato assegnato (fig. 4.4).