Documentos de Académico
Documentos de Profesional
Documentos de Cultura
Responsable de la asignatura:
Villarreal Cervantes Miguel Gabriel
Í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.
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
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
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
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.
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
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
Vi = mi ghi
L =ni=1 Ki −ni=1 Vi
5 Robótica 2.
UPIITA-IPN
d ∂L ∂L
τi = −
dt ∂ θ̇i ∂θi
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).
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 )
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 )
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
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 ].
τ = M v + C q̇ + G (9)
v = q̈d + Kp e + Kv ė (10)
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.
9 Robótica 2.
UPIITA-IPN
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.
10 Robótica 2.
UPIITA-IPN
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:
11 Robótica 2.
UPIITA-IPN
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.
13 Robótica 2.
UPIITA-IPN
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.
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
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
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
18 Robótica 2.
UPIITA-IPN
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.