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.
ROBÓTICA QUIRÚRGICA
Jaime Martínez Verdú José Luís Aznar Gomis
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.)
ROBÓTICA QUIRÚRGICA
Jaime Martínez Verdú José Luís Aznar Gomis
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.
ROBÓTICA QUIRÚRGICA
Jaime Martínez Verdú José Luís Aznar Gomis
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.
ROBÓTICA QUIRÚRGICA
Jaime Martínez Verdú José Luís Aznar Gomis
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.
ROBÓTICA QUIRÚRGICA
Jaime Martínez Verdú José Luís Aznar Gomis
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.
ROBÓTICA QUIRÚRGICA
Jaime Martínez Verdú José Luís Aznar Gomis
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.
ROBÓTICA QUIRÚRGICA
Jaime Martínez Verdú José Luís Aznar Gomis
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.
ROBÓTICA QUIRÚRGICA
Jaime Martínez Verdú José Luís Aznar Gomis
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
Jaime Martínez Verdú José Luís Aznar Gomis
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.
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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º.
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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).
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
• Parámetros DH para ir desde el sistema S1 hasta el S2: (a2; α2; θ2; d2).
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
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
2 - 13
• Parámetros DH para ir desde el sistema S2 hasta el S3: (a3; α3; θ3; d3).
Figura 2.4c: Parámetros DH para pasar del sistema S2 al sistema S3.
• Parámetros DH para ir desde el sistema S3 hasta el S4: (a4; α4; θ4; d4).
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
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
2 - 14
• Parámetros DH para ir desde el sistema S4 hasta el S5: (a5; α5; θ5; d5).
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
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
)º90sen(0)º90cos()º90sen()º90cos()º90cos()º90sen(
)º90cos(0)º90sen()º90sen()º90sen()º90cos()º90cos(
º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
)º0sen(0)º0cos()º0sen()º0cos()º0cos()º0sen(
)º0cos(0)º0sen()º0sen()º0sen()º0cos()º0cos(
º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
))(sen(0))(cos())(sen(
))(cos(0))(sen())(cos(
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.
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
))(sen(0))(cos())(sen(
))(cos(0))(sen())(cos(
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
))(sen(0))(cos())(sen(
))(cos(0))(sen())(cos(
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
))(sen(0))(cos())(sen(
))(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
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
2 - 20
La matriz de transformación homogénea para nuestro robot presenta la siguiente forma:
⋅−−−−
⋅⋅−⋅−⋅−
⋅⋅+⋅⋅
=
1000
))(sen()())(sen())(cos(0
))(cos())(sen()())(cos())(sen())(sen())(sen())(cos(
))(cos())(cos()())(cos())(cos())(sen())(cos())(sen(
1000
554355
545254544
545154544
tlltdtt
ttltdttttt
ttltdttttt
paon
paon
paon
zzzz
yyyy
xxxx
θθθ
θθθθθθθ
θθθθθθθ
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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;
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
))(cos())(sen()())(cos())(sen())(sen())(sen())(cos(
))(cos())(cos()())(cos())(cos())(sen())(cos())(sen(
1000
554355
545254544
545154544
tlltdtt
ttltdttttt
ttltdttttt
paon
paon
paon
zzzz
yyyy
xxxx
θθθ
θθθθθθθ
θθθθθθθ
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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º
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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(
es.ortonormal esson vector Ambos11
)cos(
5454
54
5
54
54
tttt
tt
t
tt
tt
aj
aYj
ajaj
θθβθθβ
θθ
θ
θθ
θθ
β
β
⋅−=⇒⋅−=
⋅−=
−
⋅−
⋅
⋅⇒⋅=
⇒==
⋅=⋅
rr
rr
rrrr
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
2 - 26
Ángulo de Euler γ:
[ ]
)))(sen(arccos())(sen()cos(
))(sen(
))(sen(
))(cos())(sen(
))(cos())(cos(
100)cos(
es.ortonormal esson vector Ambos11
)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
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
)sen()sen()cos()cos(010
)sen()cos()cos()sen(001
θ
θ
θθ
θθ
θθ
θθθθ
θθ
θθ
θθθ
θθθθ
θθθθ
γ
β
α
&
&
&
&
&
&
&
&
&
&
&
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
)sen()sen()cos()cos(010
)sen()cos()cos()sen(001
θ
θ
θθ
θθ
θθ
θθθθ
θθ
θθ
θθθ
θθθθ
θθθθ
γ
β
α
&
&
&
&
&
&
&
&
&
&
&
d
d
d
l
ll
ll
z
y
x
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
)sen()sen()cos()cos(010
)sen()cos()cos()sen(001
θ
θ
θθ
θθ
θθ
θθθθ
θθ
θθ
θθθ
θθθθ
θθθθ
β
α
&
&
&
&
&
&
&
&
&
&
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)(cos1)(cos)(sen1
)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)(cos1)(cos)(sen1
)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.
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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)(cos1)(cos)(sen1
)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
)(cos)(cos1)(cos)(sen1
))(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
)(cos)(cos1)(cos)(sen1
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
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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 1: θ4 = 45º y θ5 = 35.2644º Configuración 8: θ4 = 135º y θ5 = 215.2564º
Configuración 3: θ4 = 45º y θ5 = 324.7356º Configuración 6: θ4 = 135º y θ5 = 144.7356º
Configuración 4: θ4 = 45º y θ5 = 215.2564º Configuración 5: θ4 = 135º y θ5 = 35.2644º
Configuración 10: θ4 = 225º y θ5 = 144.7356º Configuración 15: θ4 = –45º y θ5 = 324.7356º
Configuración 9: θ4 = 225º y θ5 = 35.2644º Configuración 16: θ4 = –45º y θ5 = 215.2564º
Configuración 11: θ4 = 225º y θ5 = 324.7356º Configuración 14: θ4 = –45º y θ5 = 144.7356º
Configuración 12: θ4 = 225º y θ5 = 215.2564º Configuración 13: θ4 = –45º y θ5 = 35.2644º
CINEMÁTICA DIRECTA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
Jaime Martínez Verdú José Luís Aznar Gomis
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.
CINEMÁTICA INVERSA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
CINEMÁTICA INVERSA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
CINEMÁTICA INVERSA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
CINEMÁTICA INVERSA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
⋅⋅=
⋅⋅⋅=
⋅=
CINEMÁTICA INVERSA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
3 - 6
La matriz de transformación homogénea para nuestro robot presenta la siguiente forma:
⋅−−−−
⋅⋅−⋅−⋅−
⋅⋅+⋅⋅
=
1000
))(sen()())(sen())(cos(0
))(cos())(sen()())(cos())(sen())(sen())(sen())(cos(
))(cos())(cos()())(cos())(cos())(sen())(cos())(sen(
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).
CINEMÁTICA INVERSA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
θ
θ
CINEMÁTICA INVERSA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
θθ
θθ
⋅⋅+=↓
⋅⋅−=
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 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 )(
De este modo hemos conseguido una expresión que nos relacione directamente
la variable articular d2(t) con las componentes de la matriz T.
CINEMÁTICA INVERSA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
θ
θ
⋅−−−=↓
⋅−−−=
Llegados a este punto podemos pensar que para obtener la expresión de dicha
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 )(
De este modo hemos conseguido una expresión que nos relacione directamente
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θ
CINEMÁTICA INVERSA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
θθθθθ
θθθθθ
θθθθθ
θθ
⋅−−−=⋅⋅−=⋅⋅+=
−=⋅−=⋅+=
+=⋅−=⋅+=
=+=+=
CINEMÁTICA INVERSA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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θ
CINEMÁTICA INVERSA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
CINEMÁTICA INVERSA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
CINEMÁTICA INVERSA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
CINEMÁTICA INVERSA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
CINEMÁTICA INVERSA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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)(cos)(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)(cos)(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.
CINEMÁTICA INVERSA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
3 - 17
Llegamos al siguiente resultado
⋅
+−
⋅−+−
⋅+
+⋅−
+⋅
+
−
⋅+−
⋅−−
⋅+
⋅+
=
γ
β
α
βααβ
αα
βαβα
ααβα
αββα
βαβα
ββ
βα
ααβ
α
θ
θ
&
&
&
&
&
&
&
&
&
&
&
z
y
x
l
l
d
d
d
)(cos)(cos)(cos)(sen
)cos()sen(
)(cos)(cos)(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.
CINEMÁTICA INVERSA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
Jaime Martínez Verdú José Luís Aznar Gomis
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).
REPRESENTACIÓN GRÁFICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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]);
REPRESENTACIÓN GRÁFICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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)
REPRESENTACIÓN GRÁFICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
REPRESENTACIÓN GRÁFICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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")
REPRESENTACIÓN GRÁFICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
4 - 6
3. Ejemplos de utilización de la función implementada.
DINÁMICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
DINÁMICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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);
DINÁMICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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); % ------------------------------------------------------------
DINÁMICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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);
DINÁMICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
DINÁMICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
DINÁMICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
DINÁMICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
DINÁMICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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).
DINÁMICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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;
DINÁMICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
DINÁMICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
DINÁMICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
DINÁMICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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:
DINÁMICA DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
Jaime Martínez Verdú José Luís Aznar Gomis
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
Jaime Martínez Verdú José Luís Aznar Gomis
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
Jaime Martínez Verdú José Luís Aznar Gomis
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
Jaime Martínez Verdú José Luís Aznar Gomis
6 - 4
Articulación 1 (prismática)
Articulación 2 (prismática)
Articulación 3 (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
Jaime Martínez Verdú José Luís Aznar Gomis
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
Jaime Martínez Verdú José Luís Aznar Gomis
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.
SIMULACIÓN Y CONTROL DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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
SIMULACIÓN Y CONTROL DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
SIMULACIÓN Y CONTROL DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
7 - 4
Articulación 1 (posición final 0.1m)
Articulación 1 (posición final 0.5m)
Articulación 2 (posición final 0.1m)
Articulación 2 (posición final 0.5m)
Articulación 3 (posición final 0.1m)
Articulación 3 (posición final 0.5m)
Figura 7.3: Salidas de la posición para cada articulación.
SIMULACIÓN Y CONTROL DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
SIMULACIÓN Y CONTROL DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
SIMULACIÓN Y CONTROL DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
SIMULACIÓN Y CONTROL DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
SIMULACIÓN Y CONTROL DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
SIMULACIÓN Y CONTROL DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.)
SIMULACIÓN Y CONTROL DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
SIMULACIÓN Y CONTROL DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.
SIMULACIÓN Y CONTROL DEL QUIROBOT
Jaime Martínez Verdú José Luís Aznar Gomis
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.