1

Tesina di Robotica Industriale

A.A. 2008/2009 Professore Giorgio Bartolini

Professore Alessandro Pisano Diee

università degli studi di Cagliari

Studio del “Educational Robot Kit Bioloid”

Davide Nessi, Francesco Vacca Ing. Elettronica

2

INTRODUZIONE In questo elaborato ci proponiamo di esporre l’esperienza acquisita durante l’attività di laboratorio. Scopo di tale attività è stato quello di studiare il robot umanoide (bioloid) col fine ultimo di poter suonare una “finger drums”, interfacciando il robot con Matlab. È stata studiata la cinematica diretta ed inversa soffermando l’attenzione allo studio delle due braccia.

Apparecchiatura utilizzata nell’esperimento:

- Robotis Educational Kit “Bioloid” - Finger Drums (batteria micro a dita”) - Matlab R2009a - Adattatore usb/seriale rs232 per comunicazione col pc

Finger Drums

3

- Bacchette da batterista e relativo supporto per installazione nel

kit bioloid con relative dimensioni proporzionate al robot

Bacchette ‘home made ’e relativo supporto di fissaggio all’end effector

4

DATI TECNICI

Entriamo nel dettaglio e analizziamo gli aspetti tecnici del Kit Bioloid: Il Kit Bioloid è un umanoide formato da un’unità di elaborazione centrale chiamata CM5, da 18 servo motori in DC,e l’unità AX-S1con sensori di suono,luce,distanza.

Rappresentazione CAD Robot Bioloid

Posizione dei giunti del Kit Bioloid

5

Dimensioni reali dei singoli elementi del Biolid (misure espresse in mm):

6

CINEMATICA DIRETTA Posizioniamo le terne di riferimento in ogni giunto delle due braccia per poter calcolare la cinematica diretta. Schematizziamo il robot ed applicando le dimensioni reali dei singoli componenti:

Il singolo braccio è assimilabile ad un manipolatore RBB. A seguire le coordinate dei centri delle terne che in Simmechanics sono stati utilizzati per modellarlo: %braccio destro CS1=[0 53 0]; CS2=[0 76 0]; CG1=[0 63.64 0]; CS3=[0 76 0]; CS4=[0 144 0]; CG2=[0 98.58 0]; CS5=[0 144 0]; CS6=[0 244 0]; CG3=[0 167.96 0]; %braccio sinistro CS11=[0 -53 0]; CS21=[0 -76 0]; CG11=[0 -63.64 0];

7

CS31=[0 -76 0]; CS41=[0 -144 0]; CG21=[0 -98.58 0]; CS51=[0 -144 0]; CS61=[0 -244 0]; CG31=[0 -167.96 0];

Le rispettive matrici di rototraslazione sono: Indichiamo come

€

Aii−1 la matrice di trasformazione omogenea che ci

consente di passare dalla rappresentazione della terna

€

i −1 alla terna

€

i.

€

A10 =

C1 0 S1 00 1 0 d1−S1 0 C1 00 0 0 1

A21 =

1 0 0 00 C2 −S2 d2*C20 S2 C2 d2* S20 0 0 1

A10 * A2

1 = A20 =

C1 S1* S2 S1*C2 d2* S1* S20 C2 −S2 d2*C2 + d1−S1 S2*C1 C1*C2 d2* S2*C10 0 0 1

A32 =

1 0 0 00 C3 −S3 d3*C30 S3 C3 d3* S30 0 0 1

A20 * A3

2 = A30 =

C1 C3* S2* S1+ S3* S1*C2 C3* S1*C2 − S3* S2* S1 (d3*C3+ d2)* S2* S1+ d3* S3* S1*C20 C3*C2 − S3* S2 −C3* S2 − S3*C2 −d3* S3* S2 + (d3*C3+ d2)*C2 + d1−S1 C3* S2*C1+ S3*C1*C2 C3*C1*C2 + S3* S2*C1 (d3*C3+ d2)* S2*C1+ d3* S3*C1*C20 0 0 1

Matrici di trasformazione omogenee con Ci =cos(ϑ i) e Si =sin(ϑ i). I parametri d1=76mm,d2=68mm,d3=100mm rappresentano le lunghezze

dei rispettivi Body del braccio del robot

MODELLO SIMMECHANICS: il modello completo per le braccia del robot è stato creato con l’utilizzo del toolbox di matlab simmechanics.Per semplicità ci limitiamo a descrivere il solo braccio destro (guardando frontalmente il robot),

8

ma durante la simulazione è stato considerato anche il braccio sinistro cambiando opportunamente i valori di posizione delle terne di riferimento.

Modello Simmechanics per il braccio destro del Bioloid

Simulazione delle braccia del robot, si notino i baricentri dei singoli Body.

Con i valori trovati analiticamente scriviamo una funzione matlab mostrata in appendice la quale prende in ingresso dei valori costanti (angolo,velocità,accelerazione) assegnandoli a ciascun “Joint actuator” che porterà il braccio in un determinato punto dello spazio.

9

Ovviamente se i calcoli sono corretti avremo un riscontro con i dati ricavati durante la simulazione. Ecco un esempio:

Calcolo della cinematica diretta con la matrice di trasformazione omogenea. Nei display è visualizzata la posizione dell’organo terminale

una volta fissati tre angoli.

Posizione dell’organo terminale ricavata dal modello Simmechanics (uscita Body sensor).

si può notare che i valori calcolati sono identici a quelli simulati.

10

CINEMATICA INVERSA Grazie all’algoritmo della cinematica inversa possiamo trovare i profili delle variabili di giunto in funzione del tempo che consentono di realizzare il movimento desiderato per l’organo terminale. Nel caso in esame l’organo terminale è l’estremo della “bacchetta” Utilizziamo, per il calcolo della cinematica inversa,l’algoritmo basato sullo Jacobiano Analitico. Partiamo dalle coordinate dell’organo terminale (per semplicità riportiamo i calcoli effettuati per il braccio destro ):

€

( f1)( f2)( f3)

=

(d3*C3+ d3)* S2* S1+ d3* S3* S1*C2−d3* S3* S2 + (d3*C3+ d2)*C2 + d1(d3*C3+ d2)* S2*C1+ d3* S3*C1*C2

mentre lo Jacobiano analitico Ja risulterà:

€

Ja =

Ja11 Ja12 Ja13Ja21 Ja22 Ja23Ja31 Ja32 Ja33

=

∂f1∂ϑ1

∂f1∂ϑ 2

∂f1∂ϑ 3

∂f2∂ϑ1

∂f2∂ϑ 2

∂f2∂ϑ 3

∂f3∂ϑ1

∂f3∂ϑ 2

∂f3∂ϑ 3

Derivando otteniamo: Ja11=d3*c3*c1*s2 +d3*s3*c1*c2 + d2*c1*s2; Ja12=d3*c3*s1*c2-d3*s3*s1*s2+d2*s1*c2; Ja13=-d3*s3*s1*s2+d3*c3*s1*c2; Ja21=0; Ja22=-d2*s2-d3*s3*c2-d3*c3*s2; Ja23=-d3*c3*s2-d3*s3*c2; Ja31=-d3*c3*s1*s2-d3*s3*s1*c2-d2*s2*s1; Ja32=d3*c3*c1*c2-d3*s3*c1*s2+d2*c2*c1; Ja33=-d3*s3*c1*s2+d3*c3*c1*c2; Dove: d1=76mm d2=68mm d3=100mm

11

Il problema dell'inversione cinematica consiste nel determinare la posizione angolare dei giunti di un manipolatore, una volta assegnati la posizione e/o l'orientamento dell'organo terminale. Utilizzeremo il seguente algoritmo per l’inversione cinematica:

Algoritmo per l’inversione cinematica.

-

€

˙ x d , xd rappresentano velocità e posizione desiderata dell’organo terminale -

€

x traiettoria dell'organo terminale calcolata con la cinematica diretta;

-

€

e = xd − x, errore nello spazio operativo;

-

€

q, ˙ q posizione e velocità dei giunti; vettori di dimensione

€

n pari al numero dei giunti;

-

€

k(q) funzione della cinematica diretta. Utilizzando simulink realizziamo il seguente schema di inversione cinematica:

12

Blocco di cinematica inversa

Dove il blocco cinematica inversa contiene un mfile di matlab riportato in appendice. L’integratore posto a valle del blocco Cinematica inversa contiene delle condizioni iniziali tali per cui possiamo garantire un certo margine dalle posizioni di singolarità del nostro braccio.Il blocco di guadagno K è una matrice scelta come segue:

€

K = 300*1 0 00 1 00 0 1

Lo schema Simmechanics così realizzato sarà del tipo:

Modello Simmechanics per l’inversione cinematica.Il Subsystem in basso comprende l’algoritmo di inversione cinematica

Quindi data una traiettoria desiderata del nostro organo terminale, saranno calcolati gli angoli necessari per ottenere,con un minimo errore, tale posizione.Si noti dalla figura che gli ingressi al Subsytem di cinematica inversa saranno le coordinate desiderate dell’End Effector.

13

Modello per i Subsystem 1,2,3. Il Joint Actuator è in modalità Motion Actuator.

Abbiamo studiato due funzioni (una per il braccio sinistro ed una per il braccio destro) della quota Z (con x e y costanti) per realizzare un determinato ritmo.In particolare le due funzioni cercano di riprodurre il ritmo della canzone “we will rock you” dei Queen.

Traiettoria Braccio sinistro

Traiettoria Braccio Destro

14

Traiettoria desiderata Traiettoria Effettiva

Dal blocco di cinematica inversa sono stati generati degli angoli inviati al workspace in forma matriciale che verranno utilizzati con una opportuna mappatura in 10 bit ( valori da 0 a 1023) per determinare gli opportuni comandi di posizione e velocità che verranno trasmessi al robot durante le prove sperimentali.La simulazione conferma che l’errore sugli angoli tende a zero.

Curva gialla: errore di inseguimento del primo giunto (spalla).Curva viola: errore di inseguimento secondo giunto(braccio).Curva celeste:

errore inseguimento terzo giunto(avambraccio)

15

Cinematica inversa semplificata Nel caso in cui si abbiano due vincoli sulle variabili di giunto è possibile calcolare la cinematica inversa senza ricorrere allo studio dello Jacobiano inverso. In questo modo il carico computazionale si riduce. In questo caso la matrice

€

A30 assume la seguente forma:

€

A30 =

Rk1* s1k3

k2*c10 0 0 1

dove: k1=(d3*c3+d2)*s2+d3*s3*c2; k2=(d3*c3+d2)*s2+d3*s3*c2; Sono costanti. gli angoli per i giunti del braccio e dell’avambraccio sono fissati leggendo i valori dell’encoder dei motori quando il robot è di fronte alla batteria: ϑ2=-98.4° ϑ3=-46.2°

Dividendo il primo elemento dell’ultima colonna per il terzo elemento dell’ultima colonna si ottiene la seguente relazione tra le coordinate X e Z dell’organo terminale:

€

XZ

=k1k2tg(ϑ1)

X e Z sono legati dalla seguente relazione:

€

x = r2 − z2

Dove r è la distanza tra il giunto della spalla e l’organo terminale pari a 26cm. A questo punto è possibile invertire la relazione:

€

ϑ1 = arctan2 r2 − z2

zk2k1

16

Nella figura seguente è mostrato il modello simulink nel quale viene convertita la quota Z desiderata nell’angolo teta1.

Questi angoli sono inviati al workspace di matlab in un array. Essi sono espressi in radianti quindi per essere inviati al robot occorre convertirli. In generale vale la seguente formula di conversione

€

ϑ robot =ϑ deg

0.3+ϑ offset,i

dove:

•

€

ϑ robot è l’angolo espresso in valori dell’encoder (0-1023, valori interi).

• 0.3 è la sensibilità dell’encoder .

•

€

ϑ offset,i parametro che dipende dal servo che si sta utilizzando. In

particolare

€

ϑ offset,2 = 800 .

•

€

ϑ deg è l’angolo in gradi relativo al modello simmechanics. I valori di

€

ϑ robot inseriti nell’array del workspace andrebbero letti uno alla volta e attraverso un ciclo “for” inviati sequenzialmente al robot. Il problema riscontrato è stato nell’inviare questo tipo di stringa. Una possibile soluzione è mostrata in questo codice fprintf(s,'cid 2'); for i=1 : 101 posizione =char(teta1(i)); fprintf(s,'go %s 100 ',posizione); i=i+30; pause(0.5); end ! Tuttavia il robot non ha eseguito questo codice!

17

function out=calcolo_teta1(input) z=input(1); teta2=-98.4*(pi/180) teta3=-46.2*(pi/180) c2=cos(teta2); s2=sin(teta2); c3=cos(teta3); s3=sin(teta3); r=260; k1=(100*c3+68)*s2+100*s3*c2; k2=(100*c3+68)*s2+100*s3*c2; teta1=atan2(sqrt(r^2-z^2)*k2,z*k1); teta1=teta1*(180/pi); out=teta1; ! ! Comunicazione Kit Bioloid con MATLAB R2009A Dinamixel Ax-12+ Servo-Attuatori I Servo-Attuatori sono i “muscoli” dell’umanoide. Le caratteristiche principali sono:

- Motori DC con controllore e funzioni di rete. - 1Mbps di velocità di comunicazione - Feedback di Posizione (0-300° max), Velocità,corrente

DC,Voltaggio e temperatura - Sistema di protezione alta temperatura/voltaggio - Coppie di trazione rilevanti

Microcontrollore CM5 Il CM5 è il controllore principale dell’umanoide. È caratterizzato da un microprocessore Atmega128, che può trasmettere e ricevere dati dai servo e, attraverso connessione rs232, dal pc.

18

Comunicazione con Matlab: per comunicare con il robot attraverso Matlab è stata utilizzata la funzione “serial” che consente di inviare pacchetti di dati al cm5.Implementeremo dei programmi che realizzano diversi “ritmi”. La sintassi della funzione serial è la seguente: obj = serial('port') obj = serial('port','PropertyName',PropertyValue,...) Nel nostro caso abbiamo utilizzato: s=serial('COM1','BaudRate',57600,'Terminator','CR/LF'); !fopen(s); Il baudrate rappresenta la velocità di comunicazione tra la porta seriale Rs232 3 il controllore Cm5.La stringa 'Terminator','CR/LF' abilita la funzione Read&Write nella porta seriale.

Schema di comunicazione Seriale Pc-Umanoide

Il Kit Bioloid ha un insieme di funzioni inviabili al cm5 che consentono di attuare i servomotori e ricevere informazioni quali posizione del servo,temperatura,tensioni,correnti,etc... Utilizzando il software“Robot Terminal” fornito nel cd di supporto del Kit bioloid,visualizziamo le funzioni che potranno essere inviate direttamente da matlab:

- help: elenca tutte le funzioni disponibili del cm5(vedi figura)

19

Funzione Help.

- CID: é l’abbreviazione di Control ID. permette di cambiare l’ID del motore Dinamixel verso il quale vengono inviate le istruzioni di comando

esempio:

- Dump: crea una tabella dei valori caratteristici per ogni singolo attuatore.Esempio:

20

- GO: questo comando muove il dinamixel in una specifica posizione

ad una determinata velocità.Esempio:

questo esempio sposta in posizione 100 a velocità 80 il servo-attuatore. I valori di posizione e velocità vanno da 0 a 1024

Attraverso l’utilizzo di questa serie di comandi potremo,in accordo con i valori degli angoli restituiti dalla cinematica inversa,eseguire delle traiettorie tramite matlab. Appendice: Procedura di avvio per eseguire un generico ritmo:

• Collegare l’adattatore seriale-usb. • Collegare il cavo seriale del robot all’adattatore usb • Accendere il robot • Avviare Matlab R2009

21

• Usare il comando “inits.m” per avviare la comunicazione con matlab tramite seriale. Il file inits.m è il seguente: s=serial('COM1','BaudRate',57600,'Terminator','CR/LF'); fopen(s);

• Avviare gli m-files contenenti le varie istruzioni per eseguire il ritmo.

Cinematica diretta braccio Destro e Sinistro: function out=cin_diretta_bioloid_DESTRO(input) teta1=input(1); teta2=input(2); teta3=input(3); c1=cos(teta1); s1=sin(teta1); c2=cos(teta2); s2=sin(teta2); c3=cos(teta3); s3=sin(teta3); A10=[c1 0 s1 0;0 1 0 76;-s1 0 c1 0;0 0 0 1]; A21=[1 0 0 0; 0 c2 -s2 68*c2; 0 s2 c2 68*s2; 0 0 0 1]; A32=[1 0 0 0; 0 c3 -s3 100*c3; 0 s3 c3 100*s3; 0 0 0 1]; A43=[1 0 0 70;0 1 0 100;0 0 1 -70;0 0 0 1]; A40=A10*A21*A32*A43; out(1)=A40(1,4); out(2)=A40(2,4); out(3)=A40(3,4); function out=cin_diretta_bioloid_SINISTRO(input) teta4=input(1); teta5=input(2);

22

teta6=input(3); c4=cos(teta4); s4=sin(teta4); c5=cos(teta5); s5=sin(teta5); c6=cos(teta6); s6=sin(teta6); B10=[c4 0 s4 0;0 1 0 -76;-s4 0 c4 0;0 0 0 1]; B21=[1 0 0 0; 0 c5 -s5 -68*c5; 0 s5 c5 -68*s5; 0 0 0 1]; B32=[1 0 0 0; 0 c6 -s6 -100*c6; 0 s6 c6 -100*s6; 0 0 0 1]; B43=[1 0 0 70;0 1 0 -100; 0 0 1 -70;0 0 0 1]; B40=B10*B21*B32*B43; out(1)=B40(1,4); out(2)=B40(2,4); out(3)=B40(3,4); Cinematica Inversa: Jacobiano inverso function out=jacobiano_inverso(in) theta1= in(1); theta2= in(2); theta3= in(3); c1=cos(theta1); c2=cos(theta2); c3=cos(theta3); s1=sin(theta1); s2=sin(theta2); s3=sin(theta3); eta=[in(4) in(5) in(6)]'; Ja11=100*c3*c1*s2 +100*s3*c1*c2 + 68*c1*s2; Ja12=100*c3*s1*c2-100*s3*s1*s2+68*s1*c2; Ja13=-100*s3*s1*s2+100*c3*s1*c2; Ja21=0; Ja22=-68*s2-100*s3*c2-100*c3*s2; Ja23=-100*c3*s2-100*s3*c2; Ja31=-100*c3*s1*s2-100*s3*s1*c2-68*s2*s1; Ja32=100*c3*c1*c2-100*s3*c1*s2+68*c2*c1; Ja33=-100*s3*c1*s2+100*c3*c1*c2; Ja=[Ja11 Ja12 Ja13; Ja21 Ja22 Ja23; Ja31 Ja32 Ja33]; Ja_inv=inv(Ja); out=Ja_inv*eta; Inizializzazione porta seriale Matlab: s=serial('COM1','BaudRate',57600,'Terminator','CR/LF'); fopen(s); Quindi sono state scritte un insieme di istruzioni che permettono un movimento coordinato delle braccia per poter suonare la finger drums. Useremo la funzione fprintf(s,'stringa’) per inviare comandi al cm5. Es: movimento 1: for i= 1 : 3

23

fprintf(s,'cid 1'); fprintf(s,'go 500 100'); pause(0.5); fprintf(s,'cid 2'); fprintf(s,'go 500 100'); pause(0.5) fprintf(s,'cid 1'); fprintf(s,'go 430 100'); pause(0.5) fprintf(s,'go 480 100'); pause(0.5) fprintf(s,'cid 2') fprintf(s,'go 590 100'); pause(0.5); end for i= 1 : 3 fprintf(s,'cid 1'); fprintf(s,'go 500 500'); pause(0.3); fprintf(s,'cid 2'); fprintf(s,'go 500 500'); pause(0.3) fprintf(s,'cid 1'); fprintf(s,'go 430 500'); pause(0.3) fprintf(s,'go 480 500'); pause(0.3) fprintf(s,'go 430 500'); pause(0.3) fprintf(s,'go 480 500'); pause(0.3) fprintf(s,'cid 2') fprintf(s,'go 590 500'); pause(0.3); end. Esempio movimento 2: for i= 1 : 52 fprintf(s,'cid 1'); fprintf(s,'go 450 100'); pause(0.175); fprintf(s,'cid 2'); fprintf(s,'go 550 100'); pause(0.175) fprintf(s,'cid 1'); fprintf(s,'go 430 100'); pause(0.175) fprintf(s,'go 450 100'); pause(0.175) fprintf(s,'go 430 100'); pause(0.175) fprintf(s,'go 450 100'); pause(0.175) fprintf(s,'cid 2') fprintf(s,'go 570 100'); pause(0.175); end ! !Esempio movimento 3: for i= 1 : 5 fprintf(s,'cid 1'); fprintf(s,'go 450 300'); pause(0.1); fprintf(s,'go 430 300');

24

pause(0.1); fprintf(s,'cid 2'); fprintf(s,'go 550 300'); pause(0.1); fprintf(s,'go 570 300'); pause(0.1) end for i= 1 : 5 fprintf(s,'cid 1'); fprintf(s,'go 450 1000'); pause(0.05); fprintf(s,'go 430 1000'); pause(0.05); fprintf(s,'cid 2'); fprintf(s,'go 550 1000'); pause(0.05); fprintf(s,'go 570 1000'); pause(0.05) end for i= 1 : 5 fprintf(s,'cid 1'); fprintf(s,'go 450 1000'); pause(0.02); fprintf(s,'go 430 1000'); pause(0.02); fprintf(s,'cid 2'); fprintf(s,'go 550 1000'); pause(0.02); fprintf(s,'go 570 1000'); pause(0.02) end fprintf(s,'cid 2') fprintf(s,'go 500 1000') pause(0.2); fprintf(s,'cid 4') fprintf(s,'go 700 1000') pause(0.2); fprintf(s,'cid 2') fprintf(s,'go 520 1000') pause(0.2); fprintf(s,'go 500 1000') ! Troubleshooting I problemi riscontrati sono stati:

• Ricerca di una mini batteria di dimensioni proporzionali al robot. Quella utilizzata era fuori scala rispetto al robot.

• Costruzione bacchette in legno di dimensioni proporzionali al robot e relativo fissaggio all’organo terminale

• Eccessive vibrazioni del robot durante l’esecuzione le quali causavano spostamenti delle bacchette con conseguente compromissione del suono al contatto con la batteria.

• La batteria per produrre il suono richiede connessione a massa della bacchetta. Il problema è stato ovviato inserendo un sottile strato di alluminio nella bacchetta e collegando a massa il conatto.

• Problemi relativi alla connessione seriale con il pc risolti atttravverso l’utilizzo di un convertitore seriale-usb con opportuni drivers per Windows Xp.