trabajo robótica 4º industriales

ALUMNO: ALUMNO: ALUMNO: ALUMNO: MARTÍNEZ VERDÚ, Jaime

ALUMNO: ALUMNO: ALUMNO: ALUMNO: AZNAR GOMIS, José Luís

ASIGNATURA: ASIGNATURA: ASIGNATURA: ASIGNATURA: Control de Robots INGENIERÍA INDUSTRIALINGENIERÍA INDUSTRIALINGENIERÍA INDUSTRIALINGENIERÍA INDUSTRIAL CURSO: CURSO: CURSO: CURSO: 4º

QUIROBOTQUIROBOTQUIROBOTQUIROBOT

CAPÍTULO 1: ROBÓTICA QUIRÚRGICA. 1. CIRUGÍA ROBÓTICA VS TELECIRUGÍA 1 – 2

CIRUGÍA ROBÓTICA ROBOTS EN TELECIRUGÍA

2. LA ROBÓTICA MÉDICA HOY EN DÍA 1 – 3 3. VENTAJAS DE LA CIRUGÍA ROBÓTICA 1 – 6 4. LA SIGUIENTE FRONTERA 1 – 7 5. PLANTEAMIENTO DEL PROBLEMA 1 – 7 6. SOLUCIÓN AL PROBLEMA 1 – 8 CAPÍTULO 2: CINEMÁTICA DIRECTA DEL QUIROBOT. 1. EL FORMALISMO DE DENAVIT-HARTENBERG 2 – 1

2. RESOLUCIÓN DEL PROBLEMA DE LA CINEMÁTICA DIRECTA 2 – 7

3. IMPLEMENTACIÓN DEL CÓDIGO DE LA CINEMÁTICA DIRECTA 2 – 21

4. EJEMPLOS DE UTILIZACIÓN DE LA FUNCIÓN IMPLEMENTADA 2 – 23 5. LA CINEMÁTICA DIRECTA COMO VENTAJA: EL JACOBIANA 2 – 24 CAPÍTULO 3: CINEMÁTICA INVERSA DEL QUIROBOT. 1. DESCRIPCIÓN DEL MÉTODO DIRECTO 3 – 4

2. RESOLUCIÓN DEL PROBLEMA DE LA CINEMÁTICA INVERSA 3 – 5

3. IMPLEMENTACIÓN DEL CÓDIGO DE LA CINEMÁTICA INVERSA 3 – 10

4. EJEMPLOS DE UTILIZACIÓN DE LA FUNCIÓN IMPLEMENTADA 3 – 12

5. LA CINEMÁTICA DIRECTA COMO VENTAJA: LA JACOBIANA INVERSA 3 – 13 CAPÍTULO 4: REPRESENTACIÓN GRÁFICA DEL QUIROBOT. 1. REPRESENTACIÓN GRÁFICA EN MatLab® USANDO ALAMBRES 4 – 1

1. Explicaciones sobre el funcionamiento de la función de representación 3D 4 – 1 2. Implementación del código de la función de representación 3D: DIBUJAROBOT 4 – 2 3. Ejemplos de utilización de la función implementada 4 – 3

2. ANIMACIÓN GRÁFICA EN MATLAB® USANDO ALAMBRES 4 – 4

1. Explicaciones sobre el funcionamiento de la función de representación 3D 4 – 4 2. Implementación del código de la función de representación 3D: ANIMA 4 – 4 3. Implementación del código de la función de representación 3D: PLANIFICA 4 – 4 4. Ejemplos de utilización de la función implementada 4 – 6

CAPÍTULO 5: DINÁMICA DE ROBOTS. 1. DINÁMICA INVERSA 5 – 2 2. DINÁMICA DIRECTA 5 – 9 CAPÍTULO 6: SELECCIÓN DE SERVOACCIONAMIENTOS. 1. REQUISITOS QUE DEBEN SATISFACER NUESTROS MOTORES 6 –2 2. BÚSQUEDA DE MOTORES APROPIADOS EN CATÁLOGO 6 – 5 CAPÍTULO 7: CINEMÁTICA INVERSA DEL QUIROBOT. 1. DISEÑO DE LOS PID 7 – 1 2. SIMULACIÓN GLOBAL 7 – 6

ROBÓTICA QUIRÚRGICA

Jaime Martínez Verdú José Luís Aznar Gomis

1 - 1

CCCaaapppííítttuuulllooo 111 RRROOOBBBÓÓÓTTTIIICCCAAA QQQUUUIIIRRRÚÚÚRRRGGGIIICCCAAA

"AL PRINCIPIO CREÓ DIOS EL CIELO Y LA TIERRA…" El libro del Génesis

La cirugía robótica parece una idea tomada de una película de ciencia ficción y,

seguramente, la visión que tenemos de ella ha sido influida por imágenes como las de la película Star wars, en la que Luke Skywaker es atendido médicamente por unos robots que incluso le implantan un brazo robótico. Las posibilidades de aplicación de robots en la cirugía han motivado la investigación en este campo, y hoy en día ya son una realidad. La palabra robot proviene del checo; según el diccionario de la lengua española de la Real Academia quiere decir "trabajo o prestación personal" y la define como: "una máquina o ingenio electrónico programable, capaz de manipular objetos y realizar operaciones antes reservadas sólo a las personas".

El concepto de robot lo empleó por primera vez, en 1921, el escritor Karel Kapek, en su obra titulada R.U.R. (Robots Universales Rossum). El término se deriva del checoslovaco robota, que significa una labor tediosa o servil. En su libro, Kapek plantea que se crearon robots para servir a la sociedad, pero eventualmente hubo una rebelión que culminó en matanzas y en la esclavitud de los humanos. La idea de los robots "malignos" que pueden dañar al hombre se popularizó posteriormente en un gran número de novelas. Por esta razón, Isaac Asimov planteó en Yo, robot tres reglas inviolables para asegurar que los robots permanezcan siempre bajo el control de sus creadores.

Tabla 1.1: Diferentes robots empleados en medicina, se incluye su tipo y aplicación.



1 - 2

1. CIRUGÍA ROBÓTICA VS TELECIRUGÍA.

CIRUGÍA ROBÓTICA.

La cirugía robótica es un paso más avanzado de lo anterior, ya que se trata del proceso mediante el cual es el robot el que efectúa un procedimiento quirúrgico bajo el control de un programa de computación.

En este caso, el cirujano participa generalmente en la planificación del

procedimiento, pero es un observador en la implementación del plan ya que la ejecución del mismo es realizada exclusiva-mente por el robot.

¿Qué es lo que se obtiene con esta práctica? Se logra - entre otras cosas - que no

existan desviaciones de la trayectoria planificada, alta seguridad con velocidades de ejecución y maniobras totalmente predecibles.

ROBOTS EN TELECIRUGÍA. Mientras que en la cirugía robótica es el robot el que - una vez programado -

realiza por sí mismo la operación, en la telecirugía existen robots que efectúan íntegramente los procedimientos pero bajo la guía del cirujano.

Como sabemos, telepresencia implica un cirujano operando desde una localidad

remota, ya sea en la habitación de al lado o en las antípodas del mundo. Ello se logra manipulando brazos robóticos mediante una complicada interface que combina retroalimentación visual, auditiva y táctil. Esta interface es fundamental, ya que el cirujano sólo cuenta con los datos brindados por los sensores robóticos que actúan sobre el paciente.

Los movimientos de las manos del cirujano son transmitidos a los brazos

robóticos que los reproducen fielmente. Para los ojos del cirujano la manija que mueve en la consola y el instrumento que reproduce ese movimiento en el paciente constituye una única entidad. Esto junto al 'haptic' (dispositivo de 'force-feedback') que le da a sus manos la sensación de tacto y resistencia sobre los tejidos que manipulan los brazos robóticos, incrementan notoriamente la sensación de inmersión.

Aunque la cirugía robótica y la telecirugía tienen muchos puntos en común, los

métodos usados para el control del robot y de la interface hombre-computadora varían significativamente. Debido a la complejidad de esta interface, la telecirugía se usa principalmente para las cirugías mínima-mente invasivas, en donde se actúa con instrumentos que ingresan por pequeñas incisiones, sin usar las manos dentro de la cavidad que se está operando. Más aún, recientemente se han realiza-do experimentos usando mini-robots que inyectados en los vasos femorales son guiados hasta los vasos cerebrales de hasta 1.5 mm de diámetro.

Si bien los robots totalmente autónomos están todavía en su etapa de desarrollo

experimental, existen ya en el mercado algunos equipamientos robotizados de uso en cirugía (AESOP®, Robo-doc®, Regulus®, etc.)



1 - 3

2. LA ROBÓTICA MÉDICA HOY EN DÍA. Actualmente, los robots han sido integrados en diferentes campos, entre los que

se encuentran la manufactura de automóviles, el manejo de materiales peligrosos para el hombre, e incluso nos sustituyen en viajes al espacio que implicarían un gran riesgo y serían demasiado prolongados para un ser vivo. Específicamente en medicina se han empleado diversas tecnologías robóticas que han facilitado el tratamiento de varios padecimientos.

Tal es el caso, por ejemplo, de la cirugía del ojo asistida por computadora, en la

que se proporciona la información acerca de la geometría y características del globo ocular a un sistema computarizado, el cual guía los cortes a realizar para corregir las deficiencias visuales. Sin embargo, robots que tengan una inteligencia artificial semejante a la humana todavía no existen; es factible que en un futuro no tan lejano se diseñen robots con algo comparable a una conciencia y mente propias, que junto con una libertad de movimiento superior a la del hombre (gracias a los materiales con los que estén construidos), les van a permitir realizar actividades imposibles para nosotros o con una mejor eficiencia que la de los humanos. En la medicina esto suena atractivo a pesar de que hasta la fecha ninguna máquina cumple con lo anterior. En este ámbito, el cirujano robot correspondería a un manipulador controlado por computadora, capaz de percibir las partes del cuerpo humano y de mover los instrumentos quirúrgicos para efectuar una cirugía. En la actualidad se clasifica a los robots como pasivos, cuando permiten ubicar y mantener en posición algunos instrumentos para facilitar al cirujano el procedimiento quirúrgico, y activos, cuando el robot mueve los instrumentos y realiza la cirugía.

Figura 1.1: Componentes del robot maestro-esclavo tipo Da Vinci utilizado hoy en día

en muchos hospitales del mundo. Dentro de estos últimos existe lo que se conoce como los sistemas maestro-

esclavo, en los que el robot manipula los instrumentos, pero es el cirujano el que le indica al robot cómo hacerlo. De acuerdo con esta clasificación se han construido varios robots pasivos que permiten la realización de cirugías relativamente simples, como las biopsias estereotáxicas, en las que el neurocirujano alimenta las características del tumor a operar en un sistema computacional que controla un robot encargado de introducir la aguja para la toma de la muestra de tejido sospechoso.



1 - 4

Entre los robots activos destaca uno creado por IBM, denominado Robodoc. Se trata de un sistema experimental que permite implantar una prótesis de cadera con mayor superficie para su fijación, en un perro. El primer robot del tipo activo utilizado en humanos es el Probot, creado por el Imperial College en Londres y que ayuda a realizar una resección de tejido benigno de la próstata; este robot incorpora en su punta un sistema de ultrasonido que le permite crear una imagen tridimensional de la próstata, así el cirujano selecciona qué partes del tejido debe cortar el Probot (Tabla 1.1).

Se busca que los robots mejoren los resultados de la cirugía tradicional

volviendo los procedimientos menos agresivos; esto explica por qué la mayoría de los avances en cirugía robótica se han dado en el campo emergente de la cirugía mínimamente invasiva, conocida como cirugía laparoscópica. Ésta consiste en la introducción en el cuerpo de una cámara e instrumentos mediante los cuales se realiza la cirugía; para ello se han implementado diferentes robots, y uno de los primeros fue el robot activado por voz conocido como AESOP (siglas en inglés de Sistema Óptimo de Posicionamiento Endoscópico Automatizado), que actualmente se utiliza en forma rutinaria en centros especializados en cirugía laparoscópica Este robot consiste en un brazo mecánico conectado a una computadora que reconoce órdenes verbales sencillas y que el robot traduce en movimientos de la cámara laparoscópica. El AESOP libera un brazo del cirujano y así se disminuye el número de personas que se requieren para la cirugía, con la ventaja de que la imagen de la cirugía no va a moverse ni a temblar como lo haría un cirujano que sostiene una cámara durante un periodo largo de tiempo. El costo promedio de este robot es de 90,000 dólares.

Figura 1.2: Se observa cómo el robot maestro-esclavo tipo Da Vinci traduce los

movimientos de la mano del cirujano en movimientos del instrumental. Robots de una nueva generación son los sistemas maestro-esclavo, que incluyen

a los robots Da Vinci y Zeus. Estos sistemas permiten lo que conocemos como cirugía asistida por robot, en la cual el cirujano utiliza brazos mecánicos que repiten los movimientos que realiza en una consola. En la consola computarizada se tiene un visor que transmite la imagen que es captada por la cámara laparoscópica ubicada en uno de los brazos mecánicos. El sistema consta de un conjunto de manivelas que se adaptan al dedo pulgar e índice del cirujano, con los cuales controla el movimiento de los brazos mecánicos. Los brazos mecánicos son tres, uno para sostener la cámara laparoscópica, y otros dos que manipulan los instrumentos quirúrgicos (tijeras, pinzas, electrocauterios, porta-agujas, etcétera). Una característica importante de estos instrumentos es su libertad de movimiento en seis y siete diferentes ángulos, que intenta emular los arcos de movimiento efectuados por la articulación de la muñeca humana. Esto es un gran avance si consideramos que toda la cirugía laparoscópica tiene como limitante que los movimientos se realizan sin poder flexionar los instrumentos, siendo el cirujano el que se adapta a estas restricciones durante la cirugía.



1 - 5

Entre las ventajas que ofrece la consola se encuentra que el cirujano puede realizar la cirugía sin estar en contacto con el paciente, y no debe vestirse con ropa estéril. La imagen que se observa en el visor es tridimensional, gracias a un sistema de dos cámaras laparoscópicas en el paciente, esto le permite al cirujano tener una percepción de profundidad que podría en alguna forma sustituir la deficiencia de tacto que se tiene en este tipo de cirugía. Por otra parte, la manipulación de las manivelas para controlar los movimientos de los instrumentos por los brazos mecánicos se realiza en tiempo real. Esto tiene una importancia fundamental si consideramos que la cirugía implica movimientos rápidos y delicados para evitar un daño en el paciente. En la tecnología que se utiliza para los instrumentos se incluye la articulación tipo muñeca (Endowrist), que permite que se flexionen sobre su eje, dando una libertad de movimiento para el instrumental quirúrgico de más de tres ejes. Además, el sistema computacional tiene la capacidad de escalar los movimientos desde 2:1 hasta 5:1, así como filtrar el temblor del cirujano, haciendo posible la realización de cirugía con desplazamientos mínimos del cirujano y sin las restricciones debidas a su pulso. Aunado a esto existe la posibilidad de coordinar los movimientos de la cámara e instrumental con los movimientos del paciente; esto es especialmente útil cuando se trata de cirugía cardiaca, en la que no se requiere que el corazón del paciente se detenga. Se pueden aplicar suturas en el corazón mientras late, puesto que el cirujano gracias a los filtros de la computadora ve una imagen estática del corazón, así mismo esto permite colocar suturas para la realización de by-pass coronario (puentes arteriales en casos de infartos) y otras cirugías de corazón. La gran mayoría de cirugías asistidas por robot se realizan en procedimientos laparoscópicos como ya se mencionó, en esta cirugía se introducen en el paciente los denominados puertos, unos instrumentos que permiten inflar con gas la cavidad que se va a operar, para poder crear un espacio en el cual disponer los instrumentos y la cámara para efectuar la cirugía.

Figura 1.3: Brazos mecánicos. El central sostiene y mueve la cámara de visión interna,

y los dos laterales permiten la introducción y movimiento del instrumental.



1 - 6

La cirugía laparoscópica se inició cuando se encontró que insuflando aire en el abdomen de un animal experimental era posible insertar una cámara de cistoscopia (cámara utilizada para revisar la vejiga), que permitió observar los órganos abdominales del animal. La laparoscopia, en la práctica, comenzó a utilizarse en ginecología alrededor de 1940 para diagnosticar alteraciones en el útero; para 1986 empezó a tener un gran auge en la cirugía gastrointestinal y a finales de los años 1990, en urología. Actualmente en casi todas las especialidades quirúrgicas se utiliza la cirugía laparoscópica. Esta técnica quirúrgica reduce el daño a los tejidos, provoca menos sangrado y dolor postoperatorio, y facilita una más rápida recuperación de los pacientes. Por ello, este tipo de cirugía se está ya realizando en diferentes partes del mundo en forma rutinaria.

Es importante mencionar que otro campo de aplicación de los robots es el

entrenamiento de cirujanos. La cirugía laparoscópica tiene una curva de aprendizaje muy lenta, lo que obliga a un entrenamiento especializado y de larga duración. Se ha planteado que los robots asociados a simuladores podrían contribuir significativamente en la preparación de cirujanos; también con los sistemas maestro-esclavo se podría facilitar el uso del instrumental reduciendo el tiempo de entrenamiento para el cirujano.

3. VENTAJAS DE LA CIRUGÍA ROBÓTICA.

Las ventajas que aporta la robótica a las operaciones son, por ejemplo;

• Permite una mayor precisión en los movimientos. El robot ejecuta las acciones que le son ordenadas por el médico, editándola por medio de un sistema de cómputo, es decir eliminando errores como el temblor que la mano humana tiene por naturaleza.

• Posee un sistema de movimientos a escala de 1 a 1, de 1 a .3 y de 1 a .5, que les

permite a los cirujanos hacer cirugía de alta precisión. • Las imágenes por medio de los visores telescópicos logran aumentar hasta 20 veces

el tamaño normal, lo que permite al cirujano ver los órganos con más detalle. • Disminuye el sufrimiento de los pacientes, pues las incisiones que se realizan son

entre 5 y 10 milímetros de diámetro, lo que representa suficiente espacio para permitir la entrada de los instrumentos del robot.

• Reduce el tiempo de estancia hospitalaria de los pacientes, quienes pueden

reincorporarse a sus actividades normales en un lapso no mayor a siete días. • Otorga mayor libertad de movimiento al cirujano que en una cirugía Laparoscópica

tradicional. • Permite realizar operaciones a distancia, lo cual evita desplazarse tanto al paciente

como al médico que la efectúa.



1 - 7

4. LA SIGUIENTE FRONTERA. En el año 2000, la FDA (Food and Drug Administration), organización

encargada de regular la práctica médica y el uso de medicamentos en los Estados Unidos, aprobó el sistema quirúrgico Da Vinci para su uso en quirófanos; esto lo hace el primer sistema robotizado para cirugía en humanos. Lamentablemente, el costo del robot es de cerca de un millón de dólares, sin incluir el material desechable empleado para cada cirugía (cada pinza, tijera o cauterio cuesta alrededor de 2,000 dólares y solamente se puede utilizar en 10 cirugías).

Sin embargo, a pesar de su alto costo, las ventajas de la cirugía robótica parecen

prometedoras ya que permitirá, por ejemplo, que un mismo cirujano controle varios robots en diferentes quirófanos, o incluso efectuar telecirugías, en las que el cirujano no se encuentre ni siquiera cerca de la sala de cirugía. Podemos imaginar a un especialista realizando una intervención a distancia, incluso en el espacio, donde los astronautas colocarán al paciente bajo los brazos robotizados, y el cirujano en la Tierra llevará a cabo la cirugía.

5. PLANTEAMIENTO DEL PROBLEMA.

José María Sabater nos propuso una de las necesidades principales que se exigía

a nuestro robot y que era utilizarlo para la realización de incisiones en la tibia o el fémur de modo que, mediante un fijador externo, se posibilite la unión de ambas partes de hueso fracturado y se permita que, tras un período de tiempo, ambas partes de hueso roto queden soldadas. También debía ser capaz de introducir las varillas de metal por el interior de las incisiones (ver Figura 1.4)

Figura 1.4: Artroscopia de rodilla y estructura metálica fijadora.

También tenemos como objetivo la utilización del robot para la realización de

endoscopias que permitirán a un doctor inspecciones visuales en determinadas partes del cuerpo del paciente.

La endoscopia es una técnica diagnóstica utilizada sobre todo en medicina que

consiste en la introducción de un endoscopio a través de una incisión quirúrgica para la visualización de un órgano hueco o cavidad corporal. La endoscopia además de ser un procedimiento de diagnóstico mínimamente invasivo, también puede realizar maniobras terapéuticas como una colecistectomía laparocópica, artroscopia o la toma de biopsias. En nuestro caso, orientaremos al robot a la realización de artroscopias que es la visualización de una cavidad articular, generalmente de las rodillas.



1 - 8

6. SOLUCIÓN AL PROBLEMA. Utilizaremos para realizar todas estas tareas un robot de 5 GDL tal y como

podemos observar en la Figura 1.5, donde en el modelo alámbrico del robot es posible diferenciar dos grupos claros de articulaciones y una última adicional:

3 articulaciones de carácter PRISMÁTICO. • Una ventaja de estas articulaciones es que nos permiten una colocación del

robot ya sea atornillado al techo como incluido dentro de un bastidor de preparado para la higiene necesaria de un quirófano. Si lo sujetamos del techo tenemos la gran ventaja de que podríamos tenerlo arriba del todo cuando no se usase dando más espacio a la sala de operaciones. Además, su espacio de trabajo será tan grande como sea de grande la sala de operaciones.

• Estas articulaciones son las que contribuyen con la mayor parte de todo el

espacio de trabajo del cual dispone el robot. Es decir, casi la totalidad del espacio del trabajo que presenta el robot es básicamente debido a estas tres articulaciones.

• Estas articulaciones serán las que esencialmente aporten la posición del

efector final, es decir, será las que más "peso" tengan en el posicionamiento del efector final.

2 articulaciones de carácter ROTATIVO. • Este grupo de articulaciones, a diferencia de las anteriores, se caracteriza por

ser el grupo de articulaciones que proporcionará, en gran medida, la orientación a nuestro efector final.

• No es una muñeca esférica pues son únicamente dos articulaciones rotacionales.

Con cinco grados de libertad tendríamos suficiente siempre que "atacáramos" a

la rodilla desde arriba con un abanico de 180º máximo. Nos haría falta un grado de libertad más si quisiéramos realizar incisiones a la altura de1 gemelo pero como no es así no utilizamos ese grado de libertad. En lugar de eso, utilizamos una herramienta que sea capaz de realizar un movimiento de carácter PRISMÁTICO ADICIONAL.

El último eslabón consta de dos partes que en el dibujo están definidas. Una

parte es el eslabón cuyo final es donde se sujetará la Black&Decker y el resto hace referencia a la propia Black&Decker que suponemos es capaz de realizar un movimiento lineal. Tendríamos un robot de 5 GDL + "1" que es ajeno al robot en cuanto a cinemática.

La herramienta actuará como una articulación tipo prismática que será la que

realice el movimiento lineal de incisión. Esto es así para superar el problema que conlleva realizar un movimiento lineal en el espacio con una muñeca esférica pues ésta provocaba errores de imprecisión tal y como vimos en el tema de planificación de trayectorias.



1 - 9

De igual modo, es un 5 GDL + "1" si le añadimos el taladro y un 5 GDL + "2" si le añadimos la cámara.

Figura 1.5: Esquema general del Quirobot.

Tres articulaciones prismáticas

Dos articulaciones de rotación

BLACK&DECKER "Articulación" extra prismática

d1(t)

d2(t)

d3(t)

θ3(t)

θ4(t)

CINEMÁTICA DIRECTA DEL QUIROBOT


2 - 1

CCCaaapppííítttuuulllooo 222 CCCIIINNNEEEMMMÁÁÁTTTIIICCCAAA DDDIIIRRREEECCCTTTAAA DDDEEELLL QQQUUUIIIRRROOOBBBOOOTTT

"TODO LO QUE SE MUEVE ES MOVIDO POR OTRO" Aristóteles

Después de todo lo visto, estamos ya en condiciones de abordar el problema de

la cinemática directa, el cual trata de encontrar la forma explícita de la función que relaciona el espacio articular con el espacio cartesiano de posiciones/orientaciones. Esta función:

6: ℜ→Jf

toma como argumento un vector en el espacio de articulaciones, que tiene tantas componentes como grados de libertad tenga la cadena cinemática que se considere (que en nuestro caso es únicamente de 5 grados de libertad), y devuelve un vector de 6 componentes; las tres primeras serán la posición en el espacio del punto terminal de la cadena, expresada en un sistema de referencia externo, y las tres últimas, la orientación, expresada bien como las componentes (ax, ay, az) del vector de aproximación, bien como ángulos de orientación (Euler o roll, pitch and yaw).

1. EL FORMALISMO DE DENAVIT-HARTENBERG.

La forma en que conseguiremos conocer el vector de 6ℜ antes mencionado será

mediante la construcción de la matriz de transformación homogénea T que relaciona el sistema solidario al punto terminal con un sistema de referencia fijo arbitrariamente escogido, que llamaremos sistema del mundo. En principio, cada una de las componentes (nx, ny, nz, ox, oy, oz, ax, ay, az, px, py, pz) de la matriz T será función de algunas o todas las variables articulares, y asimismo de las constantes geométricas del manipulador.

Según se vio en clases de teoría al detallar la forma explícita de la matriz T,

multiplicándola por el vector (0; 0; 0; 1), que expresa las coordenadas homogéneas del punto terminal respecto a su propio sistema, obtendremos éstas respecto al sistema del mundo. Y, por otra parte, según se vio en la en el capítulo 3 del libro Fundamentos de Robótica de Antonio Barrientos Herramientas matemáticas para la localización

espacial, existen fórmulas que relacionan los ángulos de orientación en cualquiera de sus expresiones con los elementos de la submatriz de rotación de T.



2 - 2

Seguidamente se describe con detalle el proceso de construcción de T para cadenas cinemáticas abiertas, en las que cada articulación tenga un sólo grado de libertad. El caso de articulaciones con más de un grado de libertad no presenta ningún problema: bastaría con considerar la articulación como si se tratase de dos, unidas por un enlace ficticio de longitud nula.

Sustancialmente, el proceso consiste en fijar un sistema de coordenadas a cada

enlace, que se moverá con él, de acuerdo a un conjunto de normas fijas. A continuación, se identifican ciertos parámetros geométricos que lo relacionan con el sistema fijo al siguiente enlace, y se utilizan para escribir la matriz de transformación homogénea entre cada par de sistemas i–1Ai. En último lugar, el producto de todas las matrices de transformación generará la matriz T.

El conjunto de normas que establece cómo deben fijarse los sistemas de

coordenadas se conoce como convenio de Denavit-Hartenberg, descrito por éstos en 1955, y a los parámetros geométricos que relacionan los sistemas, parámetros de Denavit-Hartenberg (desde ahora, DH).

Comenzaremos por establecer convenciones para la nomenclatura: Según se vio en el tema 1, en cadenas cinemáticas abiertas cada par enlace-

articulación era un grado de libertad. Numeraremos los enlaces y articulaciones secuencialmente, desde el inicio de la cadena. La base de ésta, fija normalmente al suelo, será el enlace 0, y no se cuenta como grado de libertad. La articulación 1 será la que conecte la base al primer enlace móvil; las articulaciones comienzan, pues, a numerarse desde 1, y no existe articulación al final del último enlace.

El eje de una articulación es la recta definida como: • La dirección de desplazamiento, en articulaciones traslacionales.

• El eje de giro, en articulaciones rotacionales. A continuación, los parámetros DH de cada enlace son cuatro números reales,

dos de ellos representando ángulos, y los otros dos, distancias, definidos del siguiente modo:

• ai es la mínima distancia (distancia perpendicular) entre el eje de la

articulación i y el eje de la i + 1. Por extensión, también denotaremos por ai

al segmento de recta a lo largo de cual se da precisamente esa mínima distancia entre ejes.

• αi es el ángulo que forman el eje i y el i + 1, medido en un plano perpendicular al segmento ai.

• di es la distancia entre los puntos de intersección de la normal a los ejes i / i + 1 con el eje i, y la normal a los ejes i – 1 / i también con el eje i, medida a lo largo de este eje.

• θi es el ángulo entre la normal a los ejes i – 1 / i y la normal a los ejes i / i + 1, medido en un plano perpendicular al eje i.



2 - 3

Véase la representación de estos parámetros en la figura 1.1. Obsérvese que basta el conocimiento de los dos ejes en los extremos de un enlace (digamos, el i) para conocer αi y ai; sin embargo, es necesario conocer los ejes anterior y siguiente para determinar θi y di. Daremos para estos parámetros una definición alternativa cuando hayamos fijado los respectivos sistemas de coordenadas.

Figura 2.1: Parámetros DH de un enlace genérico.

Ahora, las normas para determinar los ejes de cada sistema ortonormal asociado

a un eslabón son las que siguen: • El eje zi-1 es el eje de la articulación i (con lo que zi es el de la i + 1). No

importa el sentido a lo largo de la recta en que se oriente. Se recomienda orientar todos los ejes z que sigan la misma dirección en el mismo sentido.

• El eje xi debe escogerse perpendicular a su propio z (zi) y también al z anterior (zi-1) y a lo largo de la perpendicular común (el segmento ai).

• El eje yi se determina de tal modo que el sistema forme un triedro dextrógiro (es decir, que xi × yi = zi).

• El origen se fija en la intersección de la normal eje i – 1 / eje i (es decir, el segmento ai-1) con el eje i.

Figura 2.2: Sistemas de coordenadas fijos a una articulación.



2 - 4

Véanse en la figura 2.2 los sistemas de coordenadas i – 1 e i situados sobre los enlaces del ejemplo anterior. Hay algunas excepciones y casos particulares que deben tenerse en cuenta:

• El eje x0 debe fijarse arbitrariamente como cualquier perpendicular a z0, dado

que no existe ningún z1 al cual hacerle ser también perpendicular. Análogamente, el origen del sistema 0 es un punto arbitrario sobre el eje z0.

• Debe existir un último sistema de coordenadas, fijo a la mano o herramienta, que no sigue las mismas reglas que los anteriores. Debe situarse de modo que su eje z esté en la dirección de aproximación (el avance natural de la mano) y su eje y (vector de orientación) debe apuntar de garra a garra de la pinza (en caso de pinzas con simetría cilíndrica, es arbitrario). Este sistema "especial" sólo debe añadirse si el último sistema obtenido por las reglas usuales no cumpliese estas condiciones. En ese caso, la transformación entre el último sistema natural y este sistema especial es fija, y se halla por observación directa.

• Cuando dos ejes z consecutivos son paralelos, hay infinitas perpendiculares comunes. En ese caso, lo normal es tomar el origen a la altura del centro de la articulación, y el eje x a lo largo de la normal común que pasa por ese centro.

• Cuando dos ejes z consecutivos se intersectan, determinan un plano. La normal común es la normal al plano, pero el segmento ai tiene longitud nula. En ese caso el eje x se escoge normal al plano que determinan los dos z, en cualquiera de los dos sentidos. El origen se toma en el punto de intersección de los dos ejes z.

• Cuando dos ejes z consecutivos son colineales (están superpuestos), el origen se fija arbitrariamente, así como la dirección de x, a lo largo de cualquier perpendicular a los z (que son la misma recta). Se aconseja en ese caso tomar la dirección de x lo más parecida posible a la dirección del x anterior. ai, que no está definido, se toma como 0.

Nótese que: • Para el caso de una articulación rotacional, θi es el ángulo de rotación que se trata de

una función temporal, y di, ai y αi son constantes.

• En una articulación traslacional, di es distancia de traslación función del tiempo, y θi, y αi son constantes. ai es también constante, y usualmente 0 (suele corresponder al último de los casos particulares anteriores).

Todo parámetro constante en un robot lo es por construcción del mismo, y

permanece constante en toda circunstancia, salvo que se altere mecánicamente al robot. La acción de los actuadores que provocarán el movimiento sólo cambia el parámetro variable de cada articulación, θi para rotacionales, y di para traslacionales.

Por otra parte, lo normal es construir robots en los que los ejes de las

articulaciones sean o bien paralelos, o bien perpendiculares entre sí; esto hace que los valores para θi suelan ser bien 0º, 90º, 180º ó 270º.



2 - 5

El siguiente paso es determinar la transformación que llevaría el sistema i – 1 hasta el i. Nótese que podemos hacerlos coincidir aplicando sucesivamente las siguientes transformaciones:

• Rotar alrededor de zi-1 un ángulo θi. Esto deja los ejes xi-1 y xi paralelos, pues deja a

las rectas ai-1 y ai en el mismo plano, y siendo ambas perpendiculares a zi-1.

• Trasladar a lo largo del eje z'i-1 (aunque es el mismo que zi) el origen una distancia di. Esto deja los ejes xi-1 y xi colineales. Nótese que no por ello los ejes zi-1 y zi son coincidentes.

• Trasladar a lo largo del eje x''i una distancia ai. Esto hace coincidir los orígenes, y superpone los vectores básicos en la dirección i

r

(x''i-1 y x''i).

• Rotar alrededor de x''i-1 (o de xi, ahora coinciden) un ángulo αi. Esto hará coincidir z''i-1 con zi (y, por tanto, y''i-1 con yi) y está concluido.

Así pues, podemos escribir la transformación desde el sistema i – 1 hasta el i

como

( ) ( ) ( ) ( )iiiiiiii

xROTaTrdTrzROTA αθ ,''0,0,,0,0, 111

−−− ⋅⋅⋅=

donde las matrices se han postmutiplicado, pues las transformaciones se efectúan

siempre respecto a los nuevos ejes que van resultando de la transformación anterior. En forma explícita,

−⋅

⋅

⋅

−

=−

1000

00

00

0001

1000

0100

0010

001

1000

100

0010

0001

1000

0100

00

00

1

ii

ii

i

i

ii

ii

ii

CS

SC

a

d

CS

SC

αα

ααθθ

θθ

A

Una vez operado el producto de dichas matrices obtenemos que la matriz de

transformación homogénea generalizada para parámetros θi, di, ai y αi es la siguiente:

⋅⋅−⋅

⋅⋅⋅−

=−

1000

)cos()sen(0

)sen()cos()sen()cos()cos()sen(

)cos()sen()sen()sen()cos()cos(

1

iii

iiiiiii

iiiiiii

ii

d

a

a

αα

θθαθαθ

θθαθαθ

A

El resultado final es la matriz DH para el enlace i, la cual, conociendo las

características geométricas de dicha articulación y de su enlace, da la transformación que lleva de coordenadas expresadas en el sistema i a coordenadas expresadas en el sistema i – 1. Igualmente, da la posición del origen del sistema i respecto al i – 1 (viendo la 4ª columna), así como su orientación (viendo la submatriz de rotación).



2 - 6

Ahora estamos en condiciones de dar las definiciones alternativas para los parámetros de DH, que son:

• ai es la distancia perpendicular (distancia mínima) entre los ejes zi-1 y zi, medida en

la dirección positiva de xi. • αi es el ángulo que forma el eje zi-1 con el eje zi, girando alrededor de xi, con xi

apuntando hacia el observador. • θi es el ángulo que forma el eje xi-1 con el eje xi, girando alrededor de zi-1, con zi-1

apuntando hacia el observador. • di es la distancia que queda entre los orígenes de los sistemas i – 1 y i después de

haber trasladado el origen i a lo largo de la perpendicular común ai hasta situarlo sobre el eje zi-1. Su signo viene dado por el eje zi-1.

Una vez todos los parámetros estén identificados, y las matrices DH escritas,

recordemos que lo que se pretende es encontrar la transformación entre el sistema del mundo (sistema 0) y el último (sistema n). Es obvio que

nn

n AAAA1

21

100 −⋅= L

Cada elemento de la matriz i–1Ai es función de ai; αi; θi y di, (ai y αi constantes

para cada robot, y di o bien θi variables para cada tipo de articulación), y por tanto cada elemento de 0An es, en principio, función de todos los (ai; αi; θi y di) con i = 1, 2, 3,…, n.

Resumiendo. Los pasos que deben seguirse para la construcción de la cinemática directa son: 1. Identificar cuántos grados de libertad tiene el robot, y cuántas articulaciones; si tuviera alguna articulación con más de un grado de libertad (digamos, n) habrá n sistemas superpuestos en un punto. 2. Asignar los ejes z, sabiendo que zi-1 es el eje de la articulación i. 3. Asignar todos los ejes x, sabiendo que xi es perpendicular a zi, y a zi-1 y va en la dirección de la perpendicular común. 4. Asignar los ejes y, de modo que se cumpla que xi × yi = zi. 5. Determinar los parámetros (ai; αi; θi; di) de cada articulación por inspección visual. 6. Construir la tabla de parámetros, y a partir de cada una de sus filas, usando la fórmula general de la matriz DH, escribir cada una de las i–1Ai. 7. Multiplicar todas ellas para generar la 0An.



2 - 7

2. RESOLUCIÓN DEL PROBLEMA DE LA CINEMÁTICA DIRECTA. PARÁMETROS DENAVIT-HARTENBERG.

1. Identificar cuántos grados de libertad tiene el robot, y cuántas

articulaciones; si tuviera alguna articulación con más de un grado de libertad (digamos, n) habrá n sistemas superpuestos en un punto.

Este manipulador, diseñado por José Luís Aznar y Jaime Martínez con

propósitos principalmente médicos, tiene 5 grados de libertad, tres de ellos traslacionales y dos rotacionales. Para analizarlo utilizando el procedimiento de Denavit-Hartenberg necesitamos el esquema siguiente donde se representa su modelo alámbrico. Partimos de una configuración cualesquiera, si bien es aconsejable colocarlo en una posición sencilla de analizar. Su estructura puede observarse en la figura 2.3a.

Figura 2.3a: Representación del esquema alámbrico del robot quirúrgico.

Nótese tiene 5 articulaciones que enumeraremos desde cero hasta cinco, siendo

la etiquetada con 0 aquella fija al bastidor (o en su defecto al techo). La articulación 1 es la prismática que se deshazla en dirección horizontal y la articulación 2 en dirección vertical. La articulación 3 es la primera articulación de la muñeca del robot y la articulación 4 es el segunda y última de la muñeca, que gira y lleva fijada la sujeción del efector final.

0

1

2

3

4



2 - 8

2. Asignar los ejes z, sabiendo que zi-1 es el eje de la articulación i. z0 Es horizontal, es el eje del sistema del mundo, de manera que es absolutamente fijo y se le elige de modo que coincida con el sentido positivo de la primera traslación. z1 Es horizontal, es el eje del segundo deslizamiento, se desliza en el mismo formando un plano con el eje del mundo. Se le elige de modo que coincida con el sentido positivo de la segunda traslación. z2 Es vertical, es el eje del tercer deslizamiento, y se desliza normal al plano que forman los dos ejes anteriores. Análogamente, se determina de manera que coincida con el sentido positivo de la tercera traslación. z3 Sigue la dirección de z2, y es el primer eje de revolución de la muñeca. Se elige de manera que el sentido de giro angular sea positivo al hacerlo referido a dicho eje z3. z4 Es el segundo y último eje de rotación de la mano, y siempre se mantiene en un plano horizontal. Hemos seleccionado el eje de manera el giro sea positivo al estar referido a este eje. z5 z5 se sitúa de manera que coincida con la dirección de z4. Aún si no hubiera un enlace físico al que vaya unido, es necesario incluirlo para que de cuenta de la última rotación. Por otra parte, el robot tiene 5 grados de libertad; por ello, debe haber 5 sistemas móviles (los números de la articulación 1 a 1a 5, recordemos que el 0 era fijo).

0z

1z

2z

3z

4z

5z

d1(t)

d2(t)

d3(t)

θ4(t)

θ5(t)

Figura 2.3b: Elección de los ejes zi.



2 - 9

3. Asignar todos los ejes x, sabiendo que xi es perpendicular a zi, y a zi-1 y va en la dirección de la perpendicular común.

x0 Será perpendicular a z0, y por lo tanto vertical, pero al no haber un eje z anterior, su dirección se puede elegir. Se elige hacia arriba, como se ve en la figura 2.3c. x1 Debe ser perpendicular a su propio z (z1) y al z anterior (z0). Estos dos ejes se cortan, y determinan un plano horizontal. Así pues, x1 deberá ser normal a un plano horizontal, y en la posición dibujada, hacia arriba o hacia abajo. Se elige hacia arriba (¿y por qué no?). x2 Es perpendicular a z1 y a z2, que también se cortan, determinando un plano vertical, por lo que x2 deberá ir hacia fuera o hacia dentro. Por ejemplo, se elige hacia fuera (¿y por qué no?). x3 Es un caso especial. Como z2

y z3 son coincidentes, hay infinitas direcciones transversales. Cuando dos ejes z consecutivos están superpuestos, el origen se fija arbitrariamente, así como la dirección de x, a lo largo de cualquier perpendicular a los z2 y z3 (que son la misma recta). Se aconseja en ese caso tomar la dirección de x lo más parecida posible a la dirección del x anterior. x4 Será perpendicular a z3 y a z4, y debido a que ambos intersectan determinando un plano que contendría al eje z2, x4 sería normal en dicho plano, y en este caso va hacia la fuera. x5 Es un caso especial. Como z4 y z5 son paralelos, hay infinitas direcciones perpendiculares. Se aconseja en ese caso tomar la dirección de x lo más parecida posible a la dirección del x anterior. Se escoge una, en este caso, hacia fuera.

0x

0z1x

1z

2x

2z

3x 3z

4x4z

5x5z

Figura 2.3c: Elección de los ejes xi.



2 - 10

4. Asignar los ejes y, de modo que se cumpla que xi × yi = zi.

Figura 2.3d: Elección de los ejes yi.

Respecto a los orígenes, el del sistema S0 se fija arbitrariamente al eslabón

inicial de la cadena yendo hacia fuera y hacia dentro. Los sistema S1 y S2 en un principio vienen determinados sin posibilidad de elección, ya que hay un único punto donde z0 corta a z1, y otro punto único donde z1 corta a z2. Ellos tres dependen fuertemente del fijo pues, de alguna manera, se podría decir que están referidos a él. En realidad, las tres articulaciones prismáticas están superpuestas y en el dibujo no se muestran superpuestas porque hemos dibujado el resultado de un desplazamiento de las variables articulares d1(t), d2(t), y d3(t). El sistema S3 se puede colocar a la altura que prefiramos puesto que z1 y z2 son colineales; también se puede decir que está referido al anteriorS2. El sistema S4, sin embargo, está unívocamente determinado por la intersección de z3 y z4. El origen S5 al final del eslabón con longitud l5 y, de este modo, si la orientación de este sistema es adecuada, podría servir como sistema final.

d1(t)

d2(t)

d3(t)

θ4(t)

θ5(t)

0x

0y

0z1x

1y

1z

2x

2y

2z

3x

3y

3z

4x4y

4z

5x5y

5z



2 - 11

Con respecto a esto, nos preguntamos si necesitamos poner otro sistema extra en la mano. Obsérvese que el último sistema que nos resultó por aplicación de las reglas era el 5, y que éste tiene:

• Su eje y en la dirección de orientación en sentido opuesto.

• Su eje x está en la dirección de aproximación de la mano o herramienta.

• Su eje z en la dirección normal a estos dos.

Figura 2.3e: Cambio de base para tener un triedro n o a en el efector final. De clases de teoría sabemos que debe existir un último sistema de coordenadas,

fijo a la mano o herramienta, que no sigue las mismas reglas que los anteriores. Debe situarse de modo que su eje z esté en la dirección de aproximación (el avance natural de la mano) y su eje y (vector de orientación) debe apuntar de garra a garra de la pinza (en caso de pinzas con simetría cilíndrica como ocurre en nuestra situación, es arbitrario).

Este sistema "especial" sólo debe añadirse si el último sistema obtenido por las

reglas usuales no cumpliese estas condiciones. En ese caso, la transformación entre el último sistema natural y este sistema especial es fija, y se halla por observación directa.

Por tanto, la convención para los vectores n, o y a no se cumple y es necesario

añadir otro sistema. Esta matriz sería la siguiente:

→

−=

n

o

a

Ar

r

r

dedirección lacon coincide eje deldirección La

- dedirección lacon coincide eje deldirección La

dedirección lacon coincide eje deldirección La

1000

0001

0010

0100

5

5

5

55

z

y

x

θ5(t)

4x4y

4z

5x5y

5z

ar

or

nr



2 - 12

5. Determinar los parámetros (ai; αi; θi; di) de cada articulación por inspección visual.

• Parámetros DH para ir desde el sistema S0 hasta el S1: (a1; α1; θ1; d1).

Figura 2.4a: Parámetros DH para pasar del sistema S0 al sistema S1.


Figura 2.4b: Parámetros DH para pasar del sistema S1 al sistema S2.

d1(t)

d2(t)

1x

1y

1z

θ2= 90º

d2= d2(t) a2= 0 α2= –90º

d3(t)

2x

2y

2z

1x

1y

1z

1x 1y

1z

1x 1y

1z

1x 1y

1z

1x

1y

1z

d1(t)

d2(t)

0x

0y

0z

1x

1y

1z

θ1= 0º

0x

0y

0z

d1= d1(t)

0x

0y

0za1= 0

0x

0y

0zα1= 90º

0x

0y

0z

0x

0y

0z



2 - 13


Figura 2.4c: Parámetros DH para pasar del sistema S2 al sistema S3.


Figura 2.4d: Parámetros DH para pasar del sistema S3 al sistema S4.

θ3= 0º

d3= d3(t) a3= 0 α3= 0º

d2(t)

θ4(t)

2x

2y

2z

3x

3y

3z

2x

2y

2z

2x

2y

2z2x

2y

2z 2x

2y

2z

2x

2y

2z

θ4= θ 5(t)

d4= l4 a4= 0 α4= 90º

θ4(t)

θ5(t)

3x

3y

3z

4x4y

4z3x

3z

3y

3x 3z

3y

3x 3z

3y

3x 3z

3y

3x

3z

3y



2 - 14


Figura 2.4e: Parámetros DH para pasar del sistema S4 al sistema S5.

θ5= θ 5(t)

d5= 0 a5= l5 α5= 0º

θ5(t)

4x4y

4z

5x5y

5z4x

4y

4z

4x 4y

4z

4x 4y

4z

4x 4y

4z

4x 4y

4z



2 - 15

6. Construir la tabla de parámetros, y a partir de cada una de sus filas, usando la fórmula general de la matriz DH, escribir cada una de las i–1Ai.

La tabla de los parámetros de Denavit-Hartenberg para cada uno de los

eslabones es la siguiente:

ESLABÓN θi di ai αi 1 0º d1(t) 0 90º 2 90º d2(t) 0 –90º 3 0º d3(t) 0 0º 4 θ4(t) l4 0 90º 5 θ5(t) 0 l5 0º

Las matrices de transformación homogénea las i–1Ai son las siguientes:

−=

1000

010

0100

0001

1

10

dA

−

−

=

1000

010

0001

0100

2

21

dA

=

1000

100

0010

0001

3

32

dA

−=

1000

010

0))(cos(0))(sen(

0))(sen(0))(cos(

4

44

44

43

l

tt

tt

θθ

θθ

A

⋅

⋅−

=

1000

0100

))(sen(0))(cos())(sen(

))(cos(0))(sen())(cos(

5555

5555

54

tltt

tltt

θθθ

θθθ

A

También se calculan por inspección visual directa las matrices para el cambio de

base tanto para el sistema del eslabón fijo como el del efector final:

−=

1000

0001

0010

0100

00 A

−=

1000

0001

0010

0100

55 A

En las páginas siguientes realizaremos las operaciones que nos han permitido

obtener las expresiones de las matrices i–1Ai.

• Para la primera articulación que es de carácter traslacional, su matriz de transformación homogénea 0A1 es:

( ) ( ) ( ) ( )

( )

−=

⋅⋅−⋅

⋅⋅⋅−

=

⋅⋅⋅=

1000

010

0100

0001

1000

)()º90cos()º90sen(0

)º0sen(0)º0cos()º90sen()º0cos()º90cos()º0sen(

)º0cos(0)º0sen()º90sen()º0sen()º90cos()º0cos(

º90,''0,0,0)(,0,0º0,

11

10

01010

tdtd

xROTTrtdTrzROT

A

A

• Para la segunda articulación que es de carácter traslacional, su matriz de transformación homogénea 1A2 es:

( ) ( ) ( ) ( )

( )

−

−

=

−−

⋅⋅−−⋅−

⋅⋅−⋅−−

=

−⋅⋅⋅=

1000

010

0001

0100

1000

)()º90cos()º90sen(0



º90,''0,0,0)(,0,0º90,

22

21

12121

tdtd

xROTTrtdTrzROT

A

A

• Para la tercera articulación que es de carácter traslacional, su matriz de transformación homogénea 2A3 es:

( ) ( ) ( ) ( )

( )

=

⋅⋅−⋅

⋅⋅⋅−

=

⋅⋅⋅=

1000

100

0010

0001

1000

)()º0cos()º0sen(0



º0,''0,0,0)(,0,0º0,

33

32

23232

tdtd

xROTTrtdTrzROT

A

A

• Para la cuarta articulación que es de carácter rotacional, su matriz de transformación homogénea 3A4 es:

( ) ( ) ( ) ( )

−=

⋅⋅−⋅

⋅⋅⋅−

=

⋅⋅⋅=

1000

010

0))(cos(0))(sen(

0))(sen(0))(cos(

1000

)º90cos()º90sen(0

))(sen(0))(cos()º90sen())(cos()º90cos())(sen(

))(cos(0))(sen()º90sen())(sen()º90cos())(cos(

º90,''0,0,0,0,0)(,

4

44

44

4

4444

4444

43

344343

l

tt

tt

l

tttt

tttt

xROTTrlTrtzROT

θθ

θθ

θθθθ

θθθθθ

A

A

• Para la quinta articulación que es de carácter rotacional, su matriz de transformación homogénea 4A5 es:

( ) ( ) ( ) ( )

⋅

⋅−

=

⋅⋅−⋅

⋅⋅⋅−

=

⋅⋅⋅=

1000

0100



1000

0)º0cos()º0sen(0

))(sen())(cos()º0sen())(cos()º0cos())(sen(

))(cos())(sen()º0sen())(sen()º0cos())(cos(

º0,'',0,00,0,0)(,

5555

5555

55555

55555

54

355354

tltt

tltt

tlttt

tlttt

xROTlTrTrtzROT

θθθ

θθθ

θθθθ

θθθθθ

A

A

• La matriz para realizar el cambio de base canónica al sistema S0 0A0 es:

→

−=

0

0

0

00

xdedirección lacon coincide eje deldirección La

y- dedirección lacon coincide eje deldirección La

z dedirección lacon coincide eje deldirección La

1000

0001

0010

0100

e

e

e

z

y

x

A

0x

0y

0zex

ey

ez

Empleado esta matriz de cambio de base podemos evitarnos errores al obtener las coordenadas del efector final. Si no utilizamos la matriz 0A0 y la matriz T nos informa sobre que el efector final está en las coordenadas [x* y* z*]T, esto significaría que z = x*, y = - y

*, x= z*. Si utilizamos esta 0A0 las coordenadas son directas pues un vector de

posicionamiento [x* y* z*]T implicaría que la herramienta estaría en z = x*, y = y*, x= z*.

Figura 2.5: Cambio de base para tener un triedro canónico en el sistema del mundo.



2 - 18

7. Multiplicar todas ellas para generar la 0An. Si hacemos una recopilación de las matrices de transformaciones homogéneas

tenemos que a partir de las operaciones antes realizadas tenemos que las matrices i-1Ai y la matriz T son las siguientes:

−=

1000

0001

0010

0100

00 A

−=

1000

0001

0010

0100

55 A

−=

1000

010

0100

0001

1

10

dA

−

−

=

1000

010

0001

0100

2

21

dA

=

1000

100

0010

0001

3

32

dA

−=

1000

010

0))(cos(0))(sen(

0))(sen(0))(cos(

4

44

44

43

l

tt

tt

θθ

θθ

A

⋅

⋅−

=

1000

0100



5555

5555

54

tltt

tltt

θθθ

θθθ

A

55

54

43

32

21

10

00 AAAAAAAT ⋅⋅⋅⋅⋅⋅=

55

54

43

53

32

21

10

00

30

53

30 ycon AAAAAAAAAAAT ⋅⋅=⋅⋅⋅=⋅=

−−

−=

⋅

−

−

⋅

−⋅

−=

1000

)(100

)(010

)(001

1000

)(100

0010

0001

1000

)(010

0001

0100

1000

)(010

0100

0001

1000

0001

0010

0100

3

2

1

321

30

td

td

td

tdtdtdA

⋅+−

⋅⋅⋅⋅−

⋅⋅⋅⋅

=

=

−⋅

⋅

⋅−

⋅

−=

1000

))(sen())(sen())(cos(0

))(cos())(sen())(cos())(sen())(sen())(sen())(cos(

))(cos())(cos())(cos())(cos())(sen())(cos())(sen(

1000

0001

0010

0100

1000

0100



1000

010

0))(cos(0))(sen(

0))(sen(0))(cos(

55455

54554544

54554544

5555

5555

4

44

44

53

tlltt

ttlttttt

ttlttttt

tltt

tltt

l

tt

tt

θθθ

θθθθθθθ

θθθθθθθ

θθθ

θθθ

θθ

θθ

A

⋅−−−−

⋅⋅−⋅−⋅−

⋅⋅+⋅⋅

=

⋅+

⋅⋅−⋅−⋅

⋅⋅⋅−⋅

⋅

−−

−=

1000

))(sen()())(sen())(cos(0

))(cos())(sen()())(cos())(sen())(sen())(sen())(cos(

))(cos())(cos()())(cos())(cos())(sen())(cos())(sen(

1000


))(cos())(sen())(cos())(sen())(sen())(cos())(sen(

))(cos())(cos())(sen())(sen())(cos())(cos())(cos(

1000

)(100

)(010

)(001

554355

545254544

545154544

55455

54545454

54545454

3

2

1

tlltdtt

ttltdttttt

ttltdttttt

tlltt

ttlttttt

ttlttttt

td

td

td

θθθ

θθθθθθθ

θθθθθθθ

θθθ

θθθθθθθ

θθθθθθθ

T



2 - 20

La matriz de transformación homogénea para nuestro robot presenta la siguiente forma:

⋅−−−−

⋅⋅−⋅−⋅−

⋅⋅+⋅⋅

=

1000

))(sen()())(sen())(cos(0



1000

554355

545254544

545154544

tlltdtt

ttltdttttt

ttltdttttt

paon

paon

paon

zzzz

yyyy

xxxx

θθθ

θθθθθθθ

θθθθθθθ



2 - 21

3. IMPLEMENTACIÓN DEL CÓDIGO DE LA CINEMÁTICA DIRECTA.

La función directa resuelve el problema cinemático directo del robot quirúrgico de 5 grados de libertad. Para ello toma como único parámetro un vector q donde vienen almacenadas como componentes cada una de las variables articulares de la siguiente manera:

[ ]Tqqqqqq 54321=

)(:)(:

)(:)(:)(:

5544

332211

tqtq

tdqtdqtdq

θθ =====

Ha de tenerse en consideración que las longitudes d1(t), d2(t) y d3(t)

especificadas por el usuario demandadas a las articulaciones prismáticas no pueden ser mayores a las longitudes de las guías por donde colocadas las articulaciones. Además, no está permitido introducir ángulos para las variables rotativas que provoquen singularidades y, debido a esto, no se podrán alcanzar algunas configuraciones.

Por ello, es recomendable tener en cuenta, por ejemplo, que no se exceda de 2

metros las variables prismáticas y tampoco debemos dar más de una vuelta con la variable articular θ4(t), es decir, no sobrepasar los 360º y, también, con la variable articular θ5(t) debemos trabajar entre -75º y 255º. Por tanto, el robot quirúrgico funcionará correctamente si introducimos valores de variables articulares tales como

º255)(º75º360)(0

2)(02)(02)(0

44

321

≤≤−≤≤≤≤≤≤≤≤

tt

mtdmtdmtd

θθ

El código fuente de la función DIRECTA para nuestro robot de 5 grados de

libertad es el siguiente:

1 function A05 = directa(q) 2 3 l4=0.4; 4 l5=0.2; 5 6 % Parámetros Denavit-Hartenberg del robot 7 teta = [ 0 pi/2 0 q(4) q(5) ]; 8 d = [ q(1) q(2) q(3) l4 0 ]; 9 a = [ 0 0 0 0 l5 ];

10 alfa = [ pi/2 –pi/2 0 pi/2 0 ]; 11 12 % Matrices de transformación homogénea entre sistemas de consecutivos 13 A00 = [0 0 1 0;0 -1 0 0;1 0 0 0;0 0 0 1]; 14 A01 = denavit(teta(1), d(1), a(1), alfa(1)); 15 A12 = denavit(teta(2), d(2), a(2), alfa(2)); 16 A23 = denavit(teta(3), d(3), a(3), alfa(3)); 17 A34 = denavit(teta(4), d(4), a(4), alfa(4)); 18 19 % Matriz de transformación del primer al último sistema de coordenadas 20 A05 = A00 * A01 * A12 * A23 * A34 * A45 * A00;



2 - 22

En las primeras líneas del código introducimos las dimensiones del robot, donde l5 recordemos que englobaba tanto la longitud del efector final como la longitud de la taladradora. Tal como podemos observar, el vector de coordenadas articulares q representa 5 grados de libertad del robot, 3 prismáticas y 2 rotacionales y dicho vector se introduce en la tabla DH.

En el código se puede comprobar que utiliza la función DENAVIT para realiza

los cálculos necesarios para obtener la matriz de transformación homogénea entre dos sistemas consecutivos utilizando los parámetros DH:

1 % DENAVIT Matriz de transformación homogénea. 2 % DH = DENAVIT(TETA, D, A, ALFA) devuelve la matriz de transformación 3 % homogénea 4 x 4 a partir de los parámetros de Denavit-Hartenberg 4 % D, ALFA, A y TETA. 5 % 6 % Ver también DIRECTA. 7 8 function dh=denavit(teta, d, a, alfa) 9 dh=[cos(teta) -cos(alfa)*sin(teta) sin(alfa)*sin(teta) a*cos(teta);

10 sin(teta) cos(alfa)*cos(teta) -sin(alfa)*cos(teta) a*sin(teta); 11 0 sin(alfa) cos(alfa) d; 12 0 0 0 1];

Tal y como se puede observar en el código, la matriz dh es justo la matriz de

transformación homogénea que resultaba de multiplicar las cuatro matrices de transformaciones básicas:

⋅⋅−⋅

⋅⋅⋅−

=−

1000

)cos()sen(0

)sen()cos()sen()cos()cos()sen(

)cos()sen()sen()sen()cos()cos(

1

iii

iiiiiii

iiiiiii

ii

d

a

a

αα

θθαθαθ

θθαθαθ

A

Por tanto, tendríamos resuelto el problema de la cinemática directa de nuestro

robot de una manera más eficaz que nos permite obtener la cinemática inversa de cualquier matriz T:

( ) ( ) ( )

( ) ( ) ( )543216543215543214

543213543212543211

,,,,,,,,,,,,

,,,,,,,,,,,,

qqqqqfqqqqqfqqqqqf

qqqqqfzqqqqqfyqqqqqfx

===

===

γβα

RESOLUCIÓN DEL PROBLEMA DE LA CINEMÁTICA DIRECTA

⋅−−−−

⋅⋅−⋅−⋅−

⋅⋅+⋅⋅

=

1000

))(sen()())(sen())(cos(0



1000

554355

545254544

545154544

tlltdtt

ttltdttttt

ttltdttttt

paon

paon

paon

zzzz

yyyy

xxxx

θθθ

θθθθθθθ

θθθθθθθ



2 - 23

4. EJEMPLOS DE UTILIZACIÓN DE LA FUNCIÓN IMPLEMENTADA.

Figura 2.6: Ejemplos de utilización de la función DIRECTA.

>> q=[–1 –1 0.5 –pi/6 –pi/3] q = –1.0000 –1.0000 0.5000 –0.5236 –1.0472 >> T=directa(q) T = –0.5000 –0.7500 0.4330 –0.9134 0.8660 –0.4330 0.2500 –0.9500 –0.0000 0.5000 0.8660 –0.7268 0.0000 0.0000 0.0000 1.0000

>> q=[–1 –1 0.5 –pi/6 pi/2] q = –1.0000 –1.0000 0.5000 –0.5236 1.5708 >> T=directa(q) T = –0.5000 0.8660 0.0000 –1.0000 0.8660 0.5000 0.0000 –1.0000 –0.0000 0.0000 –1.0000 –1.1000 0.0000 0.0000 0.0000 1.0000

>> q=[-1.5 -1.5 0.5 pi/6 pi/2] q = –1.5000 –1.5000 0.5000 0.5236 1.5708 >> T=directa(q) T = 0.5000 0.8660 0.0000 –1.5000 0.8660 –0.5000 –0.0000 –1.5000 0.0000 0.0000 –1.0000 –1.1000 0.0000 0.0000 0.0000 1.0000

>> q=[–1.5 –1.5 0.5 pi/4 pi/4] q = –1.5000 –1.5000 0.5000 0.7854 0.7854 >> T=directa(q) T = 0.7071 0.5000 0.5000 –1.4000 0.7071 –0.5000 –0.5000 –1.6000 0.0000 0.7071 –0.7071 –1.0414 0.0000 0.0000 0.0000 1.0000



2 - 24

5. LA CINEMÁTICA DIRECTA COMO VENTAJA: EL JACOBIANO. La ventaja principal que posee nuestro robot de aspectos quirúrgicos no procede

de la resolución de los cálculos para obtener la matriz T puesto que cualquier robot se puede resolver de una manera sencilla y rápida utilizando el método de resolución por parámetros DH. En realizad la ventaja principal en cuanto a cinemática procede de sus singularidades, o mejor dicho, de la ausencia de singularidades. Las diferentes configuraciones singulares del robot pueden ser clasificadas como:

• Singularidades en las límites en del espacio de trabajo del robot.

Se presentan cuando el extremo del robot está en algún punto del límite de

trabajo interior o exterior. En esta situación resulta obvio que el robot no podrá desplazarse en las direcciones que lo alejan de este espacio de trabajo.

En nuestro robot estaremos limitados únicamente por las cuatro paredes de la

sala de operaciones puesto que el robot podrá llegar a cualquier lugar de la habitación siempre y cuando sus guías lo permitan. Gracias a que son prismáticas nuestro espacio de trabajo es tan grande como se la sala de operaciones tal y como se muestra en la figura:

SINGULARIDAD POR TRABAJO EXTERIOR SINGULARIDAD POR TRABAJO INTERIOR

Figura 2.5: Tipos de singularidades que presenta el robot. Hablando de singularidades de trabajo exterior, la ventaja con respecto a los

demás robots, generalmente de carácter antropomórfico, es que nuestro espacio de trabajo lo podemos ampliar a toda la habitación mientras que el resto de robots sólo alcanzan hasta donde su brazo les permita. Es decir, nuestro robot está menos restringido en cuanto a capacidad de desplazamiento.

Otra ventaja, pero referida a las singularidades de trabajo interior, es que nuestro

robot sólo presenta dos singularidades pues sólo tenemos dos articulaciones rotacionales (generalmente son las articulaciones de este tipo son las que tienen problemas pues sólo pueden hacerlo unos grados determinados) a diferencia de los antropomórficos más usados por mis compañeros que tenían tantas singularidades de trabajo interior como grados de libertad tenía su robot.

Esto también es demostrable a partir de la matriz Jacobiana que viene definida

en la página siguiente.

0º < θ4(t) < 360º

-75º < θ5(t) < 225º



2 - 25

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

=⇒

⋅=

54321

54321

54321

54321

54321

54321

5

4

3

2

1

θβ

θββββ

θβ

θββββ

θα

θαααα

θθ

θθ

θθ

θ

θ

γ

β

α

ddd

ddd

ddd

zz

d

z

d

z

d

z

yy

d

y

d

y

d

y

xx

d

x

d

x

d

x

d

d

d

z

y

x

JJ

&

&

&

&

&

&

&

&

&

&

&

Las derivadas de los valores x, y y z respecto a cada una de las variables articulares son:

55455

45454

3

2

11

5451

)sen()cos(

)cos()sen(

0

0

1

))(cos())(cos()(

θθθθ

θθθθ

θθ

&

&

&

⋅⋅−=∂∂

⋅⋅−=∂∂

=∂∂

=∂∂

⋅=∂∂

⋅⋅+=

lx

lx

d

x

d

x

dd

x

ttltdx

55455

45454

3

22

1

5452

)sen()sen(

)cos()cos(

0

1

0

))(cos())(sen()(

θθθθ

θθθθ

θθ

&

&

&

⋅⋅+=∂∂

⋅⋅−=∂∂

=∂∂

⋅=∂∂

=∂∂

⋅⋅−=

ly

ly

d

y

dd

y

d

y

ttltdy

5555

4

33

2

1

5543

)cos(

0

1

0

0

))(sen()(

θθθ

θ

θ

&

&

⋅−=∂∂

=∂∂

⋅−=∂∂

=∂∂

=∂∂

⋅−−−=

lz

z

dd

z

d

z

d

z

tlltdz

Si tomamos como ángulos de Euler α, β y γ como los ángulos que forma nuestro vector a de aproximación con los vectores i, j y k de dirección de la base canónica, tenemos que los ángulos de los cosenos directores son los siguientes:

Ángulo de Euler α:

[ ]

)))(cos())((arccos(cos))(cos())(cos()cos(

))(cos())(cos(

))(sen(

))(cos())(sen(

))(cos())(cos(

001)cos(

es.ortonormal esson vector Ambos11

)cos(

5454

54

5

54

54

tttt

tt

t

tt

tt

ai

aYi

aiai

θθαθθα

θθ

θ

θθ

θθ

α

α

⋅=⇒⋅=

⋅=

−

⋅−

⋅

⋅⇒⋅=

⇒==

⋅=⋅

rr

rr

rrrr

Ángulo de Euler β:

[ ]

)))(cos())(sen(arccos())(cos())(sen()cos(

))(cos())(sen(

))(sen(

))(cos())(sen(

))(cos())(cos(

010)cos(


)cos(

5454

54

5

54

54

tttt

tt

t

tt

tt

aj

aYj

ajaj

θθβθθβ

θθ

θ

θθ

θθ

β

β

⋅−=⇒⋅−=

⋅−=

−

⋅−

⋅

⋅⇒⋅=

⇒==

⋅=⋅

rr

rr

rrrr



2 - 26

Ángulo de Euler γ:

[ ]

)))(sen(arccos())(sen()cos(

))(sen(

))(sen(

))(cos())(sen(

))(cos())(cos(

100)cos(


)cos(

55

5

5

54

54

tt

t

t

tt

tt

aj

aYk

akak

θγθγ

θ

θ

θθ

θθ

γ

γ

−=⇒−=

−=

−

⋅−

⋅

⋅⇒⋅=

⇒==

⋅=⋅

rr

rr

rrrr

Las derivadas de los ángulos de Euler respecto a cada una de las variables

articulares son:

5

52

42

54

5

4

52

42

54

4

3

2

1

54

)(cos)(cos1

)sen()cos(

)(cos)(cos1

)cos()sen(

0

0

0

)))(cos())((arccos(cos

θθθ

θθθ

θθθ

θθθ

θθα

&

&

⋅−

⋅=

∂∂

⋅−

⋅=

∂∂

=∂∂

=∂∂

=∂∂

⋅=

y

y

y

y

y

a

a

d

a

d

a

d

a

tt

5

52

42

54

5

4

52

42

54

4

3

2

1

54

)(cos)(sen1

)sen()sen(

)(cos)(sen1

)cos()cos(

0

0

0

)))(cos())(sen(arccos(

θθθ

θθθ

θθθ

θθθ

θθβ

&

&

⋅−

⋅−=

∂∂

⋅−

⋅=

∂∂

=∂∂

=∂∂

=∂∂

⋅−=

x

x

x

x

x

a

a

d

a

d

a

d

a

tt

[ ]555

25

5

4

3

2

1

5

1))cos(())(sen(1

1

0

0

0

0

)))(sen(arccos(

θθθθθ

θ

θγ

&& ⋅±=−−−

−=∂∂

=∂∂

=∂∂

=∂∂

=∂∂

−=

t

a

a

d

a

d

a

d

a

t

z

z

z

z

z

Si sólo tenemos en cuenta las velocidades en x y y z y trabajamos con la Jacobiana pseudoinversa puesto que nuestro robot no tiene tantos grados de libertad como dimensiones tiene el espacio tarea de 6 GDL:

⋅

⋅−−

⋅⋅+⋅⋅−

⋅⋅−⋅⋅−

=

5

4

3

2

1

55

545545

545545

)cos(0100

)sen()sen()cos()cos(010

)sen()cos()cos()sen(001

θ

θθ

θθθθ

θθθθ

&

&

&

&

&

&

&

&

d

d

d

l

ll

ll

z

y

x



2 - 27

⋅−⋅⋅+⋅⋅−

⋅⋅−⋅⋅−

−⋅

⋅−−

⋅⋅+⋅⋅−

⋅⋅−⋅⋅−

=⋅

55545545

54554555

545545

545545

0

100

010

001

0100

010

001

ClSSlSCl

CClCSlCl

SSlCCl

SClCSl

JJT

1))((cos1 522

525 ++=⋅ tll

T θJJ

Esta es una manera de demostrar que nuestro robot no tiene configuraciones

singulares puesto que el determinante de su matriz no puede anularse. Este hecho plasma la idea de que las prismáticas no nos restrigen el movimiento puesto que nos lo restringe las paredes y techo de la sala de operaciones.

Si tenemos en cuenta que también se toman los ángulos de los cosenos directores

α β y γ:

⋅

⋅−

⋅−⋅−

⋅+

⋅−

⋅+⋅−

⋅+

⋅−−

⋅⋅+⋅⋅−

⋅⋅−⋅⋅−

=

5

4

3

2

1

52

42

54

52

42

54

52

42

54

52

42

54

55

545545

545545

10000)(cos)(sen1

)sen()sen(

)(cos)(sen1

)cos()cos(000

)(cos)(cos1

)sen()cos(

)(cos)(cos1

)cos()sen(000

)cos(0100



θ

θ

θθ

θθ

θθ

θθθθ

θθ

θθ

θθθ

θθθθ

θθθθ

γ

β

α

&

&

&

&

&

&

&

&

&

&

&

d

d

d

l

ll

ll

z

y

x

0=⋅ TJJ

Esto es debido principalmente a que nuestro robot es de cuatro grados de libertad

y no puede dar lugar a 6 grados de libertad. Por ello quitamos uno, por ejemplo, el que hace referencia a γ:

⋅

⋅−

⋅−⋅−

⋅+

⋅−

⋅+⋅−

⋅+

⋅−−

⋅⋅+⋅⋅−

⋅⋅−⋅⋅−

=

5

4

3

2

1

52

42

54

52

42

54

52

42

54

52

42

54

55

545545

545545

10000)(cos)(sen1

)sen()sen(

)(cos)(sen1

)cos()cos(000

)(cos)(cos1

)sen()cos(

)(cos)(cos1

)cos()sen(000

)cos(0100



θ

θ

θθ

θθ

θθ

θθθθ

θθ

θθ

θθθ

θθθθ

θθθθ

γ

β

α

&

&

&

&

&

&

&

&

&

&

&

d

d

d

l

ll

ll

z

y

x



2 - 28

Por tanto, la matriz sería la siguiente:

⋅

⋅−

⋅−⋅−

⋅+

⋅−

⋅+⋅−

⋅+

⋅−−

⋅⋅+⋅⋅−

⋅⋅−⋅⋅−

=

5

4

3

2

1

52

42

54

52

42

54

52

42

54

52

42

54

55

545545

545545

)(cos)(sen1

)sen()sen(

)(cos)(sen1

)cos()cos(000

)(cos)(cos1

)sen()cos(

)(cos)(cos1

)cos()sen(000

)cos(0100



θ

θ

θθ

θθ

θθ

θθθθ

θθ

θθ

θθθ

θθθθ

θθθθ

β

α

&

&

&

&

&

&

&

&

&

&

d

d

d

l

ll

ll

z

y

x

)(cos)(cos1)(cos)(sen1

)cos()sen(

52

42

52

42

55

θθθθ

θθ

⋅−⋅−

⋅=J

Existen dos posibilidades de que se anule:

0

º270,º901)sen(

1)(cos1

º180,º01)cos(

1)(sen1

1)(cos11)(sen101)(cos11)(sen1

)1(0


)cos()sen(

º180,º00)sen(

º225º75

º270,º180,º90,º0

44

42

45

42

42

42

42

42

º180,º052

42

52

42

55º180,º0

55

5

4

5

5

=

≠⇒±≠≠−

≠⇒±≠≠−

≠−≠−⇔=⋅−⋅−

±⋅=

=⋅−⋅−

⋅=

=⇒=≤≤−

=

=

=

θ

θ

θ

θθθ

θθθ

θθθθ

θθθθ

θθ

θθθ

J

J

y

011

0

0)(cos10)(sen1

01


)cos()sen(

º270,º900)cos(

º225º75

42

42

º270,º9052

42

52

42

55º270,º90

55

5

5

5

==⋅−⋅−

⋅=

=⋅−⋅−

⋅=

=⇒=≤≤−

=

=

θθ

θθθθ

θθ

θθθ

θ

θJ

º9054 =∀ θθ y Caso 1 Límite exterior del espacio de trabajo.

º27054 =∀ θθ y Caso 2 Límite interior del espacio de trabajo. º0º270,º180,º90,º0 54 =≠ θθ y Caso 3 Límite exterior del espacio de trabajo.

º180º270,º180,º90,º0 54 =≠ θθ y Caso 4 Límite exterior del espacio de trabajo.



2 - 29

Figura 2.6a: Casos 1 y 2. Tipos de singularidades en los límites que presenta en

θ4(t) = 90º, 270º.

Figura 2.6b: Casos 3 y 4. Tipos de singularidades en los límites que presenta en

θ4(t) ≠ 0º, 90º, 180º, 270º y θ4(t ) = 0º, 180º..

∀θ4(t)

θ5(t) = 90º

4x

4y

4z

5x

5y

5z

∀θ4(t)

θ5(t) = 270º

3y

4x

4y

4z

5x

5y5z

θ4(t ) ≠ 0º, 90º, 180º, 270º

θ5(t) = 0º

3x

3y

3z

4x4y

4z

5x5y

5z θ5(t) = 180º

3x

3y

3z

4x4y

4z

5x5y

5z



2 - 30

• Singularidades en interior del espacio de trabajo del robot. Ocurren dentro de la zona de trabajo y se producen generalmente por el

alineamiento de dos o más ejes de las articulaciones del robot. Esta es una ventaja bastante que coloca a nuestro robot por encima de los demás puesto que conocemos con exactitud los valores de los ángulos para los cuales se producirían las configuraciones singulares (existen dos z2 y z3 alienados pero esto no influye puesto que se mantiene siempre así). Las posibles configuraciones singulares de alineamiento de ejes son:

º0º180,º0 54 == θθ y Caso 5: z4 y z5 se alinean con z1.

º180º270,º90 54 == θθ y Caso 6: z4 y z5 se alinean con z0.

Figura 2.7a: Caso 5 de singularidades de ejes z4 y z5 que se alinean con z1.

θ4(t ) = 180º

θ5(t) = 0º

3x

3y

3z

4x4y

4z

5x5y

5z

θ5(t) = 0º

3y

3z

4x

4y

4z

5x

5y

5z

θ4(t ) = 0º

3x

0x

0y

0z1x

1y

1z

2x

2y

2z

0x

0y

0z1x

1y

1z

2x

2y

2z



2 - 31

Figura 2.7b: Caso 6 de singularidades de ejes z4 y z5 que se alinean con z0.

La ventaja de nuestro robot es que conocemos cuando se serán singularidades y, por tanto, procuraremos no trabajar para valores de ángulos donde se produzca una alineación de ejes. Además, su matriz Jacobiana tiene una expresión analítica fácil de implementar y que agiliza las operaciones computadas.

A continuación, haremos un breve recordatorio de todas las singularidades que

posee nuestro robot, ya sea de límites como de alineamiento de ejes:

º9054 =∀ θθ y Caso 1 Límite exterior del espacio de trabajo. º27054 =∀ θθ y Caso 2 Límite interior del espacio de trabajo.

º0º270,º180,º90,º0 54 =≠ θθ y Caso 3 Límite exterior del espacio de trabajo. º180º270,º180,º90,º0 54 =≠ θθ y Caso 4 Límite exterior del espacio de trabajo.

º0º180,º0 54 == θθ y Caso 5: z4 y z5 se alinean con z1. º180º270,º90 54 == θθ y Caso 6: z4 y z5 se alinean con z0.

θ4(t ) = 90º

θ5(t) = 180º

3x

3y

3z

4x

4y

4z

5y

5z

θ4(t ) = 270º

θ5(t) = 180º

3x

3y3z

4x

4y 4z5y

5z

5x

5x

0x

0y

0z1x

1y

1z

2x

2y

2z

0x

0y

0z1x

1y

1z

2x

2y

2z



2 - 32

• Configuraciones del robot menos singulares: Jacobiano máximo o mínimo. A continuación intentaremos encontrar los valores angulares para los cuales el

valor absoluto del Jacobiano es máximo. Con estos valores encontraremos aquellas configuraciones para las cuales el robot trabaja más "cómodo" por decirlo de algún modo. Para pode minimizar la función del determinante necesitamos saber lo siguiente:

doptimalida de sCondicione

0

0


)cos()sen(

5

4

52

42

52

42

55

⇒

=∂∂

=∂∂

⋅−⋅−

⋅=

θ

θ

θθθθ

θθ

J

J

J

A continuación calcularemos ambas derivadas e intentaremos resolver el sistema

de ecuaciones:

( ) ( )0


))(cos21()(cos)cos()sen(

23

52

422

3

52

42

42

52

44

4

=⋅−⋅−

−⋅⋅⋅=∂∂

θθθθ

θθθθθJ

Sabiendo esto, tenemos que la expresión anterior se puede expresar como:

( ) ( ) 0)(cos)(cos1)(cos)(sen1

0))(cos21()(cos)cos()sen(

23

52

422

3

52

42

42

52

44

≠⋅−⋅−

=−⋅⋅⋅

θθθθ

θθθθ

Una vez obtenida la primera ecuación pasamos a la segunda:

( ) ( )0


1))(sen1()(cos)(cos)(cos)(sen

23

52

422

3

52

42

52

52

54

42

42

5

=⋅−⋅−

−+⋅+⋅⋅=∂∂

θθθθ

θθθθθθJ

Sabiendo esto, tenemos que la expresión anterior se puede expresar como:

( ) ( ) 0)(cos)(cos1)(cos)(sen1

01))(sen1()(cos)(cos)(cos)(sen

23

52

422

3

52

42

52

52

54

42

42

≠⋅−⋅−

=−+⋅+⋅⋅

θθθθ

θθθθθ

Las soluciones a este sistema de ecuaciones son:

( ) ( ) º270,º180,º90,º001)(cos11)(sen1

º180,º00)sen(

42

3

422

3

42

55

≠⇒≠⋅−⋅−

=⇒=

θθθ

θθ

( ) ( ) 42

3

422

3

42

55

00)(cos10)(sen1

º270,º900)cos(

θθθ

θθ

∀⇒≠⋅−⋅−

=⇒=

Punto crítico 1

Punto crítico 2



2 - 33

( ) ( ) º180,º00)(cos11)(cos01

º180,º00)sen(

52

3

522

3

52

44

≠⇒≠⋅−⋅−

=⇒=

θθθ

θθ

( ) ( ) º180,º00)(cos01)(cos11

º270,º900)cos(

52

3

522

3

52

44

≠⇒≠⋅−⋅−

=⇒=

θθθ

θθ

5

23

52

23

52

44

0)(cos2

11)(cos

2

11

º315,º452

2)cos(

θθθ

θθ

∀⇒≠

⋅−

⋅−

=⇒=

5

23

52

23

52

44

0)(cos2

11)(cos

2

11

º225,º1352

2)cos(

θθθ

θθ

∀⇒≠

⋅−

⋅−

=⇒−= Punto

Estos son puntos críticos es decir, mínimos y máximos o de ensilladura:

• Punto crítico 1: Recordemos que se trata de los casos 3 y 4 de límite

exterior del espacio de trabajo antes estudiados. Es decir, es un punto de ensilladura.

• Punto crítico 2: Recordemos que se trata de los casos 1 y 2 de límite exterior e interior, respectivamente, del espacio de trabajo antes estudiados. Es decir, es un punto de ensilladura.

• Punto crítico 3: Se trata de un conjunto de posiciones que no es ninguno de los 6 casos y el Jacobiano es de )cos( 5θ=J con º180,º05 ≠θ . Su

valor variará en función de º180,º05 ≠θ . Se trata de los puntos que complementan a los casos 3 y 4.

• Punto crítico 4: Se trata de un conjunto de posiciones que no es ninguno de los 6 casos y el Jacobiano es de )cos( 5θ=J con º180,º05 ≠θ . Su

valor variará en función de º180,º05 ≠θ . Se trata de los puntos que complementan a los casos 3 y 4.

• Punto crítico 5: Se trata de un conjunto de posiciones que no es ninguno de los 6 casos y el Jacobiano es de

)(cos2

11

)cos()sen(

52

55

θ

θθ

⋅−

⋅=J

( )( ) 3

3)sen(0)(sen310

)(sen1

)(sen31255

2

2

52

52

5

±=⇒=−⇒=+

−⋅=∂∂

θθθ

θθJ

Punto crítico 3

Punto crítico 4

Punto crítico 5

Punto crítico 6



2 - 34

Obtenemos como resultado que para los ángulos θ5 = 35.2644º, 144.7356º el determinante de la matriz Jacobiana es máximo y para ángulos θ5 = 324.7356º, 215.2564º y sus valores son:

2

2

3

23

2

3

2

2

11

3

2

3

1

3

11

2

11

3

11

3

1

2

2

+==⋅−

⋅=

−⋅−

−⋅

=J

2

2

3

23

2

3

2

2

11

3

2

3

1

3

11

2

11

3

11

3

1

2

2

−=−=⋅−

⋅−=

−⋅−

−⋅−

=J

Tal como podemos observar nos da igual que sean máximos o

mínimos puesto que lo que nos recompensa es que el valor absoluto del determinante sea máximo, es decir, que el Jacobiano esté lo más alejado posible de anularse.

• Punto crítico 6: Se trata de un conjunto de posiciones que no es ninguno de los 6 casos y el Jacobiano es de

)(cos2

11

)cos()sen(

52

55

θ

θθ

⋅−

⋅=J

( )( ) 3

3)sen(0)(sen310

)(sen1

)(sen31255

2

2

52

52

5

±=⇒=−⇒=+

−⋅=∂∂

θθθ

θθJ

Obtenemos como resultado que para los ángulos θ5 = 35.2644º,

144.7356º el determinante de la matriz Jacobiana es máximo y para ángulos θ5 = 324.7356º, 215.2564º y sus valores son:

2

2+=J

2

2−=J

Tal como podemos observar nos da igual que sean máximos o

mínimos puesto que lo que nos renta es que el valor absoluto del determinante sea máximo, es decir, que el Jacobiano esté lo más alejado posible de anularse.



2 - 35

Hagamos un breve repaso de las singularidades y no singularidades de nuestro robot:

SUNGULARIDADES

º9054 =∀ θθ y Caso 1 Límite exterior del espacio de trabajo. º27054 =∀ θθ y Caso 2 Límite interior del espacio de trabajo.

º0º270,º180,º90,º0 54 =≠ θθ y Caso 3 Límite exterior del espacio de trabajo. º180º270,º180,º90,º0 54 =≠ θθ y Caso 4 Límite exterior del espacio de trabajo.

º0º180,º0 54 == θθ y Caso 5: z4 y z5 se alinean con z1. º180º270,º90 54 == θθ y Caso 6: z4 y z5 se alinean con z0.

ANTI-SINGULARIDADES

Configuración 1: θ4 = 45º y θ5 = 35.2644º Configuración 2: θ4 = 45º y θ5 = 144.7356º Configuración 3: θ4 = 45º y θ5 = 324.7356º Configuración 4: θ4 = 45º y θ5 = 215.2564º Configuración 5: θ4 = 135º y θ5 = 35.2644º Configuración 6: θ4 = 135º y θ5 = 144.7356º Configuración 7: θ4 = 135º y θ5 = 324.7356º Configuración 8: θ4 = 135º y θ5 = 215.2564º Configuración 9: θ4 = 225º y θ5 = 35.2644º Configuración 10: θ4 = 225º y θ5 = 144.7356º Configuración 11: θ4 = 225º y θ5 = 324.7356º Configuración 12: θ4 = 225º y θ5 = 215.2564º Configuración 13: θ4 = –45º y θ5 = 35.2644º Configuración 14: θ4 = –45º y θ5 = 144.7356º Configuración 15: θ4 = –45º y θ5 = 324.7356º Configuración 16: θ4 = –45º y θ5 = 215.2564º A su vez, podemos simplificar estas 16 configuraciones a 8 puesto que son

configuraciones equivalentes dos a dos. A continuación vemos cuales son equivalentes:

Configuración 2: θ4 = 45º y θ5 = 144.7356º Configuración 7: θ4 = 135º y θ5 = 324.7356º




Configuración 10: θ4 = 225º y θ5 = 144.7356º Configuración 15: θ4 = –45º y θ5 = 324.7356º






2 - 36

Por tanto, tenemos que las configuraciones osn en realidad 8 que son Configuración 1: θ4 = 45º y θ5 = 35.2644º Configuración 2: θ4 = 45º y θ5 = 144.7356º Configuración 3: θ4 = 45º y θ5 = 324.7356º Configuración 4: θ4 = 45º y θ5 = 215.2564º Configuración 13: θ4 = –45º y θ5 = 35.2644º Configuración 14: θ4 = –45º y θ5 = 144.7356º Configuración 15: θ4 = –45º y θ5 = 324.7356º Configuración 16: θ4 = –45º y θ5 = 215.2564º

Por tanto, la cinemática directa ha sido una ventaja, por su sencillez, para

obtener la matriz Jacobiana y esta, a su vez, nos ha servido para obtener las configuraciones singulares (a las que procuraremos no llegar con nuestro robot) y las configuraciones menos singulares (donde tendremos que procurar que más actúe nuestro robot). Además, aquí se puede observar que los diversos casos dependen fuertemente de las articulaciones rotacionales que son las que más problemas nos están provocando; he aquí una de las grandes ventajas de emplear las prismáticas puesto que no restringen a nuestro robot prácticamente en nada.

CINEMÁTICA INVERSA DEL QUIROBOT


3 - 1

CCCaaapppííítttuuulllooo 333 CCCIIINNNEEEMMMÁÁÁTTTIIICCCAAA IIINNNVVVEEERRRSSSAAA DDDEEELLL QQQUUUIIIRRROOOBBBOOOTTT

"DE VUELTA DE TODO" Pau Donés

Hasta ahora, habíamos obtenido las coordenadas para la posición del punto

terminal a partir de los valores de las variables de articulación. Ahora nos planteamos el problema inverso: obtener los valores de las variables articulares, que son realmente lo que se envía al sistema de control del manipulador, a partir de la posición y orientación deseadas para el punto terminal. Esto es lo más usado, y absolutamente necesario, dado que las tareas a realizar o trayectorias a recorrer por un manipulador se dan casi siempre en coordenadas cartesianas referidas a algún sistema fijo.

"El objetivo del problema cinemático inverso consiste en encontrar los valores

que deben adoptar las coordenadas articulares del robot q = [q1, q2, q3, q4, q5]T para que

su extremo, o efector final, se posicione y oriente según una determinada localización espacial."

La cinemática inversa es, en general, mucho más problemática que la directa. Los problemas pueden ser:

• El punto pedido está, por su lejanía, fuera del alcance del robot.

• El punto pedido está fuera del alcance, pero debido a problemas geométricos (unos enlaces del robot chocan con otros).

• El punto pedido puede alcanzarse mediante dos o más combinaciones (valores) del vector de articulación. Cada una de estas combinaciones se llama configuración; es típico el caso de los brazos SCARA, como el del robot RT100, donde cualquier punto alcanzable puede alcanzarse con el codo doblado hacia la derecha, o hacia la izquierda (lo cual también modifica, naturalmente, los valores de las demás articulaciones).

• Las ecuaciones que relacionan las variables de articulación con las variables cartesianas no son solubles, en el sentido de que no se pueden encontrar soluciones separables, llamadas forma cerrada, que den el valor de cada variable cartesiana como función exclusivamente de las variables de articulación.



3 - 2

A diferencia de la cinemática directa, no existía hasta ahora ningún método sistemático que permita resolver la cinemática inversa de cualquier manipulador por aplicación mecánica de unas reglas fijas. Existían, más bien, técnicas que ayudan, debiendo elegir la más adaptada al caso particular. En 1998 se publicó el primer libro que contiene una exposición sistemática del problema de la cinemática inversa. Sus autores, C. D. Crane y J. Dufiy lo resuelven para el caso de cualquier robot de hasta 6 grados de libertad, dando complejas reglas de aplicación de fórmulas predeterminadas.

Así como es posible abordar el problema de la cinemática directa de una manera

sistemática a partir de la utilización de matrices de transformación homogéneas, e independientemente de la configuración del robot, no ocurre lo mismo con el problema cinemática inverso en general, pues en la mayoría de los casos el procedimiento de obtención de las ecuaciones es fuertemente dependiente de la configuración del robot. Se han desarrollado algunos procedimientos genéricos susceptibles de ser programados, de modo que mediante un computador pueda obtener el vector de los valores articulares que posicionan y orientan el efector final a partir del conocimiento de la cinemática directa. Eso conlleva el inconveniente de que estos procedimientos son métodos numéricos iterativos, cuya velocidad de convergencia e inclusive su convergencia en sí no está siempre garantizada.

A la hora de resolver el problema cinemático inverso es mucho más adecuado

encontrar una solución cerrada. Esto es una relación matemática explícita de la forma:

( )( )( )( )( )γβα

γβαγβαγβαγβα

,,,,,

,,,,,

,,,,,

,,,,,

,,,,,

55

44

33

22

11

zyxfq

zyxfq

zyxfq

zyxfq

zyxfq

=====

Este tipo de solución presenta, entre otras, las siguientes ventajas:

1. En muchas aplicaciones, el problema cinemático inverso ha de resolverse en

tiempo real (por ejemplo, en el seguimiento de una determinada trayectoria). Por tanto, una solución de tipo iterativo no garantiza tener la solución en el momento adecuado. Este es el caso que nos atañe puesto que cuando el robot esté realizando la incisión tiene que ser una incisión limpia y precisa sin tiempos muertos ni ningún problema de este tipo.

2. Al contrario de lo que ocurría en el problema cinemático directo, con cierta

frecuencia la solución del problema no es única; existiendo diferentes 5-tuplas [q1, q2, q3, q4, q5]

T que posicionada y orientan el extremo del robot del mismo modo. En estos casos una solución cerrada permite incluir determinadas reglas o restricciones que aseguren que la solución obtenida sea la más adecuada de entre las posibles.

Justo la cinemática inversa que posee nuestro robot es una gran virtud puesto

que para su obtención no se precisa emplear un método numérico iterativo ya que, a pesar de las dificultades comentadas, nuestro robot posee una cinemática relativamente simple que facilita en cierta medida la resolución de su problema cinemático inverso.

3.1



3 - 3

Para resolver la cinemática inversa de nuestro robot podríamos utilizar el método de resolución geométrico puesto que éste método permite obtener los valores de las primeras variables articulares, que son las que consiguen posicionar el robot (prescindiendo de la orientación de su extremo) y posteriormente resolver las variables articulares que se encargan de la orientación que en nuestro caso serían las dos variables angulares.

No obstante, podemos emplear un método más sencillo aún que el método geométrico y que sirve de alternativo para resolver el mismo problema.

Por tanto, se puede recurrir a manipular directamente las ecuaciones

correspondientes al problema cinemático directo. Es decir, puesto que éste establece la relación entre la matriz de orientación n o a y posicionamiento p con la matriz de transformación homogénea del robot tal que:

[ ] [ ] [ ]1,4con ∈= i,jTijpaonrrrr

Los elementos tij son función de las coordenadas articulares [q1, q2, q3, q4, q5]

T, y es posible pensar que mediante ciertas combinaciones de las 12 ecuaciones planteadas en esta matriz se puedan despejar las 6 variables articulares en función de las componentes de los vectores n, o, a y p.

Para la resolución del problema de la cinemática inversa necesitaremos recordar

la huella del robot y su representación de Denavit-Hartenberg:

Tabla 3.1: Tabla de los parámetros DH.

ESLABÓN θi di ai αi 1 0º d1(t) 0 90º 2 90º d2(t) 0 –90º 3 0º d3(t) 0 0º 4 θ4(t) l4 0 90º 5 θ5(t) 0 l5 0º

3.2



3 - 4

1. DESCRIPCIÓN DEL MÉTODO DIRECTO.

En este procedimiento se trata de usar las normas de construcción de la matriz de transformación homogénea T entre el Mundo y el Punto Terminal (es decir, entre el espacio de coordenadas cartesianas y articulares, cinemática directa), escribiendo dicha matriz de modo simbólico, es decir, dejando cada elemento en función de las variables de cada articulación qi(t) de las que dependa. A continuación, y usando la interpretación dada para la matriz T, podemos escribir que

=

44434241

34333231

24232221

14131211

1000 TTTT

TTTT

TTTT

TTTT

paon

paon

paon

zzzz

yyyy

xxxx

y dados (nx, ny, nz, ox, oy, oz, ax, ay, az, px, py, pz) que el usuario debe proveer

como especificación de la posición y orientación, se obtiene las ecuaciones:

)()()()()()()()()()( 5544332211 ttqttqtdtqtdtqtdtq θθ ≡≡≡≡≡

( )( )( )( )( )zyxzyxzyxzyx

zyxzyxzyxzyx

zyxzyxzyxzyx

zyxzyxzyxzyx

zyxzyxzyxzyx

pppaaaooonnnft

pppaaaooonnnft

pppaaaooonnnftd

pppaaaooonnnftd

pppaaaooonnnftd

,,,,,,,,,,,)(

,,,,,,,,,,,)(

,,,,,,,,,,,)(

,,,,,,,,,,,)(

,,,,,,,,,,,)(

55

44

33

22

11

=====

θθ

Se pueden dar diferentes ecuaciones para los ángulos de orientación, usando

otros elementos de la matriz. Además, las ecuaciones serían por supuesto diferentes en caso de haber optado por otra representación para la orientación en lugar de la de los ángulos de Euler (por ejemplo, los ángulos roll pitch and yaw). En cualquier caso, la idea es simplemente plantear directamente tantas ecuaciones como grados de libertad haya, y tratar de resolverlos como se pueda. Si hubiera menos de 6 grados de libertad como es nuestro caso pues tenemos únicamente 5 grados de libertad, habría, por supuesto, menos ecuaciones. Generalmente, suelen ser ecuaciones no lineales bastante complejas, y es difícil encontrar de modo simple una solución en forma cerrada.

Existe una vertiente de este método que, en principio, puede valer para cualquier

manipulador pues nos ayuda a obtener expresiones de alguna variable articular que sea más difícil de despejar que las demás. Usa como base las expresiones simbólicas para la cinemática directa. Se trata de igualar una matriz T de transformación homogénea expresada en función de sus variables (nx, ny, nz, ox, oy, oz, ax, ay, az, px, py, pz) al producto de las matrices de la cinemática directa, y premultiplicar ambos lados de la igualdad por la inversa de cada una, después de haber hallado sustituciones simbólicas para la variable articular de la cual dependa la que en este momento sea primera matriz del producto.

3.3

3.4



3 - 5

2. RESOLUCIÓN DE LA CINEMÁTICA INVERSA. MÉTODO DIRECTO//MANIPULACIÓN DE MATRICES SIMBÓLICAS.

El primer paso a dar para conseguir resolver el problema cinemática inverso es

obtener la expresión [2.3] correspondiente a nuestro robot. Es decir, debemos obtener la matriz T que relaciona el sistema de referencia S0 asociado a la base con el sistema S5 asociado a su extremo.

La figura 2.1 representa la asignación de los sistemas de referencia según los

criterios de Denavit-Hartenberg, con el robot situado en su posición de partida y la tabla 2.1 muestra los valores de los parámetros de Denavit-Hartenberg.

A partir de éstos es inmediato obtener las matrices i-1Ai y la matriz T:

−=

1000

0001

0010

0100

00 A

−=

1000

0001

0010

0100

55 A

−=

1000

010

0100

0001

1

10

dA

−

−

=

1000

010

0001

0100

2

21

dA

=

1000

100

0010

0001

3

32

dA

−=

1000

010

00

00

4

44

44

43

l

CS

SC

θθ

θθ

A

⋅

⋅−

=

1000

0100

0

0

5555

5555

54

θθθ

θθθ

SlCS

ClSC

A

55

54

43

32

21

10

00 AAAAAAAT ⋅⋅⋅⋅⋅⋅=

55

54

43

53

32

21

10

00

30

53

30

AAAA

AAAAA

AAT

⋅⋅=

⋅⋅⋅=

⋅=



3 - 6

La matriz de transformación homogénea para nuestro robot presenta la siguiente forma:

⋅−−−−

⋅⋅−⋅−⋅−

⋅⋅+⋅⋅

=

1000

))(sen()())(sen())(cos(0



1000

554355

545254544

545154544

tlltdtt

ttltdttttt

ttltdttttt

paon

paon

paon

zzzz

yyyy

xxxx

θθθ

θθθθθθθ

θθθθθθθ

Una vez obtenida la expresión de la matriz de transformación homogénea en

función de las coordenadas articulares [ d1(t), d2(t), d3(t), θ4(t), θ5(t) ]T, y supuesta una

localización de destino para el extremo del robot definida por los vectores n, o, a y p se podría intentar manipular directamente las 12 ecuaciones resultantes de T a fin de despejar todas las variables articulares en función de n, o, a y p:

))(sen()(

))(cos())(sen()(

))(cos())(cos()(

))(sen(

))(cos())(sen(

))(cos())(cos(

))(cos(

))(sen())(sen(

))(sen())(cos(

0

))(cos(

))(sen(

5543

5452

5451

5

54

54

5

54

54

4

4

tlltdp

ttltdp

ttltdp

ta

tta

tta

to

tto

tto

n

tn

tn

z

y

x

z

y

x

z

y

x

z

y

x

θθθθθ

θθθθθ

θθθθθ

θθ

⋅−−−=⋅⋅−=⋅⋅+=

−=⋅−=⋅+=

+=⋅−=⋅+=

=+=+=

• Obtención de la variable articular θ4(t):

Podemos obtener el valor de esta variable de varias maneras puesto que existen varias relaciones que expresan el valor de θ4(t) con las componentes de la matriz T:

))(sen()(

))(cos())(sen()(

))(cos())(cos()(

))(sen(

))(cos())(sen(

))(cos())(cos(

))(cos(

))(sen())(sen(

))(sen())(cos(

0

))(cos(

))(sen(

5543

5452

5451

5

54

54

5

54

54

4

4

tlltdp

ttltdp

ttltdp

ta

tta

tta

to

tto

tto

n

tn

tn

z

y

x

z

y

x

z

y

x

z

y

x

θθθθθ

θθθθθ

θθθθθ

θθ

⋅−−−=⋅⋅−=⋅⋅+=

−=⋅−=⋅+=

+=⋅−=⋅+=

=+=+=

=→=→

++=→

+=+=

y

x

y

x

y

x

y

x

n

ntt

n

n

t

t

n

n

tn

tnarctg)())(tg(

))(cos(

))(sen())(cos(

))(sen(44

4

4

4

4 θθθθ

θθ

−=→−=→

⋅+⋅−=→

⋅+=⋅−=

x

y

x

y

x

y

x

y

o

ott

o

o

tt

tt

o

o

tto

ttoarctg)())(tg(

))(sen())(cos(

))(sen())(sen(

))(sen())(cos(

))(sen())(sen(44

54

54

54

54 θθθθθθ

θθθθ

−=→−=→

⋅+⋅−=→

⋅+=⋅−=

x

y

x

y

x

y

x

y

a

att

a

a

tt

tt

a

a

tta

ttaarctg)())(tg(

))(cos())(cos(

))(cos())(sen(

))(cos())(cos(

))(cos())(sen(44

54

54

54

54 θθθθθθ

θθθθ

Como podemos observar, existen tres formas de obtener el valor de θ4(t) que se

caracterizan por relacionar dicha variable articular con los vectores de orientación del efector final.

A pesar de que todas ellas son válidas a priori, esto no es así siempre puesto

cuando quisiéramos encontrar cual es el valor correspondiente de θ4(t) cuando la variable θ5(t) es un valor de 0, π o 2π estaríamos dividiendo por cero y no podríamos obtener un valor de θ4(t).

Como conclusión, podemos afirmar que la única solución factible para obtener

una relación entre la variable articular θ4(t) y las componentes de la matriz T es la primera obtenida (posteriormente veremos que esto no es totalmente cierto).



3 - 7

• Obtención de la variable articular θ5(t):

De forma similar al caso anterior, podemos obtener el valor de esta variable de varias maneras puesto que existen varias relaciones que expresan el valor de θ5(t) con las componentes de la matriz T:

))(sen()(

))(cos())(sen()(

))(cos())(cos()(

))(sen(

))(cos())(sen(

))(cos())(cos(

))(cos(

))(sen())(sen(

))(sen())(cos(

0

))(cos(

))(sen(

5543

5452

5451

5

54

54

5

54

54

4

4

tlltdp

ttltdp

ttltdp

ta

tta

tta

to

tto

tto

n

tn

tn

z

y

x

z

y

x

z

y

x

z

y

x

θθθθθ

θθθθθ

θθθθθ

θθ

⋅−−−=⋅⋅−=⋅⋅+=

−=⋅−=⋅+=

+=⋅−=⋅+=

=+=+=

−=→−=→+−=→

+=−=

z

z

z

z

z

z

z

z

o

att

o

a

t

t

o

a

to

taarctg)())(tg(

))(cos(

))(sen(

))(cos(

))(sen(55

5

5

5

5 θθθθ

θθ

=→=→

⋅+⋅+=→

⋅+=⋅+=

x

x

x

x

x

x

x

x

a

ott

a

o

tt

tt

a

o

tta

ttoarctg)())(tg(

))(cos())(cos(

))(sen())(cos(

))(cos())(cos(

))(sen())(cos(55

54

54

54

54 θθθθθθ

θθθθ

−−

=→=−−

→⋅−⋅−=→

⋅−=⋅−=

y

y

y

y

y

y

y

y

a

ott

a

o

tt

tt

a

o

tta

ttoarctg)())(tg(

))(cos())(sen(

))(sen())(sen(

))(cos())(sen(

))(sen())(sen(55

54

54

54

54 θθθθθθ

θθθθ

Como podemos observar, de manera análoga al caso anterior, existen tres formas

de obtener el valor de θ5(t) que se caracterizan por relacionar dicha variable articular con los vectores de orientación del efector final. A pesar de que todas ellas son válidas a priori, esto no es así siempre puesto cuando quisiéramos encontrar cual es el valor correspondiente de θ5(t) cuando la variable θ4(t) es un valor de π/2 o 3π/2 estaríamos dividiendo por cero y no podríamos obtener un valor de θ5(t). Por tanto, la única solución factible para obtener una relación entre la variable articular θ5(t) y las componentes de la matriz T es la primera obtenida (al final veremos que esto no es así). • Obtención de la variable articular d1(t):

Si observamos la componente T14, vemos rápidamente que esta componente es la única de toda la matriz donde aparece el término de d1(t) y, por tanto, es de esta componente de donde conseguiremos la expresión que nos relacione la variable articular con los vectores de n, o, a y p:

))(cos())(cos()(

))(cos())(cos()(

5451

5451

ttlptd

ttltdp

x

x

θθ

θθ

⋅⋅−=↓

⋅⋅+=

Llegados a este punto podemos pensar que para obtener la expresión de dicha

variable articular bastaría con sustituir las relaciones obtenidas antes de θ4(t) y θ5(t) de la siguiente manera:

−⋅

⋅−=⇒

−=

=

z

z

y

xx

z

z

y

x

o

a

n

nlptd

o

at

n

nt

arctgcosarctgcos)(

arctg)(

arctg)(

51

5

4

θ

θ



3 - 8

Esto parece lógico pero para simplificar más los cálculos para evitar que el computador realice operaciones que pueden ser innecesarias, podemos obtener los valores de cos(θ4(t)) y cos(θ4(t)) de una manera directa a partir de la matriz de modo que en vez de realizar cuatro adquisiciones de la matriz T correspondientes a los valores nx, ny, az y oz, dos divisiones, dos arco tangentes, dos cosenos y un producto, el computador solamente tendría realizar dos adquisiciones de los valores ny y oz y un producto:

))(sen()(

))(cos())(sen()(

))(cos())(cos()(

))(sen(

))(cos())(sen(

))(cos())(cos(

))(cos(

))(sen())(sen(

))(sen())(cos(

0

))(cos(

))(sen(

5543

5452

5451

5

54

54

5

54

54

4

4

tlltdp

ttltdp

ttltdp

ta

tta

tta

to

tto

tto

n

tn

tn

z

y

x

z

y

x

z

y

x

z

y

x

θθθθθ

θθθθθ

θθθθθ

θθ

⋅−−−=⋅⋅−=⋅⋅+=

−=⋅−=⋅+=

+=⋅−=⋅+=

=+=+=

))(cos())(cos()( 5451 ttlptd x θθ ⋅⋅−= zyx onlptd ⋅⋅−=→ 51 )(

De este modo hemos conseguido una expresión que nos relacione directamente

la variable articular d1(t) con las componentes de la matriz T. • Obtención de la variable articular d2(t):

Si observamos en la componente T24, vemos rápidamente que esta componente es la única de toda la matriz donde aparece el término de d2(t) y, por tanto, es de esta componente de donde conseguiremos la expresión que nos relacione la variable articular con los vectores de n, o, a y p:

))(cos())(sen()(

))(cos())(sen()(

5452

5452

ttlptd

ttltdp

y

y

θθ

θθ

⋅⋅+=↓

⋅⋅−=


variable articular bastaría con sustituir las relaciones obtenidas antes de θ4(t) y θ5(t) de forma análoga al caso anterior.

Esto parece lógico pero para simplificar más los cálculos para evitar que el

computador realice operaciones que pueden ser innecesarias, podemos obtener los valores de sen(θ4(t)) y cos(θ4(t)) de una manera directa a partir de la matriz de modo que en vez de realizar cuatro adquisiciones de la matriz T correspondientes a los valores nx, ny, az y oz, dos divisiones, dos arco tangentes, un seno y un coseno y un producto, el computador solamente tendría realizar dos adquisiciones de los valores nx y oz y un producto:

))(sen()(

))(cos())(sen()(

))(cos())(cos()(

))(sen(

))(cos())(sen(

))(cos())(cos(

))(cos(

))(sen())(sen(

))(sen())(cos(

0

))(cos(

))(sen(

5543

5452

5451

5

54

54

5

54

54

4

4

tlltdp

ttltdp

ttltdp

ta

tta

tta

to

tto

tto

n

tn

tn

z

y

x

z

y

x

z

y

x

z

y

x

θθθθθ

θθθθθ

θθθθθ

θθ

⋅−−−=⋅⋅−=⋅⋅+=

−=⋅−=⋅+=

+=⋅−=⋅+=

=+=+=

))(cos())(sen()( 5452 ttlptd x θθ ⋅⋅+= zxy onlptd ⋅⋅+=→ 52 )(


la variable articular d2(t) con las componentes de la matriz T.



3 - 9

• Obtención de la variable articular d3(t):

Si observamos en la componente T34, vemos rápidamente que esta componente es la única de toda la matriz donde aparece el término de d3(t) y, por tanto, es de esta componente de donde conseguiremos la expresión que nos relacione la variable articular con los vectores de n, o, a y p:

))(sen()(

))(sen()(

5543

5543

tllptd

tlltdp

z

z

θ

θ

⋅−−−=↓

⋅−−−=


variable articular bastaría con sustituir la relación obtenida antes de θ5(t) de forma análoga al caso anterior.

Esto parece lógico pero para simplificar más los cálculos para evitar que el

computador realice operaciones que pueden ser innecesarias, podemos obtener el valor de sen(θ4(t)) de una manera directa a partir de la matriz de modo que en vez de realizar dos adquisiciones de la matriz T correspondientes a los valores az y oz, una división, un arco tangente, un seno y un producto, el computador solamente tendría realizar una adquisición del valor az:

))(sen()(

))(cos())(sen()(

))(cos())(cos()(

))(sen(

))(cos())(sen(

))(cos())(cos(

))(cos(

))(sen())(sen(

))(sen())(cos(

0

))(cos(

))(sen(

5543

5452

5451

5

54

54

5

54

54

4

4

tlltdp

ttltdp

ttltdp

ta

tta

tta

to

tto

tto

n

tn

tn

z

y

x

z

y

x

z

y

x

z

y

x

θθθθθ

θθθθθ

θθθθθ

θθ

⋅−−−=⋅⋅−=⋅⋅+=

−=⋅−=⋅+=

+=⋅−=⋅+=

=+=+=

))(sen()( 5543 tllptd z θ⋅−−−= zz allptd ⋅+−−=→ 543 )(


la variable articular d3(t) con las componentes de la matriz T. Por tanto, tendríamos resuelto el problema de la cinemática inversa de nuestro

robot puesto que habríamos encontrado tantas funciones como grados de libertad tiene nuestro robot que relacionen las variables articulares con las componentes de la matriz de transformación homogénea T:

( )( )( )( )( )γβα


,,,,,

,,,,,

,,,,,

,,,,,

,,,,,

55

44

33

22

11

zyxfq

zyxfq

zyxfq

zyxfq

zyxfq

=====

RESOLUCIÓN DEL PROBLEMA DE LA CINEMÁTICA INVERSA

zyx onlptd ⋅⋅−= 51 )( zxy onlptd ⋅⋅+= 52 )( zz allptd ⋅+−−= 543 )(

=

y

x

n

nt arctg)(4θ

−=

z

z

o

at arctg)(5θ



3 - 10

3. IMPLEMENTACIÓN DEL CÓDIGO DE LA CINEMÁTICA INVERSA.

La función inversa resuelve la cinemática inversa del robot quirúrgico de 5 grados de libertad. Para ello toma como único parámetro la matriz de transformación homogénea T. Ha de tenerse en consideración que determinados puntos y orientaciones que pertenezcan al espacio de trabajo del robot, no podrán alcanzarse con algunas configuraciones. Por ello es recomendable tener en cuenta que, por ejemplo, todos los vectores de orientación deben ser ortonormales y que tampoco se excedan los valores de las guías de las articulaciones prismáticas:

1 function q = inversa(T) 2 3 l4=0.4; 4 l5=0.2; 5 6 % Inicialización de las variables articulares a calcular 7 q=[0 0 0 0 0]; 8 9 % Solución de las articulaciones

10 q(1)=T(1,4)-l5*T(2,1)*T(3,2); 11 q(2)=T(2,4)+l5*T(1,1)*T(3,2); 12 q(3)=-T(3,4)-l4+l5*T(3,3); 13 q(4)=atan2(T(1,1),T(2,1)); 14 q(5)=atan2(-T(3,3),T(3,2));

Como se puede observar, este método es muy general, pero tiene el

inconveniente de los signos de los ángulos, que debería eliminarse usando la función atan2() siempre que sea posible. En cualquier caso, el mayor inconveniente lo hemos salvaguardado pues no siempre tenemos por qué encontrar ecuaciones de las cuales podamos en cada paso despejar la variable que nos interesa para poder continuar. Aclaración:

Al programar las ecuaciones obtenidas, es necesario comprobar cuándo aparecen en los denominadores cantidades que puedan anularse y que hacen referencia a posiciones donde se producirían errores a la hora de calcular la cinemática inversa por lo que no sería válida la cinemática inversa pera cualquier matriz T. En ese caso, se debería buscar la fórmula para tratar ese caso en particular. Este problema aparece en las fórmulas relacionadas con los ángulos pues aparecen dividiendo ay y oz:

=

y

x

n

nt arctg)(4θ

−=

z

z

o

at arctg)(5θ

Para evitar eso podríamos hacerlo más sencillo aún puesto que mirando en la matriz encontramos otras posibilidades de las seis ya analizadas para relacionar θ4(t) y θ5(t) con las componentes de la matriz de transformación homogénea:

))(sen()(

))(cos())(sen()(

))(cos())(cos()(

))(sen(

))(cos())(sen(

))(cos())(cos(

))(cos(

))(sen())(sen(

))(sen())(cos(

0

))(cos(

))(sen(

5543

5452

5451

5

54

54

5

54

54

4

4

tlltdp

ttltdp

ttltdp

ta

tta

tta

to

tto

tto

n

tn

tn

z

y

x

z

y

x

z

y

x

z

y

x

θθθθθ

θθθθθ

θθθθθ

θθ

⋅−−−=⋅⋅−=⋅⋅+=

−=⋅−=⋅+=

+=⋅−=⋅+=

=+=+=



3 - 11

Por tanto para obtener una expresión que resuelva el ángulo θ4(t) podemos emplear cualquiera de las dos siguientes expresiones:

)arccos()())(cos(

)arcsen()())(sen(

44

44

yy

xx

nttn

nttn

=→+==→+=

θθθθ

Puesto que podemos elegir cualquiera de las dos, elegiremos la primera sin

problema pues cualquiera de las dos es válida. Con respecto al ángulo θ4(t), operamos de forma similar:

)arccos()())(cos(

)arcsen()())(sen(

55

55

zz

zz

otto

atta

+=→+=−=→−=

θθθθ

De igual modo, optamos por elegir la primera expresión. Si efectuamos estos

cambios en el código implementado, tenemos lo siguiente:

1 function q = inversa(T) 2 3 l4=0.4; 4 l5=0.2; 5 6 % Inicialización de las variables articulares a calcular 7 q=[0 0 0 0 0]; 8 9 % Solución de las articulaciones

10 q(1)=T(1,4)-l5*T(2,1)*T(3,2); 11 q(2)=T(2,4)+l5*T(1,1)*T(3,2); 12 q(3)=-T(3,4)-l4+l5*T(3,3); 13 q(4)=asin(T(1,1)); 14 q(5)=asin(-T(3,3));

Por tanto, tendríamos resuelto el problema de la cinemática inversa de nuestro

robot de una manera más eficaz que nos permite obtener la cinemática inversa de cualquier matriz T:

( )( )( )( )( )γβα


,,,,,

,,,,,

,,,,,

,,,,,

,,,,,

55

44

33

22

11

zyxfq

zyxfq

zyxfq

zyxfq

zyxfq

=====

RESOLUCIÓN DEL PROBLEMA DE LA CINEMÁTICA INVERSA

zyx onlptd ⋅⋅−= 51 )( zxy onlptd ⋅⋅+= 52 )( zz allptd ⋅+−−= 543 )(

( )xnt arcsen)(4 =θ ( )zat −= arcsen)(5θ



3 - 12

4. EJEMPLOS DE UTILIZACIÓN DE LA FUNCIÓN IMPLEMENTADA.

>> q=[–1 –1 0.5 –pi/6 –pi/3] q = –1.0000 –1.0000 0.5000 –0.5236 –1.0472 >> T=directa(q) T = –0.5000 –0.7500 0.4330 –0.9134 0.8660 –0.4330 0.2500 –0.9500 –0.0000 0.5000 0.8660 –0.7268 0.0000 0.0000 0.0000 1.0000 >> inversa(T) ans = –1.0000 –1.0000 0.5000 –0.5236 –1.0472

>> q=[–1 –1 0.5 –pi/6 pi/2] q = –1.0000 –1.0000 0.5000 –0.5236 1.5708 >> T=directa(q) T = –0.5000 0.8660 0.0000 –1.0000 0.8660 0.5000 0.0000 –1.0000 –0.0000 0.0000 –1.0000 –1.1000 0.0000 0.0000 0.0000 1.0000 >> inversa(T) ans = –1.0000 –1.0000 0.5000 –0.5236 1.5708

>> q=[-1.5 -1.5 0.5 pi/6 pi/2] q = –1.5000 –1.5000 0.5000 0.5236 1.5708 >> T=directa(q) T = 0.5000 0.8660 0.0000 –1.5000 0.8660 –0.5000 –0.0000 –1.5000 0.0000 0.0000 –1.0000 –1.1000 0.0000 0.0000 0.0000 1.0000 >> inversa(T) ans = –1.5000 –1.5000 0.5000 0.5236 1.5708

>> q=[–1.5 –1.5 0.5 pi/4 pi/4] q = –1.5000 –1.5000 0.5000 0.7854 0.7854 >> T=directa(q) T = 0.7071 0.5000 0.5000 –1.4000 0.7071 –0.5000 –0.5000 –1.6000 0.0000 0.7071 –0.7071 –1.0414 0.0000 0.0000 0.0000 1.0000 >> inversa(T) ans = –1.5000 –1.5000 0.5000 0.7854 0.7854



3 - 13

5. LA CINEMÁTICA INVERSA COMO VENTAJA: MATRIZ JACOBIANA INVERSA. La cinemática inversa de nuestro robot es muy sencilla puesto que tan sólo con

unas adquisiciones en memoria y operaciones algebraicas como sumas, restas o multiplicaciones y, también una operación de arcoseno se consigue obtener la cinemática inversa. Esto conlleva un gasto computacional más bajo con respecto a un robot con más grados de libertad.

Además la matriz Jacobiana inversa se puede obtener por un procedimiento

parecido al de la obtención de la Jacobiana directa, pero ahora partiendo del modelo cinemático inverso:

( ) ( ) ( )

( ) ( )γβαγβαγβαγβαγβα

,,,,,,,,,,

,,,,,,,,,,,,,,,

5544

332211

zyxfqzyxfq

zyxfqzyxfqzyxfq

=====

La matriz Jacobiana se obtendrá por diferenciación con respecto del tiempo de

ambos miembros de la igualdad:

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

=⇒

⋅=

−−

γθ

βθ

αθθθθ

γθ

βθ

αθθθθ

γβα

γβα

γβα

γ

β

α

θ

θ

555555

444444

333333

222222

111111

11

5

4

3

2

1

zyx

zyx

ddd

z

d

y

d

x

d

ddd

z

d

y

d

x

d

ddd

z

d

y

d

x

d

z

y

x

d

d

d

JJ

&

&

&

&

&

&

&

&

&

&

&

Si tomamos como ángulos de Euler α, β y γ como los ángulos que forma nuestro

vector a de aproximación con los vectores i, j y k de dirección de la base canónica, tenemos que los ángulos de los cosenos directores son los siguientes:

Ángulo de Euler α:

)))(cos())((arccos(cos))(cos())(cos()cos(

)cos(

5454 tttt

aiai

θθαθθα

α

⋅=⇒⋅=

⋅=⋅rrrr

Ángulo de Euler β:

)))(cos())(sen(arccos())(cos())(sen()cos(

)cos(

5454 tttt

ajaj

θθβθθβ

β

⋅−=⇒⋅−=

⋅=⋅rrrr

Ángulo de Euler γ:

)))(sen(arccos())(sen()cos(

)cos(

55 tt

akak

θγθγ

γ

−=⇒−=

⋅=⋅ rrrr



3 - 14

A continuación buscaremos las relaciones que existen en tre las variables articulares y los ángulos de Euler (que se trataría del proceso inverso que realizamos para obtener las relaciones entre los ángulos de Euler en función de las variables articulares):

[ ][ ]

[ ] [ ]

[ ]

+−=

+−=⇒−=

−=⇒+=

+=

+=⇒=+

⋅+=+

⋅+⋅=+

⋅−+⋅+=+

⋅+=

⋅−=

⋅=⋅−=

))(cos)((cos1arcsen)(

))(cos)((cos1))(sen())((cos1))(sen(

))((cos1))((sen))((sen))((cos1

)(cos)(cosarccos)(

)(cos)(cos))(cos())((cos)(cos)(cos

))((cos))((sen))((cos)(cos)(cos

))((cos))((sen))((cos))((cos)(cos)(cos

))(cos())(sen())(cos())(cos()(cos)(cos

))(cos())(cos()(cos

))(cos())(sen()(cos

))(cos())(cos()cos(

))(cos())(sen()cos(

225

2255

25

52

52

52

52

225

2255

222

52

42

4222

52

42

52

4222

254

254

22

254

2

254

2

54

54

βαθ

βαθθθ

θθθθ

βαθ

βαθθβα

θθθβα

θθθθβα

θθθθβα

θθα

θθβ

θθαθθβ

t

ttt

tttt

t

tt

ttt

tttt

tttt

tt

tt

tt

tt

Con este dato, podemos obtener el valor de la otra variable articular de rotación θ4:

+=

+=⇒+⋅=

+=

⋅=

+

−=

+

−=⇒+⋅−=

+=

⋅−=

)(cos)(cos

)cos(cosarc)(

)(cos)(cos

)cos())(cos()(cos)(cos))(cos()cos(

)(cos)(cos))(cos(

))(cos())(cos()cos(

)(cos)(cos

)cos(arcsen)(

)(cos)(cos

)cos())(sen()(cos)(cos))(sen()cos(

)(cos)(cos))(cos(

))(cos())(sen()cos(

224

224

224

225

54

224

224

224

225

54

βα

αθ

βα

αθβαθα

βαθ

θθα

βα

βθ

βα

βθβαθβ

βαθ

θθβ

t

tt

t

tt

t

tt

t

tt



3 - 15

Las otras articulaciones se obtienen de la siguiente manera:

)cos()(

))(cos)(cos)(cos)(cos

)cos()(

)(cos)(cos))(cos(

)(cos)(cos

)cos())(cos(

))(cos())(cos()(

))(cos())(cos()())(cos())(cos()(

51

22

2251

225

224

5451

54515451

α

βαβα

α

βαθ

βα

αθ

θθ

θθθθ

⋅−=

+⋅+

⋅−=⇒

+=

+=

⋅⋅−=

⋅⋅−=⇒⋅⋅+=

lxtd

lxtd

t

t

ttlxtd

ttlxtdttltdx

)cos()(

)(cos)(cos)(cos)(cos

)cos()(

)(cos)(cos))(cos(

)(cos)(cos

)cos())(sen(

))(cos())(sen()(

))(cos())(sen()())(cos())(sen()(

52

22

2252

225

224

5452

54525452

β

βαβα

β

βαθ

βα

βθ

θθ

θθθθ

⋅−=

+⋅+

−⋅+=⇒

+=

+

−=

⋅⋅+=

⋅⋅+=⇒⋅⋅−=

lytd

lytd

t

t

ttlytd

ttlytdttltdy

))(cos)((cos1)(

))(cos)((cos1)())(cos)((cos1))(sen(

))(sen()(

))(sen()())(sen()(

22543

2254322

5

5543

55435543

βα

βαβαθ

θ

θθ

+−⋅−−+−=

+−⋅−−+−=⇒

+−=

⋅−−+−=

⋅−−+−=⇒⋅−−−=

llztd

llztdt

tllztd

tllztdtlltdz

Hagamos un pequeño resumen de las expresiones obtenidas:

)cos()( 51 α⋅−= lxtd

)cos()( 52 β⋅−= lytd

))(cos)((cos1)( 22543 βα +−⋅−−+−= llztd

+=

)(cos)(cos

)cos(cosarc)(

224

βα

αθ t

+= )(cos)(cosarccos)( 22

5 βαθ t



3 - 16

A continuación, procedemos a obtener las derivadas necesarias para confeccionar la matriz Jacobiana inversa:

0

0

))sen(

0

0

1

))cos(

1

1

51

1

1

1

51

=∂∂

=∂∂

⋅+=∂∂

=∂

∂

=∂∂

⋅=∂

∂⋅−=

γ

β

ααα

α

d

d

ld

z

d

y

d

xx

d

lxd

&

&

0

))sen(

0

0

1

0

)cos(

2

52

2

2

2

2

52

=∂∂

⋅+=∂∂

=∂∂

=∂

∂

⋅=∂

∂

=∂

∂⋅−=

γ

βββ

α

β

d

ld

d

z

d

yy

d

x

d

lyd

&

&

0

))(sen)(sen

)cos()sen(

))(sen)(sen

)cos()sen(

1

0

0

))(cos)((cos1

3

22

3

22

3

3

3

3

22543

=∂∂

−

⋅+=

∂∂

−

⋅−=

∂∂

⋅−=∂

∂

=∂

∂

=∂

∂+−⋅−−+−=

γ

ββα

βββ

αβα

ααα

βα

d

d

d

zz

d

y

d

x

d

llzd

&

&

&

0

)(cos)(cos

)cos()sen(

)(cos)(cos

)cos()sen(

0

0

0

)(cos)(cos

)cos(cosarc

4

224

224

4

4

4

224

=∂

∂+⋅

−=∂∂

+

⋅+=

∂∂

=∂

∂

=∂

∂

=∂

∂

+=

γθ

ββα

αββθ

αβα

βααθ

θ

θ

θβα

αθ

&

&

z

y

x

0

)(cos)(cos)(cos)(sen

)cos()sen(


)cos()sen(

0

0

0

)(cos)(cosarccos

5

2222

5

2222

5

5

5

5

225

=∂∂

+−

⋅−=∂∂

+−

⋅+=

∂∂

=∂

∂

=∂

∂

=∂

∂

+=

γθ

ββααβ

ααβθ

αβαβα

αααθ

θ

θ

θ

βαθ

&

&

z

y

x

Por tanto, la matriz Jacobiana inversa sería:

⋅

+−

⋅−+−

⋅+

+⋅−

+

⋅+

−

⋅+−

⋅−−

⋅+

⋅+

=

γ

β

α

βααβ

αα

βαβα

ααβα

αββα

βαβα

ββ

βα

ααβ

α

θ

θ

&

&

&

&

&

&

&

&

&

&

&

z

y

x

l

l

d

d

d

0)(cos)(cos)(cos)(sen

)cos()sen(


)cos()sen(000

0)(cos)(cos

)cos()sen(

)(cos)(cos

)cos()sen(000

0))(sen)(sen

)cos()sen(

))(sen)(sen

)cos()sen(100

0))sen(0010

00))sen(001

22222222

2222

2222

5

5

5

4

3

2

1

Pero tal y como podemos observar hemos obtenido una columna llena de ceros

puesto que nuestro robot no podía llegar a tener cinco grados de libertad. Por ello eliminamos la última columna.



3 - 17

Llegamos al siguiente resultado

⋅

+−

⋅−+−

⋅+

+⋅−

+⋅

+

−

⋅+−

⋅−−

⋅+

⋅+

=

γ

β

α

βααβ

αα

βαβα

ααβα

αββα

βαβα

ββ

βα

ααβ

α

θ

θ

&

&

&

&

&

&

&

&

&

&

&

z

y

x

l

l

d

d

d


)cos()sen(


)cos()sen(000

)(cos)(cos

)cos()sen(

)(cos)(cos

)cos()sen(000

))(sen)(sen

)cos()sen(

))(sen)(sen

)cos()sen(100

))sen(0010

0))sen(001

22222222

2222

2222

5

5

5

4

3

2

1

)(cos)(sen))(cos)((cos

)sen()sen()(cos)cos()cos()(sen

222322

221

αββα

βααβαα

−+

⋅⋅−⋅⋅=−J

Al igual que en la Jacobiana directa podemos obtener cuando se anula el

determinante y comprobar que orientaciones provocarían que se anulase nuestra Jacobiana inversa:

0)(cos)(sen))(cos)((cos

)sen()sen()(cos)cos()cos()(sen0

222322

221 =

−+

⋅⋅−⋅⋅⇒=−

αββα

βααβααJ

El denominador no se puede anular puesto que si no no existiría matriz:

anularse. puede Nunca0))(cos)((cos

90)cos()sen()(cos)(sen0)(cos)(sen

23

22

2222

→≠+

±≠⇒±≠⇒≠⇒≠−

βα

αβαβαβαβ

[ ]

βαβαββ

αα

βαβαβαβααααα

βαβαααβααβαα

=⇒=⇒=

⋅=⋅⇒=⋅−⋅=⇒==⇒=

=⋅−⋅⋅=⋅⋅−⋅⋅

)()()cos(

)sen(

)cos(

)sen(

)sen()cos()cos()sen(0)sen()cos()cos()sen(

º270,º900)cos(

º360,º180,º00)sen(

0)sen()cos()cos()sen()cos()sen(

0)sen()sen()(cos)cos()cos()(sen 22

tgtg

Por tanto, no deberíamos trabajar con el robot para dichas orientaciones pues

cambios infinitesimales en los motores provocarían altísimas variaciones en los ángulos. Las orientaciones no deben dar nunca un ángulo α de 0º, 90º 180º, 270º ó 360º ni tampoco pueden ser iguales los ángulos α y β.

De hecho, la ventaja de la sencillez de la cinemática inversa es que gracias a esto

podemos obtener de forma analítica la matriz Jacobiana inversa y podemos buscar las orientaciones en las peores posiciones. Al igual que en la Jacobiana directa, podríamos derivar el determinante respecto α y β resolver el sistema y buscar las orientaciones óptimas donde más "cómodo" se encuentra el robot.



3 - 18

Si intentáramos encontrar los valores angulares para los cuales el valor absoluto del determinante de la matriz Jacobiana inversa es máximo, encontraríamos aquellas orientaciones para las cuales el robot trabaja más "cómodo" por decirlo de algún modo. Para pode minimizar la función del determinante necesitamos saber lo siguiente:

doptimalida de sCondicione

0

0

)(cos)(sen))(cos)((cos

)sen()sen()(cos)cos()cos()(sen

1

1

222322

221

⇒

=∂

∂

=∂

∂

−+

⋅⋅−⋅⋅=

−

−

−

β

α

αββα

βααβαα

J

J

J

REPRESENTACIÓN GRÁFICA DEL QUIROBOT


4 - 1

CCCaaapppííítttuuulllooo 444 RRREEEPPPRRREEESSSEEENNNTTTAAACCCIIIÓÓÓNNN GGGRRRÁÁÁFFFIIICCCAAA DDDEEELLL QQQUUUIIIRRROOOBBBOOOTTT

"UNA IMAGEN VALE MÁS QUE MIL PALABRAS" Refranero popular

En esta parte de la práctica se van a presentar unas sencillas, pero potentes herramientas gráficas válidas para comprobar los resultados anteriores utilizando las funciones ya estudiadas.

1. REPRESENTACIÓN GRÁFICA EN MatLab® USANDO ALAMBRES.

1. Explicaciones sobre el funcionamiento de la función de representación 3D.

Para comprobar los resultados del posicionamiento de nuestro robot se ha

utilizado una nueva función nombrada dibujardirecta.m, que permite una sencilla representación tridimensional utilizando la huella D-H del robot manipulador. El código que mostraremos a continuación consta de lo siguiente

• Líneas 6 – 18: Empleando la tabla con los parámetros de DH (líneas de la 7

a la 10) seremos capaces de calcular cada una de las matrices de transformación homogéneas que relacionan los sistemas consecutivos dos a dos.

• Líneas 20 – 23: A continuación, procedemos a calcular las matrices de transformación homogénea de cada sistema respecto del sistema de la base fija. Recordemos la necesidad de usar las matrices de cambio de base que empleábamos en el sistema del mundo y el del efector final.

• Líneas 26 – 36: Empleando las matrices de transformación del primer sistema al correspondiente tenemos información suficiente como para obtener las coordenadas de cada eslabón respecto al fijo. Una vez obtenidas todos los puntos los almacenamos en tres vectores x, y y z.

• Líneas 38 – 42: Dibujamos el robot visto desde el sistema de coordenadas de la base canónica debido a lo que ya explicamos en la cinemática directa. Si no hubiéramos multiplicado A01 = A00 * A01 hubiésemos cambiado plot(x, y, z) por plot(z, -y, x).



4 - 2

2. Implementación del código de la función de representación 3D. El código es el siguiente:

1 function dibujardirecta(q) 2 3 l4=0.4; 4 l5=0.2; 5 6 % Parámetros Denavit-Hartenberg del robot 7 teta = [ 0 pi/2 0 q(4) q(5) ]; 8 d = [ q(1) q(2) q(3) l4 0 ]; 9 a = [ 0 0 0 0 l5 ];

10 alfa = [ pi/2 -pi/2 0 pi/2 0 ]; 11 12 % Matrices de transformación homogénea entre sistemas de coordenadas consecutivos 13 A00 = [0 0 1 0; 0 -1 0 0; 1 0 0 0; 0 0 0 1]; 14 A01 = denavit(teta(1), d(1), a(1), alfa(1)); 15 A12 = denavit(teta(2), d(2), a(2), alfa(2)); 16 A23 = denavit(teta(3), d(3), a(3), alfa(3)); 17 A34 = denavit(teta(4), d(4), a(4), alfa(4)); 17 A45 = denavit(teta(5), d(5), a(5), alfa(5)); 18 A55 = [0 0 1 0; 0 -1 0 0; 1 0 0 0; 0 0 0 1]; 19 20 % Matrices de transformación del primer sistema al correspondiente 21 A01 = A00 * A01; 22 A02 = A01 * A12; 23 A03 = A02 * A23; 24 A04 = A03 * A34; 25 A05 = A04 * A45 * A55; 26 27 %Vector de posicion (x, y, z) de cada sistema de coordenadas 28 x0 = 0; y0 = 0; z0 = 0; 29 x1 = A01(1,4); y1 = A01(2,4); z1 = A01(3,4); 30 x2 = A02(1,4); y2 = A02(2,4); z2 = A02(3,4); 31 x3 = A03(1,4); y3 = A03(2,4); z3 = A03(3,4); 32 x4 = A04(1,4); y4 = A04(2,4); z4 = A04(3,4); 33 x5 = A05(1,4); y5 = A05(2,4); z5 = A05(3,4); 34 35 %Se dibuja el robot 36 x = [x0 x1 x2 x3 x4 x5]; 37 y = [y0 y1 y2 y3 y4 y5]; 38 z = [z0 z1 z2 z3 z4 z5]; 39 40 plot3(x,y,z): 41 % Se coloca una rejilla a los ejes 42 grid; 43 % Se establecen los límites de los ejes 44 axis([-2 2 -2 2 -2 2]);



4 - 3

3. Ejemplos de utilización de la función implementada.

>> q=[-1.5 -1 0.5 -pi/6 -pi/3] q = -1.5000 -1.0000 0.5000 -0.5236 -1.0472 >> dibujardirecta(q)

>> q=[-1 -1 0.5 -pi/6 -pi/3] q = -1.0000 -1.0000 0.5000 -0.5236 -1.0472 >> dibujardirecta(q)



4 - 4

2. ANIMACIÓN GRÁFICA EN MatLab® USANDO ALAMBRES.

1. Explicaciones sobre el funcionamiento de la función de representación 3D. A continuación, en este apartado, haremos uso de las funciones implementadas

hasta ahora inversa.m y dibujardirecta.m. Se trata de colocar al robot en dos posiciones distintas y animar una trayectoria recta entre dichas configuraciones. Para ello se deberán generar varias posiciones intermedias.

Utilizaremos la función planifica en la cual introduciremos las coordenadas

cartesianas de los puntos inicial y final y el número de puntos intermedios. Además de esta función, utilizaremos otra función llamada animación que representa las posiciones que ha ido tomando el robot para llegar de un punto a otro.

2. Implementación del código de la función de representación 3D: ANIMA.

El código implementado para esta función es el siguiente:

1 % ANIMA Animación de la trayectoria de un robot de 5 GDL 2 % en la matriz MAT_Q, de un robot de 5 GDL. MAT_Q contiene 4 filas 3 % y una columna para cada disposición del robot. 4 5 function anima(mat_q) 6 7 n=size(mat_q,2); 8 9 for i=1:n

10 q=mat_q(:,i); 11 dibujardirecta(q) 12 hold on; 13 grid; 14 end

Tal y como puede observarse, la función muestra por pantalla cada una de las configuraciones que ha adoptado el robot haciendo llamadas a la función dibujardirecta.m empleando como argumentos cada uno de los vectores articulares generados con la función planifica.m y que están almacenados en la matriz mat_q.

3. Implementación del código de la función de representación 3D: PLANIFICA.

Para poder calcular la matriz de coordenadas articulares calculamos

primeramente todos los puntos que componen el segmento de unión entre el punto final e inicial en tantos segmentos como npuntos hallamos incluido (para ello, en cada iteración le sumamos a p1 un segmento de longitud entre p1 y p2 dividida entre los npuntos en la dirección correspondiente). También realizaremos llamadas a la función inversa para que nos calcule cada vector articular.



4 - 5

El código es el siguiente 1 % PLANIFICA Planificación de trayects en línea recta del robot 5GDL 2 % Q = PLANIFICA(p1,p2,n,s,a,npuntos) devuelve las coords articulares 3 % correspondientes al instante actual de simulación en una planificación 4 % de trayectoria en línea recta entre dos puntos cartesianos. 5 % p1 Representa el punto de partida [p1x, p1y, p1z, 1] 6 % p1 Representa el punto de llegada [p2x, p2y, p2z, 1] 7 % n Representa el vector fila de normalidad n [nx ny nz 0] 8 % s Representa el vector fila de orientación s [sx sy sz 0] 9 % a Representa el vector fila de aproximación a [ax ay az 0]

10 % npuntos Representa la cantidad de puntos que deseamos intpolar 11 % planifica la trayectoria del extremo del robot entre 2 puntos en los que 12 % nos dan la posición y orientación del extremo (orientación igual en 13 % ambos puntos, y por lo tanto tb en los intermedios) 14 15 function mat_q=planifica(p1,p2,n,s,a,npuntos) 16 17 %defino mat_q 18 mat_q=zeros(5,npuntos+2); 19 20 %ahora calculo la distancia entre inicial y final, y luego la divido en el 21 %nº de espacios entre puntos intermedio, con lo q obtendo la distancia a 22 %sumar para pasar de uno a otro en 3d 23 u=p2-p1; 24 d=u/(npuntos+1); 25 26 for i=0:(npuntos+1) 27 %calculo de la posicion del extremo del robot en las "npuntos" 28 %posiciones intermedias 29 p=p1+i*d; 30 T=[n' s' a' p']; 31 %calculo de las coordenadas articulares de cada punto intermedio 32 q=inversa(T); 33 mat_q(:,i+1)=q'; 34 end 35 36 % ahora tendremos q llamar a la funcion anima para q dibuje el robot en 37 % cada una de las posiciones intermedias conociendo las coordenadas 38 % articulares en cada punto intermedio (almacenadas en "mat_q")



4 - 6

3. Ejemplos de utilización de la función implementada.

DINÁMICA DEL QUIROBOT


1 - 1

CCCaaapppííítttuuulllooo 555 DDDIIINNNÁÁÁMMMIIICCCAAA DDDEEELLL QQQUUUIIIRRROOOBBBOOOTTT

"LA FUERZA ES IGUAL A LA MASA POR LA ACECLERACIÓN" Sir Isaac Newton

La dinámica se ocupa de la relación entre las fuerzas que actúan sobre un cuerpo

y el movimiento que en él se origina. Así, en esta práctica nos dispusimos a establecer la relación entre el movimiento de nuestro robot y las fuerzas implicadas. Nuestro modelo dinámico relaciona matemáticamente la localización del robot (definida por sus variables articulares) y sus derivadas: la velocidad y aceleración, con las fuerzas y pares aplicados en las articulaciones, y con los parámetros dimensionales del robot: masas, longitudes, inercias... La dinámica de nuestro robot la vamos a estudiar desde dos puntos de vista opuestos, es decir vamos a estudiar su dinámica directa y su dinámica inversa.

La dinámica directa consiste en obtener los valores de posición, velocidad y

aceleración de cada una de las articulaciones de nuestro robot a partir de unos determinados pares y fuerzas aplicados a cada articulación.

Por otro lado, la dinámica inversa consiste en obtener los valores de pares y

fuerzas que necesitamos aplicar a cada articulación para que nuestro robot alcance unas determinadas posiciones articulares con unas determinadas velocidades y aceleraciones articulares.

1. DINÁMICA INVERSA.

Para la dinámica inversa utilizamos la formulación Newton-Euler. Se trata de un método que nos da los valores numéricos de pares y fuerzas requeridos para alcanzar unas posiciones, velocidades y aceleraciones determinadas del robot previamente seleccionadas (valores introducidos, en el espacio cartesiano).

La formulación Newton-Euler parte del equilibrio de fuerzas y pares:

)(..

wIwwITvmF ⋅⋅+⋅== ∑∑ , y el algoritmo al que conducen se basa en

operaciones vectoriales, y es computacionalmente muy eficiente: el número de operaciones a realizar depende directamente del número de articulaciones que tengamos.



1 - 2

La función que nos da la dinámica inversa a partir de este algoritmo es newtoneuler5.m, la cual calcula el vector 5x1 de pares/fuerzas de entrada a las articulaciones. Pasándole una serie de parámetros para su cálculo: Q es el vector 5x1 de coordenadas articulares. QP es el vector 5x1 que representa la velocidad de cada articulación. QPP es el vector 5x1 que indica la aceleración de cada articulación. G es el valor de la gravedad (m/s^2). M6 es la masa de la carga externa(Kg) que transporta el brazo robot. IEXTER es la matriz 3x3 de inercia de la carga exterior(Kg-m^2). Sin embargo estos dos últimos parámetros externos son nulos para nuestro robot, ya que en su diseño hemos decidido que el taladro que debe llevar en su extremo como última articulación quede representado internamente en nuestro robot como la longitud ‘l5’ a la que le hemos asignado una determinada masa y longitud para representarlo.

function tau = newtoneuler5(q,qp,qpp,g,m6,Iexter); l4=0.4; l5=0.2; r4=0.06; r5=0.04; % ------------------------------------------------------------ % Parámetros Denavit-Hartenberg del robot % ------------------------------------------------------------ teta = [0 pi/2 0 q(4) q(5)]; d = [q(1) q(2) q(3) 0.4 0]; a = [0 0 0 0 0.2]; alfa = [pi/2 -pi/2 0 pi/2 0]; % ------------------------------------------------------------ % Factores de posicionamiento de los centros de gravedad % ------------------------------------------------------------ factor1 = -0.5; factor2 = -0.5; factor3 = -0.5; factor4 = -0.5; factor5=-0.5; % ------------------------------------------------------------ % Masa de cada elemento (Kg) % ------------------------------------------------------------ m1 = 5; m2 = 5; m3 = 5; m4 = 0; m5=7; % m4=0, porque la articulación 4 está físicamente dentro de la articulación 3 (sus sistemas de coordenadas Denavit-Hartenberg son coincidentes), y m4 real se considera dinámicamente dentro de m5. % ------------------------------------------------------------ % Coeficiente de rozamiento viscoso de cada articulación % ------------------------------------------------------------ b1 = 0.06; b2 = 0.06; b3 = 0.06; b4 = 0.05; b5=0.05; %Las 3 primeras prismáticas tienen un mayor coeficiente de rozamiento por los carriles % ------------------------------------------------------------ % Matrices de Inercia (Kg-m^2) % ------------------------------------------------------------ r10I_r01 = zeros(3,3);



1 - 3

r20I_r02 = zeros(3,3); r30I_r03 = zeros(3,3);

r40I_r04 = [ 22 4)4(3

14)4(

2

1lmrm ⋅⋅+⋅⋅ 0 0;0 22 4)4(

3

14)4(

2

1lmrm ⋅⋅+⋅⋅ 0;0 0

0.0072];

r50I_r05 = 25)5(2

1rm ⋅⋅ 0 0;0 22 5)5(

3

15)5(

2

1lmrm ⋅⋅+⋅⋅ 0;0 0

22 5)5(3

15)5(

2

1lmrm ⋅⋅+⋅⋅ ];

% ------------------------------------------------------------ % Vectores ri0pi, ri0si. % ------------------------------------------------------------ r10p1 = ri0pi(a(1), d(1), alfa(1)); r20p2 = ri0pi(a(2), d(2), alfa(2)); r30p3 = ri0pi(a(3), d(3), alfa(3)); r40p4 = ri0pi(a(4), d(4), alfa(4)); r50p5 = ri0pi(a(5), d(5), alfa(5)); r60p6 = zeros(3,1); r10s1 = ri0si(a(1), d(1), alfa(1), factor1); r20s2 = ri0si(a(2), d(2), alfa(2), factor2); r30s3 = ri0si(a(3), d(3), alfa(3), factor3); r40s4 = ri0si(a(4), d(4), alfa(4), factor4); r50s5 = ri0si(a(5), d(5), alfa(5), factor5); r60s6 = zeros(3,1); % ------------------------------------------------------------ % Matrices de transformación % ------------------------------------------------------------ r01 = dh(teta(1), alfa(1)); r10 = r01'; r12 = dh(teta(2), alfa(2)); r21 = r12'; r23 = dh(teta(3), alfa(3)); r32 = r23'; r34 = dh(teta(4), alfa(4)); r43 = r34'; r45 = dh(teta(5), alfa(5)); r54 = r45'; r56 = eye(3); r65 = r56'; % ------------------------------------------------------------ % Velocidad angular de las articulaciones % ------------------------------------------------------------ r00w0 = zeros(3,1); r10w1 = r10*r00w0; r20w2 = r21*r10w1; r30w3 = r32*r20w2; r40w4 = ri0wi(r43, r30w3, qp(4)); r50w5 = ri0wi(r54, r40w4, qp(5)); r60w6 = ri0wi(r65, r50w5, 0); % ------------------------------------------------------------



1 - 4

% Aceleración angular de las articulaciones % ------------------------------------------------------------ r00wp0 = zeros(3,1); r10wp1 = r10*r00wp0; r20wp2 = r21*r10wp1; r30wp3 = r32*r20wp2; r40wp4 = ri0wpi(r43, r30wp3, r30w3, qp(4), qpp(4)); r50wp5 = ri0wpi(r54, r40wp4, r40w4, qp(5), qpp(5)); r60wp6 = ri0wpi(r65, r50wp5, r50w5, 0, 0); % ------------------------------------------------------------ % Aceleración lineal articular % ------------------------------------------------------------ r00vp0 = [-g; 0; 0]; r10vp1 = ri0vpi_p(r10, r00vp0, r10wp1, r10w1, r10p1, qp(1), qpp(1)); r20vp2 = ri0vpi_p(r21, r10vp1, r20wp2, r20w2, r20p2, qp(2), qpp(2)); r30vp3 = ri0vpi_p(r32, r20vp2, r30wp3, r30w3, r30p3, qp(3), qpp(3)); r40vp4 = ri0vpi_r(r43, r30vp3, r40wp4, r40w4, r40p4); r50vp5 = ri0vpi_r(r54, r40vp4, r50wp5, r50w5, r50p5); r60vp6 = ri0vpi_r(r65, r50vp5, r60wp6, r60w6, r60p6); % ------------------------------------------------------------ % Aceleración del centro de masa de cada elemento % ------------------------------------------------------------ r10a1 = ri0ai(r10vp1, r10wp1, r10w1, r10s1); r20a2 = ri0ai(r20vp2, r20wp2, r20w2, r20s2); r30a3 = ri0ai(r30vp3, r30wp3, r30w3, r30s3); r40a4 = ri0ai(r40vp4, r40wp4, r40w4, r40s4); r50a5 = ri0ai(r50vp5, r50wp5, r50w5, r50s5); r60a6 = ri0ai(r60vp6, r60wp6, r60w6, r60s6); % ------------------------------------------------------------ % Fuerza en el centro de masa de cada elemento % ------------------------------------------------------------ r60f6 = ri0fi(r60a6, m6); r50f5 = ri0fi(r50a5, m5); r40f4 = ri0fi(r40a4, m4); r30f3 = ri0fi(r30a3, m3); r20f2 = ri0fi(r20a2, m2); r10f1 = ri0fi(r10a1, m1); % ------------------------------------------------------------ % Par en el centro de masa de cada elemento % ------------------------------------------------------------ r60n6 = ri0ni(r60wp6, r60w6, Iexter); r50n5 = ri0ni(r50wp5, r50w5, r50I_r05); r40n4 = ri0ni(r40wp4, r40w4, r40I_r04); r30n3 = ri0ni(r30wp3, r30w3, r30I_r03); r20n2 = ri0ni(r20wp2, r20w2, r20I_r02); r10n1 = ri0ni(r10wp1, r10w1, r10I_r01);



1 - 5

% ------------------------------------------------------------ % Fuerzas articulares % ------------------------------------------------------------ r60f6a = r60f6; r50f5a = ri0fia(r56, r60f6a, r50f5); r40f4a = ri0fia(r45, r50f5a, r40f4); r30f3a = ri0fia(r34, r40f4a, r30f3); r20f2a = ri0fia(r23, r30f3a, r20f2); r10f1a = ri0fia(r12, r20f2a, r10f1); % ------------------------------------------------------------ % Pares articulares % ------------------------------------------------------------ r20p1 = r21*(r10p1); r30p2 = r32*(r20p2); r40p3 = r43*(r30p3); r50p4 = r54*(r40p4); r60p5 = r65*(r50p5); r60n6a = r60n6; r50n5a = ri0nia(r56, r60n6a, r60f6a, r50n5, r50f5, r60p5, r50p5, r50s5); r40n4a = ri0nia(r45, r50n5a, r50f5a, r40n4, r40f4, r50p4, r40p4, r40s4); r30n3a = ri0nia(r34, r40n4a, r40f4a, r30n3, r30f3, r40p3, r30p3, r30s3); r20n2a = ri0nia(r23, r30n3a, r30f3a, r20n2, r20f2, r30p2, r20p2, r20s2); r10n1a = ri0nia(r12, r20n2a, r20f2a, r10n1, r10f1, r20p1, r10p1, r10s1); % ------------------------------------------------------------ % Fuerzas y pares de accionamientos % ------------------------------------------------------------ t_1 = f_p(r10, r10f1a, qp(1), b1); t_2 = f_p(r21, r20f2a, qp(2), b2); t_3 = f_p(r32, r30f3a, qp(3), b3); t_4 = t_r(r43, r40n4a, qp(4), b4); t_5 = t_r(r54, r50n5a, qp(5), b5); tau = [t_1; t_2; t_3; t_4; t_5]; %es el vector de fuerzas y pares que nos devuelve la función.

La implementación del algoritmo Newton-Euler para nuestro robot en la función

anterior se basa en las características dimensionales y geométricas de nuestro robot, las cuales vienen reflejadas por la siguiente representación del mismo:

La parte cartesiana físicamente es como se muestra en la figura 5.1.



1 - 6

Figura 5.1: Esquema de un robot cartesiano.

Volumen del espacio de trabajo 1m3 (el cubo de la parte inferior). La parte

cartesiana X, Y del robot se encuentra en esta imagen situada en el punto origen (0,0), mientras que la Z está un poco desplazada de su origen.

La parte rotacional quedaría representada por el siguiente esquema:

Figura 5.2: Esquema de la parte rotacional.



1 - 7

La parte rotacional del robot se encuentra en su punto (0,0) en esta imagen. Sobre esas imágenes hemos basado cómo sería nuestro robot, estableciendo las dimensiones y formas geométricas que presentaría nuestro robot en su forma real.

En el código anterior hemos prefijado inicialmente ciertos parámetros que

vienen definidos por las dimensiones de nuestro robot, para después utilizarlos en operaciones posteriores. Así, hay que resaltar que a cada uno de nuestros eslabones le hemos asignado una masa en la que hemos tenido en cuenta los elementos mecánicos que lo componen (incluido el motor), quedando: m1 = m2 = m3 = 5 kg correspondientes a los respectivos patines de las tres articulaciones prismáticas iniciales. Mientras que m4 = 4 kg y m5 = 3 kg son las masas de los eslabones restantes, representando m5 a la masa del taladro. Sin embargo, en el código hemos introducido: m4 = 0 kg y m5 = 7 kg porque la articulación 4 está físicamente dentro de la articulación 3 (sus sistemas de coordenadas Denavit-Hartenberg son coincidentes), y m4 real se considera dinámicamente dentro de m5.

M1 M2 M3 M4 M5

5 kg 5kg 5kg 0 kg 7 kg

Además hemos fijado las longitudes de los eslabones finales como: l4 = 0.4 m y

l5= 0.2 m, considerándolos en nuestros esquemas de trabajo como cilindros de radio: r4 = 0.06 m, r5 = 0.04 m.

Los coeficientes de rozamiento de cada una de las articulaciones también los

hemos fijado inicialmente: b1 = 0.06; b2 = 0.06; b3 = 0.06; b4 = 0.05; b5 = 0.05; de forma que las articulaciones prismáticas tienen un mayor coeficiente de rozamiento debido a los carriles por los que se desplazan.

Los factores de centro de gravedad de los eslabones los hemos dejado por

simplificar al -0.5, indicando que el centro de masas se encuentra en el centro de cada eslabón o patín.

El primero de los cálculos de la función es obtener las matrices de inercia de

cada eslabón. Esta operación la hemos indicado en forma algebraica a partir de los parámetros dimensionales que hemos establecido para cada eslabón de nuestro robot (m, r y l). De forma que hemos calculado el momento de inercia de cada eslabón respecto a cada uno de sus ejes y respecto del sistema Denavit-Hartenberg anterior a dicho eslabón. Quedando cada matriz de inercia formada por una diagonal en la que introducimos los momentos de inercia respecto cada eje.

Para los tres primeros eslabones (los tres patines de las articulaciones

prismáticas) la matriz de inercia es nula, ya que el sistema D-H tiene su origen sobre el momento de masas y los tres ejes pasan por lo tanto por él. Mientras que para los dos eslabones finales obtenemos un resultado no nulo, ya que sus centros de masa no coinciden con el sistema D-H de la articulación anterior, sino que tenemos calcular los momentos de inercia aplicando el teorema de Steiner que se muestra a continuación.



1 - 8

2

22

22

2

13

1

2

13

1

2

1

RMI

LMRMI

LMRMI

DZ

DY

DX

⋅⋅=

⋅⋅+⋅⋅=

⋅⋅+⋅⋅=

Eslabón 4: 2

44

244

244

2

13

1

2

1

RMI

LMRMII

DZ

DYDX

⋅⋅=

⋅⋅+⋅⋅==

Eslabón 5 (taladro): 2

55

255

255

2

13

1

2

1

RMI

LMRMII

Dz

DyDx

⋅⋅=

⋅⋅+⋅⋅==

Cálculos a partir del eje Denavit-Hartenberg de la articulación anterior a cada

eslabón. Para el eslabón 4 hemos usado el D-H de la articulación 3, y para el eslabón 5 el D-H de la articulación 5. Se puede destacar que nuestro robot va a tener unos aspectos dinámicos muy buenos, debido a la estructura que presenta, ya que es como una mesa con cuatro patas ancladas al techo (o al suelo), transmitiendo los esfuerzos debidos a las masas e inercias a la estructura que lo sostiene de una manera muy efectiva. La geometría de nuestro robot ha permitido que los momentos de inercia de las tres primeras articulaciones prismáticas hayan resultado nulos.

A continuación se suceden las distintas operaciones del algoritmo, donde

aparecen llamadas a otras funciones: DH, RI0PI, RI0SI, RI0WI, RI0WIP, RI0VPI_R,

RI0AI, RI0FI, RI0NI, RI0FIA, RI0NIA, T_R, F_P; las cuales se encargan de realizar los cálculos intermedios del algoritmo de forma estructurada y por partes, para poder llegar a la solución final de forma clara y ordenada:

RI0WI: cálculo de las velocidades angulares de las articulaciones.

RI0WPI: cálculo de la aceleraciones angulares de las articulaciones.

DH: cálculo de las matrices de transformación.

RI0PI + RI0VPI_R o RI0VPI_P: cálculo de las aceleraciones lineales.

RI0SI + RI0AI: cálculo de las aceleraciones del centro de masa de cada elemento.

RI0FI: cálculo de las fuerzas en el centro de masas de cada elemento.

RI0NI: cálculo de los pares en el centro de masas de cada elemento.

RI0FIA: cálculo de las fuerzas articulares.

RI0NIA: cálculo de los pares articulares.

T_R: cálculo de los pares de accionamientos.

F_P: cálculo de las fuerzas de accionamientos.



1 - 9

Al tener nuestro robot las tres primeras articulaciones prismática y las dos siguientes rotacionales (5 en total), hemos obtenido ‘newtoneluler5.m’ modificando ‘newtoneluler4.m’, ya que el robot de 4 grados de libertad también presenta articulaciones prismáticas. Así, además de los parámetros iniciales característicos de nuestro robot, hemos modificado en el código las llamadas a las funciones que nos calculan la velocidad angular de las articulaciones, la aceleración angular de las articulaciones y la aceleración lineal articular, especificando en estos apartados de la función newtoneluler5.m cual de nuestras articulaciones es prismática y cual rotacional, para así llevar a cabo los cálculos de estos apartados de forma correcta en función del tipo de articulación.

Destacar que como el eje Denavit-Hartenberg de la primera articulación de

nuestro robot es:

, el vector de aceleración de la gravedad que hemos introducido en la función newtoneluler5.m es: [-g,0,0] , ya que actúa en el eje x negativo de nuestro robot.

De esta forma ya tenemos la dinámica inversa de nuestro robot expresada de

forma correcta.

2. DINÁMICA DIRECTA.

Para la dinámica directa utilizamos el tercer método de Walker-Orin. Este método nos permite resolver la ecuación general del robot:

τ=+++ kqKqGqqqCqqH T )()(),()(....

H(q):=matriz no singular de los momentos de inercia.

C(q, .

q ):=efecto de las aceleraciones centrifugas y de Coriolis. G(q):=efectos de la gravedad.

K(q):=matriz jacobiana. Definimos: kqKqGqqqCB T )()(),(

..

++= , de forma que la ecuación inicial se puede expresar como: BqqH −= τ

..

)( . Así, B se calcula a partir del algoritmo de Newton-Euler ya que τ=B para 0

..

=q , de forma que únicamente nos falta obtener H(q) para luego despejar qpp. Esta es la parte donde nos decantamos por el tercer método de Walter-Orin, que obtiene H a partir de las masas, centros de masas, momentos de inercia y aceleraciones del centro de masas de cada eslabón, aprovechando que la matriz H es diagonal. (Esta última parte la hará la función h5.m dentro de la función principal).



1 - 10

La función que nos da la dinámica directa a partir de este método es walkerorin5.m, la cual calcula la dinámica directa del robot de 4GDL devolviendo el vector 4x1 que representa la aceleración de cada articulación utilizando el tercer método de Walker y Orin. Q es el vector 4x1 de variables articulares. QP es el vector 4x1 que representa la velocidad de cada articulación. TAU es el vector 4x1 que representa el par de entrada a cada articulación. MASAEXT es la masa de la carga externa. INERCIAEXT es la inercia de la carga externa. (estos dos últimos parámetros tienen valor nulo en nuestro robot) function qpp = walkerorin5(q,qp,tau,masaext,inerciaext) % Se calcula el vector b. b = newtoneuler5(q,qp,zeros(5,1),9.8,masaext,inerciaext); % Se calcula la matriz de momentos de inercia H. H = h5(q,masaext,inerciaext); % Se calcula el vector de aceleración de cada articulación. qpp = inv(H)*(tau-b);

Vemos como no hemos logrado la independencia total de la dinámica directa de la inversa, al utilizar la función newtoneuler5.m dentro de walkerorin5.m.

La función h5.m utilizada dentro de walkerorin5.m calcula la matriz de momentos de inercia H 4x4 utilizando el tercer método de Walker y Orin. Q es el vector 4x1 de variables articulares. MASAEXT es la masa de la carga externa. INERCIAEXT es la inercia de la carga externa.(estos dos últimos son valores nulos para nuestro robot) function h = h5(q, masaext, inerciaext) l4=0.4; l5=0.2; r4=0.6; r5=0.04; % Parámetros Denavit-Hartenberg del robot teta = [0 pi/2 0 q(4) q(5) ]; d = [q(1) q(2) q(3) l4 0 ]; a = [0 0 0 0 l5 ] alfa = [pi/2 -pi/2 0 pi/2 0 ]; % Masa de cada elemento (Kg) m = [5; 5; 5; 0; 7]; % Matrices de Inercias Centroidales. (Kg-m^2.) J = [ 0 0 0; 0 0 0; 0 0 0; 0 0 0; 0 0 0; 0 0 0; 0 0 0; 0 0 0; 0 0 0;



1 - 11

22 4)4(3

14)4(

2

1lmrm ⋅⋅+⋅⋅ 0 0;0 22 4)4(

3

14)4(

2

1lmrm ⋅⋅+⋅⋅ 0;0 0 24)4(

2

1rm ⋅⋅ ;

25)5(2

1rm ⋅⋅ 0 0;0 22 5)5(

3

15)5(

2

1lmrm ⋅⋅+⋅⋅ 0;0 0 22 5)5(

3

15)5(

2

1lmrm ⋅⋅+⋅⋅ ];

% La quinta inercia es la de la carga externa. J(16:18,1:3) = inerciaext; % Vector Z0. z0 = [0; 0; 1]; % Condiciones de Carga Externa. M(6) = masaext; cj_1j = zeros(3,1); Ej_1j = J(16:18,1:3); for j = 5:-1:1 % Constante para sacar la Inercia J. k = (j-1)*3 + 1; % Vectores p y s. p = [a(j); d(j)*sin(alfa(j)); d(j)*cos(alfa(j))]; s = -0.5*p; % Matrices de transformacion. aj_1j = dh(teta(j),alfa(j)); ajj_1 = aj_1j'; % Centroide e Inercia de los elementos anteriores. cjjM1 = cj_1j; EjjM1 = Ej_1j; % Masa de todos los elementos anteriores. M(j) = M(j+1) + m(j); % Nuevo centroide. cjj = ((s + p)*m(j) + M(j+1)*(cjjM1 + p))/M(j); cj_1j = aj_1j*cjj; % Distancia de traslado de las inercias. p1 = (cjjM1 + p - cjj); d1 = dot(p1,p1)*eye(3) - (p1*p1'); p2 = (s + p - cjj); d2 = dot(p2,p2)*eye(3) - (p2*p2'); % Nueva Inercia. Ej_1j = aj_1j*(EjjM1 + M(j+1)*d1 + J(k:k+2,1:3) + m(j)*d2)*ajj_1; % Fuerza y par de los elementos j hasta N. if (j == 4) | (j == 5) % Articulaciones rotacionales



1 - 12

Fj_1j = cross(z0,M(j)*cj_1j); Nj_1j = Ej_1j*z0; else % Articulaciones prismáticas Fj_1j = M(j)*z0; Nj_1j = zeros(3,1); end % Elemento j. Componente H(j,j). fi_1i = Fj_1j; ni_1i = Nj_1j + cross(cj_1j, Fj_1j); if (j == 4) | (j == 5) % Articulaciones rotacionales h(j,j) = ni_1i(3,1); else % Articulaciones prismáticas h(j,j) = fi_1i(3,1); end % Elementos j-1 hasta 1. Componentes H(1:j-1,j) i = j - 1; while i >= 1 % Vector p. p = [a(i); d(i)*sin(alfa(i)); d(i)*cos(alfa(i))]; % Matrices de rotacion. ai_1i = dh(teta(i),alfa(i)); aii_1 = ai_1i'; % Fuerza y par del anterior elemento. fiiM1 = fi_1i; niiM1 = ni_1i; % Fuerza y par de este elemento. fi_1i = ai_1i*fiiM1; ni_1i = ai_1i*(niiM1 + cross(p, fiiM1)); % Componente H(i,j). if (i == 4) | (i == 5) % Articulaciones rotacionales h(i,j) = ni_1i(3,1); else % Articulaciones prismáticas h(i,j) = fi_1i(3,1); end % H es simetrica. h(j,i)= h(i,j); i = i - 1; end end



1 - 13

Hemos obtenido la función h5.m válida para nuestro robot de 5 grados de libertad a partir de la del robot de 4 grados de libertad, modificando los parámetros dimensionales pertinentes (masas, inercias,...) y diferenciando correctamente en los cálculos las articulaciones rotacionales de las prismáticas, ya que ciertos valores se operan de una forma u otra según el tipo de articulación.

Ya tenemos por lo tanto la función que nos simula la dinámica directa de nuestro robot: walkerorin5.m.

Una vez obtenidas las funciones que nos permiten obtener la dinámica directa e inversa de nuestro robot, un ejemplo que nos indica que su funcionamiento es correcto es el siguiente:

Se puede ver como al hacer la dinámica inversa y después la directa, el resultado obtenido es el de las aceleraciones utilizadas al inicio, luego las funciones funcionan correctamente, siendo una la opuesta de la otra.



1 - 14

NOTA

Para la parte cartesiana de nuestro robot hay empresas que proporcionan módulos de desplazamientos longitudinales como el de la imagen:

Una empresa que los vende es Adept, conocida por todos por las presentaciones de algunos de nuestros compañeros. Sin embargo en la implementación real de nuestro robot nos hemos decantado por el tipo de cartesiano que aparece en las explicaciones de la práctica, el cual es del mismo tipo del que hay en Torreblanca en el departamento de Automática:



1 - 15

Este tipo de robots no tiene actualmente aplicación médica, por lo que es una alternativa original a los típicos de 6GDL rotacionales. Al usar estos módulos ampliables de desplazamientos o las estructuras de mesa del último, disponen como gran ventaja frente a los rotacionales el amplio espacio de trabajo que pueden llegar a abarcar, desde un pequeño volumen de 1m^3 hasta una habitación entera, pudiendo llevar el motor a la parte de la sala donde se requiera de sus servicios o donde pueda ser limpiado, o reparado, o almacenado... Así, este ha sido uno de los factores que nos motivó a elegir este tipo de robots.

SERVOACCIONAMIENTOS


6 - 1

CCCaaapppííítttuuulllooo 666 SSSEEERRRVVVOOOAAACCCCCCIIIOOONNNAAAMMMIIIEEENNNTTTOOOSSS

"LA PACIENCIA ES EL MOTOR DE LA NATURALEZA" O.S.Marden

Una vez que tenemos nuestro robot correctamente representado cinemática y

dinámicamente, en esta práctica nos disponemos a elegir los actuadotes que requiere

nuestro robot para poder efectuar sus movimientos cumpliendo con sus especificaciones

cinemáticas y dinámicas. Utilizaremos para esta elección diversos criterios que se irán

explicando a medida que los vayamos empleando.

En primer lugar señalar que los actuadotes serán motores eléctricos DC

BRUSHLESS del los cuales tenemos un catálogo, y cuyo esquema en simulink tiene la

forma:

Figura 6.1: Esquema simulink de los actuadores eléctricos DC BRUSHLESS.

Donde se pueden ver los parámetros que lo identifican. Así, para cada una de

nuestras articulaciones necesitamos un motor de este tipo con unas características

concretas que lo definan.

Hay que decir que en las aplicaciones reales lo que se usan habitualmente son

motores de AC, que tienen unas características superiores a las de los DC.

En una implementación real de nuestro robot pasaríamos a calcular los motores

AC que requiere, pero en la práctica por sencillez trabajamos con motores DC.

SERVOACCIONAMIENTOS


6 - 2

1. REQUISITOS QUE DEBEN SATISFACER NUESTROS MOTORES.

Necesitamos por lo tanto conocer las características de los motores que requiere

nuestro robot para cumplir con las exigencias dinámicas, para ello simulamos el

comportamiento dinámico de cada articulación individualmente utilizando siguiente

esquema simulink:

Figura 6.2: Esquema simulink del control articular.

El esquema se basa en la dinámica inversa de nuestro robot, usando la función

dininv5gdl.m, que calcula el vector 4x1 de pares/fuerzas de entrada a las articulaciones

utilizando el método de Newton-Euler. ENTRADA(1:5) representa la posición de cada

articulación. ENTRADA(5:10) es la velocidad de cada articulación. ENTRADA(11:15)

es la aceleración de cada articulación. Y utilizamos también la función newtoneuler.m

de la práctica anterior.

function par = dininv5gld(entrada)

q = entrada(1:5); % Posición de cada articulación

qp = entrada(6:10); % Velocidad de cada articulación

qpp = entrada(11:15); % Aceleración de cada articulación

% Parámetros de la carga

masaext = 0;

inerciaext = [0 0 0;0 0 0;0 0 0];

% Se calcula el vector de pares/fuerzas utilizando Newton-Euler

par = newtoneuler5(q,qp,qpp,9.8,masaext,inerciaext);

SERVOACCIONAMIENTOS


6 - 3

El esquema simulink anterior nos permite conocer cuales son los requerimientos

en cuanto a pares y fuerzas que necesita cada una de las articulaciones para poder

moverse correctamente el robot según su cinemática y dinámica.Este estudio se hace

para la posición más crítica, donde estos requerimientos serán máximos, de forma que

basándonos en estos datos a la hora de elegir los motores tendremos la seguridad de que

serán los adecuados para el correcto funcionamiento de nuestro robot médico. De forma

que las masas que en la práctica anterior le asignamos a cada eslabón, puedan ser

desplazadas por estos motores, siendo así el modelo dinámicamente correcto.

Para obtener estos datos lo que hacemos con el esquema es introducir a cada

articulación (representada por su cinemática inversa) una entrada trapezoidal de

velocidad que simule el funcionamiento en arranque (0.1 seg), en régimen permanente

(0.4 seg) y en frenado (0.1 seg), con su correspondiente señal de aceleración. Así, a

partir de la función de dinámica inversa, obtendremos la representación gráfica de los

pares máximos que necesita cada articulación para su correcto funcionamiento.

Antes de poder obtener estas gráficas, necesitamos establecer que reductores

vamos a emplear para cada motor:

• Para las articulaciones rotacionales empleamos reductores de 500, ya que

suponemos se van a utilizar motores que trabajan a 5000 rpm y queremos

que la velocidad máxima de nuestras articulaciones sea de 10 rpm.

• En cuanto a las articulaciones prismáticas, que son las tres primeras, nuestros

criterios a la hora de elegir el reductor más apropiado han sido que la

velocidad máxima fuera de 1m/s, y el criterio de prueba y error hasta que nos

fuera posible utilizar el motor de gama más alta de los disponibles, ya que en

un principio ninguno de los disponibles cumplía con las exigencias de pares

nominales y de pico.

Finalmente, los reductores empleados han sido:

Articulación 1 Articulación 2 Articulación 3 Articulación 4 Articulación 5 60 45 95 500 500

Las gráficas de pares requeridos que hemos obtenido del esquema para cada

articulación son las que se muestran en la página siguiente.

Así pues, de las gráficas obtenemos los datos de pares máximos para cada

articulación:

Articulación 1 2 3 4 5

τ pico (Nm) 3.6665 3.6162 2.5789 7.4278·10-3

0.0299

τ nominal (Nm) 0.0246 1.3333·10-3

1.2385 1.0472·10-4

0.0138

Los cuales hay que multiplicarlos por un factor de seguridad de 1.5. Así,

finalmente obtenemos los requisitos que debe cumplir cada uno de nuestros motores:

Articulación 1 2 3 4 5

τ pico (Nm) 5.4998 5.4243 3.8684 11.142x10^-3 0.04485 τ nominal (Nm) 0.0369 19.99x10^-3 1.8577 1.5708x10^-4 0.0207

SERVOACCIONAMIENTOS


6 - 4

Articulación 1 (prismática)



Articulación 4 (rotacional)

Articulación 5 (rotacional)

Figura 6.3: Resultado de la simulación en Simulink.

Analizando estas gráficas y valores de pares, podemos ver como para las tres

articulaciones prismáticas el valor de pico en el arranque y frenado es muy alto; eso se

debe a que estas articulaciones son las de mayor masa, por lo tanto necesitan un alto par

motor para iniciar y parar su movimiento. Son las que sufren el mayor esfuerzo en el

movimiento del robot completo. También influye que sus valores de coeficiente de

rozamiento estático es de 0.06, superior al de las articulaciones rotacionales (0.05).

La tercera articulación prismática tiene un valor de par nominal muy superior al

de las otras, debido en parte porque su movimiento se realiza en el eje en que actúa la

gravedad, por lo que debe soportar aceleraciones y deceleraciones mayores. Las otra dos

articulaciones prismáticas necesitan un par nominal bajo, ya que una vez iniciado el

movimiento, sus requisitos dinámicos son menores.

SERVOACCIONAMIENTOS


6 - 5

Por último, las dos articulaciones rotacionales tienen unos valores muy

pequeños, ya que al tratase de las articulaciones del extremo sus masas y requisitos

dinámicos son menores al resto. En especial son muy bajos los valores de la articulación

4, ya que se trata de una rotacional cuyo eje de giro es el z cartesiano, y por lo tanto no

sufre la acción de la gravedad y sus requerimientos dinámicos son los mismos en

cualquier posición.

2. BÚSQUEDA DE MOTORES APROPIADOS EN EL CATÁLOGO.

Una vez conocidas las especificaciones que deben cumplir cada uno de los

motores. Del catálogo disponible de motores DC elegimos para las tres articulaciones

prismáticas el de gama más alta: DA42HBB-10 ya que es el único que cumple con el

par de pico requerido por las articulaciones 1 y 2, y con el par nominal requerido por la

articulación 3. Para las dos articulaciones rotacionales elegimos el motor de gama más

baja, que proporciona sobradamente los pares requeridos por estas: DB17CDB-10. Los

datos característicos de los motores seleccionados son:

DA42HBB-10 (prismáticas) Parámetros Símbolo Valor Resistencia R 0.6 Ω

Inductancia L 1.01 mH

Constante de par KT 0.155 Nm/A

Constante de voltaje KV 0.155 V/rad/s

Corriente máxima Imáx 38.7 A

DB17CDB-10 (rotacionales) Parámetros Símbolo Valor Resistencia R 6.9 Ω

Inductancia L 1.28 mH

Constante de par KT 0.035 Nm/A

Constante de voltaje KV 0.035 V/rad/s

Corriente máxima Imáx 3.6 A

Los cuales nos proporcionan la información necesaria de cada motor para poder

crear un diagrama de bloques en Simulink que represente correctamente su

funcionamiento:

Figura 6.4: Esquema simulink del motor.

Estos motores son por lo tanto los adecuados para nuestro robot médico, ya que

todo se ha calculado para los valores de tamaños, masas, longitudes, momentos de

inercia... concretos de nuestro robot. Podemos asegurar por lo tanto que mediante estos

motores nuestro robot podrá realizará sus movimientos cumpliendo con su dinámica y

cinemática establecida. En la práctica siguiente nos centraremos en diseñar el control

del robot para que sus movimientos sean correctos y precisos, algo muy importante en

la aplicación médica que se le va a dar.

SIMULACIÓN Y CONTROL DEL QUIROBOT


7 - 1

CCCaaapppííítttuuulllooo 777 SSSIIIMMMUUULLLAAACCCIIIÓÓÓNNN YYY CCCOOONNNTTTRRROOOLLL DDDEEELLL QQQUUUIIIRRROOOBBBOOOTTT

"EL CONTROL SOBRE SÍ MISMO ES LA FACULTAD DE SELECCIONAR LOS PENSAMIENTOS PARA CONVERTIR EN ACTOS SOLO CONVENIENTES"

Manero

En la práctica anterior hemos realizado la elección de los accionamientos

adecuados para nuestro robot, de forma que tenemos los distintos motores representados en bloques de simulink. Ahora utilizaremos esos resultados para llevar a cabo la correcta sintonización de los motores para que su funcionamiento sea el deseado.

Así, nos disponemos a llevar a cabo el sistema de control de nuestro robot. Para

ello vamos a tener en cuenta una serie de simplificaciones. Para facilitar el diseño, realizamos un control desacoplado, de forma que consideramos que el comportamiento de cada articulación es independiente de las demás. Las simplificaciones nos permiten centrarnos en el cálculo de un regulador PID:

sDs

IPGPID ⋅++= ,

para cada articulación de forma independiente, utilizando las técnicas que ya

conocemos para su diseño. Por lo tanto nuestro control dinámico se va a realizar en el espacio articular.

Pese a no ser este método fiel a la realidad al 100%, sus simplificaciones en

aplicaciones reales son válidas para un gran número de robots comerciales existentes, dando unos resultados razonablemente aceptables, aunque con limitaciones. 1. DISEÑO DE LOS PID'S.

Una vez decidido el uso de un PID en cada articulación, pasamos a su diseño teniendo en cuenta que este regulador presenta una acción proporcional (P), una acción diferencial (D) y una acción integral (I). La acción proporcional nos permite principalmente modificar el tiempo de establecimiento. Con la acción diferencial modificamos la sobreoscilación. Y mediante la acción integral disminuimos el error en régimen permanente.



7 - 2

Así, diseñamos para cada articulación su regulador PID mediante el método de Prueba-Error basándonos en:

Tiempo de

levantamiento Sobreoscilación

Tiempo de establecimiento

Error en rég. permanente

↑P Decrece Aumenta Pequeño cambio Decrece ↑I Decrece Aumenta Aumenta Elimina ↑D Pequeño cambio Decrece Decrece Pequeño cambio

Tabla 7.1: Variaciones de los parámetros y de las características del sistema. La tabla nos indica como varían ciertos parámetros de la respuesta del sistema,

ante aumentos de las variables P, I, D. El siguiente esquema simulink es el que nos permite estudiar el comportamiento

de cada articulación individualizada. En él introducimos una posición a la que queremos que se desplace dicha articulación y comprobamos gráficamente la respuesta del sistema hasta llegar a esa posición. Estudiando esa respuesta sabremos como ir modificando los parámetros del PID hasta obtener la respuesta deseada. También introducimos como entradas posiciones, velocidades y pares de todas las articulaciones, siendo nulos estos valores para el resto de articulaciones, ya que la simulación se centra en una sola articulación.

Figura 7.1: Diagrama de bloques en Simulink del control.

El sistema está formado, junto con ciertos bloques fijos, por el PID a diseñar, la función de dinámica directa obtenida de la práctica 4, y el subsistema del motor de la articulación correspondiente. Este subsistema motor hay que rellenarlo con los parámetros de motor correspondientes, obtenidos de la práctica anterior:

Figura 7.2: Parámetros del motor



7 - 3

A la hora de diseñar el PID, en el estudio de la señal de posición resultante del sistema nos hemos centrado principalmente en un aspecto: que la respuesta fuera sobreamortiguada (dejando en segundo lugar otros parámetros), para que en ningún momento se alcancen posiciones superiores a la final. Hemos considerado como crítico este factor dada la aplicación que va a tener nuestro robot, ya que en las operaciones médicas en las que lo vayamos a utilizar es de extrema importancia que nunca sobrepase la posición final que le hayamos indicado, con lo cual evitamos posibles colisiones con el paciente u otros elementos del entorno de trabajo que pudieran dar lugar a consecuencias dañinas principalmente para la integridad física del paciente. Así también aseguramos la precisión en la operación médica a realizar.

Los reguladores diseñados que nos han permitido obtener la mejor respuesta

para cada articulación han sido los siguientes ( sDsIPGPID ⋅++= ):

K I D PID 1 95 0 0.12

PID 2 98 0 0.12

PID 3 95 0 0.1

PID 4 55 0 0.29

PID 5 78 0 0

Tabla 7.2: Resultados de el factor proporcional, diferencia e integral obtenidos. En simulación parece lógico que I y D den valores muy cercanos a 0 o incluso 0

para que el comportamiento simulado sea el más correcto. Pero en la implementación real de estos PID habrá que añadirles una mayor acción diferencial para mejorar el régimen transitorio y una pequeña acción integral, ya que experimentalmente se ha demostrado que es necesaria una pequeña acción integral para garantizar que el diseño del regulador resulte en un sistema estable, anulando los errores en régimen permanente. Sin embargo, esta acción integral debe ser muy pequeña, ya que nos provoca sobreoscilación en el régimen transitorio, pudiendo hacer que nuestro motor se sature, lo cual siempre hay que evitar.

De esta forma el control PID permite anular un posible error en régimen

permanente, así como asegurar que finalmente las señales final y deseada coinciden, asegurando una alta precisión en la operación médica.

El diseño de los parámetros del PID también ha obedecido al mantenimiento de la respuesta sobreamortiguada tanto ante posición final cercana como ante posición final lejana.

Utilizando los PID anteriores, obtenemos las salidas deseadas. A continuación se muestran las salidas de posición para cada una de las articulaciones, tanto para una posición final cercana: 0.1 para prismáticas y 0.08 para rotacionales; como para una posición final lejana: 0.5 para prismáticas y 1 para rotacionales.



7 - 4

Articulación 1 (posición final 0.1m)






Figura 7.3: Salidas de la posición para cada articulación.



7 - 5

Articulación 4 (posición final 0.08rad)

Articulación 4 (posición final 1rad)

Articulación 5 (posición final 0.08rad)

Articulación 5 (posición final 1rad)

En las gráficas se puede observar claramente como nuestro principal objetivo se ha cumplido: que la señal nunca supere al valor final en el régimen transitorio. Y se puede ver como se alcanza correctamente el valor final deseado en el mínimo tiempo posible. Un parámetro de interés que se obtiene de las gráficas es el tiempo establecimiento (seg), que para cada caso es:

Articulación 1 (posición final 0.1m): 0.0145 Articulación 1 (posición final 0.5m): 0.025 Articulación 2 (posición final 0.1m): 0.016 Articulación 2 (posición final 0.5m): 0.0234 Articulación 3 (posición final 0.1m): 0.014 Articulación 3 (posición final 0.5m): 0.0224 Articulación 4 (posición final 0.08rad): 0.0357 Articulación 4 (posición final 1rad): 0.05 Articulación 5 (posición final 0.08rad): 0.0148 Articulación 5 (posición final 1rad): 0.038

Estos tiempos de establecimiento son los mínimos posibles que hemos conseguido para que el sistema se mantuviera subamortiguado en cada caso. Se puede apreciar que han sido muy bajos (<0.1seg), con lo que cumplimos otro aspecto muy positivo, ya que nuestro robot será capaz de llevar a cabo sus movimientos en tiempos muy cortos, lo cual en la aplicación médica para la que lo requerimos no es crítico, por lo que no lo hemos consideramos antes como un parámetro a minimizar en nuestros criterios del cálculo de los PID. Un motivo por el que estos tiempos de respuesta sean tan cortos es el tamaño de nuestros motores, ya que hay que recordar que en su elección en la práctica anterior utilizamos un factor de seguridad de 1.5, con lo cual podemos afirmar que están sobredimensionados, y cumplen con los requisitos dinámicos con creces.



7 - 6

2. SIMULACIÓN GLOBAL. La segunda parte de esta práctica consiste en hacer una simulación de la

evolución de las posiciones de cada una de las articulaciones a partir de un esquema simulink en el que agrupamos todo lo visto hasta ahora: cinemática, dinámica, planificación de trayectorias, accionamientos y control:

Figura 7.4: Diagrama de bloques en Simulink del control global.

Con este esquema realizamos una planificación de la trayectoria que debe seguir

el extremo del robot entre dos puntos del espacio cartesiano. Para cada punto de la trayectoria obtenemos aplicando la cinemática inversa las coordenadas articulares del robot. Así, una vez conocidas las posiciones articulares, mediante el PID antes diseñado y los bloques motores de la práctica anterior, obtenemos para cada articulación de forma desacoplada los pares y fuerzas que tiene que suministrar cada motor para que dinámicamente el robot pueda llevar a cabo los movimientos articulares de forma correcta con una determinada velocidad y aceleración hasta la posición correspondiente.

Haciendo esta simulación de la posición de las articulaciones para cada instante

de tiempo que dure la simulación, obtenemos una representación gráfica de la evolución de las coordenadas articulares desde el punto inicial al final en el espacio articular.

En los bloques PID introducimos los valores de los parámetros P, I, D

correspondientes a cada articulación. En los bloques motores introducimos los parámetros característicos de cada motor de cada articulación, igual que en el esquema simulink anterior a este. En el bloque ‘planificación de trayectorias’ introducimos la función planifica5.m:

Esta función devuelve las coordenadas articulares correspondientes al instante

actual de simulación en una planificación de trayectoria en línea recta entre dos puntos cartesianos.



7 - 7

Para ello divide la trayectoria en un número de segmentos que viene impuesto por los parámetros con los que se desea simular: tiempo de simulación e intervalo de integración. ENTRADA(1) representa el instante de tiempo actual (seg). ENTRADA(2:4) es la posición cartesiana inicial de la trayectoria. ENTRADA(5:7) es la posición cartesiana final de la trayectoria. function q = planifica5(entrada) t = entrada(1); % Instante actual de simulación (seg) p1 = entrada(2:4); % Posición cartesiana inicial p2 = entrada(5:7); % Posición cartesiana final ts = 1; % Tiempo de simulación (seg) intervalo = 1e-2; % Intervalo de integración (seg) % Número de segmentos de la trayectoria nseg = ts/intervalo; % Cálculo del vector unitario u = p2-p1; mu = sqrt(u(1)^2+u(2)^2+u(3)^2); u = (1/mu)*u; % Cálculo de la distancia entre puntos d = mu/nseg; % Número de punto actual en la trayectoria (el inicial es 0) i = t*nseg; % Cálculo de la posición cartesiana actual de la mano del manipulador ps = p1+(i*d)*u; % Cálculo de las coordenadas articulares q = directa_inversa(ps);

En el bloque ‘Cinemática inversa’ introducimos la función directa_inversa.m: Esta función devuelve el vector de coordenadas articulares correspondiente a la

solución cinemática inversa de la mano del manipulador en la posición y orientación expresadas en la matriz T para cada punto de la trayectoria planificada. Para mayor simplificación se hace que la orientación del extremo del robot permanezca fija.



7 - 8

function q = directa_inversa(entrada) % Posición cartesiana p = entrada'; % Orientación deseada en el extremo del robot n = [1 0 0]'; s = [0 0 -1]'; a = [0 1 0]'; % Matriz con la orientación y posición deseadas en el extremo del robot T = [n s a entrada]; % Se calculan las coordenadas articulares q = inversa(T);

Y en el bloque ‘Robot 5gdl’ introducimos la función dindir5gd.m: Esta función calcula el vector 4x1 de aceleración de cada articulación utilizando

el tercer método de Walker y Orin. ENTRADA(1:5) representa el par de entrada a cada articulación. ENTRADA(6:10) es la posición de cada articulación. ENTRADA(11:15) es la velocidad de cada articulación.

function qpp = dindir5gdl(entrada) tau = entrada(1:5); % Par de entrada a cada articulación q = entrada(6:10); % Posición de cada articulación qp = entrada(11:15); % Velocidad de cada articulación % Parámetros de la carga (nulos ya que el taladro ya lo incluimos en nuestro robot: l5) masaext = 0; inerciaext = [0 0 0;0 0 0;0 0 0]; % Se convierten los pares de la articulación 2 y 3 en fuerzas. n = 0.85; % Eficiencia husillo p = 0.025; % Paso del husillo (mm) tau(1) = 2*pi*tau(1)/p; tau(2) = 2*pi*tau(2)/p; tau(3) = 2*pi*tau(3)/p; % Se calcula la aceleración utilizando el método de Walker y Orin. qpp = walkerorin5(q,qp,tau,masaext,inerciaext);

( En cada una de estas funciones hemos utilizado funciones de la cinemática y dinámica de nuestro robot, obtenidas y explicadas en los capítulos anteriores.)

De esta forma, a través de todos estos bloques en los que hemos introducido funciones basadas en las prácticas anteriores, unimos cinemática-dinámica-control, obteniendo así un simulador final de nuestro robot.



7 - 9

Ya no solo nos simula la cinemática como en las primeras prácticas, sino que ahora se simula de forma correcta la dinámica y el control, siendo por lo tanto una modelización mucho más correcta del movimiento de nuestro robot, a través de la cual podemos estudia mejor su comportamiento en base a la aplicación médica que realizará.

Sin embargo, cuando nos dispusimos a simular el esquema, nos dimos cuenta que la respuesta del sistema era inestable, es decir, los valores que tomaron nuestras articulaciones no eran coherentes, y en algún caso tendían a infinito:

Figura 7.5: Respuesta del sistema.

Sin embargo en la primera parte de la práctica las respuestas de las

articulaciones eran correctas individualmente utilizando los mismos reguladores PID diseñados.

Por otra parte, la simulación nos dio inicialmente un error indicándonos que los

valores de inductancia de los motores son demasiado bajos, pero hemos puesto los valor de catálogo.

Las funciones que representan la dinámica directa e inversa: walkerorin5.m y

newtoneuler5.m, las hemos implementado correctamente, ya que su diseño y resultado en la práctica correspondiente fue positivo.

Así, hemos llegado a la conclusión de que el problema era que la señal de

referencia que le entra a nuestro robot, es decir, la que sale del bloque de planificación de trayectorias, es demasiado grande, ya que nuestros reguladores los hemos diseñado para entradas más pequeñas, y una entrada de valor final tan grande provoca que nuestras variables articulares sufran unas variaciones muy bruscas que no se corresponden con la realidad.

Por lo tanto la función planifica5.m no es correcta para nuestro robot.

Gráficamente se puede ver como las señales de referencia a cada articulación son demasiado grandes, lo que hace que la respuesta articular sea inestable.



7 - 10

El ejemplo más radical que hemos obtenido en el que se puede ver este efecto es el siguiente:

Señal de referencia:

Respuesta articular:

Figura 7.6: Respuesta del sistema.

Así pues nos propusimos obtener unas trayectorias articulares coherentes, donde la

respuesta dinámica y cinemática se tuvieran en cuenta, utilizando otra función de planificación. Para ello hicimos uso de las funciones que aparecían en la práctica optativa de ‘Planificación de Trayectorias’: sincronizador.m, calculocoef.m, planificador.m, evalpos.m, evalvel.m, evalacel.m. Mediante las cuales conseguimos hacer una interpolación de la trayectoria 4-3-4, obteniendo trayectorias articulares suaves al utilizar polinomios de orden 3 y 4.

(No entramos en detalle para cada función de las antes nombradas, dado que su

código no ha sufrido modificaciones importantes, sólo las referentes a velocidad, y se trata de las funciones utilizadas en una práctica optativa.)



7 - 11

Introducimos las características necesarias de nuestros motores en las funciones antes citadas, y procedemos a las obtención de la respuesta correcta utilizando los siguientes comandos para unas determinadas posiciones inicial y final:

Así, lo que el planificador hace es a partir de las funciones a las que llama

internamente, sincroniza los movimientos teniendo en cuenta las condiciones iniciales y finales, las características dinámicas de las articulaciones. A continuación obtiene unos polinomios como señal de velocidad que proporcionan curvas más suaves. A partir de esta señal se obtienen los valores de posición y aceleración para cada instante de tiempo para cada articulación; con ellos trabajará el planificador.



7 - 12

Finalmente las funciones de evaluación de posición, velocidad y aceleración a las que llama nos muestran como ha ido la correcta evolución de la posición de las coordenadas articulares, su velocidad y su aceleración:

Figura 7.7a: Perfiles de posición y velocidad.



7 - 13

Figura 7.7b: Perfiles de posición y velocidad.

A partir de las gráficas se puede observar cuales son las respuestas que desearíamos de nuestro sistema expresado en el bloque simulink al inicio de este apartado de la práctica. Se puede ver como las posiciones alcanzan correctamente su valor final de una forma progresiva y con movimientos suaves. Esto es debido a que la típica señal de velocidad trapezoidal ha sido sustituida por curvas formadas por polinomios de orden 3 y 4. A su vez vemos como las aceleraciones que se producen al inicio y final del movimiento articular no son picos que tienden a infinito, luego evitamos que nuestros motores pudieran saturar. Esta es la respuesta, como ya he dicho, que deseamos para nuestro robot, ya que en una aplicación médica como la que va a tener no queremos que sus movimientos sean bruscos. Esto puede asustar al paciente por un lado y ocasional impactos no previstos por otro, con los consiguientes daños personales o materiales. Por ello las curvas serán lo más suaves que se puedan en la implementación real de nuestro robot.

trabajo robótica 4º industriales

Documents