Reporte Scara

1

Control de un Robot ,C-R'scamilla &ópe 9os/ Iván =elá#ue Tapia C>ristian ->med Universidad :olit/cnica de :ac>uca Dinámica " Control de Robots In). ;ario -lberto ;a)a?a ;ende  Resumen —El

robot SCARA (Selective Compliance Assembly Robot Arm) es un dispositivo equipado de libertad total en los ejes X y Y pero limitados severamente en su desplazamiento en el eje ! En el presente presente trabajo se realiza realiza el modelo matem"tico matem"tico para la obtenci#n de la cinem"tica directa y la cinem"tica inversa del Robot para de$inir las posiciones y el comportamiento de los movimientos de las articulaciones! El modelo din"mico a su vez de$in de$inee la veloc velocid idad ad del del robo robott por por medio medio de sus sus vari variab able less articulares! %odo lo anterior se realiza con el $in de aplicar y&un contr control ol ' ' y '* para revis revisar ar su compor comportam tamien iento to en los di$erentes controles!

I. I NTRODUCCIÓN a cinemática de un robot estudia el movimiento del mismo con respecto a un sistema de referencia es decir  se interesa por la descripción analtica del movimiento espa espaci cial al del del robo robott como como un unaa func funció iónn del del tiem tiempo po!! " en  particular por las relaciones entre la posición del efector final con los valores #ue toman sus coordenadas articulares $1%. '(isten dos problemas fundamentales para resolver en la cinemática del robot. 'l primero es el problema cinemático dire direct cto! o! " cons consis iste te en dete determ rmin inar ar cuál cuál es la po posi sici ción ón " orientación del e(tremo final del robot! con respecto a un sistem sistemaa de coo coorde rdenad nadas as #ue se toma toma como como refere referenci ncia! a! el se)undo! denominado problema cinemático inverso! resuelve la confi)uración #ue debe adoptar el robot para una posición " orientación del efector final conocida $*%.

&

Donde4  Ec  es la ener)ia cinetica.  E p  es la ener)ia potencial 5 en terminos del &a)ran)iano #ue dice #ue es la resta entre la ener)ia cinetica cinetica menos la ener)ia potencial potencial nos #ueda como a continuacion se describe4 1

2

 L=T − P= m v − mgh 2

62

's necesario necesario calcular calcular las torsiones torsiones de las articula articulacione cioness mediante la si)uiente ecuación4  D ( q ) ´q + C  ( ( q , q´ ) ´q + G+ F f  =τ  72 'n donde4  D es la matri de inercia C   es la matri de Coriolis G  es la )ravedad  F f  fuera de fricción Un punto importante en el desarrollo de estas ecuaciones es la obtención de la matri 8acobiana la cual nos describe la relación entre velocidades de las coordenadas de articulares " las de posición " orientación del e(tremo del robot. &a matri 9acobiana directa permite conocer las velocidades del e(tremo del robot a partir de los valores de las velocidades de cada arti articu cula laci ción ón.. :or :or su part parte! e! la matr matri i 9aco 9acobi bian anaa inve invers rsaa  permitirá conocer las velocidades articulares necesarias para obtener unas velocidades determinadas en el e(tremo del robot $*%.

II. ;OD'&O CIN';
+i). 1. Robot ,C-R- " sus articulaciones.

Una ve obtenidas las cinemáticas del robot se procede a obtener el modelo dinamico del mismo para lo cual se aplica el m/todo de 'uler0&a)ran)e el cual nos dice #ue la ener)a total es la suma de la ener)a cin/tica mas la ener)a potencial.  ET = Ec + E p 12  Ec =T  *2  E p= P 32

+i). *. Robot ,C-R-.

:ara la obtención de la cinemática directa se aplica el m/todo )eom/trico. :ara :ara lo cual cual se obtien obtienee una matri matri para para cada cada variab variable le articular como como se describe a continuación4

*

 ρ 1=

[]

β  = cos

@2

0

−1

*

*

*

3

1*2

*

l1

[ ]

γ  =

l2 C 1  ρ2= l 2 S1 l1

[ [

A2

l2 C 1+l 3 C 12  ρ 3= l2 S 1+l 3 S 12 l1

] ]

l2 C 1 + l 3 C 12  ρ4 = l2 S 1 + l 3 S12 l 1− q 3

B2

*

*

*

3

*

1BD − γ      l − z 

  162 Despues de encontrar la ecuación 162 se procede a encontrar la matri 8acobiana la cual permite localiar las  posiciones sin)ulares del robot. 'l cual #ueda definido por la si)uiente matri. 1

2

 −l S − l S   J = l C + l C   D *

III. ;OD'&O CIN';
 −l C l C + l C   D = *

 J 

+i). 3. Cinematica inversa del Robot ,C-R-.

Revisando la +i). 3 obtenemos los parámetros para calcular  112.   12 -si como para obtener α  #ueda de las si)uiente manera  y  α  = tan −1  ÷   x    112 5 aplicando tri)onometra podemos obtener para  β la si)uiente ecuación

1

1

3

*

−l S   −l C 

1*

1*

3

1*

3

1*

  D  −1 D

D

 

172

 5 el 8acobiano inverso está dado por4 3

x* + y*

−1

132 5 por Eltimo de 112!1*2!132 se obtiene la si)uiente el si)uiente vector para calcular las posiciones an)ulares del robot.  α − β 

*

=

cos

* 3

q=

De esta manera se obtienen las posiciones deseadas para cada eslabón.

d

 d + l − l     *l d  ÷       l + l − d     *l l  ÷      *

0

−1

1

1*

3

−l S   l S +l S   3

1*

*

1

1*

3

1*

D

  D  l l S   D

* 3

*

l*l3 S *

 

1@2

I=. ;OD'&O DIN<;ICO 'n esta sección se constru"e el modelo dinámico del robot ,C-R- el permite aplicar un control para observar el comportamiento del mismo. :rimero se parte de la definición del m/todo de 'uler descrito en 12!*2 " 32. De 62 se obtiene #ue al aplicarle la derivada temporal de la derivada del &a)ran)iano 1A2 con respecto a cada una de las variables articulares " aplicado al robot nos #ueda la definicon de la si)uiente manera4 d dL dL −  =T    1A2 dt  d q´ dq 5 al aplicarlo al eslabon 1 nos #ueda de la si)uiente manera

{ }

1

2

 L1= m v1 −m1 g h1=−m1 g l 1   2

1B2

1

[] 0

d v 1=

0

l1

dt 

 

12

3

[] 0

2

v 1=

T 

0

l

=0

 

(¿ ¿ 1−q )

*2

3

1

1

[

 L4= m v 4− m4 g h3 = m3 l 2 q´ 1 + 2 l 2 l3 C 2 ( q´ 1+ q´ 1 ´q2 ) + l3 ( q´ 1+ q´

0

2

2

De la misma manera se aplica para el eslabon * " nos arro8a4

2

4

2

2

2

2

*B2 1

1

2

2

2

 L2= m v 2−m 2 g h2= m 2 l 2 q´ 1−m 2 g l1   2

*12

2

2

v 2=

[ ]

l2 C 1 d l2 S 1 l1

[ ]

−l S  ´q = l C  q´ 2

1

2

dt 

v 4=  

1

1

v =l S q´ + l C  q´ =( S + C  ) ( l q´ ) =l ´q 2

2

2

2

2

2

2

2

2

2

2

2

1

1

2

1

1

1

1

2

1

2

1

2

2

v 3=

dt 

]

2

[

−l S  ´q −l S = l C   ´q + l C  2

1

1

1

1

3

3

2

12

]

( q´ + q´ ) ( q´ + q´ ) ( 25)

12

1

2

2

1

2

0

2

2

2

2

2

1

1

2

2

2

2

3

1

12

1

1

1

3

12

1

2

2 3

2

]

2

3

12

2

 LT =

1

 L ∑ = i

 

i

312

1

 L1=−m1 g l1   2

3*2

2

332

2

1

[

2

2

2

2

2

2

362

1

[

2

2

2

2

2

2

 Donde : S 1+ C 1=1 S 12=S 1 C 2 + C 1 S 2 C 12=C 1 C 2 + S 1 S2

]

372 2

2

5 de este modo se obtiene el la)ran)iano total del robot " #ueda de la si)uiente manera4 1

 LT =−( m1 + m 2 + m 3+ m 4 ) g l1 + m2 l 2 q´ 1 +

2

]

 L3= m3 l 2 q´ 1+2 l 2 l3 C 2 ( q´ 1 + q´ 1 ´q 2) + l 3 ( q´ 1 +´q2 ) −m3 g l1

 L4= m3 l 2 q´ 1 + 2 l2 l 3 C 2 ( ´q1 + q´ 1 ´q2 ) + l 3 ( q´ 1 + q´ 2 ) − m3 g ¿

2

2

2

2

2

3

*@2 2

1

(¿ ¿ 1−q )

+ l C  q´ + 2 l l C  C  ( q´ + q´  ´q ) + l C  ( q´ + q´ ) 2

2

2

l

v 3= l2 S 1 q´ 1 + 2 l 2 l3 S1 S 12 ( ´q1 + q´ 1 q´ 2 ) + l 3 S 12 ( q´ 1+ ´q2 ) 2

2

 L2= m2 l 2 q´ 1− m2 g l1  

:ara obtener v 23  es necesario reducir terminos para simplificar la ecuación " nos #ueda de la si)uiente manera4 2

1

4

2

1

2

12

Despu/s de obtener los la)ran)ianos de cada uno de los eslabones se procede a realiar la suma total de los la)ran)ianos correspondientes " nos #ueda de la si)uiente manera4

*62

[

3

*32

2

l2 C 1 + l3 C 12 d l 2 S1 + l3 S12 l1

1

2

 L3= m v 3−m 3 g h 3= m3 [ l 2 q´ 1 + 2 l 2 l 3 C 2 ( ´q 1+ q´ 1 ´q2 ) 3

1

32  

&o mismo aplica para el eslabon 3 " 6 los cuales se describen a continuacion

2

2

2

v 4=l 2 q´ 1 + 2 l2 l 3 C 2 ( q´ 1 + q´ 1 q´ 2 ) + l3 ( q´ 1 + q´ 2 ) + q´ 3

2

2

[

−l S q´ −l S ( q´ +´q ) = l C  q´ + l C  ( q´ + q´ ) −´q

*2 2

2

2

]

1

0

1

dt 

**2

2

1

[

l 2 C 1+ l3 C 12 d l 2 S 1+ l3 S 12 l 1−q 3

2

2

2

1

( m + m ) [ l q´ +2 l 2 3

4

2

2

2

1

2

l

3@2

-l cual de la misma manera se le aplica la derivada temporal de la derivada parcial de cada uno de las variables articulares. S 12+C 12=1 - continuacion se presenta para q´ 1 . 2 2 2 2 ´ ´ ´ ´ v 3= l2 q 1+ 2 l 2 l3 S 1 ( S 1 C 2+ C 1 S 2) ( q1 + q 1 q 2 ) + 2 l 2 l 3 C 1 ( C  ∂ L 2 2 2 = m2 l 2 q´ 1 + ( m 3 + m4 ) l 2 q´ 1 + l 2 l 3 C 2 ( 2 q´ 1 +´q 2 )+ l 3 ( q´ 1+ q´ 2 ) 2 2 2 2 2 2 v 3= l2 q´ 1+ 2 l 2 l3 ( S1 C 2 + C 1 S1 S 2+ C 1 C 2−C 1 S 1 S2 ) ( ´q1 + ∂ ´q1 3A2 Del metodo anterior se obtiene como resultado4 d  ∂ L 2 2 2 m2 l 2 ´q1 + ( m3+ m4 ) l 2 q´ 1−l 2 l3 S2 ( 2 q´ 1 ´q2 +´q 2 ) + l2 l 3 C 2 ( = 2 2 2 2 2 2 *A2 v 3= l2 q´ 1+ 2 l2 l 3 C 2 ( q´ 1+ q´ 1 ´q2 ) + l3 ( q´ 1+ q´ 2 )   dt  ∂ q´ 1 2

2

[

{

]

[

3B2

:ara v 4  se >ace lo mismo #ue en arrastra a q´ 32  al resultado

v 3  solo #ue se

∂L =0   ∂ q1 ,e realian las mismas operaciones para q´ 2  .

32

6

∂L =( m3 + m4 ) l 2 l3 C 2 q´ 1+ l23 ( q´ 1+ q´ 2 ) ∂ ´q2 q´

(

)

 

2

 

2

3

2

1

2

2

3

2

1

3

612

{ }

d ∂L =( m3 +m4 ) ¿ dt  ∂ q´ 2 ∂L =−( m3+ m4 ) −l 2 l 3 S 2 ( q´ 21 +´q 1 q´ 2 ) ∂ q2 5 tambien para q´ 3 ∂L =m4 ´q3   ∂ ´q3 d  ∂ L =−m4 ´q3   dt  ∂ q´ 3 ∂L =m 4 g   ∂ q2

(

0

 

−g m  F f = β ´q + K ∗ign( q´ )    D ( q ) ´q + C  ( q , q´ ) ´q + G ( q ) + F f = τ 

72

Despe8ando a

712  

7*2

q  de 7*2 nos #ueda4 −1 q = D [ τ − C −G− F f  ]

73 'n la si)uiente seccion se muestra la aplicacion de todas estas ecuaciones

)

 

6*2

632

{

662 672

Despues de realiar lo anterior se procede a calcular las torsiones #eu se presentaran el es robot. 5 las operaciones #uedan de la si)uiene manera4

 D ( q ) ´q + C  ( q , q´ ) ´q + G + F f  =τ   

[

G=

4

¿ (¿ 1+ q´ ¿ ) −l l S  ´q  ´q + l l C  q´ + l ¿ 2

[ ] 0

62

6@2

d T   D =C + C    dt   D =¿

6A2

=. R',U&T-DO, 'n esta sección se procede a llevar a cabo el control de nuestro robot mediante la a"uda de ,CI&-F #ue es una >erramienta computacional #ue nos permite visualiar cómo se comporta el robot " cambios presenta al aplicar un control :roporcional! Derivativo Inte)ral :ID2. &o primero #ue se realia en el pro)rama es obtener las cinemáticas directa e inversa del robot en diferentes scinotes  par despu/s 8untar todo en un mis arc>ivo " poder ver el comportamiento final. Despues se obtiene el 8acobiano de i)ual manera con la a"uda de ,CI&-F "a #e se tienen los tres pro)ramas se procede a realiar una interfa #ue permita obtener los parámetros del robot el cual mediante funciones " un toolbo( llamado GUI Fuilder se puede obtener un )ráfico como el de la +i). 6

( m +m +m ) l +2 ( m + m ) l l C  +( m + m ) l ( m + ( m + m ) ( l l C  + l ) ( 2

2

3

4

2

2

3

4

2

3

2

3

4

3

3

2

3

4

2

3

2

3

0

[

−2 (m 3+ m4 ) l2 l 3 S 2 ´q2 −( m3 +m4 ) l 2 l 3 S 2 q´ 2 dD = −(m3 +m4 ) l 2 l3 S2 ´q2 0 dt  0

0

¿ −( m + m ) l ¿ ¿ 2 l S  ´q ¿ −( m + m ) l l S ( q´ + q´ ) ¿ 3

3

3

4

2

( m +m ) l 3

4

0 0

6B2

4

2

3

2

2

1

0

¿

0

l S 1 ´q1

2 3

¿ C =[ 0 ¿ 0 ¿ 0 ¿ 0 ¿ 0 ¿ ]

2

 62

+i). 6. Robot ,C-R- simulado

Con los sldiers de la parte superior derec>a se pueden manipular el movimiento de los eslabones as como tambi/n el del efector final. Despu/s de ver #ue el robot cumple las caractersticas se  procede a colocar una tra"ectoria para #ue el robot pueda responder a un control de forma :roporcional. 'l cual está definido por una diferencia del error en el punto deseado " error medido el cual está definido por las si)uientes lneas de códi)o #ue describen esta acción. Error =qd-q_qp_0( 1:3,1 ) T =diag( kp )*Error 

7 &a simulación del robot se puede observar a continuación en la +i). 7

+i). 7 Robot ,C-R- control :

:ara poder observar me8or el comportamiento del robot al incorporarle un control :ID se realia la si)uiente interfa en donde se muestra las )raficas de la posición " la velocidad del robot las cuales describen el comportamiento real color aul2 " el deseado color ne)ro2. 'l cual se describe en la +i). @.

Como se puede observar en la +i). @ se muestra claramente como e(isten unas pe#ue?as variaciones en cuanto a la  posición deseada pero el funcionamiento del control es mu" optimo " se puede decir #ue si cumple con el ob8etivo de se)uir a la se?al. =I. CONC&U,ION', 'ste traba8o es de suma importancia "a #ue mediante los conocimientos ad#uiridos en cinemática de robots podemos obtener la cinemática directa e inversa del sistema! se  presentaron al)unos inconvenientes a la >ora de desarrollar las matrices debido #ue al aumentar las variables articulares el )rado de comple8idad para resolver el sistema aumenta por tal raón su utilio la >erramienta ,CI&-F para poder reducir  esos errores " observar me8or como es el comportamiento lo cual se lo)ró satisfactoriamente. Tambi/n podemos concluir #ue esta práctica nos sirve para reafirmar los conocimientos ad#uiridos. &os modelos dinámicos se puede usar para la simulación de al)oritmos de control del robot as como para la implementación de distintas estrate)ias de control.

=II.FIF&IOGR-+H$1%tesis.ipn.m(8spuibitstream1*367@AB7*31DI,'NO5CON ,FR-JO.pdf  $*% +undamentos de Robótica! -ntonio Farrientos. +i). @ Robot ,C-R- control :ID