Robotica I - Cinematica - V2018

UCSM-PPIMMEM
ROBÓTICA I
❑MODELADO CINEMÁTICO
JCC
ROBÓTICA I 30-Abr.-18 -- 0
UCSM-PPIMMEM
El problema cinemático de un robot

• Cinemática del robot : Estudio de su movimiento con respecto a un
sistema de referencia sin considerar las fuerzas que intervienen
– Relación entre la localización del extremo del robot y los valores de sus
articulaciones
– Descripción analítica del movimiento espacial en función del tiempo
• Problema cinemático directo: Determinar la posición y orientación

del extremo final del robot, con respecto a un sistema de
coordenadas de referencia, conocidos los valores de las
articulaciones y los parámetros geométricos de los elementos del
robot
• Problema cinemático inverso: Determinar la configuración que debe
adoptar el robot para alcanzar una posición y orientación del
extremo conocidas
• Modelo diferencial (matriz Jacobiana): Relaciones entre las

velocidades de movimiento de las articulaciones y las del extremo
del robot
1
UCSM-PPIMMEM
Relación entre cinemática directa e

inversa
(x,y) ?
q2 ?
q1 ?
Cinemática Directa Posición y
Coordenadas
orientación del
articulares
extremo del robot
(q1,q2,…,qn)
( x, y, z, φ,θ,ψ)
Cinemática Inversa
2
UCSM-PPIMMEM
Modos de encontrar el modelo
cinemático directo
• Métodos geométricos
– Método no sistemático (depende de la habilidad)
– Consideraciones geométricas (trigonometría,
etc.)
– Válido para robots de pocos grados de libertad
• Métodos basados en cambios de base (MTH
o Cuaternios)
– Puede ser sistematizado (Denavit-Hartenberg)
– Aplicable a cualquier cadena cinemática (n gdl)
– Ayuda de herramientas computacionales
3
UCSM-PPIMMEM
Modelo cinemático directo por

métodos geométricos
x = q2 cos q2 x = l1 cos q1 + l 2 cos ( q1 + q2 )

y = q2 cos q2 y = l1senq1 + l 2 sen ( q1 + q2 )
z=0 z=0
[noa] = Rotz (q1 ) [noa] = Rotz (q1 + q2 )
4
UCSM-PPIMMEM
MCD por métodos geométricos.

Robot de 3 GDL
r = l2 cos q2 + l3 cos(q2 + q3 )
z = l1 + l2 sen q2 + l3 sen(q2 + q3 )
x = r cos q1
y = r sen q1
[noa] = Rotz (q1 )  Roty (−q2 − q3 )  Rotx( / 2)
5
UCSM-PPIMMEM
Modelo cinemático directo

mediante cambios de base
• Asociar a cada eslabón del robot un sistema de
referencia solidario a él.
• De un sistema al siguiente se pasa mediante un
cambio de base definido por rotaciones y
traslaciones.
• Estos cambios de base dependerán de las
dimensiones del robot y de los valores de las
variables articulares qn
• Encontrar la MTH (o cuaternio-vector) que define
el cambio de base de un sistema al siguiente.
• Multiplicar los sucesivos cambios de base en el
orden adecuado, para obtener el cambio de base
entre el origen y el extremo del robot. Este
dependerá de los n grados de libertad qn
• Y representará la relación entre la posición y
orientación del extremo en el sistema de la base,
en función de las coordenadas articulares
0
A2 = 0 A1 1 A2
(Modelo Cinemático Directo)
6
UCSM-PPIMMEM
Ejemplo modelo cinemático mediante MTH

0
A1 = Rotz (q1 )  T(l1 , 0, 0) =
C1 − S1 0 0  1 0 0 l1  C1 − S1 0 l1C1 
S C1 0 0  0 1 0 0   S1 C1 0 l1S1 
=  1
=
0 0 1 0  0 0 1 0  0 0 1 0 
    
0 0 0 1  0 0 0 1  0 0 0 1 
1
A 2 = Rotz (q2 )  T(l2 , 0, 0) =
 C2 − S2 0 0  1 0 0 l2  C2 − S2 0 l2 C 2 
S C2 0 0  0 1 0 0   S 2 C2 0 l2 S 2 
= 2 =
0 0 1 0  0 0 1 0  0 0 1 0 
    
0 0 0 1  0 0 0 1  0 0 0 1 
 C1S2 − S1S2 −C1S2 − S1C 2 0 l1C1 + l 2C12  C12 −S12 0 l1C1 + l 2C12 
S C + C S −S1S2 + C1C 2 0 l1S1 + l 2S12   S12 C12 0 l1S1 + l 2S12 
T = A1 A 2 =  1 2
0 1 1 2
=
 0 0 1 0   0 0 1 0 
   
 0 0 0 1   0 0 0 1 
La caja de traslación de T recoge la posición del extremo en el x = l1C1 + l2C12
sistema de la base (xyz) y = l1S1 + l2 S12
La caja de rotación de T recoge el valor de la orientación del z=0
extremo en el sistema de la base [noa]. [noa] = Rotz (q1 + q2 )
7
UCSM-PPIMMEM
Procedimiento de Denavit Hartenberg

•¿Qué es?:
– Un procedimiento de obtención del MCD basado en la realización de
cambios de base mediante MTH
•¿Qué aporta?:
– Sistematiza la selección de los sistemas de coordenadas,
garantizando que de uno a otro se pasa mediante una secuencia
concreta de 4 movimientos simples (rotación o traslación entorno a
ejes concretos).
– Cada movimiento simple depende de un parámetro
– De este modo la matriz de cambio de base de un sistema a otro i-1Ai
responde a una expresión predefinida, función de los 4 parámetros.
– Permite el utilizar un convenio estandarizado, que sirve de:
• Lenguaje común
• Desarrollo de herramientas de cálculo
8
UCSM-PPIMMEM
Esquema del procedimiento de D-H
1. Establecer para cada elemento del robot un sistema de

coordenadas cartesiano ortonormal (xi,yi,zi) donde
i=0,2,…,n (n=número de gdl). Cada sistema de
coordenadas corresponderá a la articulación i+1 y
estará fijo en el elemento i (Algoritmo de D-H)
2. Encontrar los parámetros D-H de cada una de las
articulaciones
3. Calcular, a partir de los parámetros, las matrices i-1Ai
4. Calcular la matriz Tn = 0A1 1A2 ... n-1An
5. Tn expresa el modelo cinemático directo
9
UCSM-PPIMMEM
Transformaciones básicas de D-H

Transformaciones básicas de paso de eslabón
(definidas sobre sistema móvil →postmultiplicar):
– Rotación alrededor del eje zi-1 un ángulo qi
– Traslación a lo largo de zi una distancia di ; vector di (0,0,di)
– Traslación a lo largo de xi una distancia ai ; vector ai (0,0,ai)
– Rotación alrededor del eje xi un ángulo ai
i-1
Ai = Rotz (qi ) T(0,0,di ) T(a i ,0,0) Rotx(ai )
Cqi −Sqi 0 0  1 0 0 0  1 0 0 a i  1 0 0 0
 Sq Cq 0 0  0 1 0 0  0 1 0 0  0 Ca i -Sa i 0 
i −1
Ai =  i i
=
 0 0 1 0  0 0 1 d i  0 0 1 0   0 Sa i Ca i 0
    
 0 0 0 1  0 0 0 1  0 0 0 1  0 0 0 1
Cqi −Ca i Sqi Sa i Sqi ai Cqi 
 Sq Ca Cq − Sa i Cqi ai Sqi 
= i i i
Matriz de D-H
 0 Sa i Ca i di 
 
 0 0 0 1 
10
UCSM-PPIMMEM
Parámetros de Denavit-Hartenberg
➢ qi: Es el ángulo que forman los ejes xi-1 y xi medido en un plano perpendicular al eje zi-1, utilizando
la regla de la mano derecha. Se trata de un parámetro variable en articulaciones giratorias.
➢ di: Es la distancia a lo largo del eje zi-1 desde el origen del sistema de coordenadas (i-1)-ésimo hasta
la intersección del eje zi-1 con el eje xi. Se trata de un parámetro variable en articulaciones prismáticas.
➢ ai: Es la distancia a lo largo del eje xi que va desde la intersección del eje zi-1 con el eje xi hasta el
origen del sistema i-ésimo, en el caso de articulaciones giratorias. En el caso de articulaciones
prismáticas, se calcula como la distancia más corta entre los ejes zi-1 y zi.
➢ ai: Es el ángulo de separación del eje zi-1 y el eje zi, medido en un plano perpendicular al eje xi,
utilizando la regla de la mano derecha.
11
UCSM-PPIMMEM
Algoritmo de Denavit-Hartenberg (1)

• D-H 1.- Numerar los eslabones comenzando con 1 (primer eslabón
móvil de la cadena) y acabando con n (último eslabón móvil). Se
numerará como eslabón 0 a la base fija del robot.
• D-H 2.- Numerar cada articulación comenzando por 1 (la
correspondiente al primer grado de libertad) y acabando en n
• D-H 3.- Localizar el eje de cada articulación. Si ésta es rotativa, el
eje será su propio eje de giro. Si es prismática, será el eje a lo largo
del cual se produce el desplazamiento.
• D-H 4.- Para i de 0 a n-1 situar el eje zi sobre el eje de la
articulación i+1.
• D-H 5.- Situar el origen del sistema de la base {S0} en cualquier
punto del eje z0. Los ejes x0 e y0 se situarán de modo que formen un
sistema dextrógiro con z0
12
UCSM-PPIMMEM

• D-H 6.- Para i de 1 a n-1, situar el sistema {Si} (solidario
al eslabón i) en la intersección del eje zi con la línea
normal común a zi-1 y zi. Si ambos ejes se cortasen se
situaría {Si} en el punto de corte. Si fuesen paralelos {Si}
se situaría en la articulación i+1
• D-H 7.- Situar xi en la línea normal común a zi-1 y zi
• D-H 8.- Situar yi de modo que forme un sistema
dextrógiro con xi y zi .
• D-H 9.- Situar el sistema {Sn} en el extremo del robot de
modo que zn coincida con la dirección de zn-1 y xn sea
normal a zn-1 y zn .
13
UCSM-PPIMMEM

• D-H 10.- Obtener qi como el ángulo que hay que girar en torno a zi-1
para que xi-1 y xi queden paralelos.
• D-H 11.- Obtener di como la distancia, medida a lo largo de zi-1, que
habría que desplazar {Si-1} para que xi y xi-1 quedasen alineados.
• DH 12.- Obtener ai como la distancia medida a lo largo de xi (que
ahora coincidiría con xi-1) que habría que desplazar el nuevo {Si-1}
para que su origen coincidiese con {Si}.
• DH 13.- Obtener ai como el ángulo que habría que girar entorno a xi
(que ahora coincidiría con xi-1), para que el nuevo {Si-1} coincidiese
totalmente con {Si}.
14
UCSM-PPIMMEM
• DH 14.- Obtener las matrices de transformación i-1Ai

• DH 15.- Obtener la matriz de transformación entre la
base y el extremo del robot T = 0A1 1A2 ... n-1A .
n
• DH 16.- La matriz T define la orientación (submatriz de

rotación) y posición (submatriz de traslación) del
extremo referido a la base en función de las n
coordenadas articulares
15
UCSM-PPIMMEM
Ejemplo MCD por D-H
d3 l4
d2
l1
Robot cilíndrico 4 gdl Representación esquemática
16
UCSM-PPIMMEM
Ejemplo: DH1-DH2-DH3
• D-H 1.- Numerar los eslabones
comenzando con 1 (primer eslabón móvil
de la cadena) y acabando con n (último
eslabón móvil). Se numerará como 1 2
eslabón 0 a la base fija del robot.
(cuadrados) 2
3
• D-H 2.- Numerar cada articulación
3 4
comenzando por 1 (la correspondiente al 3
4
primer grado de libertad) y acabando en 4
n (círculos sombreados) 1
2
• D-H 3.- Localizar el eje de cada

articulación. Si ésta es rotativa, el eje
será su propio eje de giro. Si es 1
prismática, será el eje a lo largo del

cual se produce el desplazamiento. 0
(círculos)
ROBÓTICA I 17 30-Abr.-18 -- 17
UCSM-PPIMMEM
Ejemplo: DH4
• D-H 4.- Para i de 0 a n-1 situar el eje

1 2
zi sobre el eje de la articulación i+1.
Z0 Z1
– Z0 sobre eje (1) 2

Z2 3

3 4 Z3 4

2
– Z3 sobre eje (4)

1
UCSM-PPIMMEM
Ejemplo: DH5
• D-H 5.- Situar el origen del sistema de 1 2
la base {S0} en cualquier punto del eje

Z1
z0. Los ejes x0 e y0 se situarán de
modo que formen un sistema
dextrógiro con z0 2
Z2 3
3 4 Z3 4
3
4
1
2
Z0
X0
UCSM-PPIMMEM
Ejemplo: DH6
• D-H 6.- Para i de 1 a n-1, situar el
1 2
sistema {Si} (solidario al eslabón i)
en la intersección del eje zi con la
línea normal común a zi-1 y zi. Si d3 l4
ambos ejes se cortasen se 2
situaría {Si} en el punto de corte. Z2 Z3 3
Si fuesen paralelos {Si} se situaría
en la articulación i+1 3 4 4
3
d2
– Z1 : Z0 y Z1 paralelos. Situar S1 4
1 Z1
(solidario a [1]) en ②
2
– Z2 : Z1 y Z2 se cortan. Situar S2
(solidario a [2]) en punto de corte
– Z3 : Z2 y Z3 paralelos. Situar S3 1 Obsérvese que
(solidario a [3]) en ④
l1
•Z1 no sube con
Z0
X0 [2]
•Z3 no gira con [4]
UCSM-PPIMMEM
Ejemplo: DH8
• D-H 7.- Situar xi en la línea normal
d3 l4
común a zi-1 y zi
• D-H 8.- Situar yi de modo que X2 X3
forme un sistema dextrógiro con xi
y zi . Z2 Z3
d2
Z1
X1
– X1: Z0 y Z1 colineales. Se sitúa X1
en el Plano horizontal,
coincidiendo con X0 cuando q1=0
– X2: Z1 y Z2 perpendiculares. Se
l1
sitúa X2 en la dirección de Z1⊗Z2
Z0
(producto vectorial) X0
Por legibilidad no
– X3: Z2 y Z3 colineales. Se sitúa X3
en el Plano vertical perpendicular se representan los
al dibujo. ejes Y
UCSM-PPIMMEM
Ejemplo: DH9
• D-H 9.- Situar el sistema {Sn}
d3 l4
en el extremo del robot de
modo que zn coincida con la X2 X3 X4
dirección de zn-1 y xn sea
Z2 Z3 Z4
normal a zn-1 y zn
d2
– Z4 en la misma dirección que Z1
X1
Z3
– X4: Z3 y Z4 colineales. Se sitúa
X4 en el Plano vertical
l1
perpendicular al dibujo, de
modo que coincida con X3 Z0
cuando q4=0 X0
UCSM-PPIMMEM
Ejemplo: Sistemas elegidos

d3
l4
Y2 Y3 Y4
X2 X3 X4
Z2 Z3 Z4
d2
Z1 Y1
X1
l1
Z0 Y0
X0
UCSM-PPIMMEM
Ejemplo: DH10-DH11-DH12-DH13
D-H 10.- Obtener θi como el ángulo que hay
que girar en torno a zi-1 para que xi-1 y xi
queden paralelos.
d3 D-H 11.- Obtener di como la distancia, medida
l4 a lo largo de z-1i, que habría que desplazar
Y2 Y3 Y4 {Si-1} para que xi y xi-1 quedasen alineados.
X2 X3 X4 DH 12.- Obtener ai como la distancia medida
a lo largo de xi (que ahora coincidiría con xi-1)
Z2 Z3 Z4 que habría que desplazar el nuevo {Si-1} para
d2 que su origen coincidiese con {Si}.
Z1 Y1 DH 13.- Obtener αi como el ángulo que habría
X1 que girar entorno a xi (que ahora coincidiría
con xi-1), para que el nuevo {Si-1} coincidiese
Articulación q d con {Si}.a
totalmente α
1 q1 l1 0 0
l1
Z0 Y0 2 90º d2 0 90º
X0 3 0 d3 0 0
4 q4 l4 0 0
24
UCSM-PPIMMEM
Ejemplo: DH14 Cqi −Ca i Sqi Sa i Sqi ai Cqi 

 Sq Ca Cq − Sa Cq a Sq 
• DH 14. Obtener las matrices de transformación i-1Ai i −1
Ai =  i i i i i i i
 0 Sa i Ca i di 
 
 0 0 0 1 
C1 − S1 0 0  0 0 1 0
S C1 0 0  1 0 0 0 
0
A1 =  1 1
A2 = 
0 0 1 l1  0 1 0 d2 
   
0 0 0 1 0 0 0 1
1 0 0 0  C4 − S4 0 0 
0 1 0 0  S C4 0 0 
2
A3 =  3
A4 =  4
0 0 1 d3  0 0 1 l4 
   
0 0 0 1 0 0 0 1
25
UCSM-PPIMMEM
Ejemplo: DH15-DH16
• DH 15.- Obtener la matriz de transformación entre la base y el extremo del robot T
= 0A1 1A2 ... n-1A .
n
• DH 16.- La matriz T define la orientación (submatriz de rotación) y posición

(submatriz de traslación) del extremo referido a la base en función de las n
 − S1C4 S1S 4 C1 C1 (d3 + l4 ) 

CC −C1S4 S1 S1 (d3 + l4 ) 
T = 0 A1 1 A 2 2 A 3 3 A 4 =  1 4
 S4 C4 0 d 2 + l1 
 
 0 0 0 1 
26
UCSM-PPIMMEM
Modelo cinemático directo de un

robot ABB IRB 6400C (I)
Articulación q d a a
1 q1 0 0 -90
2 q2 l1 0 90
3 q3-90 0 -l2 90
4 q4 l3 0 -90
5 q5 0 0 90
6 q6 l4 0 0
C1 0 − S1 0 C 2 0 S2 0
S 0 C1 0 S 0 − C2 0
A1 =   A2 =  
0 1 1 2
 0 −1 0 0 0 1 0 l1 
   
0 0 0 1 0 0 0 1
 S3 0 − C3 − l2 S3  C4 0 − S4 0
− C 0 − S3 l2C3  S 0 C4 0
A3 =   A4 =  
2 3 3 4
 0 1 0 0  0 −1 0 l3 
   
 0 0 0 1  0 0 0 1
C5 0 S5 0 C6 − S6 0 0
S 0 − C5 0 S C6 0 0
A5 =   A6 =  
4 5 5 6
0 1 0 0 0 0 1 l4 
   
0 0 0 1 0 0 0 1
27
UCSM-PPIMMEM
Modelo cinemático directo de un

robot ABB IRB 6400C (II)
nx ox ax px 
n oy ay py 
T= 0 A11 A 2 2 A3 3 A 4 4 A5 5 A 6 =  
y
 nz oz az pz 
 
0 0 0 1
n x = ( C1 C2 S3 + S1C3 )( C4C5C6 − S4 S6 ) + C1S2 ( S4C5C6 + C4 S6 ) + ( − C1C2C3 + S1S3 ) S5C6
n y = ( − S1C2 S3 + S1C3 )( C4C5C6 − S4 S6 ) + S1S2 ( S4C5C6 + C4 S6 ) + ( − S1C2C3 − C1S3 ) S5C6
n z = ( − S2 S3 )( C4C5C6 − S4 S6 ) + C2 ( S4C5C6 + C4 S6 ) + S2C3S5C6
ox = ( C1 C2 S3 + S1C3 )( − C4C5C6 − S4 S6 ) + C1S2 ( − S4C5C6 + C4 S6 ) + ( − C1C2C3 + S1S3 )( − S5C6 )
oy = ( − S1C2 S3 + S1C3 )( − C4C5C6 − S4 S6 ) + S1S2 ( − S4C5C6 + C4 S6 ) + ( − S1C2C3 − C1S3 )( − S5C6 )
oz = ( − S2 S3 ) ( − C4C5C6 − S4 S6 ) + C2 ( − S4C5C6 + C4 S6 ) + S2C3 ( − S5C6 )
px = ( C1 C2 S3 + S1C3 )( l4C4 S5 ) + C1S2 ( l4 S4 S5 ) + ( C1 C2C3 + S1S3 )( − l4C5 + l3 ) +
(− l C C S − l S C − l S )
2 1 2 3 2 1 3 1 1
py = ( − S C S − C C )( l C S ) + S S ( l S S ) + ( − C C C − C S )( − l C
1 2 3 1 3 4 4 5 1 2 4 4 5 1 2 3 1 3 4 5 + l3 ) +
(− l S C S − l C C + l C )
2 1 2 3 2 1 3 1 1
pz = ( - S S )( l C C ) + C ( l S S ) + S C ( − l C + l ) + l S S
2 3 4 4 5 2 4 4 5 2 3 4 5 3 2 2 3
28
UCSM-PPIMMEM
Modelo cinemático mediante

cuaternios
Idéntico planteamiento al de las MTH, pero utilizando cuaternios y
vectores para expresar las transformaciones:
Resultado de trasladar
según pi y rotar según Qi
1. Seleccionar sistemas de coordenadas

2. Encontrar los cambios de base entre sistemas
consecutivos, expresados mediante cuaternios y vectores
3. Componer los cambios de base parciales.
4. En el cambio de base global, queda recogido el MCD
29
UCSM-PPIMMEM
Modelo cinemático mediante cuaternios

Para obtener la relación entre {S0} y {S4} se irá
convirtiendo sucesivamente {S0} en {S1}, {S2},
{S3} y {S4} según la siguiente serie de
transformaciones
1. Desplazamiento de {S0} una distancia l1 a lo largo
del eje z0 y giro un ángulo q1 alrededor del eje z0 ,
llegándose a {S1}.
2. Desplazamiento de {S1} una distancia l2 a lo largo
del eje x1 y giro un ángulo q2 alrededor del nuevo
eje z , para llegar al sistema {S2}.
3. Desplazamiento a lo largo del eje x2 una distancia l3
para llegar al sistema {S3}.
4. Desplazamiento de {S3} una distancia q3 a lo largo
del eje z3 y giro en torno a z4 de un ángulo q4 ,
llegándose finalmente a {S4}.
30
UCSM-PPIMMEM
cuaternios
con
31
UCSM-PPIMMEM
cuaternios
Operando con cuaternios se tiene
ˆ ,0,0,Sˆ ) R
R 0 = Q1Q2Q3Q4 R 4 = Q1234 R 4 = (C124 124 4
32
UCSM-PPIMMEM

cuaternios
El extremo del robot a4=(0,0,0), se encuentra en:
ˆ
(0,a0 ) = (0, a 4x C ˆ ˆ ˆ
112244 − a 4y S112244 + l3C1122 + l 2 C11 , x = a 0x = l3cos( q1 + q 2 ) + l2cos q1
ˆ ˆ ˆ ˆ y = a 0y = l3sen( q1 + q 2 ) + l2sen q1
a 4y C 112244 − a 4x S112244 + l3S1122 + l 2S11 , a 4z − q 3 + l1 )
ˆ
= (0,l3C ˆ ˆ ˆ z = a 0z = l1 − q3
1122 + l 2 C11 ,l3S1122 + l 2S11 ,l1 − q 3 )
El sistema del extremo (R4=(1,0,0,0))se encuentra girado

respecto del de la base, según:
ˆ ,0,0,Sˆ ) R
R 0 = Q1Q2Q3Q4 R 4 = Q1234 R 4 = (C124 124 4 Rotz ( q1 + q 2 + q 4 )
33
UCSM-PPIMMEM
Cinemática Inversa
(x,y)
Objetivo: encontrar los ?
q2
valores que deben adoptar
las coordenadas articulares
del robot (q1, …, qn) para ?
q1
que su extremo se
posicione y oriente según
una determinada
localización espacial [noap]
34
UCSM-PPIMMEM
Modos de abordar la cinemática

inversa
35
UCSM-PPIMMEM
Problemas de la Cinemática
Inversa
• No siempre existe solución cerrada
• Posible solución múltiple
• Dificultad analítica si se busca una
solución cerrada
• Problemas de convergencia y mínimos
locales si se usa método iterativo
36
UCSM-PPIMMEM
Soluciones múltiples
• Unas coordenadas en el
espacio de la tarea, pueden
corresponder a varias
• Se deben evitar cambios
bruscos de configuración
Espacio de las Espacio de la tarea

articulaciones [noap]
(q1,…qn)
37
UCSM-PPIMMEM
Dificultad analítica
Sistema de 12 ecuaciones no lineales con n incógnitas
Directo:
q1,..q6 datos
nx,…px incógnitas
(− l C C S − l S C − l S )
2 1 2 3 2 1 3 1 1
py = ( − S C S − C C )( l C S ) + S S ( l S S ) + ( − C C C − C S )( − l C + l ) +
1 2 3 1 3 4 4 5 1 2 4 4 5 1 2 3 1 3 4 5 3
(− l S C S − l C C + l C )
2 1 2 3 2 1 3 1 1
2 3 4 4 5 2 4 4 5 2 3 4 5 3 2 2 3
Inverso:
q1,..q6 incógnitas
nx,…px datos
¿Cómo despejar las incógnitas?

38
UCSM-PPIMMEM
Resolución de la cinemática inversa por

métodos geométricos
• Aplicable a robots con poco grados de
libertad.
• Busca relaciones geométricas
(típicamente trigonométricas) entre las
coordenadas en el espacio de la tarea y el
de las articulaciones.
39
UCSM-PPIMMEM
Ejemplo de resolución de la cinemática inversa

por métodos geométricos (I)
 py 
q1 = arctg 
 px 
r 2 = px2 + py2 

r 2 + pz2 = l 22 + l32 + 2l 2l3cos q 3 
px2 + py2 + pz2 − l 22 − l32
cos q 3 =
2l 2l3
sen q 3 =  1 − cos2 q 3
Solución Doble
  1 − cos2q 
q3 = arctg
3

 cos q3 
px2 + py2 + pz2 − l 22 − l32
con cos q3 =
2l 2l3
40
UCSM-PPIMMEM
Ejemplo de resolución de la cinemática

inversa por métodos geométricos (II)
q2 =  − a
 
 pz  p
 = arctg  = arctg  z 
 r   p2 + p2 
 x y 
 l3 sen q3 
a = arctg 
 l 2 + l3 cos q3 
 p   l3 sen q 3 
q2 = arctg  z  − arctg 
  p2 + p2   l + l cos q3
 x y  2 3
Solución Doble
41
UCSM-PPIMMEM
Resolución de la cinemática inversa

a partir de las MTH
• Se conoce la localización del robot T=[noap]
(o equivalente) a donde se le quiere llevar
• Se conoce la cinemática del robot definida
por sus parámetros DH y por lo tanto:
1 (q1) … n-1An (qn)
0A 0A
n(q1,..,qn)=
• Se trata de encontrar q1,…,qn que satisfagan

0A
n(q1,..,qn)=[noap]
• Esta ecuación corresponde a 12 ecuaciones

no lineales con n incógnitas
42
UCSM-PPIMMEM
¿Cómo despejar las incógnitas?

(− l C C S − l S C − l S )
2 1 2 3 2 1 3 1 1
py = ( − S C S − C C )( l C S ) + S S ( l S S ) + ( − C C C − C S )( − l C
1 2 3 1 3 4 4 5 1 2 4 4 5 1 2 3 1 3 4 5 + l3 ) +
(− l S C S − l C C + l C )
2 1 2 3 2 1 3 1 1
2 3 4 4 5 2 4 4 5 2 3 4 5 3 2 2 3
q1,..q6 incógnitas
nx,…px datos
43
UCSM-PPIMMEM
Ejemplo de resolución de la cinemática inversa a

partir de las MTH (I)
Artic. q d a a
1 q1 l1 0 90º
2 q2 0 0 -90º
3 0 q3 0 0
C1 0 S1 0 C2 0 − S2 0 1 0 0 0 
 S 0 − C 0 S 0 C2 0 0 1 0 0 
A1 =   A2 =   A3 =  
0 1 1 1 2 2
0 1 0 l1   0 −1 0 0 0 0 1 q3 
     
0 0 0 1 0 0 0 1 0 0 0 1 
C1C2 − S1 − C1S2 0 C1C2 − S1 − C1S2 − q3C1S2 
S C C1 − S1S2 0  S C C1 − S1S2 − q3S1S2 
A2 =   T= A3 =  
0 1 2 0 1 2
 S2 0 C2 l1   S2 0 C2 q3C2 + l1 
   
 0 0 0 1  0 0 0 1 
1. Obtener el MCD
44
UCSM-PPIMMEM

partir de las MTH (II)
2. Aislar q1 C1 S1 0 0   nx ox a x p x 
0 1 − l1 ny oy ay py 
( A1 ) T3 = A 2 A3 =  S −0C
0 −1 0 1 2
0 0  nz oz az pz 
=
1 1
T= 0 A11 A 2 2 A3   
0 0 0 1  0 0 0 1 
( 0
A1 ) T=1A 2 2 A3
−1
C 2 0 − S2 01 0 0 0  C 2 0 − S2 − S2 q3 
S  0 1 0 0   S2 0 C 2 C 2 q3 
( 1
A2 )
−1 0
( A1 ) T= 2 A3
−1
= 2 0 C 2 0  =
 0 − 1 0 0  0 0 1 q3   0 − 1 0 0 
    
0 0 0 1  0 0 0 1  0 0 0 1 
Elemento 3,4  py   py 
S1p x − C1p y = 0  tan(q1 ) =    q1 = arctan  
 px   px 
¿Qué pasa si px=py=0)?
45
UCSM-PPIMMEM

partir de las MTH (III)
3. Continuar aislando el resto de las incógnitas
 C2 S 2 0 0 C1 S1 0 0  1 0 0 0
 0 0 − 1 0 0 0 1 − l  0 1 0 0 
T= 0 A11 A 2 2 A3   1 T =
− S 2 C2 0 0  S1 − C1 0 0  0 0 1 q3 
( 0
A1 ) T=1A 2 2 A3
−1

 0 0 0 1  0

0 0 1 
 
0 0 0 1

( 1
A2 )
−1 0
( A1 ) T= 2 A3
−1
 C2C1 C2 S1 S 2 − l1S 2   nx ox ax p x  1 0 0 0
 −S C 0 0  n o ay p y  0 1 0 0 
 1 1  y y
=
− S 2C1 − S 2 S1 C2 − C2l1   nz oz az p z  0 0 1 q3 
    
 0 0 0 1 0 0 0 1  0 0 0 1
px2 + py2
q2 = arctan
l1 − pz
Elementos 1,4 y 3,4 q3 = C2 ( pz − l1 ) − S2 px2 + py2
46
UCSM-PPIMMEM
Desacoplo cinemático
• Los procedimientos anteriores son complejos, o inutilizables,
cuando el número de grados de libertad es elevado.
• El desacoplamiento cinemático trata de dividir el problema de
6 gdl en 2 de 3 gdl
• Es aplicable cuando los 3 últimos grados de libertad se cortan
en un punto
Y6
X6
Z6
Punto de corte
de los 3 gdl
UCSM-PPIMMEM
Desacoplo cinemático (II)

• A partir de la posición y orientación deseadas
[noap], se obtiene el punto de corte de los 3
últimos gdl (punto de la muñeca: pm)
• Se resuelve el modelo cinemático inverso
para el robot de 3 gdl (q1,q2,q3) que va desde
la base hasta pm
• La resolución de q1,q2,q3, condiciona la
posición y orientación del robot en pm
• Se resuelve el problema cinemático inverso
para el robot que va desde pm hasta el punto
final p, encontrando q4,q5,q6
48
UCSM-PPIMMEM
Desacoplo cinemático (II)

• Desacopla los 3 primeros gdl
de los 3 últimos (utilizados
para orientar el efector final)
– A partir de la posición y
orientación deseadas se
establece la posición donde
debe estar la “muñeca” (punto
de corte de los ejes de las 3
últimas articulaciones)
– Se calculan los valores de
q1,q2,q3 que posicionan la
muñeca en este punto.
– Con los datos de orientación y
los valores ya calculados de
q1,q2 y q3 se obtiene los de
q4,q5,q6
UCSM-PPIMMEM
Desacoplo cinemático (III)
Conocemos Pr, queremos conocer Pm
• Conocida la posición y orientación final deseada,

se puede conocer las coordenadas de donde
debe estar O5 (muñeca)
– Puesto que dir(z5)= dir(z6) (conocida)
– Dist(O5O6 ) es d6 = l4 → pm = pr – l4 z6
• pr es donde se quiere ir [px py pz] pm
• z6es como se quiere orientar al extremo;l vector “a” pr
[ax,ay,az]
• Luego se puede conocer pm :coordenadas de O5
• q1,q2,q3 se obtienen para poner O5 en dichas

coordenadas (método geométrico)
• A partir de 0R6 (= [noa] conocida) y de 0R3 (conocida
al conocer q1,q2,q3) se obtiene 3R6, que depende de
q4,q5,q6
– 0R 3R
3 6 =[noa]→ 3R6 =(3R0)T[noa]
• De las 9 ecuaciones que resultan se despejan
q4,q5,q6 (se puede proceder despejando variable a
variable)
UCSM-PPIMMEM
Modelo diferencial
• Establece la relación entre las velocidades
de las articulaciones, con las velocidades del
extremo del robot.
• Es utilizado por el sistema de control del
robot para establecer qué velocidades debe
imprimir a cada articulación (a través de sus
respectivos actuadores) para conseguir que
el extremo desarrolle una trayectoria
temporal concreta.
• El modelo diferencial queda concretado en la
denominada matriz Jacobiana.
51
UCSM-PPIMMEM
Observación previa
• En general la matriz Jacobiana es la matriz de derivadas
parciales de un sistema de ecuaciones de varias variables
• En robótica se tomarán las ecuaciones las que definen el
modelo cinemático (Relación entre (q1,q2,…,qn) y ( x, y, z,
φ,θ,ψ) . El modelo diferencial relacionaría, mediante la matriz
Jacobiana, las derivadas temporales de ambos vectores.
• Pero también puede interesar conocer las velocidades de
traslación y rotación del extremo (vx,vy,vz, wx,wy,wz) respecto
de un sistema de coordenadas, en función de las velocidades
articulares.
• Se puede por ello hablar de diferentes tipos de Jacobianas
(analítica, geométrica). Se usará una u otra según interés
52
UCSM-PPIMMEM
Jacobiana analítica
?
(x, y)
Establece la relación entre las
velocidades (o incrementos) de q 2?
(q1,q2,…,qn) y ( x, y, z, φ,θ,ψ)
q?
1
Jacobiana analítica directa
Velocidades Velocidades de la
articulares localización del
(q1 , q 2 , q3 , q 4 , q 5 , q 6 ) extremo del robot
(x, y, z,  ,q , )
Jacobiana analítica inversa
UCSM-PPIMMEM
Obtención de la Jacobiana analítica

Diferenciación con respecto del tiempo del modelo cinemático directo
x = fx ( q1 ,, qn ) y = f y ( q1 ,, qn ) z = fz ( q1 ,, qn )
a = fa ( q1 ,, qn )  = f ( q1 ,, qn )  = f ( q1 ,, qn )
f
n
f y
n
fz
n
 =  xq
x i  =
y i
q 
z = i
q
1 qi 1 qi 1 qi
n
fa n
f n
f
a =
 qi  =
 i
q  =
 i
q
1 qi 1 qi 1 qi
O expresado en Ja: Jacobiana

forma matricial Analítica
54
UCSM-PPIMMEM
Jacobiana analítica de un robot SCARA
55
UCSM-PPIMMEM
Jacobiana geométrica
Establece la relación entre
(q1 ,q2 ,q3 ,q4 ,q5 ,q6 ) y (vx , v y , vz , w x , w y , w z )
Jacobiana geométrica directa
Velocidades Velocidades lineales y

articulares angulares del
(q1 , q 2 , q3 , q 4 , q 5 , q 6 ) extremo del robot
(vx , vy , vz , w x , w y , w z )
Jacobiana geométrica inversa
UCSM-PPIMMEM
Obtención de la Jacobiana geométrica
Las velocidades lineales se obtienen

por diferenciación (como la Jacobiana
Analítica)
Las velocidades angulares se obtienen

a partir de la matriz antisimétrica Ω,
que se obtiene a partir de la caja R de
rotación de [noap]
57
UCSM-PPIMMEM
Obtención numérica de la
Jacobiana geométrica
0
0  ni
0 0
oi 0
ai pi 
0 zi = 0 Ai (1: 3,3)
Ai =  
0 0 0 1 i
pn = 0 An (1: 3,4) - 0 Ai (1: 3,4)
  0 zi-1 × i-1pn 
  gdl de
  rotación
 0
zi-1 
 
J = [J1 | J2 |.....| Jn ] con Ji = 

  0
zi-1 
   gdl de
   traslación
  0 
ROBÓTICA I
58 30-Abr.-18 -- 58
UCSM-PPIMMEM
Relación entre la Jacobiana

analítica y geométrica
I 0
J=   ×Ja
0 Q 
0 -Sφ CφSθ 
Con Q = 0 Cφ SφSθ  Siendo (φ,θ,ψ) los ángulos de Euler ZVW
 1 0 Cθ 
 CφCθCψ - SφSψ -CφCθSψ - SφCψ CφSθ 
R = Rotz ( φ ) Roty ( θ ) Rotz ( ψ ) = SφCθCψ + CφSψ -SφCθSψ + CφCψ SφSθ 
 -SθCψ SθSψ Cθ 
Nota: Si q=0 o q=, resultan indeterminados los valores de  y  y por lo tanto

no es posible obtener la matriz Q, no pudiéndose aplicar la relación anterior
59
UCSM-PPIMMEM
Jacobiana Inversa
Permite obtener las velocidades articulares, a
partir de las velocidades en el espacio de la
tareas
x  vx 
 q1   y  q1  v 
       y
   z     vz 
  = Ja   ;   = J  
−1 −1
       wx 
  q     wy 
 qn     qn   
   wz 
60
UCSM-PPIMMEM
Métodos de cálculo de la Jacobiana

Inversa
• Inversión simbólica de la matriz jacobiana
– Gran complejidad (matriz 6x6)
• A partir del modelo cinemático inverso

 q1   x   f1 f1 
 x   
   
q1 = f1 ( x , y , z , a ,  ,  )  
        
    = J 
−1
J −1 =     
     
qn = fn ( x , y , z , a ,  ,  )        
 fn fn 
q n     x   
  
• Evaluación numérica de J e inversión numérica
– Necesidad de recómputo continuo
– En ocasiones J no es cuadrada → Matriz pseudoinversa
– En ocasiones | J | = 0 → Puntos singulares
61
UCSM-PPIMMEM
Jacobiana Pseudoinversa (i)

• J+: Aplicable cuando J no es cuadrada
• Más filas que columnas:
– Grados de libertad del robot < dimensión espacio de la tarea
– El movimiento del robot está sometido a ciertas restricciones
– No existirán unas velocidades articulares que consigan las velocidades
cartesianas deseadas
– Conformarse con las que minimizan el error cuadrático (Pseudoinversa por la
izquierda J+i )
• Más columnas que filas:
– Grados de libertad del robot > dimensión espacio de la tarea → Robot redundante
– Existen infinitos vectores de velocidades articulares que consiguen las
velocidades cartesianas deseadas.
– Utilizar la que minimiza la norma del vector de velocidades articulares
(Pseudoinversa por la derecha J+d)
62
UCSM-PPIMMEM
Jacobiana Pseudoinversa (ii)

• Pseudoinversa por la izquierda. Ji+ = (JT J)-1 JT
Vector de velocidades de q que
minimiza el error en x  q1 
cumple que Ji+J = I
 y  
   
z  
  = J 
   
q   
   
   qn 
• Pseudoinversa por la derecha.

J+d = JT (JJT )-1
Vector de velocidades de q de
menor norma, que cumple
x  q1 
cumple que JJ+d = I
 y  
   
z  
  = J 
   
q   
   
   qn 
63
UCSM-PPIMMEM
Configuraciones Singulares
• Aquellas en las que | J | = 0 (Jacobiano nulo)

• Incremento infinitesimal coordenadas
cartesianas implica incremento infinito
• Implica pérdida de algún grado de libertad
• Tipos
– Singularidades en los límites del espacio de trabajo del robot
– Singularidades en el interior del espacio de trabajo del robot
• Requieren su estudio y eliminación
64
UCSM-PPIMMEM
Ejemplo paso por punto singular
 x  − ( l3S12 + l 2S1 ) − l3S12 0   q1  Det ( J ) = l2l3 S2

    
 y  =  l3C12 + l 2C1 l3C12 0  q2 
 z   0 0 − 1 q3 

• Robot tipo SCARA en
trayectoria en línea recta
velocidad lineal constante
• La velocidad de q2, en las

inmediaciones del punto
singular (q2=) crece, siendo
infinito en el mismo punto
singular
65
UCSM-PPIMMEM
Consecuencias de punto singular
• Cerca de la singularidad
elevada velocidad, giros
bruscos,…
• En la singularidad posible
colapso del robot
• Ejemplo:
– Singularidad robot PUMA
560
UCSM-PPIMMEM
Resumen modelo cinemático

• Permite conocer las órdenes de mando a los
accionadores de cada articulación para que el extremo
del robot alcance una determinada localización espacial.
• Permite saber donde está el extremo a partir de la
lectura de los captadores de posición de cada
articulación.
• Permite relacionar las velocidades del extremo y de los
actuadores
• Los modelos son de obtención relativamente compleja y
deben ser utilizados en tiempo real durante el
funcionamiento de robot
67

Robotica I - Cinematica - V2018

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Robotica I - Cinematica - V2018

Uploaded by

Copyright:

Available Formats

UCSM-PPIMMEM

El problema cinemático de un robot

• Problema cinemático directo: Determinar la posición y orientación

• Modelo diferencial (matriz Jacobiana): Relaciones entre las

Relación entre cinemática directa e

Modelo cinemático directo por

x = q2 cos q2 x = l1 cos q1 + l 2 cos ( q1 + q2 )

MCD por métodos geométricos.

Modelo cinemático directo

Ejemplo modelo cinemático mediante MTH

Procedimiento de Denavit Hartenberg

Esquema del procedimiento de D-H

1. Establecer para cada elemento del robot un sistema de

Transformaciones básicas de D-H

Algoritmo de Denavit-Hartenberg (1)

Algoritmo de Denavit-Hartenberg (2)

Algoritmo de Denavit-Hartenberg (3)

Algoritmo de Denavit-Hartenberg (4)

• DH 14.- Obtener las matrices de transformación i-1Ai

• DH 16.- La matriz T define la orientación (submatriz de

Ejemplo MCD por D-H

primer grado de libertad) y acabando en 4

• D-H 3.- Localizar el eje de cada

prismática, será el eje a lo largo del

• D-H 4.- Para i de 0 a n-1 situar el eje

– Z0 sobre eje (1) 2

– Z1 sobre eje (2) 3

– Z2 sobre eje (3) 1

– Z3 sobre eje (4)

• D-H 5.- Situar el origen del sistema de 1 2

la base {S0} en cualquier punto del eje

Ejemplo: Sistemas elegidos

Ejemplo: DH14 Cqi −Ca i Sqi Sa i Sqi ai Cqi 

• DH 16.- La matriz T define la orientación (submatriz de rotación) y posición

 − S1C4 S1S 4 C1 C1 (d3 + l4 ) 

Modelo cinemático directo de un

Modelo cinemático directo de un

Modelo cinemático mediante

1. Seleccionar sistemas de coordenadas

Modelo cinemático mediante cuaternios

Modelo cinemático mediante

El sistema del extremo (R4=(1,0,0,0))se encuentra girado

Modos de abordar la cinemática

Espacio de las Espacio de la tarea

¿Cómo despejar las incógnitas?

Resolución de la cinemática inversa por

Ejemplo de resolución de la cinemática inversa

Ejemplo de resolución de la cinemática

Resolución de la cinemática inversa

• Se trata de encontrar q1,…,qn que satisfagan

• Esta ecuación corresponde a 12 ecuaciones

¿Cómo despejar las incógnitas?

Ejemplo de resolución de la cinemática inversa a

Ejemplo de resolución de la cinemática inversa a

¿Qué pasa si px=py=0)?

Ejemplo de resolución de la cinemática inversa a

Desacoplo cinemático (II)

Desacoplo cinemático (II)

• Conocida la posición y orientación final deseada,

• q1,q2,q3 se obtienen para poner O5 en dichas

Jacobiana analítica directa

Obtención de la Jacobiana analítica

O expresado en Ja: Jacobiana

Jacobiana analítica de un robot SCARA