CINEMTICA INVERSA DE UN BRAZO ROBTICO DE

3 GRADOS DE LIBERTAD

HUGO ENRIQUE LEN GARCA

ANTONIO MOTA LUGO
LUIS ANGEL VILLALOBOS GMEZ

UNIVERSIDAD SANTO TOMS

DIVISIN DE INGENIERAS Y ARQUITECTURA
FACULTAD DE INGENIERA MECATRNICA
BUCARAMANGA
2016

Contenido

1. PARMETROS DE DENAVIT HARTENBERG .................................................... 3

2. CINEMTICA INVERSA ....................................................................................... 5
2.1

ARTICULACIN q3 ........................................................................................ 7

2.2

ARTICULACIN q2 ........................................................................................ 8

2.3

ARTICULACIN q1 ........................................................................................ 9

3. RECOPILACIN DE RESULTADOS.................................................................. 10
4. COMPROBACIN .............................................................................................. 11
4.1

RESULTADOS ............................................................................................. 11

5. INTERFAZ GUI ................................................................................................... 13

1. PARMETROS DE DENAVIT HARTENBERG

La siguiente figura muestra los sistemas Si
parmetros de Denavit Hartenberg (DH).

con los cuales se obtuvieron los

Figura 1. Parmetros de Denavit Hartenberg DH1 a DH9.

NOTA: Las dimensiones A, B, C, D y E que se muestran en la figura 3,

corresponden con la siguiente tabla:

Cota
A
B
C
D
E

Dimensin
92 mm
87.4 mm
77.4 mm
136.5 mm
7 mm

La siguiente tabla muestra los valores DH obtenidos para cada una de las
articulaciones.
Articulacin
qi

di

ai

q1
q2
q3

0 + q1
0 + q2
0 + q3

A
0
E

B
C
D

-/2
0
0

Con los datos de la tabla anterior, se obtienen las respectivas matrices de

transformacin i 1 Ai , las cuales son:
0

C1
S
1
0
A1 =
0

A1 Trz (q1 ) Tt (0 0 A) Tt (B 0 0) Trx

Tt (C 0 0) Trx 0

A 2 Trz (q2 ) Tt (0 0 0)

A1 Trz (q1 ) Tt (0 0 E) Tt (D 0 0) Trx 0

0 -S1 B*C1
0 C1 B*S1
-1 0
A

0 0
1

C2 -S2 0 C*C2
S C 0 C*S
2
2
1
A2 = 2
0 0 1 0

0 0 0 1

C3
S
2
A3 = 3
0

-S3 0 D*C3
C3 0 D*S3
0 1
E

0 0
1

Nota: Por comodidad, dadas expresiones extensas, se toman cos(qi ) Ci y

sin( qi ) Si .
La matriz de transformacin ThSimb que determina la posicin y orientacin del sistema

S 3 ubicado en el extremo del robot respecto al sistema de la base S 0 se obtiene al

multiplicar las matrices anteriores en el orden siguiente:

ThSimb 0 T3 =0 A11A22 A3

2. CINEMTICA INVERSA
La matriz simblica ( ThSimb ) obtenida se iguala a la matriz objetivo ( Thobj ), que es la
matriz a la cual se desea dirigir el extremo del robot, para lo cual se tiene que

Thobj

nh oh ah Ph
x
x
x
x

nhy ohy ahy Phy

nhz ohz ahz Phz

Thobj ThSimb
Thobj 0 A11A2 2 A3
En este caso el robot solo cuenta con 3 GDL, por tanto la matriz que determina la
posicin y orientacin tanto de la mueca Tm como de la herramienta Th del robot es
igual. Dando lugar a la siguiente afirmacin:

Th Tm
Lo cual sustenta que las articulaciones del brazo solo describen la posicin en la
cual se sita la herramienta del brazo, siendo despreciable la orientacin del mismo.
El vector que determina la posicin de la mueca Pm y de la herramienta Ph del
robot es el mismo, como se muestra en la siguiente imagen:

Figura 2. Vector de la posicin de la herramienta y mueca.

La figura anterior da lugar a las siguientes equivalencias:

Pm Ph

Pmx Phx ; Pmy Phy ; Pmz Phz

El vector de posicin es un vector conocido, ya que sabiendo que Tmobj Thobj (las dos
son equivalentes, solo que una matriz es simblica y la otra es la matriz objetivo de
la mueca) se extraen las tres primeras filas de la cuarta columna de la matriz ThSimb ,
la cual representa la posicin de la herramienta, es decir el vector Pm . Lo cual se ve
representado por:

Pm BC ES kC C DC C C DC S S
1
1 2
1 2 3
1 2 3
x 1
Pm Pmy BS1 EC1 kS1C2 DS1C2C3 DS1S2S3

A kS2 DC 2S3 DS2C3
Pmz

Nota: Para evitar confusiones se ha reemplazado la constante de distancia C por la

variable k .
Las matrices crean un sistema de tres ecuaciones que, reacomodando algunos
trminos, resultan

Pmx BC1 ES1 kC1C2 DC1C2C3 DC1S2S3

(1)

Pmy BS1 EC1 kS1C2 DS1C2C3 DS1S2S3

(2)

Pmz A kS2 DC2S3 DS2C3

(3)

Para resolver el sistema de ecuaciones se realizan las siguientes sustituciones

a kC2
b DC2C3
d DS2S3
e B a bd
Sustituyendo, las ecuaciones 1 y 2 se reescriben de la siguiente manera:

Pmx BC1 ES1 aC1 bC1 dC1 eC1 ES1

(1)

Pmy BS1 EC1 aS1 bS1 dS1 EC1 eS1

(2)

2.1 ARTICULACIN q3
Se toman las ecuaciones 1 y 2, se elevan al cuadrado y se suman algebraicamente,
dando como resultado

Pmx 2 Pmy 2 e2 E2
Despejando la variable e , y sustituyendo su valor se obtiene

Pmx 2 Pmy 2 E2 B kC2 DC2C3 DS2S3

Pasando la constante B a la izquierda de los trminos, resulta

Pmx 2 Pmy 2 E2 B kC2 DC2C3 DS2S3

(4)

Despus se elevan al cuadrado las ecuaciones 3 y 4 al cuadrado y se suman, y

despus de una serie de despejes y factorizaciones se tiene

Pmx 2 Pmy 2 E 2 B Pmz A

k 2 D2 2kDC3

Despejando la variable C3 y sustituyendo las posiciones Pm por Ph se obtiene

C3

Phx 2 Phy 2 E 2 B2 2B Phx 2 Phy 2 E 2 Pmz 2 A2 2 Pmz A k 2 D2

2kD

Aplicando la identidad trigonomtrica 1 C32 S32 , el seno de q3 resulta

S3 1 C32

Utilizando atan2, se obtiene el valor de q3, dando dos posibles resultados

atan2 1 C

,C

q31 atan2 1 C32 , C3

q32

2
3

Dos soluciones, sin dependencia de otras.

2.2 ARTICULACIN q2
Aplicando la Regla de Cramer al sistema de ecuaciones las ecuaciones 3 y 4, se
obtienen dos expresiones, una para S2 y otra para C 2 .

Pmz A kS2 DC 2S3 DS2C3

(3)

Pmx 2 Pmy 2 E 2 B kC2 DC2C3 DS2S3

(4)

Q Pmx 2 Pmy 2 E 2 B

x k DC3

R Pmz A

y DS3

Sustituyendo los trminos anteriores y factorizando las ecuaciones 3 y 4 se

reescriben como

Q xC2 yS2
R yC2 xS2
Aplicando Cramer se obtiene
Q x y C 2
R y x S

2

x
S2
y

Q xR yQ

R x 2 y 2

Q y xQ yR
C2

2
2
R x x y

Sustituyendo los valores de cada variable para las expresiones anteriores y

resolviendo se obtiene

S2
C2

k DC3 Phz A DS3

Phx 2 Phy 2 E 2 B

k D 2kDC3
2

k DC3

Phx 2 Phy 2 E 2 B DS3 Phz A

k D 2kDC3
2

Utilizando atan2, se obtiene el valor de q2, dando solo un posible resultado

q2 atan2 S2 , C2
Solucin nica en funcin de f q3 .

2.3 ARTICULACIN q1
Aplicando la Regla de Cramer al sistema de ecuaciones las ecuaciones 1 y 2, se
obtienen dos expresiones, una para S1 y otra para C1 .

Pmx eC1 ES1

(1)

Pmy EC1 eS1

(2)

Aplicando Cramer se obtiene

Pmx e E C1


Pmy E e S1

e
S1
E

Pmx E ePmx EPmy

C1

e
e2 E 2
Pmy

Pmx ePmy EPmx

Pmy
e2 E 2

Sustituyendo los valores de cada variable para las expresiones anteriores y

resolviendo se obtiene

B kC2 DC2C3 DS2S3 Ph EPh

2
B kC2 DC2C3 DS2S3 E 2
B kC2 DC2C3 DS2S3 Ph EPh
C1
2
B kC2 DC2C3 DS2S3 E 2

S1

Utilizando atan2, se obtiene el valor de q2, dando solo un posible resultado

q1 atan2 S1 , C1
Solucin nica en funcin de f q2 , q3 .

3. RECOPILACIN DE RESULTADOS
A continuacin se muestra una tabla con las soluciones obtenidas anteriormente para cada una de las variables.
De all se podr establecer la relacin que existe entre stas:

qi

atan2 1 C

,C

q31 atan2 1 C32 , C3

q3
q32

q2

q1

f qi ,..., qn

SOLUCIONES PARA LAS VARIABLES ARTICULARES

2
3

C3

Phx 2 Phy 2 E 2 B2 2B Phx 2 Phy 2 E 2 Pmz 2 A2 2 Pmz A k 2 D2

2kD

k DC P A DS P 2 P 2 E 2 B
k DC3
3
hz
3
hx
hy
q2 atan2
,
k 2 D2 2kDC3

Phx 2 Phy 2 E 2 B DS3 Phz A

k 2 D2 2kDC3

B kC2 DC2C3 DS2S3 Phy EPhx B kC2 DC2C3 DS2S3 Phx EPhy
q1 atan2
,
2
B kC DC C DS S 2 E 2
B kC2 DC2C3 DS2S3 E 2
2
2 3
2 3

q3

q2 , q3

Con base en los resultados de la tabla se puede notar que existen dos posibles soluciones que satisfacen cualquier
posicin que puede tomar el robot para llegar a un punto en especfico. Para ello en la siguiente figura se muestra
un rbol con las posibles combinaciones que el brazo puede tomar.

4. COMPROBACIN
Por ltimo se verifica si es que todas las posibles combinaciones de las variables
articulares qi satisfacen la matriz objetivo Thobj .
Las distancias de los eslabones del robot son:

A 92 cm

B 87.4 cm

C 77.4 cm

D 136.5 cm

E 7 cm

Y la matriz objetivo es:

Thobj

nh oh ah Ph 0.5000 -0.5000 -0.7071 125.1014

x
x
x
x
0.5000 -0.5000 0.7071 135.0009

nhy ohy ahy Phy

-0.7071
-0.7071
0.0000
-81.9201

n
o
a
P
hz hz hz hz 0
0
0
1

4.1 RESULTADOS
======================= COMBINACIN 1 =======================
q1=

q2=

q3=

0.7854

0.5561

0.7867

ThCalculada=
0.1598 -0.6888 -0.7071 125.1441
0.1598 -0.6888

0.7071 135.0470

-0.9741 -0.2260

0.0000 -81.8278

1.0000

ThObj=
0.5000 -0.5000 -0.7071 125.1014
0.5000 -0.5000

0.7071 135.0009

-0.7071 -0.7071

-81.9201

1.0000

======================= COMBINACIN 2 =======================

q1=

q2=

q3=

0.7854

1.5712

-0.7867

ThCalculada=
0.5005 -0.4995 -0.7071 125.1441
0.5005 -0.4995

0.7071 135.0470

-0.7064 -0.7078

0.0000 -81.8278

1.0000

ThObj=
0.5000 -0.5000 -0.7071 125.1014
0.5000 -0.5000

0.7071 135.0009

-0.7071 -0.7071

-81.9201

1.0000

12

5. INTERFAZ GUI
Con ayuda del modelo del robot creado en SolidWorks al inicio de semestre, los
parmetros de Denavit Hartenberg, el anlisis de la cinemtica inversa que se obtuvo
en el presente trabajo y el complemento GUI de Matlab se cre una interfaz visual que
muestra los movimientos de las articulaciones para llegar a una posicin objetivo, es
decir se cre un programa que muestra la trayectoria de los elementos del robot.
Para llegar a tal modelo tambin se emplearon los complementos SimMechanics y
Simulink, en la siguiente figura se muestra el modelo creado a partir de Simulink.

Figura 3. Modelo en Simulink.

De la interfaz creada se pueden obtener algunas conclusiones:

El robot logra llegar a las coordenadas objetivo, de tal manera que se puede
llegar a ese punto con cualquiera de las dos soluciones.
Tiene un conflicto: el modelo no respeta las condiciones fsicas del robot, por lo
que puede crear trayectorias que interfieran con los eslabones, por lo tanto
fsicamente no puede llegar a esos puntos.
En algunas ocasiones, la posicin objetivo del gripper solo puede ser satisfecha
fsicamente por una solucin del robot.

13

A continuacin mostramos algunas imgenes de las diferentes configuraciones que

puede adoptar el robot de acuerdo a la GUI.

14

15

16