You are on page 1of 21

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

Implementación de un control par calculado para un robot RR y un RRR


utilizando Matlab y Working Model

Mecatrónica IX. Robótica 2


Autores:
Grupo:
Martı́nez Chepe Jaime Jonathan
9MV2
Osornio Orduña David
9MV1
Rodrı́guez Ramı́rez Juan de Dios
9MV1

Responsable de la asignatura:
Villarreal Cervantes Miguel Gabriel

6 de diciembre del 2010


UPIITA-IPN

Índice
1. INTRODUCCIÓN 2

2. MARCO TEORÍCO 3
2.1. Cinemática directa e inversa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.1.1. Robot RR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.1.2. Robot RRR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.2. Módelo dinámico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.2.1. Robot RR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.2.2. Robot RRR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.3. Generación de trayectoria . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.4. Control par calculado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

3. DESARROLLO 9
3.1. Robot de 2 GDL planar RR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
3.1.1. Simulación en Matlab. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
3.1.2. Animación en Working Model con vı́nculo con Matlab. . . . . . . . . . . . . . . . . 10
3.2. Robot de 3 GDL planar RRR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

4. CONCLUSIONES 20

1 Robótica 2.
UPIITA-IPN

1. INTRODUCCIÓN
En este documento reportaran las simulaciones de un robot de 2 GDL planar RR y un
robot de 3 GDL planar RRR. Esto se hará con la ayuda de Matlab y Working Model los cuales estarán
comunicados con la ayuda de Matlab Proxy. De manerá general la funcionalidad de cada uno de estos
programas es:

Matlab. Actuará como el controlador del robot, en este sé genrará la trayectoria que se desea seguir
y sé pragramará el modelo dinámico del robot ası́ como su controlador.

Working Model. Tendrá comunicación con Matlab y ayudará a observar el comportamiento fisico del
robot.

Matlab Proxy. Servirá para comunicar a Matlab con Working Model.

Primero se simulará en Matlab el control de movimiento del robot RR y después se proseguirá simular
el robot en Working Model y se obtendrán las gráficas pertinentes.
Después de lo anterior se proseguirá con la simulación del robot RRR, el cual deberá tomar un objeto y
trasladarlo a una posición dada. Para esto el robot será controlado y se le definirá una trayectoria para
lograr su tarea.

2 Robótica 2.
UPIITA-IPN

2. MARCO TEORÍCO
2.1. Cinemática directa e inversa
La cinemática inversa es resolver el problema de: dada la posición y orientación deseadas de la her-
ramienta respecto a la estación encontrar los ángulos de articulación que logren este resultado deseado
(Craig, 2006).

2.1.1. Robot RR

i αi−1 ai−1 di θi
π
1 0 0 0 θ1 − 2
2 0 L1 0 θ2
3 0 L2 0 0

Cuadro 1: Parámetros de Denavit-Hartenberg

     
sin θ1 cos θ1 0 0 cos θ2 − sin θ2 0 L1 1 0 0 L2

0T =  − cos θ1 sin θ1 0 0 
 1 T =  sin θ2
 cos θ2 0 0  2T =  0
 1 0 0 
1  0 0 1 0  2  0 0 1 0  3  0 0 1 0 
0 0 0 1 0 0 0 1 0 0 0 1
 
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 
(01 T )(12 T )(23 T ) =03 T =  
 0 0 1 0 
0 0 0 1
Para la cinemática inversa se tienen las siguientes ecuaciones:
 2
x + y 2 − L21 − L22

θ2 = arc cos (1)
2L1 L2
!
π y L sin θ2
θ1 = + arctan − arcsin p2 (2)
2 x x2 + y 2

2.1.2. Robot RRR

i αi−1 ai−1 di θi
π
1 0 0 0 θ1 − 2
2 0 L1 0 θ2
3 0 L2 0 θ3
4 0 L3 0 0

Cuadro 2: Parámetros de Denavit-Hartenberg

3 Robótica 2.
UPIITA-IPN

     
sin θ1 cos θ1 0 0 cos θ2 − sin θ2 0 L1 cos θ3 − sin θ3 0 L2
0 T =  − cos θ1 sin θ1 0 0  1 T =  sin θ2 cos θ2 0 0  2 T =  sin θ3 cos θ3 0 0 
  
  
1  0 0 1 0  2  0 0 1 0  3  0 0 1 0 
0 0 0 1 0 0 0 1 0 0 0 1
 
1 0 0 L3
3T =  0 1 0 0 


4  0 0 1 0 
0 0 0 1

Para resolver la cinemática inversa del robot de 3 GDL planar RRR se decidió hacer uso del método
geométrico por que resulta más sencillo que los métodos basados en los parámetros D-H. Se dedujeron las
ecuaciones para las articulaciones del robot como a continuación se desarrolla. Se consideró la configuración
con codo hacia abajo como se ve en la figura 1.

Figura 1: Obtención de la cinemática inversa

Dado que se conoce la orientación del ultimo eslabón (φ) se reduce el problema del robot RR a un
robot RR haciendo el siguiente cambio de variable.

x0 = x − L3 cos φ

y0 = y − L3 sin φ
q
ρ = 2 (x0 )2 + (y 0 )2
L22 = L21 + ρ2 − 2L1 ρ cos β
-2L1 ρ cos β = L22 − (L21 + ρ2 )
2L1 ρ cos β= (L21 + ρ2 
) − L22
(L21 +ρ2 )−L22
β = a cos 2L1ρ

4 Robótica 2.
UPIITA-IPN

 0
y
θ = a tan x0

π
q1 = −β − θ (3)
2
ρ2 = L21 + L22 − 2L1 L2 cos γ
−2L1 L2 cos γ = ρ2 − (L21 + L22 )
2L1 L2 cosγ = (L21 + L22 ) − ρ2
(L21 +L22 )−ρ2
γ = a cos 2L1 L2

q2 = π − γ (4)
Para hallar la ultima variable de articulación q3 se sabe que la orientación del robot RRR está definida
por la suma de la tres variables de articulación q1 , q2 y q3 , la orientación del efector final se mide desde
eje x y positivo en dirección antihoraria.
π
2 = q1 + q2 + q3 + (−φ)

π
q3 = +φ − (q1 +q2 ) (5)
2

2.2. Módelo dinámico


La formulación del modelo dinámico de Newton-Euler es un método de ”balance de fuerzas”dinámicas,
y la formulación lagrangiana es un método de ”balance de energı́as”de la dinámica. Desde luego, para el
mismo manipulador, ambos métodos proporcionarán las misma ecuaciones de movimiento (Craig, 1996).
A continuación se describe brevemente como los pasos a seguir para calcular el modelo dinámico de un
robot.

1.- Calcular la posición para el centro de masa para todos los eslabones.
2.- Calcular las velocidades para el centro de masa para cada eslabón.
3.- Calcular la energı́a cinética para cada eslabón.

1 0
 1i
ki = mi VciT 0 Vci + ωici Iii ωi
2 2

4.- Calcular ña energı́a potencial para cada eslaboón.

Vi = mi ghi

5.- Obtener el Lagrangiano.

L =ni=1 Ki −ni=1 Vi

6.- Obtener las ecuaciones de Lagrange.

5 Robótica 2.
UPIITA-IPN

 
d ∂L ∂L
τi = −
dt ∂ θ̇i ∂θi

La ecucación en el espacio de estados

A menudo es conveniente expresar las ecuaciones dinámicas de un manipulador en una sola ecuación
que 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 simétrica 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
     
τ1 θ̈1 θ̇1
τ = M q̈ + C q̇ + G ⇒ =M +C +G
τ2 θ̈2 θ̇2
 2 2
 
M11 I2z + m2 P2x + P2y + L1 P2x cos θ2− L1 P2y sin θ2
M= 2 2
 2 2
I2z + m2 P2x + P2y + L1 cos θ2 P2x − L1 sin θ2 P2y I2z + m2 P2x + P2y
 
−2θ̇2 L1 P2y m2 cos θ2 − 2θ̇2 L1 P2x m2 sin θ2 −θ̇2 L1 m2 (cos θ2 P2y + sin θ2 P2x )
C=
θ̇1 L1 m2 (cos θ2 P2y + sin θ2 P2x ) 0
 
gP2y m2 cos (θ1 + θ2 ) + gP2x m2 sin (θ1 + θ2 ) + gPy m1 cos θ1 − gL1 m2 sin θ1 + gPx m1 sin θ1
G=
gm2 (cos(θ1 + θ2 )P2y + sin(θ1 + θ2 )P2x )

M11 = Iz + I2z + L21 m2 + Px2 m1 + Py2 m1 + P2x


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

2.2.2. Robot RRR


         
M11 M12 M13 θ̈1 C11 C12 C13 θ̇1 G11 τ1
 M21 M22 M23   θ̈2  +  C21 C22 C23   θ̇2  +  G21  =  τ2 
M31 M32 M33 θ̈3 C31 C32 C33 θ̇3 G31 τ3

6 Robótica 2.
UPIITA-IPN


M11 = m1 l12 + Iz + I2z + I3z + m2 L21 + l12 + 2L1 l2 cos(θ2 + γ2 ) + m3 L21 + L22 + l32 + 2L1 L2 cos θ2


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

M12 = I2z + I3z + m2 l2 (l2 + L1 cos(θ2 + γ2 )) + m3 L22 + l32 + L1 L2 cos θ2 + L1 l3 cos(θ2 + θ3 + γ3 )

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

M21 = I2z + I3z + m2 l2 (l2 + L1 cos(θ2 + γ2 )) + m3 L22 + l32 + L1 L2 cos θ2 + L1 l3 cos(θ2 + θ3 + γ3 )

+2L2 l3 cos(θ3 + γ3 )
M22 = I2z + I3z + m2 l22 + m3 L22 + l32 + 2L2 l3 cos(θ3 + γ3 )


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


M31 = I3z + m3 l3 (l3 + L1 cos(θ2 + θ3 + γ3 ) + L2 cos(θ3 + γ3 ))
M32 = I3z + m3 l3 (l3 + L2 cos(θ3 + γ3 ))
M33 = I3z + m3 l32

 
C11 = −2m1 L1 l2 θ̇2 sin(θ2 + γ2 ) − m3 L1 L2 θ̇2 sin θ2 + l3 (θ̇2 + θ̇3 ) sin(θ2 + θ3 + γ3 )
−2m3 L2 l3 θ̇3 sin(θ3 + γ3 )
 
C12 = −m2 L1 l2 θ̇2 sin(θ2 + γ2 ) − m3 L1 L2 θ̇2 sin θ2 + l3 (θ̇2 + θ̇3 ) sin(θ2 + θ3 + γ3 )
−2m3 L2 l3 θ̇3 sin(θ3 + γ3 )
 
C13 = −m3 l3 L1 (θ̇2 + θ̇3 ) sin(θ2 + θ3 + γ3 ) + L2 θ̇3 sin(θ3 + γ3 )
 
C21 = −m3 2L2 l3 θ̇3 sin(θ3 + γ3 ) + L1 L2 θ̇2 sin θ2 + L1 l3 (θ̇2 + θ̇3 ) sin(θ2 + θ3 + γ3 )
m2 L1 l2 θ̇1 sin(θ2 + γ2 ) + m3 L1 θ̇1 (L2 sin θ2 + l3 sin(θ2 + θ3 + γ3 )) − m2 L1 l2 θ̇2 sin(θ2 + γ2 )
C22 = −2m3 L2 l3 θ̇3 sin(θ3 + γ3 ) + L1 θ̇1 (m2 l2 sin(θ2 + γ2 ) + m3 (L2 sin θ2 + l3 sin(θ2 + θ3 + γ3 )))
C23 = −m3 L2 l3 θ̇3 sin(θ3 + γ3 ) + m2 L1 l2 θ̇1 sin(θ2 + θ3 + γ3 )
 
C31 = −m3 L2 l2 θ̇3 sin(θ3 + γ3 ) + L1 l3 (θ̇2 + θ̇3 ) sin(θ2 + θ3 + γ3 ) − L2 l3 θ̇1 sin(θ3 + γ3 )
C32 = m3 L2 (l3 θ̇2 − l2 θ̇3 ) sin(θ3 + γ3 ) + m3 θ̇1 (2L2 l3 sin(θ3 + γ3 ) + L1 l3 sin(θ2 + θ3 + γ3 ))
C33 = m3 l3 (L2 sin(θ3 + γ3 ) + L1 sin(θ2 + θ3 + γ3 ))
G11 = m1 gl1 sin(θ1 + γ1 ) + m2 g (L1 sin θ1 + l2 sin(θ1 + θ2 + γ2 ))
+m3 g (L1 sin θ1 + L2 sin(θ1 + θ2 ) + l3 sin(θ1 + θ2 + θ3 + γ3 ))
G21 = m2 gl2 sin(θ1 + θ2 + γ2 ) + m3 g (L2 sin(θ1 + θ2 ) + l3 sin(θ1 + θ2 + θ3 + γ3 ))
G31 = m3 gl3 sin(θ1 + θ2 + θ3 + γ3 )

7 Robótica 2.
UPIITA-IPN

2.3. Generación de trayectoria


Para la generación de trayectoria se utilizó un polinomio de Bézier, el cual es el siguiente:

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


t−t1
Donde: 4= t2 −t1

Sustituyendo la ecuación 6 en las ecuaciones 7 y 8, se obtiene la trayectoria que debe de seguir el robot
desde 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 la
siguiente forma:

τ = M v + C q̇ + G (9)
v = q̈d + Kp e + Kv ė (10)

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

Figura 2: Diagrama a bloques del control par calculado

8 Robótica 2.
UPIITA-IPN

3. DESARROLLO
3.1. Robot de 2 GDL planar RR
3.1.1. Simulación en Matlab.
En la Figura 3 se muestra la simulación en Matlab del robot RR, en este caso para estas gráficas se
resolvio la ecuación diferencial del módelo dinámico del robot RR con:

tau = M v + C q̇ + G

Por lo tato en este caso τ ya no va a ser una constante si no que va a estar cambiando en el tiempo por
la acción del controlador de par calculado.

Posición angular del eslabón 1 Posición angular del eslabón 2

Velocidad angular del eslabón 1 Velocidad angular del eslabón 2

Figura 3: Simulación en Matlab

9 Robótica 2.
UPIITA-IPN

3.1.2. Animación en Working Model con vı́nculo con Matlab.


Ahora se prosegirá a programar el control en Matlab, esto se hizo con los códigos fuentes mostrados
en las Figuras 4 y 6.

Lo primero que hay que hacer es abrir Matlab y en su Command Window insertar la función
qdeseada(p), la cual tiene un sólo parametro de entrada que es la secuencia de puntos que se desea
recorrer, 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 código fuente de la Figura 4 funciona de la siguiente manera:

Lineas 9-12. Se hacen globales las matrices de posición angular deseada(qdes), velocidad angular
deseada (dqdes) y aceleración angular deseada (ddqdes). Y también se definen algunos parametros
del robot y el tiempo que tardará cada trayectoria.

Lı́nea 13. Ecuación de Bézier, es la misma que la ecuación 6 que se mostró anteriormente.

Lı́neas 15-18. Se calculán los polinomios de Bézier para cada una de las trayectorias en x y y
(ecuaciones 7 y 8).

Lı́neas 20-21. Se calcula la cinemática inversa para todos los puntos de la trayectoria.

Lı́neas 23-29. Calculo de la derivada de los ángulos θ1 y θ2 , obtenidos de la cinemática inversa.

Figura 4: Código fuente que genera la trayectoria deseada

10 Robótica 2.
UPIITA-IPN

Figura 5: Código fuente para el enlace entre Matlab y Working Model

El código fuente de la figura 5, nos permite el enlace de Matlab con Working Model. Primero Working
Model envia las entradas correspondientes a la función [r1,r2]=crr(q1,q2,dq1,dq2,t), dónde de acuerdo al
módelo 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 continuación se explicará el código fuente de la Figura 5:

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

Lı́nea 3. Se recuperán las variables globales generadas en el código fuente de la Figura 4.

Lı́neas 4-5. Se establecen los parámetros fisicos del robot .

Lı́neas 9-21. Módelo dinámico del robot.

Lı́nea 23. Definen las costantes del controlador.

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

Lı́neas 25-27. Aplicación de la ley de control y obtención de τ1 y τ2 .

11 Robótica 2.
UPIITA-IPN

Figura 6: Captura de la simulación del robot RR en Working Model

12 Robótica 2.
UPIITA-IPN

En las Figuras 7 y 8 se muestran las gráficas del error (e) y la derivada del error (ė) del controlador,
los datos para estas gráficas se obtienen capturando los datos de error del código fuente de la Figura 5,
en la lı́nea 24 de este código fuente se obtuvierón los valores para estas gráficas.

Figura 7: Gráfica del error para thete1 y theta2

Figura 8: Gráfica de la derivada del error para dtheta1 y dtheta2

13 Robótica 2.
UPIITA-IPN

3.2. Robot de 3 GDL planar RRR


Para las simulación del Robot RRR en Working Model es el mismo procedimiento que el anterior,
lo primero es la progrmación en Matlab, para la cual se hicierón una serie de programas los cuales se
explicarán en las siguientes hojas. Los dos principales programas que se deben de tener en cuenta son:

main. Este programa (Figura 9) se tendrá que ejecuatar antes de iniciar la simulación, ya que gener-
ará 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) será el controlador del robot y es-
tará enviando y recibiendo datos a Working Model.

Figura 9: Código fuente del programa main

Para el programa de la Figura 9 es necesario de otros programas ya que este manda a llamarlos durante
le ejecución del mismo. A continuación se muestran estos programas:

En la lı́nea 23 se observa que se llama a la función bezier pr, la cual sirve para calcular el polinomio
de bezier para la trayectoria dada. Su código fuente se muestra en la Figura 11.

En la lı́nea 26 se observa que se llama a la función cinRRR, la cual contiene el módelo dinámico
del robot RRR. Su código fuente se muestra en la Figura 12. Observese que en las lı́neas 3-21 se
asignan los valores fı́sicos que necesita el módelo.

14 Robótica 2.
UPIITA-IPN

Figura 10: Código fuente del programa control

Con la ayuda de los programas anteriores se puede hacer la animación en Working Model junto con Matlab
en la Figura 13 se muestra el robot en Working Model en su posición inicial.
Una vez que se incializa la animación, el robot RRR irá y moverá la pelota de un lugar a otro, en la
Figura 14 se muestran varias capturas de la animación en proceso.

15 Robótica 2.
UPIITA-IPN

Figura 11: Código fuente para el polinomio de Bézier

16 Robótica 2.
UPIITA-IPN

Figura 12: Código fuente que contiene el módelo dinámico del robot RRR

17 Robótica 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 animación en Working Model

18 Robótica 2.
UPIITA-IPN

Figura 15: Gráficas obtenidas del programa main

19 Robótica 2.
UPIITA-IPN

4. CONCLUSIONES
Para el robot RR se tuvo una simulación exitosa, un poco más de complicaciones fueron la comuni-
cación con Matlab ya que para la comunicación sea exitosa se tiene que tomar en cuenta una serie de
factores, como los punto y comas que lleve el programa y ciertas estructuras que son necesarias seguir
para que la comunicación sea exitosa. Además hay tomar en cuenta que Working Model tanto la posición
como la velocidad angular de los eslabones son referenciados con respecto al marco de referencia x, y.

En cuanto al control del robot se comprobó que realmente tuviera un buen control y que al variar
las constantes Kp y Kv el control de este cambiaba de manera significativa, se podrı́a tener un control
bueno con Kp y Kv, y al momento de cambiar estas constantes por otros valores numéricos el robot se
salı́a completamente de control. De aquı́ la importancia de una buena sintonización de estas constantes
y también se verifico que no es tan trivial encontrar las constantes adecuadas, ya que se requiere de un
gran número de pruebas para tener unas constantes que respondan de manera efectiva.

Referencias
[1] John J. Craig, Robótica, Prentice Hall, Tercera Edición, 2006.

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

20 Robótica 2.

You might also like