CAPTULO 2

Modelamiento Dinmico



Como trabajo previo al Robot Manipulador Esfrico de 3 Grados de Libertad tenemos los
trabajos realizados por: Ing. Corts Gmez Celina, Ing. Corts Gmez Rutilo Omar Ing.
Prez Trevio Moiss, Ing. Monzn Guzmn Guillermo Dr. Mauricio C. Mndez Canseco,
de la Universidad Anhuac del Sur. Mxico, D.F., titulado Diseo y Construccin de un
Brazo Manipulador con Tres Grados de Libertad.

Basados en las referencias dadas, este trabajo presenta el modelado anlisis y control de un
manipulador plano subactuado con tres grados de libertad. El manipulador est formado por
dos juntas rotacionales, que permite al conjunto seguir trayectorias definidas en el plano. El
modelo matemtico del sistema se obtiene a travs de las ecuaciones de Lagrange-Euler. Se
comprueba que el sistema tiene una dinmica interna estable, lo que permitir disear una
ley de control capaz de seguir trayectorias y atenuar perturbaciones.

2.1. Modelado de la Planta

El modelado de la planta consiste en determinar la descripcin matemtica (usualmente un
conjunto de ecuaciones diferenciales) del proceso a ser controlado, que en nuestro caso es
el robot manipulador esfrico. Tal modelado describe la dinmica del proceso y tiene por
objetivo conocer la relacin entre el movimiento del robot y las fuerzas implicadas en el
mismo. Para determinar las ecuaciones del modelo usaremos el algoritmo de Lagrange-
Euler.

2.2. Descripcin del Sistema
El Sistema Robot Manipulador Esfrico de 3 Grados de Libertad que en adelante lo
llamaremos SRM (Spherical Robot Manipulator) de 3GDL es mostrado en la Figura 2.1.

Figura 2.1: Esquema del Robot Manipulador Esfrico de 3GL

A continuacin se describe los tres grados de libertad usados en el proceso:
1. Primer Grado de Libertad: Est compuesto por un servomotor DC1 de 24 VDC con
encoder ptico incorporado, y una base conformado por un disco y un prisma de
aluminio; de tal manera que en la parte inferior del disco de radio

y altura

, ir
acoplado con una bocina de bronce dicho servomotor en posicin vertical y sobre la
parte superior del disco ir un prisma de seccin cuadrada

, y de altura

, a su
vez en la parte superior de dicho prisma ir el otro servomotor en posicin
horizontal que corresponde al segundo grado de libertad. En la Figura 2.2 se
muestra una vista del esquema con las medidas del SRM de 3GL.

Figura 2.2: Esquema con las medidas del Manipulador

2. Segundo Grado de Libertad: compuesto por un servomotor DC2 a 24 VDC con
encoder ptico incorporado, y una varilla rgida de aluminio, de tal manera que en el
eje de dicho servomotor, en posicin horizontal, ir acoplado con una bocina de
bronce dicha varilla de longitud

, en el extremo de dicha varilla ir el otro

servomotor en posicin horizontal que corresponde al tercer grado de libertad.
3. Tercer Grado de Libertad: compuesto por otro servomotor DC3 a 24 VDC con
encoder ptico incorporado, y una varilla rgida de aluminio, de tal manera que en el
eje de dicho servomotor, en posicin horizontal, ir acoplado con una bocina de
bronce dicha varilla de longitud

, en el extremo de dicha varilla podr ir un

efector final.

2.3. El Modelo Dinmico
Para determinar la ecuacin dinmica del manipulador existen varios mtodos tales como el
mtodo de Newton-Euler, el mtodo de Lagrange-Euler, entre otros. Para el manipulador en
estudio usaremos el mtodo de Lagrange-Euler que emplea la frmula Lagraniana
conjuntamente con la representacin de Denavit-Hartenberg (D-H).
En la Figura 2.3 se muestra los sistemas de referencias usados para la representacin D-H.

Figura 2.3: Sistemas de referencias del Manipulador

: Sistema de referencia base (eje de motor 1)

: Sistema de referencia 1 (eje de motor 2)

: Sistema de referencia 2 (eje de motor 3)

: Sistema de referencia 3

: ngulos de las articulaciones

2.3.1 El Procedimiento Denavit-Hartenberg
Un manipulador robtico consiste de una secuencia de cuerpos rgidos (los eslabones)
articulados por junturas rotacionales o prismticas. Cada par de articulacin-eslabn
representa un grado de libertad (GL). Un sistema de coordenadas ortonormales
(

) puede ser establecido usando la regla de la mano derecha para cada

articulacin , donde . As un manipulador con GL posee pares de
articulaciones. El ltimo sistema de coordenadas (

) constituye el sistema de
coordenadas del efector final, el cual es usualmente articulado al ltimo eslabn. El
manipulador es unido a una base de referencia . El sistema de coordenada base es definido
como (

). Las coordenadas de la base son tambin las coordenadas inerciales del

manipulador. El sistema de coordenadas base aumentado es definido como:
(

).
El procedimiento D-H origina una matriz de transformacin homognea que representa el
sistema inercial del manipulador. Para describir tal matriz, requerimos establecer el sistema
de coordenadas (

) del manipulador en , para determinar los

parmetros de las articulaciones y eslabones con el fin de desarrollar la matriz de
transferencia homognea.

Estableciendo el Sistema de Coordenadas D-H
Para establecer el sistema de coordenadas D-H (

) , se usa las
siguientes reglas:
1. El sistema de coordenadas base (

) se determina usando la regla de la

mano derecha. El eje

se escoge libremente y est localizado a lo largo del eje del

movimiento del primer eslabn.
2. El eje

es alineado con el eje del movimiento (giratorio o prismtico) de la

articulacin.
3. El origen del sistema de coordenadas

est en la interseccin del eje

o en la interseccin de la normal comn entre los ejes de dos articulaciones

consecutivas

y el eje

.
4. El eje

es perpendicular al eje

.
5. Aplicar la regla de la mano derecha en la articulacin para determinar

.
6. Generalmente, la articulacin es una articulacin giratoria. El sistema de
coordenadas (

), puede estar en cualquier parte del efector final con tal

de que el eje

est a lo largo de la direccin del eje

y apuntando hacia afuera

del manipulador. El eje

es perpendicular a ambos ejes:

Parmetros D-H
Los cuatro parmetros geomtricos del manipulador (

), se asocian con
cada par articulacin-eslabn y son requeridos para la descripcin de una articulacin
giratoria o prismtica. Despus de establecer el sistema de coordenadas del manipulador
tales parmetros pueden ser determinados de la siguiente manera:
1.

, es el ngulo de rotacin del eje

con el eje

en el plano del eje

.
Este es variable si la articulacin es giratoria.
2.

, es la distancia del origen del sistema de coordenadas

()
a lo
largo del eje

, hasta la interseccin del eje

con el eje

. Esta distancia

es variable si la articulacin es prismtica.

3.

, es la distancia a lo largo del eje

, que va desde la interseccin del eje

con el eje

hasta el origen del sistema de coordenadas

en el caso de
articulaciones giratorias.
4.

, es el ngulo de separacin entre los ejes

medido en un plano
perpendicular al eje

, utilizando la regla de la mano derecha.

Matriz de Transformacin Homognea D-H
La matriz de transformacin homognea D-H caracteriza cada sistema de coordenadas del
par articulacin-eslabn con respecto al sistema de coordenadas previo. Por consiguiente un
punto

del sistema de coordenadas

se puede expresar como (

) en el
sistema de coordenadas
()
ejecutando sucesivamente las siguientes
operaciones:
1. (

): Rotacin de un ngulo

alrededor del eje

, para alinear el eje

con el eje

.
(

): Translacin a lo largo del eje

de una distancia

, hasta hacer
coincidir el eje

con el eje

.
3. (

): Translacin a lo largo del eje

una distancia

, que va desde
la interseccin del eje

y el eje

al origen del sistema de coordenadas

.
4. (

): Rotacin de un ngulo

, del eje

con el eje

alrededor del eje

.

El producto de estas cuatro operaciones bsicas produce la matriz de transformacin
homognea

, del eslabn con respecto al eslabn o articulacin con respecto a

la articulacin . Por lo tanto:

)(

) (

) (

)
[
(

) (

)
(

) (

)


] [

] [

]

=
[

) (

)(

) (

)(

)
(

) (

)(

) (

)(

)
(

) (

(2.1)
Donde y , son las funciones y respectivamente. La siguiente matriz de
trasformacin homognea:

(2.2)

Determina la localizacin del sistema de coordenada (

) con respecto al sistema

de coordenada base
2.3.2. El Algoritmo de Lagrange-Euler
El procedimiento de Lagrange-Euler requiere ejecutar los siguientes pasos:
1. Asignar a cada par articulacin-eslabn del manipulador un sistema de
coordenadas D-H.
2. Obtener las matrices de transformacin homognea

para .
3. Obtener las matrices

que es el efecto del movimiento de la articulacin en

todos los puntos del eslabn :

(2.3)

Donde

relaciona al sistema de coordenadas ( ) con el sistema de

coordenadas base, y

toma los siguientes valores:

[




]

[




]
4. Los efectos de interaccin entre las articulaciones son expresadas por las
siguientes matrices

) {

(2.4)
5. Obtener el tensor de inercia

del eslabn mediante:

(2.5)
Dnde: (

) es el centro de gravedad () del eslabn con respecto al

sistema de coordenadas

son los momentos de inercia

con respecto al sistema de coordenadas (

),

es la masa del cuerpo i,

son las distancias del del cuerpo al sistema de coordenadas

(

), e

son los productos correspondientes del momento

de inercia.
6. Obtener la matriz H de inercia simtrica de dimensin , la cual est
relacionada con la aceleracin y cuyos elementos se expresan como:

()
(2.6)
Donde,

es la traza y es el nmero de grados de libertad.

7. Obtener los trminos

definidos por:

()
(2.7)
8. Obtener el vector no lineal fuerza centrfuga y Coriolis de orden cuyos
elementos

son dados por:

(2.8)
9. Obtener el vector no lineal de fuerza de gravedad d de orden n 1 cuyos
elementos son:

(2.9)
Donde, *

+ es el vector gravedad fila expresado en el sistema de

coordenadas base y

(el elemento 1 es un factor de escala) es

el vector centro de masa del eslabn y expresado en el sistema de coordenadas

.
Finalmente la ecuacin dinmica del manipulador, con el mtodo Lagrange-Euler
toma la forma:

(2.10)

Donde,

es el vector de fuerzas y torques aplicado a cada coordenada generalizada

.
2.4. Aplicacin y Programacin del Modelo
La aplicacin del trabajo es el robot manipulador esfrico de tres grados de libertad. Para
determinar la ecuacin dinmica de dicho manipulador se aplica el algoritmo de Lagrange-
Euler.
A continuacin se presenta los siguientes clculos que usaremos para la programacin con
matemtica simblica del Toolbox de MATLAB.
2.4.1. Clculos
Clculo de los Parmetros D-H
Primeramente se fijan los sistemas de referencia en las articulaciones como se muestra en la
Figura 2.3, posteriormente se determina los parmetros D-H, los cuales se muestran en la
Tabla 2.1:

1
2
3

Tabla 2.1: valores de los parmetros DH
Clculo de las Matrices de Transformacin Homognea
Reemplazando los valores de la Tabla 2.1 en la ecuacin (2.1), obtenemos las matrices de
transformacin homognea

: Posicin y orientacin del sistema de referencia 1 (

) de la base con respecto

al sistema de referencia base (

[
(

) (

)
(

) (

: Posicin y orientacin del sistema de referencia 2 (

) de la primera varilla
con respecto al sistema de referencia 1 (

):

[
(

) (

)
(

) (

)


]

: Posicin y orientacin del sistema de referencia 3 (

) de la segunda varilla
con respecto al sistema de referencia 2 (

):

[
(

) (

)
(

) (

)


]

: Posicin y orientacin del sistema de referencia 2 (

) de la primera varilla con

respecto al sistema de referencia base (

):

: Posicin y orientacin del sistema de referencia 3 (

) de la segunda varilla con

respecto al sistema de referencia base (

):

se determina mediante matemtica simblica de MATLAB, y el programa

se muestra en el apndice A.

Clculo del Momento de Inercia del Disco
El momento de inercia del disco respecto al sistema de referencia 1 (

, (ver
Figura 2.4), est dado por:

Y los productos de los momentos de inercia son:

Figura 2.4: Momento de inercia del disco respecto al sistema de referencia 1 (

).
Clculo del momento de inercia del prisma
El momento de inercia del prisma respecto al sistema de referencia 1 (

, (ver
Figura 2.5), est dado por:

Figura 2.5: Momento de inercia del prisma respecto al sistema de referencia 1 (

Y los productos de los momentos de inercia son:

Clculo del Momento de Inercia de la Base (Disco ms Prisma)
El momento de inercia de la base es la suma de los momentos de inercia del disco ms el
prisma, y est dado por:

Clculo de la Matriz de la Base

De la ecuacin (2.5) se obtiene la matriz de pseudoinercia de la base

(2.11)

Donde,

es la longitud del eje de referencia 1 (

) al de la base,

es igual a

y la inercia viene dada por:

.
Clculo de la Matriz de Pseudoinercia de la Primera Varilla

El momento de inercia de la primera varilla

, se obtiene de [3], [6]

, y

. Reemplazando estos valores en la ecuacin (2.5), obtenemos la

matriz de pseudoinercia

(2.12)

Donde,

es la longitud del eje de referencia 1 (

) al de la primera varilla y

es igual a

.
Clculo de la Matriz de Pseudoinercia de la Segunda Varilla

.
El momento de inercia de la segunda varilla

, se obtiene de [3], [6]:

. Reemplazando estos valores en la ecuacin (3.5), obtenemos la

matriz de pseudoinercia

(2.13)

Donde,

es la longitud del eje de referencia 2 (

) al de la segunda varilla y

es
igual a

.

2.4.2. Programacin del Algoritmo Lagrange-Euler
El programa de clculo de la ecuacin dinmica del robot manipulador esfrico de tres
grados de libertad (3 GDL) se determina aplicando el Toolbox de Matemtica Simblica de
MATLAB, [9] y se presenta en el apndice A. La ecuacin dinmica obtenida para los tres
grados de libertad es:
Primer grado de libertad:

(2.14)
Segundo grado de libertad:

(2.15)
Tercer grado de libertad:

(2.16)
que son los elementos del torque

, dado en la ecuacin (2.10).

2.4.3. El Modelo de los Actuadores
1. Modelado del Actuador 1 de la Base del Manipulador

En la Figura 2.6, se muestra el servomotor DC1, correspondiente a la base.
El voltaje de entrada

aplicado a la armadura est dado por:

(2.17)
La fuerza electromotriz del servomotor DC es:

(2.18)

En el cual

es la relacin de piones. El torque

del servomotor est dado por:

(2.19)
Por lo tanto la ecuacin del torque

requerido por el actuador para la base tiene la forma:

(2.20)
Donde

es el torque de la carga y

es el torque de friccin de Coulomb.

Reemplazando (2.20) en (2.19) resulta:

(2.21)
Dnde:

) (

)(

)(

))

Figura 2.6: Servomotor DC1 de la base del Manipulador

Despreciando la inductancia de armadura o sea

, obtenemos de (2.17) y (2.18)

(2.22)
Despejando

en (2.21) y reemplazando en (2.22), obtenemos la ley de control para la

primera entrada:

(2.23)
Dnde:

)

2. Modelado del Actuador 2 de la Primera Varilla del Manipulador
En la Figura 2.7, se muestra el servomotor DC2 correspondiente a la primera varilla. En la
misma Figura se muestra el circuito elctrico del servomotor DC2, controlado tambin por
la armadura. Dicho servomotor se encuentra ubicado en la parte superior del prisma. Para
obtener la ley de control para la segunda entrada se procede de la misma manera que para el
primer servomotor, obteniendo de esta manera:

(2.24)
Dnde:

Figura 2.7: Servomotor DC2 de la primera varilla del Manipulador

)(

3. Modelado del Actuador 3 de la Segunda Varilla del Manipulador
En la Figura 2.8, se muestra el servomotor DC3 correspondiente a la segunda varilla. En la
misma Figura se muestra el circuito elctrico del servomotor DC3, controlado tambin por
la armadura. Dicho servomotor se encuentra ubicado en extremo de la primera varilla. Para
obtener la ley de control para la tercera entrada se procede de la misma manera que para el
primer servomotor, obteniendo de esta manera:

Figura 2.8: Servomotor DC3 de la segunda varilla del Manipulador
Obtenemos la seal de control para el servomotor DC3.

(2.25)
Dnde:

)(

)(

))

)

2.4.4. Modelo en el Espacio Estado
La ecuacin dinmica del manipulador obtenida en las ecuaciones (2.14), (2.15) y (2.16), se
representa en el espacio de estados mediante la siguiente asignacin de variables:

(2.26)

(2.27)

(2.28)

(2.29)

(2.30)

(2.31)
Dnde:

Posicin angular de la base

Posicin angular de la primera varilla

Posicin angular de la segunda varilla

Velocidad angular de la base

Velocidad angular de la primera varilla

Velocidad angular de la segunda varilla

La ecuacin no lineal para sistemas multivariables es:
() () (2.32)
Su representacin en forma matricial:

[

()

()

()

()

()

()]

()

()

()

()

() ]

()

()

()
] (2.33)
Reemplazando las ecuaciones (2.26) y (2.29) en la ecuacin (2.23) se obtiene

en
variables de estado:

) (2.34)

As mismo, reemplazando las ecuaciones (2.27) y (2.30) en la ecuacin (2.24) se obtiene

en variables de estado:

) (2.35)
De la misma manera, reemplazando las ecuaciones (2.28) y (2.31) en la ecuacin (2.25) se
obtiene

en variables de estado:

) (2.36)
La ecuacin dinmica del manipulador obtenida en las ecuaciones (2.14), (2.15) y (2.16),
tambin se representa en variables de estado:

(2.37)

(2.38)

(2.39)
Posteriormente, se reemplaza la ecuacin (2.37) en (2.34) y se obtiene la primera entrada

) (2.40)
Luego, se reemplaza la ecuacin (2.38) en (2.35) y se obtiene la segunda entrada

) (2.41)
Finalmente, se reemplaza la ecuacin (2.39) en (3.36) y se obtiene la tercera entrada

)
(2.42)
As mismo de (2.40) se obtiene:

)
()

de (2.41) y (2.42) se obtiene

()

()
Dnde:

)(

Finalmente reemplazando las ecuaciones (2.29), (2.30), (2.31), (2.43), (2.44) y (2.45), en la
ecuacin (2.33) se obtiene:

()

()

()

()
(

()

()

()

()

()

La ecuacin salida viene representada por:
[

]
[

(2.46)


Lista de las Variables
Todas las variables que intervienen en el sistema se describen en la Tabla 1.1. Los valores
que se presentan han sido tomados de los servomotores DC con encoder ptico
MATSUSHITA modelo GMX-7MCO19B y modelo GMX-6MPO13A. Dnde: C.F.I.
(coeficiente de friccin viscosa) y M.I. (momento de inercia).

Tabla 1.1: Valores de las variables.

Smbolo Descripcin Valor

Seal de control servo DC1, DC2, DC3 (En simulacin) V

Aceleracin de la gravedad 9.81 m/seg2

Distancia extremo superior del prisma al DC2 0.05 m

Coeficiente friccin de Coulomb 1 y 2DOF 0.001488 kgm2/seg

Coeficiente friccin de Coulomb 3DOF 0.00015 kgm2/seg

Masa del disco 0.55 kg

Masa del prisma 1.0 kg

Masa de la 1ra. y 2da. varilla 0.20 kg

Altura del disco 0.01 m

Radio del disco 0.07 m

Altura del prisma 0.20 m

Ancho y Largo de la seccin del prisma 0.044 m

Longitud de la 1ra. y 2da. varilla 0.2 m

Long. del eje de ref. S1 al CG 1ra. varilla 0.14 m

Long. del eje de ref. S2 al CG 2da. varilla 0.14 m

Long. del eje de ref. S2 al CG 1ra. varilla (L2 Lxs2 ) m

Long. del eje de ref. S3 al CG 2da. varilla (L3 Lxs3 ) m

M.I. del slido (disco ms prisma) 0.0013 kgm2

M.I. de la 1ra. varilla 0.0273 kgm2

M.I. de la 2da. varilla 0.0273 kgm2

M.I. servomotor DC1, DC2 0.00059 kgm2

M.I. servomotor DC3 0.000238 kgm2

M.I. equivalente servomotor DC1 0.2177 kgm2

C.F.V. equivalente servomotor DC1 0.054 Nms/rad

M.I. equivalente servomotor DC2 0.3954 kgm2

C.F.V. equivalente servomotor DC2 0.0589 Nms/rad

M.I. equivalente servomotor DC3 0.000305 kgm2

C.F.V. equivalente servomotor DC3 0.000538 Nms/rad

Reduccin servomotor DC1,DC2 18.5

Reduccin servomotor DC3 12.5

Resist. armadura servomotor DC1,DC2 3.50 Ohmios

Resist. armadura servomotor DC3 5.30 Ohmios

Inductancia armadura servom. DC1,DC2 Despreciable

Inductancia armadura servomotor DC3 Despreciable

Voltaje armadura servomotor DC1,DC2 24 V

Voltaje armadura servomotor DC3 24 V

Constante ganancia driver 1, 2, 3 2.17

Cte. torque servomotor DC1,DC2 0.04364 N-m/A

Cte. torque servomotor DC3 0.04217 N-m/A

Cte. fem servomotor DC1,DC2 0.04364 V/rad/s

Cte. fem servomotor DC3 0.04217 V/rad/s