TEMA 4: CINEMÁTICA DEL ROBOTIngeniería de Sistemas y Automática Control de Robots y Sistemas Sensoriales Robótica Industrial ISA.- Ingeniería de Sistemas y Automática Cinemática del robot Cinemática directa Cinemática Inversa Matriz Jacobiana Robotica Industrial- Cinemática del robot 2 El problema cinemático de un robot s Cinemática del robot : Estudio de su movimiento con respecto a un sistema de referencia – Descripción analítica del movimiento espacial en función del tiempo – Relaciones localización del extremo del robot-valores articulares s 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 ángulos 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 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 s s Robotica Industrial- Cinemática del robot 3 γ) Cinemática Inversa Robotica Industrial.β.z.y.….q2.Cinemática del robot 4 .Relación entre cinemática directa e inversa Cinemática Directa Valor de las coordenadas articulares (q1.α.qn) Posición y orientación del extremo del robot (x. q4 . q6 ) Robotica Industrial. q5 . q3 . q5 . q5 . q2 . q4 . q5 . q3 . q2 . q4 . q3 . q2 . q3 . q2 . q2 .Resolución del problema cinemático directo con matrices de transformación homogéneas s Objetivo: Encontrar una matriz de transformación homogénea T que relacione posición y orientación del extremo del robot con respecto a un sistema de referencia fijo situado en su base x = f x (q1 . q5 . q5 . q6 ) z = f z (q1 .Cinemática del robot 5 . q3 . q3 . q2 . q6 ) β = fβ (q1 . q4 . q6 ) α = fα (q1 . q6 ) γ = f γ (q1 . q4 . q4 . q6 ) y = f y (q1 . Modelo cinemático directo de un robot planar de 2 gdl x = l1 cos q1 + l 2 cos (q1 + q2 ) y = l1 sen q1 + l 2 sen (q1 + q2 ) Robotica Industrial.Cinemática del robot 6 . Cinemática del robot 7 .Las matrices de transformación AyT s s Matriz i-1Ai : matriz de transformación homogénea que representa la posición y orientación relativa entre los sistemas asociados a dos eslabones consecutivos del robot Conexión de matrices A: 0 A 3 = 0 A11 A 2 2 A 3 s Matriz T : matriz 0An cuando se consideran todos los grados de libertad del robot T= 0 A 6 = 0 A1 1 A 2 2 A 3 3 A 4 4 A 5 5 A 6 Robotica Industrial. 0. vector ai (0.Convenio de conexión de elementos contiguos de Denavit-Hartenberg s Transformaciones básicas de paso de eslabón: Rotación alrededor del eje zi-1 un ángulo θi Traslación a lo largo de zi-1 una distancia di . d i ) T (a i .di) Traslación a lo largo de xi una distancia ai .Sα i Cα i 0 0 0 = 0 1 Robotica Industrial.0) T ( x .0.ai) Rotación alrededor del eje xi un ángulo αi i-1 A i = T ( z.0. vector di (0. θ i ) T (0.0. α i ) Cθ i Sθ i i −1 Ai = 0 0 Cθi Sθ i = 0 0 − Sθ i Cθ i 0 0 0 0 1 0 − Cα i Sθi Cα i Cθi Sα i 0 0 0 1 0 0 0 1 d i 0 0 1 0 Sα i Sθi ai Cθi − Sα i Cθi ai Sθi Cα i di 0 1 0 1 0 0 0 0 1 0 0 1 0 0 0 1 0 0 0 a i 1 0 0 0 0 Cα i 1 0 0 Sα i 0 1 0 0 0 .Cinemática del robot 8 . αi) – Dos distancias (di. ai) Robotica Industrial.Cinemática del robot 9 .Parámetros de Denavit-Hartenberg (I) s s s s Definen el paso de un sistema de referencia asociado a una articulación al siguiente Sólo dependen de las características geométricas de cada eslabón y de las articulaciones que le unen con el anterior y siguiente (no dependen de la posición del robot) Definen las matrices A que permiten el paso de un sistema de referencia asociado a una articulación al siguiente y por tanto definen las matrices T Son 4: – Dos ángulos (θi. En el caso de articulaciones prismáticas. utilizando la regla de la mano derecha. medido en un plano perpendicular al eje xi. 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.Parámetros de Denavit-Hartenberg (II) s s s s θi: Es el ángulo que forman los ejes xi-1 y xi medido en un plano perpendicular al eje zi-1. 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. utilizando la regla de la mano derecha. Se trata de un parámetro variable en articulaciones giratorias. Robotica Industrial.Cinemática del robot 10 . se calcula como la distancia más corta entre los ejes zi-1 y zi. αi: Es el ángulo de separación del eje zi-1 y el eje zi. en el caso de articulaciones giratorias. Se trata de un parámetro variable en articulaciones prismáticas. Cinemática del robot 11 .Parámetros de Denavit-Hartenberg para un eslabón giratorio Robotica Industrial. n (n=número de gdl).Cinemática del robot 12 .zi) donde i=1.2.…..yi.. Cada sistema de coordenadas corresponderá a la articulación i+1 y estará fijo en el elemento i Encontrar los parámetros D-H de cada una de las articulaciones Calcular las matrices Ai Calcular la matriz Tn = 0A1 1A2 . n-1An Robotica Industrial.Obtención del modelo cinemático directo de un robot Establecer para cada elemento del robot un sistema de coordenadas cartesiano ortonormal (xi. el eje será su propio eje de giro. será el eje a lo largo del cual se produce el desplazamiento.Situar el origen del sistema de la base {S0} en cualquier punto del eje z0.Algoritmo de Denavit-Hartenberg (I) s s s s s D-H 1. Se numerará como eslabón 0 a la base fija del robot.Para i de 0 a n-1 situar el eje zi sobre el eje de la articulación i+1.. Los ejes x0 e y0 se situarán de modo que formen un sistema dextrógiro con z0 Robotica Industrial. Si ésta es rotativa..Numerar cada articulación comenzando por 1 (la correspondiente al primer grado de libertad) y acabando en n D-H 3.Cinemática del robot 13 . D-H 5. Si es prismática..Localizar el eje de cada articulación... D-H 2.Numerar los eslabones comenzando con 1 (primer eslabón móvil de la cadena) y acabando con n (último eslabón móvil). D-H 4. Si fuesen paralelos {Si} se situaría en la articulación i+1 D-H 7. D-H 11.Cinemática del robot 14 .Obtener θi como el ángulo que hay que girar en torno a zi-1 para que xi-1 y xi queden paralelos...Para i de 1 a n-1.Obtener di como la distancia. 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.. D-H 10.. medida a lo largo de zi-1..Situar yi de modo que forme un sistema dextrógiro con xi y zi . D-H 9. que habría que desplazar {Si-1} para que xi y xi-1 quedasen alineados.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 .Algoritmo de Denavit-Hartenberg (II) s s s s s s D-H 6.. Robotica Industrial.Situar xi en la línea normal común a zi-1 y zi D-H 8. Si ambos ejes se cortasen se situaría {Si} en el punto de corte. 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}..Obtener las matrices de transformación i-1Ai DH 15.Cinemática del robot 15 ..Algoritmo de Denavit-Hartenberg (III) s s s s s DH 12. DH 14..Obtener la matriz de transformación entre la base y el extremo del robot T = 0A1 1A2 . para que el nuevo {Si-1} coincidiese totalmente con {Si}..Obtener αi como el ángulo que habría que girar entorno a xi (que ahora coincidiría con xi-1).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 Robotica Industrial. DH 16.... DH 13. n-1An. Modelo cinemático directo de un robot cilíndrico (I) Articulación 1 2 3 4 θ q1 90º 0 q4 d l1 d2 d3 l4 a 0 0 0 0 α 0 90º 0 0 Robotica Industrial.Cinemática del robot 16 . Cinemática del robot .Modelo cinemático directo de un robot cilíndrico (II) C1 S 1 0 A1 = 0 0 1 0 2 A3 = 0 0 − S1 C1 0 0 0 1 0 0 0 0 1 0 0 0 l1 1 0 1 1 A2 = 0 0 C4 S 4 3 A4 = 0 0 0 0 1 0 1 0 0 0 0 d2 0 1 − S4 0 C4 0 0 1 0 0 0 0 0 0 1 d3 0 1 0 0 l4 1 − S1C4 CC 1 4 T = 0 A11 A 2 2 A 3 3 A 4 = S4 0 S1S 4 − C1S4 C4 0 C1 C1 ( d3 + l4 ) S1 S1 ( d3 + l4 ) 0 d 2 + l1 0 1 17 Robotica Industrial. Modelo cinemático directo de un robot ABB IRB 6400C (I) Articulación 1 2 3 4 5 6 θ θ1 θ2 θ3-90 θ4 θ5 θ6 d 0 l1 0 l3 0 l4 a 0 0 -l2 0 0 0 α -90 90 90 -90 90 0 Robotica Industrial.Cinemática del robot 18 . Cinemática del robot .Modelo cinemático directo de un robot ABB IRB 6400C (II) C1 S 1 0 A1 = 0 0 S3 − C 3 2 A3 = 0 0 0 − S1 0 C1 0 0 − 1 0 0 0 0 1 0 − C3 0 − S3 1 0 0 0 − l2 S3 l2C3 0 1 C 2 S 2 1 A2 = 0 0 C4 S 4 3 A4 = 0 0 0 S2 0 − C2 1 0 0 0 0 − S4 C4 0 −1 0 0 0 0 0 l1 1 0 0 l3 1 C5 S 5 4 A5 = 0 0 0 S5 0 − C5 1 0 0 0 0 0 0 1 C6 S 6 5 A6 = 0 0 − S6 C6 0 0 0 0 l 1 4 0 1 0 0 19 Robotica Industrial. Modelo cinemático directo de un robot ABB IRB 6400C (III) nx n y 0 1 2 3 4 5 T = A1 A 2 A 3 A 4 A5 A 6 = nz 0 ox oy oz 0 ax ay az 0 px py pz 1 n x = ( C1 C2 S3 + S1C3 )( C4C5C6 − S4 S6 ) + C1S2 ( S 4C5C6 + C4 S6 ) + ( − C1C2C3 + S1S3 ) S5C6 n z = ( − S2 S3 )( C4C5C6 − S 4 S6 ) + C2 ( S4C5C6 + C4 S6 ) + S2C3S5C6 n y = ( − S1C 2 S3 + S1C3 )( C4C5C6 − S4 S6 ) + S1S2 ( S4C5C6 + C4 S6 ) + ( − S1C2C3 − C1S3 ) S5C6 ox = ( C1 C2 S3 + S1C3 )( − C4C5C6 − S4 S6 ) + C1S 2 ( − S4C5C6 + C4 S6 ) + ( − C1C2C3 + S1S3 )( − S5C6 ) oz = ( − S2 S3 ) ( − C4C5C6 − S4 S 6 ) + C2 ( − S4C5C6 + C4 S6 ) + S 2C3 ( − S5C6 ) oy = ( − S1C2 S3 + S1C3 )( − C4C5C6 − S4 S6 ) + S1S2 ( − S4C5C6 + C4 S6 ) + ( − S1C2C3 − C1S3 )( − S5C6 ) p x = ( C1 C 2 S3 + S1C3 )( l4C4 S5 ) + C1S2 ( l4 S4 S5 ) + ( C1 C2C3 + S1S3 )( − l4C5 + l3 ) + py = pz = (− l C C S − l S C − l S ) ( − S C S − C C )(l C S ) + S S ( l S S ) + ( − C C C − C S )( − l C (− l S C S − l C C + l C ) (.S S )(l C C ) + C (l S S ) + S C ( − l C + l ) + l S S 2 1 2 3 2 1 3 1 1 1 2 3 1 3 4 4 5 1 2 4 4 5 1 2 3 1 3 4 2 1 2 3 3 2 1 3 1 1 2 3 4 4 5 2 4 4 5 2 4 5 3 2 2 3 5 + l3 ) + Robotica Industrial.Cinemática del robot 20 . – Condiciones suficientes para que exista: ( ( Tres ejes de articulación adyacentes interseccionan en un punto (robot PUMA y robot Stanford) Tres ejes de articulación adyacentes son paralelos entre sí (robot Elbow) Robotica Industrial.Cinemática Inversa s s s s Objetivo: encontrar los valores que deben adoptar las coordenadas articulares del robot para que su extremo se posicione y oriente según una determinada localización espacial La resolución no es sistemática Depende de la configuración del robot (soluciones múltiples) No siempre existe solución en forma cerrada.Cinemática del robot 21 . z.γ).Cinemática del robot 22 .α. k = 1.….y.n Posibilidad de resolución en tiempo real ( Posibilidad de selección de la solución más adecuada ( Posibilidad de simplificaciones ( No siempre es posible ( Robotica Industrial.β.Posibilidades de solución del problema cinemático inverso Procedimiento genérico a partir de los parámetros D-H Método iterativo ( Problemas de velocidad y convergencia ( Búsqueda de solución cerrada: qk = fk (x. cuaterniones duales..Métodos de solución del problema cinemático inverso Métodos geométricos – Se suele utilizar para las primeras variables articulares – Uso de relaciones geométricas y trigonométricas (resolución de triángulos) Resolución a partir de las matrices de transformación homogénea – Despejar las n variables qi en función de las componentes de los vectores n. Desacoplamiento cinemático – En robots de 6 GDL – Separación de orientación y posicionamiento Otros: álgebra de tornillo.Cinemática del robot 23 ..métodos iterativos. o. Robotica Industrial. a y p. Cinemática del robot 2 2 2 2 px + py + pz − l2 2 − l3 2l2l3 24 .Ejemplo de resolución de la cinemática inversa por métodos geométricos (I) py q1 = arctg px 2 2 + py r 2 = px 2 2 = l2 r 2 + pz 2 + l 3 + 2l 2 l 3cos q 3 ⇒ cos q 3 = 2 2 2 2 + py + pz − l2 px 2 − l3 2l 2 l 3 sen q 3 = ± 1 − cos 2 q 3 ± 1 − cos2q 3 q3 = arctg cos q3 con cos q3 = Robotica Industrial. Ejemplo de resolución de la cinemática inversa por métodos geométricos (II) q2 = β − α pz β = arctg = arctg ± r l sen q3 α = arctg 3 l 2 + l3 cos q3 2 2 px + py pz q2 = arctg ± l 3 sen q 3 − arctg 2 2 l l c os q + + py px 2 3 3 pz Robotica Industrial.Cinemática del robot 25 . Cinemática del robot 26 .Ejemplo de resolución de la cinemática inversa a partir de las matrices de transformación homogénea (I) A r tic . 1 2 3 θ q1 q2 0 d l1 0 q3 a 0 0 0 α 90º -9 0 º 0 C1 S 1 0 A1 = 0 0 0 S1 0 − C1 1 0 0 0 − S1 C1 0 0 C1C2 S C 1 2 0 A2 = S2 0 0 0 l1 1 − C1S 2 − S1S2 C2 0 C2 S 2 1 A2 = 0 0 0 0 l1 1 0 0 0 1 C1C2 − S1 S C C1 1 2 T = 0 A3 = S2 0 0 0 0 − S2 C2 0 −1 0 0 0 1 0 0 0 0 1 0 0 2 A3 = 0 0 1 q3 0 0 0 1 − C1S2 − q3C1S2 − S1S2 − q3S1S2 C2 q3C2 + l1 0 1 Robotica Industrial. Cinemática del robot 27 .Ejemplo de resolución de la cinemática inversa a partir de las matrices de transformación homogénea (II) C1 S1 0 0 −1 0 0 1 2 ( A1 ) T3 = A2 A3 = S1 − C1 0 0 A3 T= 2 A3 ( A ) T= A ( A) ( A) 0 −1 1 1 1 −1 0 2 1 T = 0 A1 1 A 2 2 A 3 2 2 −1 C 2 S 2 = 0 0 0 − S2 0 C2 −1 0 0 0 0 1 0 0 0 0 1 0 0 1 0 0 0 0 1 0 0 0 nx ox ax px 1 − l1 ny oy a y py = 0 0 nz oz az pz 0 1 0 0 0 1 0 C2 0 − S2 − S2q3 C2 C2q3 0 S2 0 = q3 0 − 1 0 0 0 0 1 1 0 `py S1 px − C1 py = 0 ⇒ tan ( q1 ) = px ⇒ ` py q1 = arctan px q2 = arctan 2 2 + py px l1 − pz 2 2 + py q3 = C 2 ( pz − l1 ) − S2 px Robotica Industrial. α.y.Matriz Jacobiana Jacobiana Directa Velocidad de las articulaciones (q1.β.γ) .qn) Jacobiana Inversa Matriz Jacobiana: permite conocer las velocidades del extremo del robot a partir de las velocidades de cada articulación Robotica Industrial.q2.Cinemática del robot 28 velocidades del extremo del robot (x.z.…. q ) n 1 n ∂f = ∑ xq i x 1 ∂qi n =∑ α 1 n ∂fα q ∂qi i ∂f y =∑ q i y 1 ∂qi n ∂f β i β=∑ q 1 ∂qi n =∑ z ∂fz i q 1 ∂qi n ∂fγ i γ =∑ q 1 ∂qi n 1 q x y z J = ⋅ α β n γ q ∂fx ∂q 1 con J = ∂fγ ∂q 1 ∂fx ∂qn ∂fγ ∂qn Robotica Industrial. q ) β = f ( q . q ) x = fx ( q1 . q ) n 1 n z = f z ( q1 . . .Cinemática del robot 29 . q ) γ = f ( q . γ .Relaciones diferenciales . β . . q ) α = f ( q . α 1 n n y = f y ( q1 . Cinemática del robot 30 .Jacobiana de un robot SCARA x = l3C12 + l 2C1 y = l3S12 + l 2S1 z = l1 − q3 x − ( l3S12 + l 2S1 ) − l3S12 = l3C12 + l 2C1 l 3C12 y 0 0 z q 1 2 q 3 − 1 q 0 0 Robotica Industrial. α . y .Cinemática del robot 31 . γ ) 1 q x = J −1 n q γ ∂f1 ∂x J −1 = ∂fn ∂x ∂f1 ∂γ ∂fn ∂γ Robotica Industrial. β .Métodos de cálculo de la Jacobiana inversa s Inversión simbólica de la matriz jacobiana – Gran complejidad (matriz 6x6) s Evaluación numérica de J e inversión numérica – Necesidad de recómputo continuo – En ocasiones J no es cuadrada – En ocasiones | J | = 0 matriz pseudoinversa s A partir del modelo cinemático inverso q1 = f1 ( x . β . y . z . γ ) qn = fn ( x . α . z . Configuraciones singulares s s s s Aquellas en las que | J | = 0 (Jacobiano nulo) Incremento infinitesimal coordenadas cartesianas implica incremento infinito coordenadas articulares 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 s Requieren su estudio y eliminación Robotica Industrial.Cinemática del robot 32 .