3 Cinem Tica 2

Cinemática de manipuladores
- Introducción
- Herramientas matemáticas para localización espacial
Representación de la posición
Representación de la orientación
Representación de la localización: la matriz de transformación
Uso de transformaciones en robótica
Otras representaciones para las localizaciones
- Modelo geométrico directo
- Modelo Geométrico inverso
Definición: problemática de resolución
Métodos de resolución analítica
Enfoque geométrico
Método de la transformación inversa
Método desacoplado posición-orientación
- Modelo cinemático: el jacobiano
Robótica Industrial. CINEMÁTICA
Introducción
La cinemática de manipuladores tiene como objetivo el estudio analítico del movimiento del
manipulador (a nivel espacial y de velocidades), sin considerar las fuerzas y momentos que lo
producen.
• La relación entre las variables espaciales ligadas a las articulaciones (coordenadas

articulares), y las variables que proporcionan la posición y orientación del extremo del
manipulador (coordenadas operacionales).
• La relación entre las velocidades articulares y la velocidad del órgano terminal.
mundo del robot ↔ mundo operacional
A nivel espacial, 2 modelos fundamentales a obtener:
1. Modelo geométrico directo: conocidos los parámetros geométricos de cada eslabón, y las
coordenadas articulares, ¿cuál será la localización del elemento terminal?
2. Modelo geométrico inverso: conocida la situación deseada del elemento terminal, ¿Es
posible alcanzarla?, y si es así: ¿Cuáles serán las coordenadas articulares? ¿Representan
una configuración única?
- Introducción
Enfoque geométrico
Representación de la Posición
La posición de un punto en el espacio viene dada por el vector de posición
Coordenadas cartesianas Coordenadas cilíndricas Coordenadas esféricas

Z Z Z
p
φ
p p
z z r
x Y r Y Y
y θ θ
X X X
Simplicidad y generalidad: coordenadas cartesianas
Traslación → cambio de posición

Dado un sistema de referencia (un origen O y 3 versores i, j, k) y un sistema de referencia
girado (el mismo origen y 3 versores i', j', k'), las coordenadas de un punto P respecto los dos
sistemas de referencia son las componentes del vector de posición p respecto a ambos:
⎡
⎢ px ⎤⎥ ⎡
⎢ px ' ⎤⎥
p xyz
= ⎢
⎢
p y ⎥⎥ p = ⎢
x' y'z ' ⎢
p y ' ⎥⎥
⎢⎣ pz ⎥⎦ ⎢⎣ pz ' ⎥⎦ Relación entre componentes: p|xyz = R p|x'y'z' (R: matriz de rotación)
Rotación → cambio de orientación (también puede modificar la posición)
⎧ px = i p = i i' px' + i j' p y' + i k' pz'

⎧⎪p xyz = px i + p y j + pz k ⎪
⎨ donde ⎨ p y = jp = j i' px' + j j' p y' + jk' pz'
⎪⎩p x'y' z' = px' i'+ p y' j'+ pz' k' ⎪ p = k p = k i' p + kj' p + k k' p
⎩ z x' y' z'
⎛ i i' i j' i k' ⎞

⎜ ⎟
es decir : R = ⎜ j i' j j' jk' ⎟ → matriz de cosenos directores
⎜ k i' k j' k k'⎟
⎝ ⎠
Propiedades de las rotaciones:
• R ortonormal
• No conmutativa
• Elemento neutro: I3
⎛1 0 0 ⎞ ⎛ Cθ 0 Sθ ⎞ ⎛ Cθ − Sθ 0⎞
⎜ ⎟ ⎜ ⎟ ⎜ ⎟
Rotaciones básicas: Rot ( x , θ ) = ⎜ 0 Cθ − Sθ ⎟ ; Rot ( y, θ ) = ⎜ 0 1 0 ⎟; Rot ( z, θ ) = ⎜ Sθ Cθ 0⎟
⎜ 0 Sθ Cθ ⎟⎠ ⎜ − Sθ 0 Cθ ⎟⎠ ⎜ 0 1 ⎟⎠
⎝ ⎝ ⎝ 0
z z z
θ ⎛ 1 0 0 ⎞⎛ 0 − 1 0 ⎞ ⎛ 0 − 1 0 ⎞⎛ 1 0 0 ⎞
⎜ ⎟⎜ ⎟ ⎜ ⎟⎜ ⎟
θ ⎜ 0 0 − 1⎟⎜ 1 0 0 ⎟ ≠ ⎜ 1 0 0 ⎟⎜ 0 0 − 1⎟
θ θ ⎜ 0 1 0 ⎟⎜ 0 0 1 ⎟ ⎜ 0 0 1 ⎟⎜ 0 1 0 ⎟
θ ⎝ ⎠⎝ ⎠ ⎝ ⎠⎝ ⎠
y y θ y
x x x
Rot (x, 90º) * Rot (z, 90º) <> Rot (z, 90º) * Rot (x, 90º)
Angulos de EULER (z y’z’’)
z” z”’
z z’ Euler (φ, θ, ψ) = Rot (z”, ψ) * Rot (y’, θ) * Rot (z, φ)
φ
ψ y”’ Expresado desde la referencia original:
θ
y’ y”
Euler (φ, θ, ψ) = Rot (z, φ) * Rot (y, θ) * Rot (z, ψ)
y
⎛ CφCθCψ − SφSψ − CφCθSψ − SφCψ CφSθ ⎞

x ⎜ ⎟
x’ Euler (φ , θ , ψ ) = ⎜ SφCθCψ + CφSψ − SφCθSψ + CφCψ SφSθ ⎟
x”’ ⎜ Cθ ⎟⎠
x”
⎝ − SθCψ SθSψ
A partir de la matriz de rotación (Tomando θ entre 0 y π):
φ = atan2 (ay, ax)

θ = atan2 (cosφ ax + senφ ay, az)
ψ = atan2 (-senφ nx + cosφ ny, -senφ ox + cosφ oy)
Angulos Roll-Pitch-Yaw
z
φ
RPY (φ, θ, ψ) = Rot (z, φ) * Rot (y, θ) * Rot (x, ψ)
θ
⎛ CφCθ CφSθSψ − SφCψ CφSθCψ + SφSψ ⎞
⎜ ⎟
ψ y RPY (φ, θ, ψ) = ⎜ SφCθ SφSθSψ + CφCψ SφSθCψ − CφSψ ⎟
⎜ − Sθ CθSψ CθCψ ⎟
x ⎝ ⎠
A partir de la matriz (tomando θ entre –π/2 y π/2):
φ = atan2 (ny, nx)

θ = atan2 (-nz, cosφ nx + senφ ny,)
ψ = atan2 (-senφ ax + cosφ ay, -senφ ox + cosφ oy)

Par de rotación
Giro θ en torno a un eje de dirección r
z
⎛ rx 2 (1 − Cθ) + Cθ rx ry (1 − Cθ) − rz Sθ rx rz (1 − Cθ) + ry Sθ ⎞

⎜ ⎟
Rot (r , θ) = ⎜ rx ry (1 − Cθ) + rz Sθ ry (1 − Cθ) + Cθ ry rz (1 − Cθ) − rx Sθ ⎟
2
θ ⎜⎜ ⎟⎟
⎝ rx rz (1 − Cθ) − ry Sθ ry rz (1 − Cθ) + rx Sθ rz (1 − Cθ) + Cθ ⎠
2
r
y
⎧ (o z − a y ) 2 + ( a x − n z ) 2 + ( n y − o x ) 2
⎪θ = arctan
⎪ nx + o y + a z − 1
⎨
A partir de la matriz de rotación: ⎪ ⎛ oz − a y a x − nz n y − ox ⎞
⎪r = ⎜⎜ 2 Sθ S θ S θ
⎟⎟
⎩ ⎝ 2 2 ⎠
Quaternios
• Utilizados en algunos lenguajes usuales (rapid - ABB)
• Permiten expresar orientación en forma compacta
[ ]
Q = q 0 , q 1 , q 2 , q 3 = [ s, v ]
Propiedades:
• Q = [ s,−v ]
*
[ ] [ ] [
• Q1 + Q2 = s1 , v 1 + s2 , v 2 = s1 + s2 , v 1 + v 2 ]
• aQ = [ as, av ]
[ ]
• Q1 * Q2 = s1s2 − v 1v 2 , v 1 × v 2 + s1v 2 + s2 v 1 no conmutativo
• Q = q02 + q12 + q2 2 + q32
Q*
−1
•
Q =
Q

Quaternios
⎡ θ θ⎤
Q = rot θ = ⎢⎣ 2 ⎥⎦
• Expresión de la orientación como giro en torno a eje: ( k , ) cos , k sen
2
• Aplicación de rotación a un punto con vector de posición r: Q * [ 0, r ] * Q
*
• Composición de rotaciones: producto de cuaterniones

• Rotación Q seguida de traslación p: [0, r '] = Q * [0, r ]* Q + [0, p ]
*
• Traslación p seguida de rotación Q: [0, r '] = Q * [0, r + p ]* Q

*
⎛ 2 1 ⎞
⎜ q0 + q1 − q1q2 − q3q0 q1q3 + q2 q0 ⎟
2
⎜ 2 ⎟
⎜
R = q1q2 + q3q0 q0 + q2 −
2 2 1
q2 q3 − q1q0 ⎟
⎜ ⎟
• Relación con las rotaciones: ⎜
2
⎟
1
⎜ q1q3 − q2 q0 q2 q3 + q1q0 q0 + q3 − ⎟
2 2
⎝ 2⎠
1 1
en sentido inverso: q0 = 2 nx + o y + a z + 1 q1 =
2
nx − o y − a z + 1
1 1
q2 = − nx + o y − a z + 1 q3 = − nx − o y + a z + 1
2 2
Representación de la localización: matrices de

transformación
Transformación: aplicación biyectiva de R3 en R3 que representa una traslación y una rotación
en el espacio
⎛ R p⎞
T = ⎜⎜ ⎟⎟
⎝ f s⎠
• R es la submatriz ortonormal de rotación (Rt=R-1, |R|=1)

• p es el vector de posición
• f y s representan la relación de perspectiva y escalado (En robótica f= (0 0 0) y s = 1)
⎛ nx ox ax px ⎞ ⎛ px ⎞
⎜ ⎟ ⎜ ⎟
⎜n oy ay py ⎟ ⎜p ⎟
T =⎜ y ⎟ p =⎜ y⎟
n oz az pz ⇒ representación homogénea de la posición: p
⎜ z ⎟ ⎜ z⎟
⎜0 ⎟ ⎜ ⎟
⎝ 0 0 1⎠ ⎝1⎠

Matrices de transformación
Rotación y translación de un punto respecto a un sistema fijo de referencia
z
t
⎛ p'x ⎞ ⎛ px ⎞
⎜ ⎟ ⎜ ⎟ p’
⎜ p ' y ⎟ ⎛ R t ⎞⎜ p y ⎟
⎜ p' ⎟ = ⎜⎜ 0 1⎟⎟⎜ p ⎟ R
⎜ z⎟ ⎝ ⎠⎜ z ⎟ p
⎜ 1 ⎟ ⎜1⎟
⎝ ⎠ ⎝ ⎠ y
x
Uso dual: transformación de coordenadas entre 2 sistemas de referencia (cambio de base)
z’
z y’ ⎛ px ⎞ ⎛ px ' ⎞
p ⎜ ⎟ ⎜ ⎟
⎜ py ⎟ ⎜ py' ⎟
T ⎜p ⎟ = T ⎜p ⎟
x’ ⎜ z⎟ ⎜ z' ⎟
⎜1⎟ ⎜ 1 ⎟
y ⎝ ⎠ ⎝ ⎠
x
Lectura alternativa del uso dual:
z y’
T
x’
y
x
representación de la situación (posición + orientación) relativa entre sistemas de referencia

z’

Composición de transformaciones: producto de matrices homogéneas de transformación
⎛ R p1 ⎞ ⎫
z1 z2
z0 y1 T1 = ⎜⎜ 1
0
⎟⎟ ⎪
⎝ 0 1 ⎠ ⎪→0T *1T = ⎛⎜ R1R2 p1 + R1 p2 ⎞⎟
⎬ ⎜ 0 ⎟
⎛ R2 p2 ⎞⎪
1 1 2
0 T2 ⎝ 1 ⎠
T1
x1 x2
y2 1
T 2 = ⎜
⎜ 0 1 ⎟⎪ ⎟
y0 ⎝ ⎠⎭
• No conmutativa
0
T2
x0
• Asociativa
• Elemento neutro: I4
⎛ R p⎞ ⎛ Rt − Rt p ⎞
⎜ ⎟ −1
⎜ ⎟
Transformación inversa: inversión de la matriz: T = ⎜ 0 1 ⎟ → T = ⎜ 0 ⎟
⎝ ⎠ ⎝ 1 ⎠
Casos particulares:
• Traslaciones (R=I3)
• Rotaciones (p=(0 0 0)t)
⎛ R p ⎞ ⎛ I 3 p ⎞⎛ R 0 ⎞ ⎛ R 0 ⎞⎛ I 3 R t p ⎞
⎜ ⎟ ⎜ ⎟⎜ ⎟ ⎜
Separación en rotación y traslación puras: T = ⎜ 0 1 ⎟ = ⎜ 0 1 ⎟⎜ 0 1 ⎟ = ⎜ 0 1 ⎟⎜ 0
⎟⎜ ⎟
⎝ ⎠ ⎝ ⎠⎝ ⎠ ⎝ ⎠⎝ 1 ⎟⎠
Ejemplos de uso en robótica:
Modelado (estructurado) del entorno del robot
Bloque
Taladro
B
TT A
TM: Situación de la mesa respecto a ABS
M
TB: Situación del bloque respecto a mesa
M B
TB TT: Situación del taladro respecto al bloque
z
Mesa
A
TM
¿Situación del taladro respecto a ABS? ATT = ATM * MTB * BTT

Otro ejemplo: modelado entorno y especificación destino
M
TR: Situación del manipulador respecto al mundo
R
TE: Situación del extremo del brazo respecto a la base
E
TH: Situación de la herramienta respecto extremo brazo
M
TO: Situación del objeto respecto al mundo
O
TH: Situación del asidero respecto al objeto
¿Localización absoluta del asidero (MTH)? → MTH = MTR * RTE * ETH = MTO * OTH
¿Situación a la que habrá de llevar la herramienta del robot respecto de su base (RTH)?
→ R
TH = RTE * ETH = (MTR)-1 * MTO * OTH
Otras representaciones
Expresión de localización a nivel de usuario (internamente se suelen convertir a transformación)
Vector de posición + ángulos de Euler/RPY (x,y,z,φ,θ,ψ)
• Muy compacta (mínima)

• Inversión y composición no triviales
Vector de posición + par de rotación (x,y,z, rx, ry,rz,θ)
• Compacta (7 elementos)
• Composición complicada
Vector de posición + quaternios (x,y,z,q0,q1,q2,q3)
• Compacta (7 elementos)
• Composición sencilla
Se usa en algunos lenguajes (rapid – ABB)

- Introducción
Enfoque geométrico
Modelo geométrico directo
X = f (q)
X: Situación de la herramienta respecto de la base: coordenadas operacionales o de la tarea
• Bien sea como transformación: X = 0T6

• Bien como representación vectorial compacta: X = (x, y, z, φ, θ, ψ)t
q: configuración articular del robot (coordenadas articulares): q = (q1, q2, q3, q4, q5, q6)t
Resolución sistemática (Denavit y Hartemberg, 1955), basada en transformaciones:
Procedimiento en 4 pasos…
1. Identificación y etiquetado de sólidos y ejes

2. Asignación protocolaria de referencias a los sólidos
3. Determinación de la situación relativa entre sólidos consecutivos (transformaciones de

paso) i-1Ti = fi(qi).
4. Composición de transformaciones en pos de la situación absoluta del último eslabón

(herramienta)
0 0 1 2 3 4 5
T6 = T1 T2 T3 T4 T5 T6 = f(q)

Protocolo de asignación de referencias:
1.- Origen Oi en la intersección entre la perpendicular común a Ji y Ji+1 con ésta última.
* Ejes Ji y Ji+1 paralelos: elegir una perpendicular común
* Ejes Ji y Ji+1 se cortan: el punto es el de intersección
* Caso especial: sólidos 0 y herramienta
2.- Versor zi paralelo a Ji+1
3.- Versor xi paralelo a zi-1×zi
4.- Versor yi es el producto vectorial zi×xi

La asignación anterior permite reducir a 4 los parámetros necesarios para determinar la
ubicación relativa entre referencias consecutivas (Parámetros de Denavit - Hartemberg)
ai: distancia entre zi-1 y zi medida a lo largo de xi

di: distancia entre xi-1 y xi medida a lo largo de zi-1
αi: angulo entre zi-1 y zi medido en torno a xi
θi: angulo entre xi-1 y xi medido en torno a zi-1

¿Cuál es la matriz de transformación i-1Ti (matriz de paso)? Desde la referencia asociada a i-1:
1.- Giro θi en torno a zi-1 ⇒ ¡xi-1 y xi paralelos!

2.- Traslación una distancia di a lo largo de zi-1.
3.- Traslación una distancia ai a lo largo del nuevo xi-1, (xi) ⇒ ¡ya coinciden los orígenes!
4.- Giro αi en torno al eje xi. Ya coinciden los 2 sistemas de referencia.
Expresando esta sucesión respecto al sistema de referencia asociado al sólido i-1:

⎛ Cθi − SθiCαi SθiSαi aiCθi ⎞
⎜ ⎟
i −1 ⎜ S θi C θi C α i − C θi S α i a i S θi ⎟
Ti = ⎜
di ⎟
i-1
Ti = Rot(zi-1, θi) Trasl(0,0,di) Trasl(ai,0,0) Rot(xi-1, αi) 0 Sα i Cαi
⎜ ⎟
⎜ 0 ⎟
⎝ 0 0 1 ⎠
Si el enlace es rotacional, el único parámetro variable es θi , siendo ésta la coordenada

articular correspondiente. Si el enlace es de tipo prismático, dicha variable será di.

Una vez realizada la asignación de referencias, extraida la tabla de parámetros D-H y obtenidas
las matrices de paso:
a) Método directo
0 0 1 2 3 4 5
T6 = T1 T2 T3 T4 T5 T6
b) Método de las matrices intermedias U

n-1
Sean Un = Tn
n-2 n-2
Un-1 = Tn-1Un = Tn
.............
0 0
U1 = T1U2 = Tn
• La asignación de referencias influye en el nº de cáculos (será menor si ai y/o di nulos).

• La 3ª componente de rotación se calcula mediante el producto vectorial de las otras 2.
.
Robot cilíndrico (4 ejes)
θi αi ai di
1 θ1 0º 0 l1
2 90º 90º 0 d2
3 0º 0º 0 d3
4 θ4 0º 0 l4
ABB IRB 2400
θi αi ai di
1 θ1 -90º 0 l1
2 θ2 0º l2 0
3 θ3 90º 0 0
4 θ4 -90º 0 l3
5 θ5 90º 0 0
6 θ6 0º 0 l4

PUMA 560
Stanford arm

- Introducción
Enfoque geométrico
Modelo geométrico inverso

Relación inversa entre:
• coordenadas operacionales X = (x, y, z, φ, θ, ψ)t ↔ 0

T6
• coordenadas articulares q = (q1, q2, q3, q4, q5, q6)t
q = f -1(X)
Imprescindible en control de robot:
Tarea especificada en espacio operacional ↔ Movimiento y su control en el espacio articular
¡Resolución problemática!
• Solución no única
• Configuraciones singulares
• Complejidad algebraica/geométrica

Multiplicidad de soluciones
El nº de soluciones depende de número de articulaciones, de la complejidad cinemática
(parámetros DH no nulos) y de las singularidades.
Análisis formal de los grados de libertad (gdl)
• Caso general: se requieren 6.

• Excepciones: montaje sobre plano horizontal (4 gdl)
• Configuraciones singulares: algunas articulaciones no aportan nuevos gdl
Análisis del caso general (gdl necesarios = 6)
Si nº articulaciones < 6: generalmente no existe solución

Si nº articulaciones = 6:
Si conf. no singular: existe nº finito de soluciones
Si conf singular: existen ∞ soluciones
Si nº articulaciones > 6: generalmente existen ∞ soluciones
Multiplicidad de soluciones
Ejemplo: robot angular de 6 ejes: 8 diferentes soluciones
4 soluciones brazo + 2 muñeca (flip – no flip)

Singularidades
Configuraciones en las que se pierde 1 o más gdl (1 o más articulaciones no aportan nuevos gdl)
Singularidades del brazo (normalmente asociadas a los límites del volumen de trabajo):
10
2
q1
5 0
-2
0 10 20 30
0
Y
t
0.5
-5
0
qd1
-0.5
-10
-1
-1.5
-10 -5 0 5 10 0 10 20 30
X t
Singularidades
Singularidades en muñeca (alineación en Roll-pitch-roll):
Requieren tratamiento especial en el modelo inverso y su consideración en la generación de

trayectorias

Métodos de resolución
- Numéricos (iterativo basado en Jacobiano)
• No siempre converge
• Lentitud
- Analíticos:
• Eficiencia de la solución cerrada

• Soluciones múltiples fácilmente evaluables
Varios métodos de resolución analítica:
Enfoque geométrico (Ejemplos en Barrientos 07 y en Fu - Gonzalez – Lee 88)
Enfoques algebráicos:
Método de la transformación inversa (Paul 81)

Método desacoplado posición – orientación (Pieper 68)
Resolución geométrica
Basada en la descomposición en adecuadas secciones planas y posterior resolución de
geometrías triangulares
Problemas:
• Determinación de soluciones múltiples

• Signos y origen de ángulos (adaptación a DH)
• Compleja para robots con muchos ejes (>3)
Ejemplo: robot de 2 ejes rotacionales: py
⎧ p x + p y − a1 − a 2
2 2 2 2 a2
⎪q 2 = ± acos q2
⎪ 2a1a2
p x + p y = a1 + a 2 + 2a1a 2 cos q 2 ⇒ ⎨
2 2 2 2
⎛ ⎞
⎪q = atan ⎜ y ⎟ − atan ⎛⎜ a 2 sen q2 ⎞⎟
p
a1
⎪⎩ 1 ⎜p ⎟ ⎜ a + a cos q ⎟
⎝ x⎠ ⎝ 1 2 2 ⎠
q1
• Codo abajo y codo arriba: soluciones + y - para q2 px

• ¿Y si posición deseada está en tercer cuadrante?
Métodos analíticos con enfoque algebráico
Los métodos algebráicos están basados en la manipulación de las ecuaciones del modelo
geométrico directo:
⎛ nx ox ax px ⎞
⎜ ⎟
⎜n oy ay py ⎟
0
T6 (q1 , q 2 ,L , q 6 ) = ⎜ y
n oz az pz ⎟
⎜ z ⎟
⎜0 1 ⎟⎠
⎝ 0 0
donde la matriz representa la localización deseada
• Ecuaciones no lineales
• Las expresiones están fuertemente acopladas
¿Quién es el valiente que despeja así?
Método de la tranformación inversa

Método utilizado por Paul y descrito en Barrientos, que permite cierta sistematización:
Utiliza dos tipos de matrices: Matrices algebráicas U y matrices numéricas V
Se calcula Se iguala con Se obtiene
V0 = localización U1 q1
V1 = 0T1-1*V0 U2 q2
V2 = 1T2-1*V1 U3 q3
V3 = 2T3-1*V2 U4 q4
V4 = 3T4-1*V3 U5 q5
V5 = 4T5-1*V4 U6 q6
Consejos prácticos:
• Resolver en los 4 cuadrantes (atan2 en lugar de atan)

• Desdoblar soluciones si sólo se puede despejar el seno o el coseno
• Conviene despejar de la 4ª columa en las 3 primeras articulaciones

Solución desacoplada posición - orientación
Propuesto por Pieper (1968) Si los ejes de la muñeca se cortan en un punto (a4=a5=d5=0)
→ Su posición no depende de θ4 θ5 y θ6
2 fases secuenciadas en el tiempo:
1. Resolución del brazo (q1, q2 y q3)

2. Resolución de la muñeca (q4, q5 y q6)
1. Resolución del brazo Punto de partida: la ubicación deseada para la herramienta (TCP):
⎛ nx ox a x p x ⎞
⎜ ⎟
⎜ ny oy a y p y ⎟
loc _ deseada _ TOOL = ⎜
n oz a z p z ⎟
⎜ z ⎟
⎜0 0 0 1⎟
⎝ ⎠
Conocida la geometría de la herramienta, es posible conocer la “ubicación deseada” para el

punto muñeca:

En muchas ocasiones (p. ej. con una herramienta tipo garra), la posición “deseada” del punto
muñeca puede obtenerse a partir de la localización deseada de la herramienta por medio de una
simple traslación a lo largo de z6 de magnitud -d6 (o d6):
pos _ deseada _ muñeca = PdT (loc _ deseada _ TOOL * trasl (0,0,−d 6 ) =

⎛ ⎛ nx ox ax px ⎞ ⎛ 1 0 0 0 ⎞⎞
⎜⎜ ⎟ ⎜ ⎟ ⎟ ⎛ px − d6 ax ⎞
⎜⎜ n oy ay py ⎟ ⎜ 0 1 0 0 ⎟⎟ ⎜ ⎟
= PdT ⎜ ⎜ y * ⎟ = ⎜ p y − d6a y ⎟
n oz az pz ⎟ ⎜ 0 0 1 − d6 ⎟
⎜⎜ z ⎟ ⎜ ⎟ ⎟ ⎜⎝ p z − d 6 a z ⎟⎠
⎜
⎜ 0 1 ⎟⎠ ⎜⎝ 0 1 ⎟⎠ ⎟⎠
⎝⎝ 0 0 0 0
Por otro lado, la posición de la muñeca es función de las 3 primeras variables articulares:
( )
pos _ muñeca = PdT 0T1*1T2 *2 T3 * trasl (0,0, d 4 ) = f ( q1 , q2 , q3 )
Igualando pos_muñeca y pos_deseada_muñeca → 3 ec. y 3 incógnitas → ¡despejar q1 q2 y q3!

2. Resolución de la muñeca.
Punto de partida:
• Las primeras variables articulares: q1 q2 y q3

• La orientación deseada para la herramienta:
⎛ nx ox ax ⎞
⎜ ⎟
ori _ deseada _ TOOL = ⎜ n y oy ay ⎟
⎜n oz az ⎟⎠
⎝ z
Planteamiento…
⎛ nx ox ax ⎞
⎜ ⎟
0 0 3 3
( )
R6 = R3 * R6 ⇒ R6 = f (θ 4 ,θ 5 ,θ 6 ) = R3
0 −1
* ⎜ ny oy ay ⎟
9 ecuaciones y 3 incognitas: ¡Despejar!
⎜n oz a z ⎟⎠
⎝ z

Consejos prácticos:
• Resolver en los 4 cuadrantes (atan2 en lugar de atan)

• Desdoblar soluciones si sólo se puede despejar el seno o el coseno
Ecuaciones típicas: a cos q i + b sin qi = c
Solución (si a 2 + b 2 ≥ c 2 ): b
⎛c⎞ ⎧Φ = atan 2(b, a ) Φ

qi = Φ ± arccos⎜ ⎟ con ⎨
⎝r⎠ ⎩r = + a +b
2 2
qi
a cos qi − b sin qi = f
Otro caso habitual: a sin q + b cos q = g qi a
i i
c
⎛ag −b f a f +bg ⎞
Solución: q i = atan 2⎜ 2 , 2 ⎟
⎝ a +b a + b2 ⎠
2

Ejemplo
Robot polar de 3 articulaciones θi αi ai di
1 θ1 90º 0 l1
2 θ2 -90º 0 0
3 0º 0º 0 d3
⎛ C1 0 S1 0⎞
⎜ ⎟
⎜S 0 − C1 0⎟
0
T1 = ⎜ 1
0 1 0 l1 ⎟
⎜ ⎟
⎜0 1 ⎟⎠
⎝ 0 0
⎛ C2 0 − S2 0⎞ ⎛1 0 0 0⎞
⎜ ⎟ ⎜ ⎟
⎜S 0 C2 0⎟ ⎜0 1 0 0⎟
1
T2 = ⎜ 2 ; 2
T3 = ⎜
0 −1 0 0⎟ 0 0 1 q3 ⎟
⎜ ⎟ ⎜ ⎟
⎜0 1 ⎟⎠ ⎜0 0 0 1 ⎟⎠
⎝ 0 0 ⎝
Ejemplo
En este caso sólo es de aplicación la 1ª parte del método. Partiríamos de un sistema de 3
ecuaciones y 3 incógnitas:
⎛? ? ? px ⎞ ⎛ C1C2 − S1 − C1S 2 − q3C1S 2 ⎞

⎜ ⎟ ⎜ ⎟
⎜? ? ? p y ⎟ 0 1 2 ⎜ S1C2 C1 − S1S 2 − q3 S1S 2 ⎟
⎜? = T1* T2 * T3 ⎜
? ? pz ⎟ S 0 C2 q3C2 + l1 ⎟
⎜ ⎟ ⎜ 2 ⎟
⎜0 ⎟ ⎜ ⎟
⎝ 0 0 1⎠ ⎝ 0 0 0 1 ⎠
⎧ py
⎪⎪ S1 = − q S
1ª articulación: Despejando de las ecs. 1 y 2: ⎨⎪
3 2
px
C1 =
⎩⎪ − q3 S 2
Dado que q3 es siempre >0, el valor de q1 dependerá únicamente del signo de q2:
• Solución “de frente”: S2<0

• Solución “de espaldas”: S2>0
⎧ q1 = arctan 2( p y , p x )
⎨
⎩q1 = arctan 2(− p y ,− p x )

Ejemplo
2ª articulación
px
Despejando S2 de la ecuación 1: − q3C1S 2 = p x ⇒ S 2 = −
q3C1
px ⎫
S2 = −
q3C1 ⎪⎪
⇒ q2 = arctan 2(− p x / C1 , p z − l1 )
Considerando ahora la ec. 3 tendremos: C = p z − l1 ⎬⎪
q3 ⎪⎭
2
¿Configuraciones? Como q2= f(q1), las posibles soluciones (frente/espaldas) ya están

implícitamente consideradas.
3ª articulación
p z − l1
Despejando de la ec. 3: q3 =
C2
Ejemplo 2ª fase
2ª fase del método (revisión de Barrientos, pg. 145-146):
Punto de partida:
• Las tres primera articulaciones (q1, q2 y q3)

• La orientación deseada (matriz noa)
• Las expresiones de las 3 últimas matrices de paso
⎛ C4 0 − S4 ⎞ ⎛ C5 0 S5 ⎞ ⎛ C6 − S6 0⎞
⎜ ⎟ ⎜ ⎟ 5 ⎜ ⎟
3
R4 = ⎜ S 4 0 C4 ⎟; 4
R5 = ⎜ S5 0 − C5 ⎟; R6 = ⎜ S6 C6 0⎟
⎜0 − 1 0 ⎟⎠ ⎜0 0 ⎟⎠ ⎜0 1 ⎟⎠
⎝ ⎝ 1 ⎝ 0
⎛ C 4 C5 C 6 − S 4 S 6 − C4C5 S 6 − S 4C6 C4 S 5 ⎞
⎜ ⎟
R6 = ⎜ S 4C5C6 + C4 S 6
3
− S 4C5 S6 + C4C6 S 4 S5 ⎟
Contribución de la muñeca a la orientación total:
⎜ − S 5C 6 S5 S6 C5 ⎟⎠
⎝

Ejemplo 2ª fase
⎛ nx ox a x ⎞ ⎛ C 4 C5 C 6 − S 4 S 6 − C 4 C5 S 6 − S 4 C 6 C4 S5 ⎞
⎜ ⎟ ⎜ ⎟
Planteamiento de las ecuaciones:
(R)
0
3
−1
⎜ ny oy a y ⎟ = ⎜ S 4 C5 C 6 + C 4 S 6 − S 4 C5 S 6 + C 4 C 6 S 4 S5 ⎟
⎜n oz a z ⎟⎠ ⎜⎝ − S 5C 6 S5 S 6 C5 ⎟⎠
⎝ z
El miembro de la izda. es conocido (de la 1ª fase y de la orientación deseada) Aunémoslo por

comodidad en una matriz (rij) y replanteemos la igualdad:
⎛ r11 r12 r13 ⎞ ⎛ C4C5C6 − S 4 S 6 − C4C5 S 6 − S 4C6 C4 S 5 ⎞

⎜ ⎟ ⎜ ⎟
⎜ r21 r22 r23 ⎟ = ⎜ S 4C5C6 + C4 S 6 − S 4C5 S6 + C4C6 S 4 S5 ⎟
⎜r r33 ⎟⎠ ⎜⎝ − S 5C6 C5 ⎟⎠
⎝ 31 r32 S5 S6
9 ecuaciones, 3 incógnitas (elevada redundancia). La selección de las ecuaciones sigue cierta

política:
• Simplicidad (se elige la 3ª fila y la 3ª columna)

• Despejar si es posible, tanto el seno como el coseno de cada variable (evitar desdobles)
Ejemplo 2ª fase
⎧ C5 = r33
⎪ S S =r
⎪⎪ 4 5 23
⎨ C4 S5 = r13
Ecuaciones seleccionadas: ⎪ S S = r
⎪ 5 6 32
⎪⎩− S5C6 = r31
De la 1ª ecuación:
⎧ θ > 0 configuración " flip"

θ 5 = ±acos(r33 )⎨ 5 ¡ELEGIR!
⎩θ 5 < 0 configuración " noflip"
⎛r r ⎞
Un vez elegida la opción, de las ecs. 2 y 3: θ 4 = atan2⎜⎜ S , S ⎟⎟ válida ∀ config.
23 13
⎝ 5 5 ⎠
⎛r −r ⎞
Y finalmente, de las ecs. 4 y 5: θ 6 = atan2⎜⎜ S , S ⎟⎟ válida ∀ config.
32 31
⎝ 5 5 ⎠

- Introducción
Enfoque geométrico
Modelo cinemático: el jacobiano

Modelos geométricos directo e inverso: X = f (q ) q = f −1 ( X )
Modelo cinemático: relación entre las velocidades articulares y operacionales: X& = J ( q ) q&
→ J(q) es la matriz jacobiana del robot
Su expresión/obtención depende fuertemente de cómo se exprese la velocidad del órgano

terminal en el espacio operacional:
• Velocidad expresada en su propia referencia: X n x y( z x y z )

& = n V = n v ,n v , n v ,n w , n w ,n w t = n J (q )q&
n
=( v , v , v , w , w , w )= J
t
• Velocidad expresada en la referencia 0: & =0 V
X 0 0 0 0 0 0 0
(q )q&
n x y z x y z n
⎛ 0 Rn 0 ⎞n
⎜ ⎟ J n (q )
Relación entre ambas versiones: J n (q ) =⎜ 0
0
⎝
0
R n ⎟⎠

0
Obtención analítica de J n (q ) a partir del modelo geométrico directo: → diferenciar f
La matriz jacobiana puede fraccionarse en 2 partes diferenciadas:
⎛ 0 Jpos 3 (q ) ⎞
0
J 3 (q ) = ⎜⎜ 0 ⎟
⎟
⎝ Jori 3 (q ) ⎠
0
Diferenciando la parte de posición, obtendremos la 1ª parte de la matriz Jpos 3 (q ) …
⎧ dp x n
∂f x ⎫ ⎛ ∂f x ∂f x ⎞
p
⎪ x = f x ( q K q n ) → v x = = ∑ ∂
q&i ⎪ ⎜ L ⎟
⎜ ∂q1 ∂q n ⎟
1
⎪ dt i = 1 q i ⎪
⎪ dp n ∂f
⎪ ⎜ ∂f ∂f y ⎟
X = f ( q ) ⇒ ⎨ p y = f y ( q1 K qn ) → v y = y = ∑ y q&i ⎬⇒ 0 Jpos n (q ) = ⎜ y L ⎟
⎪ dt i =1 ∂qi ⎪ ⎜ ∂q1 ∂q n ⎟
⎪ dp z n
∂f z ⎪ ⎜ ∂f z ∂f z ⎟
⎪ p z = f z ( q1 K qn ) → vz = dt = ∑ ∂q q&i ⎪ ⎜ ∂q L
∂qn ⎟⎠
⎩ i =1 i ⎭ ⎝ 1

Diferenciando la parte de orientación y despejando adecuadamente, obtendremos la 2ª parte de
0
la matriz Jori 3 (q ) …
⎛ 0 − wz wy ⎞
dR n
∂R ⎜ ⎟
=∑ q&i = ⎜ wz 0 − wx ⎟R ⇒ 0 Jori n (q )
dt i =1 ∂qi ⎜− w
⎝ y wx 0 ⎟⎠
" Elevada complejidad computacional: existen otros métodos más eficientes
Características de la matriz jacobiana
• Si nº de articulaciones coincide con el nº de gdl de la tarea, entonces J(q) es cuadrada

• Dificultad de cómputo (depende de la configuración)
• En las configuraciones singulares, el determinante (jacobiano) se anula

Utilidad de la matriz jacobiana
1. Transformación de velocidades entre los espacios operacional y articular: Vn = J n (q )q&
0 0
2. Transformación de esfuerzos entre los 2 espacios (muy útil si interacción con el entorno):
Sean fe y fq los vectores de esfuerzos en la herramienta (sensor) y en las articulaciones:
fe = ( f x , f y , f z , τ x , τ y ,τ z ) ; f q = (τ 1 ,τ 2 , τ 3 ,τ 4 , τ 5 ,τ 6 )
t t
⎛ τ1 ⎞ ⎛ fx ⎞
⎜ ⎟ ⎜ ⎟
⎜τ 2 ⎟ ⎜ fy ⎟
⎜τ ⎟ ⎜f ⎟
t n t t n t
fe ⋅ Vn = f q ⋅ q& ⇒ fe ⋅ J n (q )q& = f q ⋅ q& ⇒ ⎜ 3
⎟ = J n (q )⎜ z ⎟
n t
⎜τ 4 ⎟ ⎜τ x ⎟
⎜τ ⎟ ⎜τ ⎟
⎜ 5⎟ ⎜ y⎟
⎜τ ⎟ ⎜τ ⎟
⎝ 6⎠ ⎝ z⎠
3. Obtención numérica del modelo geométrico inverso basado en la matriz jacobiana inversa
Ejemplo de obtención
Robot polar de 3 articulaciones
⎛ C1C 2 − S1 − C1 S 2 − q3C1 S 2 ⎞
⎜ ⎟
⎜S C C1 − S1 S 2 − q3 S1 S 2 ⎟
0
T3 = ⎜ 1 2
S 0 C2 q3C 2 + l1 ⎟
⎜ 2 ⎟
⎜ 0 ⎟
⎝ 0 0 1 ⎠

Diferenciando la parte de posición…
p x = − q3C1S 2 → vx = q3 S1S 2 q&1 − q3C1C2 q&2 − C1S 2 q&3 ⎫ ⎛ q3 S1S 2 − q3C1C2 − C1S 2 ⎞
⎪ 0 ⎜ ⎟
p y = − q3 S1S 2 → v y = − q3C1S 2 q&1 − q3 S1C2 q&2 − S1S 2 q&3 ⎬⇒ Jpos 3 (q ) = ⎜ − q3C1S 2 − q3 S1C2 − S1S 2 ⎟
p z = q3C2 + l1 → vz = − q3 S 2 q&2 + C2 q&3 ⎪ ⎜ − q3 S 2 C2 ⎟⎠
⎭ ⎝ 0
Diferenciando la parte de orientación…
Primero obtenemos la derivada de la matriz de rotación del modelo geométrico directo:
⎛ − S1C2 q&1 − C1S 2 q&2 − C1q&1 S1S 2 q&1 − C1C2 q&2 ⎞

n
∂ R ∂ R ∂ R ∂ R ⎜ ⎟
& =∑
R q&i = q&1 + q&2 + q&3 = ⎜ C1C2 q&1 − S1S 2 q&2 − S1q&1 − C1S 2 q&1 − S1C2 q&2 ⎟
i =1 ∂qi ∂q1 ∂q 2 ∂q3 ⎜ ⎟
⎝ C2 q&2 0 − S 2 q&2 ⎠
Planteamiento para obtener la relación entre w y q’:
⎛ 0 − wz wy ⎞ ⎛ 0 − wz wy ⎞
⎜ ⎟ ⎜ ⎟
& =⎜ w
R 0 − wx ⎟R ⇒ ⎜ wz 0 − wx ⎟ = R& Rt =
z
⎜− w wx 0 ⎟⎠ ⎜− w wx 0 ⎟⎠
⎝ y ⎝ y
⎛ − S1C2 q&1 − C1S 2 q& 2 − C1q&1 S1S 2 q&1 − C1C2 q& 2 ⎞⎛ C1C2 S1C2 S2 ⎞
⎜ ⎟⎜ ⎟
= ⎜ C1C2 q&1 − S1S 2 q& 2 − S1q&1 − C1S 2 q&1 − S1C2 q& 2 ⎟⎜ − S1 C1 0⎟
⎜ C2 q& 2 − S 2 q& 2 ⎟⎜ − C S − S S C2 ⎟⎠
⎝ 0 ⎠⎝ 1 2 1 2
Despejando de los elementos (3,2) (1,3) y (2,1) de la matriz resultante obtenemos…
wx = S1q&2 ⎫ ⎛ 0 S1 0⎞
⎪ 0 ⎜ ⎟
wy = −C1q&2 ⎬⇒ Jori 3 (q ) = ⎜ 0 − C1 0⎟
wz = q&1 ⎪ ⎜1 0 ⎟⎠
⎭ ⎝ 0

Finalmente:
⎛ q3 S1S 2 − q3C1C2 − C1S 2 ⎞

⎜ ⎟
⎜ − q3C1S 2 − q3 S1C2 − S1S 2 ⎟
⎛ Jpos 3 (q ) ⎞ ⎜
0
0 − q3 S 2 C2 ⎟
0
J 3 (q ) = ⎜⎜ 0 ⎟=⎜ ⎟
⎟
⎝ Jori 3 (q ) ⎠ ⎜ 0 S1 0 ⎟
⎜ 0 − C1 0 ⎟⎟
⎜
⎜ 0 ⎟⎠
⎝ 1 0
Debido a la simplicidad de este robot, algunos valores de la matriz jacobiana (la parte de la
velocidad angular) eran previsibles de manera intuitiva.
Jacobiana inversa
Obtención entre las velocidades articulares a partir de la velocidad del órgano terminal:
& = J(q) q& ⇒ q& = J * (q)X

X &
¿J*(q)? Si la dimensión de J(q)= m x n (m: GDL de la tarea, n: nº de articulaciones)…
−1
• Si m = n, J (q) = J (q)
*
(
• Si m > n, J (q) = J (q) ⋅ J(q)
* t
)
−1
J t (q) pseudoinversa izquierda (solución más plausible)
( −1
)
• Si m < n, J (q) = J (q) ⋅ J(q) ⋅ J (q) pseudoinversa derecha ( q& mínima de entre ∞)
* t t
Obtención problemática:
• Inversión simbólica a partir de J(q) → gran dificultad

• Inversión numérica de J(q) → problemas en las proximidades de las singularidades
• Diferenciación a partir del modelo geométrico inverso → problemas con soluciones
múltiples.

Jacobiana inversa y las singularidades
Configuraciones singulares: la jacobiana inversa no puede calcularse. Casos posibles:
• Si m = n, J(q) = 0 Singularidad si jacobiano nulo

• Si m > n, J (q) ⋅ J(q) = 0
t
• Si m < n, J(q) ⋅ J (q) = 0

t
" Velocidades finitas en el espacio de la tarea se corresponden con velocidades ∞ en el

espacio articular (para una o más articulaciones).
Para el robot polar de 3 articulaciones, considerando su uso exclusivo en tareas de posicionado

(3 GDL → considerar 0Jpos3(q)), puede comprobarse que se dan singularidades en q2 OR q3 = 0:
q3 S1S 2 − q3C1C2 − C1S 2

Jpos 3 (q ) = − q3C1S 2 − q3 S1C2 − S1S 2 = − S 2 q3
0 2
0 − q3 S 2 C2
Obtención numérica del modelo geométrico

inverso por medio del jacobiano:
Dado Xd, ¿q? Algoritmo:
q:=q0 (valor “semilla”)

repetir
X:=f(q)
∆X:= Xd - X
∆q:=J-1(∆X)
q:=q+∆q
hasta que ∆X<ε
" La matriz jacobiana inversa se obtiene numéricamente

" Posibles problemas de convergencia

3 Cinem Tica 2

Cargado por

Información del documento

Título original

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

3 Cinem Tica 2

Cargado por

Copyright:

Formatos disponibles

Cinemática de manipuladores

Robótica Industrial. CINEMÁTICA

• La relación entre las variables espaciales ligadas a las articulaciones (coordenadas

mundo del robot ↔ mundo operacional

A nivel espacial, 2 modelos fundamentales a obtener:

Robótica Industrial. CINEMÁTICA

Coordenadas cartesianas Coordenadas cilíndricas Coordenadas esféricas

Simplicidad y generalidad: coordenadas cartesianas

Traslación → cambio de posición

Robótica Industrial. CINEMÁTICA

Rotación → cambio de orientación (también puede modificar la posición)

⎧ px = i p = i i' px' + i j' p y' + i k' pz'

⎛ i i' i j' i k' ⎞

⎛ CφCθCψ − SφSψ − CφCθSψ − SφCψ CφSθ ⎞

A partir de la matriz de rotación (Tomando θ entre 0 y π):

φ = atan2 (ay, ax)

Robótica Industrial. CINEMÁTICA

A partir de la matriz (tomando θ entre –π/2 y π/2):

φ = atan2 (ny, nx)

Robótica Industrial. CINEMÁTICA

⎛ rx 2 (1 − Cθ) + Cθ rx ry (1 − Cθ) − rz Sθ rx rz (1 − Cθ) + ry Sθ ⎞

Robótica Industrial. CINEMÁTICA

Robótica Industrial. CINEMÁTICA

• Composición de rotaciones: producto de cuaterniones

• Traslación p seguida de rotación Q: [0, r '] = Q * [0, r + p ]* Q

Robótica Industrial. CINEMÁTICA

Representación de la localización: matrices de

• R es la submatriz ortonormal de rotación (Rt=R-1, |R|=1)

Robótica Industrial. CINEMÁTICA

Robótica Industrial. CINEMÁTICA

representación de la situación (posición + orientación) relativa entre sistemas de referencia

Robótica Industrial. CINEMÁTICA

¿Situación del taladro respecto a ABS? ATT = ATM * MTB * BTT

Robótica Industrial. CINEMÁTICA

Vector de posición + ángulos de Euler/RPY (x,y,z,φ,θ,ψ)

• Muy compacta (mínima)

Vector de posición + par de rotación (x,y,z, rx, ry,rz,θ)

Vector de posición + quaternios (x,y,z,q0,q1,q2,q3)

Se usa en algunos lenguajes (rapid – ABB)

Robótica Industrial. CINEMÁTICA

Modelo geométrico directo

X: Situación de la herramienta respecto de la base: coordenadas operacionales o de la tarea

• Bien sea como transformación: X = 0T6

1. Identificación y etiquetado de sólidos y ejes

Robótica Industrial. CINEMÁTICA

Modelo geométrico directo

3. Determinación de la situación relativa entre sólidos consecutivos (transformaciones de

4. Composición de transformaciones en pos de la situación absoluta del último eslabón

Robótica Industrial. CINEMÁTICA

Robótica Industrial. CINEMÁTICA

Modelo geométrico directo

ai: distancia entre zi-1 y zi medida a lo largo de xi

Robótica Industrial. CINEMÁTICA

1.- Giro θi en torno a zi-1 ⇒ ¡xi-1 y xi paralelos!

Expresando esta sucesión respecto al sistema de referencia asociado al sólido i-1:

Si el enlace es rotacional, el único parámetro variable es θi , siendo ésta la coordenada

Modelo geométrico directo

b) Método de las matrices intermedias U

• La asignación de referencias influye en el nº de cáculos (será menor si ai y/o di nulos).

Robótica Industrial. CINEMÁTICA

ABB IRB 2400

Robótica Industrial. CINEMÁTICA

Robótica Industrial. CINEMÁTICA

Enfoque geométrico (Ejemplos en Barrientos 07 y en Fu - Gonzalez – Lee 88)