Modelo Cinemático Inverso PDF

Modelo cinemático inverso Robótica Industrial Grado en Ingª. en Tecnologías Industriales, mención electrónica industrial 7º semestre Iñaki Arocena Elorza...

Modelo cinemático inverso Robótica Industrial Grado en Ingª. en Tecnologías Industriales, mención electrónica industrial 7º semestre Iñaki Arocena Elorza Dept. de Ingeniería Universidad Pública de Navarra 1 / 61 Modelo cinemático inverso Introducción El problema cinemático inverso El modelo cinemático inverso Resuelve las variables articulares para una posición y orientación espacial deseada. Sistema de ecuaciones no lineales: no existe ningún método de resolución general y son generalmente difíciles de resolver. En robótica se recurre a métodos alternativos cuya dificultad depende de la morfología del robot. Las morfologías típicas simplifican la resolución y proporcionan una solución cerrada (determinística en el tiempo). En los demás casos la solución numérica (no determinística) es la única alternativa (por ej. articulaciones redundantes). 3 / 61 Modelo cinemático inverso Introducción El problema cinemático inverso Número de soluciones Tres casos, dependiendo de las dimensiones de los espacios de tarea (m) y articular (n) n < m: sistema incompatible: No se puede alcanzar cualquier pose arbitraria (posición y orientación) en el espacio de tarea. n > m: sistema compatible indeterminado: Robot con articulaciones redundantes, existen infinitas soluciones (configuraciones) para un mismo punto. n = m: 0, 1 o múltiples soluciones, principal diferencia respecto a los sistemas lineales. 4 / 61 Modelo cinemático inverso Introducción El problema cinemático inverso Número de soluciones continuación... n = m: 0, 1 o múltiples soluciones: No existe solución: punto deseado está fuera del espacio de trabajo o limitaciones articulares en un punto interior. Solución única: limitación articular en el interior del volumen de trabajo o en su periferia. Múltiples soluciones: posibilidad de distintas configuraciones para una misma posición y orientación espacial: codo-arriba/abajo, hombro izquierda/derecha, etc. 5 / 61 Modelo cinemático inverso Introducción El problema cinemático inverso Ejemplo de configuraciones múltiples de un robot PUMA (a) Hombro derecho (b) Hombro izquierdo con codo arriba con codo arriba 6 / 61 Modelo cinemático inverso Introducción El problema cinemático inverso Ejemplo de configuraciones múltiples de un robot PUMA (c) Hombro derecho-codo (d) Hombro izquierdo-codo abajo abajo 7 / 61 Modelo cinemático inverso Introducción El problema cinemático inverso Métodos Las tipologías clásicas permiten la aplicación de métodos de resolu- ción “sencillos” y una solución cerrada. Existen fundamentalmente dos métodos: Geométrico: planteamiento trigonométrico en distintos planos que establecen una relación entre posición espacial y variables articulares. Transformación inversa: plantea ecuaciones (no lineales) con una incógnita entre los elementos de las matrices de transformación homogéneas deseadas y del modelo cinemático. 8 / 61 Modelo cinemático inverso Desacoplo cinemático Problema del acoplo cinemático Acoplo cinemático La inserción de la herramienta genera una acoplo entre posición y orientación del efector final. El centro de la muñeca de muñecas rotulianas es un punto único con solución del brazo y de la muñeca desacoplados. Fuera de dicho punto habrá acoplo entre posición y orientación. La inserción de la herramienta desplaza el efector final a un punto con posición y orientación acoplados. El método de Pieper (1968) resuelve dicho problema para muñecas esféricas. 10 / 61 Modelo cinemático inverso Desacoplo cinemático Desacoplo cinemático Punto de desacoplo El “centro de la muñeca” está localizado en el punto de intersección de los tres ejes articulares de la muñeca (θ4 , θ5 y θ6 ) w → “centro de la muñeca” eslabón 3 w = o4 = o5 , o3 puede o3 q4 s (y6) estar fuera. q6 a (z6) z6 (herramienta), q5 z5 o6 alineado con z5 z3 n (x6) z4 (eslabón 6) o4 ,o5 “centro de la muñeca” 11 / 61 Modelo cinemático inverso Desacoplo cinemático Desacoplo cinemático Método de Desacoplo Cinemático (Pieper, 1968) Resuelve el problema del acoplo separando del problema cinemático en dos problemas desacoplados: la posición (brazo) y la orientación (muñeca). Problema de la posición: utiliza w para resolver las variables articulares del tronco (q1 ), hombro (q2 ) y codo (q3 ). Problema de la orientación: resuelve las tres variables articulares de la muñeca: θ4 , θ5 y θ6 , w R6 , que obtienen la orientación deseada [n s a]. Nótese que la solución del brazo influye también en la solución de la orientación y por tanto deberá realizarse primero. Esto es: [n, s, a] = 0Rw (θ1 , θ2 , θ3 )w R6 (θ4 , θ5 , θ6 ) 12 / 61 Modelo cinemático inverso Desacoplo cinemático Desacoplo cinemático Método de Desacoplo Cinemático (Pieper, 1968) Obtención de las coordenadas del centro de la muñeca Fundamental: z5 y z6 l6=long. herr. alineados. eslabón 3 5 d6 s (y6) l6 : longitud de la o4,o5 q6 a (z6) herramienta q4 w z5 o6 p posición efector final z4 q5 z3 n (x6) 0 d6=p w z0 x0 o0 y0 w = p − l6 0R5 z5 13 / 61 Modelo cinemático inverso Desacoplo cinemático Desacoplo cinemático Método de Desacoplo Cinemático (Pieper, 1968) w = p − l6 0R5 z5 l6 0R5 z5 expresa el vector 5d6 (vector de o5 a o6 ) en coordenadas de la base. z5 = [0 0 1]T y por tanto solo nos interesa la última columna de 0R5 z5 0R es desconocida, pero puede ser sustituida por la 5 orientación deseada 0R6 = [n, s, a] que sí es conocida, que a su vez puede descomponerse en 0 R6 = 0R5 5R6 Por la alineación de los ejes z5 y z6 tenemos que 5 R6 = Rz5 ,θ6 donde Rz5 ,θ6 matriz de rotación básica. 14 / 61 Modelo cinemático inverso Desacoplo cinemático Desacoplo cinemático Método de Desacoplo Cinemático (Pieper, 1968) Dado que Rz5 ,θ6 no modifica la orientación de z6 0 R5 Rz5 ,θ6 = 0R6 r11 r12 r13 cos(θ6 ) − sin(θ6 ) 0      ∗ ∗ r13 r r r sin(θ ) cos(θ ) 0 =  ∗ ∗ r23        21 22 23   6 6 r31 r32 r33 0 0 1 ∗ ∗ r33 donde ‘*’ representa cualquier expresión. De esta forma las coordenadas z (terceras columnas) de 0R5 y 6 son iguales. 0R 15 / 61 Modelo cinemático inverso Desacoplo cinemático Desacoplo cinemático Método de Desacoplo Cinemático (Pieper, 1968) Dado que la multiplicación por z5 o z6 seleccionan la última columna de 0R5 o 0R6 , por tanto: l6 0R6 z6 = l6 0R5 z5 Sustituyendo 0R6 por la orientación deseada [n, s, a], obtenemos que w = p − l6 0R5 z5 = p − l6 [n, s, a]z6 esto es w = p − l6 a A diferencia de p, w viene determinado únicamente por las tres articulaciones de la estructura del brazo, q1 , q2 y q3. 16 / 61 Modelo cinemático inverso Desacoplo cinemático Solución del problema de la posición Método geométrico Solución trigonométrica aplicada a diversos planos formados por la estructura del brazo. Simple y directo para brazos con subestructuras planarias y muñecas rotulianas. Utilizado principalmente para la solución del problema del brazo (tronco, hombro y codo). Se establecen ecuaciones trigonométricas en función del centro de la muñeca w en distintos planos. Cada ecuación trigonométrica resuelve una variable articular. 17 / 61 Modelo cinemático inverso Desacoplo cinemático Ejemplo de aplicación del método geométrico Brazo antropomórfico de hombro central Obtención del modelo cinemático inverso del robot Minimover l4 p z¢0 < 0′0 x0′ y0′ z0′ >: base auxiliar w px = px′ , py = py′ , pz′ = pz − l1 l2 z l3' 0 wz w = [wx , wy , wz ] “centro muñeca” o¢0 y¢0 respecto de < 0′0 x0′ y0′ z0′ > x¢0 r l1 altura del “hombro” pz z0 l4 longitud de la herramienta l1 y0 x0 o0 q1 (wx,wy) (px,py) 18 / 61 Modelo cinemático inverso Desacoplo cinemático Ejemplo de aplicación del método geométrico Procedimiento Ejemplo de brazo antropomórfico de hombro central 1 Establecer el sistema coordenado temporal < 0′0 x0′ y0′ z0′ > (opcional) 2 Obtener las coordenadas del “centro de la muñeca”, w = [wx , wy , wz ]T , respecto de < 0′0 x0′ y0′ z0′ > w = p′ − l4 a p′ = p − [0, 0, l1 ]T con l1 altura hombro a: 3ª columna de la matriz de orientación [n, s, a] l4 : longitud de la herramienta 3 Si la orientación estuviera definida mediante una representación no matricial habría que convertir dicha representación a la forma matricial ([n, s, a]). 19 / 61 Modelo cinemático inverso Desacoplo cinemático Ejemplo de aplicación del método geométrico Solución de θ1 Ventaja de la subestructura planaria "brazo-antebrazo": desocopla θ1 de las soluciones de θ2 y θ3. 1 Solución de θ1 : wy   θ1 = arctan wx es independiente de θ2 y θ3 Se utiliza la función arctan 2() para eliminar ambigüedades:     0 ≤ θ ≤ π/2 para x >0 e y > 0,  π/2 ≤ θ ≤ π para x 0,  θ = arctan 2(y , x ) =    π ≤ θ ≤ −π/2 para x 0 e 0 “brazo-atrás”: θ1 + π y r < 0 22 / 61 Modelo cinemático inverso Desacoplo cinemático Ejemplo de aplicación del método geométrico Solución de θ3 Método geométrico aplicado al plano x1 z0 de la subestructura pla- naria brazo-antebrazo 1 Aplicamos el teorema del o3 , w x3 coseno al triángulo o\ 1 o2 o3 r 2 + wz2 = l22 + l32 + 2l2 l3 cos θ3 z0 r 2 + w z2 l3 q3 x wz 2 donde r 2 = wx2 + wy2 l2 o2 2 Resolviendo cos θ3 obtenemos: o1 r x1 wx2 + wy2 + wz2 − l22 − l32 cos θ3 = Plano x1 z0 , z1 y z2 salientes 2l2 l3 wz altura respecto de {0′ }. 23 / 61 Modelo cinemático inverso Desacoplo cinemático Ejemplo de aplicación del método geométrico Solución de θ3 continuación... La solución arccos() no está bien definida: cos θ = cos(−θ). Se utilizarán preferentemente arctan() o arctan 2() Para ello, utilizamos sin θ3 = ± 1 − cos2 θ3 donde cos θ3 es p la función hallada Obteniendo así la solución ± 1 − cos2 θ3 p ! θ3 = arctan cos θ3 O preferentemente, q θ3 = arctan 2(± 1 − cos2 θ3 ), cos θ3 ) 24 / 61 Modelo cinemático inverso Desacoplo cinemático Ejemplo de aplicación del método geométrico Solución de θ3 Existen dos soluciones para θ3 que corresponden a las configura- ciones de “codo-arriba” y “codo-abajo” x2 q30 wz y1 x2 wz q2 r>0 x1 q2 r>0 x1 “codo-arriba” θ3 < 0 “codo-abajo” θ3 > 0 25 / 61 Modelo cinemático inverso Desacoplo cinemático Ejemplo de aplicación del método geométrico Solución de θ3 El signo de θ3 para “codo-arriba” y “codo-abajo” se invertirá al cambiar de la configuración de “brazo-atrás” a “brazo-adelante”. r es la proyección de w sobre el eje x1 siendo r > 0 para “brazo-adelante” y r < 0 para “brazo-atrás”. Para “brazo-adelante” tendremos: “codo-arriba” para sin θ3 < 0 → −π < θ3 < 0 “codo-abajo” para sin θ3 > 0 → 0 < θ3 < π Para “brazo-atrás”(requiere tronco =θ1 + π) se invierten las condiciones: “codo-arriba” para sin θ3 > 0 → 0 < θ3 < π “codo-abajo” para sin θ3 < 0 → −π < θ3 < 0 26 / 61 Modelo cinemático inverso Desacoplo cinemático Ejemplo de aplicación del método geométrico Solución de θ3 “codo-arriba”o “codo-abajo” son una elección del usuario. General- mente cada fabricante utilizará una de las dos por defecto. Se descartarán las soluciones que no satisfagan θ3máx ≤ θ3 ≤ θ3min , no existiendo solución si ambas las excediesen. La multiplicidad de soluciones mejora la accesibilidad del robot y puede evitar obstáculos. La existencia de las dos soluciones de θ3 no garantiza la existencia de la solución para codo-arriba” y “codo-abajo”, dependerá θ2 27 / 61 Modelo cinemático inverso Desacoplo cinemático Ejemplo de aplicación del método geométrico Solución “codo-abajo” de θ2 o3 , w x3 z0 Establecemos las variables angulares α y β, tal que: wz q r 2 + wz2 l3 l 3 sin 3 x2 θ2 = β − α q3 o2 q3 a l2 l 3 cos β: ángulo entre hipotenusa b q y x1 , respecto de z1 o1 2 r x1 α: ángulo entre hipotenusa y x2 , respecto de z1. Plano x1 z0 : z1 y z2 salientes α sigue el signo de θ3 28 / 61 Modelo cinemático inverso Desacoplo cinemático Ejemplo de aplicación del método geométrico Solución de θ2 θ2 = β − α donde wz wz β = arctan( ) = arctan( q )y r ± w2 + w2 x y l3 sin θ3 α = arctan( ) l2 + l3 cos θ3 Nótese que α tiene el mismo signo que θ3 ( considerando una medición angular −π ≤ α ≤ π en lugar de 0 ≤ α ≤ 2π). Nótese que para l2 > l3 (caso típico) el denominador de la función arctan() será siempre positivo 29 / 61 Modelo cinemático inverso Desacoplo cinemático Ejemplo de aplicación del método geométrico Solución de θ2 wz l3 sin θ3 θ2 = arctan( q ) − arctan( ) ± wx2 + wy2 l2 + l3 cos θ3 q Dependiendo del signo de r = ± wx2 + wy2 obtendremos nuevamente dos soluciones “brazo-adelante” para r > 0 “brazo-atrás” para r < 0 Es una elección del usuario, por defecto el robot utiliza la configuración “brazo-adelante” “brazo-atrás” ocupa un volumen de trabajo menor que “brazo-adelante”, pero puede ser la única solución 30 / 61 Modelo cinemático inverso Desacoplo cinemático Ejemplo de aplicación del método geométrico Solución “codo-arriba” de θ2 Dos triángulos o\ 1 o2 o3 y Plano x1 z0 : z1 y z2 salientes o[ 1 ro3 x2 o3 , w Mismo procedimiento que z0 o2 -q3 l3 x3 para “codo-abajo” θ2 = β − α l2 -a r 2 + wz2 wz q2 b Salvo que α < 0 o1 r x1 α y θ3 llevan ambos el r mismo signo (β > 0). 31 / 61 Modelo cinemático inverso Desacoplo cinemático Ejemplo de aplicación del método geométrico Vector solución θ Se pueden agrupar las soluciones “codo-arriba/abajo” con “brazo- adelante/atrás” introduciendo dos modificadores CODO y BRAZO CODO=-1 para “codo-arriba” y 1 para “codo-abajo” BRAZO= 1 para “brazo-adelante” y -1 para “brazo-atrás” θ1 = arctan 2 (wy , wx ) + (1 − BRAZO) ∗ π/2 θ3 = BRAZO ∗ CODO ∗ arctan 2( 1 − cos θ3 ), cos θ3 ) p q θ2 = arctan 2(wz , BRAZO wx2 + wy2 ) − arctan 2(l3 sin θ3 , l2 + l3 cos θ3 ) 32 / 61 Modelo cinemático inverso Desacoplo cinemático Ejemplo de aplicación del método geométrico Vector solución θ con wx2 + wy2 + wz2 − l22 − l32 cos θ3 = 2l2 l3 El producto de los parámetros BRAZO y CODO en θ3 expresa la duplicidad de soluciones:“brazo-atrás”*“codo-abajo”= “brazo-adelante”*“codo-arriba” BRAZO=-1 en θ2 , obtiene la solución suplementaria π − θ2 de la solución θ2 con BRAZO=1. 33 / 61 Modelo cinemático inverso Desacoplo cinemático Ejemplo de aplicación del método geométrico Vector solución θ La solución “brazo-atrás” puede obtenerse directamente de “brazo-adelante” sin necesidad de nuevos cálculos: θ1 (atrás) = θ1 (adelante + π) θ2 (atrás-arriba) = π − θ2 (adelante-arriba) θ2 (atrás-abajo) = π − θ2 (adelante-abajo) θ3 (atrás-arriba) = θ3 (adelante-abajo) θ3 (atrás-abajo) = θ3 (adelante-arriba) 34 / 61 Modelo cinemático inverso Desacoplo cinemático Ejemplo de aplicación del método geométrico Obtención del vector θ Configuraciones “codo-arriba/abajo” para “brazo-atrás” q3 0 z0 y1 y1 x2 wz wz q2 q2 x1 r [n3 , s3 , a3 ] = 3R4 (θ4 )4R5 (θ5 )5R6 (θ6 )
−1 donde [n3 , s3 , a3 ] = 0R3 (θ1 , θ2 , θ3 ) [n, s, a]. Por Denavit-Hartenberg, la orientación entre dos sistemas coordenados consecutivos es Ri = Rz,θi Rx ,αi i−1 αi = ángulo de torsión y θi variable/parámetro articular. 48 / 61 Modelo cinemático inverso Desacoplo cinemático Solución del problema de la orientación Planteamiento del problema La expresión matricial de la orientación 3R6 queda así: 3 R6 = (Rz3 ,θ4 Rx4 ,α4 ) (Rz4 ,θ5 Rx5 ,α5 ) (Rz5 ,θ6 Rx6 ,α6 ) Para la muñeca rotuliana “típica”, α4 = π/2, α5 = −π/2 y α6 = 0, obteniendo así 0 0 0       n3x s3x a3x c4 s4 c5 −s5 c6 −s6 n3y s3y a3y  =  s4 0 −c4   s5 0 c5   s6 c6 0 n3z s3z a3z 0 1 0 0 −1 0 0 0 1 49 / 61 Modelo cinemático inverso Desacoplo cinemático Solución del problema de la orientación Planteamiento del problema Podemos intentar resolver el problema a partir de c4 c5 c6 + s4 s6 −c4 c5 s6 + s4 c6     n3x s3x a3x −c4 s5 n3y s3y a3y  = s4 c5 c6 − c4 s6 −s4 c5 s6 − c4 c6 −s4 s5  n3z s3z a3z s5 c6 −s5 s6 + s4 c6 c5 Del elemento (3,3) → θ5 = arccos(a3z ), sin embargo, esta solución es inconsistente: cos θ = cos(−θ) Trataremos de buscar una solución mejor mediante la técnica de la transformación inversa. 50 / 61 Modelo cinemático inverso Desacoplo cinemático Solución del problema de la orientación Aplicación del método de la transformada inversa
−1
−1 Pre o postmultiplicando por 5R 6 o 3R 4 ambos lados de la ecuación matricial
−1
T Postmultiplicando por 5R 6 = 5R 6 , obtenemos n3x s6 + s3x c6     n3x c6 − s3x s6 a3x c4 c5 −s4 −c4 s5 n3y c6 − s3y s6 n3y s6 + s3y c6 a3y  =  s4 c5 c4 −s4 s5  n3z c6 − s3z s6 n3z s6 + s3z c6 a3z s5 0 c5 Del elemento (3,2) → n3z s6 + s3z c6 = 0, de donde obtenemos s6   θ6 = arctan = arctan 2(−s3z , n3z ) c6 Nótese que se puede formar también una segunda ecuación con s3z > 0 y n3z < 0 obteniendo arctan 2(s3z , −n3z ) dando lugar a una segunda solución θ6 + π. 51 / 61 Modelo cinemático inverso Desacoplo cinemático Solución del problema de la orientación Aplicación del método de la transformada inversa La solución para θ4 podemos obtenerla de los elementos (1,2) y (2,2) −s4 = n3x s6 + s3x c6 c4 = n3y s6 + s3y c6 cuya solución es s4   θ4 = arctan = arctan 2(−n3x s6 − s3x c6 , n3y s6 + s3y c6 ) c4 Para θ6 + π cambian los signos de sin θ6 y cos θ6 intercambiando los signos de las dos parámetros de arctan 2 dando lugar a la segunda solución θ4 + π. 52 / 61 Modelo cinemático inverso Desacoplo cinemático Solución del problema de la orientación Aplicación del método de la transformada inversa Finalmente, obtenemos θ5 de los elementos (3,1) y (3,3) s5 = n3z c6 − s3z s6 c5 = a3z cuya solución es s5   θ5 = arctan = arctan 2(n3z c6 − s3z s6 , a3z ) c5 A θ6 + π le corresponderá la solución −θ5 ya que sin θ6 y cos θ6 cambian de signo pero a3z se mantiene constante. 53 / 61 Modelo cinemático inverso Desacoplo cinemático Solución del problema de la orientación Comprobación del resultado obtenido Obtengamos, como ejemplo, la solución de la muñeca para la con- figuración “casa” Por inspección vemos que: eslabón 3 w s (y6) 1 0 0   y5 q6 a (z6) 3 R6 = 0 1 0   q4 z5 0 0 1 z4 z3 q5 y4 n (x6) De donde obtenemos que x4 x5 (x3) θ6 = arctan 2(−s3z , n3z ) = arctan 2(0, 0) = 0 54 / 61 Modelo cinemático inverso Desacoplo cinemático Solución del problema de la orientación Comprobación del resultado obtenido arctan 2(0, 0) no está definida, sin embargo, la mayoría de las funciones de lenguajes de programación numéricos (C,Matlab, etc.) adoptan arctan 2(0, 0) = 0 Resolviendo ahora θ4 para θ6 = 0 θ4 = arctan 2(−n3x s6 −s3x c6 , n3y s6 +s3y c6 ) = arctan(0, 1) = 0 Finalmente, obtenemos θ5 para θ6 = 0 θ5 = arctan 2(n3z c6 − s3z s6 , a3z ) = arctan(0, 1) = 0 Por tanto la solución de la muñeca para la configuración casa es [θ4 , θ5 , θ6 ]T = [0, 0, 0]T 55 / 61 Modelo cinemático inverso Desacoplo cinemático Solución del problema de la orientación Solución de la orientación del Robot Minimover Solución de θ4 y θ5 , las dos variables articulares de la muñeca del Minimover, por el método geométrico θ4 → elevación, y3 l5 θ5 → rotación final y4 s (y5) No tiene desviación q4 w z3 z4 o3 y w coinciden. x3 q a (z5) 5 [n3 , s3 , a3 ] = x4 0R (θ , θ , θ ) −1 [n, s, a]
n (x5) 3 1 2 3 56 / 61 Modelo cinemático inverso Desacoplo cinemático Solución del problema de la orientación Solución de la orientación del Robot Minimover Obtenemos la orientación de la mano respecto de < o3 , x3 , y3 , z3 > [n3 , s3 , a3 ] = 3R4 (θ4 )4R5 (θ5 ) donde cos θi − sin θi 0 1 0 0    i−1 Ri = Rθi ,zi−1 Rαi ,xi =  sin θi cos θi 0 0 cos αi − sin αi  0 0 1 0 sin αi cos αi Para α4 = −π/2, α5 = 0 y offset θ4 = −π/2 0 0     −s4 −c4 c5 −s5 3 R4 =  c4 0 −s4  y 4R5 =  s5 c5 0 0 −1 0 0 0 1 57 / 61 Modelo cinemático inverso Desacoplo cinemático Solución del problema de la orientación Solución de la orientación del Robot Minimover Formamos ahora la ecuación matricial [n3 , s3 , a3 ] = 3R4 (θ4 )4R5 (θ5 )     n3x s3x a3x c4 c5 −s4 s5 c4 n3y s3y a3y  = −s4 c5 c4 s5 s4  n3z s3z a3z −s5 −c5 0 Podemos resolver θ4 directamente de los elementos (1,3) y (2,3) s4 = a3x y c4 = a3y De donde obtenemos s4   θ4 = arctan = arctan 2(a3y , a3x ) c4 58 / 61 Modelo cinemático inverso Desacoplo cinemático Solución del problema de la orientación Solución de la orientación del Robot Minimover Resolvemos asimismo θ5 de los elementos (3,1) y (3,2) −s5 = n3z y − c5 = s3z cuya solución es s5   θ5 = arctan = arctan 2(−n3z , −s3z ) c5 59 / 61 Modelo cinemático inverso Desacoplo cinemático Solución del problema de la orientación Solución de la orientación del Robot Minimover Comprobación del resultado obtenido en la configuración “casa” de la mano Por por inspección, la orientación 3R5 en la configuración “casa” es: 0 0 1   y3 3 R5 = [n3 s3 a3 ] = −1 0 0   l5 0 −1 0 y4 s (y5) donde q4 w z3 z4   x3 q a (z5) n3x s3x a3x 5 x4 [n3 s3 a3 ] ≡ n3y s3y a3y    n (x5) n3z s3z a3z 60 / 61 Modelo cinemático inverso Desacoplo cinemático Solución del problema de la orientación Comprobación del resultado obtenido De donde obtenemos θ4 = arctan 2(−a3x , a3y ) = arctan 2(−1, 0) = −π/2 equivale al offset de la articulación 4 θ5 = arctan 2(−n3z , −s3z ) = arctan 2(0, −(−1)) = 0 Se corresponde con la configuración “casa” de la mano. 61 / 61