Unidad Profesional Interdisciplinaria en Ingenierıa y Tecnologıas Avanzadas

Implementacion de un control par calculado para un robot RR y un RRRutilizando Matlab y Working Model

Mecatronica IX. Robotica 2Autores:Martınez Chepe Jaime JonathanOsornio Orduna DavidRodrıguez Ramırez Juan de Dios

Grupo:9MV29MV19MV1

Responsable de la asignatura:Villarreal Cervantes Miguel Gabriel

6 de diciembre del 2010

UPIITA-IPN

Indice

1. INTRODUCCION 2

2. MARCO TEORICO 32.1. Cinematica directa e inversa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.1.1. Robot RR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.1.2. Robot RRR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.2. Modelo dinamico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52.2.1. Robot RR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.2.2. Robot RRR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2.3. Generacion de trayectoria . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82.4. Control par calculado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

3. DESARROLLO 93.1. Robot de 2 GDL planar RR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

3.1.1. Simulacion en Matlab. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93.1.2. Animacion en Working Model con vınculo con Matlab. . . . . . . . . . . . . . . . . 10

3.2. Robot de 3 GDL planar RRR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

4. CONCLUSIONES 20

1 Robotica 2.

UPIITA-IPN

1. INTRODUCCION

En este documento reportaran las simulaciones de un robot de 2 GDL planar RR y unrobot de 3 GDL planar RRR. Esto se hara con la ayuda de Matlab y Working Model los cuales estarancomunicados con la ayuda de Matlab Proxy. De manera general la funcionalidad de cada uno de estosprogramas es:

Matlab. Actuara como el controlador del robot, en este se genrara la trayectoria que se desea seguiry se pragramara el modelo dinamico del robot ası como su controlador.

Working Model. Tendra comunicacion con Matlab y ayudara a observar el comportamiento fisico delrobot.

Matlab Proxy. Servira para comunicar a Matlab con Working Model.

Primero se simulara en Matlab el control de movimiento del robot RR y despues se proseguira simularel robot en Working Model y se obtendran las graficas pertinentes.Despues de lo anterior se proseguira con la simulacion del robot RRR, el cual debera tomar un objeto ytrasladarlo a una posicion dada. Para esto el robot sera controlado y se le definira una trayectoria paralograr su tarea.

2 Robotica 2.

UPIITA-IPN

2. MARCO TEORICO

2.1. Cinematica directa e inversa

La cinematica inversa es resolver el problema de: dada la posicion y orientacion deseadas de la her-ramienta respecto a la estacion encontrar los angulos de articulacion que logren este resultado deseado(Craig, 2006).

2.1.1. Robot RR

i αi−1 ai−1 di θi1 0 0 0 θ1 − π

22 0 L1 0 θ23 0 L2 0 0

Cuadro 1: Parametros de Denavit-Hartenberg

01T =

sin θ1 cos θ1 0 0− cos θ1 sin θ1 0 0

0 0 1 00 0 0 1

12T =

cos θ2 − sin θ2 0 L1

sin θ2 cos θ2 0 00 0 1 00 0 0 1

23T =

1 0 0 L2

0 1 0 00 0 1 00 0 0 1

(01T )(12T )(23T ) =03 T =

sin (θ1 + θ2) cos (θ1 + θ2) 0 L2 sin (θ1 + θ2) + L1 sin θ1− cos (θ1 + θ2) sin (θ1 + θ2) 0 −L2 cos (θ1 + θ2)− L1 cos θ1

0 0 1 00 0 0 1

Para la cinematica inversa se tienen las siguientes ecuaciones:

θ2 = arc cos

(x2 + y2 − L2

1 − L22

2L1L2

)(1)

θ1 =π

2+ arctan

(yx

)− arcsin

(L2 sin θ2√x2 + y2

)(2)

2.1.2. Robot RRR

i αi−1 ai−1 di θi1 0 0 0 θ1 − π

22 0 L1 0 θ23 0 L2 0 θ34 0 L3 0 0

Cuadro 2: Parametros de Denavit-Hartenberg

3 Robotica 2.

UPIITA-IPN

01T =

sin θ1 cos θ1 0 0− cos θ1 sin θ1 0 0

0 0 1 00 0 0 1

12T =

cos θ2 − sin θ2 0 L1

sin θ2 cos θ2 0 00 0 1 00 0 0 1

23T =

cos θ3 − sin θ3 0 L2

sin θ3 cos θ3 0 00 0 1 00 0 0 1

34T =

1 0 0 L3

0 1 0 00 0 1 00 0 0 1

Para resolver la cinematica inversa del robot de 3 GDL planar RRR se decidio hacer uso del metodogeometrico por que resulta mas sencillo que los metodos basados en los parametros D-H. Se dedujeron lasecuaciones para las articulaciones del robot como a continuacion se desarrolla. Se considero la configuracioncon codo hacia abajo como se ve en la figura 1.

Figura 1: Obtencion de la cinematica inversa

Dado que se conoce la orientacion del ultimo eslabon (φ) se reduce el problema del robot RR a unrobot RR haciendo el siguiente cambio de variable.

x′ = x− L3 cosφ

y′ = y − L3 sinφ

ρ = 2

√(x′)2 + (y′)2

L22 = L2

1 + ρ2 − 2L1ρ cosβ-2L1ρ cosβ = L2

2 − (L21 + ρ2)

2L1ρ cosβ = (L21 + ρ2)− L2

2

β = a cos((L2

1+ρ2)−L2

22L1ρ

)4 Robotica 2.

UPIITA-IPN

θ = a tan(y′

x′

)q1=

π

2−β − θ (3)

ρ2 = L21 + L2

2 − 2L1L2 cos γ−2L1L2 cos γ = ρ2 − (L2

1 + L22)

2L1L2 cos γ = (L21 + L2

2)− ρ2

γ = a cos((L2

1+L22)−ρ2

2L1L2

)q2= π − γ (4)

Para hallar la ultima variable de articulacion q3 se sabe que la orientacion del robot RRR esta definidapor la suma de la tres variables de articulacion q1, q2 y q3, la orientacion del efector final se mide desdeeje x y positivo en direccion antihoraria.π2 = q1 + q2 + q3 + (−φ)

q3=π

2+φ− (q1+q2) (5)

2.2. Modelo dinamico

La formulacion del modelo dinamico de Newton-Euler es un metodo de ”balance de fuerzas”dinamicas,y la formulacion lagrangiana es un metodo de ”balance de energıas”de la dinamica. Desde luego, para elmismo manipulador, ambos metodos proporcionaran las misma ecuaciones de movimiento (Craig, 1996).A continuacion se describe brevemente como los pasos a seguir para calcular el modelo dinamico de unrobot.

1.- Calcular la posicion para el centro de masa para todos los eslabones.2.- Calcular las velocidades para el centro de masa para cada eslabon.3.- Calcular la energıa cinetica para cada eslabon.

ki =1

2mi

(0V T

ci0Vci

)+

1

2

i

ωcii Iiiωi

4.- Calcular na energıa potencial para cada eslaboon.

Vi = mighi

5.- Obtener el Lagrangiano.

L =ni=1 Ki −ni=1 Vi

6.- Obtener las ecuaciones de Lagrange.

5 Robotica 2.

UPIITA-IPN

τi =d

dt

(∂L

∂θi− ∂L

∂θi

)

La ecucacion en el espacio de estados

A menudo es conveniente expresar las ecuaciones dinamicas de un manipulador en una sola ecuacionque oculte algunos de los detalles, pero que muestre parte de la estructura de las ecuaciones (Craig, 1996).

τ = M(q)q + C(q, q)q +G(q)

τ ε Rn : Vector de fuerzas y pares de entrada.q ε Rn : Vector de coordenadas generalizadas.M(q) ε Rnxn : Matriz de inercia, tiene que ser simetrica y definida positiva.C(q,q) ε Rnxn : Matriz con las fuerzas centrıfigas y de coriolis.G(q) ε Rn : Vector de fuerzas de gravedad.

2.2.1. Robot RR

τ = Mq + Cq +G ⇒[τ1τ2

]= M

[θ1θ2

]+ C

[θ1θ2

]+G

M =

[M11 I2z +m2

(P 22x + P 2

2y + L1P2x cos θ2 − L1P2y sin θ2)

I2z +m2

(P 22x + P 2

2y + L1 cos θ2P2x − L1 sin θ2P2y

)I2z +m2

(P 22x + P 2

2y

) ]

C =

[−2θ2L1P2ym2 cos θ2 − 2θ2L1P2xm2 sin θ2 −θ2L1m2 (cos θ2P2y + sin θ2P2x)

θ1L1m2 (cos θ2P2y + sin θ2P2x) 0

]

G =

[gP2ym2 cos (θ1 + θ2) + gP2xm2 sin (θ1 + θ2) + gPym1 cos θ1 − gL1m2 sin θ1 + gPxm1 sin θ1

gm2 (cos(θ1 + θ2)P2y + sin(θ1 + θ2)P2x)

]M11 = Iz + I2z + L2

1m2 + P 2xm1 + P 2

ym1 + P 22xm2 + P 2

2ym2 + 2L1P2xm2 cos θ2 − 2L1P2ym2 sin θ2

2.2.2. Robot RRR M11 M12 M13

M21 M22 M23

M31 M32 M33

θ1θ2θ3

+

C11 C12 C13

C21 C22 C23

C31 C32 C33

θ1θ2θ3

+

G11

G21

G31

=

τ1τ2τ3

6 Robotica 2.

UPIITA-IPN

M11 = m1l21 + Iz + I2z + I3z +m2

(L21 + l21 + 2L1l2 cos(θ2 + γ2)

)+m3

(L21 + L2

2 + l23 + 2L1L2 cos θ2

+2l3 (L1 cos(θ2 + θ3 + γ3) + L2 cos(θ3 + γ3)))

M12 = I2z + I3z +m2l2 (l2 + L1 cos(θ2 + γ2)) +m3

(L22 + l23 + L1L2 cos θ2 + L1l3 cos(θ2 + θ3 + γ3)

+2L2l3 cos(θ3 + γ3))

M13 = I3z +m3l3 (l3 + L1 cos(θ2 + θ3 + γ3) + L2 cos(θ3 + γ3))

M21 = I2z + I3z +m2l2 (l2 + L1 cos(θ2 + γ2)) +m3

(L22 + l23 + L1L2 cos θ2 + L1l3 cos(θ2 + θ3 + γ3)

+2L2l3 cos(θ3 + γ3))

M22 = I2z + I3z +m2l22 +m3

(L22 + l23 + 2L2l3 cos(θ3 + γ3)

)M23 = I3z +m3l3 (l3 + L2 cos(θ3 + γ3))

M31 = I3z +m3l3 (l3 + L1 cos(θ2 + θ3 + γ3) + L2 cos(θ3 + γ3))

M32 = I3z +m3l3 (l3 + L2 cos(θ3 + γ3))

M33 = I3z +m3l23

C11 = −2m1L1l2θ2 sin(θ2 + γ2)−m3L1

(L2θ2 sin θ2 + l3(θ2 + θ3) sin(θ2 + θ3 + γ3)

)−2m3L2l3θ3 sin(θ3 + γ3)

C12 = −m2L1l2θ2 sin(θ2 + γ2)−m3L1

(L2θ2 sin θ2 + l3(θ2 + θ3) sin(θ2 + θ3 + γ3)

)−2m3L2l3θ3 sin(θ3 + γ3)

C13 = −m3l3

(L1(θ2 + θ3) sin(θ2 + θ3 + γ3) + L2θ3 sin(θ3 + γ3)

)C21 = −m3

(2L2l3θ3 sin(θ3 + γ3) + L1L2θ2 sin θ2 + L1l3(θ2 + θ3) sin(θ2 + θ3 + γ3)

)m2L1l2θ1 sin(θ2 + γ2) +m3L1θ1 (L2 sin θ2 + l3 sin(θ2 + θ3 + γ3))−m2L1l2θ2 sin(θ2 + γ2)

C22 = −2m3L2l3θ3 sin(θ3 + γ3) + L1θ1 (m2l2 sin(θ2 + γ2) +m3(L2 sin θ2 + l3 sin(θ2 + θ3 + γ3)))

C23 = −m3L2l3θ3 sin(θ3 + γ3) +m2L1l2θ1 sin(θ2 + θ3 + γ3)

C31 = −m3

(L2l2θ3 sin(θ3 + γ3) + L1l3(θ2 + θ3) sin(θ2 + θ3 + γ3)− L2l3θ1 sin(θ3 + γ3)

)C32 = m3L2(l3θ2 − l2θ3) sin(θ3 + γ3) +m3θ1 (2L2l3 sin(θ3 + γ3) + L1l3 sin(θ2 + θ3 + γ3))

C33 = m3l3 (L2 sin(θ3 + γ3) + L1 sin(θ2 + θ3 + γ3))

G11 = m1gl1 sin(θ1 + γ1) +m2g (L1 sin θ1 + l2 sin(θ1 + θ2 + γ2))

+m3g (L1 sin θ1 + L2 sin(θ1 + θ2) + l3 sin(θ1 + θ2 + θ3 + γ3))

G21 = m2gl2 sin(θ1 + θ2 + γ2) +m3g (L2 sin(θ1 + θ2) + l3 sin(θ1 + θ2 + θ3 + γ3))

G31 = m3gl3 sin(θ1 + θ2 + θ3 + γ3)

7 Robotica 2.

UPIITA-IPN

2.3. Generacion de trayectoria

Para la generacion de trayectoria se utilizo un polinomio de Bezier, el cual es el siguiente:

ϕ(t, t1, t2) = 171647 − 900948 + 2002049 − 24024410 + 16380411 − 6006412 + 924413 (6)

Donde: 4 = t−t1t2−t1

Sustituyendo la ecuacion 6 en las ecuaciones 7 y 8, se obtiene la trayectoria que debe de seguir el robotdesde un punto P1 = [x1, y1] hasta un punto P2 = [x2, y2].

x(t1, t2) = (x2 − x1)ϕ(t, t1, t2) + x1 (7)

y(t1, t2) = (y2 − y1)ϕ(t, t1, t2) + y1 (8)

2.4. Control par calculado

El control de par calculado nos sirve para controlar el movimiento de un robot dado y se define de lasiguiente forma:

τ = Mv + Cq +G (9)

v = qd +Kpe+Kv e (10)

En la figura 2 se muestra el diagrama a bloques de este control.

Figura 2: Diagrama a bloques del control par calculado

8 Robotica 2.

UPIITA-IPN

3. DESARROLLO

3.1. Robot de 2 GDL planar RR

3.1.1. Simulacion en Matlab.

En la Figura 3 se muestra la simulacion en Matlab del robot RR, en este caso para estas graficas seresolvio la ecuacion diferencial del modelo dinamico del robot RR con:

tau = Mv + Cq +G

Por lo tato en este caso τ ya no va a ser una constante si no que va a estar cambiando en el tiempo porla accion del controlador de par calculado.

Posicion angular del eslabon 1 Posicion angular del eslabon 2

Velocidad angular del eslabon 1 Velocidad angular del eslabon 2

Figura 3: Simulacion en Matlab

9 Robotica 2.

UPIITA-IPN

3.1.2. Animacion en Working Model con vınculo con Matlab.

Ahora se prosegira a programar el control en Matlab, esto se hizo con los codigos fuentes mostradosen las Figuras 4 y 6.

Lo primero que hay que hacer es abrir Matlab y en su Command Window insertar la funcionqdeseada(p), la cual tiene un solo parametro de entrada que es la secuencia de puntos que se desearecorrer, para este caso en particular serıan los puntos p = [0,−6; 2,−4; 2,−2; 4,−2; 4,−4; 2,−4; 0,−6] =[P1;P2;P3;P4;P5;P2;P1] (Ver Figura 6).En general para el codigo fuente de la Figura 4 funciona de la siguiente manera:

Lineas 9-12. Se hacen globales las matrices de posicion angular deseada(qdes), velocidad angulardeseada (dqdes) y aceleracion angular deseada (ddqdes). Y tambien se definen algunos parametrosdel robot y el tiempo que tardara cada trayectoria.

Lınea 13. Ecuacion de Bezier, es la misma que la ecuacion 6 que se mostro anteriormente.

Lıneas 15-18. Se calculan los polinomios de Bezier para cada una de las trayectorias en x y y(ecuaciones 7 y 8).

Lıneas 20-21. Se calcula la cinematica inversa para todos los puntos de la trayectoria.

Lıneas 23-29. Calculo de la derivada de los angulos θ1 y θ2, obtenidos de la cinematica inversa.

Figura 4: Codigo fuente que genera la trayectoria deseada

10 Robotica 2.

UPIITA-IPN

Figura 5: Codigo fuente para el enlace entre Matlab y Working Model

El codigo fuente de la figura 5, nos permite el enlace de Matlab con Working Model. Primero WorkingModel envia las entradas correspondientes a la funcion [r1,r2]=crr(q1,q2,dq1,dq2,t), donde de acuerdo almodelo dinamico del robot RR estas son:

Salidas provenientes de Matlab y que recibe Working Model. [r1,r2]=[τ1, τ2]

Entradas a Matlab provenientes de Working Model. (q1,q2,dq1,dq2,t)=[θ1, θ2, θ1, θ2, t].

A continuacion se explicara el codigo fuente de la Figura 5:

Lınea 2. Acomoda las entradas en dos matrices de 1x2.

Lınea 3. Se recuperan las variables globales generadas en el codigo fuente de la Figura 4.

Lıneas 4-5. Se establecen los parametros fisicos del robot .

Lıneas 9-21. Modelo dinamico del robot.

Lınea 23. Definen las costantes del controlador.

Lınea 24. Calculo de el error y su derivada (e y e).

Lıneas 25-27. Aplicacion de la ley de control y obtencion de τ1 y τ2.

11 Robotica 2.

UPIITA-IPN

Figura 6: Captura de la simulacion del robot RR en Working Model

12 Robotica 2.

UPIITA-IPN

En las Figuras 7 y 8 se muestran las graficas del error (e) y la derivada del error (e) del controlador,los datos para estas graficas se obtienen capturando los datos de error del codigo fuente de la Figura 5,en la lınea 24 de este codigo fuente se obtuvieron los valores para estas graficas.

Figura 7: Grafica del error para thete1 y theta2

Figura 8: Grafica de la derivada del error para dtheta1 y dtheta2

13 Robotica 2.

UPIITA-IPN

3.2. Robot de 3 GDL planar RRR

Para las simulacion del Robot RRR en Working Model es el mismo procedimiento que el anterior,lo primero es la progrmacion en Matlab, para la cual se hicieron una serie de programas los cuales seexplicaran en las siguientes hojas. Los dos principales programas que se deben de tener en cuenta son:

main. Este programa (Figura 9) se tendra que ejecuatar antes de iniciar la simulacion, ya que gener-ara los datos y las trayectorias necesarias que necesita el robot.

control(q1,q2,q3,q1 d,q2 d,q3 d,t). Este programa (Figura 10) sera el controlador del robot y es-tara enviando y recibiendo datos a Working Model.

Figura 9: Codigo fuente del programa main

Para el programa de la Figura 9 es necesario de otros programas ya que este manda a llamarlos durantele ejecucion del mismo. A continuacion se muestran estos programas:

En la lınea 23 se observa que se llama a la funcion bezier pr, la cual sirve para calcular el polinomiode bezier para la trayectoria dada. Su codigo fuente se muestra en la Figura 11.

En la lınea 26 se observa que se llama a la funcion cinRRR, la cual contiene el modelo dinamicodel robot RRR. Su codigo fuente se muestra en la Figura 12. Observese que en las lıneas 3-21 seasignan los valores fısicos que necesita el modelo.

14 Robotica 2.

UPIITA-IPN

Figura 10: Codigo fuente del programa control

Con la ayuda de los programas anteriores se puede hacer la animacion en Working Model junto con Matlaben la Figura 13 se muestra el robot en Working Model en su posicion inicial.

Una vez que se incializa la animacion, el robot RRR ira y movera la pelota de un lugar a otro, en laFigura 14 se muestran varias capturas de la animacion en proceso.

15 Robotica 2.

UPIITA-IPN

Figura 11: Codigo fuente para el polinomio de Bezier

16 Robotica 2.

UPIITA-IPN

Figura 12: Codigo fuente que contiene el modelo dinamico del robot RRR

17 Robotica 2.

UPIITA-IPN

Figura 13: Robot en Working Model

4 segundos 6.3 segundos

9.5 segundos 14.3 segundos

Figura 14: Capturas de la animacion en Working Model

18 Robotica 2.

UPIITA-IPN

Figura 15: Graficas obtenidas del programa main

19 Robotica 2.

UPIITA-IPN

4. CONCLUSIONES

Para el robot RR se tuvo una simulacion exitosa, un poco mas de complicaciones fueron la comuni-cacion con Matlab ya que para la comunicacion sea exitosa se tiene que tomar en cuenta una serie defactores, como los punto y comas que lleve el programa y ciertas estructuras que son necesarias seguirpara que la comunicacion sea exitosa. Ademas hay tomar en cuenta que Working Model tanto la posicioncomo la velocidad angular de los eslabones son referenciados con respecto al marco de referencia x, y.

En cuanto al control del robot se comprobo que realmente tuviera un buen control y que al variarlas constantes Kp y Kv el control de este cambiaba de manera significativa, se podrıa tener un controlbueno con Kp y Kv, y al momento de cambiar estas constantes por otros valores numericos el robot sesalıa completamente de control. De aquı la importancia de una buena sintonizacion de estas constantesy tambien se verifico que no es tan trivial encontrar las constantes adecuadas, ya que se requiere de ungran numero de pruebas para tener unas constantes que respondan de manera efectiva.

Referencias

[1] John J. Craig, Robotica, Prentice Hall, Tercera Edicion, 2006.

[2] Dennis G. Zill, Michael R. Cullen, Ecuaciones diferenciales con problemas de valores en la frontera,Thomson, Quinta Edicion, 2002.

20 Robotica 2.