APLICACIÓN DE LA TEORÍA DE MANIPULADORES REDUNDANTES AL ESTUDIO

DEL DESEMPEÑO DEL BRAZO HUMANO

JUAN CAMILO BOTERO MONTERO

Trabajo de grado para optar al título de Magíster en Ingeniería Mecánica

PROFESOR ASESOR: CARLOS FRANCISCO RODRÍGUEZ H. PhD. PROFESOR ASOCIADO

UNIVERSIDAD DE LOS ANDES FACULTAD DE INGENIERÍA

DEPARTAMENTO DE INGENIERÍA MECÁNICA BOGOTÁ

2003

MIM - 2004 - I - 01

2

AGRADECIMIENTOS Quisiera agradecer al profesor Carlos Francisco Rodríguez por su constante ayuda y guía a lo largo del proceso que significó la realización de esta tesis. Su visión y sus aportes fueron fundamentales para poder llegar a este documento como resultado final. Para el profesor Rodríguez mis más sinceros agradecimientos acompañados de mi respeto y mi admiración. Al profesor Alfonso Pámanes del Instituto Tecnológico de La Laguna por sus vitales aportes al trabajo y su hospitalidad durante mi estadía en Torreón, México. Al equipo médico y técnico del proyecto “Desarrollo de una Herramienta de Análisis Cuantitativo de Movimiento en Extremidades Superiores”; al Ing. Hugo Quintero por su soporte en la visualización de resultados; al personal de apoyo del Laboratorio de Análisis de Movimiento de la Universidad de los Andes y el Instituto de Ortopedia Infantil Roosevelt. También quisiera agradecer al profesor Álvaro Pinilla ya que sin su ayuda hubiese sido imposible hacer la maestría en la Universidad de los Andes. Por último, quisiera agradecer especialmente a mi familia, quienes siempre me apoyaron y estuvieron conmigo. A todos, Gracias.

MIM - 2004 - I - 01

3

CONTENIDO

pág. INTRODUCCIÓN 2 1. BREVE INTRODUCCIÓN A LOS MOVIMIENTOS DEL BRAZO HUMANO 4 2. MODELO BIOMECÁNICO DEL MIEMBRO SUPERIOR 5 3. CINEMÁTICA DIFERENCIAL DEL BRAZO HUMANO 8 4. CÁLCULO DE LOS MOMENTOS DE ACTUACIÓN ARTICULARES 11 5. ELIPSOIDES DE MANIPULABILIDAD 12 5.1 MANIPULABILIDAD DE FUERZA 12 5.2 MANIPULABILIDAD DE VELOCIDAD 13 5.3 ÍNDICES DE MANIPULABILIDAD 15 5.4 OTROS ÍNDICES DE DESEMPEÑO 15 6. ELIPSOIDES DE MANIPULABILIDAD COMO FRONTERAS DE FUERZA Y VELOCIDAD MÁXIMAS 16 7. APLICACIÓN AL ESTUDIO BIOMECÁNICO DEL BRAZO HUMANO 21 8. DETERMINACIÓN DE POSTURAS ÓPTIMAS 26 8.1 SOLUCIÓN DE MÍNIMA NORMA 26

MIM - 2004 - I - 01

4

pág 8.2 SOLUCIÓN HOMOGÉNEA 26 9. DETERMINACIÓN DE TRAYECTORIAS ÓPTIMAS PARA REALIZAR TAREAS SIMPLES 28

10. CONCLUSIONES Y RECOMENDACIONES 33 BIBLIOGRAFÍA 35 ANEXOS 37

MIM - 2004 - I - 01

5

LISTA DE TABLAS

pág. Tabla 1. Relación Eslabones y Grados de Libertad (DOF) 7 Tabla 2. Configuraciones para Rascarse el Hombro & Índice de Manipulabilidad 21 Tabla 3. Configuración Prueba Predicción de Velocidad 22 Tabla 4. Direcciones Prueba Predicción de Velocidad 22 Tabla 5. Resultados Predicción de Velocidad 22 Tabla 6. Configuración Prueba Predicción de Fuerza 24 Tabla 7. Direcciones Prueba Predicción de Fuerza 24 Tabla 8. Resultados Predicción de Fuerza 25 Tabla 9. Tarea llevar mano desde Bloq Mayús hasta Intro. 29 Tabla 10. Tarea agarrar un vaso y llevarlo a la boca 30

MIM - 2004 - I - 01

6

LISTA DE FIGURAS

pág. Figura 1. Modelo Biomecánico del brazo. 5 Figura 2. Movimientos Asociados al Miembro Superior 6 Figura 3. Modelo Simple 9 Figura 4. Elipsoide de Manipulabilidad de Fuerza 13 Figura 5. Elipsoide de Manipulabilidad de Velocidad 14 Figura 6. Relación Elipsoides de Manipulabilidad 14 Figura 7. Elipses de Manipulabilidad de Fuerza 16 Figura 8. Índice de Manipulabilidad de Fuerza 16 Figura 9. Longitud Semieje Mayor Elipse de Fuerza 17 Figura 10. Longitud Semieje Menor Elipse de Fuerza 17 Figura 11. Ángulo entre la Horizontal y el Semieje Mayor 18 Figura 12. Elipses de Manipulabilidad de Velocidad 18 Figura 13. Índice de Manipulabilidad de Velocidad 19 Figura 14. Longitud Semieje Mayor Elipse de Velocidad 19 Figura 15. Longitud Semieje Menor Elipse de Velocidad 20 Figura 16. Ángulo entre la Horizontal y el Semieje Mayor 20 Figura 17. Índice de Manipulabilidad de Velocidad 21 Figura 18. Transductor de Fuerza – Par 23 Figura 19. Montaje Transductor Fuerza – Par 23 Figura 20. Elipsoide de Manipulabilidad de Fuerza 24 Figura 21. Elipsoide como Frontera de Fuerza Máxima 25

MIM - 2004 - I - 01

7

pág

Figura 22. Proceso de Optimización Mediante Reconfiguración 27 Figura 23. Perfil de Velocidad Típico para una Tarea Simple 28 Figura 24. Índice de Manipulabilidad de Velocidad 29 Figura 25. Evolución Óptima de la Flexión y la Abducción del Hombro 30 Figura 26. Evolución Óptima de la Flexión del Hombro 30 Figura 27. Evolución Óptima de la Flexión del Codo 31 Figura 28. Posición Real del Extremo de la Mano 31 Figura 29. Posición Optimizada del Extremo de la Mano 32

MIM - 2004 - I - 01

8

LISTA DE ANEXOS

pág. Anexo A. Movimientos del Hombro 37 Anexo B. Movimientos del Codo 38 Anexo C. Movimientos de la Muñeca 39 Anexo D. Programa Cinemática Diferencial del Brazo 40 Anexo E. Matriz Jacobiana Modelo Completo 42 Anexo F. Programa Optimización de Trayectorias para Tarea Teclado 46 Anexo G. Transductor Fuerza – Par ATI Mini 45 51

MIM - 2004 - I - 01

9

RESUMEN El presente documento presenta una aproximación al estudio biomecánico del miembro superior mediante la aplicación de conceptos propios del área de la robótica. Se modela el brazo humano como un robot manipulador redundante y por medio de técnicas analíticas es posible calcular pares de actuación en las articulaciones. Se explota el concepto de índices de desempeño para predecir posturas y trayectorias óptimas del brazo para realizar diferentes tareas evitando la utilización de patrones estadísticos de movimiento. Los autores presentan resultados que servirán como una herramienta efectiva para la evaluación y el diagnóstico de patologías del miembro superior y como apoyo en el análisis de gestos laborales y deportivos. Este trabajo hace parte del proyecto de investigación financiado por COLCIENCIAS “Desarrollo de una Herramienta de Análisis Cuantitativo de Movimiento en Extremidades Superiores” (Contrato 142-2002), el cual se encuentra en ejecución.

MIM - 2004 - I - 01

10

INTRODUCCIÓN

El movimiento del cuerpo humano y las fuerzas asociadas a éste han sido objeto de estudio desde la década de 1960. En este campo, la gran mayoría de las investigaciones se han dirigido hacia el estudio de los miembros inferiores, ya que la marcha humana es un movimiento sencillo, cíclico e incluso predecible. Sin embargo esto no ocurre en el estudio de los miembros superiores, cuya funcionalidad es de vital importancia para cualquier individuo. Los brazos son instrumentos versátiles, empleados en gran variedad de actividades de diversas maneras. Por lo tanto, el tipo de modelo propuesto para miembros inferiores resulta inadecuado e insuficiente cuando se quiere analizar los miembros superiores [13]. Para entender completamente el funcionamiento del miembro superior es necesario un modelo acorde con la naturaleza de los brazos y una metodología que permita el estudio a fondo de su desempeño. Hasta ahora la evaluación del desempeño del miembro superior se ha basado en evaluaciones cualitativas realizadas a criterio del especialista (médico, fisioterapeuta, etc). Esta forma de evaluación es subjetiva y por lo tanto puede llevar a conclusiones erradas, ya que toda la evaluación se basa en el criterio del especialista. Para evitar la subjetividad de este tipo de evaluaciones, es común utilizar patrones estadísticos de normalidad de movimiento. Estos patrones se obtienen del estudio detallado de un número de personas realizando una actividad determinada, después de lo cual se determina estadísticamente un cierto rango de “normalidad” en el movimiento. Así, cualquier individuo que se aleje de este patrón será considerado “anormal”. A pesar de basarse en un tratamiento matemático riguroso, esta forma de evaluación depende de parámetros subjetivos, como la selección de la muestra representativa para la tarea escogida ó la desviación que debe ser permitida para considerar un movimiento “anormal”, ya que la cuantificación de la normalidad no deja de ser subjetiva. El objetivo de este trabajo es el de proponer una herramienta que permita realizar una evaluación del desempeño del miembro superior humano de una forma objetiva y cuantitativa. Una aproximación a la evaluación del desempeño de los miembros superiores, tanto en movimiento como en fuerza, es la aplicación de algunos criterios propios del área de la robótica al área de la biomecánica. Aunque en la actualidad diversos autores trabajan en este campo, por ejemplo en la determinación del espacio de trabajo de las extremidades superiores [1], es escasa la literatura que se encuentra sobre este tema. Este documento tiene como objetivo el presentar un nuevo método de evaluación del desempeño del miembro superior. Se entiende desempeño como la capacidad de generar fuerzas y movimientos controlados (asociados a velocidades) en la mano, a partir de momentos aplicados en las articulaciones. Se presenta brevemente una explicación de la teoría de robótica utilizada y se hace hincapié en la determinación de los momentos articulares de actuación y la aplicación de conceptos de manipulabilidad en el estudio de las fuerzas y movimientos desarrollados por el brazo.

MIM - 2004 - I - 01

11

Aunque en el documento se encuentran algunas explicaciones básicas acerca de los fenómenos físicos y conceptos involucrados, es recomendable que el lector tenga conocimientos básicos acerca de el modelaje de manipuladores y anatomía del brazo humano.

MIM - 2004 - I - 01

12

1. BREVE INTRODUCCIÓN A LOS MOVIMIENTOS DEL BRAZO HUMANO El brazo humano es una herramienta versátil que ha permitido al ser humano realizar innumerables tareas a lo largo de su historia evolutiva. Sus movimientos están claramente definidos por los especialistas en anatomía [6,7]. Sin embargo, a manera de rápida referencia a continuación se presentan algunas definiciones intuitivas de los términos más comunes sobre los movimientos del brazo que el lector encontrará en la literatura especializada [7]. Flexión del Hombro: consiste en levantar el brazo hacia adelante, alejándolo del plano que contiene a los hombros. Abducción del Hombro: consiste en levantar el brazo de costado, alejándolo del eje de simetría del cuerpo. Rotación del Hombro. Consiste en rotar el brazo en torno a su propio eje. Flexión del Codo : consiste en “doblar” el codo, llevando el antebrazo sobre el brazo. Entre otros, es el movimiento que permite llevar los alimentos a la boca [7]. Prono–supinación del Codo : consiste en rotar el antebrazo sobre su propio eje. Este movimiento ayuda en la tarea de comer, economizando el gesto de flexión del codo. Flexión de la Muñeca: consiste en “doblar” la muñeca, llevando la mano sobre el antebrazo. Desviación de la Muñeca: consiste en alejar la mano del eje del antebrazo. Es un movimiento bastante reducido. Todos estos movimientos son medidos desde la posición anatómica del brazo [7]. Es aconsejable que el lector revise la bibliografía recomendada al final del documento para un mejor entendimiento de los movimientos del brazo [6,7].

MIM - 2004 - I - 01

13

Brazo

Mano

Tórax

Antebrazo

Brazo

Mano

Tórax

Antebrazo

2. MODELO BIOMECÁNICO DEL MIEMBRO SUPERIOR Para llevar a cabo el análisis cuantitativo de desempeño del miembro superior humano es necesario definir un modelo que describa correctamente su dinámica. Con este fin, se modela el brazo como un manipulador [14], es decir, un mecanismo en cadena abierta de cuatro eslabones que representan, a saber: el tórax, el brazo, el antebrazo y la mano. En el modelo propuesto, por simplicidad, no se toma en cuenta el efecto del movimiento de la clavícula, aunque su posterior inclusión es sencilla y directa. La figura 1 muestra un esquema del modelo utilizado. Figura 1. Modelo Biomecánico del Miembro Superior. El modelo presenta cuatro eslabones que se consideran cuerpos rígidos indeformables: Tórax: Origen del sistema articulado. Húmero: Desde la articulación gleno-humeral hasta la articulación de codo. Antebrazo: Desde el codo hasta la muñeca. Lo constituyen los huesos cúbito y radio. Mano: A partir de la muñeca. Se denomina el Tórax como la Base del modelo, y la Mano como el Órgano Terminal del modelo. Asimismo se definen tres articulaciones como uniones mecánicas entre los cuerpos rígidos anteriormente definidos:

MIM - 2004 - I - 01

14

Hombro: Las articulaciones acromio-clavicular y gleno-humeral se modelan como una esfera con tres grados de libertad: flexión-extensión, abducción-aducción y rotación axial. Codo: Las articulaciones radio–cubital, radio–humeral, cúbito–humeral se reducen a una sola articulación con dos grados de libertad: flexión-extensión, pronación-supinación. Muñeca: Dos grados de libertad: flexión–extensión, desviación cúbito–radial. El modelo posee siete grados de libertad en total, asociados a siete variables angulares qi, con i = 1,2,...,7, que representan los siete movimientos del brazo definidos anteriormente. En resumen, el hombro se modela como una unión con tres grados de libertad (q1, q2, y q3); el codo con dos (q4, y q5) y la muñeca con dos (q6, y q7). La figura 2 muestra esquemáticamente los movimientos asociados. Figura 2. Movimientos Asociados al Miembro Superior. De esta manera cualquier configuración del brazo estará completamente definida mediante el vector de coordenadas generalizadas [14]: q = [q1 q2 q3 q4 q5 q6 q7]T (Ec. 1) En la tabla 1 se relacionan las posiciones articulares con la definición anatómica de los movimientos del miembro superior.

q1

q2

q3

q4

q5 q6

q7

MIM - 2004 - I - 01

15

Tabla 1. Relación Eslabones y Grados de Libertad (DOF).

Eslabón Articulación DOF Movimiento Asociado Tórax Ninguna 0 Ninguno Brazo Hombro 3 Flexión (q1), Abducción (q2) y Rotación (q3) Antebrazo Codo 2 Flexión (q4) y Pronación (q5) Mano Muñeca 2 Flexión (q6) y Desviación (q7)

Durante el desarrollo de este documento se hará uso de un modelo más simple para acercar al lector al entendimiento de los conceptos utilizados y las ideas desarrolladas. Este modelo consiste en dos eslabones simulando el brazo y el conjunto antebrazo–mano, respectivamente, y dos grados de libertad simulando la flexión del hombro (q1) y la flexión del codo (q4). Para distinguir los modelos utilizados durante el documento se hará referencia al modelo de dos grados de libertad como Modelo Simple (2 DOF’s), y al modelo de siete grados de libertad como Modelo Completo (7 DOF’s).

MIM - 2004 - I - 01

16

3. CINEMÁTICA DIFERENCIAL DEL BRAZO HUMANO En el modelaje de manipuladores, el movimiento resultante del órgano terminal es obtenido mediante la composición de movimientos básicos entre los eslabones, refiriendo estos movimientos de un eslabón respecto al eslabón inmediatamente anterior [17]. La estrategia consiste en definir marcos de referencia en cada uno de los eslabones del modelo y representar la posición y orientación entre dos marcos adyacentes (usualmente mediante matrices de rotación). Esta definición de movimientos entre eslabones consecutivos es ampliamente tratado en la literatura especializada [2,16,17]. Mediante la composición de matrices de rotación entre los diferentes marcos del modelo propuesto, es posible determinar la posición y orientación del órgano terminal del manipulador, es decir, la mano. Para esto se define un marco de referencia global en el tórax T. En cada una de las articulaciones se define un marco de referencia local: B, A, M. Cada uno de estos marcos define una tríada de vectores unitarios mutuamente perpendiculares. Por ejemplo, en el antebrazo se tienen un marco de referencia A definido por los vectores unitarios a1, a2 y a3, mientras que en la mano los vectores unitarios m1, m2 y m3 definen el marco M. La relación entre los ai y los mi definirá los movimientos del marco M respecto al marco A (Ver anexos A, B y C para detalles de la construcción de los marcos de referencia utilizados). Con este modelo es posible determinar la posición, velocidad y aceleración de cualquier punto ó cuerpo del modelo. Esta información se utilizará para resolver el problema cinemático inverso del modelo, cuya solución determina la configuración que debe adoptar el brazo para una posición y orientación del extremo conocidas [2]. En el estudio de la cinemática diferencial, la meta es encontrar una relación entre las velocidades de las articulaciones (en este caso, la derivada temporal del vector de configuración q) y la velocidad lineal y angular del órgano terminal [16]. Es claro que esta información es de vital importancia para el análisis biomecánico del brazo. Una vez han sido definidos los sistemas de coordenadas locales y las variables articulares asociadas a cada grado de libertad, es posible definir la matriz Jacobiana Geométrica como el operador que relaciona las velocidades articulares con las velocidades del órgano terminal [12]:

qJx && )(q= (Ec. 2)

donde, J(q) es una matriz de (6 x n) la cual es función de las variables articulares qi y n es el número de grados de libertad del manipulador. En el caso del modelo propuesto para el miembro superior n es igual a 7.

MIM - 2004 - I - 01

17

Para calcular la matriz Jacobiana del modelo propuesto se utilizó el programa AUTOLEV®, de la compañía OnLine Dynamics Inc, el cual es un manipulador matemático simbólico diseñado para el análisis dinámico de cuerpos en el espacio. A continuación, a manera de ilustración, se muestra el cálculo de la matriz Jacobiana para modelo simple. La figura 3 muestra las variables relevantes en la determinación de la matriz Jacobiana. Figura 3. Modelo Simple

Las variables articulares son q1 y q2. La longitud de los eslabones es a1 y a2, respectivamente. El resultado obtenido es una matriz J de (6x2), la cual se muestra a continuación.

++++−+−−

=

11000000

)cos()cos()cos()sin()sin()sin(

21221211

21221211

qqaqqaqaqqaqqaqa

J

Es claro que la matriz Jacobiana varía a medida que varían q1 y q2, por lo que deberá ser recalculada cada vez que el mecanismo cambie su configuración. En el anexo D se muestra la matriz Jacobiana del modelo completo. Una vez calculada esta matriz, es posible aplicar algunos criterios propios de los robots manipuladores al brazo humano. Algunas investigaciones han sido llevadas a cabo en este campo [1]. En este documento se discutirán tres criterios: la magnitud de los pares articulares de actuación [12], el elipsoide de manipulabilidad y el índice de manipulabilidad [19, 20]. Además se aplica el concepto de índices de desempeño para la determinación de

q1

a1

a2

q2

MIM - 2004 - I - 01

18

posturas que maximizaran algún criterio cinemático en la realización de tareas con el brazo humano. Diferentes autores han contribuido a este tema [3]. Se utilizaron técnicas de optimización para la determinación de configuraciones óptimas [11].

MIM - 2004 - I - 01

19

4. CÁLCULO DE LOS MOMENTOS DE ACTUACIÓN ARTICULARES Se define f como el vector de fuerzas y momentos externos aplicados en el extremo del manipulador, y tf el vector de momentos articulares de actuación. Se puede establecer mediante un simple cálculo de conservación de potencia que [12]:

fJt Tf )(q= (Ec. 3)

donde JT(q) denota la transpuesta de la matriz jacobiana. De esta forma, es posible calcular los momentos necesarios para resistir una fuerza dada en el extremo del manipulador (en este caso, la mano).

A manera de ejemplo suponga el modelo simple. Se quiere simular el levantamiento de una carga. Se aplica una fuerza constante en dirección Y negativa (vertical hacia abajo) en el extremo del mecanismo, es decir f = [0 –F 0 0 0 0]T, se obtiene por medio de AUTOLEV® que los momentos de actuación son de la forma:

+−

++−=

)cos())cos()cos((

212

21211

qqFaqqaqaF

ft

De este análisis es importante resaltar cómo la matriz Jacobiana posee información tanto de cómo se está transmitiendo tanto velocidad (movimiento controlado) como fuerza mediante el órgano terminal. Es natural pensar que el estudio de esta matriz podrá dar información del desempeño del brazo. Esta técnica es ampliamente utilizada en robótica para la evaluación de manipuladores simples [8]. El uso de la aplicación de esta herramienta al modelo completo depende de tener una medida confiable de la fuerza que se está ejerciendo con la mano, para poder predecir los momentos de actuación en las articulaciones. Utilizar este tipo de técnicas en el análisis de gestos deportivos y laborales podrá determinar posturas y movimientos que minimicen el impacto sobre las articulaciones el miembro superior.

MIM - 2004 - I - 01

20

5. ELIPSOIDES DE MANIPULABILIDAD El concepto de manipulabilidad es ampliamente utilizado en el área de la robótica para la evaluación de robots manipuladores [20]. Aunque la interpretación de este índice ha sido objeto de discusión entre varios autores, en este artículo se interpreta la manipulabilidad como la eficacia con la cual un manipulador transmite fuerza y velocidad en su órgano terminal [18]. Se utilizan los conceptos de elipsoide de manipulabilidad de velocidad y elipsoide de manipulabilidad de fuerza para analizar el desempeño del brazo en diferentes configuraciones. Para la aplicación de estos conceptos se desprecian los efectos dinámicos e inerciales del brazo, así como los efectos gravitacionales. Igualmente se asume que los actuadores ubicados en las articulaciones se comportan bajo la restricción que el vector de momentos de actuación tiene norma unitaria [19]. 5.1 MANIPULABILIDAD DE FUERZA El elipsoide de manipulabilidad de fuerza es un espacio geométrico que caracteriza las fuerzas que pueden ser generadas en el extremo del manipulador por un conjunto dado de momentos articulares, si el manipulador se encuentra en una postura dada [19]. En otras palabras, es una medida de la facilidad con la cual se puede transformar los momentos articulares en una fuerza del extremo, dadas una dirección de aplicación y una configuración del brazo. Si se asume que el vector de momentos de actuación es unitario, es decir que cualquier momento de actuación puede ser producido mediante una combinación lineal de los momentos de cada uno de los actuadores, el momento de actuación total que produce el movimiento describe un espacio geométrico igual a una esfera en el espacio de momentos. Esta representación geométrica es útil al describir el comportamiento de los actuadores: sin importar la dirección de actuación, los actuadores generarán un momento de actuación directamente proporcional al radio de la esfera. Sin embargo, es natural pensar que la configuración del manipulador determina las direcciones en las cuales es más fácil ejercer fuerza. Como en cualquier mecanismo, existirán posiciones más favorables para resistir fuerza, en especial, cuando el ángulo de desviación se acerque a 0º [15]. Es decir, en el espacio de fuerza, las fuerzas generadas por el extremo del manipulador no describen una esfera [16]. Bajo la suposición anteriormente enunciada sobre los actuadores, Yoshikawa [19, 20] demostró que las posibles fuerzas desarrolladas por el extremo del manipulador u órgano terminal, describen un elipsoide en el espacio de fuerza. Además de esto, demostró que la magnitud de la fuerza que se puede ejercer en una dirección dada es directamente proporcional a la distancia que existe entre al centro del elipsoide y su superficie. Este concepto se aclara en la figura 4 a continuación.

MIM - 2004 - I - 01

21

u

fx

fy

α

Figura 4. Elipsoide de Manipulabilidad de Fuerza. Suponiendo que el movimiento del manipulador se restringe al plano, el elipsoide de fuerza es una elipse. El órgano terminal se encentra ubicado en el centro de la elipse mostrada. Entonces, la magnitud de la fuerza que el manipulador puede generar en la dirección del vector u (que va desde el centro de la elipse hasta su superficie) es directamente proporcional a la magnitud del vector, que denominamos α. Yoshikawa [19] demuestra que la magnitud del vector se calcula como:

uJJu TT )()(

1

qq=α (Ec. 4)

donde J y JT denotan la matriz Jacobiana y su transpuesta, respectivamente. Por lo tanto, para una misma configuración, el manipulador estará en capacidad de desarrollar más ó menos fuerza en diferentes direcciones, proporcional al factor α, conocido también como la Ventaja Mecánica del Manipulador VMM [18]. Es importante resaltar que el factor α varía según la configuración del manipulador, ya que depende explícitamente de la matriz Jacobiana. Además, de la figura resulta claro que existe una dirección en la cual la fuerza que se puede generar es máxima y otra en la cual la fuerza es mínima, coincidentes con los semiejes principales del elipsoide. Es posible demostrar que las direcciones de fuerza máxima y mínima coinciden con los autovectores de la matriz J(q)JT(q)) y que estas direcciones serán perpendiculares. 5.2 MANIPULABILIDAD DE VELOCIDAD Debido a la dualidad Fuerza–Velocidad, es claro que el manipulador tendrá direcciones preferentes para desarrollar altas velocidades. Análogo al elipsoide de manipulabilidad de fuerza, es posible definir el elipsoide de manipulabilidad de velocidad tal y como se muestra en la figura 5.

MIM - 2004 - I - 01

22

β

Vx

u

Vy

Figura 5. Elipsoide de Manipulabilidad de Velocidad. Nuevamente se restringe el movimiento del manipulador al plano. La velocidad máxima que puede generar el manipulador en la dirección u es directamente proporcional a la magnitud de dicho vector β. Es posible definir un coeficiente de transformación de velocidad β [3]:

uJJu TT 1))()((

1−

=qq

β (Ec. 5)

Este índice también se conoce como la Relación de Velocidad del Manipulador RVM [18]. Es claro, que debido a la conservación de la energía, las direcciones preferentes en fuerza serán las menos aptas para desarrollar altas velocidades, y viceversa. Esto se ve reflejado en los elipsoides de manipulabilidad, ya que el semieje mayor del elipsoide de manipulabilidad de fuerza coincidirá con el semieje menor del elipsoide de manipulabilidad de velocidad. Esta relación se muestra en la figura 6. Figura 6. Relación Elipsoides de Manipulabilidad

Velocidad

Fuerza

MIM - 2004 - I - 01

23

5.3 ÍNDICES DE MANIPULABILIDAD El índice de manipulabilidad para manipuladores redundantes se define como [19]:

))()(( qqw TJJDet= (Ec. 6) donde J(q) y JT(q) denotan la matriz Jacobiana y su transpuesta, respectivamente. El índice w es una medida directamente proporcional al volumen del elipsoide de manipulabilidad de velocidad [16]. Usualmente el índice de manipulabilidad es interpretado como una medida de la eficacia con la cual se transforman los desplazamientos articulares qi en desplazamientos del órgano terminal. Se define el índice de manipulabilidad de fuerza como:

w

w1

'= (Ec. 7)

El índice w’ se puede interpretar como la eficacia con la cual se transforman los momentos articulares en fuerzas y momentos en el órgano terminal [16]. 5.4 OTROS ÍNDICES DE DESEMPEÑO Existen diferentes índice de desempeño utilizados en robótica que permiten determinar cuantitativamente el desempeño de un manipulador al realizar una tarea [8]. Entre los más importantes se destacan [9]: Ø El número de condición de la traspuesta de la matriz Jacobiana, el cual es una

medida del error con el cual se trasmite fuerza ó velocidad mediante el órgano terminal.

Ø La relación de disipación de potencia, la cual es una medida de la potencia disipada por los actuadores en función de la configuración del manipulador.

Ø La disponibilidad de recorrido articular, la cual es una medida de lo cerca ó lejos que se encuentra el manipulador de sus límites articulares.

Durante el desarrollo de este trabajo se utilizaron los criterios anteriormente enunciados, pero solo se reportan los resultados obtenidos con el criterio de manipulabilidad. Para mayor información acerca de la definición matemática de estos índices y su interpretación se puede consultar la bibliografía pertinente [8, 9].

MIM - 2004 - I - 01

24

6. ELIPSOIDES DE MANIPULABILIDAD COMO FRONTERAS DE FUERZA Y VELOCIDAD MÁXIMAS

A continuación se presentarán algunos ejemplos de la aplicación de los modelos matemáticos y físicos explicados anteriormente al estudio del desempeño del brazo. Para visualizar el tipo de información que se obtiene mediante el método propuesto, por simplicidad, se utilizará el modelo simple. La figura 7 muestra ocho diferentes configuraciones con sus respectivas elipses de manipulabilidad de fuerza. Las elipses están dibujadas a una escala de 1:100 respecto a su magnitud real. Figura 7. Elipses de Manipulabilidad de Fuerza.

Como se dijo anteriormente, es natural que las elipses cambien tanto su orientación como la magnitud de sus semiejes a medida que cambia la configuración del brazo. El semieje mayor de la elipse tiende a infinito cuando el mecanismo se acerca a una configuración singular. La figura 8 muestra el comportamiento del índice de manipulabilidad de fuerza w’, el cual tiende a infinito teórico en las configuraciones singulares. Figura 8. Índice de Manipulabilidad de Fuerza.

0

50

100

150

200

0 30 60 90 120 150 180

Ángulo Relativo Barras [Deg]

w'

MIM - 2004 - I - 01

25

Elipse de Fuerza

00,5

11,5

22,5

33,5

1 2 3 4 5 6 7 8 9 10 11 12

Configuración

Lo

ng

. Eje

Men

or

Para las configuraciones mostradas las figuras 9, 10 y 11 muestran la longitud del semieje mayor de la elipse, la longitud del semieje menor de la elipse y el ángulo entre la horizontal y el semieje mayor. Figura 9. Longitud Semieje Mayor Elipse Fuerza Figura 10. Longitud Semieje Menor Elipse Fuerza.

Elipse de Fuerza

0

20

40

60

80

100

120

1 2 3 4 5 6 7 8 9 10 11 12

Configuración

Lo

ng

. Eje

May

or

MIM - 2004 - I - 01

26

Elipse de Fuerza

-100

-80

-60

-40

-20

01 2 3 4 5 6 7 8 9 10 11 12

Configuración

Án

gu

lo

Figura 11. Ángulo entre la Horizontal y el Semieje Mayor. La figura 12 muestra las elipses de manipulabilidad de velocidad para las mismas configuraciones anteriores a una escala de 1:10. Es posible ver, comparando con la figuras 7 cómo los ejes de las elipses de manipulabilidad de fuerza y de velocidad coinciden, pero sus semiejes están invertidos. En las figura 13 se muestra cómo varía el índice de manipulabilidad de velocidad w, anulándose en las configuraciones singulares (donde el manipulador pierde un grado de libertad). Figura 12. Elipses de Manipulabilidad de Velocidad

Al acercarse a una singularidad el mecanismo pierde la posibilidad de moverse en una dirección, por lo que el semieje menor tiende a cero y la elipse tiende a una recta.

MIM - 2004 - I - 01

27

0

0,02

0,04

0,06

0,08

0,1

0 30 60 90 120 150 180

Ángulo Relativo Barras [Deg]

w

Elipse de Velocidad

00,10,20,30,40,50,60,70,8

1 2 3 4 5 6 7 8 9 10 11 12

Configuración

Lo

ng

. Eje

May

or

Figura 13. Índice de Manipulabilidad de Velocidad.

Además es posible observar cómo el máximo valor de manipulabilidad de velocidad se alcanza cuando el ángulo entre las dos barras se acerca a 90º. Esto es natural para el ser humano, ya que en las tareas que requieren gran control en la velocidad, por ejemplo, al escribir, el brazo tiende a estar a 90º del antebrazo [16]. Al igual que en el caso anterior, las figuras 14, 15 y 16 muestran la longitud del semieje mayor de la elipse, la longitud del semieje menor de la elipse y el ángulo entre la horizontal y el semieje mayor. Figura 14. Longitud Semieje Mayor Elipse Velocidad

MIM - 2004 - I - 01

28

Figura 15. Longitud Semieje Menor Elipse Velocidad Figura 16. Ángulo entre la Horizontal y el Semieje Mayor

Elipse de Velocidad

0

0,05

0,1

0,15

0,2

0,25

1 2 3 4 5 6 7 8 9 10 11 12

Configuración

Lo

ng

. Eje

Men

or

Elipse de Velocidad

0

20

40

60

80

100

1 2 3 4 5 6 7 8 9 10 11 12

Configuración

Án

gu

lo

MIM - 2004 - I - 01

29

0

5000

10000

15000

20000

25000

30000

35000

Hombro - Misma Mano Hombro - Mano Contraria

Índi

ce d

e M

anip

ulab

ilida

d (w

)

7. APLICACIÓN AL ESTUDIO BIOMECÁNICO DEL BRAZO HUMANO Los resultados mostrados para el manipulador planar de dos eslabones pueden ser implementados en el modelo del brazo completo, en algunos casos sencillos de configuraciones relativamente simples. Como primera aproximación, se estudió la configuración del brazo en la ejecución de una tarea sencilla: rascarse un hombro. Es natural pensar que al realizar esta tarea se quiera encontrar de manera precisa el sitio exacto donde se encuentra la molestia, esto es, sería necesario aumentar la manipulabilidad de velocidad en la ejecución de esta tarea. Se aproximaron las configuraciones que adoptaría una persona para rascarse el hombro con la mano del mismo lado y con la mano contraria. Las configuraciones en grados se muestran en la tabla 2. Tabla 2. Configuraciones para Rascarse el Hombro & Índice de Manipulabilidad

Configuración q1 q2 q3 q4 q5 q6 q7 w

Mismo Lado 0 0 0 150 0 45 0 9554

Mano Contraria 30 -10 -90 90 -110 20 0 30876

Mediante el uso de los índices de desempeño es posible predecir que una persona sana utilizará la mano contraria al hombro que le molesta (índice de manipulabilidad de velocidad w = 30876), en vez de utilizar la mano del mismo lado para calmar la comezón (índice de manipulabilidad de velocidad w = 9554). El resultado lo muestra claramente la figura 17. Figura 17. Índice Manipulabilidad de Velocidad. Se realizó una prueba de validación de la aplicación de la manipulabilidad de velocidad. Se estudió la velocidad máxima desarrollada por un individuo bajo una condición de

MIM - 2004 - I - 01

30

configuración de miembro superior conocida. Por medio del software de análisis de movimiento APAS® de la compañía Ariel Dynamics, el cual utiliza una técnica de medición basada en marcadores reflectivos y películas de video, es posible medir la velocidad del extremo de la mano en un instante de tiempo determinado. Este software y el equipo necesario para realizar la prueba se encuentra ubicado en el Laboratorio de Análisis de Movimiento de la Universidad de los Andes, en el Instituto de Ortopedia Infantil Roosevelt en Bogotá, Colombia. Se tomaron medidas antropométricas del individuo (longitud de su brazo, antebrazo y mano) y se analizó la configuración q = [ 0º 0º 0º 90º 0º 0º 0º]T, la cual equivale a flexionar el antebrazo 90º de la posición anatómica [6]. Las direcciones de movimiento fueron u1 = [0.67 -0.54 0.50] y u2 = [-0.21 -0.96 0.15]. Las tablas 3 y 4 muestran las condiciones de la prueba de velocidad máxima. Tabla 3. Configuración Prueba Predicción Velocidad.

Configuración q1 q2 q3 q4 q5 q6 q7

Ángulo [Deg] 0 0 0 90 0 0 0

Tabla 4. Direcciones Prueba Predicción Velocidad

Dirección Movimiento X Y Z

U1 0.67 -0.54 0.50

U2 -0.21 -0.96 0.15

Los índices de manipulabilidad de velocidad en estas dos direcciones son: en la dirección u1 es β1 = 0.51 y en la dirección u2 es β2 = 0.37. Por lo tanto, la relación entre las velocidades en las direcciones u1 y u2 debe ser (0.51:0.37 = 1.38). Se encontró que, dada la configuración q, la velocidad máxima desarrollada por el individuo en la dirección es de u1 es de 10.6 m/s. Según el planteamiento descrito anteriormente, el sujeto estaría en capacidad de desarrollar 7.6 m/s en la dirección u2. Sin embargo la velocidad medida en esta dirección fue de 6.2 m/s, un 20 % menor a la esperada. Esta diferencia se debe al alto nivel de subjetividad que existe en pedirle a una persona que desarrolle una velocidad máxima con su cuerpo. Factores como los grupos musculares involucrados en cada uno de los movimientos y los cambios súbitos en la configuración puede afectar la medida. Sin embargo, se puede ver que el elipsoide de manipulabilidad de velocidad es en efecto una frontera del espacio de velocidades del brazo. Es decir, teóricamente ninguna velocidad podría superar la velocidad calculada por el índice β. Los resultados se muestran en la tabla 5 a continuación. Tabla 5. Resultados Predicción de Velocidad.

Dirección Movimiento

β Magnitud Teórica [m/s] Magnitud Real [m/s]

U1 0.51 10.6 10.6 U2 0.37 7.6 6.2

MIM - 2004 - I - 01

31

Es importante resaltar que el modelo solamente predice de forma correcta las fronteras de máxima velocidad cuando la dirección con la que se estima la velocidad corresponde a la máxima velocidad esperada por la teoría. Es decir, los mejores resultados se obtendrían al realizar la prueba moviéndose a máxima velocidad en la dirección del semieje mayor del elipsoide de velocidad. De la misma forma se realizó una prueba análoga para los elipsoides de manipulabilidad de fuerza. Por medio de un sensor de fuerza–par se midieron las fuerzas isométricas desarrolladas por el brazo en una configuración dada y se escalaron los elipsoides de fuerza. Se utilizó un transductor de Fuerza–Par de la firma ATI® Modelo Mini 45 cuya especificación se encuentra detallada en el anexo G. Este sensor es capaz de medir fuerzas y momentos en las tres direcciones como se muestra esquemáticamente en la figura 18 a continuación. Figura 18. Transductor de Fuerza–Par. Se realizó un montaje sencillo pero apropiado para medir la fuerza isométrica de un paciente cualquiera [5]. En la figura 19 se ilustra el montaje utilizado. Figura 19. Montaje Transductor de Fuerza–Par.

MIM - 2004 - I - 01

32

La configuración analizada fue q = [0º 0º 0º 90º -90º 0º 0º]T, y las direcciones analizadas fueron u1 = [-0.04 -0.01 1]T y u2 = [-0.3 0.95 0]T. La tablas 6 y 7 resumen las condiciones de la prueba. Tabla 6. Configuración Prueba Predicción Fuerza

Configuración q1 q2 q3 q4 q5 q6 q7

Ángulo [Deg] 0 0 0 90 -90 0 0

Tabla 7. Direcciones Prueba Predicción Fuerza

Dirección Movimiento X Y Z

U1 (“Arriba”) -0.04 -0.01 1.00

U2 (“Al lado”) -0.30 0.95 0.00

En la figura 20 se muestra el elipsoide de manipulabilidad de fuerza en la configuración estudiada. Figura 20. Elipsoide de Manipulabilidad de Fuerza Los índices de manipulabilidad de fuerza en estas dos direcciones se calcularon como α1 = 1.734 en la dirección u1 y α2 = 1.997 en la dirección u2. Por lo tanto, la relación entre las posibles fuerzas desarrolladas en estas dos direcciones debe ser (1.734:1.997 = 0.868). Se midió la fuerza isométrica [5] en la dirección u1 en 140.29 N. El método predice que la fuerza desarrollada en u2 debe ser menor a 161.56 N. Por medio del sensor se pudo

MIM - 2004 - I - 01

33

Z

ELIPSOIDE PREDICHO

ELIPSOIDE REAL

establecer que la fuerza desarrollada en u2 es de 71.09 N. Es decir, la fuerza real es aproximadamente la mitad de la fuerza predicha por el modelo. La tabla 8 muestra los resultados obtenido durante la prueba. Tabla 8. Resultados Predicción de Fuerza.

Dirección Movimiento

α Magnitud Teórica [N] Magnitud Real [N]

“Arriba” 1.734 140.3 140.3 “Al lado” 1.997 161.5 71.1

Esta diferencia se debe a la no linealidad de los músculos como actuadores y al fenómeno de transferencia muscular, es decir, dependiendo de la dirección de transmisión de fuerza existirán diferentes músculos en el brazo que actúen para realizar esta tarea. Análogo a la prueba de velocidad máxima, la estimación de fuerza máxima resulta ser válida solamente si la fuerza utilizada para estimar las fuerzas en las otras direcciones, coincide con la mayor fuerza esperada. Este concepto se aclara en la figura 21 a continuación.. Figura 21. Elipsoide como Frontera de Fuerza Máxima Mediante este análisis es posible determinar configuraciones óptimas para la ejecución de una tarea que requiera generar velocidad ó fuerza en una dirección de movimiento específica. Su aplicación en el campo de la ergonomía va desde el diseño de estaciones de trabajo eficientes hasta evaluación de trabajadores para empleos físicamente exigentes.

MIM - 2004 - I - 01

34

8. DETERMINACIÓN DE POSTURAS ÓPTIMAS Dotar a un manipulador de más grados de libertad de los necesarios para realizar su tarea le permitirá acceder a configuraciones que evitarán de forma efectiva obstáculos en el entorno, además de la posibilidad de aumentar el volumen de su espacio de trabajo [2]. El brazo humano es un manipulador redundante, ya que la posee más grados de libertad de los necesarios para posicionar y orientar una herramienta en el espacio. En el caso de un manipulador redundante el modelo cinemático inverso es de la forma [3]: zJJIxJq )( ++ −+= && (Ec .8) en donde J+ denota la pseudo-inversa derecha de la matriz Jacobiana [16], I es la matriz identidad de [n+1] x [n+1] (n es el número de grados de libertad del manipulador) y z es un vector arbitrario en Rn+1. 8.1 SOLUCIÓN DE MÍNIMA NORMA El primer término de la derecha de la ecuación 8 se conoce como Solución de Mínima Norma y provee la mínima norma euclidiana, es decir, ubica el extremo del órgano terminal en el punto de la tarea deseado [11]. Se calcula mediante la minimización de la norma del vector que va desde el punto actual en el cual está ubicado el extremo del órgano terminal hasta el punto de la tarea deseado. Cuando esta norma sea igual a cero, el extremo del órgano terminal del manipulador estará ubicado en el punto de la tarea deseado. 8.2 SOLUCIÓN HOMOGÉNEA El segundo término a la derecha de la ecuación 8 es la Solución Homogénea de las velocidades articulares. Esto es, provee un movimiento interno de las articulaciones que no produce movimiento en el órgano terminal. Este proceso se conoce como Reconfiguración. Es común utilizar la solución homogénea para optimizar algún criterio cinemático [3, 11]. Si se define el vector arbitrario de la ecuación 8 como: )(qHz ∇= k (Ec. 9) donde )(qH∇ es el gradiente del índice de desempeño que se quiere maximizar y k es un coeficiente escalar positivo denominado Rapidez de Convergencia, la reconfiguración ocurrirá en la dirección en la cual aumenta el índice de desempeño [11].

MIM - 2004 - I - 01

35

Posición InicialSolución Mínima NormaOptimización Cinemática

Punto Inicial

Punto de la Tarea

El proceso de determinación de las configuraciones óptimas para realizar una tarea en el espacio se lleva a cabo mediante los siguientes pasos:

1.) Llevar el extremo del órgano terminal al i-ésimo punto de la tarea minimizando la norma euclidiana del vector de posición entre el órgano terminal y este punto

2.) Reconfigurar optimizando el criterio cinemático escogido. El proceso de optimización mediante reconfiguración se ilustra en la figura 22 por medio de un manipulador redundante de tres eslabones cuya tarea se especifica en el plano. Figura 22. Proceso de Optimización Mediante Reconfiguración. Inicialmente el manipulador mostrado se lleva al punto deseado (Punto de la Tarea) mediante la solución de mínima norma. Una vez allí, se lleva a cabo el proceso de reconfiguración mediante el método del gradiente explicado anteriormente. De esta forma, en la postura final se obtendrá un valor óptimo del criterio cinemático escogido para el proceso. Mediante el método propuesto es posible determinar la mejor postura que un trabajador, por ejemplo, debe adoptar para realizar una tarea específica.

MIM - 2004 - I - 01

36

9. DETERMINACIÓN DE TRAYECTORIAS ÓPTIMAS PARA REALIZAR TAREAS SIMPLES

En el capítulo anterior se explicó el método de optimización de posturas del miembro superior. Sin embargo, muchas de las tareas que realiza el ser humano no se limitan a llevar el brazo a una cierta posición, sino que involucran movimiento entre dos ó más puntos en el espacio. Más aún, en la mayoría de ocasiones no basta con alcanzar una posición en el espacio, sino que además se debe garantizar la orientación de la mano para poder realizar la tarea (escribir, golpear un clavo con un martillo, etc). En este capítulo se expondrá un método para determinar trayectorias óptimas de tareas simples. Llamaremos una tarea simple ó sencilla a una acción que pueda ser especificada mediante una configuración inicial y una configuración final en el espacio. Numerosos estudios han demostrado que el ser humano tiene ciertas características invariantes al realizar una tarea sencilla [4]: Ø Regla 1:

La trayectoria de la mano seguirá una línea recta Ø Regla 2:

El perfil de velocidad en dirección del movimiento será acampanado, es decir, una curva suave, simétrica y con un único máximo local ubicado en el punto medio de la trayectoria Ø Regla 3:

Las características anteriores son invariantes ante la duración del movimiento Teniendo en cuenta estas tres reglas, el perfil de velocidad entre los puntos inicial y final de la tarea exhibirán un comportamiento como el mostrado en la figura 23. Figura 23. Perfil de Velocidad Típico para una Tarea Simple. Bajo estos principios se definió una tarea que simula la acción de llevar la mano desde la tecla Bloq Mayús hasta la tecla Intro en un teclado de computador estándar. La tabla 9

MIM - 2004 - I - 01

37

muestra la ubicación espacial de los puntos de la tarea propuesta, definidos en el marco de referencia global. Las medidas están en centímetros. Tabla 9. Tarea llevar mano desde Bloq Mayús hasta Intro.

Punto Tarea X Y Z Inicial 30 30 -38 Final 30 5 -38

Para la trayectoria analizada la orientación de la mano no cambia a lo largo de la tarea. Entonces es posible especificar el vector de velocidad x& y así resolver el problema cinemático inverso. Para determinar la trayectoria óptima se utilizaron dos estrategias: la primera supone maximizar el criterio cinemático elegido )(qH en los puntos extremos e interpolar la trayectoria; la segunda utiliza los principios enunciados anteriormente determinando el vector de velocidad x& y realizando cada movimiento en la dirección del gradiente del criterio cinemático escogido )(qH∇ . Se eligió como criterio cinemático la manipulabilidad de velocidad, aunque es posible escoger otros criterios cinemáticos para este fin [8]. El método del gradiente arroja mejores resultados: es natural pensar que el cerebro actúe tomando en cuenta la información de posiciones anteriores y condiciones actuales. Es imposible pensar que el cerebro tiene configuraciones óptimas almacenadas en una base de datos y genera movimientos mediante interpolaciones de configuraciones previamente conocidas. La figura 24 muestra la evolución de este criterio durante el movimiento predicho mediante la estrategia del gradiente. Figura 24. Índice de Manipulabilidad de Velocidad.

42

45

48

51

54

0 20 40 60 80 100

Porcentaje de Movimiento [%]

w e

- 3

La figura 25 muestra la variación óptima de la flexión y la abducción del hombro en función del porcentaje de movimiento realizado para la tarea analizada.

MIM - 2004 - I - 01

38

Figura 25. Evolución Óptima de la Flexión y Abducción del Hombro

0

10

20

30

40

50

Porcentaje de Movimiento [%]

[Deg

]

Flexión Abducción

Es posible ver que la variación en el ángulo mostrado coincide fielmente con el movimiento que adoptaría una persona sana para realizar la tarea propuesta. Se realizó otra simulación de una tarea simple: agarrar un vaso que reposa sobre una mesa y beber. Por medio del laboratorio de análisis de movimiento se grabó este movimiento de un paciente sano y se determinaron los puntos iniciales y finales. La definición de este movimiento como una tarea simple se presenta en la tabla 10. Tabla 10. Tarea agarrar un vaso y llevarlo a la boca.

Punto Tarea X Y Z Inicial 30 -1 -19 Final 16 4 3

La figura 26 muestra la variación óptima (calculada) y real (medida) de la flexión del hombro para la tarea de beber. Figura 26. Evolución Óptima de la Flexión del Hombro.

10152025303540455055

0 20 40 60 80 100

Porcentaje de Movimiento [%]

[Deg

]

Óptima Real

MIM - 2004 - I - 01

39

La figura 27 muestra la evolución óptima de la flexión del codo comparada contra la forma en la cual el movimiento realmente se hizo. Figura 27. Evolución Óptima de la Flexión del codo. La figura 28 muestra las posiciones de la mano filmadas (reales) mientras que la figura 29 muestra la posición de la mano predicha por el modelo (optimizadas). Se puede ver como las formas “suaves” se conservan. Esto es una prueba que la trayectoria seguida por la mano cumple los principios sobre los cuales se basa la teoría: suavidad en la velocidad y trayectorias en línea recta. Figura 28. Posición Real del Extremo de la Mano.

80

95

110

125

140

155

170

185

0 20 40 60 80 100

Porcentaje de Movimiento [%]

[Deg

]

Óptima Real

-30

-20

-10

0

10

20

30

40

0 20 40 60 80 100

Porcentaje de Movimiento [%]

[cm

]

xreal yreal zreal

MIM - 2004 - I - 01

40

Figura 29. Posición Optimizada del Extremo de la Mano.

Las variaciones en las curvas real–predicción son consecuencia del proceso de optimización. Así, mediante la técnica propuesta es posible determinar qué tan bien está realizando una persona una tarea sin necesidad de compararla contra un patrón externo.

-30

-20

-10

0

10

20

30

40

0 20 40 60 80 100

Porcentaje de Movimiento [%]

[cm

]

xpredicción ypredicción zpredicción

MIM - 2004 - I - 01

41

10. CONCLUSIONES Y RECOMENDACIONES Este documento presenta un método para la evaluación del desempeño del brazo humano. Conociendo la configuración del brazo y las fuerzas aplicadas en la mano es posible hallar una aproximación a los momentos de actuación en las articulaciones, información de gran importancia para diversas áreas del conocimiento. Sin embargo, el modelo propuesto carece de información acerca de las fuerzas inerciales y las fuerzas gravitacionales y supone que el principio de superposición es aplicable a un sistema no lineal como lo es el brazo humano. Sin embargo la simplicidad del modelo de cálculo de momentos articulares lo hace especialmente atractivo. De otro lado, el método permite determinar configuraciones del brazo que maximicen la transmisión de fuerza ó velocidad. Se demostró que, aunque la saturación de los músculos del brazo para desarrollar velocidades y fuerzas máximas no es una medida del todo objetiva que depende de factores externos no controlables, es posible determinar valores máximos de velocidad utilizando el concepto de índice de manipulabilidad de velocidad y de fuerza. Se debe tener especial cuidado en que la estimación de las fuerzas y velocidades máximas en diferentes direcciones, esté basada en una medición de la fuerza ó la velocidad máxima teórica. La aplicación del concepto de manipulabilidad para robots manipuladores hasta el momento ha sido de forma estática: para una configuración conocida se dispone de dos elipsoides de manipulabilidad. Al aplicar este concepto al brazo humano, se obtiene alguna medida de la variación de los índices de manipulabilidad durante un ciclo de movimiento. Es interesante observar cómo cambian los elipsoides a medida que cambia la configuración del brazo en el tiempo y como se puede utilizar este cambio para que se optimice la tarea que se quiere realizar. La determinación de configuraciones que optimizan criterios de desempeño cinemáticos es una herramienta ampliamente utilizada en la planificación de trayectorias de robots manipuladores. Su aplicación al estudio biomecánico del brazo humano propone un vasto campo de aplicación: en el campo de la ergonomía puede ser utilizado en la determinación de posturas que incrementen la precisión con la cual se realiza una tarea, mientras que en el análisis de gestos deportivos permitiría identificar posturas que marcan la diferencia entre los deportistas de alto rendimiento. Además estos índices de desempeño pueden ser de gran interés para el diagnóstico de patologías y diseño de terapias de rehabilitación de miembro superior, ya que proveen información de qué tanta influencia tiene el movimiento degenerado de las articulaciones en tareas cotidianas como lavarse los dientes ó tomar café, sin necesidad de comparar al paciente contra un estándar externo. Basado en principios sencillos e incluso naturales, se desarrolló un algoritmo que permite optimizar trayectorias de tareas sencillas mediante el uso de criterios cinemáticos. Hasta el momento se ha utilizado el índice de manipulabilidad por ser el criterio cinemático que mejores resultados arroja en las simulaciones, pero se mantiene abierta la posibilidad de

MIM - 2004 - I - 01

42

utilizar otros criterios cinemáticos, como por ejemplo, la disponibilidad de rango articular JRAE. Además, se verificó la validez de los principios supuestos mediante la validación experimental. El método propuesto evita la necesidad de tener patrones externos de movimiento para la evaluación cuantitativa del desempeño del brazo humano. Es posible predecir posturas y trayectorias convenientes tomando en cuenta las posibilidades del individuo para generar fuerza ó velocidad, y así poder determinar de forma cuantitativa qué tan bien se está realizando la tarea en cuestión.

MIM - 2004 - I - 01

43

BIBLIOGRAFÍA

1. Abdel- Malek, K Yang, J, & Duncan, J. Unpubl. Human Performances Measures: Mathematics. University of Iowa, U.S.A.

2. Barrientos, A., Peñín, L., Balaguer, C. & Aracil, R. 1997. Fundamentos de Robótica. McGraw–Hill (ed), Madrid, España.

3. Chiu, S.L. 1988. Task Compatibility of Manipulator Postures. En The International

Journal of Robotics Research 7 (5) : 13 – 21.

4. Flash T. & Hogan N. 1985. The coordination of arm movements: an experimentally confirmed mathematical model. En The Journal of Neuroscience 5:1688–1703.

5. Gallagher S., Moore J. & Stobbe T. 1998. Physical Strength Assessment in Ergonomics. AIHA Press. Fairfax, VA. U.S.A.

6. Hislop, H. & Montgomery, J. 2002. DANIELS-WORTHIGHAM’S MUSCLE

TESTING: Techniques of Manual Examination. W.B. Saunders Company (ed). Philadelphia, U.S.A.

7. Kapandji, I.A. 1993. Cuadernos de Fisiología Articular. Miembro Superior. Toray

Masson (ed). Barcelona, España.

8. Klein, C.A., Blaho, B.E. 1987. Dexterity Measures for the Design and Control of

Kinematically Redundant Manipulators. En The International Journal of Robotics Research 6 (2) : 72 – 83.

9. Montes, J. 2000. Criterios de Implantación para Manipuladores Simples. Tesis

Magíster Ingeniería Mecánica, Universidad de los Andes. Bogotá, Colombia. 10. Pámanes, A. 1992. Contribution à l’etude de l’accesibilité aux tâches et á la

deternination du placement optimal de robots manipulateurs. Tesis Doctoral en Ingeniería Mecánica, Universidad de Poitiers, Francia.

MIM - 2004 - I - 01

44

11. Pámanes, A., Barrón, A. & Pinedo, C. 1999. Constrained Optimization in Redundacy Resolution of Robotic Manipulators. En Tenth World Congress on the Theory of Machines and Mechanisms. Oulu, Finlandia: 1057 – 1066.

12. Paul, R., Stevenson, C.N. 1983. Kinematic of Robot Wrist. En The International

Journal of Robotics Research 2 (1) : 31 – 38. 13. Rau,G, Disselhorst-Klug, R. & Schmidt, R. 2000. Movement Biomechanics goes

upwards: from the leg to the arm. En Journal of Biomechanics. 33(10): 1207 – 1216.

14. Rodríguez, C.F, Quintero, H. & Botero, J.C. 2003. Herramienta Cuantitativa para

Evaluación de la Funcionalidad del Brazo. En Contribuciones Tecnológicas para la Discapacidad. I Jornadas CYTED sobre Tecnologías de Apoyo a la Discapacidad. Natal, Brasil. pp: 33 – 40.

15. Erdman, A. & Sandor, G. 1997. Mechanisms Design. Vol. 1. Prentice–Hall Inc (ed).

New Jersey, U.S.A.

16. Sciavicco, L. & Siciliano, B. 1996. Modeling and Control of Robot Manipulators.

McGraw – Hill (ed), NY. U.S.A. 17. Shilling, R.J. 1990. Fundamentals of Robotics: Analysis and Control. Prentice-Hall

Inc. (ed). New Jersey, U.S.A. 18. Staffetti, E, Bruyninckx, H. & De Schutter, J. 2002. On the Invariance of

Manipulability Indices. En J. Lenarcic and F. Thomas (eds.), Advances in Robot Kinematics, Kluwer Academic Publishers: 57-66.

19. Yoshikawa, T. 1984. Analysis and Control of Robot Manipulators with Redundancy.

En Robotics Research: The First International Symposium. M. Brady & R. Paul, MIT Press (ed). Cambridge, MA. U.S.A.

20. Yoshikawa, T. 1985. Manipulability of Robotic Mechanisms. En The International Journal of Robotics Research 4 (2): 3 – 9.

MIM - 2004 - I - 01

45

ANEXO A. MOVIMIENTOS DEL HOMBRO

Flexión: tiene como eje de rotación el vector a1. Sus límites de movimiento son +180º y – 45º. Abducción: tiene como eje de rotación el vector a2 = a3 x a1 . Sus límites de movimiento son +180º y – 70º. Rotación. tiene como eje de rotación el vector a3. Sus límites de movimiento son +90º y – 90º.

MIM - 2004 - I - 01

46

ANEXO B. MOVIMIENTOS DEL CODO Flexión: tiene como eje de rotación el vector b1. Sus límites de movimiento son +150º y 0º. Pronación: tiene como eje de rotación el vector b3. Sus límites de movimiento son +10º y – 170º.

MIM - 2004 - I - 01

47

ANEXO C. MOVIMIENTOS DE LA MUÑECA

Flexión: tiene como eje de rotación el vector c1. Sus límites de movimiento son +80º y – 80º. Desviación: tiene como eje de rotación el vector c2 = c3 x c1 . Sus límites de movimiento son +10º y – 45º.

MIM - 2004 - I - 01

48

ANEXO D. PROGRAMA CINEMÁTICA DIFERENCIAL DEL BRAZO % File: BRAZO.AL [AUTOLEV 3] % Date: Septiembre 17, 2003 % Author: Juan Camilo Botero M. % Purpose: Modelo de Brazo 1 % Problem: Descripción de la cinemática diferencial y cálculo de la Jacobiana %-------------------------------------------------------------------- % Default settings AUTOEPSILON 1.0E-14 AUTORHS ON AUTOZ OFF DEGREES ON DIGITS 4 OVERWRITE OFF %-------------------------------------------------------------------- % Newtonian, bodies, frames, particles, points NEWTONIAN N FRAMES CERO BODIES A,B FRAMES I BODIES C POINTS HOMBRO, CODO, MUÑECA, DEDO %-------------------------------------------------------------------- % Variables, constants, specified, mass, inertia VARIABLES Q{7}'' CONSTANTS L1,L2,L3 %-------------------------------------------------------------------- % Geometry relating unit vectors N_CERO=[0,-1,0;-1,0,0;0,0,-1] DIRCOS(CERO,A,BODY123,Q1,Q2,Q3) DIRCOS(A,B,BODY123,Q4,0,0) DIRCOS(B,I,BODY123,0,0,Q5) DIRCOS(I,C,BODY123,Q6,Q7,0) %-------------------------------------------------------------------- % Position vectors P_HOMBRO_CODO>=L1*A3> P_CODO_MUÑECA>=L2*B3> P_MUÑECA_DEDO>=L3*C3> %-------------------------------------------------------------------- % Positions POSICION>=EXPRESS(P_HOMBRO_DEDO>,N) X=DOT(POSICION>,N1>) Y=DOT(POSICION>,N2>) Z=DOT(POSICION>,N3>) %-------------------------------------------------------------------- % Angular velocities OMEGA>=W_C_N> M1=DOT(OMEGA>,N1>) M2=DOT(OMEGA>,N2>) M3=DOT(OMEGA>,N3>) %-------------------------------------------------------------------- % Jacobian Matrix J[1,1]=D(X,Q1) J[1,2]=D(X,Q2) J[1,3]=D(X,Q3) J[1,4]=D(X,Q4) J[1,5]=D(X,Q5) J[1,6]=D(X,Q6)

MIM - 2004 - I - 01

49

J[1,7]=D(X,Q7) J[2,1]=D(Y,Q1) J[2,2]=D(Y,Q2) J[2,3]=D(Y,Q3) J[2,4]=D(Y,Q4) J[2,5]=D(Y,Q5) J[2,6]=D(Y,Q6) J[2,7]=D(Y,Q7) J[3,1]=D(Z,Q1) J[3,2]=D(Z,Q2) J[3,3]=D(Z,Q3) J[3,4]=D(Z,Q4) J[3,5]=D(Z,Q5) J[3,6]=D(Z,Q6) J[3,7]=D(Z,Q7) J[4,1]=D(M1,Q1') J[4,2]=D(M1,Q2') J[4,3]=D(M1,Q3') J[4,4]=D(M1,Q4') J[4,5]=D(M1,Q5') J[4,6]=D(M1,Q6') J[4,7]=D(M1,Q7') J[5,1]=D(M2,Q1') J[5,2]=D(M2,Q2') J[5,3]=D(M2,Q3') J[5,4]=D(M2,Q4') J[5,5]=D(M2,Q5') J[5,6]=D(M2,Q6') J[5,7]=D(M2,Q7') J[6,1]=D(M3,Q1') J[6,2]=D(M3,Q2') J[6,3]=D(M3,Q3') J[6,4]=D(M3,Q4') J[6,5]=D(M3,Q5') J[6,6]=D(M3,Q6') J[6,7]=D(M3,Q7') %--------------------------------------------------------------------

MIM - 2004 - I - 01

50

ANEXO E. MATRIZ JACOBIANA MODELO COMPLETO

J[1,1] = L1*COS(Q1)*COS(Q2) + L2*(COS(Q1)*COS(Q2)*COS(Q4)-SIN(Q4)*(SIN(Q1)*COS(Q3)+SIN(Q2)*SIN(Q3)*COS(Q1))) + L3*((SIN(Q7)*COS(Q5)+SIN(Q5)* SIN(Q6)*COS(Q7))*(SIN(Q1)*SIN(Q3)-SIN(Q2)*COS(Q1)*COS(Q3))+COS(Q1)*COS( Q2)*(COS(Q4)*COS(Q6)*COS(Q7)+SIN(Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS(Q5)* COS(Q7)))-(SIN(Q1)*COS(Q3)+SIN(Q2)*SIN(Q3)*COS(Q1))*(SIN(Q4)*COS(Q6)* COS(Q7)-COS(Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS(Q5)*COS(Q7)))) J[1,2] = -SIN(Q1)*(L1*SIN(Q2)+L2*(SIN(Q2)*COS(Q4)+SIN(Q3)*SIN(Q4)*COS( Q2))+L3*(COS(Q2)*COS(Q3)*(SIN(Q7)*COS(Q5)+SIN(Q5)*SIN(Q6)*COS(Q7))+SIN( Q2)*(COS(Q4)*COS(Q6)*COS(Q7)+SIN(Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS(Q5)* COS(Q7)))+SIN(Q3)*COS(Q2)*(SIN(Q4)*COS(Q6)*COS(Q7)-COS(Q4)*(SIN(Q5)*SIN (Q7)-SIN(Q6)*COS(Q5)*COS(Q7))))) J[1,3] = -L2*SIN(Q4)*(SIN(Q3)*COS(Q1)+SIN(Q1)*SIN(Q2)*COS(Q3)) - L3*(( SIN(Q7)*COS(Q5)+SIN(Q5)*SIN(Q6)*COS(Q7))*(COS(Q1)*COS(Q3)-SIN(Q1)*SIN( Q2)*SIN(Q3))+(SIN(Q3)*COS(Q1)+SIN(Q1)*SIN(Q2)*COS(Q3))*(SIN(Q4)*COS(Q6) *COS(Q7)-COS(Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS(Q5)*COS(Q7)))) J[1,4] = -L2*(SIN(Q1)*SIN(Q4)*COS(Q2)-COS(Q4)*(COS(Q1)*COS(Q3)-SIN(Q1)* SIN(Q2)*SIN(Q3))) - L3*(SIN(Q1)*COS(Q2)*(SIN(Q4)*COS(Q6)*COS(Q7)-COS( Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS(Q5)*COS(Q7)))-(COS(Q1)*COS(Q3)-SIN(Q1) *SIN(Q2)*SIN(Q3))*(COS(Q4)*COS(Q6)*COS(Q7)+SIN(Q4)*(SIN(Q5)*SIN(Q7)-SIN (Q6)*COS(Q5)*COS(Q7)))) J[1,5] = L3*(SIN(Q1)*SIN(Q4)*COS(Q2)*(SIN(Q7)*COS(Q5)+SIN(Q5)*SIN(Q6)* COS(Q7))+(SIN(Q3)*COS(Q1)+SIN(Q1)*SIN(Q2)*COS(Q3))*(SIN(Q5)*SIN(Q7)-SIN (Q6)*COS(Q5)*COS(Q7))-COS(Q4)*(SIN(Q7)*COS(Q5)+SIN(Q5)*SIN(Q6)*COS(Q7)) *(COS(Q1)*COS(Q3)-SIN(Q1)*SIN(Q2)*SIN(Q3))) J[1,6] = -L3*COS(Q7)*(SIN(Q1)*COS(Q2)*(SIN(Q6)*COS(Q4)+SIN(Q4)*COS(Q5)* COS(Q6))+SIN(Q5)*COS(Q6)*(SIN(Q3)*COS(Q1)+SIN(Q1)*SIN(Q2)*COS(Q3))+(SIN (Q4)*SIN(Q6)-COS(Q4)*COS(Q5)*COS(Q6))*(COS(Q1)*COS(Q3)-SIN(Q1)*SIN(Q2)*SIN(Q3))) J[1,7] = -L3*((SIN(Q3)*COS(Q1)+SIN(Q1)*SIN(Q2)*COS(Q3))*(COS(Q5)*COS( Q7)-SIN(Q5)*SIN(Q6)*SIN(Q7))+SIN(Q1)*COS(Q2)*(SIN(Q7)*COS(Q4)*COS(Q6)- SIN(Q4)*(SIN(Q5)*COS(Q7)+SIN(Q6)*SIN(Q7)*COS(Q5)))+(COS(Q1)*COS(Q3)-SIN (Q1)*SIN(Q2)*SIN(Q3))*(SIN(Q4)*SIN(Q7)*COS(Q6)+COS(Q4)*(SIN(Q5)*COS(Q7)+SIN(Q6)*SIN(Q7)*COS(Q5)))) J[2,1] = 0 J[2,2] = -L1*COS(Q2) - L2*(COS(Q2)*COS(Q4)-SIN(Q2)*SIN(Q3)*SIN(Q4)) - L3*(COS(Q2)*(COS(Q4)*COS(Q6)*COS(Q7)+SIN(Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)* COS(Q5)*COS(Q7)))-SIN(Q2)*COS(Q3)*(SIN(Q7)*COS(Q5)+SIN(Q5)*SIN(Q6)*COS (Q7))-SIN(Q2)*SIN(Q3)*(SIN(Q4)*COS(Q6)*COS(Q7)-COS(Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS(Q5)*COS(Q7)))) J[2,3] = -COS(Q2)*(L2*SIN(Q4)*COS(Q3)-L3*(SIN(Q3)*(SIN(Q7)*COS(Q5)+SIN (Q5)*SIN(Q6)*COS(Q7))-COS(Q3)*(SIN(Q4)*COS(Q6)*COS(Q7)-COS(Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS(Q5)*COS(Q7))))) J[2,4] = L2*(SIN(Q2)*SIN(Q4)-SIN(Q3)*COS(Q2)*COS(Q4)) + L3*(SIN(Q2)*( SIN(Q4)*COS(Q6)*COS(Q7)-COS(Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS(Q5)*COS( Q7)))-SIN(Q3)*COS(Q2)*(COS(Q4)*COS(Q6)*COS(Q7)+SIN(Q4)*(SIN(Q5)*SIN( Q7)-SIN(Q6)*COS(Q5)*COS(Q7))))

MIM - 2004 - I - 01

51

J[2,5] = -L3*(SIN(Q2)*SIN(Q4)*(SIN(Q7)*COS(Q5)+SIN(Q5)*SIN(Q6)*COS(Q7) )-COS(Q2)*COS(Q3)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS(Q5)*COS(Q7))-SIN(Q3)* COS(Q2)*COS(Q4)*(SIN(Q7)*COS(Q5)+SIN(Q5)*SIN(Q6)*COS(Q7))) J[2,6] = -L3*COS(Q7)*(SIN(Q5)*COS(Q2)*COS(Q3)*COS(Q6)-SIN(Q2)*(SIN(Q6) *COS(Q4)+SIN(Q4)*COS(Q5)*COS(Q6))-SIN(Q3)*COS(Q2)*(SIN(Q4)*SIN(Q6)-COS (Q4)*COS(Q5)*COS(Q6))) J[2,7] = -L3*(COS(Q2)*COS(Q3)*(COS(Q5)*COS(Q7)-SIN(Q5)*SIN(Q6)*SIN(Q7) )-SIN(Q2)*(SIN(Q7)*COS(Q4)*COS(Q6)-SIN(Q4)*(SIN(Q5)*COS(Q7)+SIN(Q6)* SIN(Q7)*COS(Q5)))-SIN(Q3)*COS(Q2)*(SIN(Q4)*SIN(Q7)*COS(Q6)+COS(Q4)*( SIN(Q5)*COS(Q7)+SIN(Q6)*SIN(Q7)*COS(Q5)))) J[3,1] = L1*SIN(Q1)*COS(Q2) + L2*(SIN(Q1)*COS(Q2)*COS(Q4)+SIN(Q4)*(COS (Q1)*COS(Q3)-SIN(Q1)*SIN(Q2)*SIN(Q3))) - L3*((SIN(Q3)*COS(Q1)+SIN(Q1)* SIN(Q2)*COS(Q3))*(SIN(Q7)*COS(Q5)+SIN(Q5)*SIN(Q6)*COS(Q7))-SIN(Q1)*COS (Q2)*(COS(Q4)*COS(Q6)*COS(Q7)+SIN(Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS(Q5) *COS(Q7)))-(COS(Q1)*COS(Q3)-SIN(Q1)*SIN(Q2)*SIN(Q3))*(SIN(Q4)*COS(Q6)* COS(Q7)-COS(Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS(Q5)*COS(Q7)))) J[3,2] = COS(Q1)*(L1*SIN(Q2)+L2*(SIN(Q2)*COS(Q4)+SIN(Q3)*SIN(Q4)*COS( Q2))+L3*(COS(Q2)*COS(Q3)*(SIN(Q7)*COS(Q5)+SIN(Q5)*SIN(Q6)*COS(Q7))+SIN (Q2)*(COS(Q4)*COS(Q6)*COS(Q7)+SIN(Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS(Q5) *COS(Q7)))+SIN(Q3)*COS(Q2)*(SIN(Q4)*COS(Q6)*COS(Q7)-COS(Q4)*(SIN(Q5)* SIN(Q7)-SIN(Q6)*COS(Q5)*COS(Q7))))) J[3,3] = -L2*SIN(Q4)*(SIN(Q1)*SIN(Q3)-SIN(Q2)*COS(Q1)*COS(Q3)) - L3*(( SIN(Q1)*COS(Q3)+SIN(Q2)*SIN(Q3)*COS(Q1))*(SIN(Q7)*COS(Q5)+SIN(Q5)*SIN( Q6)*COS(Q7))+(SIN(Q1)*SIN(Q3)-SIN(Q2)*COS(Q1)*COS(Q3))*(SIN(Q4)*COS( Q6)*COS(Q7)-COS(Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS(Q5)*COS(Q7)))) J[3,4] = L2*(SIN(Q4)*COS(Q1)*COS(Q2)+COS(Q4)*(SIN(Q1)*COS(Q3)+SIN(Q2)* SIN(Q3)*COS(Q1))) + L3*(COS(Q1)*COS(Q2)*(SIN(Q4)*COS(Q6)*COS(Q7)-COS( Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS(Q5)*COS(Q7)))+(SIN(Q1)*COS(Q3)+SIN( Q2)*SIN(Q3)*COS(Q1))*(COS(Q4)*COS(Q6)*COS(Q7)+SIN(Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS(Q5)*COS(Q7)))) J[3,5] = L3*((SIN(Q1)*SIN(Q3)-SIN(Q2)*COS(Q1)*COS(Q3))*(SIN(Q5)*SIN( Q7)-SIN(Q6)*COS(Q5)*COS(Q7))-SIN(Q4)*COS(Q1)*COS(Q2)*(SIN(Q7)*COS(Q5)+ SIN(Q5)*SIN(Q6)*COS(Q7))-COS(Q4)*(SIN(Q1)*COS(Q3)+SIN(Q2)*SIN(Q3)*COS( Q1))*(SIN(Q7)*COS(Q5)+SIN(Q5)*SIN(Q6)*COS(Q7))) J[3,6] = L3*COS(Q7)*(COS(Q1)*COS(Q2)*(SIN(Q6)*COS(Q4)+SIN(Q4)*COS(Q5)* COS(Q6))-SIN(Q5)*COS(Q6)*(SIN(Q1)*SIN(Q3)-SIN(Q2)*COS(Q1)*COS(Q3))-( SIN(Q1)*COS(Q3)+SIN(Q2)*SIN(Q3)*COS(Q1))*(SIN(Q4)*SIN(Q6)-COS(Q4)*COS(Q5)*COS(Q6))) J[3,7] = L3*(COS(Q1)*COS(Q2)*(SIN(Q7)*COS(Q4)*COS(Q6)-SIN(Q4)*(SIN(Q5) *COS(Q7)+SIN(Q6)*SIN(Q7)*COS(Q5)))-(SIN(Q1)*SIN(Q3)-SIN(Q2)*COS(Q1)* COS(Q3))*(COS(Q5)*COS(Q7)-SIN(Q5)*SIN(Q6)*SIN(Q7))-(SIN(Q1)*COS(Q3)+ SIN(Q2)*SIN(Q3)*COS(Q1))*(SIN(Q4)*SIN(Q7)*COS(Q6)+COS(Q4)*(SIN(Q5)*COS(Q7)+SIN(Q6)*SIN(Q7)*COS(Q5)))) J[4,1] = -COS(Q2)*(COS(Q3)*(SIN(Q3)*COS(Q1)+SIN(Q1)*SIN(Q2)*COS(Q3))- SIN(Q1)*SIN(Q2)-SIN(Q3)*(COS(Q1)*COS(Q3)-SIN(Q1)*SIN(Q2)*SIN(Q3))) J[4,2] = -SIN(Q3)*(SIN(Q3)*COS(Q1)+SIN(Q1)*SIN(Q2)*COS(Q3)) - COS(Q3)*(COS(Q1)*COS(Q3)-SIN(Q1)*SIN(Q2)*SIN(Q3)) J[4,3] = SIN(Q1)*COS(Q2)

MIM - 2004 - I - 01

52

J[4,4] = -SIN(Q3)*COS(Q1) - SIN(Q1)*SIN(Q2)*COS(Q3) J[4,5] = SIN(Q1)*COS(Q2)*COS(Q4) + SIN(Q4)*(COS(Q1)*COS(Q3)-SIN(Q1)*SIN(Q2)*SIN(Q3)) J[4,6] = -COS(Q7)*((SIN(Q3)*COS(Q1)+SIN(Q1)*SIN(Q2)*COS(Q3))*(COS(Q5)* COS(Q7)-SIN(Q5)*SIN(Q6)*SIN(Q7))+SIN(Q1)*COS(Q2)*(SIN(Q7)*COS(Q4)*COS( Q6)-SIN(Q4)*(SIN(Q5)*COS(Q7)+SIN(Q6)*SIN(Q7)*COS(Q5)))+(COS(Q1)*COS( Q3)-SIN(Q1)*SIN(Q2)*SIN(Q3))*(SIN(Q4)*SIN(Q7)*COS(Q6)+COS(Q4)*(SIN(Q5) *COS(Q7)+SIN(Q6)*SIN(Q7)*COS(Q5)))) - SIN(Q7)*((SIN(Q3)*COS(Q1)+SIN( Q1)*SIN(Q2)*COS(Q3))*(SIN(Q7)*COS(Q5)+SIN(Q5)*SIN(Q6)*COS(Q7))-SIN(Q1) *COS(Q2)*(COS(Q4)*COS(Q6)*COS(Q7)+SIN(Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS (Q5)*COS(Q7)))-(COS(Q1)*COS(Q3)-SIN(Q1)*SIN(Q2)*SIN(Q3))*(SIN(Q4)*COS( Q6)*COS(Q7)-COS(Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS(Q5)*COS(Q7)))) J[4,7] = SIN(Q1)*COS(Q2)*(SIN(Q6)*COS(Q4)+SIN(Q4)*COS(Q5)*COS(Q6)) + SIN(Q5)*COS(Q6)*(SIN(Q3)*COS(Q1)+SIN(Q1)*SIN(Q2)*COS(Q3)) + (SIN(Q4)* SIN(Q6)-COS(Q4)*COS(Q5)*COS(Q6))*(COS(Q1)*COS(Q3)-SIN(Q1)*SIN(Q2)*SIN(Q3)) J[5,1] = -1 J[5,2] = 0 J[5,3] = -SIN(Q2) J[5,4] = -COS(Q2)*COS(Q3) J[5,5] = -SIN(Q2)*COS(Q4) - SIN(Q3)*SIN(Q4)*COS(Q2) J[5,6] = -SIN(Q7)*(COS(Q2)*COS(Q3)*(SIN(Q7)*COS(Q5)+SIN(Q5)*SIN(Q6)* COS(Q7))+SIN(Q2)*(COS(Q4)*COS(Q6)*COS(Q7)+SIN(Q4)*(SIN(Q5)*SIN(Q7)-SIN (Q6)*COS(Q5)*COS(Q7)))+SIN(Q3)*COS(Q2)*(SIN(Q4)*COS(Q6)*COS(Q7)-COS( Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS(Q5)*COS(Q7)))) - COS(Q7)*(COS(Q2)*COS (Q3)*(COS(Q5)*COS(Q7)-SIN(Q5)*SIN(Q6)*SIN(Q7))-SIN(Q2)*(SIN(Q7)*COS( Q4)*COS(Q6)-SIN(Q4)*(SIN(Q5)*COS(Q7)+SIN(Q6)*SIN(Q7)*COS(Q5)))-SIN(Q3) *COS(Q2)*(SIN(Q4)*SIN(Q7)*COS(Q6)+COS(Q4)*(SIN(Q5)*COS(Q7)+SIN(Q6)*SIN (Q7)*COS(Q5)))) J[5,7] = SIN(Q5)*COS(Q2)*COS(Q3)*COS(Q6) - SIN(Q2)*(SIN(Q6)*COS(Q4)+ SIN(Q4)*COS(Q5)*COS(Q6)) - SIN(Q3)*COS(Q2)*(SIN(Q4)*SIN(Q6)-COS(Q4)*COS(Q5)*COS(Q6)) J[6,1] = COS(Q2)*(SIN(Q3)*(SIN(Q1)*COS(Q3)+SIN(Q2)*SIN(Q3)*COS(Q1))- SIN(Q2)*COS(Q1)-COS(Q3)*(SIN(Q1)*SIN(Q3)-SIN(Q2)*COS(Q1)*COS(Q3))) J[6,2] = -COS(Q3)*(SIN(Q1)*COS(Q3)+SIN(Q2)*SIN(Q3)*COS(Q1)) - SIN(Q3)*(SIN(Q1)*SIN(Q3)-SIN(Q2)*COS(Q1)*COS(Q3)) J[6,3] = -COS(Q1)*COS(Q2) J[6,4] = SIN(Q2)*COS(Q1)*COS(Q3) - SIN(Q1)*SIN(Q3) J[6,5] = SIN(Q4)*(SIN(Q1)*COS(Q3)+SIN(Q2)*SIN(Q3)*COS(Q1)) - COS(Q1)* COS(Q2)*COS(Q4) J[6,6] = COS(Q7)*(COS(Q1)*COS(Q2)*(SIN(Q7)*COS(Q4)*COS(Q6)-SIN(Q4)*( SIN(Q5)*COS(Q7)+SIN(Q6)*SIN(Q7)*COS(Q5)))-(SIN(Q1)*SIN(Q3)-SIN(Q2)*COS (Q1)*COS(Q3))*(COS(Q5)*COS(Q7)-SIN(Q5)*SIN(Q6)*SIN(Q7))-(SIN(Q1)*COS( Q3)+SIN(Q2)*SIN(Q3)*COS(Q1))*(SIN(Q4)*SIN(Q7)*COS(Q6)+COS(Q4)*(SIN(Q5) *COS(Q7)+SIN(Q6)*SIN(Q7)*COS(Q5)))) - SIN(Q7)*((SIN(Q7)*COS(Q5)+SIN( Q5)*SIN(Q6)*COS(Q7))*(SIN(Q1)*SIN(Q3)-SIN(Q2)*COS(Q1)*COS(Q3))+COS(Q1) *COS(Q2)*(COS(Q4)*COS(Q6)*COS(Q7)+SIN(Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS (Q5)*COS(Q7)))-(SIN(Q1)*COS(Q3)+SIN(Q2)*SIN(Q3)*COS(Q1))*(SIN(Q4)*COS(

MIM - 2004 - I - 01

53

Q6)*COS(Q7)-COS(Q4)*(SIN(Q5)*SIN(Q7)-SIN(Q6)*COS(Q5)*COS(Q7)))) J[6,7] = SIN(Q5)*COS(Q6)*(SIN(Q1)*SIN(Q3)-SIN(Q2)*COS(Q1)*COS(Q3)) + ( SIN(Q1)*COS(Q3)+SIN(Q2)*SIN(Q3)*COS(Q1))*(SIN(Q4)*SIN(Q6)-COS(Q4)*COS( Q5)*COS(Q6)) - COS(Q1)*COS(Q2)*(SIN(Q6)*COS(Q4)+SIN(Q4)*COS(Q5)*COS(Q6))

MIM - 2004 - I - 01

54

ANEXO F. PROGRAMA OPTIMIZACIÓN TRAYECTORIA TAREA BEBER

clear all; %Variables auxialiares i=1; bandera=0; %Longitudes de los eslabones l1=31; l2=29; l3=11; L=[l1;l2;l3]; %Límites articulares UB=(pi/180)*[180;50;90;150;10;80;10]; LB=-(pi/180)*[0;5;90;0;170;70;45]; %Posición inicial de las variables articulares Q=(pi/180)*[10;10;0;90;-90;2;0]; Q_ant=zeros(7,1); %Punto inicial de la tarea P_inicial=[30;-1;-19]; P_final=[16;4;3]; %Orientacion inicial de la tarea phi=(pi/180)*-120; tetha=(pi/180)*88; psi=(pi/180)*-20; orientacion=[phi;tetha;psi]; %Orientacion final de la tarea orientacion_final=[(pi/180)*-120;(pi/180)*118;(pi/180)*-20]; %Vector vector=P_final-P_inicial; %Norma norma_vector=norm(vector); %Unitario vector_unitario=vector/norma_vector; %Tiempo de Movimiento tf=1; %Llenar el vector de velocidad t=0; while(t<=tf) velocidad(i,1)=(-140*norma_vector/tf^6)*t^6 + (420*norma_vector/tf^5)*t^5 + (-420*norma_vector/tf^4)*t^4 + (140*norma_vector/tf^3)*t^3; i=i+1; t=t+(tf/100); end velocidad(i,1)=0; %Llenar los vectores de cambio de orientacion

MIM - 2004 - I - 01

55

t=0; for i=1:length(velocidad) phipunto(i,1)=(-140*(abs(phi-orientacion_final(1)))/tf^6)*t^6 + (420*(abs(phi-orientacion_final(1)))/tf^5)*t^5 + (-420*(abs(phi-orientacion_final(1)))/tf^4)*t^4 + (140*(abs(phi-orientacion_final(1)))/tf^3)*t^3; tethapunto(i,1)=(-140*(abs(tetha-orientacion_final(2)))/tf^6)*t^6 + (420*(abs(tetha-orientacion_final(2)))/tf^5)*t^5 + (-420*(abs(tetha-orientacion_final(2)))/tf^4)*t^4 + (140*(abs(tetha-orientacion_final(2)))/tf^3)*t^3; psipunto(i,1)=(-140*(abs(psi-orientacion_final(3)))/tf^6)*t^6 + (420*(abs(psi-orientacion_final(3)))/tf^5)*t^5 + (-420*(abs(psi-orientacion_final(3)))/tf^4)*t^4 + (140*(abs(psi-orientacion_final(3)))/tf^3)*t^3; i=i+1; t=t+(tf/100); end %Incremento temporal deltat=1/(length(velocidad)-1); %Factor de rapidez de configuración k=9.6e-6; %Punto inicial de la trayectoria Punto=P_inicial; %Proceso Optimización 1 %Minimizar error de la posición cero a la posición deseada options=optimset('LargeScale','off'); options=optimset(options,'TolX',1e-8); options=optimset(options,'TolFun',1e-8); options=optimset(options,'MaxFunEvals',1e6); options=optimset(options,'MaxIter',1e4); Q=fmincon('norma',Q,[],[],[],[],LB,UB,'restriccion',options,Punto,L); %Proceso de optimizacion 2 %Maximizar criterio de desempeño garantizando orientacion options=optimset('LargeScale','off'); options=optimset(options,'TolX',1e-8); options=optimset(options,'TolFun',1e-8); options=optimset(options,'MaxFunEvals',1e6); options=optimset(options,'MaxIter',1e4); Q=fmincon('max_man1',Q,[],[],[],[],LB,UB,'rest11',options,Punto,L,orientacion); Q_inicial=Q; %Empezamos el ciclo for i=1:length(velocidad(:,1)) %Para una representación Euler ZYZ wx=-sin(phi)*tethapunto(i)+cos(phi)*sin(tetha)*psipunto(i); wy=cos(phi)*tethapunto(i)+sin(phi)*sin(tetha)*psipunto(i); wz=phipunto(i)+cos(tetha)*psipunto(i); %Velocidad en el extremo del órganos terminal xpunto=[velocidad(i)*vector_unitario(1);velocidad(i)*vector_unitario(2);velocidad(i)*vector_unitario(3);wx;wy;wz]; %Matriz Jacobiana J=jacobiana(Q,L);

MIM - 2004 - I - 01

56

%PseudoInversa de la Jacobiana PSEUDO=J'*inv((J*J')); %Función inversa de velocidad if bandera == 0 Deltah(1,1)=(manipulabilidad(Q,L)-(manipulabilidad(Q,L)+0.5))/(Q(1)+0.1); Deltah(2,1)=(manipulabilidad(Q,L)-(manipulabilidad(Q,L)+0.5))/(Q(2)+0.1); Deltah(3,1)=(manipulabilidad(Q,L)-(manipulabilidad(Q,L)+0.5))/(Q(3)+0.1); Deltah(4,1)=(manipulabilidad(Q,L)-(manipulabilidad(Q,L)+0.5))/(Q(4)+0.1); Deltah(5,1)=(manipulabilidad(Q,L)-(manipulabilidad(Q,L)+0.5))/(Q(5)+0.1); Deltah(6,1)=(manipulabilidad(Q,L)-(manipulabilidad(Q,L)+0.5))/(Q(6)+0.1); Deltah(7,1)=(manipulabilidad(Q,L)-(manipulabilidad(Q,L)+0.5))/(Q(7)+0.1); bandera=1; else if (Q(1)-Q_ant(1)~=0) Deltah(1,1)=(manipulabilidad(Q,L)-manipulabilidad(Q_ant,L))/(Q(1)-Q_ant(1)); else Deltah(1,1)=(manipulabilidad(Q,L)-(manipulabilidad(Q,L)+0.5))/(Q(1)+0.1); end if (Q(2)-Q_ant(2)~=0) Deltah(2,1)=(manipulabilidad(Q,L)-manipulabilidad(Q_ant,L))/(Q(2)-Q_ant(2)); else Deltah(2,1)=(manipulabilidad(Q,L)-(manipulabilidad(Q,L)+0.5))/(Q(2)+0.1); end if(Q(3)-Q_ant(3)~=0) Deltah(3,1)=(manipulabilidad(Q,L)-manipulabilidad(Q_ant,L))/(Q(3)-Q_ant(3)); else Deltah(3,1)=(manipulabilidad(Q,L)-(manipulabilidad(Q,L)+0.5))/(Q(3)+0.1); end if(Q(4)-Q_ant(4)~=0) Deltah(4,1)=(manipulabilidad(Q,L)-manipulabilidad(Q_ant,L))/(Q(4)-Q_ant(4)); else Deltah(4,1)=(manipulabilidad(Q,L)-(manipulabilidad(Q,L)+0.5))/(Q(4)+0.1); end if(Q(5)-Q_ant(5)~=0) Deltah(5,1)=(manipulabilidad(Q,L)-manipulabilidad(Q_ant,L))/(Q(5)-Q_ant(5)); else Deltah(5,1)=(manipulabilidad(Q,L)-(manipulabilidad(Q,L)+0.5))/(Q(5)+0.1); end if(Q(6)-Q_ant(6)~=0) Deltah(6,1)=(manipulabilidad(Q,L)-manipulabilidad(Q_ant,L))/(Q(6)-Q_ant(6)); else Deltah(6,1)=(manipulabilidad(Q,L)-(manipulabilidad(Q,L)+0.5))/(Q(6)+0.1); end if(Q(7)-Q_ant(7)~=0) Deltah(7,1)=(manipulabilidad(Q,L)-manipulabilidad(Q_ant,L))/(Q(7)-Q_ant(7)); else Deltah(7,1)=(manipulabilidad(Q,L)-(manipulabilidad(Q,L)+0.5))/(Q(7)+0.1); end end %Cálculo del vector Nu=k*Deltah; %Modelo Cinemático Inverso qpunto=PSEUDO*xpunto + (eye(7)-PSEUDO*J)*Nu; %Guardar la configuracion en un vector auxiliar Q_ant=Q; Q(1)=qpunto(1)*deltat + Q_ant(1); Q(2)=qpunto(2)*deltat + Q_ant(2); Q(3)=qpunto(3)*deltat + Q_ant(3); Q(4)=qpunto(4)*deltat + Q_ant(4); Q(5)=qpunto(5)*deltat + Q_ant(5);

MIM - 2004 - I - 01

57

Q(6)=qpunto(6)*deltat + Q_ant(6); Q(7)=qpunto(7)*deltat + Q_ant(7); %Guardar cada variable articular en un vector por separado q1(i)=Q(1); q2(i)=Q(2); q3(i)=Q(3); q4(i)=Q(4); q5(i)=Q(5); q6(i)=Q(6); q7(i)=Q(7); %Verificar posicion del extremo Punto=posicion(Q,L); posx(i)=Punto(1); posy(i)=Punto(2); posz(i)=Punto(3); %Calcular la orientación EULER ZYZ obtenida con los q's calculados %Matriz de Rotación M(1,1) = sin(q6(i))*sin(q7(i))*(sin(q5(i))*(sin(q3(i))*cos(q1(i))+sin(q1(i))*sin(q2(i))*cos(q3(i)))+cos(q5(i))*(sin(q1(i))*sin(q4(i))*cos(q2(i))-cos(q4(i))*(cos(q1(i))*cos(q3(i))-sin(q1(i))*sin(q2(i))*sin(q3(i))))) - sin(q7(i))*cos(q6(i))*(sin(q1(i))*cos(q2(i))*cos(q4(i))+sin(q4(i))*(cos(q1(i))*cos(q3(i))-sin(q1(i))*sin(q2(i))*sin(q3(i)))) - cos(q7(i))*(cos(q5(i))*(sin(q3(i))*cos(q1(i))+sin(q1(i))*sin(q2(i))*cos(q3(i)))-sin(q5(i))*(sin(q1(i))*sin(q4(i))*cos(q2(i))-cos(q4(i))*(cos(q1(i))*cos(q3(i))-sin(q1(i))*sin(q2(i))*sin(q3(i))))); M(1,2) = sin(q6(i))*(sin(q1(i))*cos(q2(i))*cos(q4(i))+sin(q4(i))*(cos(q1(i))*cos(q3(i))-sin(q1(i))*sin(q2(i))*sin(q3(i)))) + cos(q6(i))*(sin(q5(i))*(sin(q3(i))*cos(q1(i))+sin(q1(i))*sin(q2(i))*cos(q3(i)))+cos(q5(i))*(sin(q1(i))*sin(q4(i))*cos(q2(i))-cos(q4(i))*(cos(q1(i))*cos(q3(i))-sin(q1(i))*sin(q2(i))*sin(q3(i))))); M(1,3) = cos(q6(i))*cos(q7(i))*(sin(q1(i))*cos(q2(i))*cos(q4(i))+sin(q4(i))*(cos(q1(i))*cos(q3(i))-sin(q1(i))*sin(q2(i))*sin(q3(i)))) - sin(q7(i))*(cos(q5(i))*(sin(q3(i))*cos(q1(i))+sin(q1(i))*sin(q2(i))*cos(q3(i)))-sin(q5(i))*(sin(q1(i))*sin(q4(i))*cos(q2(i))-cos(q4(i))*(cos(q1(i))*cos(q3(i))-sin(q1(i))*sin(q2(i))*sin(q3(i))))) - sin(q6(i))*cos(q7(i))*(sin(q5(i))*(sin(q3(i))*cos(q1(i))+sin(q1(i))*sin(q2(i))*cos(q3(i)))+cos(q5(i))*(sin(q1(i))*sin(q4(i))*cos(q2(i))-cos(q4(i))*(cos(q1(i))*cos(q3(i))-sin(q1(i))*sin(q2(i))*sin(q3(i))))); M(2,1) = sin(q7(i))*cos(q6(i))*(sin(q2(i))*cos(q4(i))+sin(q3(i))*sin(q4(i))*cos(q2(i)))+ sin(q6(i))*sin(q7(i))*(sin(q5(i))*cos(q2(i))*cos(q3(i))-cos(q5(i))*(sin(q2(i))*sin(q4(i))-sin(q3(i))*cos(q2(i))*cos(q4(i)))) - cos(q7(i))*(cos(q2(i))*cos(q3(i))*cos(q5(i))+sin(q5(i))*(sin(q2(i))*sin(q4(i))-sin(q3(i))*cos(q2(i))*cos(q4(i)))); M(2,2) = cos(q6(i))*(sin(q5(i))*cos(q2(i))*cos(q3(i))-cos(q5(i))*(sin(q2(i))*sin(q4(i))-sin(q3(i))*cos(q2(i))*cos(q4(i)))) - sin(q6(i))*(sin(q2(i))*cos(q4(i))+sin(q3(i))*sin(q4(i))*cos(q2(i))); M(2,3) = -cos(q6(i))*cos(q7(i))*(sin(q2(i))*cos(q4(i))+sin(q3(i))*sin(q4(i))*cos(q2(i))) - sin(q7(i))*(cos(q2(i))*cos(q3(i))*cos(q5(i))+sin(q5(i))*(sin(q2(i))*sin(q4(i))-sin(q3(i))*cos(q2(i))*cos(q4(i)))) - sin(q6(i))*cos(q7(i))*(sin(q5(i))*cos(q2(i))*cos(q3(i))-cos(q5(i))*(sin(q2(i))*sin(q4(i))-sin(q3(i))*cos(q2(i))*cos(q4(i)))); M(3,1) = sin(q7(i))*cos(q6(i))*(cos(q1(i))*cos(q2(i))*cos(q4(i))-sin(q4(i))*(sin(q1(i))*cos(q3(i))+sin(q2(i))*sin(q3(i))*cos(q1(i)))) + sin(q6(i))*sin(q7(i))*(sin(q5(i))*(sin(q1(i))*sin(q3(i))-sin(q2(i))*cos(q1(i))*cos(q3(i)))-cos(q5(i))*(sin(q4(i))*cos(q1(i))*cos(q2(i))+cos(q4(i))*(sin(q1(i))*cos(q3(i))+sin(q2(i))*sin(q3(i))*cos(q1(i))))) - cos(q7(i))*(cos(q5(i))*(sin(q1(i))*sin(q3(i))-sin(q2(i))*cos(q1(i))*cos(q3(i)))+sin(q5(i))*(sin(q4(i))*cos(q1(i))*cos(q2(i))+cos(q4(i))*(sin(q1(i))*cos(q3(i))+sin(q2(i))*sin(q3(i))*cos(q1(i))))); M(3,2) = cos(q6(i))*(sin(q5(i))*(sin(q1(i))*sin(q3(i))-sin(q2(i))*cos(q1(i))*cos(q3(i)))-cos(q5(i))*(sin(q4(i))*cos(q1(i))*cos(q2(i))+cos(q4(i))*(sin(q1(i))*cos(q3(i))+sin(q2(i))*sin(q3(i))*cos(q1(i))))) - sin(q6(i))*(cos(q1(i))*cos(q2(i))*cos(q4(i))-sin(q4(i))*(sin(q1(i))*cos(q3(i))+sin(q2(i))*sin(q3(i))*cos(q1(i)))); M(3,3) = -cos(q6(i))*cos(q7(i))*(cos(q1(i))*cos(q2(i))*cos(q4(i))-sin(q4(i))*(sin(q1(i))*cos(q3(i))+sin(q2(i))*sin(q3(i))*cos(q1(i)))) - sin(q7(i))*(cos(q5(i))*(sin(q1(i))*sin(q3(i))-sin(q2(i))*cos(q1(i))*cos(q3(i)))+sin(q5(i))*(sin(q4(i))*cos(q1(i))*cos(q2(i))+cos(q4(i))*(sin(q1(i))*cos(q3(i))+sin(q

MIM - 2004 - I - 01

58

2(i))*sin(q3(i))*cos(q1(i))))) - sin(q6(i))*cos(q7(i))*(sin(q5(i))*(sin(q1(i))*sin(q3(i))-sin(q2(i))*cos(q1(i))*cos(q3(i)))-cos(q5(i))*(sin(q4(i))*cos(q1(i))*cos(q2(i))+cos(q4(i))*(sin(q1(i))*cos(q3(i))+sin(q2(i))*sin(q3(i))*cos(q1(i))))); %Matriz de Transformación R=[0,-1,0;-1,0,0;0,0,-1]*M; %Cálculo de los angulos de Euler alcanzados phi=atan2(R(2,3),R(1,3)); tetha=atan2(sqrt((R(1,3))^2+(R(2,3))^2),R(3,3)); psi=atan2(R(3,2),-(R(3,1))); %Cálculo de la Manipulabilidad w(i)=manipulabilidad(Q,L); %Orientacion z(i)=phi*180/pi; yp(i)=tetha*180/pi; zpp(i)=psi*180/pi; fid=fopen('Cafe_Tra.txt','At+'); fprintf(fid,'%6.4f %6.4f %6.4f %6.4f %6.4f %6.4f %6.4f %6.4f %6.4f\n',0,0,0,0,0,0,0,0,0); fprintf(fid,'%6.4f %6.4f %6.4f %6.4f %6.4f %6.4f %6.4f %6.4f 6.4f\n',0,0,Q(1)*180/pi,Q(2)*180/pi,Q(3)*180/pi,Q(4)*180/pi,90+Q(5)*180/pi,Q(6)*180/pi,Q(7)*180/pi); %Cierre del Ciclo end fclose(fid); %fid=fopen('posicion.txt','At+'); %fprintf(fid,'%6.4f\n',posz); %fclose(fid); Q_final=Q; Configuraciones=[Q_inicial*(180/pi),Q_final*(180/pi)]

MIM - 2004 - I - 01

59

ANEXO G. TRANSDUCTOR FUERZA – PAR ATI MINI 45 Las especificaciones completas del transductor se encuentran de forma gratuita en www.ati-ia.com.