Desarrollo de sistema de control para un manipulador de ...

Memoria del Trabajo Fin de Maacutester realizado por

Fidel Peacuterez Meneacutendez

para la obtencioacuten del tiacutetulo de

Maacutester en Ingenieriacutea de Automatizacioacuten e Informaacutetica Industrial

Desarrollo de sistema de control para un manipulador de seis grados de libertad

Febrero 2014

2 Desarrollo de sistema de control para un manipulador de seis grados de libertad

Iacutendice

1 INTRODUCCIOacuteN 7

11 Estado actual de la roboacutetica 7

12 Caracteriacutesticas actuales de los robots 8 121 Causas de la falta de precisioacuten y continuidad de trayectorias 10

13 Objetivos del proyecto 11

14 Descripcioacuten del manipulador 12 141 Eslabones 13 142 Articulaciones 17 143 Compensadores 19

15 Estructura del Trabajo 19 151 Diagrama de Gantt 20

2 ESTUDIO CINEMAacuteTICO 22

21 Paraacutemetros Denavit-Hartenberg 22

22 Matrices de Transformacioacuten Homogeacuteneas 24 221 Fixed Angles X-Y-Z 26

23 Cinemaacutetica directa 27

24 Cinemaacutetica inversa meacutetodo geomeacutetrico 29

25 Cinemaacutetica de la Velocidad el Jacobiano 40 251 Caacutelculo del Jacobiano 42 252 Singularidades 44

3 ESTUDIO DINAacuteMICO 46

31 Distribucioacuten de la Masa del Manipulador 46 311 Centros de Masas 46

32 Pseudo-Matriz de Inercia 47

33 Dinaacutemica Inversa 49 331 Formulacioacuten Euler-Lagrange 50

3311 Velocidad de un punto del manipulador 51 3312 Energiacutea Cineacutetica 52 3313 Energiacutea Potencial 53 3314 Ecuaciones del Movimiento del Manipulador 54 3315 Introduccioacuten de los efectos de friccioacuten 56 3316 Dinaacutemica de los actuadores 57

Desarrollo de sistema de control para manipulador de seis grados de libertad 3

34 Dinaacutemica Directa 58

4 CONTROL 60

41 Generador de trayectorias 60 411 Modelado matemaacutetico 63

42 Sensores 66 421 Sensores electromagneacuteticos rotativos 66 422 Resolvers 67 423 Encoacuteder Incremental 69

43 Actuadores 70 431 Servomotores 71

4311 Motores Brushless 72 4312 Etapas reductoras 72

432 Drivers 73

44 Modelado del sistema de control 74 441 Linealizacioacuten 75 442 Control por par calculado 76

4421 Controlador PD 79 4422 Espacio de estados (PD) 80 4423 Eleccioacuten de las ganancias 83

443 Control por dinaacutemica inversa 84 4431 Controlador lineal PID 86 4432 Espacio de estados (PID) 88

444 Control Digital 91 4441 Ecuaciones en diferencias 92

445 Control en el Espacio Cartesiano 96 4451 Control con dinaacutemica en el espacio articular 96 4452 El jacobiano en el domino de fuerzas 101 4453 Control con dinaacutemica en el espacio cartesiano 102

446 Control de fuerza 104 4461 Control Hiacutebrido Posicioacuten-Fuerza 106

5 CAacuteLCULOS NUMEacuteRICOS 110

51 Regulador PID en tiempo continuo 110

52 Reguladores PID y PD en tiempo discreto 112

6 SIMULACIOacuteN 114

61 Simulacioacuten dinaacutemica 114

62 Simulacioacuten de trayectorias 115

63 Control por par calculado 117


64 Control por dinaacutemica inversa 122

7 CONCLUSIONES Y TRABAJOS FUTUROS 129

8 PRESUPUESTO 131

81 Hardware 131

82 Software 132

83 Costes de personal 132

84 Coste total del proyecto 133

9 BIBLIOGRAFIacuteA 134


MEMORIA



1 Introduccioacuten

El presente Trabajo Fin de Maacutester de Ingenieriacutea de Automatizacioacuten e Informaacutetica Industrial de la Universidad de Oviedo se ha elaborado durante la realizacioacuten de las Praacutecticas en Empresa en la Fundacioacuten PRODINTEC Este Trabajo forma parte del proyecto MILL2MATIC actualmente en estado de desarrollo y enmarcado dentro del Programa Nacional de cooperacioacuten PuacuteblicondashPrivada dentro del subprograma INNPACTO 2012

En este capiacutetulo se muestra el sistema general en el que se engloba el proyecto MILL2MATIC sus principales objetivos y la parte que este trabajo aporta a su desarrollo y consecucioacuten

Figura 1 Celda roboacutetica de la FUNDACION PRODINTEC

11 Estado actual de la roboacutetica

Para comprender el objetivo que se plantea en este proyecto es necesario conocer primero las posibilidades actuales de la roboacutetica especialmente la parte referida a manipuladores de seis grados de libertad muy extendidos en la industria

El campo de la roboacutetica industrial se define como el estudio disentildeo y uso de robots para la ejecucioacuten de procesos industriales El estaacutendar ISO 83731994 (Robots Industriales Manipuladores) define un robot industrial como un manipulador programable en tres o maacutes ejes multipropoacutesito controlado automaacuteticamente y reprogramable

En industrias como la de la automocioacuten donde se realizan muchas tareas repetitivas como operaciones de pintado la soldadura o la manipulacioacuten en las uacuteltimas deacutecadas


se ha producido la progresiva implantacioacuten de sistemas robotizados para cumplir dichas tareas Los robots desarrollados hasta el momento han mostrado ser capaces y eficientes sustituyendo y mejorando los resultados de labores que anteriormente se realizaban de manera manual o semiautomaacutetica

Sin embargo en tareas en las que la repetitividad no es el factor determinante por ejemplo en las que se exigen otro tipo de cualidades como la precisioacuten o la resolucioacuten de tareas complejas la roboacutetica no ha penetrado a un nivel comparable En tareas donde se fabrican piezas uacutenicas o series muy cortas con tolerancias estrechas (del orden de 01 mm) y trayectorias complejas hasta el momento las aplicaciones robotizadas no han pasado de instalaciones a nivel de laboratorio La automatizacioacuten de algunas de estas aplicaciones se consigue hoy en diacutea a traveacutes de maacutequinas herramienta como pueden ser los centros de mecanizado de alta velocidad o las maacutequinas de encintado automaacutetico (ATL) las cuales ofrecen soluciones con una gran robustez y fiabilidad consiguieacutendose una gran precisioacuten en el seguimiento de trayectorias

No obstante las maacutequinas-herramienta traen consigo una serie de desventajas como por ejemplo su elevado coste asociado propoacutesito especiacutefico baja flexibilidad y volumen elevado por lo que suelen necesitar de una gran superficie de planta para su instalacioacuten La roboacutetica dada su flexibilidad menor coste polivalencia y alta productividad se presenta como una alternativa ventajosa de automatizacioacuten para procesos como los anteriormente mencionados

De esta manera el objetivo general del proyecto MILL2MATIC es avanzar en la implementacioacuten de la roboacutetica en la industria acercaacutendola en mayor medida a procesos donde no se aplica actualmente mediante la superacioacuten de las limitaciones comentadas

De manera resumida las necesidades actuales y por tanto los retos a los que se enfrenta el proyecto global se pueden enunciar como

Obtencioacuten de una mayor precisioacuten de posicionamiento en los robots o manipuladores tanto de forma puntual como a lo largo de una trayectoria a fin de garantizar la fabricacioacuten de piezas con reducidas tolerancias

Mejora en la continuidad en las trayectorias a elevadas velocidades a fin de alcanzar productividades y acabados similares a las de una maacutequina herramienta

12 Caracteriacutesticas actuales de los robots

A partir de la definicioacuten y de las caracteriacutesticas mencionadas en el apartado anterior se pueden destacar como principales virtudes de la roboacutetica industrial actual la polivalencia y la flexibilidad Hablamos de dispositivos polivalentes porque son maacutequinas de propoacutesito general que deben ser programadas para una determinada tarea Su funcioacuten se determina ademaacutes de por la programacioacuten por la herramienta utilizada para llevar a cabo la tarea Su flexibilidad reside en que la programacioacuten y el cambio de herramienta pueden cambiar a lo largo de su vida uacutetil con una simple


reconfiguracioacuten de sus paraacutemetros A estas virtudes hay que sumar la repetitividad que presentan en la posicioacuten alcanzada (en torno a 01 mm de margen) Por uacuteltimo cabe destacar la alta productividad que consiguen gracias a que la ligereza de la estructura posibilita movimientos muy veloces

En cuanto a las limitaciones las maacutes importantes se centran en la dificultad de programacioacuten en la baja precisioacuten de posicionamiento alcanzable y en la falta de continuidad en trayectorias de gran densidad de puntos Respecto a la falta de precisioacuten cabe mencionar que es debida principalmente a la elasticidad de su estructura que produce deflexiones importantes difiacuteciles de modelar de forma precisa Otra causa es el control que no utiliza un sistema de lazo cerrado en el que se realimente con un sistema de medicioacuten externo la posicioacuten de la herramienta sino que por el contrario son los resolver de los motores los que controlan la posicioacuten de las articulaciones lo que por definicioacuten no permite alcanzar altas precisiones En cuanto a la continuidad en las trayectorias eacutesta resulta totalmente satisfactoria en el caso de trabajos a baja velocidad Sin embargo cuando se trata de aumentar la velocidad de operacioacuten el control no es capaz de mantener la suavidad en el movimiento debido a que no tiene un tiempo de respuesta lo suficientemente corto y en menor medida a problemas mecaacutenicos como holguras imprecisiones en la fabricacioacuten etc El hecho de que se trate de un sistema de baja precisioacuten pero alta repetitividad se debe a que cuando el robot es enviado a un punto objetivo y a una orientacioacuten dada el control hace los caacutelculos necesarios para que cada eje se posicione hasta llegar a esa posicioacuten y orientacioacuten real calculada que tiene una desviacioacuten respecto al objetivo Habitualmente no existe una medida directa de la posicioacuten y orientacioacuten del efector final uno debe confiar en la geometriacutea asumida del manipulador y en su rigidez para calcular la posicioacuten del efector final a partir de las posiciones de las articulaciones medidas De este modo la precisioacuten se ve afectada por errores computacionales errores de fabricacioacuten flexioacuten de los eslabones backlash en las unidades de reduccioacuten etc Este error resulta ser aproximadamente constante por lo que cada vez que el robot va a un punto lo haraacute desviaacutendose del punto objetivo una distancia igual al valor de la precisioacuten pero siempre caeraacute dentro de una esfera de radio el valor de la repetitividad Es decir es probable que en x movimientos del robot a un punto objetivo determinado nunca estemos sobre dicho punto objetivo pero siempre tendremos la certeza de que la posicioacuten real del robot se encuentra dentro de una esfera de radio RP marcada por la repetitividad siendo la precisioacuten absoluta AP el valor de la distancia entre el punto objetivo y el centro de esa esfera de radio RP Todos estos conceptos pueden verse representados en la Figura 2 a continuacioacuten


Figura 2 Concepto de precisioacuten y repetitividad

En cuanto a la falta de continuidad de las trayectorias eacutesta estaacute motivada por una actuacioacuten de los motores maacutes lenta que la que demanda para el movimiento del manipulador Este fenoacutemeno se materializa en forma de microparadas en cada uno de los puntos que forman la trayectoria que generan vibraciones y otras perturbaciones que afectan al proceso de fabricacioacuten Por ejemplo en el caso del mecanizado el acabado de las superficies teoacutericamente lisas resulta deficiente debido a la elevada rugosidad y a la presencia de muescas

A la vista de estas limitaciones se puede entender por queacute las aplicaciones habituales de los robots en la industria son aquellas que se caracterizan por ser sencillas repetitivas y no requerir precisioacuten o que debido a la alta productividad es asumible un tiempo de programacioacuten y puesta a punto elevado Se justifica asiacute la baja implantacioacuten en aplicaciones con altos requisitos de tolerancia o con necesidad de trayectorias complejas

121 Causas de la falta de precisioacuten y continuidad de trayectorias

En cuanto a la precisioacuten y a la continuidad de trayectorias se ahondaraacute en este apartado en las causas por las que estos aspectos son limitantes a la hora de utilizar manipuladores roboacuteticos en tareas en las que se precisan dichas caracteriacutesticas Las principales causas son

Sensoacuterica las desviaciones atribuibles a la sensoacuterica integrada en los sistemas roboacuteticos actuales tiene una implicacioacuten directa en el correcto funcionamiento del mismo ya que son los elementos que devuelven los valores utilizados para el posicionamiento del robot o manipulador Este tipo de defectos se pueden corregir mediante calibraciones Las desviaciones derivadas de la sensoacuterica pueden venir de

Precisioacuten del sensor capacidad que tiene el sensor de tener una precisioacuten interna adecuada es decir de su repetitividad y de su capacidad de transformar la medicioacuten de dichos paraacutemetros en medidas digitales

Resolucioacuten limitacioacuten del sensor seguacuten su caracteriacutesticas mecaacutenicas constitutivas

Excentricidad defectos asociada al montaje del sensor en el sistema Influencias externas errores que proceden de influencias externas

como pueden ser cargas adicionales temperaturas interferencias etc

Control es el responsable del correcto funcionamiento y movilidad del sistema y por esta razoacuten la mayor fuente de desviaciones Las desviaciones de las que es fuente el control se pueden englobar en

Transformaciones Matemaacuteticas Todo el funcionamiento del manipulador es controlado a grandes rasgos por algoritmos matemaacuteticos que representan su funcionamiento y que ante


determinados valores de entrada ofrecen los valores de salida programados Estos algoritmos que representan no solo el funcionamiento cinemaacutetico y dinaacutemico sino tambieacuten la manera como el robot se mueve en espacio y controla sus actuadores estaacuten disentildeados en base a desarrollos matemaacuteticos que tienen errores asociados y altos costes computacionales

Precisioacuten computacional El hardware disentildeado para poder albergar toda esta matemaacutetica trabaja con un determinado tipo de variable limitadas en espacio y tamantildeo

Capacidad computacional En funcioacuten de la matemaacutetica desarrollada y de la precisioacuten requerida se desarrolla el hardware que tenga la capacidad operativa de realizar las operaciones para poder enviar las sentildeales a la actuacioacuten En realidad el hardware tiene retrasos y sumados a los ya mencionados antes se obtiene el tiempo total de refresco o la capacidad del sistema para actuar frente a determinados estiacutemulos Este tiempo de refresco impone el liacutemite de actuacioacuten del control y por consiguiente la capacidad del sistema para poder moverse con una precisioacuten determinada

Mecaacutenica La constitucioacuten mecaacutenica de un robot y sus accionamientos son tambieacuten una de las fuentes principales de peacuterdida de precisioacuten En este sentido las deformaciones que sufren los eslabones del robot en su movimiento provocan que exista una diferencia entre la definicioacuten matemaacutetica teoacuterica de funcionamiento y su comportamiento real Del mismo modo los actuadores tienen un liacutemite de operacioacuten vieacutendose afectados por factores externos por lo que pierden resolucioacuten en funcioacuten de los mismos paraacutemetros que la mecaacutenica

Instalacioacuten La puesta en marcha de uno de estos equipos supone un reto desde el punto de vista de la precisioacuten del mismo ya que muchos de los defectos asociados a un robot se crea en ese momento Los errores de posicionamiento y la falta de una calibracioacuten en la instalacioacuten final hacen de esta fase una fuente de peacuterdida de precisioacuten en su funcionamiento

Se identifican por tanto el control del robot y la sensoacuterica integrada como los factores limitantes fundamentales de cara a mejorar tanto la precisioacuten absoluta del robot (capacidad para situarse en un punto determinado dentro del volumen de trabajo) como en la continuidad en trayectorias

13 Objetivos del proyecto

El objetivo general del proyecto MILL2MATIC consiste en la mejora de las capacidades actuales de los manipuladores o robots industriales de manera que puedan presentarse como una alternativa a las maacutequinas herramienta para determinados procesos donde las tolerancias estrechas y las trayectorias complejas hacen que la roboacutetica actual no sea una alternativa factible


La consecucioacuten de este objetivo general implica el cumplimiento de los siguientes objetivos especiacuteficos que representan una mejora con respecto a los sistemas actuales

Desarrollo de un sistema de control con nuevas capacidades que tenga en cuenta el origen de las desviaciones (mecaacutenicas externas matemaacuteticas etc) que dan lugar a la falta de precisioacuten actual de los robots a traveacutes de su implementacioacuten en un hardware especiacutefico

Integracioacuten de sistemas de medicioacuten externos basados en interferometriacutea laacuteser que no esteacuten en contacto con la mecaacutenica del robot y que permitan un mejor posicionamiento tanto estaacutetico como dinaacutemico del conjunto gracias al cierre del lazo de control de posicionamiento de la herramienta Se busca conseguir asiacute una mayor precisioacuten y una mejora en la continuidad de las trayectorias

El resultado final esperado es la obtencioacuten de un sistema de control que mejore la precisioacuten y exactitud del posicionamiento del efector final Esa mejora vendraacute dada por un menor periodo de funcionamiento del lazo de control por el uso de modelos matemaacuteticos maacutes completos del manipulador y por la adicioacuten de sensoacuterica que proporcione otro tipo de datos que permitan mejorar el sistema de control

El objetivo de este Trabajo Final de Maacutester englobado dentro de los objetivos del proyecto Mill2Matic seraacute la propuesta y realizacioacuten de lazos de control que permitan llevar a cabo las mejoras propuestas anteriormente

14 Descripcioacuten del manipulador

Un manipulador roboacutetico es un conjunto de cuerpos conectados en una cadena por medio de juntas o articulaciones Estos cuerpos se denominan enlaces o eslabones Cada una de las articulaciones une a dos eslabones consecutivos En los manipuladores pueden existir dos tipos de articulaciones la prismaacutetica y la rotatoria Una articulacioacuten prismaacutetica tambieacuten conocida como junta deslizante posibilita a un eslaboacuten el movimiento lineal sobre el otro eslaboacuten que conecta la articulacioacuten Una articulacioacuten rotatoria permite el giro un eslaboacuten con respecto al otro eslaboacuten que conecta la articulacioacuten En el caso del manipulador KUKA KR500-2 todas las articulaciones que lo conforman son de tipo rotatorio Cada una de ellas a su vez establece cada uno de los grados de libertad por lo que el manipulador tiene 6 articulaciones Este modelo de manipulador es un modelo destinado a cargas muy pesadas lo que hace de eacutel un sistema especialmente adecuado para su utilizacioacuten en procesos de mecanizado Ademaacutes ofrece una capacidad de carga y un espacio de trabajo mucho mayor y prolonga considerablemente las distancias entre los puntos de gravedad de referencia Ademaacutes posee una alimentacioacuten de energiacutea estrechamente centildeida al cuerpo o directamente integrada lo cual le permite reducir los contornos de perturbacioacuten de manera que la superficie de colocacioacuten del robot resulta mucho maacutes aprovechable


141 Eslabones

En las siguientes figuras se separa la estructura del manipulador para diferenciar las partes fundamentales que lo componen

Figura 3 Brazo del manipulador

Figura 4 Muntildeeca del manipulador

La figura 3 constituye el brazo antropomoacuterfico del manipulador y es la responsable de su posicionamiento en el espacio mientras que la figura 4 constituye la muntildeeca del sistema y supondraacute la orientacioacuten final del sistema de coordenadas del extremo del robot (efector final) Realmente la posicioacuten y orientacioacuten alcanzadas finalmente no dependeraacuten uacutenicamente de las partes respectivas mencionadas sino que seraacute un conjunto de ambas ya que la muntildeeca posiciona el efector final en funcioacuten del posicionamiento previo del brazo Lo mismo sucede con el brazo respecto de la muntildeeca Tanto el brazo como la muntildeeca forman un conjunto que es el manipulador y deben coordinarse conjuntamente para posicionar y orientar el efector final

Cada una de las dos partes estaraacute dividida a su vez en eslabones conectados a traveacutes de articulaciones rotativas Cada una de ellas se muestra a continuacioacuten de manera independiente del resto del manipulador resaltada en color amarillo frente al resto de eslabones


El primer eslaboacuten se mostraraacute en las dos partes que lo componen ya que estaacute compuesto por dos piezas que forman el conjunto que se estudia en relacioacuten al movimiento de la articulacioacuten La primera de ellas es la base

Figura 5 Base (primer eslaboacuten)

En ella se encuentra el sistema de referencia del manipulador Es una parte fija sobre la que se ancla el manipulador al suelo por ello el sistema de referencia estaacute situado sobre ella y es el punto al que se refieren los datos de posicioacuten y orientacioacuten del efector final en lo que se denominaraacute espacio cartesiano La segunda pieza que conforma el primer eslaboacuten se muestra en la siguiente figura

Figura 6 Primer eslaboacuten


Esta pieza es la que rota en torno a la base formando la primera articulacioacuten El segundo eslaboacuten es el siguiente

Figura 7 Segundo eslaboacuten

Este segundo eslaboacuten estaacute formado por una uacutenica pieza al igual que el resto de los eslabones siguientes El tercer eslaboacuten se muestra a continuacioacuten

Figura 8 Tercer eslaboacuten


Este es el uacuteltimo eslaboacuten que conforma el brazo Los siguientes forman parte de la muntildeeca del manipulador Todos ellos se muestran a continuacioacuten

Figura 9 Cuarto eslaboacuten

Figura 10 Quinto eslaboacuten


Figura 11 Sexto eslaboacuten

El uacuteltimo eslaboacuten el sexto estaacute formado por la pieza resaltada de la figura anterior En ella se incluye el efector final que representa un offset que no seraacute tenido en cuenta para los caacutelculos cinemaacuteticos del manipulador ya que el uso de una herramienta modificariacutea ese offset permitiendo mantener los modelos matemaacuteticos que se desarrollaraacuten en el apartado 2

142 Articulaciones

Ademaacutes de los eslabones que contiene el manipulador otra parte fundamental para su estudio son las articulaciones sobre las que se produce el movimiento de los propios eslabones El manipulador Kuka KR500-2 es un manipulador de seis grados de libertad Cada uno de ellos es aportado por cada una de sus seis articulaciones todas ellas rotacionales En la siguiente figura se representa el manipulador con el eje de giro de cada una de ellas


Figura 12 Ejes de rotacioacuten

En rosa aparecen las articulaciones del brazo que posicionan el efector final y en verde se muestran las articulaciones de la muntildeeca que lo orientan

Las articulaciones tienen unas limitaciones angulares y velocidades de funcionamiento que definen el dextrous space (puntos alcanzables con cualquier orientacioacuten) y el espacio alcanzable del robot (puntos alcanzables)

Figura 13 Dextrous space y espacio alcanzable

Las limitaciones de dichas articulaciones tanto de posicioacuten como de velocidad estaacuten definidas a continuacioacuten

Eje Limites Velocidad Maacutexima


A1 (θ1) plusmn185ordm 69 ordms

A2 (θ2) +110ordm a -40ordm 69 ordms

A3 (θ3) +60 a -184ordm 69 ordms




Tabla 1

143 Compensadores

Ademaacutes de los eslabones y las articulaciones el manipulador dispone de un sistema de compensacioacuten dinaacutemica conformado por un cilindro hidraacuteulico-neumaacutetico El objetivo de este sistema es compensar el peso de los eslabones posteriores a la segunda articulacioacuten y de la carga en el efector final o herramienta que tiene un efecto dinaacutemico muy elevado sobre la segunda articulacioacuten al ser eacutesta la articulacioacuten posterior a la base y sobre la que se articula el resto del peso del manipulador Ademaacutes de este sistema en el codo del robot se incorpora un contrapeso cuyo objetivo es compensar el peso de la carga asiacute como de los eslabones cuarto quinto y sexto de manera que las posiciones maacutes extremas no tengan un efecto dinaacutemico muy elevado que exigiese una actuacioacuten muy exigente para el actuador de la articulacioacuten

15 Estructura del Trabajo

Para el desarrollo del trabajo se han hecho una serie de tareas que se describiraacuten a continuacioacuten

Documentacioacuten recopilacioacuten de informacioacuten sobre roboacutetica e introduccioacuten de los objetivos a realizar durante la elaboracioacuten del proyecto

Desarrollo matemaacutetico e implementacioacuten de la cinemaacutetica directa a partir de la documentacioacuten recopilada se afronta el problema cinemaacutetico directo del manipulador Kuka KR500-2

Desarrollo matemaacutetico e implementacioacuten de la cinemaacutetica inversa con el desarrollo de la cinemaacutetica directa ya realizado se procede entonces a resolver


el problema cinemaacutetico inverso para el manipulador dado a traveacutes del meacutetodo geomeacutetrico

Desarrollo matemaacutetico e implementacioacuten de la cinemaacutetica de la velocidad para completar el estudio cinemaacutetico del manipulador se desarrollan e implementan los desarrollos de la cinemaacutetica de velocidad cuyo principal objetivo es permitir conocer el Jacobiano

Desarrollo matemaacutetico e implementacioacuten de la dinaacutemica con los modelos cinemaacuteticos ya realizados se avanza un paso maacutes considerando el movimiento en relacioacuten a las fuerzas que lo originan

Estudio de sistemas de control para manipuladores estudio de leyes de control compatibles con los modelos desarrollados del manipulador Aprendizaje de los meacutetodos existentes y adaptacioacuten a las necesidades propias del proyecto

Desarrollo de la simulacioacuten del manipulador a traveacutes de toolboxes de Simulink se realizan diferentes modelos que representan virtualmente las caracteriacutesticas cinemaacuteticas y dinaacutemicas del manipulador

Implementacioacuten de sistemas de control realizacioacuten de lazos de control de acuerdo con los modelos implementados en Simulink con el fin de comprobar la correccioacuten de los mismos y realizar posteriormente pruebas HIL en el hardware de control especiacutefico

Realizacioacuten de la memoria del Trabajo Fin de Maacutester redaccioacuten de la memoria que recoge el trabajo realizado previamente

151 Diagrama de Gantt

Cada una de las tareas descritas es mostrada en un diagrama de Gantt junto con la planificacioacuten temporal del desarrollo de cada una de ellas Se ha utilizado para su realizacioacuten el software Gantt Project


Figura 14 Diagrama de Gantt

sep-13 sep-13 oct-13 nov-13 nov-13 dic-13 dic-13 ene-14 feb-14

Documentacioacuten

Desarrollo matemaacutetico e implementacioacuten de la Cinemaacutetica Directa

Desarrollo matemaacutetico e implementacioacuten de la Cinemaacutetica Inversa

Desarrollo matemaacutetico e implementacioacuten de la Cinemaacutetica de Velocidad

Desarrollo matemaacutetico e implementacioacuten de la Dinaacutemica

Estudio de Sistemas de Control para manipuladores

Desarrollo de la simulacioacuten del manipulador

Implementacioacuten de sistemas de control

Realizacioacuten de la memoria del trabajo


2 Estudio cinemaacutetico

Tras la descripcioacuten inicial del sistema se ha procedido a su descripcioacuten matemaacutetica desarrollada a continuacioacuten para poder ser implementada maacutes adelante mediante el uso del software de simulacioacuten MATLAB El uso de este software nos permitiraacute tener un modelo sobre el que implementar los conceptos de control propuestos y probar los resultados previamente a su desarrollo sobre el hardware de control

El desarrollo de los modelos matemaacuteticos que definen las cinemaacuteticas del manipulador es el primer paso a realizar para el estudio mecaacutenico que daraacute el conocimiento del comportamiento del sistema para poder controlarlo A traveacutes de la cinemaacutetica directa se podraacute conocer la posicioacuten del extremo o punto final del manipulador que seraacute el punto de intereacutes dado que es donde se encontraraacute sujeta la herramienta que se utilizaraacute para realizar el trabajo oportuno a traveacutes de las posiciones de cada una de sus articulaciones La cinemaacutetica inversa por el contrario permite conocer las posiciones articulares de los datos de posicioacuten y orientacioacuten del extremo del manipulador

La cinemaacutetica es la ciencia que trata el movimiento sin tener en cuenta las fuerzas que lo causan Dentro de la cinemaacutetica se estudia la posicioacuten la velocidad y la aceleracioacuten Por lo tanto el estudio de la cinemaacutetica de manipuladores se refiere a todas las propiedades geomeacutetricas y basadas en el tiempo de los movimientos El estudio de la cinemaacutetica del manipulador implica entre otras cosas suponer cambios en la ubicacioacuten de los sistemas de referencia de cada articulacioacuten a medida que el mecanismo se mueve

La notacioacuten de las articulaciones se haraacute comenzando desde la base (primera articulacioacuten) hasta el efector final (sexta articulacioacuten) siguiendo la cadena mecaacutenica que forma el manipulador

21 Paraacutemetros Denavit-Hartenberg

Para el desarrollo de los modelos tanto de la cinemaacutetica directa como de la inversa se utilizaraacute la representacioacuten de Denavit-Hartenberg Se trata de un procedimiento que asigna a cada una de las articulaciones un sistema de referencia estando el origen sobre el eje de la articulacioacuten de dicho sistema y dotaacutendolo de tres ejes (X Y Z) Se establecen asiacute coordenadas cartesianas o coordenadas rectangulares que son un tipo de coordenadas ortogonales usadas en espacios eucliacutedeos para la representacioacuten del movimiento o posicioacuten en fiacutesica caracterizadas porque usa como referencia ejes ortogonales entre siacute que se cortan en un punto origen Las coordenadas cartesianas se definen asiacute como la distancia al origen de las proyecciones ortogonales de un punto dado sobre cada uno de los ejes Si el sistema en si es un sistema tridimensional se denomina espacio cartesiano

Los sistemas de referencia se definen de la siguiente manera

El eje Z se coloca en la direccioacuten del eje de articulacioacuten


El eje X es paralelo a la normal comuacuten entre Zn-1 Zn Si no hay una uacutenica normal comuacuten (ejes Z paralelos) entonces d (abajo) es un paraacutemetro libre

El eje Y es orto-normal a los dos anteriores seguacuten la regla de la mano derecha

Los paraacutemetros de Denavit-Hartenberg permiten el paso de un eslaboacuten al siguiente es decir los eslabones unidos por una articulacioacuten mediante cuatro transformaciones baacutesicas Las transformaciones dependen exclusivamente de las caracteriacutesticas constructivas del robot Las transformaciones baacutesicas son

Rotacioacuten 훼 alrededor del eje 푥 es la aacutengulo entre 푧 y 푧 Rotacioacuten 휃 alrededor del eje 푧 es el aacutengulo entre 푥 y 푥 Traslacioacuten 푑 a lo largo del eje 푧 entre 푥 y 푥 Traslacioacuten 푎 a lo largo del eje 푥 entre 푧 y 푧

La definicioacuten de estas caracteriacutesticas puede variar respecto del eje de referencia utilizado (el i o el i-1) Es importante tener en cuenta la definicioacuten establecida y no cambiarla en ninguacuten momento para que los paraacutemetros utilizados conduzcan al resultado correcto de las cinemaacuteticas directa e inversa En la siguiente figura se muestra un ejemplo de dos articulaciones unidas por un eslaboacuten en la que se identifican las transformaciones baacutesicas de Denavit-Hartenberg

Figura 15

Conociendo coacutemo deben colocarse los ejes en cada articulacioacuten y como se realizan las transformaciones baacutesicas se procede ahora a obtener los paraacutemetros de Denavit-Hartenberg para el manipulador Kuka KR500-2 Se colocaraacuten en la siguiente tabla que recoge los paraacutemetros de cada uno de los eslabones


i αi-1 (rad) ai-1 (m) di (m) θ i (rad)

1 -pi2 0500 1045 θ1

2 0 -1300 0 θ 2

3 -pi2 -0055 0 θ 3

4 pi2 0 -1025 θ 4

5 -pi2 0 0 θ 5

6 0 0 0290 θ 6

Tabla 2

22 Matrices de Transformacioacuten Homogeacuteneas

Continuando con la notacioacuten de Denavit-Hartenberg se puede obtener una matriz de transformacioacuten homogeacutenea para cada grado de libertad en la cual ya estaraacuten incluidos todos los paraacutemetros antes descritos Utilizando las transformaciones homogeacuteneas a traveacutes de las matrices homogeacuteneas es posible representar las rotaciones y traslaciones relativas entre los distintos eslabones que componen el manipulador Las matrices de transformacioacuten homogeacutenea se representan con la letra T precedida de un subiacutendice y un superiacutendice que representan el sistema de referencia de la articulacioacuten precedente (i-1) y del posterior (i) respectivamente

Si se desea describir la transformacioacuten completa entre las dos articulaciones es decir entre los sistemas de coordenadas i e i-1 se puede escribir

PTTTTP iPi

QP

RQ

iR

i middotmiddotmiddotmiddot11 (21)

Siendo iP un punto definido en el sistema de referencia i proyectado en el sistema de referencia i-1 Operando sobre la expresioacuten anterior se obtiene


PTP iii

i middot11 (22)

De donde se deduce que

TTTTT Pi

QP

RQ

iR

ii middotmiddotmiddot11

(23)

Si consideramos esta matriz de transformacioacuten formada por cuatro transformaciones individuales podemos ver que la expresioacuten anterior puede interpretarse como una serie de rotaciones y desplazamientos alrededor de los ejes X y Z

)()middot()middot()middot( 111

iZiZiXiXi

i dDRaDRT (24)

Si se utiliza la notacioacuten screw en el que un movimiento es la combinacioacuten de una rotacioacuten y una traslacioacuten alrededor de un eje la expresioacuten anterior puede representarse mediante

)()middot( 111

iiZiiXi

i dScrewaScrewT (25)

La expresioacuten anterior puede ser expresada por sus teacuterminos matriciales de la siguiente manera

푇 =

푐표푠휃 minus푠푖푛휃 middot 푐표푠훼 푠푖푛휃 middot 푠푖푛훼 푎 middot 푐표푠휃푠푖푛휃 푐표푠휃 middot 푐표푠훼 minus푐표푠휃 middot 푠푖푛훼 푎 middot 푠푖푛휃

0 푠푖푛훼 푐표푠훼 푑0 0 0 1

(26)

Asiacute se obtiene la forma general de la matriz de transformacioacuten homogeacutenea que expresa la transformacioacuten entre los sistemas de coordenadas de dos eslabones consecutivos Ademaacutes se puede expresar la relacioacuten entre cualquier par de eslabones aunque no sean consecutivos Asiacute se puede obtener una matriz de transformacioacuten que seraacute muy importante para los desarrollos cinemaacuteticos y dinaacutemicos del manipulador la matriz de transformacioacuten entre la base y el efector final que expresaraacute la orientacioacuten y posicioacuten del efector final en las coordenadas cartesianas Cada una de las columnas expresa en sus tres primeras filas comenzando por la izquierda y hacia la derecha el vector de orientacioacuten en el eje X el Y el Z y la posicioacuten en el mismo orden de


coordenadas Por lo tanto las tres primeras columnas se corresponden con la matriz de rotacioacuten entre los eslabones tratados y la uacuteltima seraacute el vector de posicioacuten La uacuteltima fila se utiliza para que la matriz sea cuadrada y poder realizar operaciones matriciales de manera maacutes sencilla En la praacutectica tiene significado fiacutesico y expresa la relacioacuten con transformaciones de perspectiva y escalado que pueden tener aplicacioacuten en otros campos como la visioacuten artificial

221 Fixed Angles X-Y-Z

Para representar e interpretar las orientaciones se utilizaraacute ademaacutes de los vectores correspondientes de las matrices de transformacioacuten homogeacuteneas otra notacioacuten diferente llamada fixed angles X-Y-Z Se utilizaraacute por la mayor simplicidad a la hora de representar las orientaciones al tratarse de datos uacutenicos y no de vectores ademaacutes de ser la manera maacutes comuacuten utilizada en la industria para transmitir este tipo de informacioacuten pudiendo tratarse de la notacioacuten aquiacute propuesta o a traveacutes de la notacioacuten de Euler En cualquier caso la informacioacuten recogida en una matriz de rotacioacuten puede ser representada por tres datos cuya informacioacuten es equivalente En este proyecto su uso se limita para representar la orientacioacuten del efector final respecto de la base del manipulador ya que el resto de relaciones seraacuten tratadas mediante las matrices de transformacioacuten homogeacuteneas Cada uno de los tres datos se llamaraacute giro (α) inclinacioacuten (훽) y oscilacioacuten (γ) Para la introduccioacuten de referencias estos paraacutemetros seraacuten el formato utilizado y a traveacutes de su conversioacuten a vectores de orientacioacuten se trabajaraacute con ellos en los modelos matemaacuteticos

El origen de la matriz de rotacioacuten equivalente 푅 (훼훽 훾) es sencillo porque todas las rotaciones se producen alrededor de los ejes del sistema de referencia es decir

푅 (훼훽 훾) = 푅 (훼)푅 (훽)푅 (훾) =

=푐표푠훼 minus푠푒푛훼 0푠푒푛훼 푐표푠훼 0

0 0 1middot

푐표푠훽 0 푠푒푛훽0 1 0

minus푠푒푛훽 0 푐표푠훽middot

1 0 00 푐표푠훾 minus푠푒푛훾0 푠푒푛훾 푐표푠훾

(27)

Es importante tener en cuenta el orden de las rotaciones utilizadas en la expresioacuten anterior Pensando en los teacuterminos de las rotaciones como operadores se han aplicado las rotaciones (desde la derecha) de 푅 (훾) despueacutes푅 (훽) y finalmente 푅 (훼) Multiplicando la expresioacuten se obtiene

푅 (αβ γ) =

=cos훼 middot 푐표푠훽 cos훼 middot sen훽 middot 푠푒푛훾 minus 푠푒푛훼 middot 푐표푠훾 cos훼 middot sen훽 middot 푐표푠훾 + 푠푒푛훼 middot 푠푒푛훾sen훼 middot 푐표푠훽 sen훼 middot sen훽 middot sen훾 + 푐표푠훼 middot 푐표푠훾 sen훼 middot sen훽 middot 푐표푠훾 minus 푐표푠훼 middot 푠푒푛훾minus푠푒푛훽 푐표푠훽 middot 푠푒푛훾 푐표푠훽 middot 푐표푠훾

(28)


El problema inverso la de extraccioacuten de aacutengulos equivalentes fijos X-Y-Z desde una matriz de rotacioacuten es tambieacuten un proceso de intereacutes La solucioacuten depende de la resolucioacuten de un conjunto de ecuaciones trascendentales hay nueve ecuaciones y tres incoacutegnitas si la ecuacioacuten (28) es equiparada a una matriz de rotacioacuten dada Entre las nueve ecuaciones hay seis dependencias por lo que en esencia tenemos tres ecuaciones y tres incoacutegnitas Expresando

푅 (훼훽 훾) =푟 푟 푟푟 푟 푟푟 푟 푟

(29)

Cada uno de los paraacutemetros de inclinacioacuten giro y oscilacioacuten (en este sentido) equivalen entonces a

훽 = 푎푡푎푛2(minus푟 푟 + 푟 (210)

훼 = 푎푡푎푛2(푟푐표푠훽

푟푐표푠훽 (211)

훾 = 푎푡푎푛2(푟32푐표푠훽 푟33

푐표푠훽) (212)

Aunque existe una segunda solucioacuten mediante el uso de la raiacutez cuadrada positiva en la foacutermula de 훽 calculamos siempre la solucioacuten uacutenica para la cual -90degle 훽le 90ordm Esto es generalmente una buena praacutectica porque entonces se puede definir una relacioacuten uno-a-uno entre las funciones de orientacioacuten Sin embargo en algunos casos el caacutelculo de todas las soluciones es importante Si 훽 = plusmn 90ordm (por lo que 푐표푠훽 = 0) la solucioacuten degenera En esos casos soacutelo la suma o la diferencia de α e γ puede ser calculada Una convencioacuten posible es elegir 훼 = 0 en estos casos

23 Cinemaacutetica directa

El objetivo del desarrollo del modelo cinemaacutetico directo del manipulador es obtener para unos valores de articulaciones dados tanto la posicioacuten como la orientacioacuten del efector final De esta manera mediante la medicioacuten de la posicioacuten de las articulaciones se puede conocer para cualquier valor de eacutestas donde se encuentra el extremo del robot

Para la resolucioacuten de este modelo matemaacutetico es necesario utilizar las matrices de transformacioacuten homogeacuteneas descritas en el apartado anterior Como se vio cada una de ellas representa el giro y la posicioacuten relativa de cada uno de los eslabones


proporcionando la informacioacuten de la posicioacuten y orientacioacuten de la articulacioacuten posterior al eslaboacuten tratado Se relaciona asiacute el sistema de coordenadas de un eslaboacuten con el de su eslaboacuten anterior Despueacutes se procede a encadenar las transformaciones individuales de forma que se obtiene la posicioacuten y orientacioacuten de un link n respecto al eslaboacuten 0 (base)

Una vez que se han definido los paraacutemetros de Denavit-Hartemberg y seguacuten se definan las matrices de transformacioacuten de toda la cadena cinemaacutetica del manipulador el planteamiento de las ecuaciones de la cinemaacutetica directa del manipulador se hace de forma directa

El primer paso consiste entonces en determinar cada una de las matrices de transformacioacuten siguientes

푇 푇 푇 푇 푇 푇 (213)

A partir de ellas se calcularaacute la matriz homogeacutenea que relacione el efector final con la base que seraacute la matriz que permitiraacute obtener de forma directa los datos en el espacio cartesiano del efector final Para ello se multiplicaraacuten de forma encadenada las matrices de las que se dispone utilizando la siguiente propiedad

푇 = 푇 푇 푇hellip 푇 (214)

Asiacute la matriz a calcular 푇6 se obtendraacute de la siguiente expresioacuten

푇 = 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) (215)

Donde cada una de las matrices se define para el manipulador Kuka KR500-2

(216)

(217)

1000010

middotsincos0sinmiddotcossin0cos

1

1111

1111

01 d

aa

T

10000100

middotsin0cossinmiddotcos0sincos

2222

2222

12

aa

T


(218)

(219)

(220)

(221)

24 Cinemaacutetica inversa meacutetodo geomeacutetrico

Para el desarrollo de la cinemaacutetica inversa existen diferentes meacutetodos El meacutetodo elegido se basa en el estudio de la geometriacutea del manipulador respecto de cada una de las articulaciones rotativas que lo componen Otro tipo de meacutetodos de resolucioacuten suponen un desarrollo maacutes largo y complejo como es la resolucioacuten algebraica Dado que cualquier meacutetodo conduce a la resolucioacuten de los valores articulares para una posicioacuten y orientacioacuten dadas del efector se eligioacute un meacutetodo de sencilla resolucioacuten desde el punto de vista computacional factor importante para finalmente conseguir que los modelos se ejecuten raacutepidamente para conseguir una alta frecuencia de funcionamiento del lazo de control a implementar

La expresioacuten arctan2 que aparece en este apartado equivale a la funcioacuten arcotangente con dos argumentos El propoacutesito de utilizar dos argumentos en lugar de uno es reunir

10000010


3333

3333

23

aa

T

1000010

0cos0sin0sin0cos

4

44

44

34 dT

100000100cos0sin0sin0cos

55

55

45

T

1000100

00cossin00sincos

6

66

66

56 dT


informacioacuten sobre los signos de las entradas con el fin de devolver el cuadrante correspondiente del aacutengulo calculado

El sistema de referencia del manipulador es el uacutenico cuya articulacioacuten rota (la direccioacuten Z) perpendicular a la base del robot (primera articulacioacuten) Por lo tanto para poder calcular el aacutengulo girado basta con utilizar la posicioacuten del plano perpendicular a la direccioacuten del giro (formado por X e Y) del efector final (paraacutemetro conocido) Para conseguir que el modelo sea maacutes flexible y poder utilizar herramientas de cualquier morfologiacutea en lugar del punto marcado por el efector final (sin herramienta) se utilizaraacute el sistema de referencia anterior es decir sin tener en cuenta la brida y la herramienta si la hubiera Ese punto seraacute la ubicacioacuten de la quinta articulacioacuten por tanto y se llamaraacute P Para calcularlo a la posicioacuten marcada por la posicioacuten y orientacioacuten del efector final se deben restar las dimensiones de la herramienta acoplada En el caso actual en el que se supone el manipulador sin ninguacuten tipo de herramienta el punto P se calcula

푃 = [푥푦 푧] minus 푑6 lowast 푍6 (222)

d6 representa el paraacutemetro de Denavit-Hartenberg que se corresponde con la longitud de la brida que se resta a la posicioacuten del efector final en la direccioacuten Z de la uacuteltima articulacioacuten

A traveacutes de sus coordenadas X e Y se puede conocer por tanto el giro de la primera articulacioacuten

휃 = 푎푟푐푡푎푛2(푃푥푃푦) (223)

Para calcular el giro de la segunda articulacioacuten es necesario un planteamiento geomeacutetrico maacutes complejo que para la primera


Figura 16

El primer paso es el caacutelculo del segmento que une la tercera articulacioacuten con el efector final con el fin de tener una medida directa evitando el offset que tiene el manipulador en el codo

푙 = 푎 + 푙 (224)

A partir de las coordenadas X e Y de la posicioacuten del efector final respecto del sistema de referencia actual se calcularaacute la distancia entre la segunda articulacioacuten y el extremo del robot

푟 = 푃 + 푃 (225)

El aacutengulo formado por el segmento que une ambos puntos con la direccioacuten X del sistema de referencia de la segunda articulacioacuten seraacute necesario para el posterior caacutelculo del aacutengulo de giro de la articulacioacuten

훽 = 푎푟푐푡푎푛2(minus푃푥푃푦) (226)

Otro aacutengulo que es necesario calcular es el del mismo segmento r con respecto al segundo eslaboacuten Para la obtencioacuten de este aacutengulo se recurre a la ley de los cosenos

훼 = 푎푟푐표푠푙 + 푟 + 푙

2 middot 푟 middot 푙 (227)


Con estos dos uacuteltimos valores ya es posible calcular el aacutengulo de giro de la segunda articulacioacuten Dependiendo de la configuracioacuten del manipulador puede tratarse de dos giros diferentes En el caso de que la tercera articulacioacuten situacutee al efector final entre ella y la segunda articulacioacuten en la direccioacuten Z del sistema de referencia original (el de la primera articulacioacuten) la configuracioacuten que se produce se llamaraacute codo abajo y hace que el caacutelculo del giro de la segunda articulacioacuten se realice con la siguiente expresioacuten

휃 =휋2

+ 훼 minus 훽 (228)

Un ejemplo de configuracioacuten de codo abajo seriacutea el siguiente

Figura 17

Se observa que respecto de la vertical que situacutea la posicioacuten de referencia (cero grados) teniendo en cuenta el sentido de los aacutengulos calculados se debe seguir la expresioacuten anterior para poder calcular el aacutengulo 휃 El caso contrario se llama codo arriba e implica para el caacutelculo de la segunda articulacioacuten la utilizacioacuten de la siguiente expresioacuten

휃 =휋2minus 훼 minus 훽 (229)

Un ejemplo de esta configuracioacuten aparece en la siguiente imagen


Figura 18

En este caso los sentidos de los giros hacen que sea necesario la resta de los aacutengulos α y 훽 Ejemplos de estas configuraciones sobre la representacioacuten 3D del manipulador son mostrados en las siguientes imaacutegenes en las que se muestra como para una misma posicioacuten y orientacioacuten del efector final existen al menos dos soluciones vaacutelidas que implican el uso de una u otra configuracioacuten

Figura 19 Configuracioacuten codo arriba

Para el caacutelculo del giro de la tercera articulacioacuten el proceso es similar al de la segunda


Figura 20 Configuracioacuten codo abajo

Figura 21

Inicialmente se calcula la hipotenusa r que une la segunda articulacioacuten con el efector final al igual que en el caso anterior

푟 = 푃 + 푃 (230)

Los aacutengulos a calcular ahora son distintos Uno de ellos es el aacutengulo formado por el tercer eslaboacuten y el segmento que une la tercera articulacioacuten con el efector final


휑 = 푎푟푐표푠푎 + 푙 + 푙

2 middot 푎 middot 푙 (231)

El otro es el aacutengulo que forma esa misma distancia al efector final pero en este caso con el segundo eslaboacuten Una vez maacutes se utiliza para despejar la ley de los cosenos

휀 = 푎푟푐표푠푙 + 푙 minus 푟

2 middot 푙 middot 푙 (232)

Finalmente para calcular la posicioacuten angular se deben utilizar dichos aacutengulos en funcioacuten de la configuracioacuten que presente el manipulador Para la configuracioacuten codo abajo la expresioacuten es

휃 = 휋 minus 휑 + 휀 (233)

En el caso de la configuracioacuten codo arriba

휃 = minus휋 minus 휑 minus 휀 (234)

Para las siguientes articulaciones las de la muntildeeca el proceso es diferente En funcioacuten de la posicioacuten de la muntildeeca se pueden identificar dos nuevas configuraciones que se llamaraacuten muntildeeca arriba y muntildeeca abajo Se suman por tanto a las dos anteriores lo que da lugar a que para una misma posicioacuten y orientacioacuten del efector final se puedan dar cuatro posiciones articulares diferentes

Ambas configuraciones de la muntildeeca se definen observando la orientacioacuten del sistema de coordenadas del efector final con respecto al sistema de referencia de la cuarta articulacioacuten Para analizar las configuraciones se utiliza un paraacutemetro de orientacioacuten que hace referencia a la orientacioacuten del vector unitario Y6 (o X6) con respecto al vector unitario Y4 y que se define como

=0

푌6 middot 푌4푋6 middot 푌4

푠푖푌6 middot 푌4 = 0푦푋6 middot 푌4 = 0푠푖푌6 middot 푌3 ne 0푠푖푌6 middot 푌3 = 0

(235)

Mediante este paraacutemetro se estableceraacute el giro de la cuarta articulacioacuten lo que posteriormente influiraacute para los giros de la quinta y la sexta articulacioacuten se orienten de manera que se alcance la posicioacuten y orientacioacuten dadas Dado que la cuarta y la sexta articulacioacuten rotan en torno a un plano comuacuten y sus liacutemites mecaacutenicos son muy elevados (plusmn350ordm en ambos casos) se pueden dar las dos configuraciones de la


muntildeeca en las que la cuarta y la sexta articulacioacuten tengan aacutengulos complementarios y la quinta articulacioacuten tenga un aacutengulo opuesto

En el caso de la cuarta articulacioacuten el primer paso consiste en hallar la direccioacuten del eje de giro de la quinta articulacioacuten

Figura 22

푧 = 푧 times 푧 (236)

A partir del plano formado por las direcciones de giro de la cuarta y la sexta articulacioacuten se puede hallar esa direccioacuten ya que el plano es siempre perpendicular a ella dado que los giros de la muntildeeca respecto de las articulaciones cuarta y sexta no lo modifican El procedimiento de caacutelculo consistiraacute en hallar el producto vectorial de ambos vectores Una vez conocida la direccioacuten del eje de giro de la quinta articulacioacuten se procede a la proyeccioacuten de los ejes z5 y x5 sobre el sistema de referencia de la cuarta articulacioacuten


Figura 23

푎 = minus푧 middot 푥 (237)

푏 = 푧 middot 푦 (238)

Para el caacutelculo del aacutengulo es necesario tener en cuenta la configuracioacuten de la muntildeeca Se crearaacute un nuevo paraacutemetro M que establezca se signo del giro de manera que se pueden dar dos configuraciones en las que la cuarta articulacioacuten presente giros opuestos

Configuracioacuten Muntildeeca M

Muntildeeca abajo 1 ge0 1

Muntildeeca abajo 1 lt0 -1

Muntildeeca arriba -1 ge0 -1

Muntildeeca arriba -1 lt0 1

Tabla 3

Este nuevo paraacutemetro se utilizaraacute en la funcioacuten arcotangente estableciendo en funcioacuten de la orientacioacuten final el aacutengulo que tiene que girar la cuarta articulacioacuten para obtener la configuracioacuten deseada


휃 = 푎푟푐푡푎푛2(minus푀 middot 푎푀 middot 푏) (239)

El meacutetodo de resolucioacuten de las ecuaciones posteriores se basa de nuevo en el caacutelculo de productos vectoriales cuyos resultados sirven para obtener el aacutengulo de giro a traveacutes de la trigonometriacutea

Una vez calculado θ4 tenemos totalmente definido el sistema de coordenadas de la quinta articulacioacuten de modo que el aacutengulo θ5 puede calcularse a partir de los aacutengulos formados entre los vectores directores de esta articulacioacuten y el efector final todos conocidos Para ello procedemos de nuevo a la proyeccioacuten de ambos sistemas de coordenadas sobre un mismo plano en este caso el plano x5y5

Figura 24

El procedimiento de obtencioacuten del aacutengulo 휃 es anaacutelogo al de la articulacioacuten anterior


Figura 25

푎 = 푦 middot minus푧 (240)

푏 = 푥 middot 푧 (241)

휃 = 푎푟푐푡푎푛2(푏푎) (242)

Finalmente proyectamos de nuevo los sistemas de coordenadas de la quinta y sexta articulacioacuten en el mismo plano esta vez proyectando en la direccioacuten de giro de la sexta articulacioacuten z6 en el plano x5z5 Una vez maacutes se sigue el mismo procedimiento para la obtencioacuten del giro de la articulacioacuten


Figura 26

푎 = 푥 middot 푧 (243)

푏 = minus푦 middot 푧 (244)

휃 = 푎푟푐푡푎푛2(minus푎 푏) (245)

25 Cinemaacutetica de la Velocidad el Jacobiano

Mediante los modelos de la cinemaacutetica directa e inversa se consigue el control estaacutetico del posicionamiento situando el efector final en una localizacioacuten determinada Sin embargo la posicioacuten y orientacioacuten del efector final no es el uacutenico elemento a considerar sino que tambieacuten lo seraacuten las velocidades con las que este se moveraacute hasta alcanzar esta localizacioacuten Esta tarea requiere la coordinacioacuten de la velocidad instantaacutenea del efector final (lineal y angular) y la de las articulaciones (angular) En este apartado se procede a la obtencioacuten de las relaciones de velocidad es decir la relacioacuten existente entre las velocidades lineal y angular del efector final y las articulaciones

Matemaacuteticamente las relaciones de velocidad entre ambos espacios son determinadas mediante el Jacobiano de la funcioacuten de la cinemaacutetica directa El Jacobiano de esta funcioacuten va a ser una matriz la cual es uno de los elementos maacutes importantes en el anaacutelisis y control del movimiento en roboacutetica ya que juega un papel muy importante en muchos aspectos se utiliza para el caacutelculo y ejecucioacuten de


trayectorias para la determinacioacuten de configuraciones singulares la derivacioacuten de las ecuaciones de la dinaacutemica o para la transformacioacuten de fuerzas y el caacutelculo de los pares de las articulaciones A partir de las ecuaciones de la cinemaacutetica directa la transformacioacuten de velocidad entre ambos espacios se calcula mediante la utilizacioacuten del Jacobiano

Se define la velocidad del efector final como un vector del que forman parte tanto la velocidad lineal del mismo como la velocidad angular

푦 = 푣휔 (246)

Siendo 푦 el vector de velocidad en el espacio cartesiano del efector final 푣 su velocidad lineal y 휔 su velocidad angular

Las expresiones que se muestran a continuacioacuten permiten realizar la transformacioacuten entre el espacio cartesiano y el articular de la velocidad lineal y angular del efector final para un instante de tiempo determinado Para una velocidad angular de giro de las articulaciones se obtiene

vn Jv 0 (247)

Jn

0 (248)

Donde vJ y J son matrices de dimensiones 3xn y 0nv y 0

n representan la velocidad lineal y angular del efector final respectivamente expresadas respecto al sistema de referencia de la base Estas expresiones resultan en la siguiente igualdad que las comprende a ambas

)(Jy (249)

y se denomina tambieacuten la velocidad del cuerpo riacutegido J es la matriz jacobiana (dependiente de las posiciones articulares) y es el vector de velocidades articulares Para que la expresioacuten sea correcta el Jacobiano debe estar compuesto de la siguiente manera


JJ

J v (250)

Derivando la expresioacuten (249) se obtiene la expresioacuten equivalente para la transformacioacuten entre el espacio cartesiano y el espacio articular de la aceleracioacuten del manipulador

푦 = J(θ) middot θ +푑푑푡

middot 퐽(휃) 휃 (251)

Para el caacutelculo de la matriz jacobiana del manipulador de seis grados de libertad tratado en el proyecto cuyas articulaciones son todas rotativas Esta matriz permite definir la transformacioacuten de las velocidades y las aceleraciones entre el espacio cartesiano y articular

251 Caacutelculo del Jacobiano

La submatriz J es la parte del Jacobiano que se utiliza para la transformacioacuten de la

velocidad y la aceleracioacuten angular entre los espacios articular y cartesiano Partiendo de la expresioacuten general de la velocidad angular de un eslaboacuten geneacuterico

휔 = 푅 middot 휔 + 휃 middot 푍 (252)

Donde 휔 es la velocidad angular del eslaboacuten i+1 respecto del mismo sistema de coordenadas 푅 la matriz de rotacioacuten del eslaboacuten anterior 휔 su velocidad angular y 휃 la velocidad de la articulacioacuten sobre la que se sustenta por uacuteltimo 푍 el vector de orientacioacuten de la direccioacuten Z de la articulacioacuten que gira el eslaboacuten del que se quiere conocer la velocidad angular

Si se considera toda la cadena cinemaacutetica se obtiene

n

iiinnn zkRkRk

1

01

01

0121

0 middotmiddotmiddotmiddotmiddotmiddot

(253)

En la ecuacioacuten (252) en la que se tomoacute la velocidad angular de cada una de las articulaciones y se multiplicoacute por la matriz de rotacioacuten correspondiente para


referenciarla al sistema de coordenadas de la base de modo que el resultado final obtenido fue la suma de todas ellas se demostroacute que la velocidad del efector final estaraacute determinada por la de todas las articulaciones que conforman el manipulador

Una vez obtenida la relacioacuten (253) se iguala a la parte de la expresioacuten de la cinemaacutetica de velocidad (248) relativa a la velocidad angular obteniendo Jω

][ 01

01

00 nzzzJ (254)

Por otro lado la submatriz vJ es la parte del Jacobiano que se utiliza para realizar la transformacioacuten de la velocidad y la aceleracioacuten lineal entre los espacios articular y cartesiano La velocidad lineal del efector final se corresponde con la derivada del vector posicioacuten

푃 = 푣 (255)

Este vector se corresponde con la cuarta columna de la matriz de transformacioacuten homogeacutenea 푇 y es funcioacuten de todos los aacutengulos de las articulaciones Por ello siguiendo la regla de la cadena para su derivacioacuten se obtiene

i

n

i i

NN

vv

middot1

00

(256)

Si se igualan las expresiones (247) y la expresioacuten general de la cinemaacutetica de velocidad (253) se obtiene que la parte del Jacobiano vJ es

i

Nv

vJ

0

(257)

Para el caacutelculo de cada columna de Jvi se considera que el resto de articulaciones estaacuten fijas de modo que la distancia i-1Pn respecto al sistema de coordenadas i-1 es constante como si perteneciesen al mismo cuerpo riacutegido

Partiendo de la expresioacuten general de la velocidad lineal de un eslaboacuten geneacuterico

푣 = 푣 + 휔 times 푃 (258)


Tomando la base como sistema de referencia se obtiene

푣 = 휔 times 푃 (259)

Sustituyendo la expresioacuten 휔 y realizando el caacutelculo de 푃 como la resta de los vectores de posicioacuten de la articulacioacuten y el efector final tambieacuten referenciados a la base se obtiene la expresioacuten para el caacutelculo de Jv

퐽 = 푍 times ( 푃 minus 푃 ) (260)

Ahora ya es posible definir el Jacobiano a partir de las submatrices calculadas

0

1

01

001 )(

i

iniv

zPPz

JJ

J

(261)

En el caso del manipulador tratado que tiene seis grados de libertad la expresioacuten que define el Jacobiano es

0

501

00

05

06

05

01

06

01

00

06

00 )()()(

zzzPPzPPzPPz

J

(262)

252 Singularidades

Las singularidades son configuraciones articulares en las que el manipulador pierde uno o varios grados de libertad Estas posiciones se pueden conocer a traveacutes del Jacobiano Aquellas configuraciones en las que el Jacobiano pierde el rango completo es decir que su determinante es cero deben ser evitadas en las trayectorias ya que se trata de singularidades

En una singularidad el movimiento en al menos una direccioacuten no es posible independientemente del valor de velocidad de las articulaciones que se considere Ademaacutes cerca de una posicioacuten singular se pueden producir valores excesivamente elevados en la velocidad de alguna articulacioacuten

Muchas singularidades ocurren en los liacutemites del espacio de trabajo del manipulador por ejemplo cuando el brazo se encuentra totalmente extendido Otras se producen por indefiniciones surgidas en los modelos matemaacuteticos


Ademaacutes de en los liacutemites del espacio de trabajo donde el manipulador pierde grados de libertad al encontrarse totalmente extendido se han identificado dos configuraciones maacutes que se corresponden con singularidades

Cuando los eslabones cuarto y sexto se encuentran totalmente alineados (θ5=0) el meacutetodo utilizado para calcular los giros de dichas articulaciones falla al no poder utilizar el criterio de la configuracioacuten muntildeeca arriba o muntildeeca abajo En ese caso no es posible calcular el giro de las articulaciones obteniendo una indefinicioacuten

La segunda singularidad se encuentra cuando la posicioacuten del efector final (menos su offset) se encuentra situado justamente sobre el sistema de referencia de la primera articulacioacuten haciendo que su posicioacuten en el espacio cartesiano en la direccioacuten X e Y sea cero En ese caso el giro de la primera articulacioacuten no puede ser calculado puesto que se obtiene nuevamente una indefinicioacuten


3 Estudio Dinaacutemico

Hasta ahora se ha estudiado el movimiento del manipulador si tener en cuenta las fuerzas que lo causan es decir las consideraciones tenidas en cuenta eran cinemaacuteticas se estudiaron las relaciones de posiciones y velocidades estaacuteticas En este apartado se abordaraacuten dichas fuerzas cuyo estudio se conoce como dinaacutemica Su conocimiento seraacute necesario para tener en cuenta los factores dinaacutemicos reales del manipulador que daraacuten lugar a un control que tiene en cuenta que el funcionamiento del manipulador no es ideal En esta seccioacuten se procede por tanto al desarrollo de las ecuaciones de movimiento del manipulador en las que se presenta la relacioacuten entre el movimiento y las fuerzas y los pares

El primer paso para el estudio dinaacutemico del manipulador es definir y estudiar las caracteriacutesticas dinaacutemicas del manipulador Estas caracteriacutesticas son los centros de gravedad y tensores de inercia Estos paraacutemetros deben ser conocidos para resolver y plantear las ecuaciones de la dinaacutemica Al igual que ocurre con el estudio cinemaacutetico existen dos puntos de vista a la hora de estudiar este problema Uno de ellos consiste en que dada una trayectoria velocidad y aceleracioacuten del efector final se calculen los pares que es necesario aplicar en las articulaciones para alcanzar esos valores Esta formulacioacuten se denomina dinaacutemica inversa y seraacute la que se utilice para el desarrollo del control del manipulador El otro planteamiento consiste en conocer el comportamiento dinaacutemico del manipulador cuando se le aplican una serie de pares en sus articulaciones A partir de ellas se calcularaacute cuaacutel seraacute el movimiento resultante que estos producen Este punto de vista se utilizaraacute en la simulacioacuten del manipulador conocido como planteamiento de la dinaacutemica directa

31 Distribucioacuten de la Masa del Manipulador

La primera de las caracteriacutesticas dinaacutemicas a estudiar es la distribucioacuten de la masa en el manipulador La mayor parte de eacutesta la conforman los eslabones por lo que seraacute el objeto de estudio principal de este apartado

311 Centros de Masas

El centro de masas de un cuerpo es el punto geomeacutetrico que dinaacutemicamente se comporta como si en eacutel estuviera aplicada la resultante de todas las fuerzas externas al cuerpo Permite trabajar con el punto obtenido de manera que la geometriacutea del cuerpo no influya en los caacutelculos al haberla tenido ya en cuenta simplificando el proceso El sistema formado por toda la masa concentrada en el centro de masas es por lo tanto un sistema equivalente al original Cuando se trabaja con cuerpos situados en la tierra los teacuterminos Centro de Masas y Centro de Gravedad son equivalentes debido a que el cuerpo estaacute sometido a un campo gravitatorio constante


Figura 27 Situacioacuten de los centros de masas

Como se comentoacute al inicio de esta seccioacuten el principal objeto de estudio seraacuten los eslabones Por ello se comienza por la localizacioacuten de los centros de masas de cada uno de ellos Es importante conocer su posicioacuten relativa respecto de la articulacioacuten a la que van unida y sobre la que su masa ejerce un efecto dinaacutemico directo Es decir se debe conocer su posicioacuten respecto de la articulacioacuten sobre la que rotan y no la posterior a la que tambieacuten estaacuten unidos A partir de esa posicioacuten relativa respecto del sistema de referencia de cada una de las articulaciones correspondientes se podraacute conocer su posicioacuten para cualquier ubicacioacuten en la que se encuentre el manipulador y por tanto se conoceraacute su efecto sobre la dinaacutemica del mismo Para el caacutelculo de las posiciones de los centros de masas respecto de los sistemas de referencia se utilizan los datos ofrecidos por el fabricante

Dado que se utilizan matrices de transformacioacuten homogeacuteneas que contienen el giro de cada uno de los sistemas de referencia la posicioacuten de los centros de masas se puede considerar fija respecto de ellos por lo que conociendo los datos de posicioacuten de cada uno de ellos es suficiente para poder realizar los caacutelculos de los teacuterminos dinaacutemicos en los que este paraacutemetro tiene influencia

32 Pseudo-Matriz de Inercia

Para determinar el tensor de inercia de cada eslaboacuten se ha utilizado el archivo de calibracioacuten del propio robot existente en la instalacioacuten Se han extraiacutedo tambieacuten los valores teoacutericos desde el modelo CAD del mismo (suponiendo una densidad de material de 200 Kgm3)

La masa de los eslabones se va a ver sometida a fuerzas de inercia durante el movimiento del manipulador produciendo esfuerzos en las articulaciones del mismo que deben considerarse en los caacutelculos dinaacutemicos


En los sistemas con un solo grado de libertad a menudo hablamos de la masa de un cuerpo riacutegido En el caso del movimiento de rotacioacuten alrededor de un solo eje la nocioacuten del momento de inercia es conocida Para un cuerpo riacutegido que es libre para moverse en tres dimensiones hay infinitos ejes de rotacioacuten posibles En el caso de rotacioacuten alrededor de un eje arbitrario necesitamos una forma completa de la caracterizacioacuten de la distribucioacuten de la masa del cuerpo riacutegido En este caso se introduce el tensor de inercia que para nuestros propoacutesitos puede ser considerado como una generalizacioacuten del momento escalar de inercia de un objeto El tensor de momentos de inercia es una matriz simeacutetrica de dimensiones 4x4 que se caracteriza por tener la distribucioacuten de la masa del cuerpo riacutegido Esta matriz es constante y se calcula soacutelo una vez para cada eslaboacuten Esta depende de su geometriacutea y su distribucioacuten de masa La expresioacuten general de esta matriz para un eslaboacuten i es

dmrrI Ti

eslaboacuteni

ii middotmiddot (31)

Donde

푟 = [푥 푦 푧 1] (32)

son las coordenadas del centro de masa y dm es la masa infinitesimal del eslaboacuten i localizada en dicho punto Expandiendo la expresioacuten anterior se obtiene la expresioacuten general de la pseudomatriz de inercia de un eslaboacuten i

dmdmzdmydmxdmzdmzdmyzdmzxdmydmyzdmydmyxdmxdmzxdmyxdmx

I

middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot

2

2

2

(33)

Para poder expresar la pseudomatriz de inercia con los datos propios del manipulador que se estaacute tratando se debe ajustar la matriz de la expresioacuten (33) a los datos de los que se dispone proporcionados por el fabricante Entre ellos estaacuten los elementos 퐼 퐼 푦 퐼 que se conocen como los momentos principales de inercia

dmzyI XX )( 22 (34)

dmzxIYY )( 22 (35)


dmyxIZZ )( 22 (36)

Los productos de inercia tambieacuten son un dato conocido

dmxyI XY middot (37)

dmxzI XZ middot (38)

dmyzIYZ middot (39)

Por uacuteltimo estaacuten los momentos de primer orden

dmxxm middot (310)

dmyym middot (311)

dmzzm middot (312)

Utilizando estos datos en la matriz (33) de manera que eacutesta siga estando formada por los mismos elementos se obtiene

mzmymxm

zmIIIII

ymIIIII

xmIIIII

IZZYYXX

YZXZ

YZZZYYXX

XY

XZXYZZYYXX

2

2

2

(313)

33 Dinaacutemica Inversa

Existen dos meacutetodos para realizar los modelos matemaacuteticos de la dinaacutemica inversa de un sistema mecaacutenico la formulacioacuten de Newton-Euler y la formulacioacuten de Euler-


Lagrange Ambos meacutetodos proporcionan las mismas ecuaciones de movimiento pero desde dos puntos de vista diferentes La formulacioacuten de Newton-Euler es un meacutetodo iterativo que se basa en el balance de fuerzas del sistema Con este planteamiento se trata cada eslaboacuten de forma independiente de modo que para cada uno de ellos y de forma sucesiva se definen las ecuaciones de su movimiento lineal y angular Como cada eslaboacuten estaacute unido a sus contiguos en estas ecuaciones aparecen fuerzas de los pares de las articulaciones asiacute como la influencia del movimiento de unos eslabones sobre otros La formulacioacuten de Euler-Lagrange se basa sin embargo en el balance de las energiacuteas Ese balance seraacute la diferencia entre energiacutea cineacutetica y potencial aunque tambieacuten es posible tener en cuenta las peacuterdidas por friccioacuten seca y viscosa en las articulaciones Ademaacutes de esta diferencia fundamental de planteamiento ambos meacutetodos se diferencian en que aunque las ecuaciones de movimiento resultantes sean las mismas mediante La formulacioacuten de Euler-Lagrange se puede tener en cuenta las flexiones del mecanismo al contrario que con la formulacioacuten de Newton-Euler por lo que el resultado seraacute maacutes cercano al sistema real que se quiere modelar Esto es debido a que las articulaciones aunque sean de una rigidez bastante elevada se pueden considerar flexibles por lo que la energiacutea potencial incluiraacute teacuterminos que contienen la energiacutea almacenada en los elementos elaacutesticos En el balance de fuerzas sin embargo no se pueden incluir estos teacuterminos Por ese motivo la formulacioacuten utilizada ha sido la de Euler-Lagrange tanto en el desarrollo teoacuterico como en el de la simulacioacuten

Histoacutericamente ambas se han ido utilizando de la misma forma ya que son equivalentes praacutecticamente en todos los sentidos (computacioacuten resultados etc) La formulacioacuten de Newton-Euler facilita el caacutelculo de las fuerzas generalizadas que es necesario aplicar para conseguir una determinada evolucioacuten en el tiempo mientras que la formulacioacuten de Euler-Lagrange describe la evolucioacuten de las fuerzas generalizadas sin calcular la relacioacuten de la misma con la trayectoria utilizaacutendose habitualmente en el caacutelculo numeacuterico

331 Formulacioacuten Euler-Lagrange

La formulacioacuten de Euler-Lagrange es un meacutetodo sistemaacutetico para la obtencioacuten del modelo dinaacutemico de un manipulador de n grados de libertad en el que se establece la relacioacuten entre las posiciones de las articulaciones velocidades aceleraciones y los pares aplicados al manipulador Los pares que aparecen en el sistema son momentos no conservativos generados por los actuadores las fuerzas de friccioacuten en las articulaciones y los pares inducidos en las articulaciones El aspecto de las posibles fuerzas existentes si se produjese una resistencia al movimiento de la herramienta que se tradujese en fuerzas que se oponen al movimiento no seraacute tenido en cuenta en este apartado Ese aspecto seraacute incluido en el apartado Control de fuerza en el que se propone un lazo de control que contemplariacutea ese supuesto de funcionamiento con fuerzas resistentes en la herramienta pero sin modificar la formulacioacuten de la dinaacutemica inversa

En los siguientes subapartados se presenta el desarrollo de las ecuaciones del modelo dinaacutemico del manipulador En ellas se utilizan las matrices de transformacioacuten


homogeacuteneas T obtenidas en el modelo cinemaacutetico presentado en el apartado Matrices de Transformacioacuten Homogeacuteneas En primer lugar se calcula la velocidad de un eslaboacuten geneacuterico y despueacutes su tensor de inercia Estos valores se utilizan para el caacutelculo de la energiacutea cineacutetica A continuacioacuten se calcula la energiacutea potencial Una vez obtenidos ambos valores se formula el Lagrangiano y se obtienen las ecuaciones que nos proporcionan el modelo dinaacutemico

3311 Velocidad de un punto del manipulador

Para el caacutelculo de la energiacutea cineacutetica de un manipulador de n grados de libertad es necesario calcular la velocidad de cada una de las articulaciones En esta seccioacuten se muestra el desarrollo del caacutelculo de la velocidad en un punto del mismo

Figura 28

En la figura anterior se muestra un eslaboacuten respecto de dos sistemas de referencia de las articulaciones sobre las que rota (i-1 e i) ademaacutes del sistema de referencia 0 El punto designado por la letra P seraacute el punto sobre el que se calcularaacute la velocidad La posicioacuten del punto P respecto al eslaboacuten i es expresada mediante el vector r

La velocidad de dicho punto respecto al sistema de coordenadas de la base 0 se corresponde con la expresioacuten

푣 equiv 푣 = 푟 = 푇

휃

middot 휃 middot 푟 (314)

donde se debe tener en cuenta el hecho de que la derivada de la posicioacuten respecto al propio sistema de coordenadas es nulo como se expresa en la siguiente igualdad


0ii r (315)

La velocidad valor a calcular es la derivada de la posicioacuten Debido a que las matrices de transformacioacuten T contienen elementos trigonomeacutetricos su derivada parcial respecto a los aacutengulos implica operaciones complejas y con un alto costo computacional Por ello y debido a que el mismo resultado se puede obtener mediante la utilizacioacuten de operaciones matriciales maacutes eficientes computacionalmente que las derivadas parciales se utilizaraacuten estas uacuteltimas Matemaacuteticamente estos pasos pueden implementarse mediante la utilizacioacuten de una matriz de dimensiones 4x4 denominada Qj definida para articulaciones de revolucioacuten Esta equivalencia con la derivada se muestra en la siguiente ecuacioacuten

푇

휃= 푄 middot 푇 (316)

Es importante tener en mente el hecho de que la derivada parcial de una matriz de transformacioacuten homogeacutenea de una matriz i

i T1 respecto a j representa el efecto del

movimiento de la articulacioacuten j sobre el eslaboacuten i La velocidad de un eslaboacuten 푣 expresada en la ecuacioacuten (314) se simplifica utilizando la matriz Qj y queda como

푣 =푑푟푑푡

= 푇 middot 푄 middot 푇 middot 휃 middot 푟 (317)

3312 Energiacutea Cineacutetica

La derivada de la energiacutea cineacutetica de una masa infinitesimal dmi perteneciente a un eslaboacuten i situada en ir

0 y que se mueve con una velocidad ii vv 0 respecto al sistema de coordenadas de la base 0 es

(318)

Esta expresioacuten tambieacuten puede escribirse utilizando las matrices de transformacioacuten homogeacuteneas de cada uno de los eslabones utilizando la funcioacuten matemaacutetica traza

2)(21

iii vdmdK


(319)

La energiacutea cineacutetica del eslaboacuten completo es por lo tanto la integral de esta expresioacuten

(320)

El teacutermino iT

ii

ii dmrr middotmiddot corresponde con la pseudomatriz de inercia de modo que

(321)

Expresada para un mecanismo de n grados de libertad la energiacutea cineacutetica total seriacutea

(322)

La energiacutea cineacutetica es por lo tanto una funcioacuten escalar que depende de la posicioacuten y velocidad de las articulaciones

3313 Energiacutea Potencial

La energiacutea potencial 푃 de un eslaboacuten i en un campo gravitatorio g es

ii

iiiii rTgmrgmP middotmiddotmiddot)middot(middot 00 (323)

donde los vectores ir0 y i

ir representan la posicioacuten del centro de masa del eslaboacuten i respecto a los sistemas de coordenadas 0 e i respectivamente y la aceleracioacuten debida a la gravedad definida por el vector

(324)

kj

i

j

Ti

kkki

Ti

ii

ii

ki

jjji TQTdmrrTQTTrdK middotmiddot)middotmiddotmiddot(middotmiddot)middotmiddot(middotmiddot

21

1

11

0

1

11

0

kj

i

j

Ti

kkki

Ti

ii

ii

ki

jjji TQTdmrrTQTTrK middotmiddot)middotmiddotmiddot(middotmiddot)middotmiddot(middotmiddot

21

1

11

0

1

11

0

kj

i

j

Ti

kkki

i

ki

jjji TQTITQTTrK middot)middotmiddotmiddot()middotmiddot(middotmiddot

21

1

11

0

1

11

0

kj

n

i

i

j

i

k

Ti

kkkii

jjji TQTITQTTrK middotmiddot])middotmiddotmiddot()middotmiddot[(middot

21

1 1 1

11

011

0

TZYX gggg ]0[


Se trata de un vector 4x1 respecto al sistema de coordenadas de la base 0 El signo negativo de la energiacutea potencial indica que el trabajo se realiza sobre el sistema para mover el eslaboacuten i en contra de la fuerza de la gravedad La energiacutea potencial total del manipulador es la suma de la energiacutea potencial de todos sus eslabones de forma que

(325)

Como ir0 depende de la posicioacuten de las articulaciones θi la energiacutea potencial de un

manipulador se describe por una funcioacuten escalar que depende de estas variables

3314 Ecuaciones del Movimiento del Manipulador

Una vez obtenidas las expresiones de la energiacutea cineacutetica y potencial de los eslabones del manipulador se puede obtener la expresioacuten del Lagrangiano a traveacutes de la siguiente ecuacioacuten

퐿 = 퐾 minus 푃 (326)

(327)

De acuerdo con la formulacioacuten de Euler-Lagrange los pares que se deben generar en las articulaciones siguen la siguiente expresioacuten

휏 =푑푑푡

middot퐿휃

minus퐿휃

(328)

El par de cada articulacioacuten viene dado por la ecuacioacuten anterior que extendida utilizando la igualdad de la ecuacioacuten (328) se define de la siguiente manera

(329)

Donde

n

ii

iii

n

ii rTgmPP

1

0

1middotmiddotmiddot

n

ii

iiikj

n

i

i

j

i

k

Ti

kkkii

jjj rTgmTQTITQTTrL

1

0

1 1 1

11

011

0 middotmiddotmiddotmiddotmiddot])middotmiddotmiddot()middotmiddot[(middot21

ikj

n

j

n

kijkj

n

jiji GhM

middotmiddotmiddot)(1 11


los coeficientes de Mij estaacuten relacionados con la aceleracioacuten de cada articulacioacuten y representan la inercia que carga el actuador Se conoce como la matriz de inercias y se define

(330)

Los coeficientes hijk representan el par de reaccioacuten inducido por la velocidad en la articulacioacuten i Los iacutendices j y k estaacuten relacionados con las velocidades de las articulaciones respectivas cuya interaccioacuten dinaacutemica provoca par de reaccioacuten en la articulacioacuten i Se conoce como la fuerza de Coriolis que actuacutea en la articulacioacuten i debido a la velocidad de las articulaciones j y k

(331)

Los teacuterminos en los que aparece la gravedad g representan el momento generado por la gravedad en cada articulacioacuten Los coeficientes de la matriz Gi representan las fuerzas de la gravedad que actuacutean sobre el eslaboacuten i debido a la masa de los eslabones desde i hasta n es decir cada eslaboacuten del manipulador se ve sometido a los momentos generados por el peso propio y el de los eslabones posteriores

(332)

Los teacuterminos dij determinan la velocidad de cambio de los puntos ii r del

eslaboacuten i es decir la variacioacuten de la posicioacuten y orientacioacuten del sistema de coordenadas i respecto al sistema de coordenadas de la base 0 debido a la variacioacuten de la posicioacuten de la articulacioacuten j

(333)

n

jip

Tpippjij dIdTrM

)max(middotmiddot

n

kjip

Tpip

p

pkijk dI

dd

Trh)max(

middotmiddot)(

pp

n

ipiPi rdgmG

1

middotmiddotmiddot

ijijTQT

d ij

jjij 0

middotmiddot 11

0


(334)

La ecuacioacuten del modelo dinaacutemico lineal proporcionaraacute un conjunto de n ecuaciones diferenciales no lineales de segundo orden para cada uno de los eslabones del manipulador Estas ecuaciones son denominadas ecuaciones del movimiento o ecuaciones dinaacutemicas del movimiento del manipulador

Ademaacutes de los eslabones el contrapeso tiene efecto en la inercia del manipulador e introduciraacute una variacioacuten dinaacutemica en la tercera articulacioacuten Esta consideracioacuten ya se ha tenido en cuenta en el caacutelculo del centro de gravedad de dicha articulacioacuten

Para simplificar la ecuacioacuten (346) se define un vector que auacutene los pares de todos los eslabones y que simplifique la expresioacuten expresaacutendola de la siguiente manera

(335)

En la que M seraacute la matriz completa de inercias a la que habraacute que multiplicar las aceleraciones de las articulaciones establecidas en un vector H seraacute la matriz con los teacuterminos de Coriolis y centriacutefugos y G es la matriz de la gravedad

3315 Introduccioacuten de los efectos de friccioacuten

Durante el desarrollo de las ecuaciones de la dinaacutemica del manipulador no se han tenido en cuenta todos los efectos que estaacuten actuando sobre el manipulador La introduccioacuten de todos ellos es imposible ya que ademaacutes de no poder conocer todos los datos necesarios la utilizacioacuten de un modelo demasiado complejo no resultariacutea realizable aportando ademaacutes poca diferencia con respecto a los factores ya tenidos en cuenta que son los de mayor influencia en la dinaacutemica Hasta ahora se han considerado las fuerzas que surgen debido a la mecaacutenica de soacutelido riacutegido Sin embargo para tener un modelo dinaacutemico maacutes completo del manipulador se introduciraacute un nuevo factor que tiene una influencia apreciable Ese factor es la friccioacuten En todas las articulaciones del manipulador se disponen de unidades de reduccioacuten donde la friccioacuten puede ser considerablemente alta

Existen diferentes modelos para el modelado de la friccioacuten En primer lugar se tiene la Friccioacuten Viscosa en la que el par que se opone al movimiento debido a la friccioacuten es proporcional a la velocidad de giro de la articulacioacuten

kijikjiTQTQTjkiTQTQT

di

jjj

kkk

ik

kkj

jj

k

ij

0middotmiddotmiddotmiddotmiddotmiddotmiddotmiddot

)( 11

11

0

11

11

0

)()()middot( GHM


(336)

siendo es la constante de friccioacuten viscosa del material

En segundo lugar se tiene la Friccioacuten de Coulomb en la que se considera el signo de la velocidad de giro de la articulacioacuten

(337)

Donde c es la constante de Coulomb del material

El modelo final a utilizar incluye ambos efectos de friccioacuten de tal forma que

(338)

La friccioacuten depende tambieacuten de la posicioacuten de las articulaciones debido sobre todo a que las etapas de reduccioacuten no son perfectamente redondas por lo que su excentricidad provoca que la friccioacuten variacutee Por lo tanto la funcioacuten que determine la friccioacuten seraacute dependiente de las posiciones y de las velocidades angulares

Una vez determinado el modelo de friccioacuten este se antildeade como un teacutermino maacutes en la ecuacioacuten de la dinaacutemica inversa

(339)

3316 Dinaacutemica de los actuadores

Para tener un modelo dinaacutemico completo del sistema real deberiacutea antildeadirse a las ecuaciones del propio manipulador la influencia de los actuadores sobre la dinaacutemica Al tratarse de motores de corriente continua factores como la inductancia o la constante de par influyen en la dinaacutemica al no comportarse el motor de manera ideal Sin embargo los factores dinaacutemicos de los motores seraacuten compensados por los Drivers (apartado 433) por lo que en el lazo de control no seraacute necesario tenerlos en cuenta Aun asiacute es interesante plantear la expresioacuten dinaacutemica con la influencia de estos paraacutemetros para conocer realmente lo que estaacute sucediendo en el lazo de control del servomotor y tener constancia de todos los paraacutemetros que se tienen en cuenta a la hora de realizar el control de un manipulador completo incluyendo sus actuadores que en este caso son motores Se supone una inductancia del inducido despreciable

middotfriccioacuten

)middotsgn( cfriccioacuten

)middotsgn(middot cfriccioacuten

)()()()middot( FGHM


Las dinaacutemicas de los motores que mueven las articulaciones vienen dadas por las ecuaciones desacopladas para cada uno de ellos

퐽 middot 휃 + 퐵 middot 휃 + 퐹 + 푅 = 퐾 middot 푣 (340)

Donde 퐽 es la inercia del motor B la constante de amortiguacioacuten del rotor 퐹 la friccioacuten del motor 푅 la resistencia de armadura 퐾 la constante de par y 푣 la tensioacuten del motor 휃 y 휃 son la aceleracioacuten y la velocidad del rotor respectivamente

La relacioacuten de transmisioacuten del acoplamiento del motor i a la articulacioacuten del brazo asociada es 푟 que se define de manera que

휃 = 푟 middot 휃 (341)

Utilizando la igualdad anterior en la foacutermula (356)

(퐽 + 푅 middot 푀) middot 휃 + (퐵 + 푅 middot 퐻 ) middot 휃 + (푅 middot 퐹 + 푅 middot 퐹) + 푅 middot 퐺 = 푅 middot 퐾 middot 푣 (342)

Esta foacutermula equivaldriacutea a una nueva expresioacuten dinaacutemica tal que

휏 = 푀 (휃) middot 휃 + 퐻 휃 휃 + 퐹 휃 휃 + 퐺 (휃) (343)

34 Dinaacutemica Directa

Como se vio en el caacutelculo de la dinaacutemica inversa cuando se utiliza la formulacioacuten de Euler-Lagrange se obtienen unas matrices que describen dinaacutemicamente al mecanismo Por lo tanto una vez que se han obtenido dichas matrices el caacutelculo dinaacutemico directo tambieacuten puede ser llevado a cabo simplemente variando el dato del que se dispone es decir en lugar de introducir las aceleraciones y obtener el par se utilizariacutean los datos de par necesario y se obtendriacutean las aceleraciones generadas por estos pares

El planteamiento de la dinaacutemica directa de un manipulador permite conocer la posicioacuten velocidad y aceleracioacuten de los elementos que lo constituyen ante un par externo aplicado en sus articulaciones mediante los actuadores del sistema Este modelo se utilizaraacute para la simulacioacuten dinaacutemica del movimiento de los manipuladores

Para la simulacioacuten del manipulador se necesita un modelo que represente el comportamiento del manipulador ante la recepcioacuten de los pares generados por el control Por una parte se estableceraacute una equivalencia a traveacutes de la dinaacutemica inversa


ya realizada y por otro se crearaacute un modelo en Simulink (61-Simulacioacuten dinaacutemica) que represente al manipulador y simule su dinaacutemica directa

Para el planteamiento de una equivalencia matemaacutetica se parte de las ecuaciones dinaacutemicas del movimiento desarrolladas para la dinaacutemica inversa A partir de ellas el caacutelculo de la simulacioacuten requiere uacutenicamente la resolucioacuten de la misma para la aceleracioacuten

)]()()()[(1 FGHM (344)

Dadas las condiciones iniciales del movimiento del manipulador se pueden aplicar teacutecnicas de integracioacuten numeacuterica para integrar la aceleracioacuten y calcular futuras posiciones y velocidades del mismo (paraacutemetros que seraacuten realimentados en los lazos de control) Para ello existen numerosos meacutetodos matemaacuteticos diferentes El meacutetodo elegido es el trapezoidal y es realizado a traveacutes del bloque de integracioacuten disponible en Simulink


4 Control

El problema de control de robots manipuladores es el problema de determinar las entradas conjuntas necesarias para hacer que el efector final ejecute un movimiento ordenado Las entradas conjuntas pueden ser fuerzas conjuntas o pares siendo estos paraacutemetros entradas a los actuadores por ejemplo entradas de tensioacuten a los motores en funcioacuten del modelo utilizado para el disentildeo del controlador El movimiento ordenado se suele especificar como una secuencia de posiciones y orientaciones del efector final o como una trayectoria continua Hay muchas teacutecnicas de control y metodologiacuteas que se pueden aplicar para el control de los manipuladores El meacutetodo de control particular elegido asiacute como la manera en que se aplica puede tener un impacto significativo en el rendimiento del manipulador y por consiguiente en el rango de sus posibles aplicaciones Por ejemplo el seguimiento de trayectorias continuas requiere una arquitectura de control diferente que un control punto a punto que posiciona al manipulador en el punto objetivo sin tener en cuenta la trayectoria seguida hasta alcanzarlo Ademaacutes el disentildeo mecaacutenico del propio manipulador influiraacute en el tipo de sistema de control necesario Esto crea la necesidad de una compensacioacuten entre hardware y software es decir entre la estructura mecaacutenica del sistema y la arquitectura y la programacioacuten del controlador

La mayoriacutea de los manipuladores son accionados por actuadores que suministran una fuerza o un par motor para provocar el movimiento de los eslabones En este caso se necesita un algoritmo para calcular pares de torsioacuten que haraacute que se produzca el movimiento deseado El problema de la dinaacutemica es fundamental para el disentildeo de tales algoritmos pero no constituye en siacute misma una solucioacuten Una preocupacioacuten primordial de un sistema de control de posicioacuten es compensar automaacuteticamente los errores en el conocimiento de los paraacutemetros de un sistema y suprimir las perturbaciones que tienden a inestabilizar el sistema Para lograr esto los sensores de posicioacuten y velocidad son muestreados por el algoritmo de control que calcula los comandos de par para los actuadores En los robots industriales es importante tener en cuenta su dinaacutemica no lineal cuando se sintetizan los algoritmos de control

41 Generador de trayectorias

Para realizar el posterior control dinaacutemico del sistema primero es necesario generar la trayectoria que se debe seguir para alcanzar la posicioacuten y orientacioacuten del efector final deseada a partir de la posicioacuten y orientacioacuten en el instante de tiempo actual Dicha trayectoria debe generarse teniendo en cuenta una serie de limitaciones y caracteriacutesticas con el fin de evitar choques disminuir el tiempo total de los movimientos etc

Existen dos posibles soluciones para realizar la generacioacuten de las trayectorias de las articulaciones Cada una de ellas tiene sus ventajas y sus inconvenientes asiacute que se exponen ambas a continuacioacuten y se elegiraacute la maacutes adecuada en funcioacuten de las prioridades del proyecto


Una posible solucioacuten es la de generar la trayectoria en el espacio cartesiano y posteriormente obtener los puntos de las variables articulares con las que trabajaraacute el control dinaacutemico Utilizando este meacutetodo se generaraacute una trayectoria libre de colisiones maacutes faacutecilmente obteniendo en todo momento la posicioacuten y orientacioacuten deseada de cada articulacioacuten Como desventaja este meacutetodo tiene un alto coste computacional que lo ralentiza al tener que utilizarse la cinemaacutetica inversa para la transformacioacuten de cada uno de los puntos del espacio cartesiano en valores de las variables articulares Por lo tanto es un meacutetodo lento en comparacioacuten con la generacioacuten de la trayectoria directamente en las variables articulares La generacioacuten de trayectorias en el espacio cartesiano presenta ademaacutes el problema de que existen muacuteltiples soluciones al utilizar la cinemaacutetica inversa por lo que podriacutea no tratarse de una trayectoria adecuada al utilizar configuraciones de las articulaciones que implican movimientos que no son oacuteptimos en cuanto a tiempo o a eficiencia energeacutetica Otra de las desventajas al utilizar el espacio cartesiano como referencia para la generacioacuten de la trayectoria es que no se pueden tener en cuenta las singularidades del manipulador por lo que alguna de las velocidades articulaciones podriacutea aumentar excesivamente causando un problema Tampoco se tiene en cuenta el espacio de trabajo por lo que algunos puntos pueden ser inalcanzables para el manipulador

La otra posible solucioacuten consiste en generar la trayectoria directamente en las variables articulares Teniendo los puntos de inicio y la referencia final del efector se trasladan dichos puntos del espacio cartesiano a las variables articulares y posteriormente se generan los puntos de la trayectoria directamente en las variables que utilizaraacute el control dinaacutemico Generar la trayectoria directamente con las variables a utilizar posteriormente evita el uso reiterado de las conversiones que se produciacutean con el meacutetodo anterior Ademaacutes se tienen en cuenta los condicionantes directos que presenta el manipulador a la hora de colocarse en una determinada posicioacuten o los puntos que puede llegar a alcanzar por ejemplo Sin embargo es difiacutecil evitar las colisiones ademaacutes de que no se puede distinguir la posicioacuten de la orientacioacuten al tratarse de un uacutenico dato y no de una matriz como en el caso de las variables en el espacio cartesiano

Teniendo en cuenta las ventajas y desventajas presentadas se opta por utilizar un generador de trayectorias que trabaje directamente en el espacio de las articulaciones teniendo en cuenta que se debe prestar especial atencioacuten que las trayectorias esteacuten libres de colisiones Este meacutetodo seraacute realizado para la simulacioacuten de manera que se puedan crear trayectorias con las que probar los lazos de control que se propondraacuten es este apartado Sin embargo teniendo en cuenta la finalidad del proyecto la implementacioacuten necesitaraacute la realizacioacuten de trayectorias en el espacio cartesiano Se propone utilizar el manipulador para tareas de mecanizado por lo que la trayectoria seguida por la herramienta debe poder ser totalmente definida y controlada Esta parte no seraacute realizada en este trabajo aunque forme parte del proyecto Mill2Matic

Ademaacutes de la referencia a utilizar para definir las trayectorias a seguir por el manipulador se debe elegir el tipo de trayectoria deseado Para nuestro manipulador la trayectoria que se generaraacute seraacute de tipo coordinada o isoacutecrona Esto quiere decir que la articulacioacuten que maacutes tiempo necesite para alcanzar la posicioacuten final condicionaraacute a las demaacutes de manera que todas ellas empiecen y terminen su


movimiento en el mismo instante de tiempo De esta manera se evitan esperas innecesarias permitiendo a las articulaciones maacutes raacutepidas utilizar unas especificaciones menos exigentes para los motores que las mueven

Figura 29 Modelo utilizado en la simulacioacuten de las trayectorias

Los datos de posicioacuten y velocidad deseados en la herramienta o efector final donde se tendraacuten en cuenta las dimensiones de la herramienta utilizada (o en caso de que no se utilice ninguna se utilizaraacuten las coordenadas de la brida final del manipulador) hacen referencia a las coordenadas respecto del sistema la referencia del sistema Estaacuten por lo tanto expresadas en el espacio cartesiano

Otro dato de entrada ademaacutes de la posicioacuten y orientacioacuten de la herramienta deseadas va a ser la velocidad lineal deseada en ese mismo punto final del manipulador Las velocidades se expresaraacuten tambieacuten en el espacio cartesiano de manera que esteacuten referidas a la base del manipulador

Para la obtencioacuten de los valores articulares correspondientes se crearaacuten las trayectorias a partir de los datos cartesianos Mediante el uso de la inversa del Jacobiano (J-1) se transformaraacute la velocidad del efector final a coordenadas articulares de cada una las articulaciones del manipulador Ademaacutes a traveacutes de la cinemaacutetica inversa se obtendraacuten los valores de posiciones y velocidades angulares a partir de la posicioacuten y orientacioacuten del efector final Las velocidades articulares obtenidas seraacuten las necesarias para alcanzar la velocidad lineal deseada Por lo tanto durante las aceleraciones y deceleraciones de las articulaciones no se alcanzaraacute ese valor Cuando todas las articulaciones alcancen el valor de velocidad obtenido del Jacobiano seraacute cuando se alcance la velocidad lineal introducida Este dato seraacute tenido en cuenta por el generador de trayectorias a la hora de establecer la coordinacioacuten isoacutecrona

Trabajando en el espacio cartesiano es necesario tener en cuenta la limitacioacuten de que la trayectoria definida puede contener puntos que esteacuten fuera del espacio alcanzable de trabajo Para tratar este problema existen dos posibles soluciones Una de ellas es para el movimiento del manipulador cuando se intenta alcanzar uno de esos puntos indicando que la trayectoria deseada no puede ser realizada Otra de ellas es realizar un caacutelculo previo al inicio del movimiento de los puntos de la tratectoria para poder indicar inicialmente de la trayectoria seraacute realizable Dependiendo de la capacidad de caacutelculo o por ejemplo de los problemas que puedan resultar de finalizar un


movimiento en un punto intermedio de la trayectoria se debe elegir una de las dos formas anteriores de afrontar el problema

411 Modelado matemaacutetico

Para realizar pruebas en Matlab se realizaraacute primero una simulacioacuten de generacioacuten de trayectorias en el espacio articular Las trayectorias de cada articulacioacuten estaraacuten limitadas en velocidad y aceleracioacuten y se sincronizaraacuten de manera que inicien y finalicen los movimientos en los mismos instantes de tiempo Mediante esta simulacioacuten se podraacute comprobar el funcionamiento del lazo de control aunque finalmente no sea el modelo de trayectorias a utilizar Las trayectorias en el espacio cartesiano seraacuten generadas posteriormente en el hardware de control

A partir de los datos de entrada que representan las posiciones y orientaciones a alcanzar a traveacutes de la cinemaacutetica inversa se calculan los valores de los aacutengulos que deben formar las articulaciones El conjunto de valores articulares se conoce como puntos de viacutea y definen los puntos a traveacutes de los cuales una articulacioacuten deberaacute pasar conforme a los paraacutemetros de entrada introducidos Como dato de entrada se puede introducir tambieacuten los tiempos que se quiere que tarde el manipulador en llegar a las posiciones y orientaciones dadas Esos tiempos pueden no ser realizables ya que se dispone de una serie de datos que limitan la aceleracioacuten y la velocidad maacutexima de cada articulacioacuten Ademaacutes las coordenadas cartesianas que se introducen como paraacutemetros deben corresponderse con puntos que se encuentren dentro del espacio de trabajo para que la posicioacuten sea alcanzable por el manipulador

Una vez que los datos de entrada han sido introducidos se calcularaacute una funcioacuten definida a trozos formada por funciones lineales para la trayectoria Las funciones lineales unen los puntos correspondientes a los datos de entrada consecutivos de la trayectoria

Las ecuaciones que definen los tramos de aceleracioacuten velocidad constante y deceleracioacuten del perfil trapezoidal de velocidad angular se describen por las ecuaciones que se presentan a continuacioacuten En el caso del tramo de aceleracioacuten la ecuacioacuten que calcula la duracioacuten del periodo es

푡 =휔훼

(41)

Siendo 휔 la velocidad angular maacutexima a alcanzar en el tramo de velocidad constante y 훼 la aceleracioacuten maacutexima caracteriacutestica de esta fase Dado que se quiere que el perfil de velocidad sea trapezoidal este tiempo calculado coincidiraacute tambieacuten con el tiempo de deceleracioacuten de cada una de las articulaciones


푡 = 푡 (42)

Con estos dos datos y a partir del incremento de posicioacuten angular que se le pide a la articulacioacuten se puede calcular el tiempo que la articulacioacuten se mantiene a maacutexima velocidad

∆휃 =12

middot 푡 middot 휔 + 푡 middot 휔 +12

middot 푡 middot 휔 (43)

Donde 푡 define la duracioacuten de esa fase Despejando y teniendo en cuenta la igualdad de la ecuacioacuten (42) se obtiene que su valor es

푡 =∆휃휔

minus 푡 (44)

Con los valores que definen la duracioacuten de cada una de las fases del perfil y con los valores de velocidad y aceleracioacuten angular ya se pueden definir completamente los valores de posicioacuten velocidad y aceleracioacuten de cada articulacioacuten para la realizacioacuten de una trayectoria hasta un punto dado En la fase de aceleracioacuten las ecuaciones utilizadas son para la posicioacuten velocidad y aceleracioacuten respectivamente

휃 =12

middot 훼 middot 푡 (45)

휔 = 훼 middot 푡 (46)

훼 = 훼 (47)

Tras la fase de aceleracioacuten se produce la fase de velocidad constante definida

휃 = 휔 middot 푡 (48)


휔 = 휔 (49)

훼 = 0 (410)

Por uacuteltimo estaacute la fase de deceleracioacuten en la que las ecuaciones son

휃 = 휔 middot 푡 minus12


휔 = 휔 minus 훼 middot 푡 (412)

훼 = minus훼 (413)

Todos los caacutelculos anteriores no tienen en cuenta para la generacioacuten de la trayectoria que eacutesta sea coordinada Tampoco tienen en cuenta el dato de velocidad que deben alcanzar Para integrar estas dos condiciones una vez calculadas las trayectorias con las foacutermulas desarrolladas en este apartado se debe establecer el tiempo para alcanzar el siguiente punto de cada una de las articulaciones como el maacuteximo obtenido de todas ellas de manera que la articulacioacuten que necesite maacutes tiempo para alcanzar la posicioacuten deseada seraacute la que condicione al resto Ademaacutes hay que tener en cuenta las limitaciones de cada una de las articulaciones impuestas por los motores y los aspectos cinemaacuteticos del propio manipulador Para ajustar el nuevo tiempo se incrementaraacuten los tiempos de aceleracioacuten y deceleracioacuten de manera que coincidan con los tiempos de la trayectoria maacutes lenta La consecuencia de este ajuste es el descenso de la aceleracioacuten utilizada aunque se intente alcanzar la misma velocidad para poder cumplir con la velocidad lineal y rotacional en la herramienta con una trayectoria isoacutecrona

Para la simulacioacuten en MATLAB se eligioacute la generacioacuten de trayectorias en el espacio articular por los motivos mencionados al inicio del apartado Sin embargo el uso final del manipulador propuesto en el proyecto consistiraacute en realizar tareas de mecanizado que actualmente realizan maacutequinas especializadas Para esas tareas ademaacutes de la velocidad del efector final tambieacuten se necesita que la trayectoria siga liacuteneas rectas o circulares en el espacio por lo que la trayectoria finalmente deberiacutea ser generada en el espacio cartesiano para poder conseguirlo Esta tarea puede ser realizada en el hardware de control por medio de instrucciones especiacuteficas que aseguran la trayectoria coordinada de todas las articulaciones ademaacutes de respetar los liacutemites de los motores intentando alcanzar la velocidad de referencia introducida para un


sistema de referencia determinado que como se acaba de mencionar se realizaraacute para el efector final con respecto al sistema de coordenadas del manipulador (espacio cartesiano) La capacidad de coacutemputo del hardware de control permitiraacute que la conversioacuten de las trayectorias al espacio articular en caso de que fuese necesaria se haga en un tiempo que permita la obtencioacuten de datos en el periodo de muestreo del lazo de control

42 Sensores

Para el control del manipulador en necesario disponer de sentildeales que contengan informacioacuten del mismo para poder realizar la realimentacioacuten del lazo de control Los lazos de control propuestos son lazos cerrados en los que se necesita conocer el estado del sistema a controlar en cada instante de tiempo en el que se ejecuta el control (frecuencia de muestreo) Las ventajas del uso de la realimentacioacuten en los lazos de control son las siguientes

Estabilidad la realimentacioacuten permite controlar sistemas inestables

Robustez el conocimiento de los valores reales permite que la sentildeal de control sea adecuada incluso en situaciones que presenten alguna anomaliacutea o perturbacioacuten

Correccioacuten permite controlar sistemas incluso cuando estos tienen errores de modelado

El sistema a controlar precisa debido a las ventajas enumeradas un sistema de control en lazo cerrado en el que se trabaje con paraacutemetros medidos directamente sobre el propio manipulador que serviraacuten de realimentacioacuten

Las variables maacutes comunes que se utilizan en los manipuladores roboacuteticos son las posiciones velocidades y aceleraciones angulares La informacioacuten de estas medidas utilizada en los modelos matemaacuteticos del manipulador (cinemaacuteticas y dinaacutemicas) permiten conocer el comportamiento del manipulador y su posicionamiento articular y cartesiano sin necesidad de utilizar sensoacuterica adicional para la medida de otros paraacutemetros Aun asiacute el uso de sistemas de medicioacuten de interferometriacutea laacuteser o de sistemas de visioacuten por ejemplo podriacutean ser tambieacuten utilizados para realizar otros tipos de control o mejorar los propuestos Dado que el manipulador Kuka KR500-2 del que se dispone incorpora como sensores resolvers analoacutegicos los lazos de control propuestos se adaptan a la disposicioacuten de medidas de posicioacuten y velocidad angular El uso del dispositivo Laacuteser-Tracker como sensor adicional que aporta informacioacuten de la posicioacuten y orientacioacuten del efector final da lugar tambieacuten a la propuesta de otros tipos de lazo de control

421 Sensores electromagneacuteticos rotativos


Los sensores rotativos se usan para medir la posicioacuten y la velocidad angular en ejes Se trata de sensores analoacutegicos Estaacuten formados por un elemento giratorio que se conoce como el rotor que se trata de un eje que gira solidario al eje del cual se quieren conocer las medidas y por otro elemento el estator que es estaacutetico y se situacutea coaxialmente al rotor

Este tipo de sensores constan de las mismas partes que los motores o generadores eleacutectricos Sin embargo dado que su objetivo es medir y no la conversioacuten de energiacutea presentan respecto de los generadores una carga muy pequentildea con el fin de alterar lo miacutenimo posible al dispositivo al que van acoplados Esta es la causa por la cual su tamantildeo es menor y por la que las sentildeales eleacutectricas que ofrecen son de escasa potencia ademaacutes de ser el motivo diferenciador de los dispositivos a los que se acoplan

422 Resolvers

Los resolvers son sensores electromagneacuteticos rotativos que miden posiciones y velocidades angulares Entre sus ventajas principales se encuentran su gran robustez y precisioacuten en las medidas asiacute como una gran fiabilidad y capacidad de trabajo en ambientes extremos Su uso estaacute muy extendido en roboacutetica frente a otros tipos de sensores debido a las ventajas antes mencionadas necesarias sobre todo en manipuladores de uso en entornos industriales en los que hay mucho ruido electromagneacutetico y que requieren una precisioacuten elevada para la realizacioacuten de las tareas Ademaacutes presentan otra serie de caracteriacutesticas que los hacen adecuados para un gran nuacutemero de aplicaciones de diferentes sectores como en aeronaacuteutica o en tecnologiacutea aeroespacial donde no existe ninguna alternativa dadas las condiciones de funcionamiento y el desempentildeo requerido

Definidos por los elementos que los componen los resolvers son transformadores variables rotativos en los que tanto el rotor como el estator disponen de dos devanados respectivamente y que estaacuten dispuestos perpendicularmente El procedimiento que permite conocer la posicioacuten angular consiste en excitar con una sentildeal senoidal uno de los devanados del estator mientras que el otro permanece sin alimentacioacuten Debido a la disposicioacuten perpendicular de los devanados y de la disposicioacuten del estator respecto del rotor (tambieacuten perpendicular) se induce en uno de los devanados del rotor una tensioacuten senoidal maacutexima en la posicioacuten de referencia o inicial siendo nula en el otro devanado Esta situacioacuten de referencia se produce cuando existe la direccioacuten de alineacioacuten Cuando se produce un giro es decir cuando el rotor se desplaza un aacutengulo θ la tensioacuten en los devanados del rotor variacutea seguacuten las siguientes expresiones

푒 = 푒 cos휃 (414)


푒 = 푒 sen휃 (415)

Siendo 푒 la tensioacuten en el estator y 푒 la tensioacuten en el rotor Las expresiones indican que el coeficiente de acoplamiento entre el rotor y el estator variacutea seguacuten sea la posicioacuten angular del eje

A partir de las amplitudes de tensioacuten de estas dos sentildeales se puede deducir el giro que las produce Es necesario conocer la frecuencia de la sentildeales senoidal que alimenta al resolver para poder interpretar correctamente las sentildeales ya que si no podriacutea dar lugar a error la forma de la onda que se produce

Ademaacutes de para poder conocer la posicioacuten angular los resolvers tambieacuten son utilizados para medir la velocidad angular de los ejes Cuando el eje del rotor gira con una velocidad angular mucho menor de la frecuencia de la sentildeal de excitacioacuten del devanado primario la sentildeal de salida estaraacute modulada en amplitud con una frecuencia angular igual a la velocidad angular de giro Esta caracteriacutestica permite a los resolvers poder medir la velocidad angular de giro de los ejes a los que se acoplan En la siguiente figura se representa un ejemplo de los valores que se pueden obtener de un resolver

Figura 30 Ejemplo de sentildeales generadas por un resolver analoacutegico

Sobre la sentildeal de alimentacioacuten a medida que se produce el giro se van variando las sentildeales del seno y del coseno del aacutengulo girado La sentildeal roja representa la forma de


onda que contiene la informacioacuten de la sentildeal medida A partir del desfase de las dos sentildeales se puede obtener finalmente las medidas de posicioacuten y velocidad angular

423 Encoacuteder Incremental

La diferencia fundamental entre los encoders y los resolvers se encuentra en que el primero es un sistema digital y el segundo un sistema analoacutegico Para comunicar la informacioacuten obtenida de los resolvers los Drivers convierten la informacioacuten en sentildeales digitales correspondientes a las mismas medidas pero correspondientes a las de los dispositivos conocidos como encoders incrementales por lo que se expone su funcionamiento ya que tambieacuten se trata de una sentildeal con la que se va a trabajar

Los encoders son codificadores oacutepticos que estaacuten compuestos por un disco que gira de manera solidaria al eje sobre el que se realiza la medida Ese disco estaacute dividido por varios sectores que presentan alguna caracteriacutestica oacuteptica diferenciadora entre espacios consecutivos Generalmente los sectores estaacuten formados por ranuras que permiten diferenciar el espacio en el que estaacute el material del disco y en el que no Para la deteccioacuten oacuteptica se suele utilizar un emisor de luz y un fotodetector a ambos lados del disco De esta manera a traveacutes de los pulsos generados por la deteccioacuten de las ranuras se puede conocer la posicioacuten angular por medio del giro calculado En el caso de los encoders incrementales el meacutetodo de caacutelculo consiste en contabilizar el nuacutemero de pulsos generados a partir de una ranura que se toma como referencia La resolucioacuten estaacute determinada por el nuacutemero de ranuras del disco Cuanto mayor sea el nuacutemero de ranuras menor seraacute el giro que se puede llegar a detectar Al igual que los resolvers los encoders tambieacuten pueden medir la velocidad angular a partir de la frecuencia de la sentildeal recibida en el fotodetector Ademaacutes de la posicioacuten y la velocidad angular se puede detectar el sentido de giro con este tipo de dispositivos Para ello la solucioacuten maacutes habitual que suelen implementar consiste en situar dos fotodetectores contiguos de manera que dependiendo del orden de activacioacuten de ambos se puede conocer el sentido del giro A continuacioacuten se muestra una graacutefica de ejemplo de las sentildeales digitales que proporciona un encoder

Figura 31 Ejemplo de pulsos generados por un encoacuteder incremental

Las sentildeales A y B representan los pulsos que deben ser contados para determinar la posicioacuten y la velocidad El desfase entre ambas sentildeales indicariacutea el sentido de giro La


sentildeal C representa el pulso que se genera en la ranura de referencia Con el fin de tener una posicioacuten de referencia el codificador tiene en su disco una segunda pista que tiene una sola graduacioacuten Esta disposicioacuten produce en la salida de este tercer canal un pulso con cada rotacioacuten completa (360deg)

43 Actuadores

La sentildeal que se calcula en el lazo de control por dinaacutemica inversa es un vector que contiene los pares a realizar en cada articulacioacuten para seguir la trayectoria especificada Esa sentildeal es enviada al manipulador del que posteriormente se obtienen las medidas de los sensores Los actuadores que deberaacuten efectuar el movimiento son servomotores que permiten el posicionamiento preciso de su rotor Existen por lo tanto tantos motores como articulaciones ya que cada uno del ellos generaraacute el movimiento de la articulacioacuten a la que estaacuten asociados Seraacute por lo tanto necesario traducir la sentildeal enviada desde el lazo de control a valores propios de los motores que generen ese par de fuerzas calculado Esa funcioacuten la cumplen los Drivers que conocen el comportamiento de los motores y les indican la cantidad de corriente que deben suministrar para generar el par necesario en cada periodo

La actuacioacuten de los motores para seguir las trayectorias deseadas por el usuario estaacute limitada por la velocidad y aceleracioacuten maacuteximas de los propios motores Cada uno de ellos actuaraacute gracias a los Drivers que los controlan y a la trayectoria generada que tendraacute en cuenta tambieacuten sus limitaciones conforme a un perfil trapezoidal de velocidad En ese perfil de velocidad se alcanzaraacute (siempre que sea posible) la velocidad necesaria para conseguir la velocidad lineal introducida como referencia para la herramienta Hasta llegar a esa velocidad angular se produce una rampa de aceleracioacuten que puede ser ajustada Tras el periodo de velocidad constante se produce una rampa de deceleracioacuten de la misma duracioacuten que la de aceleracioacuten Todos los perfiles son ajustados de manera que se cumpla un movimiento coordinado de las juntas es decir que todas comienzan y finalizan el movimiento a la vez Este ajuste se realiza siempre teniendo en cuenta la junta que maacutes tiempo va a requerir para alcanzar la posicioacuten final Existen dos opciones para realizarla una de ellas es mantener la aceleracioacuten utilizada variando la velocidad maacutexima que se alcanza no consiguiendo asiacute alcanzar nunca la velocidad de referencia en la herramienta del manipulador la otra opcioacuten es variar las rampas de aceleracioacuten y deceleracioacuten tanto su pendiente como su duracioacuten de manera que se ajusten para recorrer el mismo espacio sin cambiar la velocidad maacutexima que se alcanza Estos ajustes no son siempre ideales pues para ciertos movimientos de juntas es imposible alcanzar la velocidad necesaria Por lo tanto el dato deseado de velocidad de la herramienta que introduciriacutea el usuario no puede alcanzarse para todos los movimientos aunque eacuteste sea menor que la velocidad maacutexima que puede alcanzar Esto se debe a que el movimiento es demasiado pequentildeo para alcanzar la velocidad maacutexima con el liacutemite de aceleracioacuten del motor Tambieacuten puede ser debido a que tras el ajuste para conseguir la coordinacioacuten de las juntas el tiempo de movimiento sea demasiado grande para conseguir la velocidad deseada en alguna de ellas


Un paraacutemetro a tener en cuenta para evitar tirones en los movimientos y la sobrecarga de los motores es el jerk Se trata de una magnitud fiacutesica que se define como la tercera derivada del movimiento Se trata por tanto de una magnitud que influye directamente sobre la aceleracioacuten definieacutendola en el tiempo de manera que no se produzcan cambios en ella de manera instantaacutenea Mediante esta magnitud se define correctamente el funcionamiento de los motores ya que utilizando uacutenicamente la posicioacuten la velocidad y la aceleracioacuten se supondriacutean aceleraciones instantaacuteneas que el motor nunca seriacutea capaz de proporcionar exigieacutendole una accioacuten que provocariacutea sobrecorrientes Las magnitudes fiacutesicas del movimiento son expresadas de la siguiente forma teniendo en cuenta dicha magnitud

푎 = 푎 + 푗 middot ∆푡 (416)

푣 = 푣 + 푎 middot ∆푡 +12

middot 푗 middot ∆푡 (417)

푥 = 푥 + 푣 middot ∆푡 +12

middot 푎 middot ∆푡 +16


A partir de la trayectoria en el espacio cartesiano para cada articulacioacuten se define un perfil trapezoidal que se corresponda con las capacidades del motor actuador hasta alcanzar la posicioacuten final

431 Servomotores

Un servomotor es un dispositivo similar a un motor de corriente continua que tiene la capacidad de ubicarse en cualquier posicioacuten dentro de su rango de operacioacuten y mantenerse estable en dicha posicioacuten Estaacute formado por un motor eleacutectrico que puede ser controlado tanto en velocidad como en posicioacuten y un sensor que mida dichos paraacutemetros y los realimente para poder controlarlos

En el caso del control de ejes como es el caso de los manipuladores roboacuteticos intervienen tanto el servomotor (con encoder o resolver acoplado) como su elemento especiacutefico de control para realizar la tarea de actuacioacuten sobre el sistema El elemento de control puede ser autoacutenomo (Driver) o una tarjeta especial de un PLC

Los servomotores poseen estructura compacta en la que incorporan dentro de la misma el sensor de medida (en este caso un resolver) el cual suministra las informacioacuten del estado del actuador al controlador es decir el Driver del servomotor Los servomotores incorporan un freno mecaacutenico como elemento de seguridad que bloquea el eje en caso de que se produzca una falta de tensioacuten debido a alguacuten fallo para asiacute evitar la caiacuteda de los eslabones posteriores a la articulacioacuten en la que se encuentre


4311 Motores Brushless

Los servomotores que utiliza el manipulador KUKA KR500-2 son los compuestos por motores de corriente alterna sin escobillas de tipo brushless Actualmente son los maacutes utilizados en la industria Estaacuten formados por un estator segmentado en el que el espacio rellenado de cobre es casi el doble que en el caso de los motores tradicionales Esta caracteriacutestica permite desarrollar una mayor potencia con un menor volumen El rotor incorpora imanes permanentes construidos con los elementos neodimio hierro y boro que proporcionan mayor densidad de flujo lo que resulta en un mejor rendimiento y en una obtencioacuten de mayor par a pesar del menor tamantildeo El tiempo de posicionamiento del servomotor se reduce gracias a la reduccioacuten de la inercia del rotor lo que permite alcanzar altas velocidades en tiempos reducidos Las principales ventajas de los motores brushless frente a otro tipo de motores como parte actuadora de los servomotores vienen dadas por las posibilidades que ofrece de controlar su velocidad y posicionamiento entre las que se encuentran unas respuestas muy raacutepidas a las sentildeales de arranque paro y variaciones sobre la marcha Consiguen gran exactitud en el posicionamiento tienen pocas peacuterdidas y tienen fiabilidad de funcionamiento

La velocidad a la que la corriente de armadura puede cambiar estaacute limitada por la inductancia del motor y por la capacidad liacutemite de su fuente de tensioacuten La frecuencia natural del sistema en lazo cerrado debe ser mucho menor que la frecuencia de corte que establecen la inductancia y la fuente de tensioacuten de los motores

4312 Etapas reductoras

El uso de etapas reductoras acopladas tras los motores responden a la necesidad de generar un par muy elevado en las articulaciones y que no puede ser proporcionado por los propios motores Las etapas reductoras estaacuten formadas por trenes de engranajes planetarios cicloidales cuyas prestaciones se adaptan a las necesidades del manipulador Permiten realizar operaciones de precisioacuten con inercia reducida gran rigidez y gran capacidad de sobrecarga La dinaacutemica de los motores es lineal y controlada y el efecto de la reduccioacuten de engranajes es en gran parte para desacoplar el sistema mediante la reduccioacuten del acoplamiento de la inercia entre las articulaciones Sin embargo la presencia de los engranajes introduce friccioacuten y backlash entre otros paraacutemetros que provocan un comportamiento no ideal de la transmisioacuten

Los lazos de control utilizaraacuten siempre unidades referidas a las articulaciones del manipulador y no a los actuadores Por lo tanto seraacute necesario tener en cuenta que las sentildeales de par que se comanden seraacuten las necesarias para mover la articulacioacuten y en ninguacuten caso se corresponderaacuten con el par que debe generar el motor Ademaacutes las sentildeales realimentadas de medidas en el actuador y no en la estructura mecaacutenica del manipulador por lo que ademaacutes de ser medidas indirectas para el control del mecanismo deben ser igualmente transformadas para poder trabajar con valores articulares


La potencia se mantiene tras el tren de engranajes por lo que

푃 = 푃 (419)

Siendo 푃 la potencia de entrada y 푃 la potencia de salida La potencia se define como

푃 = 휏 middot 휔 (420)

Para conseguir aumentar el par y teniendo en cuenta que la potencia es constante se debe disminuir la velocidad angular en la salida La relacioacuten en el tren de engranajes entre la velocidad de entrada y la de salida es

휔 middot 푟 = 휔 middot 푟 (421)

La relacioacuten de transmisioacuten del tren de engranajes y el valor a tener en cuenta a la hora de transformar los valores en el lazo de control es entonces

휔휔

=푟푟

(422)

432 Drivers

Un Driver equivale a un variador de velocidad disentildeado especiacuteficamente para el control de los servomotores Utiliza un conversor para la tensioacuten de entrada y un inversor para la tensioacuten de salida que es la que aplicaraacute al motor Esta sentildeal es definida por el circuito de control (microprocesador) correspondiente Mediante el Driver se trabaja en lazo cerrado (realimentacioacuten) lo que permite detectar los posibles errores en la actuacioacuten del motor y dar la oportuna orden de correccioacuten del mismo

Los Drivers de los motores recibiraacuten las sentildeales analoacutegicas de los resolvers de los motores que controlan respectivamente Mediante conversores analoacutegicosdigitales se obtienen sentildeales que se interpretan como las posiciones y velocidades angulares de los motores De esta manera los Drivers realizan su propio lazo de control en el que se busca obtener la posicioacuten maacutes exacta posible en funcioacuten de la referencia de par que se le exige a cada motor Asiacute se disminuye el error de posicionamiento de los


actuadores y este error mecaacutenico no pasa a formar parte del error lazo de control del manipulador mejorando la respuesta

El lazo de control puede ser de cuatro clases posicioacuten velocidad par y corriente En el caso de nuestro manipulador su lazo de control enviaraacute una sentildeal que se corresponderaacute con el par que se debe generar en cada articulacioacuten para conseguir el movimiento deseado del efector final Por lo tanto a partir de esa referencia se haraacute el lazo de control propio del servomotor Los Drivers actuales estaacuten dotados de una serie de caracteriacutesticas orientadas a facilitar la labor del programador del mismo y aumentar el rendimiento Algunas de las maacutes significativas son

Reduccioacuten del tiempo de caacutelculo de la CPU ndash posicionados maacutes raacutepidos Autoajuste Online ndash se ajusta automaacuteticamente a los movimientos de la

maacutequina Utilizacioacuten de filtros de supresioacuten de resonancia para contrarrestar el ruido de

resonancia mecaacutenica de alta frecuencia Control de velocidad realimentado Filtro de supresioacuten de resonancia del eje Seleccioacuten del modo de control de velocidad proporcionalintegral Deteccioacuten automaacutetica del tipo de motor Posibilidad de manejo manual de los servomotores Buacutesqueda de origen Liacutemite de par (para evitar sobrepar) Procesamiento regenerativo (absorcioacuten de la energiacutea regenerada al

desacelerar) Limitador de desplazamiento por software No son necesarios los limitadores

mecaacutenicos

Los Drivers tienen la capacidad de transformar las sentildeales de los de resolvers a su equivalente correspondiente a los encoders incrementales Se aprovecharaacute para realimentar los sensores a los Drivers y posteriormente los pulsos digitales obtenidos de los encoders son enviados al hardware de control Este proceso permite evitar el tratamiento de las sentildeales de los resolvers maacutes complicado que en el caso de las sentildeales digitales equivalentes

Para la programacioacuten el fabricante del Driver suministra un software mediante el cual se pueden programar todos los paraacutemetros de usuario necesarios para un total control del servomotor

44 Modelado del sistema de control

Un manipulador es un sistema multivariable desde el punto de vista del control en el que las entradas estaacuten formadas por vectores de posicioacuten de las articulaciones de sus velocidades y de sus aceleraciones La dimensioacuten de los vectores se corresponde con el nuacutemero de grados de libertad del manipulador que en el caso del KUKA KR500-2 es de 6 Respecto a las salidas se dispondraacute de los vectores de posicioacuten y de velocidad con los datos obtenidos a traveacutes de los sensores que miden las condiciones


del manipulador Se trata por tanto de un sistema MIMO (Multiple-Input Multiple-Output) Se van a proponer dos esquemas de control adecuados para este tipo de sistemas mecaacutenicos El primero de ellos el control por par calculado seraacute el primero a implementar con el fin de simular su comportamiento antes de implementar un sistema de control maacutes complejo Posteriormente se presenta el control por dinaacutemica inversa que es un tipo de control maacutes complejo pero maacutes correcto para el control del sistema real Para el control por par calculado se propone un controlador PD el maacutes utilizado para controles que tienen en cuenta la dinaacutemica del sistema y para el control por dinaacutemica inversa un controlador de tipo PID que mejora el comportamiento del controlador lineal pero aumenta la complejidad del sistema La implementacioacuten final de uno u otro se haraacute en funcioacuten de los resultados y necesidades requeridas El desarrollo de un controlador de tipo PD para el control por dinaacutemica inversa se hariacutea de manera anaacuteloga al desarrollo hecho para el control por par calculado Lo mismo ocurririacutea para un regulador de tipo PID para el control por par calculado

441 Linealizacioacuten

El modelo dinaacutemico del manipulador constituye una descripcioacuten matemaacutetica del comportamiento del mismo Se trataraacute siempre de una aproximacioacuten pues el conocimiento absoluto del sistema no puede llegar a conocerse nunca Ademaacutes la inclusioacuten de excesivos aspectos dariacutea lugar a un modelo demasiado complejo que requeririacutea demasiado tiempo de coacutemputo y que no seriacutea viable para la utilizacioacuten en un lazo de control La precisioacuten de la aproximacioacuten depende de la cantidad de aspectos tenidos en cuenta Por lo tanto se deben tener en cuenta uacutenicamente los factores suficientes que describan el sistema de manera que aporte una solucioacuten vaacutelida

El modelo dinaacutemico del manipulador seraacute no lineal seguacuten los paraacutemetros tenidos en cuenta (meacutetodo de Euler-Lagrange) Para su utilizacioacuten en el lazo de control seriacutea maacutes coacutemoda la disponibilidad de un modelo lineal por lo que puede optarse por linealizar el modelo Podriacutea hacerse una linealizacioacuten local que se utilizase para hacer aproximaciones de las ecuaciones no lineales en el entorno de un punto de funcionamiento El modelo lineal describe correctamente al no lineal (real) cuando el sistema evoluciona cerca del punto de trabajo A medida que se aleja de dicho punto la aproximacioacuten se hace maacutes inexacta hasta el punto de resultar no vaacutelida Por ello deberiacutean elegirse varios puntos de trabajo que permitiesen linealizar el modelo para todas las condiciones de trabajo del manipulador

En lugar de realizar aproximaciones que permitan obtener modelos lineales con los que trabajar en el lazo de control existen dos alternativas que permiten utilizar el modelo no lineal del manipulador De esta manera se simplifica el proceso y se trabaja con el modelo real realizado Se trata del control por par calculado y el control por dinaacutemica inversa

El problema del control del manipulador como un sistema no lineal es que los polos se mueven alrededor en el plano real-imaginario como una funcioacuten de la posicioacuten Por lo tanto no podriacuteamos seleccionar ganancias fijas que mantuviesen los polos en una ubicacioacuten conveniente (por ejemplo con amortiguamiento criacutetico) Se puede


considerar una ley de control maacutes complicada en la que las ganancias sean variables en el tiempo variando en funcioacuten de la posicioacuten del manipulador de manera tal que el sistema se mantenga siempre criacuteticamente amortiguado Esto podriacutea hacerse mediante el caacutelculo del efecto no lineal del manipulador para que fuese cancelado exactamente por un teacutermino no lineal en la ley de control de manera que la rigidez general se quedariacutea constante en todo momento Tal esquema de control se podriacutea llamar una ley de control de linealizacioacuten ya que utilizariacutea un teacutermino no-lineal para cancelar la no linealidad en el sistema controlado de forma que el conjunto del sistema cerrado sea lineal

442 Control por par calculado

El control se realizaraacute utilizando el modelo de la dinaacutemica inversa por lo que deberaacute ser lo maacutes exacto posible para que el control sea preciso y se ajuste al manipulador real El objetivo de este meacutetodo de control es el seguimiento de una trayectoria dada que seraacute el dato de entrada del bloque de control Eacuteste meacutetodo llamado control por par calculado tiene una serie de ventajas que lo hacen adecuado para el control de sistemas mecaacutenicos como los de los manipuladores

Al tener en cuenta los aspectos dinaacutemicos del sistema se genera directamente una sentildeal de control que compensa dichos aspectos

Se calcula el par que exige un movimiento particular asiacute que es posible proteger al sistema de grandes pares que podriacutean surgir si no se tuviesen en cuenta los aspectos dinaacutemicos

Las perturbaciones que puedan aparecer son reducidas ya que es un meacutetodo feed-forward (de compensacioacuten) por lo que es capaz de anticiparse

El controlador utiliza el modelo del sistema en el bucle de compensacioacuten con el fin para desacoplar la dinaacutemica del sistema

Ademaacutes de la utilizacioacuten del modelo dinaacutemico este tipo de control incorpora el uso de un controlador PD o PID

La representacioacuten en un diagrama de bloques del sistema de control se muestra en la figura


Figura 32 Control por par calculado con regulador PD

Dado que soacutelo se necesitan conocer los valores de la trayectoria deseada muchos de estos teacuterminos pueden calcularse previamente a la ejecucioacuten del lazo de control y almacenarse offline Esto supone una ventaja respecto al meacutetodo de control por dinaacutemica inversa que utiliza para el modelo dinaacutemico las variables reales del sistema se requiere ocupar un mayor espacio de memoria a cambio de tener un lazo de control mucha maacutes raacutepido al disponer de valores calculados previamente facilitando el caacutelculo online Se establece por tanto una ley de control que no necesita caacutelculos complejos por lo que se puede establecer una frecuencia mayor para el lazo de control respecto del uso de los caacutelculos del modelo dinaacutemico en tiempo real El lazo de control interno (el servo) simplemente tendraacute que multiplicar los errores de las ganancias con los pares basados en el modelo antildeadidos con una menor frecuencia La principal caracteriacutestica del control por par calculado es el desacoplamiento entre el modelo del manipulador y la parte servo El modelo del robot es no lineal sin embargo la particioacuten permite que el servo sea un controlador lineal simplificando el lazo de control De esta manera el modelo dinaacutemico no lineal cancela los efectos que tienen lugar en el manipulador

Para el disentildeo del control se supone la respuesta natural de nuestro sistema mecaacutenico como un sistema en el que el par de cada una de las articulaciones depende de los paraacutemetros de posicioacuten velocidad y aceleracioacuten angular medidos en las articulaciones a traveacutes de las matrices y vectores que definen la dinaacutemica del manipulador

휏 = 푀(휃) middot θ + 퐻 휃 θ + 퐺(휃) + 퐹 휃 θ (423)

siendo M la matriz de inercias del manipulador H el vector de teacuterminos centriacutefugos y de Coriolis G el vector de teacuterminos de la gravedad y F el modelo de friccioacuten del manipulador La expresioacuten del par como vimos en el estudio de la dinaacutemica inversa hace referencia al valor que es necesario generar en las articulaciones para generar el


movimiento deseado En el caso de los lazos de control se debe tener en cuenta que ese par seraacute generado por un actuador el cual tiene acoplada una etapa de reduccioacuten Por la tanto el paraacutemetro de par que se le indique deberaacute tener en cuenta dicha reduccioacuten por lo que el paraacutemetro de par obtenido finalmente deberaacute ser reducido con el factor de reduccioacuten de cada uno de los actuadores

El meacutetodo de par calculado como se comentoacute anteriormente hace uso de un caacutelculo offline en el que la dinaacutemica se evaluacutea sobre los valores deseados obtenidos del generador de trayectorias Se supone por tanto que el par dependiente del manipulador es el mismo que el dependiente de los paraacutemetros de la trayectoria

휏 = 푀(휃푑) middot θ푑 + 퐻 휃푑 θ푑 + 퐺(휃푑) + 퐹 휃푑 θ푑 (424)

Siendo 휃푑 θ푑 푦 휃푑 la posicioacuten y velocidad angular deseadas

Dado que se trabajaraacute con el modelo dinaacutemico del manipulador el cual no es un modelo exacto de la realidad se trabajaraacute con los siguientes teacuterminos

휏prime = 푀(휃푑) middot θ +퐻 휃푑 θ푑 + 퐺(휃푑) + 퐹 휃푑 θ푑 (425)

siendo 푀 퐻 퐺 y 퐹 las estimaciones de los valores reales de dichos paraacutemetros

Seguacuten el modelo que se muestra en la figura 39 la sentildeal de control que se generaraacute a partir de la suma de las salidas del controlador lineal y del controlador no lineal seraacute

휏prime = 푀(휃푑) middot θ푑 +퐻 휃푑 θ푑 + 퐺(휃푑) + 퐹 휃푑 θ푑 + 퐾푑 middot 푒 + 퐾푝 middot 푒 (426)

Siendo la definicioacuten del error

푒 = 휃 minus 휃 (427)

El error se considera como un elemento que no afecta a las diferencias de la dinaacutemica calculada sino que es un elemento independiente que compensaraacute tambieacuten el error en ese mismo caacutelculo a traveacutes del propio error de seguimiento de la trayectoria El control por dinaacutemica inversa hace uso de caacutelculos dinaacutemicos maacutes exactos por lo que el regulador haraacute frente uacutenicamente a los errores de seguimiento de la trayectoria


4421 Controlador PD

La ley de control para esta parte que constituye el controlador lineal del sistema es disentildeada teniendo en cuenta que los sensores del manipulador obtienen informacioacuten de posicioacuten y velocidad angular De modo que mediante esos datos se puede hacer un control de posicioacuten a traveacutes del seguimiento de trayectorias que es el objetivo de utilizacioacuten final del manipulador Con la estrategia de control propuesta se busca tener un error de seguimiento a una trayectoria deseada muy pequentildeo pese a la presencia de incertidumbres parameacutetricas y sin generar grandes esfuerzos de control

El controlador PD y el teacutermino feedforward que incluye las aceleraciones de la trayectoria de referencia utilizados en el controlador lineal estaacuten basados en una ley de control estabilizante (Spong Anderson 1992)

Los controladores de tipo PD son los maacutes comunes en este tipo de lazos de control La parte proporcional consiste en el producto entre la sentildeal de error y la constante proporcional y su finalidad es lograr que el error en reacutegimen permanente se aproxime a cero la parte derivativa trata de mantener el error en un valor miacutenimo corrigieacutendolo proporcionalmente con la misma velocidad con la que se produce evitando asiacute que el error se incremente Las matrices que se utilizaraacuten a partir de ahora para disentildear los controladores seraacuten matrices diagonales en las que cada uno de sus teacuterminos controlaraacute cada una de las articulaciones por separado Los lazos de control propuestos daraacuten lugar a sistemas en lazo cerrado desacoplados lo que hace posible el control independiente de cada articulacioacuten aunque se utilice una uacutenica expresioacuten debido al uso de vectores y matrices

Igualando las ecuaciones (425) y (426) se obtiene la siguiente foacutermula

푀(휃푑) middot 휃 = 푀(휃푑) middot 휃 푑 + 퐾푑 middot 푒 + 퐾푝 middot 푒 (428)

Utilizando la definicioacuten del error de la ecuacioacuten (427)

푀(휃푑) middot 푒 + 퐾푑 middot 푒 + 퐾푝 middot 푒 = 0 (429)

Considerando que no existe ninguna perturbacioacuten en el sistema En caso de que existiese la foacutermula seriacutea la siguiente

푀(휃푑) middot 푒 + 퐾푑 middot 푒 + 퐾푝 middot 푒 = 휏 (430)


Con una perturbacioacuten constante el error de velocidad y el de aceleracioacuten son nulos Sin embargo el error de posicioacuten seraacute en reacutegimen estacionario

퐾푝 middot 푒 = 휏 (431)

푒 =휏퐾푝

(432)

Dadas la posicioacuten velocidad y aceleracioacuten angular obtenidas del generador de trayectorias el control trataraacute de cancelar todas las no linealidades y aplicar el par necesario para superar la inercia del actuador Este seriacutea el resultado de la suma de los dos controladores el lineal (parte servo) y el no lineal (modelo del sistema)

4422 Espacio de estados (PD)

La dinaacutemica del error en lazo cerrado se representa a traveacutes de la ecuacioacuten (429) despejando 푒 para conocer la relacioacuten con las variables del sistema

푒 = minus푀 (휃푑) middot 퐾푑 middot 푒 minus 푀 (휃푑) middot 퐾푝 middot 푒 (433)

Se utilizaraacute para establecer la situacioacuten de los polos del sistema el disentildeo en el espacio de estados La representacioacuten en el espacio de estados del mismo permite conocer y controlar su dinaacutemica y sus respuestas Las variables del sistema cuyo nuacutemero estaacute determinado por su orden seraacuten las variables de estado del sistema Estas variables seraacuten a su vez funciones de la variable independiente de tiempo Se utilizaraacute como representacioacuten el plano complejo que se conoce como plano s Se usa para visualizar la ubicacioacuten de las raiacuteces y de los ceros de la funcioacuten de transferencia de un sistema LTI (Linear Time-Invariant) La visualizacioacuten graacutefica de las raiacuteces (es decir de aquellos valores que anulan la ecuacioacuten caracteriacutestica es decir el denominador) y de los ceros (aquellos valores que anulan el numerador de la funcioacuten de transferencia) permite inferir el comportamiento del sistema La funcioacuten de transferencia se expresa normalmente como un cociente de polinomios de la variable s de la transformada de Laplace y de ahiacute el nombre de plano s

Su seleccioacuten debe realizarse de manera que sean capaces de conocer la dinaacutemica del sistema a lo largo del tiempo Es decir deben ser el conjunto maacutes pequentildeo de variables que permitan predecir la evolucioacuten del sistema conociendo las entradas La ecuacioacuten diferencial que mide la variacioacuten del vector de estado con respecto al tiempo es una ecuacioacuten diferencial lineal respecto al tiempo que representa el comportamiento dinaacutemico del sistema Mediante la utilizacioacuten de una matriz de ganancias K se halla la relacioacuten entre las variables del sistema y las variables de estado


푑푑푡

푒푒 = 퐾 middot 푒

푒 (434)

Para conseguir la equivalencia de la ecuacioacuten (433) y teniendo en cuenta la ecuacioacuten (434) se utilizan los siguientes teacuterminos para la matriz K

푑푑푡

푒푒 = 0 퐼

minus푀 middot 퐾푝 minus푀 middot 퐾푑 middot 푒푒 (435)

Donde I es la identidad Para el caacutelculo de la ecuacioacuten caracteriacutestica del sistema que nos serviraacute para establecer la condicioacuten de estabilidad del mismo se parte de la matriz K Un escalar s es un valor propio de K si y soacutelo si existe un vector propio v ne 0 tal que

K middot 푣 = 푠 middot 푣 (436)

(푠퐼 minus 퐾) middot 푣 = 0 (437)

Dado que v es distinto de cero la matriz smiddotI-K seraacute singular lo que a su vez resulta en que su determinante seraacute 0 Asiacute las raiacuteces de la funcioacuten

푑푒푡(푠 middot 퐼 minus 퐾) (438)

son los valores propios de K Igualando a cero (polos del sistema) y resolviendo el determinante se define la ecuacioacuten caracteriacutestica

푑푒푡(푠 middot 퐼 minus 퐾) = 0 (439)

풔 00 풔 minus 0 퐼

minus푀 middot 퐾푝 minus푀 middot 퐾푑 = 0 (440)

풔 minus퐼푀 middot 퐾푝 풔 + 푀 middot 퐾푑 = 0 (441)


푠 + 푀 (휃푑) middot 퐾푑 middot 푠 +푀 (휃푑) middot 퐾푝 = 0 (442)

Al tratarse de la ecuacioacuten caracteriacutestica del sistema se realizaraacute el criterio de estabilidad de Routh para determinar las restricciones a la hora de escoger los valores de las ganancias del regulador La primera restriccioacuten viene dada por la presencia de coeficientes negativos o nulos con la presencia de al menos un coeficiente positivo Dado que el coeficiente de s3 es constante y positivo ninguno de los otros coeficientes podraacute ser negativo o nulo El siguiente paso es construir la tabla con los coeficientes de la funcioacuten para su anaacutelisis

푠2 1 푀 middotKp

푠1 푀 middot 퐾푑 0

푠0 푀 middot 퐾푑 middot 퐾푝푀 middot 퐾푑

0

Tabla 4

El criterio de Routh establece que la cantidad de raiacuteces positivas o con parte real positiva es igual a la cantidad de cambios de signo en los coeficientes de la primera columna Es decir que si existe alguacuten cambio de signo en los coeficientes existiraacute alguna raiacutez que haga inestable al sistema Teniendo en cuenta esta condicioacuten se establecen las siguientes restricciones

푀 middot 퐾푑 gt 0 (443)

푀 middot 퐾푝 gt 0 (444)

M-1 debe tener el mismo signo para cada valor correspondiente de Kd y Kp en el producto de matrices (para cada articulacioacuten) para que el valor resultante sea positivo y el sistema sea estable no producieacutendose ninguacuten cambio de signo en la tabla y cumpliendo asiacute la condicioacuten de estabilidad

Claramente a medida que la configuracioacuten del manipulador cambia los cambios de ganancia efectiva de circuito cerrado tambieacuten variacutean y los polos pasan a moverse en el plano real-imaginario Este es un problema que hace que esta ley de control no sea viable para su uso en un manipulador real ya que para poder asegurarse del correcto funcionamiento deberiacutean variarse las ganancias en funcioacuten de las inercias por lo que hariacutea falta disponer de los valores para todas las posiciones y cargas posibles Sin embargo la ley de control es uacutetil como punto de partida para el disentildeo de otros


controladores ya que a pesar de del movimiento de los polos se puede encontrar una buena gama de ganancias constantes de tal manera que los polos esteacuten garantizados para permanecer en lugares razonablemente favorables

4423 Eleccioacuten de las ganancias

Ademaacutes de los liacutemites establecidos en el anterior subapartado se deben tener en cuenta otras consideraciones ademaacutes de la estabilidad del sistema a la hora de establecer las ganancias de las acciones proporcional y derivativa en este caso

Un sistema de segundo orden como el obtenido con la ley de control por par calculado representa los siguientes paraacutemetros

푠 + 2 middot 휉 middot 휔 + 휔 (445)

Siendo 휉 la constante de amortiguamiento y 휔 la frecuencia natural del sistema

Para el control de manipuladores el valor elegido para la constante de amortiguamiento debe cumplir la siguiente restriccioacuten

휉 le 1 (446)

Eligiendo el valor liacutemite 휉 = 1 se tendraacute un sistema criacuteticamente amortiguado Un sistema criacuteticamente amortiguado es en realidad un caso concreto de sistema subamortiguado Se asegura con eacutel la respuesta maacutes raacutepida posible sin que se produzcan sobreoscilaciones Es importante mencionar que los manipuladores roboacuteticos no pueden tener ninguacuten tipo de sobreoscilacioacuten pues hariacutea alcanzar puntos en el espacio que no se encuentran en la trayectoria definida para el movimiento que se esteacute realizando Ademaacutes podriacutea producir situaciones de peligro al sobrepasar los puntos definidos de paso Por lo tanto se debe establecer un valor de ξ menor o igual a uno siendo el caso ideal el amortiguamiento criacutetico

La frecuencia natural gobierna la velocidad de respuesta en cada componente del error Debe ser grande para respuestas raacutepidas y se selecciona en funcioacuten de los objetivos de desempentildeo Asiacute las trayectorias deseadas deben tenerse en cuenta en la seleccioacuten de este paraacutemetro ademaacutes de otros factores que influiraacuten en la respuesta del sistema Se presentan ahora los factores adicionales en esta eleccioacuten

Hay algunos liacutemites superiores en la eleccioacuten de la frecuencia natural del sistema Aunque los eslabones de los manipuladores son riacutegidos todos presentan cierta flexibilidad Suponiendo una frecuencia de resonancia para un eslaboacuten igual a


휔 =푘퐽

(447)

Donde 푘 es la rigidez del eslaboacuten y 퐽 su inercia La frecuencia de resonancia es la frecuencia con la que un sistema presenta la vibracioacuten maacutexima posible Por lo tanto se debe evitar excitar dicha frecuencia para evitar vibraciones que perjudicasen la precisioacuten del manipulador asiacute como para evitar la inestabilidad del sistema a cause de dichas vibraciones Para no excitar el modo de resonancia de los eslabones se debe seleccionar una frecuencia natural tal que

휔 lt휔2

(448)

La inercia de los eslabones cambia con la configuracioacuten del manipulador de modo que su valor maacuteximo deberiacutea ser utilizado en la computacioacuten del liacutemite de eleccioacuten de la frecuencia natural

Otro liacutemite superior estaacute marcado por consideraciones sobre la saturacioacuten del actuador Si las ganancias del controlador son demasiado grandes el par 휏 generado por la ley de control puede llegar a sus liacutemites maacuteximos entrando en saturacioacuten

Como se nombroacute al inicio de este subapartado las trayectorias de referencia tambieacuten deben ser tenidas en cuenta la hora de elegir el paraacutemetro de la frecuencia natural a traveacutes de las ganancias del controlador El contenido de frecuencia de la entrada deseada o referencia pone un liacutemite inferior absoluto en la velocidad de respuesta necesaria para no producir grandes errores en el seguimiento de la trayectoria Normalmente el ancho de banda de las trayectorias no limita este paraacutemetro ya que el ancho de banda de las trayectorias no debe ser elevado pues se busca que los movimientos sean lo maacutes suaves que sea posible

443 Control por dinaacutemica inversa

El control por dinaacutemica inversa sigue el mismo principio que el control por par calculado con la diferencia de que en este tipo de control la parte no lineal (modelo dinaacutemico) se utiliza en un lazo interno que depende de las variables reales del manipulador y no de las que describen la trayectoria a seguir De esta manera se tiene un control que representa mejor la realidad pero con un mayor coste computacional al tener que calcular todos los teacuterminos dinaacutemicos durante la ejecucioacuten del lazo de control El esquema de este tipo de control para un sistema mecaacutenico como es el manipulador se muestra en la siguiente imagen


Figura 33 Control por dinaacutemica inversa con controlador PID

La parte basada en el modelo del manipulador de la ley de control haraacute uso del supuesto conocimiento de los paraacutemetros dinaacutemicos del sistema Esta parte de la ley de control estaraacute configurada de tal manera que se reduciraacute el sistema de modo que equivalga a la matriz de inercias como se realizaraacute en la ecuacioacuten (451) Esto permite que el disentildeo de la parte servo (controlador lineal) sea muy simple La segunda parte de la ley de control haraacute uso de la retroalimentacioacuten para modificar el comportamiento del sistema La parte basada en el modelo de control aparece en una ley de control de la forma

휏 = 훼 middot 휏 + 훽 (449)

A partir de las matrices y vectores que definen la dinaacutemica del manipulador representadas en la ecuacioacuten (424) y de la ecuacioacuten (449) se obtiene la siguiente igualdad

푀(휃) middot 휃 + 퐻 휃 휃 + 퐺(휃) + 퐹 휃 휃 = 훼 middot 휏 + 훽 (450)

Ahora se establecen los valores de los teacuterminos

훼 = 푀(휃) (451)

훽 = 퐻(휃휃 ) + 퐺(휃) + 퐹(휃휃 ) (452)

Por lo tanto y dado que la matriz de inercias es invertible ya que es una matriz triangular con todos sus elementos diagonales no nulos se obtiene que


휃 = 휏 (453)

El teacutermino 휏rsquo es entonces una aceleracioacuten articular que al ser multiplicada por la matriz de inercias daraacute lugar a una parte del par que debe generar el manipulador para alcanzar los puntos de la trayectoria especificados en el tiempo Esto concuerda con la ecuacioacuten que relaciona el par con la inercia cuando la distancia al sistema de referencia es constante

휏 = 퐼 middot 훼 (454)

Donde I seriacutea la matriz de inercias y α la aceleracioacuten angular La aceleracioacuten de control representa una nueva entrada al sistema que seraacute la sentildeal que genere el controlador lineal que representaraacute la parte servo del control La ecuacioacuten (453) se conoce como sistema de doble integrador ya que representa n integradores desacoplados dobles El sistema es por tanto lineal y desacoplado Esto significa que cada entrada 휃k puede ser disentildeada para controlar un sistema lineal escalar

4431 Controlador lineal PID

Los controladores de tipo PID no son muy utilizados en este tipo de control dado que al tener en cuenta los aspectos dinaacutemicos del sistema mecaacutenico el error en reacutegimen permanente no va a suponer un problema importante Ademaacutes se mantienen las posiciones sin necesidad de una actuacioacuten activa del controlador lineal lo que seriacutea necesario de no tener en cuenta el modelo dinaacutemico Aun asiacute la utilizacioacuten del teacutermino integral mejora el comportamiento del control a cambio de incrementar el orden del sistema hacieacutendolo maacutes complejo y aumentando las posibilidades de inestabilidad En caso de que fuese necesaria su implementacioacuten se hace el desarrollo del mismo a continuacioacuten

Respecto de un controlador de tipo PD la accioacuten integral disminuye y elimina el error en reacutegimen permanente

Asumiendo que el generador de trayectorias proporcionaraacute los valores deseados de posicioacuten velocidad y aceleracioacuten angular de cada articulacioacuten en un tiempo definido t se elige para el servocontrol la siguiente foacutermula de seguimiento de trayectoria

휃 = 휃 푑 + 퐾 middot 푒 + 퐾 middot 푒 + 퐾 middot 푒 푑푡 (455)


Siendo 휃 푑 la aceleracioacuten angular deseada Kp Kd y Ki las matrices de constantes que contienen los teacuterminos del controlador proporcional derivativo e integral respectivamente y E el error definido por la siguiente ecuacioacuten

푒 = 휃 푑 minus 휃 (456)

La utilizacioacuten del teacutermino integral ademaacutes de los proporcionales y derivativos puede introducir complicaciones ya que se debe tener cuidado para mantener la estabilidad del sistema El controlador maacutes utilizado en el control por par calculado es de tipo PD Sin embargo a pesar de que conociendo los paraacutemetros del manipulador se consigue un control efectivo la presencia de perturbaciones constantes hace que el error que presenta en estado estacionario sea distinto de cero lo cual se soluciona antildeadiendo la accioacuten integral Por el contrario el uso del PID hace que el sistema se convierta en uno de tercer orden como se veraacute a continuacioacuten incrementando la complejidad del mismo

La ecuacioacuten que describe el error cometido al elegir este controlador de tipo PID para la parte servo es utilizando las ecuaciones (455) y (456)

푒 + 퐾푑 middot 푒 + 퐾푝 middot 푒 + 퐾푖 middot 푒 푑푡 = 0 (457)


푒 + 퐾푑 middot 푒 + 퐾푝 middot 푒 + 퐾푖 middot 푒 푑푡 = 휏 (458)

Con una perturbacioacuten constante la parte integral elimina el error en reacutegimen estacionario como demuestran las ecuaciones

푒 + 퐾푑 middot 푒 + 퐾푝 middot 푒 + 퐾푖 middot e = 휏prime (459)

퐾 middot 푒 = 0 (460)

푒 = 0 (461)


Como se puede observar en la ecuacioacuten (459) el sistema que se obtiene es un sistema de tercer orden

Dadas la posicioacuten y velocidad actual del manipulador (datos obtenidos a traveacutes de los sensores) junto con los datos de la trayectoria el control trataraacute de cancelar todas las no linealidades (β) y aplicar exactamente el par necesario para superar la inercia del actuador Este seriacutea el resultado de la suma de los dos controladores el lineal (parte servo) y el no lineal (modelo del sistema)

4432 Espacio de estados (PID)

Dado que el sistema a controlar utiliza muacuteltiples entradas y salidas la representacioacuten en el espacio de estados del mismo permite conocer y controlar su dinaacutemica y sus respuestas Las variables del sistema cuyo nuacutemero estaacute determinado por su orden seraacuten las variables de estado del sistema Estas variables seraacuten a su vez funciones de la variable independiente de tiempo Su seleccioacuten debe realizarse de manera que sean capaces de conocer la dinaacutemica del sistema a lo largo del tiempo Es decir deben ser el conjunto maacutes pequentildeo de variables que permitan predecir la evolucioacuten del sistema conociendo las entradas La ecuacioacuten diferencial que mide la variacioacuten del vector de estado con respecto al tiempo es una ecuacioacuten diferencial lineal respecto al tiempo que representa el comportamiento dinaacutemico del sistema

Se introduce una nueva variable para representar el error del teacutermino integral

휀 = 푒 (462)

Mediante la utilizacioacuten de una matriz de ganancias K se halla la relacioacuten entre las variables del sistema y las variables de estado

푑푑푡

휀푒푒

= 퐾 middot휀푒푒

(463)

A traveacutes de la ecuacioacuten (457) se despeja 푒 para conocer la relacioacuten con las variables del sistema

푒 = minus 퐾푑 middot 푒 minus 퐾푝 middot 푒 minus 퐾푖 middot 휀 (464)



푑푑푡

휀푒푒

=0 퐼 00 0 퐼minus퐾푖 minus퐾푝 minus퐾푑

middot휀푒푒

(465)


퐾 middot 푣 = 푠 middot 푣 (466)


Dado que v es distinto de cero la matriz smiddotI-K seraacute singular lo que a su vez resulta en que su determinante seraacute cero Asiacute las raiacuteces de la funcioacuten




풔 0 00 풔 00 0 풔

minus0 퐼 00 0 퐼minus퐾푖 minus퐾푝 minus퐾푑

= 0 (470)

푠 minus퐼 00 푠 minus퐼퐾푖 퐾푝 푠 + 퐾푑

= 0 (471)

푠 + 퐾푑 middot 푠 + 퐾푝 middot 푠 + 퐾푖 = 0 (472)

Al tratarse de la ecuacioacuten caracteriacutestica del sistema se realizaraacute el criterio de estabilidad de Routh para determinar las restricciones a la hora de escoger los valores de las ganancias del regulador La primera restriccioacuten viene dada por la presencia de coeficientes negativos o nulos con la presencia de al menos un coeficiente positivo


Dado que el coeficiente de s3 es constante y positivo ninguno de los otros coeficientes podraacute ser negativo o nulo El siguiente paso es construir la tabla con los coeficientes de la funcioacuten para su anaacutelisis

s3 1 Kp

s2 Kd Ki

s1 퐾푑 middot 퐾푝 minus 퐾푖퐾푑

0

s0 퐾푑 middot 퐾푝 middot 퐾푖 minus 퐾푖퐾푑 middot 퐾푝 + 퐾푖

퐾푑

0

Tabla 5

Los valores de las constantes Kp Kd y Ki deben ser positivos Teniendo en cuenta esta condicioacuten se establecen tambieacuten las siguientes restricciones

퐾푑 middot 퐾푝 minus 퐾푖퐾푑

gt 0 (473)

퐾푑 middot 퐾푝 gt 퐾푖 (474)

Para la uacuteltima fila de la tabla la expresioacuten se reduce a

퐾푑 middot 퐾푝 middot 퐾푖 minus 퐾푖퐾푑 middot 퐾푝 + 퐾푖

퐾푑gt 0 (475)

퐾푑 middot 퐾푝 middot 퐾푖 gt 퐾푖 (476)

Para que las filas uno y dos de la tabla cumplan el criterio se establece que sus valores deben ser negativos De las filas tres y cuatro resultan las ecuaciones (474) y (476) que son equivalentes Por lo tanto a la hora de establecer los valores Kp Kd y Ki se deben tener en cuenta junto con la restriccioacuten de que sean valores positivos

En el caso del controlador PID como se vio en este mismo apartado se trata de un sistema de tercer orden Al tratarse de un sistema de orden superior se estariacutea tratando con un modelo complejo El objetivo es entonces aproximar el sistema a uno de segundo orden de manera que se conozca el comportamiento y su anaacutelisis sea maacutes sencillo A traveacutes del lugar de las raiacuteces que es el lugar geomeacutetrico de los polos y los ceros de una funcioacuten de transferencia se pueden ubicar los polos de la ecuacioacuten caracteriacutestica de manera que sea posible aproximar el sistema por uno de orden


inferior Ya que se dispone de la ecuacioacuten caracteriacutestica del sistema que equivale al denominador de la funcioacuten de transferencia del sistema los valores que hacen cero a dicha ecuacioacuten seraacuten los polos del sistema Para realizar la aproximacioacuten hay que tener en cuenta que la influencia de los polos de la cadena cerrada variacutea La reduccioacuten del orden del sistema simplifica tanto la fase de anaacutelisis como de disentildeo El objetivo es que el sistema quede definido por la siguiente expresioacuten que define a un sistema de segundo orden

푑(푠) = 푠 + 2 middot 휉 middot 푤 middot 푠 middot +푤 (477)

Siendo 푤 la frecuencia natural del sistema y 휉 la constante de amortiguamiento La ecuacioacuten (472) que representa el sistema de tercer orden mediante la reduccioacuten a un sistema de segundo orden pasariacutea a ser equivalente a la ecuacioacuten (477)

El primer paso es establecer el polo dominante del sistema Eacuteste seraacute el maacutes cercano al eje imaginario del lugar de las raiacuteces y se expresa con la notacioacuten dom Una vez definido cual es el polo dominante para poder establecer que un polo tendraacute una respuesta transitoria despreciable deberaacute encontrar a una distancia del eje imaginario tal que

σ gt 6 middot 휎 (478)

Siendo σ la distancia del polo al eje imaginario Por lo tanto para pasar de un sistema de tercer orden a uno de segundo se debe colocar uno de los polos a una distancia al menos seis veces mayor del eje imaginario respecto de la distancia al mismo eje del polo dominante

444 Control Digital

Los planteamientos realizados hasta ahora se han hecho sobre la base del tiempo continuo Sin embargo la implementacioacuten del lazo en un hardware de control va a suponer que las sentildeales de entrada tengan un periodo de muestreo por lo que se pasariacutea a trabajar en tiempo discreto Ademaacutes los caacutelculos a realizar en el lazo de control requeriraacuten un tiempo de coacutemputo y deberaacuten completarse en un tiempo menor al tiempo de muestreo para que cada nueva sentildeal recibida sea tenida en cuenta Actualmente los sistemas de control roboacuteticos trabajan con un periodo que oscila entre los diez y los veinte milisegundos Dado que los planteamientos han sido realizados sobre tiempo continuo para pasar a tener en cuenta el funcionamiento en tiempo discreto existen dos opciones La primera y maacutes obvia es discretizar el planteamiento realizado Una segunda opcioacuten en conservar el planteamiento actual y probar a traveacutes de la simulacioacuten periodos de muestreo que aseguren buenos resultados con el planteamiento de tiempo continuo Ademaacutes existen reglas y teoremas que permiten elegir un periodo de muestreo que haga que el resultado del


control sea el correcto Esta segunda opcioacuten seriacutea una aproximacioacuten que evitariacutea la discretizacioacuten del lazo Sin embargo la primera opcioacuten permite tener en cuenta el efecto del periodo de muestreo en el lazo de control de manera exacta Por el contrario para discretizar dinaacutemicas no lineales como es la del manipulador no existe un meacutetodo exacto sino que se tratariacutea tambieacuten de una aproximacioacuten El resultado seriacutea ademaacutes un conjunto de ecuaciones muy complejas incluso para manipuladores maacutes simples que el nuestro

Seguacuten el lazo de control disentildeado se dispone de dos controladores separados Uno de ellos es el controlador lineal que utiliza los errores de posicioacuten y velocidad en las articulaciones del manipulador y genera una sentildeal de control a traveacutes de un regulador PD o PID El otro es un controlador no lineal en el que estaacuten contenidas las ecuaciones dinaacutemicas del manipulador Teniendo en cuenta lo expuesto en el paacuterrafo anterior se puede optar por discretizar el controlador lineal ya que no supone ninguacuten obstaacuteculo y para el controlador no lineal se buscariacutean periodos de muestreo adecuados para su utilizacioacuten con las ecuaciones de tiempo continuo que seriacutean comprobadas en la simulacioacuten Generalmente se asume que los caacutelculos se podraacuten realizar suficientemente raacutepido para que la aproximacioacuten de tiempo continuo de la parte no lineal sea vaacutelida

Para determinar la frecuencia miacutenima a la que debe funcionar el lazo de control existen una serie de condiciones Cumplieacutendolas se asegura que la discretizacioacuten de las sentildeales no afectaraacute al modelo continuo propuesto Las condiciones a tener en cuenta son las siguientes

Sentildeales de entrada los puntos de las trayectorias asiacute como las sentildeales de los sensores establecen una frecuencia miacutenima en el lazo de control El ancho de banda del lazo de control debe ser al menos el doble del ancho de banda de las sentildeales de entrada

Atenuacioacuten de las perturbaciones el periodo de muestreo del lazo de control debe ser menor que la correlacioacuten de tiempo de la perturbacioacuten sino eacutesta no seraacute suprimida

Resonancias estructurales la frecuencia de muestreo del lazo de control debe ser al menos dos veces mayor que la frecuencia natural de resonancia

4441 Ecuaciones en diferencias

Para trabajar en tiempo discreto el controlador debe ser definido mediante una ecuacioacuten en diferencias que lo describa Para el controlador PID en el control por dinaacutemica inversa se utilizaraacute el meacutetodo de Tustin (o transformacioacuten bilineal) para obtener a partir de la funcioacuten de transferencia del filtro lineal e invariante en el tiempo que se encuentra definida en el dominio continuo del tiempo (LTI) una funcioacuten de transferencia perteneciente a un filtro lineal e invariante en el tiempo que se encuentre definido en el dominio discreto del tiempo La transformacioacuten bilineal es una aproximacioacuten de primer orden de la funcioacuten logariacutetmica natural que consiste en realizar una asignacioacuten exacta del plano Z al plano S El intereacutes del uso del plano Z se debe a


que es el plano anaacutelogo al plano S que define la transformada de Laplace para resolver ecuaciones en diferencias finitas

En controlador lineal tendraacute tres entradas y una uacutenica salida que seraacute la sentildeal de control enviada al controlador no lineal que haraacute que el sistema global se simplifique en el sistema lineal definido por el controlador que se estaacute tratando Cada una de las sentildeales de entrada que son la referencia de aceleracioacuten el error de velocidad y el error de posicioacuten influyen en la misma sentildeal de control al ser eacutesta uacutenica por lo que la salida tendraacute una funcioacuten de transferencia que seraacute la suma de cada una de las funciones de transferencia definidas por la relacioacuten entre la sentildeal de control y cada una de las entradas Estando definida la sentildeal de control como

푎 = 휃 푑 + 퐾 middot 푒 + 퐾 middot 푒 + 퐾 middot 푒 푑푡 (479)

La sentildeal de control tiene una funcioacuten de transferencia con respecto a la referencia de aceleracioacuten articular

푎 = 휃 (480)

푎휃 푑

= 1 (481)

El subiacutendice de la sentildeal de control hace referencia a la relacioacuten de cada una de las entradas para finalmente componer la sentildeal de control a partir de la suma de cada una de ellas En el caso del error de velocidad articular

푎 = 푒 middot 퐾 (482)

푎푒

= 퐾 (483)

Por uacuteltimo el error de posiciones articulares

푎 = 퐾푝 middot 푒 + 퐾푖 middot 푒 푑푡 (484)


En esta uacuteltima relacioacuten se aplicaraacute la transformada de Laplace para transformar la ecuacioacuten diferencial lineal (484) en una ecuacioacuten algebraica A las relaciones de sentildeales definidas en las ecuaciones (481) y (483) tambieacuten habriacutea que aplicar esta transformada pero dado el caso de que se trata de relaciones constantes el resultado seriacutea el mismo tras aplicar la transformada por lo que no es necesario aplicarla La expresioacuten que relaciona la sentildeal de control con el error de posicioacuten seraacute entonces tras aplicar la transformada de Laplace

푎 = 푒 middot 퐾푝 +퐾푠

= 푒 middot퐾푝 middot 푠 + 퐾푖

푠 (485)

Para el control digital es necesario ahora hacer un nuevo cambio que permita obtener una expresioacuten que refleje el funcionamiento del sistema en tiempo discreto En un sistema de control en tiempo discreto una ecuacioacuten en diferencias lineales caracteriza la dinaacutemica del sistema Para determinar la respuesta a una entrada dada se debe resolver dicha ecuacioacuten en diferencias La notacioacuten utilizada en este caso es la de la transformada en Z La transformada en Z para sistemas discretos desempentildea un papel anaacutelogo a la transformada de Laplace para sistemas continuos Va a permitir representar la relacioacuten entre la entrada o entradas con la salida o salidas en las que influyen mediante un cociente de polinomios Esto facilitaraacute el caacutelculo de las operaciones Asiacute la transformada en Z seraacute una serie de potencias de Z--1 cuyos coeficientes seraacuten los valores de las muestras de las sentildeales de entrada La manera en que se consideraraacute la transformada Z de las entradas consiste en usar Z-1 para enumerar el lugar de cada una de las muestras Asiacute Z0 haraacute referencia a la muestra actual Z-1 a la muestra tomada en el periodo de tiempo de muestreo anterior y asiacute sucesivamente Es decir multiplicar por Z equivale a adelantar un intervalo de muestreo mientras que dividir entre Z es equivalente a retrasar un intervalo de muestreo

La transformada en Z representaraacute la discretizacioacuten del controlador y para realizarla se utilizaraacute el meacutetodo de Tustin Para aplicarlo se consideraraacute el periodo de muestreo que tendraacuten las sentildeales en el hardware de control Esto permitiraacute obtener la ecuacioacuten en diferencias que describa la relacioacuten de la sentildeal de control con el error de posicioacuten para posteriormente expresar la sentildeal de control en funcioacuten de las tres entradas que la conformaraacuten La ecuacioacuten final programaraacute constituyendo el propio controlador lineal con accioacuten proporcional derivativa e integral El meacutetodo de Tustin utiliza la siguiente equivalencia para realizar la transformacioacuten desde la transformada de Laplace a la transformada en Z

푠 cong2푇

middot푧 minus 1푧 + 1

(486)

Aplicando la relacioacuten anterior a la ecuacioacuten (485) y operando se obtiene


푎푒

=퐾푝 middot 2 middot 푧 minus 2

푇 middot 푧 + 푇 + 퐾푖2 middot 푧 minus 2푇 middot 푧 + 푇

=퐾푝 middot (2 middot 푧 minus 2) + 퐾푖 middot (푇 middot 푧 + 푇)

2 middot 푧 minus 2 (487)

푎푒

=2 middot 퐾푝 middot 푧 minus 2 middot 퐾푝 + 푇 middot 퐾푖 middot 푧 + 푇 middot 퐾푖


푎 middot (푧 minus 1) = 푒 middot 퐾푝 middot 푧 minus 퐾푝 +푇2

middot 퐾푖 middot 푧 +푇2

middot 퐾푖 (489)



middot 퐾푖 (490)

푎 middot (1minus 푧 ) = 푒 middot 퐾푝 minus 퐾푝 middot 푧 +푇2

middot 퐾푖 +푇2

middot 퐾푖 middot 푧 (491)

푎 minus 푎 = 퐾푝 middot 푒 minus 퐾푝 middot 푒 +푇2

middot 퐾푖 middot 푒 +푇2

middot 퐾푖 middot 푒 (492)

푎 = 푎 + 퐾푝 +푇2

middot 퐾푖 middot 푒 + (푇2

middot 퐾푖 minus 퐾푝) middot 푒 (493)

Finalmente se llega a la siguiente expresioacuten que seraacute la ecuacioacuten en diferencias que constituiraacute el controlador PID del sistema en la que deberaacuten sustituirse los valores que se elijan para las ganancias del mismo

푎 = 휃 + 퐾푑 middot 푒 + 푎 + 퐾푝 +푇2



El regulador de tipo PD representa relaciones directas de cada una de sus entradas con sentildeal de salida es decir la sentildeal de control Por lo tanto la ecuacioacuten en diferencias que describe a ese controlador es

푎 = 휃 + 퐾푑 middot 푒 + 퐾푝 middot 푒 (495)


445 Control en el Espacio Cartesiano

Los lazos de control propuestos hasta ahora trabajaban en el espacio de las articulaciones de manera que los puntos interpolados de las trayectorias eran continuamente transformados a valores articulares por medio de la cinemaacutetica inversa Se propondraacute ahora un planteamiento similar al control por dinaacutemica inversa pero que trabaja con puntos del espacio cartesiano El conocimiento de este tipo de control permitiraacute la implementacioacuten del sistema de medida Laacuteser-Tracker que proporciona la posicioacuten y orientacioacuten de la punta de la herramienta del manipulador o del efector final (seguacuten si se utiliza herramienta o no) respecto del sistema de referencia ubicado en la base del robot La medida obtenida puede ser directamente comparada con los puntos generados de la trayectoria corrigiendo la posicioacuten en el espacio y no en funcioacuten de medidas indirectas que conducen a dicha posicioacuten como son las posiciones de las articulaciones Para el planteamiento de este lazo de control por dinaacutemica inversa en el espacio cartesiano existen dos opciones Una opcioacuten seriacutea la utilizacioacuten de una dinaacutemica realizada en el espacio cartesiano es decir diferente a la realizada en este proyecto que estuviera referida al espacio cartesiano La otra opcioacuten consiste en adaptar el lazo de control para utilizar el mismo modelo dinaacutemico pero trabajando en el espacio cartesiano Ambas son presentadas a continuacioacuten

4451 Control con dinaacutemica en el espacio articular

La posicioacuten y orientacioacuten del efector final estaraacuten definidas por el siguiente conjunto de variables

푦(푡) = [푋푌푍퐴퐵퐶] (496)

De las que XY y Z definen la posicioacuten en el espacio y AB y C la orientacioacuten A traveacutes de estos datos que seraacuten los que proporcione el sistema de medida Laacuteser-Tracker se podraacute hacer el control El error que lo permita estaraacute definido por los errores de posicioacuten y de orientacioacuten

푒 =푒푒 (497)

Siendo ep el error de posicioacuten y eo el error de orientacioacuten A su vez cada uno de ellos se define de la siguiente manera El error de posicioacuten seraacute

푒 = 푝 minus 푝 (498)


Donde pd es la posicioacuten deseada definida por la trayectoria generada y p la posicioacuten real medida El error de orientacioacuten seraacute

푒 =12

middot (푛 times 푛 + 표 times 표 + 푎 times 푎 ) (499)

Siendo nd od y ad los vectores de orientacioacuten obtenidos a partir de las medidas del sensor y n o y a los vectores de orientacioacuten definidos para cada punto de la trayectoria Ya que se puede utilizar un controlador de tipo PID como en el caso de los lazos de control ya propuestos se definiraacute tambieacuten la integral de este error de posicioacuten

휀 = 푒 푑푡 (4100)

Ademaacutes de los errores de posicioacuten y orientacioacuten (y su integral) es necesario definir el error de velocidad que a su vez estaraacute tambieacuten compuesto por dos tipos de errores La funcioacuten de velocidad en el espacio se define por el siguiente vector

푦 = 푣ω (4101)

푣 es el vector de velocidad lineal y 휔 es el vector de velocidad angular A partir de estos valores se calcularaacute el error comparando este vector con el vector de velocidades establecidas para la trayectoria

푦 =푣ω (4102)

Con estas dos definiciones ya es posible establecer el nuevo error

푒 = 푦 minus 푦 (4103)

Por uacuteltimo es necesario definir el error de aceleracioacuten que aunque no se utilizaraacute en el lazo de control es necesario para poder realizar la ley de control La aceleracioacuten en el espacio cartesiano estaraacute definida por el siguiente vector


푦 = 푎α (4104)

푎 es el vector de aceleracioacuten lineal y 훼es el vector de aceleracioacuten angular Las aceleraciones establecidas para una trayectoria dada se establecen en el siguiente vector

푦 =푎α (4105)

Por lo tanto el error de aceleracioacuten quedaraacute definido de la siguiente manera


La relacioacuten entre la velocidad lineal y angular en el espacio cartesiano y los valores de la velocidad angular de cada una de las articulaciones estaacute definida por el jacobiano

푦 = 퐽 middot 휃 (4107)

Al igual que con las velocidades el jacobiano tambieacuten relaciona las aceleraciones articulares y cartesianas

푦 = 퐽 middot 휃 + 퐽 middot 휃 (4108)

La dinaacutemica del manipulador definida por el meacutetodo de Euler-Lagrange que estaacute representada por la ecuacioacuten (343) pasa a tener la siguiente expresioacuten para dar lugar al control a traveacutes de variables en el espacio cartesiano a traveacutes de la igualdad definida en la ecuacioacuten (la anterior a esta (y segunda))

휏 = 푀 middot 퐽 middot 푦 minus 퐽 middot 휃 +퐻 휃휃 + 퐺(휃) + 퐹 휃휃 + 휏 (4109)

Despejando el vector de aceleracioacuten en el espacio cartesiano se obtiene


푦 = 퐽 middot 휃 minus 퐽 middot 푀 middot (퐻 휃휃 + 퐺(휃) + 퐹 휃휃 + 휏 minus 휏) (4110)

Utilizando la ecuacioacuten (4103) sobre la ecuacioacuten anterior se obtiene la siguiente expresioacuten

푒 = 푦 minus 퐽 middot 휃 + 퐽 middot 푀 middot (퐻 휃휃 + 퐺(휃) + 퐹 휃휃 + 휏 minus 휏) (4111)

A partir de esta nueva definicioacuten del error de aceleracioacuten se procederaacute a separar la parte correspondiente a las perturbaciones ya que esta no formaraacute parte de la ley de control

푢 = 푦 minus 퐽 middot 휃 + 퐽 middot 푀 middot (퐻 휃휃 + 퐺(휃) + 퐹 휃휃 minus 휏) (4112)

푤 = 퐽 middot 푀 middot 휏 (4113)

Se obtiene por tanto una expresioacuten que define el comportamiento ideal del sistema u y otra que representa la respuesta ante perturbaciones w A partir de la ecuacioacuten (4112) se despeja el par para obtener finalmente la expresioacuten que permite controlar al manipulador

휏 = 푀 middot 퐽 middot 푦 minus 퐽 middot 휃 minus u + 퐻 휃휃 + 퐺(휃) + 퐹 휃휃 (4114)

Ahora es necesario definir la sentildeal de control u Para ello se puede recurrir a uno de los controladores utilizados en los subapartados 4421 y 4431 En este caso se va a utilizar el de tipo PID por las ventajas que presenta frente al de tipo PD Por la tanto la sentildeal de control u se define de la siguiente forma

푢 = minus퐾 middot 푒 minus 퐾 middot 푒 minus 퐾 middot 휀 (4115)

Utilizando esta expresioacuten en la ecuacioacuten (4114) se llega a la siguiente definicioacuten que es la que estableceraacute la configuracioacuten del lazo de control


휏 = 푀 middot 퐽 middot 푦 minus 퐽 middot 휃 + 퐾 middot 푒 + 퐾 middot 푒 + 퐾 middot 휀 +퐻 휃휃 + 퐺(휃)+ 퐹 휃휃 (4116)

A partir del Jacobiano ya calculado en el apartado 25 de este trabajo se puede obtener su inversa a traveacutes de dos meacutetodos diferentes

El primero de ellos consistiriacutea en calcular los teacuterminos simboacutelicos del Jacobiano para el manipulador A partir de ellos se invertiriacutea la matriz y se tendriacutea una expresioacuten que permitiriacutea calcular la inversa del Jacobiano para cualquier posicioacuten sustituyendo los valores articulares Sin embargo pese a que una vez que se tiene la expresioacuten simboacutelica no es necesario volver a calcular la matriz la complejidad de la misma para un manipulador de seis grados de libertad hace que la sustitucioacuten de los teacuterminos por los valores determinados para cada instante de tiempo sea un proceso lento desde el punto de vista computacional

El otro meacutetodo consiste en realizar primero la evaluacioacuten numeacuterica del Jacobiano para posteriormente realizar la inversioacuten Con este meacutetodo es necesario el recoacutemputo de las matrices tanto del Jacobiano como de su inversa para cada instante de tiempo Auacuten asiacute computacionalmente es un proceso maacutes sencillo que el anterior en el caso del manipulador tratado por lo que es el meacutetodo elegido que se incorporariacutea en la ejecucioacuten de este lazo de control

Figura 34 Control por dinaacutemica inversa con referencias en el espacio cartesiano dinaacutemica articular y controlador PID

Sin embargo esta ley de control presenta el inconveniente de que para el caacutelculo de la derivada del Jacobiano (퐽) se necesitariacutea tener su expresioacuten simboacutelica Para obtenerla deberiacutea derivarse la matriz jacobiana respecto del tiempo estando esta expresada tambieacuten seguacuten sus teacuterminos simboacutelicos Para manipuladores de menos grados de libertad este proceso puede ser llevado a cabo pero en el caso de manipuladores de seis grados de libertad los modelos son demasiado complejos para llevar a cabo su


caacutelculo y posteriormente utilizar la expresioacuten obtenida para sustituir los valores dados en cada instante de tiempo

4452 El jacobiano en el domino de fuerzas

Para poder comenzar el estudio de un sistema de control que incorpore la dinaacutemica en el espacio cartesiano es necesario definir primero como relacionarlo con los paraacutemetros con los que ya se estaba trabajando

Cuando sobre un mecanismo actuacutean fuerzas el trabajo se produce si el mecanismo se mueve a traveacutes de un desplazamiento El trabajo se define como el producto de una fuerza aplicada sobre un cuerpo y del desplazamiento del cuerpo en la direccioacuten de esta fuerza El trabajo se mide en unidades de energiacutea por lo que debe ser la misma medida en cualquier conjunto de coordenadas generalizadas En concreto se puede equiparar el trabajo realizado en teacuterminos cartesianos con el trabajo realizado en teacuterminos del espacio articular En el caso multidimensional el trabajo es el producto escalar de un vector de fuerza o par y un vector de desplazamiento Por lo tanto tenemos

퐹 middot 훿푋 = 휏 middot 훿휃 (4117)

Siendo F el vector de fuerzas cartesiano que actuacutea sobre el efector final 훿푋 el desplazamiento infinitesimal del efector final en coordenadas cartesianas 휏 el vector de pares de las articulaciones y 훿휃 el vector de desplazamiento infinitesimal de las articulaciones

La relacioacuten entre los espacios articular y cartesiano se produce a traveacutes del jacobiano

훿푋 = 퐽 middot 훿휃 (4118)

Utilizando esta igualdad sobre la foacutermula (4117) se obtiene la siguiente expresioacuten

퐹 middot 퐽 middot 훿휃 = 휏 middot 훿휃 (4119)

Esta uacuteltima expresioacuten es equivalente ademaacutes a la utilizacioacuten de los vectores de fuerza y par traspuestos a ambos lados de la igualdad



Simplificando la ecuacioacuten anterior pasa a ser

퐹 middot 퐽 = 휏 (4121)

El valor de par puede ser ahora expresado en funcioacuten de la fuerza y el jacobiano

휏 = 퐽 middot 퐹 (4122)

La matriz traspuesta del jacobiano mapea la fuerzas que actuacutean sobre el efector final en el espacio cartesiano a los pares de cada una de las articulaciones Antildeadiendo esta ecuacioacuten en la expresioacuten de la dinaacutemica inversa hasta ahora propuesta se obtiene un modelo matemaacutetico que ademaacutes tiene en cuenta las fuerzas que actuacutean sobre el efector final

휏 = M(휃) middot 휃 + H 휃 휃 + 퐹 휃 휃 + G(휃) + 퐽 middot 퐹 (4123)

4453 Control con dinaacutemica en el espacio cartesiano

La ley de control que se propone ahora implica realizar cambios al modelo dinaacutemico ya realizado del manipulador Sin embargo no requiere el uso de la derivada del jacobiano por lo que en este caso la ley de control puede ser llevada a cabo para el manipulador objetivo

Hasta ahora la ecuacioacuten de la dinaacutemica del manipulador ha sido formulada en teacuterminos de la posicioacuten y velocidad del espacio articular ya que de esta forma se ha podido aprovechar la naturaleza de la cadena cinemaacutetica del manipulador y realizar un desarrollo maacutes sencillo

Para reformular esta expresioacuten de modo que se obtenga el modelo dinaacutemico del manipulador en el espacio cartesiano se utiliza la relacioacuten establecida por el Jacobiano en el dominio de las fuerzas La expresioacuten con los teacuterminos dinaacutemicos expresariacutea entonces la magnitud de la fuerza en lugar del par

)()()()middot( xXXX FGHyMF (4124)

][ ZYXZYX nnnFFFF (4125)

Donde F es un vector cuyas componentes son las fuerzas y pares actuando sobre el efector final del manipulador e y es un vector que representa la posicioacuten y orientacioacuten


del mismo representado en el espacio cartesiano (respecto a la base) Para que la ecuacioacuten (4124) sea correcta las matrices que la componen deben definirse

)()middot()middot()( 1 JMJM TX

(4126)

))()middot()()middot(()( 1 JMHJH TX

(4127)

)()middot()( GJG TX

(4128)

)()middot()( FJF TX

(4129)

Al igual que se hizo en el caso del espacio articular se pueden usar las ecuaciones dinaacutemicas cartesianas en un controlador de desacoplamiento y linealizacioacuten Debido a que con la expresioacuten (4124) se calcula un vector de fuerza que debe aplicarse al efector final se tendraacute que utilizar la transpuesta del Jacobiano con el fin de implementar el control es decir despueacutes de que la fuerza es calculada se calcularaacuten los pares que son necesarios aplicar para conseguir el efecto sobre el manipulador tal que se aplicase esa fuerza


La ley de control quedariacutea definida por la siguiente expresioacuten

휏 = 퐽 middot 푀 (휃) middot 푦 + 퐾 middot 푒 + 퐾 middot 푒 +퐾 middot 휀 + H θ θ + G (θ) + F θ θ (4131)

El lazo de control se muestra en la siguiente figura


Figura 35 Control por dinaacutemica inversa con referencias en el espacio cartesiano dinaacutemica cartesiana y controlador PID

446 Control de fuerza

Para las tareas realizadas habitualmente por los manipuladores roboacuteticos el control de posicioacuten que se ha visto hasta ahora proporciona un resultado satisfactorio ya que el seguimiento de una trayectoria hace que la tarea se realice correctamente por ejemplo en el caso de desplazamiento de cargas Generalmente no se produce contacto con elementos del entorno que dificulten el movimiento o sobre los que haya que realizar ciertas presiones por ejemplo Cuando una tarea conlleva alguna de estas caracteriacutesticas el control de posicioacuten puede no ser suficiente al estar interviniendo factores que lo alterariacutean Esos factores que se pueden traducir en fuerzas que actuacutean sobre el sistema pueden ser tambieacuten controlados

Como ejemplo se puede utilizar el caso de una tarea que consista en grabar una placa de acero Se necesitariacutea para ello no solo las posiciones en el espacio que describen las letras o la forma a realizar sino tambieacuten la fuerza que se debe aplicar en esos puntos para conseguir la profundidad deseada Si no se conocen exactamente todos los puntos de la placa (caso comuacuten ya que las piezas tienen imperfecciones) mediante uacutenicamente el control de posicioacuten se podriacutea apenas rozar la superficie o atravesarla Si ademaacutes de definir las posiciones en el plano definido por la pieza a grabar se incluye una referencia de fuerza a aplicar (normal al plano) no se produciraacute el problema anterior Aunque se podriacutea conseguir una alta precisioacuten en el manipulador uacutenicamente con el control de posicioacuten lo que hariacutea que no fuese necesario el control de la fuerza los elementos del entorno son normalmente desconocidos asiacute que este tipo de control se hace necesario para tareas que impliquen contacto de la herramienta o del efector final Este ejemplo pone de manifiesto la necesidad de incorporar el control de fuerza a los sistemas de control hasta ahora propuestos es decir se propondraacute un lazo de control hiacutebrido que incorpore tanto el control de posicioacuten como el de fuerza


Cuando se consideran las fuerzas de contacto del efector final se debe hacer un modelo del elemento sobre el que se estaacute actuando Para introducir el desarrollo de la ley de control de la fuerza se haraacute un modelo de interaccioacuten simple entre el efector final y el elemento de contacto

El control de fuerza consiste en indicar una referencia de ese paraacutemetro con el fin de que el manipulador alcance la posicioacuten constante correspondiente que se nombraraacute 푥 Otra posicioacuten que es necesario definir es la posicioacuten constante 푥 que seraacute la posicioacuten en la que se encuentre el cuerpo sobre el que actuaraacute el manipulador La fuerza actuante se define entonces como

푓 = 푘 middot (푥 minus 푥 ) (4132)

Donde 푘 es una constante que denota la rigidez del cuerpo de contacto y 푥 es la posicioacuten para cada instante de tiempo Se supone un modelo de interaccioacuten de un sistema de segundo grado con un solo grado de libertad 푥 La ecuacioacuten fiacutesica que describe ese sistema es entonces

푓 = 푚 middot 푥 + 푘 middot (푥 minus 푥 ) (4133)

En el que 푚 es la masa del sistema a controlar Esta misma expresioacuten escrita en teacuterminos de la variable a controlar es

푓 = 푚 middot 푘 middot 푓 + 푓 (4134)

Usando el concepto de particionar el controlador al igual que se hizo en las demaacutes leyes de control y utilizando en controlador de tipo PD se obtiene la siguiente expresioacuten para la fuerza

푓 = 푚 middot 푘 middot 푓 + 퐾푑 middot 푒 + 퐾푝 middot 푒 + 푓 (4135)

Siendo 푓 la fuerza deseada para cada punto de la trayectoria y 푒 el error de seguimiento de trayectoria definido por

푒 = 푓 minus 푓 (4136)


A esta ley de control ideal se le pueden aplicar algunos cambios de manera que se adapte mejor a las condiciones reales de funcionamiento Normalmente las trayectorias especificadas en el dominio de la fuerza son constantes ya que las tareas que implican contacto suelen mantenerlo con la misma fuerza para toda la trayectoria Asiacute que normalmente los teacuterminos 푓 y 푓 son nulos Ademaacutes la obtencioacuten del teacutermino 푓 deberiacutea obtenerse por derivacioacuten con la ley propuesta lo que conlleva introducir en la sentildeal mucho ruido al estar este presente generalmente en las medidas de fuerza Como alternativa teniendo en cuenta que la medida de la velocidad es habitual en los manipuladores se puede obtener ese valor mediante

푓 = 푘 middot 푥 (4137)

Con estas dos consideraciones se propone una nueva ley de control que se expresa

푓 = 푚 middot 퐾푝 middot 푘 middot 푒 minus 퐾푑 middot 푥 + 푓 (4138)

La propuesta de esta ley de control se basa en un modelo sencillo de segundo orden Otros modelos maacutes complejos pueden ser utilizados obteniendo una representacioacuten maacutes adecuada del comportamiento real del manipulador En cualquier caso el objetivo es obtener el paraacutemetro de la fuerza a traveacutes de los datos conocidos

La sentildeal de control que es necesario obtener a traveacutes del controlador es

푎 = 퐾푝 middot 푘 middot 푒 minus 퐾푑 middot 푥 (4139)

4461 Control Hiacutebrido Posicioacuten-Fuerza

El controlador hiacutebrido Posicioacuten-Fuerza permite utilizar el seguimiento de trayectorias en el espacio y el control de fuerza simultaacuteneamente Su principio de funcionamiento consistiraacute en dividir el espacio de trabajo en dos subespacios a traveacutes de una formulacioacuten que lo permita La formulacioacuten del espacio de trabajo deberaacute por lo tanto determinar queacute direcciones deberaacuten utilizarse para la fuerza o para la posicioacuten controlada

Se utilizaraacuten dos ejemplos de casos extremos en los que se muestra la divisioacuten del espacio de trabajo necesaria En el primer caso el manipulador se mueve a traveacutes del espacio de manera libre es decir sin ninguacuten tipo de contacto En este caso las limitaciones naturales son todas limitaciones de fuerza ya que no hay ninguna fuerza de reaccioacuten por lo que todas las fuerzas se ven limitadas a cero Con un manipulador que tiene seis grados de libertad eacuteste seriacutea libre para variar su posicioacuten en cada uno


de ellos pero no seriacutea capaz de ejercer fuerzas en ninguna direccioacuten En el segundo caso la situacioacuten del manipulador se plantea con su efector final pegado a una pared En este caso el manipulador estaacute sujeto a seis limitaciones posicioacuten natural ya que no es libre para ser reposicionado Sin embargo el manipulador es libre de ejercer fuerzas y pares en la pared con sus seis grados de libertad En la praacutectica se suele tener en cuenta el control de la fuerza en el contexto de las tareas parcialmente restringidas en las que algunos grados de libertad del sistema estaraacuten bajo control de posicioacuten y otros estaraacuten sujetos al control de fuerza Por lo tanto nos interesa considerar los esquemas de control hiacutebridos El controlador hiacutebrido de posicioacuten y de fuerza debe resolver tres problemas

Control de posicioacuten del manipulador a lo largo de las direcciones en las que existe una restriccioacuten natural de fuerza

Forzar el control del manipulador a lo largo de las direcciones en las que existe una restriccioacuten natural de posicioacuten

Disentildear un esquema para la implantacioacuten de la mezcla arbitraria de estos modos de control

Antes de plantear el caso de un manipulador de seis grados de libertad se va a considerar un caso maacutes simple como explicacioacuten del modo de funcionamiento Se supondraacute un manipulador con tres grados de libertad con articulaciones prismaacuteticas que actuacutea en las direcciones Z Y X los cuales son modificados por cada una de las tres articulaciones La primera variaraacute la direccioacuten Z la segunda la direccioacuten Y y por uacuteltimo la tercera estaraacute relacionada con la direccioacuten X El efector final estaraacute en contacto con una superficie orientado de tal manera que la normal estaraacute en la direccioacuten Y Por lo tanto se requiere un control de fuerza en esa direccioacuten pero el control de posicioacuten se realizaraacute en las direcciones X y Z Entonces se podriacutea tomar como referencia una trayectoria de posicioacuten en las direcciones X y Z mientras que de forma independiente la referencia de una trayectoria de fuerza se dariacutea en la direccioacuten Y

Si quisieacutesemos ser capaces de cambiar la naturaleza de la superficie de contacto tal que su normal tambieacuten puede ser la direccioacuten X o Z se puede generalizar el sistema de control del manipulador de la siguiente manera se construiriacutea la estructura del controlador de manera que pudieacutesemos especificar una trayectoria de posicioacuten completa en los tres grados de libertad asiacute como una trayectoria de fuerza en los mismos tres grados de libertad Por supuesto no podemos controlar al mismo tiempo las seis referencias por lo que se van a establecer configuraciones para indicar queacute componentes de la trayectoria seraacuten seguidos en cada momento dado

Se disentildearaacuten controladores de posicioacuten y de fuerza de forma independiente Especiacuteficamente el controlador de posicioacuten lineal seraacute disentildeado para las variables del espacio de trabajo que representan el movimiento tangente a la superficie Por otra parte el controlador de fuerza lineal se puede disentildear para las variables del espacio de trabajo que representan la fuerza normal a la superficie

A traveacutes de la relacioacuten establecida por el Jacobiano entre el par y la fuerza el control de fuerza puede ser utilizado conjuntamente con el control en el espacio cartesiano


(con dinaacutemica cartesiana) de forma sencilla dado que ambas leyes de control expresan un valor de fuerza que posteriormente se transforma en par para enviar las referencias a los actuadores El problema reside entonces en asignar queacute direcciones y orientaciones se corresponderaacuten con cada uno de los controladores

Para ello se utilizaraacuten matrices en las que se especifique para cada una de las posiciones y articulaciones si seraacute controlada en posicioacuten o en fuerza Al estar trabajando en el espacio cartesiano esta seleccioacuten es posible y tras la transformacioacuten de los valores de fuerza en pares se obtiene el par de cada una de las articulaciones La utilizacioacuten por tanto de otras leyes de control que trabajen en el espacio articular para el control de posicioacuten no es posible con este planteamiento

Dichas matrices se nombran como S y S La matriz S que se utilizaraacute para elegir los grados de libertad en los que se controle la posicioacuten seraacute una matriz diagonal cuyos valores seraacuten unos y ceros Cuando se trate de un uno es esa posicioacuten u orientacioacuten se produciraacute el control por posicioacuten En el caso de que sea un cero se ignoraraacute el dato ya que se corresponderaacute con el control de fuerza Se deduce entonces que la matriz Srsquo es complementaria de la anterior Se define por lo tanto como

푆 = 퐼 minus 푆 (4140)

Siendo 퐼 la matriz identidad 푆 es entonces tambieacuten una matriz diagonal que filtra los grados de libertad sobre los que debe proporcionar su ley de control El lazo de control se representa a continuacioacuten

Figura 36 Controlador hiacutebrido Posicioacuten-Fuerza


A las matrices de ganancias de los controladores se han antildeadido los subiacutendices correspondientes al tipo de control En el caso del controlador de posicioacuten las acciones derivativa proporcional e integral son 퐾푑푝퐾푝푝 푦 퐾푖푝 respectivamente Para el controlador de fuerza se ha propuesto en el subapartado anterior una ley de control con un controlador de tipo PD asiacute que las acciones proporcional y derivativa son 퐾푝푓 푦 퐾푣푑푓 respectivamente

La sentildeal de control de cada uno de los tipos de control es multiplicada por las matrices S y Srsquo para la seleccioacuten de los grados de libertad y posteriormente sumadas para tener una sentildeal que contenga la sentildeal necesaria para el control de todos los grados de libertad a la que se llamaraacute 푎primeprime

푎 = 푆 middot 푎 + 푆 middot 푎prime (4141)

A partir de esta uacuteltima sentildeal el proceso es el mismo que en el caso del control en el espacio cartesiano con la dinaacutemica especiacutefica de dicho espacio


5 Caacutelculos numeacutericos

51 Regulador PID en tiempo continuo

Se realiza a continuacioacuten un ejemplo en el que se calcularaacuten unas ganancias del controlador PID que permitan aproximar la respuesta del sistema a uno de segundo orden pudiendo establecer la frecuencia natural y la constante de amortiguamiento deseadas El proceso se realizaraacute de forma inversa es decir estableciendo el comportamiento que se quiere obtener del sistema se calcularaacuten las ganancias del regulador PID que permitan tener un sistema equivalente

Primero se establecen

휔 = 2 (51)

휉 = 1 (52)

Estos valores dan lugar a unas ganancias y a una ecuacioacuten caracteriacutestica tal que

퐾푝 = 휔 = 4 (53)

퐾푑 = 2 middot 휉 middot 휔 = 4 (54)

푑(푠) = 푠 + 4 middot 푠 + 4 (55)

En el caso de un sistema criacuteticamente amortiguado en el lugar de las raiacuteces se produce la presencia de un polo doble como se puede observar

푠 + 4 middot 푠 + 4 = 0 (56)

푠 = minus2 푠 = minus2 (57)

푑(푠) = (푠 + 2) middot (푠 + 2) (58)


Para conseguir la equivalencia se estableceraacute que el polo doble constituya el polo dominante de manera que el polo adicional del sistema de tercer orden debe cumplir

σ gt 6 middot 2 (59)

Se elige por tanto un valor que cumpla este criterio

σ = 20 (510)

Este polo da lugar entonces a la siguiente ecuacioacuten caracteriacutestica

푑 ( ) = (푠 + 2) middot (푠 + 2) middot (푠 + 20) (511)

푑 ( ) = 푠 + 24 middot 푠 + 84 middot 푠 + 80 (512)

Las ganancias del controlador PID son por tanto

퐾푑 = 24 (513)

퐾푝 = 84 (514)

퐾푖 = 80 (515)

Se debe comprobar que estas ganancias cumplen los criterios establecidos anteriormente para que el sistema sea estable Los valores son positivos asiacute que solo resta comprobar la relacioacuten (474)

24 middot 84 gt 80 (516)

Se tiene finalmente un sistema de tercer orden cuyo comportamiento se puede aproximar a una de segundo orden conocido y que cumple el criterio de estabilidad


52 Reguladores PID y PD en tiempo discreto

Se realiza a continuacioacuten un ejemplo de dos controladores PID y PD que seraacuten definidos por ecuaciones en diferencias para un periodo de muestreo establecido Las ganancias coinciden con las elegidas en el ejemplo anterior Se elige ademaacutes un periodo de muestreo cuyo valor seraacute el objetivo a alcanzar en el montaje real del sistema

푇 = 0001푠 (517)

En este caso se presentan las matrices de ganancias completas que son las matrices que contienen las ganancias para cada una de las articulaciones o grados de libertad seguacuten se trate de un control en el espacio articular o cartesiano respectivamente Eacutestas deben ser diagonales pero sus ganancias pueden variar en el caso de que las caracteriacutesticas de un eslaboacuten hagan que tenga una frecuencia de resonancia mayor por ejemplo Las matrices de ganancias para un regulador de tipo PID son

퐾 =

⎣⎢⎢⎢⎢⎡24 0 0 0 0 00 24 0 0 0 00 0 24 0 0 00 0 0 24 0 00 0 0 0 24 00 0 0 0 0 24⎦

⎥⎥⎥⎥⎤

(518)

퐾 =

⎣⎢⎢⎢⎢⎡84 0 0 0 0 00 84 0 0 0 00 0 84 0 0 00 0 0 84 0 00 0 0 0 84 00 0 0 0 0 84⎦

⎥⎥⎥⎥⎤

(519)

퐾푖 =

⎣⎢⎢⎢⎢⎡80 0 0 0 0 00 80 0 0 0 00 0 80 0 0 00 0 0 80 0 00 0 0 0 80 00 0 0 0 0 80⎦

⎥⎥⎥⎥⎤

(520)

Aplicando los valores a la ecuacioacuten en diferencias del controlador de tipo PID se obtiene el siguiente resultado

푎 = 푞 + 20 middot 푒 + 푎 + 8404 middot 푒 minus 8396 middot 푒 (521)


En el caso del regulador PD la expresioacuten seriacutea la siguiente teniendo en cuenta la equivalencia establecida en el subapartado anterior entre el sistema de segundo orden y el de tercer orden

푎 = 푞 + 4 middot 푒 + 4 middot 푒 (522)


6 Simulacioacuten

Se presenta en este apartado algunos ejemplos de las simulaciones realizadas para probar la correccioacuten de las leyes de control estudiadas en el trabajo En concreto se presentan los resultados de los controles por par calculado y por dinaacutemica inversa El resto de leyes de control seraacuten probadas sobre el hardware de control por la complejidad que presenta el desarrollo de modelos que simulen partes como la generacioacuten de trayectorias en el espacio cartesiano o la medicioacuten de la fuerza aplicada por el efector final por ejemplo

61 Simulacioacuten dinaacutemica

Inicialmente para simular el movimiento del manipulador se haraacute uso de la dinaacutemica directa del mismo Mediante esta simulacioacuten se podraacute comprobar que el lazo de control estaacute correctamente disentildeado En un principio no se tendraacuten en cuenta los accionadores encargados de generar el par requerido en cada articulacioacuten por lo que el sistema seraacute ideal Posteriormente se introduciraacuten dichos actuadores junto con sus lazos de control (servomotores con sus respectivos Drivers) para conseguir una simulacioacuten que se corresponda con el modelo real del manipulador

Para simular la dinaacutemica directa del manipulador se haraacute uso de las matrices y vectores obtenidos a traveacutes del meacutetodo de Lagrange en la dinaacutemica directa representados en la ecuacioacuten (344) Simulando la dinaacutemica directa del manipulador de esta manera se obtienen las aceleraciones angulares de las articulaciones Para conseguir los datos de posiciones y velocidades angulares que seraacuten las variables que se realimentan para el control se debe integrar la aceleracioacuten obtenida El meacutetodo elegido determinaraacute la precisioacuten y eficiencia de la simulacioacuten

Mediante el uso de la toolbox de MATLAB SimDriverline se puede crear un modelo del manipulador que tenga en cuenta los aspectos dinaacutemicos modelados permitiendo obtener una respuesta que sea medida en el manipulador y posteriormente realimentada de las posiciones y velocidades angulares Esto permitiriacutea sustituir la simulacioacuten anterior que se limita a despejar la variable de salida del modelo dinaacutemico En este paso ya se tendriacutea un modelo del que se obtienen respuestas que se acercaraacuten maacutes a la realidad cuanto maacutes exacto sea el modelo dinaacutemico del manipulador

No se dispone de los suficientes datos para hacer un modelo que se ajuste a la dinaacutemica realizada por lo que para poder realizar este paso se deberiacutean realizar pruebas praacutecticas que diesen como resultado el conocimiento de los paraacutemetros necesarios para completar la simulacioacuten como por ejemplo el efecto del counter-balancing system sobre la dinaacutemica del robot la accioacuten de los frenos de los motores etc

El uacuteltimo paso consistiriacutea en antildeadir al modelo disentildeado en SimDriverline de otros elementos presentes en el manipulador real que influencian las variables a medir


como pueden ser las dinaacutemicas de los motores Para este tipo de elementos que seraacuten elementos de potencia se usaraacute la toolbox SimPowerSystems

62 Simulacioacuten de trayectorias

En este apartado se presentan ejemplos de trayectorias generadas para realizar simulaciones de movimientos que se podriacutean pedir al robot Estos ejemplos con los mismos valores seraacuten los utilizados para las leyes de control que se simularaacuten en los siguientes apartados

Los perfiles de velocidad para cada una de las articulaciones son perfiles trapezoidales y se coordinan de manera isoacutecrona Los valores se ajustan a las limitaciones de los motores de manera que no se establezcan nunca referencias que no puedan ser alcanzadas o que impliquen un trabajo que pueda llegar a dantildear los actuadores El ajuste se realiza de manera que los periodos de aceleracioacuten y deceleracioacuten de todas las articulaciones coincidan consiguiendo que no se produzcan tirones en el movimiento del robot

Los valores introducidos de giro deseado se corresponden a la posicioacuten final a alcanzar y estaacuten especificados en grados Los valores elegidos son

th1 = 10ordm (61)

th2 = 15ordm (62)

th3 = 20ordm (63)

th4 = 5ordm (64)

th5 = minus12ordm (65)

th6 = 31ordm (66)


El perfil de velocidad para cada una de las articulaciones es entonces tratando de alcanzar la maacutexima velocidad posible que permita el movimiento coordinado

Figura 37 Perfiles trapezoidales de velocidad angular

Las posiciones comandadas a partir de estas velocidades seraacuten desde la posicioacuten inicial hasta las referencias introducidas a alcanzar

Figura 38 Posiciones angulares de las articulaciones

Por uacuteltimo se representan las aceleraciones de esta trayectoria concreta


Figura 39 Aceleraciones angulares

En el caso de tener en cuenta el jerk los escalones que se producen tendriacutean menos pendiente y se produciriacutea un cambio maacutes suave entre las fases con aceleracioacuten y la fase de velocidad constante Este paraacutemetro seraacute tenido en cuenta en el hardware de control que incorpora oacuterdenes especiacuteficas para poder establecerlo


La primera simulacioacuten realizada fue la correspondiente a la ley de control de par calculado Se realiza suponiendo un tiempo continuo con el fin de comprobar primero el correcto funcionamiento de la ley de control Para ello se utiliza la ley de control propuesta en el apartado 442 El esquema desarrollado en Simulink del lazo de control es el siguiente

Figura 40 Representacioacuten en Simulink del control por par calculado

En eacutel se distinguen todas las partes mostradas en la figura 32 El bloque generador de trayectorias recibe directamente datos de los giros deseados en el espacio articular En posteriores simulaciones se introduciraacute la informacioacuten en el espacio cartesiano y se


transformaraacute al espacio articular tal como seriacutea el funcionamiento real del manipulador cuando la ley de control utilice informacioacuten del espacio articular

El siguiente bloque contiene las matrices dinaacutemicas definidas para nuestro manipulador

Figura 41 Bloque que resuelve la dinaacutemica inversa

En eacutel se introducen los datos de las trayectorias directamente para calcular los valores de los pares que se deberiacutean generar en cada una de las articulaciones Posteriormente se suman las sentildeales de realimentacioacuten Dichas sentildeales proceden del caacutelculo previo de los errores y de su multiplicacioacuten por las matrices de ganancias proporcional y derivativa Se muestran en la siguiente figura los valores de los pares de cada una de las articulaciones para la trayectoria simulada

Figura 42 Pares de cada una de las articulaciones

Cada una de las articulaciones tiene unas caracteriacutesticas dinaacutemicas diferentes por lo que articulaciones como la segunda (representada en rosa) requieren una mayor cantidad de par para poder realizar los movimientos

El siguiente bloque el de la dinaacutemica directa recibe de la dinaacutemica inversa el valor de la sentildeal de control que se considera el par


Figura 43 Bloque que resuelve la dinaacutemica directa

Ademaacutes se realimentan los valores de las posiciones y velocidades angulares reales para a su vez poder calcularlas correctamente y no en funcioacuten de los valores de la trayectoria que no tiene porque corresponder exactamente con los valores medidos La siguiente figura representa el ejemplo de la sentildeal producida para cada articulacioacuten durante la simulacioacuten

Figura 44 Sentildeales de control

Los bloques de integracioacuten serviraacuten para obtener las velocidades y las posiciones angulares a partir de la aceleracioacuten angular calculada

Se supone que se parte de posicioacuten inicial en la que todos los valores angulares son igual a cero

El valor obtenido a la salida del sistema comparado con la referencia de posicioacuten de la trayectoria se muestra a continuacioacuten


Figura 45 Trayectorias de todas las articulaciones

El seguimiento de trayectoria se cumple por lo que el objetivo final de la ley de control es alcanzado Examinando especiacuteficamente el comportamiento de la sexta articulacioacuten se veraacute el resultado anterior maacutes ampliado

Figura 46 Seguimiento de la trayectoria de la sexta articulacioacuten

Para la ejecucioacuten que proporciona estos resultados se establecieron unos valores para las ganancias

퐾푑 = 20 (67)

퐾푝 = 100 (68)

Se usa esta articulacioacuten para comprobar los resultados dado que es la que menor inercia presenta por lo tanto este resultado es representativo del comportamiento que


se puede esperar de la ley de control ya que el resto de articulaciones se veraacuten menos afectadas por cualquier tipo de factor al presentar una mayor resistencia al cambio Como se puede apreciar en la figura 51 el tiempo de establecimiento es muy elevado con una sobreoscilacioacuten maacutexima de 002ordm Este resultado evidencia el hecho de que los polos no estaacuten fijos ya que con las ganancias elegidas el sistema deberiacutea comportarse como un sistema criacuteticamente amortiguado Sin embargo el hecho de que la matriz de inercias variacutee hace que el resultado no se ajuste correctamente a la referencia introducida La matriz de inercias tiene valores muy elevados al tratarse de un robot de grandes dimensiones por lo que su inversa tiene valores muy pequentildeos Esto hace que las ganancias a elegir deban ser muy grandes ya que como se ve en este ejemplo la velocidad de respuesta es muy baja

Se prueban a continuacioacuten unas ganancias mayores que proporcionen un resultado mejor que se ajuste maacutes raacutepidamente a la referencia a seguir Las ganancias son ahora

퐾푑 = 200 (69)

퐾푝 = 10000 (610)

El resultado del seguimiento de la sexta articulacioacuten es ahora

Figura 47 Seguimiento de trayectoria de la articulacioacuten 6

Como se aprecia en la figura anterior la sobreoscilacioacuten es menor con un maacuteximo de 0005ordm Sin embargo el tiempo de establecimiento sigue siendo muy elevado ademaacutes de que la oscilacioacuten es variable Sigue estando presente por lo tanto el problema de que los polos no estaacuten fijos y los resultados no son por tanto los adecuados ya que no se puede controlar ese aspecto

Se concluye entonces que la ley de control es correcta y que los resultados obtenidos son correctos pero los problemas inherentes de la ley de control hacen que no sea un sistema muy adecuado para su implementacioacuten final por lo que se pasaraacute ahora a presentar las pruebas realizadas con el control por dinaacutemica inversa



La siguiente simulacioacuten se realiza con el control por dinaacutemica inversa En este caso la representacioacuten en Simulink es

Figura 48 Representacioacuten en Simulink del control por dinaacutemica inversa

Se utiliza como antildeadido respecto de la simulacioacuten anterior la cinemaacutetica inversa para obtener las posiciones articulares a partir de los datos de referencia de la posicioacuten y orientacioacuten del efector final en el espacio cartesiano como se observa en la siguiente figura que representa la estructura general del funcionamiento de esta ley de control junto con la generacioacuten de trayectorias

Figura 49 Representacioacuten general de los bloques de simulacioacuten del control por dinaacutemica inversa

A partir de las referencias se utiliza la transformacioacuten de aacutengulos fijos para obtener los datos para poder trabajar con las transformadas homogeacuteneas de la cinemaacutetica


inversa A partir de ella se introduce en el bloque de control los valores de las posiciones articulares de la posicioacuten a alcanzar

En esta simulacioacuten se utiliza el tiempo discreto para acercar maacutes el funcionamiento a la ejecucioacuten real que tendraacute el modelo final Por ello se utilizan otros bloques adicionales que permiten realizar este tipo de simulacioacuten

Tras el caacutelculo de los errores de posicioacuten y de velocidad se utilizan dos bloques correspondientes a bloqueadores de orden cero con el objetivo de conseguir que entre cada dos ciclos de ejecucioacuten consecutivos se mantenga constante el uacuteltimo valor establecido En ellos se establece el periodo de muestreo que seraacute de una mileacutesima al igual que en el ejemplo numeacuterico realizado en el apartado 52

Figura 50 Bloque del bloqueador de orden cero

Despueacutes de los bloqueadores se utilizan dos bloques de retraso de sentildeal de manera que a traveacutes de ellos se puedan obtener los valores del periodo de muestreo anterior al actual para su utilizacioacuten en la ecuacioacuten en diferencias que representa el funcionamiento del controlador lineal En la siguiente figura se muestra este tipo de bloque

Figura 51 Bloque de retraso de sentildeal

La funcioacuten Controlador contiene la ecuacioacuten en diferencias del controlador A ella entran ademaacutes de los errores de posicioacuten y velocidad la aceleracioacuten correspondiente a la trayectoria definida asiacute como el valor del error de velocidad y la sentildeal de control respecto del error de posicioacuten del periodo de muestreo anterior Ademaacutes los valores de las ganancias del controlador tambieacuten son definidos en eacutel de manera que a partir de la ecuacioacuten en diferencias geneacuterica se puedan sustituir los valores elegidos y obtener directamente la expresioacuten asociada a ellos que generaraacute la sentildeal de control adecuada Se establecen en eacutel por tanto las ecuaciones en diferencias del controlador PD y PID para realizar diferentes pruebas


Figura 52 Bloque que implementa el controlador del sistema

El siguiente bloque contiene las matrices dinaacutemicas y su funcionamiento es el mismo que en el caso del control por par calculado con la diferencia de que en este caso se utilizan los valores de la salida del manipulador y no los correspondientes a la trayectoria teniendo asiacute los valores correctos para compensar la dinaacutemica real del manipulador

A la salida del par de este bloque se han antildeadido bloques de saturacioacuten al valor de cada una de las articulaciones con el fin de evitar que se comanden valores superiores al valor maacuteximo que se puede generar en cada una de ellas determinado por la capacidad del motor y la etapa reductora asociada Este valor es proporcionado por el fabricante

Figura 53 Bloques de saturacioacuten

El uacuteltimo bloque se corresponde con la dinaacutemica directa La diferencia respecto del bloque utilizado en el control por par calculado es que en este caso se utilizan los mismos caacutelculos que en la dinaacutemica inversa ya que eacutesta estaacute realizada en funcioacuten de los valores reales medidos

La primera prueba se realiza con un controlador de tipo PD Los resultados del control de posicioacuten son los siguientes con los valores de las ganancias establecidas en el apartado 51 para el sistema equivalente de segundo orden


Figura 54 Seguimiento de las trayectorias de las articulaciones

Analizando ahora el resultado de seguimiento de la sexta articulacioacuten se observa que con una ganancia mucho menor se obtiene un mejor resultado que con el control por par calculado

Figura 55 Seguimiento de trayectoria de la sexta articulacioacuten

En este caso solamente existe una pequentildea sobreoscilacioacuten cuando se finaliza el movimiento debido a que la aceleracioacuten cambia muy raacutepidamente a cero Utilizando un jerk que suavizase ese cambio en el movimiento este efecto no tendriacutea esta influencia en la respuesta de la articulacioacuten Auacuten asiacute se observa que el error de posicioacuten es correctamente eliminado y el posicionamiento es correcto

Utilizando el mismo controlador se introduce ahora un valor constante al valor de par comandado a cada una de las articulaciones de modo que simule una perturbacioacuten Se utiliza un valor de 200 Nm como atenuacioacuten a cada una de las sentildeales El resultado de seguimiento es entonces para la misma articulacioacuten


Figura 56 Respuesta con una perturbacioacuten constante en la sexta articulacioacuten

Se ve que el controlador PD no es capaz de eliminar el error que introduce esa perturbacioacuten resultando en un error de posicioacuten absoluto de 0587ordm lo que supone una desviacioacuten importante del efector final

En la misma simulacioacuten se analiza tambieacuten el seguimiento de la tercera articulacioacuten que tiene una inercia mucho mayor La representacioacuten es

Figura 57 Respuesta con una perturbacioacuten constante en la tercera articulacioacuten

En este caso la perturbacioacuten de 200Nm tiene un efecto mucho menor debido a que el par generado para ella es mayor a causa de que sus teacuterminos dinaacutemicos son maacutes elevados El error de posicioacuten absoluto en reacutegimen estacionario es en este caso de 0001ordm

Dado que al no disponer de accioacuten integral las perturbaciones no son atenuadas y se reflejan en un error constante en la posicioacuten en reacutegimen permanente se prueba a realizar un controlador de tipo PID Se prueba primero el control sin ninguacuten tipo de perturbacioacuten mostrando el resultado de la sexta articulacioacuten Se utilizan los valores de las ganancias equivalentes a las del controlador PD para un sistema de tercer orden


Figura 58 Seguimiento en la sexta articulacioacuten con un controlador de tipo PID

El resultado mejora respecto del controlador PD afectando de menor manera la alta frecuencia que supone el cambio de aceleracioacuten y eliminando tambieacuten el error de posicioacuten alcanzando finalmente la posicioacuten deseada

Ahora se procede a introducir la misma perturbacioacuten que en el caso del controlador de tipo PD El resultado nuevamente para la sexta articulacioacuten es

Figura 59 Resultado con una perturbacioacuten constante

A pesar de que el error es eliminado la baja frecuencia natural del sistema hace que la respuesta sea lenta Por lo tanto se procede ahora a aumentar los valores de las ganancias del controlador Los valores utilizados son ahora

퐾 =

⎣⎢⎢⎢⎢⎡33 0 0 0 0 00 33 0 0 0 00 0 33 0 0 00 0 0 33 0 00 0 0 0 33 00 0 0 0 0 33⎦

⎥⎥⎥⎥⎤

(611)


퐾 =

⎣⎢⎢⎢⎢⎡216 0 0 0 0 0

0 216 0 0 0 00 0 216 0 0 00 0 0 216 0 00 0 0 0 216 00 0 0 0 0 216⎦

⎥⎥⎥⎥⎤

(612)

퐾 =

⎣⎢⎢⎢⎢⎡400 0 0 0 0 0

0 400 0 0 0 00 0 400 0 0 00 0 0 400 0 00 0 0 0 400 00 0 0 0 0 400⎦

⎥⎥⎥⎥⎤

(613)

Se vuelve a analizar el resultado de la uacuteltima articulacioacuten

Figura 60

Comparando las dos figuras anteriores se observa que la velocidad de respuesta es mayor con unas ganancias mayores que establecen una frecuencia natural del sistema maacutes elevada


7 Implementacioacuten

Durante la uacuteltima fase del trabajo tras la realizacioacuten de diversas simulaciones sobre el software MatlabSimulink se procede a la implementacioacuten de los modelos obtenidos en el hardware de control Dicho hardware seraacute el encargado de realizar el control final del manipulador y se utilizaraacute previamente para realizar las pruebas de Hardware-in-the-loop utilizando la maqueta virtual desarrollada en Simulink

Dicha maqueta representa el comportamiento del manipulador teniendo en cuenta los aspectos cinemaacuteticos y dinaacutemicos desarrollados en este trabajo Se han desarrollado varios modelos que representan la cinemaacutetica directa la cinemaacutetica inversa la dinaacutemica directa y la dinaacutemica inversa En la siguiente figura se muestra una captura del software en la que se representa el modelo de la dinaacutemica directa

Figura 61 Modelo de la Dinaacutemica Directa del manipulador

Este modelo en concreto seraacute muy uacutetil para realizar las pruebas de los lazos de control ya que como se ha visto en este documento en todos ellos el manipulador recibe los pares que se deben generar A la salida del mismo se pretenden obtener las posiciones y velocidades articulares que se correspondan con las medidas realizadas por los resolvers en la realidad Esta descripcioacuten se corresponde con la dinaacutemica directa por lo que a partir de ella se podraacute obtener la realimentacioacuten y comprobar que la trayectoria descrita es la correcta

El hardware de control que se utiliza se comunica por tanto con la maqueta del manipulador Actualmente se estaacuten programando en dicho hardware los modelos matemaacuteticos del manipulador El proceso consiste en traducir el trabajo ya realizado en Matlab al lenguaje propio del hardware Una vez finalizado este proceso se procederaacute a la simulacioacuten Hardware-in-the-loop


8 Conclusiones y trabajos futuros

Los sistemas de control de los fabricantes de roboacutetica industrial actuales al ser cerrados no permiten conocer las leyes de control que utilizan Auacuten asiacute la falta de precisioacuten que presentan hace que la implementacioacuten de las leyes de control propuestas puedan mejorar los resultados utilizaacutendose un hardware especiacutefico con la suficiente potencia para conseguir un periodo de ejecucioacuten bajo y tambieacuten mediante la posibilidad de utilizar otro tipo de sensoacuterica Ademaacutes la propuesta de un lazo de control abierto ofrece la posibilidad de modificar o ampliar sus elementos lo que otorga flexibilidad para poder introducir cualquier tipo de mejora

Con la disposicioacuten de los modelos matemaacuteticos del manipulador es posible realizar muacuteltiples leyes de control que conduzcan a un seguimiento de trayectoria adecuado Los modelos propuestos utilizan la dinaacutemica inversa para cancelar las no linealidades del sistema Pero se podriacutean realizar tambieacuten otros planteamientos que utilizasen esos modelos para realizar otro tipo de leyes que utilicen sensores de otro tipo de magnitudes como por ejemplo la aceleracioacuten

El estudio y conocimiento de los modelos cinemaacuteticos y dinaacutemicos es por tanto la base para el desarrollo de cualquier tipo de control La correccioacuten de los mismos asiacute como la inclusioacuten de la mayor cantidad de paraacutemetros posible que describan con la mayor exactitud el modelo real es el punto de partida del desarrollo de cualquier control que pueda ofrecer buenos resultados de precisioacuten

El uso de otro tipo de planteamientos para el desarrollo de los modelos matemaacuteticos que sustituyan a las matrices de transformacioacuten homogeacuteneas utilizadas tambieacuten puede ser planteado para conseguir un mejor tiempo de coacutemputo de los modelos y evitar la aparicioacuten de singularidades El planteamiento por dobles cuaternios dariacutea esas ventajas lo que conllevariacutea tambieacuten una mejora de la precisioacuten del posicionamiento del robot

El presente Trabajo Fin de Maacutester es el punto de partida para la ejecucioacuten de las tareas restantes para la finalizacioacuten del proyecto MILL2MATIC

Programacioacuten de los modelos matemaacuteticos en el hardware especiacutefico para control de movimiento de ejes interpolados

Realizacioacuten de pruebas Hardware-in-the-Loop del control implementado mediante la conexioacuten del hardware al modelo de MatlabSimulink

Implementacioacuten en maacutequina y puesta en marcha

Planteamiento de otras leyes de control que permitan el uso de otro tipo de sensores de medida directa sobre la estructura mecaacutenica del manipulador


9 Presupuesto

En el desarrollo de este Trabajo Fin de Maacutester ha sido necesario disponer del software y hardware necesarios Entre el software utilizado destaca el sistema operativo Windows Vista Office 2010 y Matlab 2008b

Para calcular el presupuesto se han dividido los costes en las siguientes partes

Coste de Hardware (CH) Coste de Software (CS) Material (CM) Gastos de personal (CP)

91 Hardware

En esta seccioacuten se recogen todos los gastos relacionados con el material informaacutetico utilizado La estimacioacuten de los costes se realiza a partir de la siguiente expresioacuten

kUTaTuCC Ti middot

100middotmiddot (81)

Donde

Ci coste computable al concepto CT coste total de su adquisicioacuten Tu tiempo estimado de uso en antildeos Ta tiempo estimado de amortizacioacuten en antildeos U amortizacioacuten porcentual para el equipo K iacutendice de inflacioacuten y reparaciones

La duracioacuten total de este proyecto ha sido de 5 meses En la siguiente tabla se recogen los costes globales debidos al hardware utilizado junto con el software que veniacutea incluido en el precio de compra

Hardware CT(euro) Tu(antildeos) Ta(antildeos) U() Ci(euro)

Ordenador Portaacutetil Licencia Windows y

Office 2010 2500 7 2 595 52083

Gastos totales de Hardware 52083

Tabla 6


92 Software

En esta seccioacuten se determina el gasto debido a las licencias de los programas utilizados para el desarrollo del trabajo De nuevo la estimacioacuten de los costes se realiza mediante la expresioacuten (81)

Software CT(euro) Tu(antildeos) Ta(antildeos) U() Ci(euro)

Licencia Matlab 2008b 200000 7 2 595 41650

Licencia Simulink 2008b 100000 7 2 595 20825

Toolbox Simscape para MatlabSimulink

100000 7 2 595 20825

Gastos totales de Software 83300

Tabla 7

93 Costes de personal

En la realizacioacuten del proyecto los gastos debidos a los recursos de personal se recogen en la tabla a continuacioacuten

Tarea Cunitario(euro) t(h) Ci(euro)

Desarrollo del TFM 1174 792 929966

Gastos Totales de Personal 929966

Tabla 8

El coste por hora de ingenieriacutea ha sido calculado en base a lo estipulado en el convenio nacional de empresas de ingenieriacutea y oficinas de estudios teacutecnicos


94 Coste total del proyecto

Finalmente se calcula el coste total del proyecto sumando los resultados obtenidos en las secciones previas

Gasto Coste (euro)

Hardware 52083

Software 83300

Personal 929966

Coste bruto del proyecto 1065349

Gastos generales (16) 17046

Beneficio industrial (0) 000

Total 1082395

IVA (21 ) 227303

COSTE TOTAL DEL PROYECTO 1309698

Tabla 9


10 Bibliografiacutea

[1] Carl DCrane J D (1998) Kinematic Analysis of robot manipulators Cambridge University Press [2] Craig J (2005) Introduction to Robotics Mechanics and Control Pearson Education

[3] Dinesh Manocha J F (1994) Efficient Inverse Kinematics for General 6R Manipulators IEEE Transactions on robotics and automation

[4] Dombre Etienne Khalil Wisama (2001) Robot Manipulators Modeling Performance Analisys and Control

[5] Gene HGolub C F (1996) Matrix Computations

[6] Gordillo J R (2010) Planeacioacuten de Trayectorias en Sistemas de Manipuladores Roboacuteticos Muacuteltiples

[7] Ioannis Z Emiris B M (1999) Matrices in Elimination Theory

[8] John Faber Archila Diacuteaz M S (2008) Study and modeling of KR 6 KUKA robot Riacuteo de Janeiro Laboratorio de Roboacutetica Universidad Federal

[9] LLewis F (2004) Robot Manipulator Control Theory and Practice

[10] Marghitu D B (2005) Kinematic Chains and Machine Components Design Elsevier Academic Press

[11] Mark WSpong S H Robot Modeling and Control

[12] Mark WSpong Anderson R J (1992) Asymptotic Stability for force reflecting teleoperatiors with time delay

[13] Murray R M (1994) A Matemathical Introduction to Robotic Manipulation Berkeley University of California

[14] N Jazar Reza (2010) Theory of Applied Robotics

[15] Ollero Baturone Aniacutebal (2001) Roboacutetica manipuladores y robots moacuteviles

[16] Paul Van Dooren P D (1983) The Eigenstructure of an Arbitrary Polynomial Matrix Computational Aspects

[17] Peacuterez Garciacutea Miguel Aacutengel (2002) Instrumentacioacuten Electroacutenica

[18] R K Mittal I (2003) Robotics and Control McGraw-Hill

[19] Rosillo Guerrero Nuria Estudio e implementacioacuten de controladores dinaacutemicos para robots industriales en tiempo real


[20] Sahu S (2008) Development of Alternative Methods for Robot Kinematics Orissa India Department of Mechanical Engineering National Institute of Technology Rourkela

[21] Sandin P E (2003) Robot Mechanisms and Mechanical Devices McGraw-Hill

[22] Saxena T (1997) Efficient variable elimination using Resultants State University of New York at Albany Departament of Computer Science

[23] Shuguang Qiao Q L-J (2007) Inverse kinematic analysis of the general 6R serial manipulators based on double quaternions Mechanism and Machine Teory

[24] Sturmfels B (2002) Solving Systems of Polynomial Equations University of California Berkeley Departament of Mathematics

[25] Su H-J (2004) Computer-Aided Constrained Robot Design Using Mechanism Synthesis Therory University of California

[26] W Lindegaard Karl-Peter (2003) Acceleration feedback in Dynamic Positioning


Iacutendice

1 INTRODUCCIOacuteN 7

11 Estado actual de la roboacutetica 7

12 Caracteriacutesticas actuales de los robots 8 121 Causas de la falta de precisioacuten y continuidad de trayectorias 10

13 Objetivos del proyecto 11

14 Descripcioacuten del manipulador 12 141 Eslabones 13 142 Articulaciones 17 143 Compensadores 19

15 Estructura del Trabajo 19 151 Diagrama de Gantt 20

2 ESTUDIO CINEMAacuteTICO 22

21 Paraacutemetros Denavit-Hartenberg 22

22 Matrices de Transformacioacuten Homogeacuteneas 24 221 Fixed Angles X-Y-Z 26

23 Cinemaacutetica directa 27

24 Cinemaacutetica inversa meacutetodo geomeacutetrico 29

25 Cinemaacutetica de la Velocidad el Jacobiano 40 251 Caacutelculo del Jacobiano 42 252 Singularidades 44

3 ESTUDIO DINAacuteMICO 46

31 Distribucioacuten de la Masa del Manipulador 46 311 Centros de Masas 46

32 Pseudo-Matriz de Inercia 47

33 Dinaacutemica Inversa 49 331 Formulacioacuten Euler-Lagrange 50

3311 Velocidad de un punto del manipulador 51 3312 Energiacutea Cineacutetica 52 3313 Energiacutea Potencial 53 3314 Ecuaciones del Movimiento del Manipulador 54 3315 Introduccioacuten de los efectos de friccioacuten 56 3316 Dinaacutemica de los actuadores 57



4 CONTROL 60





432 Drivers 73

















8 PRESUPUESTO 131

81 Hardware 131

82 Software 132





MEMORIA



1 Introduccioacuten




















































141 Eslabones























142 Articulaciones












A3 (θ3) +60 a -184ordm 69 ordms




Tabla 1

143 Compensadores




















Documentacioacuten

























Figura 15




1 -pi2 0500 1045 θ1

2 0 -1300 0 θ 2

3 -pi2 -0055 0 θ 3

4 pi2 0 -1025 θ 4

5 -pi2 0 0 θ 5

6 0 0 0290 θ 6

Tabla 2




PTTTTP iPi

QP

RQ

iR




PTP iii

i middot11 (22)


TTTTT Pi

QP

RQ

iR


(23)



iZiZiXiXi

i dDRaDRT (24)


)()middot( 111

iiZiiXi



푇 =



(26)







푅 (훼훽 훾) = 푅 (훼)푅 (훽)푅 (훾) =


0 0 1middot




(27)


푅 (αβ γ) =


(28)




(29)






푐표푠훽) (212)









푇 푇 푇 푇 푇 푇 (213)




푇 = 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) (215)


(216)

(217)

1000010


1

1111

1111

01 d

aa

T

10000100


2222

2222

12

aa

T


(218)

(219)

(220)

(221)




10000010


3333

3333

23

aa

T

1000010

0cos0sin0sin0cos

4

44

44

34 dT


55

55

45

T

1000100

00cossin00sincos

6

66

66

56 dT










Figura 16


푙 = 푎 + 푙 (224)


푟 = 푃 + 푃 (225)








휃 =휋2



Figura 17





Figura 18






Figura 21


푟 = 푃 + 푃 (230)









휃 = 휋 minus 휑 + 휀 (233)





=0



(235)





Figura 22




Figura 23









Tabla 3






Figura 24



Figura 25






Figura 26










푦 = 푣휔 (246)



vn Jv 0 (247)

Jn

0 (248)



)(Jy (249)



JJ

J v (250)











n

iiinnn zkRkRk

1

01

01

0121


(253)





][ 01

01

00 nzzzJ (254)


푃 = 푣 (255)


i

n

i i

NN

vv

middot1

00

(256)


i

Nv

vJ

0

(257)



푣 = 푣 + 휔 times 푃 (258)







0

1

01

001 )(

i

iniv

zPPz

JJ

J

(261)


0

501

00

05

06

05

01

06

01

00

06

00 )()()(

zzzPPzPPzPPz

J

(262)

252 Singularidades

























dmrrI Ti

eslaboacuteni


Donde

푟 = [푥 푦 푧 1] (32)



I


2

2

2

(33)


dmzyI XX )( 22 (34)

dmzxIYY )( 22 (35)


dmyxIZZ )( 22 (36)




dmyzIYZ middot (39)


dmxxm middot (310)

dmyym middot (311)

dmzzm middot (312)


mzmymxm

zmIIIII

ymIIIII

xmIIIII

IZZYYXX

YZXZ

YZZZYYXX

XY

XZXYZZYYXX

2

2

2

(313)













Figura 28




휃




0ii r (315)


푇





푣 =푑푟푑푡





(318)


2)(21

iii vdmdK


(319)


(320)

El teacutermino iT

ii


(321)


(322)




ii




(324)

kj

i

j

Ti

kkki

Ti

ii

ii

ki


21

1

11

0

1

11

0

kj

i

j

Ti

kkki

Ti

ii

ii

ki


21

1

11

0

1

11

0

kj

i

j

Ti

kkki

i

ki


21

1

11

0

1

11

0

kj

n

i

i

j

i

k

Ti

kkkii


21

1 1 1

11

011

0

TZYX gggg ]0[



(325)






(327)


휏 =푑푑푡

middot퐿휃

minus퐿휃

(328)


(329)

Donde

n

ii

iii

n

ii rTgmPP

1

0

1middotmiddotmiddot

n

ii

iiikj

n

i

i

j

i

k

Ti

kkkii

jjj rTgmTQTITQTTrL

1

0

1 1 1

11

011


ikj

n

j

n

kijkj

n

jiji GhM




(330)


(331)


(332)



(333)

n

jip

Tpippjij dIdTrM

)max(middotmiddot

n

kjip

Tpip

p

pkijk dI

dd

Trh)max(

middotmiddot)(

pp

n

ipiPi rdgmG

1

middotmiddotmiddot

ijijTQT

d ij

jjij 0

middotmiddot 11

0


(334)




(335)






di

jjj

kkk

ik

kkj

jj

k

ij


)( 11

11

0

11

11

0

)()()middot( GHM


(336)



(337)



(338)



(339)



middotfriccioacuten



)()()()middot( FGHM


















)]()()()[(1 FGHM (344)



4 Control

























푡 =휔훼

(41)



푡 = 푡 (42)


∆휃 =12




푡 =∆휃휔

minus 푡 (44)


휃 =12



훼 = 훼 (47)




휔 = 휔 (49)

훼 = 0 (410)










42 Sensores











422 Resolvers



푒 = 푒 cos휃 (414)


푒 = 푒 sen휃 (415)















43 Actuadores












431 Servomotores













푃 = 푃 (419)






휔휔

=푟푟

(422)

432 Drivers










mecaacutenicos










































4421 Controlador PD













푒 =휏퐾푝

(432)








푑푑푡


푒 (434)


푑푑푡

푒푒 = 0 퐼















푠2 1 푀 middotKp



0

Tabla 4














휉 le 1 (446)





휔 =푘퐽

(447)


휔 lt휔2

(448)













훼 = 푀(휃) (451)

훽 = 퐻(휃휃 ) + 퐺(휃) + 퐹(휃휃 ) (452)



휃 = 휏 (453)




















푒 = 0 (461)







휀 = 푒 (462)


푑푑푡

휀푒푒


(463)





푑푑푡

휀푒푒


middot휀푒푒

(465)








풔 0 00 풔 00 0 풔


= 0 (470)


= 0 (471)





s3 1 Kp

s2 Kd Ki


0


퐾푑

0

Tabla 5



gt 0 (473)




퐾푑gt 0 (475)











444 Control Digital
















푎 = 휃 (480)

푎휃 푑

= 1 (481)



푎푒

= 퐾 (483)







푠 (485)



푠 cong2푇


(486)



푎푒





푎푒





middot 퐾푖 (489)



middot 퐾푖 (490)


middot 퐾푖 +푇2





푎 = 푎 + 퐾푝 +푇2














푦(푡) = [푋푌푍퐴퐵퐶] (496)


푒 =푒푒 (497)





푒 =12



휀 = 푒 푑푡 (4100)


푦 = 푣ω (4101)


푦 =푣ω (4102)





푦 = 푎α (4104)


푦 =푎α (4105)



























































(4126)


(4127)

)()middot()( GJG TX

(4128)

)()middot()( FJF TX

(4129)




























































휔 = 2 (51)

휉 = 1 (52)


퐾푝 = 휔 = 4 (53)


푑(푠) = 푠 + 4 middot 푠 + 4 (55)


푠 + 4 middot 푠 + 4 = 0 (56)


푑(푠) = (푠 + 2) middot (푠 + 2) (58)





σ = 20 (510)





퐾푑 = 24 (513)

퐾푝 = 84 (514)

퐾푖 = 80 (515)


24 middot 84 gt 80 (516)





푇 = 0001푠 (517)


퐾 =

⎣⎢⎢⎢⎢⎡24 0 0 0 0 00 24 0 0 0 00 0 24 0 0 00 0 0 24 0 00 0 0 0 24 00 0 0 0 0 24⎦

⎥⎥⎥⎥⎤

(518)

퐾 =

⎣⎢⎢⎢⎢⎡84 0 0 0 0 00 84 0 0 0 00 0 84 0 0 00 0 0 84 0 00 0 0 0 84 00 0 0 0 0 84⎦

⎥⎥⎥⎥⎤

(519)

퐾푖 =

⎣⎢⎢⎢⎢⎡80 0 0 0 0 00 80 0 0 0 00 0 80 0 0 00 0 0 80 0 00 0 0 0 80 00 0 0 0 0 80⎦

⎥⎥⎥⎥⎤

(520)







6 Simulacioacuten














th1 = 10ordm (61)

th2 = 15ordm (62)

th3 = 20ordm (63)

th4 = 5ordm (64)


th6 = 31ordm (66)


































퐾푑 = 20 (67)

퐾푝 = 100 (68)





퐾푑 = 200 (69)

퐾푝 = 10000 (610)














































퐾 =

⎣⎢⎢⎢⎢⎡33 0 0 0 0 00 33 0 0 0 00 0 33 0 0 00 0 0 33 0 00 0 0 0 33 00 0 0 0 0 33⎦

⎥⎥⎥⎥⎤

(611)


퐾 =

⎣⎢⎢⎢⎢⎡216 0 0 0 0 0

0 216 0 0 0 00 0 216 0 0 00 0 0 216 0 00 0 0 0 216 00 0 0 0 0 216⎦

⎥⎥⎥⎥⎤

(612)

퐾 =

⎣⎢⎢⎢⎢⎡400 0 0 0 0 0

0 400 0 0 0 00 0 400 0 0 00 0 0 400 0 00 0 0 0 400 00 0 0 0 0 400⎦

⎥⎥⎥⎥⎤

(613)


Figura 60





















9 Presupuesto




91 Hardware


kUTaTuCC Ti middot


Donde





Office 2010 2500 7 2 595 52083


Tabla 6


92 Software






100000 7 2 595 20825


Tabla 7






Tabla 8





Gasto Coste (euro)

Hardware 52083

Software 83300

Personal 929966




Total 1082395

IVA (21 ) 227303


Tabla 9































4 CONTROL 60





432 Drivers 73

















8 PRESUPUESTO 131

81 Hardware 131

82 Software 132





MEMORIA



1 Introduccioacuten




















































141 Eslabones























142 Articulaciones












A3 (θ3) +60 a -184ordm 69 ordms




Tabla 1

143 Compensadores




















Documentacioacuten

























Figura 15




1 -pi2 0500 1045 θ1

2 0 -1300 0 θ 2

3 -pi2 -0055 0 θ 3

4 pi2 0 -1025 θ 4

5 -pi2 0 0 θ 5

6 0 0 0290 θ 6

Tabla 2




PTTTTP iPi

QP

RQ

iR




PTP iii

i middot11 (22)


TTTTT Pi

QP

RQ

iR


(23)



iZiZiXiXi

i dDRaDRT (24)


)()middot( 111

iiZiiXi



푇 =



(26)







푅 (훼훽 훾) = 푅 (훼)푅 (훽)푅 (훾) =


0 0 1middot




(27)


푅 (αβ γ) =


(28)




(29)






푐표푠훽) (212)









푇 푇 푇 푇 푇 푇 (213)




푇 = 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) (215)


(216)

(217)

1000010


1

1111

1111

01 d

aa

T

10000100


2222

2222

12

aa

T


(218)

(219)

(220)

(221)




10000010


3333

3333

23

aa

T

1000010

0cos0sin0sin0cos

4

44

44

34 dT


55

55

45

T

1000100

00cossin00sincos

6

66

66

56 dT










Figura 16


푙 = 푎 + 푙 (224)


푟 = 푃 + 푃 (225)








휃 =휋2



Figura 17





Figura 18






Figura 21


푟 = 푃 + 푃 (230)









휃 = 휋 minus 휑 + 휀 (233)





=0



(235)





Figura 22




Figura 23









Tabla 3






Figura 24



Figura 25






Figura 26










푦 = 푣휔 (246)



vn Jv 0 (247)

Jn

0 (248)



)(Jy (249)



JJ

J v (250)











n

iiinnn zkRkRk

1

01

01

0121


(253)





][ 01

01

00 nzzzJ (254)


푃 = 푣 (255)


i

n

i i

NN

vv

middot1

00

(256)


i

Nv

vJ

0

(257)



푣 = 푣 + 휔 times 푃 (258)







0

1

01

001 )(

i

iniv

zPPz

JJ

J

(261)


0

501

00

05

06

05

01

06

01

00

06

00 )()()(

zzzPPzPPzPPz

J

(262)

252 Singularidades

























dmrrI Ti

eslaboacuteni


Donde

푟 = [푥 푦 푧 1] (32)



I


2

2

2

(33)


dmzyI XX )( 22 (34)

dmzxIYY )( 22 (35)


dmyxIZZ )( 22 (36)




dmyzIYZ middot (39)


dmxxm middot (310)

dmyym middot (311)

dmzzm middot (312)


mzmymxm

zmIIIII

ymIIIII

xmIIIII

IZZYYXX

YZXZ

YZZZYYXX

XY

XZXYZZYYXX

2

2

2

(313)













Figura 28




휃




0ii r (315)


푇





푣 =푑푟푑푡





(318)


2)(21

iii vdmdK


(319)


(320)

El teacutermino iT

ii


(321)


(322)




ii




(324)

kj

i

j

Ti

kkki

Ti

ii

ii

ki


21

1

11

0

1

11

0

kj

i

j

Ti

kkki

Ti

ii

ii

ki


21

1

11

0

1

11

0

kj

i

j

Ti

kkki

i

ki


21

1

11

0

1

11

0

kj

n

i

i

j

i

k

Ti

kkkii


21

1 1 1

11

011

0

TZYX gggg ]0[



(325)






(327)


휏 =푑푑푡

middot퐿휃

minus퐿휃

(328)


(329)

Donde

n

ii

iii

n

ii rTgmPP

1

0

1middotmiddotmiddot

n

ii

iiikj

n

i

i

j

i

k

Ti

kkkii

jjj rTgmTQTITQTTrL

1

0

1 1 1

11

011


ikj

n

j

n

kijkj

n

jiji GhM




(330)


(331)


(332)



(333)

n

jip

Tpippjij dIdTrM

)max(middotmiddot

n

kjip

Tpip

p

pkijk dI

dd

Trh)max(

middotmiddot)(

pp

n

ipiPi rdgmG

1

middotmiddotmiddot

ijijTQT

d ij

jjij 0

middotmiddot 11

0


(334)




(335)






di

jjj

kkk

ik

kkj

jj

k

ij


)( 11

11

0

11

11

0

)()()middot( GHM


(336)



(337)



(338)



(339)



middotfriccioacuten



)()()()middot( FGHM


















)]()()()[(1 FGHM (344)



4 Control

























푡 =휔훼

(41)



푡 = 푡 (42)


∆휃 =12




푡 =∆휃휔

minus 푡 (44)


휃 =12



훼 = 훼 (47)




휔 = 휔 (49)

훼 = 0 (410)










42 Sensores











422 Resolvers



푒 = 푒 cos휃 (414)


푒 = 푒 sen휃 (415)















43 Actuadores












431 Servomotores













푃 = 푃 (419)






휔휔

=푟푟

(422)

432 Drivers










mecaacutenicos










































4421 Controlador PD













푒 =휏퐾푝

(432)








푑푑푡


푒 (434)


푑푑푡

푒푒 = 0 퐼















푠2 1 푀 middotKp



0

Tabla 4














휉 le 1 (446)





휔 =푘퐽

(447)


휔 lt휔2

(448)













훼 = 푀(휃) (451)

훽 = 퐻(휃휃 ) + 퐺(휃) + 퐹(휃휃 ) (452)



휃 = 휏 (453)




















푒 = 0 (461)







휀 = 푒 (462)


푑푑푡

휀푒푒


(463)





푑푑푡

휀푒푒


middot휀푒푒

(465)








풔 0 00 풔 00 0 풔


= 0 (470)


= 0 (471)





s3 1 Kp

s2 Kd Ki


0


퐾푑

0

Tabla 5



gt 0 (473)




퐾푑gt 0 (475)











444 Control Digital
















푎 = 휃 (480)

푎휃 푑

= 1 (481)



푎푒

= 퐾 (483)







푠 (485)



푠 cong2푇


(486)



푎푒





푎푒





middot 퐾푖 (489)



middot 퐾푖 (490)


middot 퐾푖 +푇2





푎 = 푎 + 퐾푝 +푇2














푦(푡) = [푋푌푍퐴퐵퐶] (496)


푒 =푒푒 (497)





푒 =12



휀 = 푒 푑푡 (4100)


푦 = 푣ω (4101)


푦 =푣ω (4102)





푦 = 푎α (4104)


푦 =푎α (4105)



























































(4126)


(4127)

)()middot()( GJG TX

(4128)

)()middot()( FJF TX

(4129)




























































휔 = 2 (51)

휉 = 1 (52)


퐾푝 = 휔 = 4 (53)


푑(푠) = 푠 + 4 middot 푠 + 4 (55)


푠 + 4 middot 푠 + 4 = 0 (56)


푑(푠) = (푠 + 2) middot (푠 + 2) (58)





σ = 20 (510)





퐾푑 = 24 (513)

퐾푝 = 84 (514)

퐾푖 = 80 (515)


24 middot 84 gt 80 (516)





푇 = 0001푠 (517)


퐾 =

⎣⎢⎢⎢⎢⎡24 0 0 0 0 00 24 0 0 0 00 0 24 0 0 00 0 0 24 0 00 0 0 0 24 00 0 0 0 0 24⎦

⎥⎥⎥⎥⎤

(518)

퐾 =

⎣⎢⎢⎢⎢⎡84 0 0 0 0 00 84 0 0 0 00 0 84 0 0 00 0 0 84 0 00 0 0 0 84 00 0 0 0 0 84⎦

⎥⎥⎥⎥⎤

(519)

퐾푖 =

⎣⎢⎢⎢⎢⎡80 0 0 0 0 00 80 0 0 0 00 0 80 0 0 00 0 0 80 0 00 0 0 0 80 00 0 0 0 0 80⎦

⎥⎥⎥⎥⎤

(520)







6 Simulacioacuten














th1 = 10ordm (61)

th2 = 15ordm (62)

th3 = 20ordm (63)

th4 = 5ordm (64)


th6 = 31ordm (66)


































퐾푑 = 20 (67)

퐾푝 = 100 (68)





퐾푑 = 200 (69)

퐾푝 = 10000 (610)














































퐾 =

⎣⎢⎢⎢⎢⎡33 0 0 0 0 00 33 0 0 0 00 0 33 0 0 00 0 0 33 0 00 0 0 0 33 00 0 0 0 0 33⎦

⎥⎥⎥⎥⎤

(611)


퐾 =

⎣⎢⎢⎢⎢⎡216 0 0 0 0 0

0 216 0 0 0 00 0 216 0 0 00 0 0 216 0 00 0 0 0 216 00 0 0 0 0 216⎦

⎥⎥⎥⎥⎤

(612)

퐾 =

⎣⎢⎢⎢⎢⎡400 0 0 0 0 0

0 400 0 0 0 00 0 400 0 0 00 0 0 400 0 00 0 0 0 400 00 0 0 0 0 400⎦

⎥⎥⎥⎥⎤

(613)


Figura 60





















9 Presupuesto




91 Hardware


kUTaTuCC Ti middot


Donde





Office 2010 2500 7 2 595 52083


Tabla 6


92 Software






100000 7 2 595 20825


Tabla 7






Tabla 8





Gasto Coste (euro)

Hardware 52083

Software 83300

Personal 929966




Total 1082395

IVA (21 ) 227303


Tabla 9
































8 PRESUPUESTO 131

81 Hardware 131

82 Software 132





MEMORIA



1 Introduccioacuten




















































141 Eslabones























142 Articulaciones












A3 (θ3) +60 a -184ordm 69 ordms




Tabla 1

143 Compensadores




















Documentacioacuten

























Figura 15




1 -pi2 0500 1045 θ1

2 0 -1300 0 θ 2

3 -pi2 -0055 0 θ 3

4 pi2 0 -1025 θ 4

5 -pi2 0 0 θ 5

6 0 0 0290 θ 6

Tabla 2




PTTTTP iPi

QP

RQ

iR




PTP iii

i middot11 (22)


TTTTT Pi

QP

RQ

iR


(23)



iZiZiXiXi

i dDRaDRT (24)


)()middot( 111

iiZiiXi



푇 =



(26)







푅 (훼훽 훾) = 푅 (훼)푅 (훽)푅 (훾) =


0 0 1middot




(27)


푅 (αβ γ) =


(28)




(29)






푐표푠훽) (212)









푇 푇 푇 푇 푇 푇 (213)




푇 = 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) (215)


(216)

(217)

1000010


1

1111

1111

01 d

aa

T

10000100


2222

2222

12

aa

T


(218)

(219)

(220)

(221)




10000010


3333

3333

23

aa

T

1000010

0cos0sin0sin0cos

4

44

44

34 dT


55

55

45

T

1000100

00cossin00sincos

6

66

66

56 dT










Figura 16


푙 = 푎 + 푙 (224)


푟 = 푃 + 푃 (225)








휃 =휋2



Figura 17





Figura 18






Figura 21


푟 = 푃 + 푃 (230)









휃 = 휋 minus 휑 + 휀 (233)





=0



(235)





Figura 22




Figura 23









Tabla 3






Figura 24



Figura 25






Figura 26










푦 = 푣휔 (246)



vn Jv 0 (247)

Jn

0 (248)



)(Jy (249)



JJ

J v (250)











n

iiinnn zkRkRk

1

01

01

0121


(253)





][ 01

01

00 nzzzJ (254)


푃 = 푣 (255)


i

n

i i

NN

vv

middot1

00

(256)


i

Nv

vJ

0

(257)



푣 = 푣 + 휔 times 푃 (258)







0

1

01

001 )(

i

iniv

zPPz

JJ

J

(261)


0

501

00

05

06

05

01

06

01

00

06

00 )()()(

zzzPPzPPzPPz

J

(262)

252 Singularidades

























dmrrI Ti

eslaboacuteni


Donde

푟 = [푥 푦 푧 1] (32)



I


2

2

2

(33)


dmzyI XX )( 22 (34)

dmzxIYY )( 22 (35)


dmyxIZZ )( 22 (36)




dmyzIYZ middot (39)


dmxxm middot (310)

dmyym middot (311)

dmzzm middot (312)


mzmymxm

zmIIIII

ymIIIII

xmIIIII

IZZYYXX

YZXZ

YZZZYYXX

XY

XZXYZZYYXX

2

2

2

(313)













Figura 28




휃




0ii r (315)


푇





푣 =푑푟푑푡





(318)


2)(21

iii vdmdK


(319)


(320)

El teacutermino iT

ii


(321)


(322)




ii




(324)

kj

i

j

Ti

kkki

Ti

ii

ii

ki


21

1

11

0

1

11

0

kj

i

j

Ti

kkki

Ti

ii

ii

ki


21

1

11

0

1

11

0

kj

i

j

Ti

kkki

i

ki


21

1

11

0

1

11

0

kj

n

i

i

j

i

k

Ti

kkkii


21

1 1 1

11

011

0

TZYX gggg ]0[



(325)






(327)


휏 =푑푑푡

middot퐿휃

minus퐿휃

(328)


(329)

Donde

n

ii

iii

n

ii rTgmPP

1

0

1middotmiddotmiddot

n

ii

iiikj

n

i

i

j

i

k

Ti

kkkii

jjj rTgmTQTITQTTrL

1

0

1 1 1

11

011


ikj

n

j

n

kijkj

n

jiji GhM




(330)


(331)


(332)



(333)

n

jip

Tpippjij dIdTrM

)max(middotmiddot

n

kjip

Tpip

p

pkijk dI

dd

Trh)max(

middotmiddot)(

pp

n

ipiPi rdgmG

1

middotmiddotmiddot

ijijTQT

d ij

jjij 0

middotmiddot 11

0


(334)




(335)






di

jjj

kkk

ik

kkj

jj

k

ij


)( 11

11

0

11

11

0

)()()middot( GHM


(336)



(337)



(338)



(339)



middotfriccioacuten



)()()()middot( FGHM


















)]()()()[(1 FGHM (344)



4 Control

























푡 =휔훼

(41)



푡 = 푡 (42)


∆휃 =12




푡 =∆휃휔

minus 푡 (44)


휃 =12



훼 = 훼 (47)




휔 = 휔 (49)

훼 = 0 (410)










42 Sensores











422 Resolvers



푒 = 푒 cos휃 (414)


푒 = 푒 sen휃 (415)















43 Actuadores












431 Servomotores













푃 = 푃 (419)






휔휔

=푟푟

(422)

432 Drivers










mecaacutenicos










































4421 Controlador PD













푒 =휏퐾푝

(432)








푑푑푡


푒 (434)


푑푑푡

푒푒 = 0 퐼















푠2 1 푀 middotKp



0

Tabla 4














휉 le 1 (446)





휔 =푘퐽

(447)


휔 lt휔2

(448)













훼 = 푀(휃) (451)

훽 = 퐻(휃휃 ) + 퐺(휃) + 퐹(휃휃 ) (452)



휃 = 휏 (453)




















푒 = 0 (461)







휀 = 푒 (462)


푑푑푡

휀푒푒


(463)





푑푑푡

휀푒푒


middot휀푒푒

(465)








풔 0 00 풔 00 0 풔


= 0 (470)


= 0 (471)





s3 1 Kp

s2 Kd Ki


0


퐾푑

0

Tabla 5



gt 0 (473)




퐾푑gt 0 (475)











444 Control Digital
















푎 = 휃 (480)

푎휃 푑

= 1 (481)



푎푒

= 퐾 (483)







푠 (485)



푠 cong2푇


(486)



푎푒





푎푒





middot 퐾푖 (489)



middot 퐾푖 (490)


middot 퐾푖 +푇2





푎 = 푎 + 퐾푝 +푇2














푦(푡) = [푋푌푍퐴퐵퐶] (496)


푒 =푒푒 (497)





푒 =12



휀 = 푒 푑푡 (4100)


푦 = 푣ω (4101)


푦 =푣ω (4102)





푦 = 푎α (4104)


푦 =푎α (4105)



























































(4126)


(4127)

)()middot()( GJG TX

(4128)

)()middot()( FJF TX

(4129)




























































휔 = 2 (51)

휉 = 1 (52)


퐾푝 = 휔 = 4 (53)


푑(푠) = 푠 + 4 middot 푠 + 4 (55)


푠 + 4 middot 푠 + 4 = 0 (56)


푑(푠) = (푠 + 2) middot (푠 + 2) (58)





σ = 20 (510)





퐾푑 = 24 (513)

퐾푝 = 84 (514)

퐾푖 = 80 (515)


24 middot 84 gt 80 (516)





푇 = 0001푠 (517)


퐾 =

⎣⎢⎢⎢⎢⎡24 0 0 0 0 00 24 0 0 0 00 0 24 0 0 00 0 0 24 0 00 0 0 0 24 00 0 0 0 0 24⎦

⎥⎥⎥⎥⎤

(518)

퐾 =

⎣⎢⎢⎢⎢⎡84 0 0 0 0 00 84 0 0 0 00 0 84 0 0 00 0 0 84 0 00 0 0 0 84 00 0 0 0 0 84⎦

⎥⎥⎥⎥⎤

(519)

퐾푖 =

⎣⎢⎢⎢⎢⎡80 0 0 0 0 00 80 0 0 0 00 0 80 0 0 00 0 0 80 0 00 0 0 0 80 00 0 0 0 0 80⎦

⎥⎥⎥⎥⎤

(520)







6 Simulacioacuten














th1 = 10ordm (61)

th2 = 15ordm (62)

th3 = 20ordm (63)

th4 = 5ordm (64)


th6 = 31ordm (66)


































퐾푑 = 20 (67)

퐾푝 = 100 (68)





퐾푑 = 200 (69)

퐾푝 = 10000 (610)














































퐾 =

⎣⎢⎢⎢⎢⎡33 0 0 0 0 00 33 0 0 0 00 0 33 0 0 00 0 0 33 0 00 0 0 0 33 00 0 0 0 0 33⎦

⎥⎥⎥⎥⎤

(611)


퐾 =

⎣⎢⎢⎢⎢⎡216 0 0 0 0 0

0 216 0 0 0 00 0 216 0 0 00 0 0 216 0 00 0 0 0 216 00 0 0 0 0 216⎦

⎥⎥⎥⎥⎤

(612)

퐾 =

⎣⎢⎢⎢⎢⎡400 0 0 0 0 0

0 400 0 0 0 00 0 400 0 0 00 0 0 400 0 00 0 0 0 400 00 0 0 0 0 400⎦

⎥⎥⎥⎥⎤

(613)


Figura 60





















9 Presupuesto




91 Hardware


kUTaTuCC Ti middot


Donde





Office 2010 2500 7 2 595 52083


Tabla 6


92 Software






100000 7 2 595 20825


Tabla 7






Tabla 8





Gasto Coste (euro)

Hardware 52083

Software 83300

Personal 929966




Total 1082395

IVA (21 ) 227303


Tabla 9






























MEMORIA



1 Introduccioacuten




















































141 Eslabones























142 Articulaciones












A3 (θ3) +60 a -184ordm 69 ordms




Tabla 1

143 Compensadores




















Documentacioacuten

























Figura 15




1 -pi2 0500 1045 θ1

2 0 -1300 0 θ 2

3 -pi2 -0055 0 θ 3

4 pi2 0 -1025 θ 4

5 -pi2 0 0 θ 5

6 0 0 0290 θ 6

Tabla 2




PTTTTP iPi

QP

RQ

iR




PTP iii

i middot11 (22)


TTTTT Pi

QP

RQ

iR


(23)



iZiZiXiXi

i dDRaDRT (24)


)()middot( 111

iiZiiXi



푇 =



(26)







푅 (훼훽 훾) = 푅 (훼)푅 (훽)푅 (훾) =


0 0 1middot




(27)


푅 (αβ γ) =


(28)




(29)






푐표푠훽) (212)









푇 푇 푇 푇 푇 푇 (213)




푇 = 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) (215)


(216)

(217)

1000010


1

1111

1111

01 d

aa

T

10000100


2222

2222

12

aa

T


(218)

(219)

(220)

(221)




10000010


3333

3333

23

aa

T

1000010

0cos0sin0sin0cos

4

44

44

34 dT


55

55

45

T

1000100

00cossin00sincos

6

66

66

56 dT










Figura 16


푙 = 푎 + 푙 (224)


푟 = 푃 + 푃 (225)








휃 =휋2



Figura 17





Figura 18






Figura 21


푟 = 푃 + 푃 (230)









휃 = 휋 minus 휑 + 휀 (233)





=0



(235)





Figura 22




Figura 23









Tabla 3






Figura 24



Figura 25






Figura 26










푦 = 푣휔 (246)



vn Jv 0 (247)

Jn

0 (248)



)(Jy (249)



JJ

J v (250)











n

iiinnn zkRkRk

1

01

01

0121


(253)





][ 01

01

00 nzzzJ (254)


푃 = 푣 (255)


i

n

i i

NN

vv

middot1

00

(256)


i

Nv

vJ

0

(257)



푣 = 푣 + 휔 times 푃 (258)







0

1

01

001 )(

i

iniv

zPPz

JJ

J

(261)


0

501

00

05

06

05

01

06

01

00

06

00 )()()(

zzzPPzPPzPPz

J

(262)

252 Singularidades

























dmrrI Ti

eslaboacuteni


Donde

푟 = [푥 푦 푧 1] (32)



I


2

2

2

(33)


dmzyI XX )( 22 (34)

dmzxIYY )( 22 (35)


dmyxIZZ )( 22 (36)




dmyzIYZ middot (39)


dmxxm middot (310)

dmyym middot (311)

dmzzm middot (312)


mzmymxm

zmIIIII

ymIIIII

xmIIIII

IZZYYXX

YZXZ

YZZZYYXX

XY

XZXYZZYYXX

2

2

2

(313)













Figura 28




휃




0ii r (315)


푇





푣 =푑푟푑푡





(318)


2)(21

iii vdmdK


(319)


(320)

El teacutermino iT

ii


(321)


(322)




ii




(324)

kj

i

j

Ti

kkki

Ti

ii

ii

ki


21

1

11

0

1

11

0

kj

i

j

Ti

kkki

Ti

ii

ii

ki


21

1

11

0

1

11

0

kj

i

j

Ti

kkki

i

ki


21

1

11

0

1

11

0

kj

n

i

i

j

i

k

Ti

kkkii


21

1 1 1

11

011

0

TZYX gggg ]0[



(325)






(327)


휏 =푑푑푡

middot퐿휃

minus퐿휃

(328)


(329)

Donde

n

ii

iii

n

ii rTgmPP

1

0

1middotmiddotmiddot

n

ii

iiikj

n

i

i

j

i

k

Ti

kkkii

jjj rTgmTQTITQTTrL

1

0

1 1 1

11

011


ikj

n

j

n

kijkj

n

jiji GhM




(330)


(331)


(332)



(333)

n

jip

Tpippjij dIdTrM

)max(middotmiddot

n

kjip

Tpip

p

pkijk dI

dd

Trh)max(

middotmiddot)(

pp

n

ipiPi rdgmG

1

middotmiddotmiddot

ijijTQT

d ij

jjij 0

middotmiddot 11

0


(334)




(335)






di

jjj

kkk

ik

kkj

jj

k

ij


)( 11

11

0

11

11

0

)()()middot( GHM


(336)



(337)



(338)



(339)



middotfriccioacuten



)()()()middot( FGHM


















)]()()()[(1 FGHM (344)



4 Control

























푡 =휔훼

(41)



푡 = 푡 (42)


∆휃 =12




푡 =∆휃휔

minus 푡 (44)


휃 =12



훼 = 훼 (47)




휔 = 휔 (49)

훼 = 0 (410)










42 Sensores











422 Resolvers



푒 = 푒 cos휃 (414)


푒 = 푒 sen휃 (415)















43 Actuadores












431 Servomotores













푃 = 푃 (419)






휔휔

=푟푟

(422)

432 Drivers










mecaacutenicos










































4421 Controlador PD













푒 =휏퐾푝

(432)








푑푑푡


푒 (434)


푑푑푡

푒푒 = 0 퐼















푠2 1 푀 middotKp



0

Tabla 4














휉 le 1 (446)





휔 =푘퐽

(447)


휔 lt휔2

(448)













훼 = 푀(휃) (451)

훽 = 퐻(휃휃 ) + 퐺(휃) + 퐹(휃휃 ) (452)



휃 = 휏 (453)




















푒 = 0 (461)







휀 = 푒 (462)


푑푑푡

휀푒푒


(463)





푑푑푡

휀푒푒


middot휀푒푒

(465)








풔 0 00 풔 00 0 풔


= 0 (470)


= 0 (471)





s3 1 Kp

s2 Kd Ki


0


퐾푑

0

Tabla 5



gt 0 (473)




퐾푑gt 0 (475)











444 Control Digital
















푎 = 휃 (480)

푎휃 푑

= 1 (481)



푎푒

= 퐾 (483)







푠 (485)



푠 cong2푇


(486)



푎푒





푎푒





middot 퐾푖 (489)



middot 퐾푖 (490)


middot 퐾푖 +푇2





푎 = 푎 + 퐾푝 +푇2














푦(푡) = [푋푌푍퐴퐵퐶] (496)


푒 =푒푒 (497)





푒 =12



휀 = 푒 푑푡 (4100)


푦 = 푣ω (4101)


푦 =푣ω (4102)





푦 = 푎α (4104)


푦 =푎α (4105)



























































(4126)


(4127)

)()middot()( GJG TX

(4128)

)()middot()( FJF TX

(4129)




























































휔 = 2 (51)

휉 = 1 (52)


퐾푝 = 휔 = 4 (53)


푑(푠) = 푠 + 4 middot 푠 + 4 (55)


푠 + 4 middot 푠 + 4 = 0 (56)


푑(푠) = (푠 + 2) middot (푠 + 2) (58)





σ = 20 (510)





퐾푑 = 24 (513)

퐾푝 = 84 (514)

퐾푖 = 80 (515)


24 middot 84 gt 80 (516)





푇 = 0001푠 (517)


퐾 =

⎣⎢⎢⎢⎢⎡24 0 0 0 0 00 24 0 0 0 00 0 24 0 0 00 0 0 24 0 00 0 0 0 24 00 0 0 0 0 24⎦

⎥⎥⎥⎥⎤

(518)

퐾 =

⎣⎢⎢⎢⎢⎡84 0 0 0 0 00 84 0 0 0 00 0 84 0 0 00 0 0 84 0 00 0 0 0 84 00 0 0 0 0 84⎦

⎥⎥⎥⎥⎤

(519)

퐾푖 =

⎣⎢⎢⎢⎢⎡80 0 0 0 0 00 80 0 0 0 00 0 80 0 0 00 0 0 80 0 00 0 0 0 80 00 0 0 0 0 80⎦

⎥⎥⎥⎥⎤

(520)







6 Simulacioacuten














th1 = 10ordm (61)

th2 = 15ordm (62)

th3 = 20ordm (63)

th4 = 5ordm (64)


th6 = 31ordm (66)


































퐾푑 = 20 (67)

퐾푝 = 100 (68)





퐾푑 = 200 (69)

퐾푝 = 10000 (610)














































퐾 =

⎣⎢⎢⎢⎢⎡33 0 0 0 0 00 33 0 0 0 00 0 33 0 0 00 0 0 33 0 00 0 0 0 33 00 0 0 0 0 33⎦

⎥⎥⎥⎥⎤

(611)


퐾 =

⎣⎢⎢⎢⎢⎡216 0 0 0 0 0

0 216 0 0 0 00 0 216 0 0 00 0 0 216 0 00 0 0 0 216 00 0 0 0 0 216⎦

⎥⎥⎥⎥⎤

(612)

퐾 =

⎣⎢⎢⎢⎢⎡400 0 0 0 0 0

0 400 0 0 0 00 0 400 0 0 00 0 0 400 0 00 0 0 0 400 00 0 0 0 0 400⎦

⎥⎥⎥⎥⎤

(613)


Figura 60





















9 Presupuesto




91 Hardware


kUTaTuCC Ti middot


Donde





Office 2010 2500 7 2 595 52083


Tabla 6


92 Software






100000 7 2 595 20825


Tabla 7






Tabla 8





Gasto Coste (euro)

Hardware 52083

Software 83300

Personal 929966




Total 1082395

IVA (21 ) 227303


Tabla 9































1 Introduccioacuten




















































141 Eslabones























142 Articulaciones












A3 (θ3) +60 a -184ordm 69 ordms




Tabla 1

143 Compensadores




















Documentacioacuten

























Figura 15




1 -pi2 0500 1045 θ1

2 0 -1300 0 θ 2

3 -pi2 -0055 0 θ 3

4 pi2 0 -1025 θ 4

5 -pi2 0 0 θ 5

6 0 0 0290 θ 6

Tabla 2




PTTTTP iPi

QP

RQ

iR




PTP iii

i middot11 (22)


TTTTT Pi

QP

RQ

iR


(23)



iZiZiXiXi

i dDRaDRT (24)


)()middot( 111

iiZiiXi



푇 =



(26)







푅 (훼훽 훾) = 푅 (훼)푅 (훽)푅 (훾) =


0 0 1middot




(27)


푅 (αβ γ) =


(28)




(29)






푐표푠훽) (212)









푇 푇 푇 푇 푇 푇 (213)




푇 = 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) (215)


(216)

(217)

1000010


1

1111

1111

01 d

aa

T

10000100


2222

2222

12

aa

T


(218)

(219)

(220)

(221)




10000010


3333

3333

23

aa

T

1000010

0cos0sin0sin0cos

4

44

44

34 dT


55

55

45

T

1000100

00cossin00sincos

6

66

66

56 dT










Figura 16


푙 = 푎 + 푙 (224)


푟 = 푃 + 푃 (225)








휃 =휋2



Figura 17





Figura 18






Figura 21


푟 = 푃 + 푃 (230)









휃 = 휋 minus 휑 + 휀 (233)





=0



(235)





Figura 22




Figura 23









Tabla 3






Figura 24



Figura 25






Figura 26










푦 = 푣휔 (246)



vn Jv 0 (247)

Jn

0 (248)



)(Jy (249)



JJ

J v (250)











n

iiinnn zkRkRk

1

01

01

0121


(253)





][ 01

01

00 nzzzJ (254)


푃 = 푣 (255)


i

n

i i

NN

vv

middot1

00

(256)


i

Nv

vJ

0

(257)



푣 = 푣 + 휔 times 푃 (258)







0

1

01

001 )(

i

iniv

zPPz

JJ

J

(261)


0

501

00

05

06

05

01

06

01

00

06

00 )()()(

zzzPPzPPzPPz

J

(262)

252 Singularidades

























dmrrI Ti

eslaboacuteni


Donde

푟 = [푥 푦 푧 1] (32)



I


2

2

2

(33)


dmzyI XX )( 22 (34)

dmzxIYY )( 22 (35)


dmyxIZZ )( 22 (36)




dmyzIYZ middot (39)


dmxxm middot (310)

dmyym middot (311)

dmzzm middot (312)


mzmymxm

zmIIIII

ymIIIII

xmIIIII

IZZYYXX

YZXZ

YZZZYYXX

XY

XZXYZZYYXX

2

2

2

(313)













Figura 28




휃




0ii r (315)


푇





푣 =푑푟푑푡





(318)


2)(21

iii vdmdK


(319)


(320)

El teacutermino iT

ii


(321)


(322)




ii




(324)

kj

i

j

Ti

kkki

Ti

ii

ii

ki


21

1

11

0

1

11

0

kj

i

j

Ti

kkki

Ti

ii

ii

ki


21

1

11

0

1

11

0

kj

i

j

Ti

kkki

i

ki


21

1

11

0

1

11

0

kj

n

i

i

j

i

k

Ti

kkkii


21

1 1 1

11

011

0

TZYX gggg ]0[



(325)






(327)


휏 =푑푑푡

middot퐿휃

minus퐿휃

(328)


(329)

Donde

n

ii

iii

n

ii rTgmPP

1

0

1middotmiddotmiddot

n

ii

iiikj

n

i

i

j

i

k

Ti

kkkii

jjj rTgmTQTITQTTrL

1

0

1 1 1

11

011


ikj

n

j

n

kijkj

n

jiji GhM




(330)


(331)


(332)



(333)

n

jip

Tpippjij dIdTrM

)max(middotmiddot

n

kjip

Tpip

p

pkijk dI

dd

Trh)max(

middotmiddot)(

pp

n

ipiPi rdgmG

1

middotmiddotmiddot

ijijTQT

d ij

jjij 0

middotmiddot 11

0


(334)




(335)






di

jjj

kkk

ik

kkj

jj

k

ij


)( 11

11

0

11

11

0

)()()middot( GHM


(336)



(337)



(338)



(339)



middotfriccioacuten



)()()()middot( FGHM


















)]()()()[(1 FGHM (344)



4 Control

























푡 =휔훼

(41)



푡 = 푡 (42)


∆휃 =12




푡 =∆휃휔

minus 푡 (44)


휃 =12



훼 = 훼 (47)




휔 = 휔 (49)

훼 = 0 (410)










42 Sensores











422 Resolvers



푒 = 푒 cos휃 (414)


푒 = 푒 sen휃 (415)















43 Actuadores












431 Servomotores













푃 = 푃 (419)






휔휔

=푟푟

(422)

432 Drivers










mecaacutenicos










































4421 Controlador PD













푒 =휏퐾푝

(432)








푑푑푡


푒 (434)


푑푑푡

푒푒 = 0 퐼















푠2 1 푀 middotKp



0

Tabla 4














휉 le 1 (446)





휔 =푘퐽

(447)


휔 lt휔2

(448)













훼 = 푀(휃) (451)

훽 = 퐻(휃휃 ) + 퐺(휃) + 퐹(휃휃 ) (452)



휃 = 휏 (453)




















푒 = 0 (461)







휀 = 푒 (462)


푑푑푡

휀푒푒


(463)





푑푑푡

휀푒푒


middot휀푒푒

(465)








풔 0 00 풔 00 0 풔


= 0 (470)


= 0 (471)





s3 1 Kp

s2 Kd Ki


0


퐾푑

0

Tabla 5



gt 0 (473)




퐾푑gt 0 (475)











444 Control Digital
















푎 = 휃 (480)

푎휃 푑

= 1 (481)



푎푒

= 퐾 (483)







푠 (485)



푠 cong2푇


(486)



푎푒





푎푒





middot 퐾푖 (489)



middot 퐾푖 (490)


middot 퐾푖 +푇2





푎 = 푎 + 퐾푝 +푇2














푦(푡) = [푋푌푍퐴퐵퐶] (496)


푒 =푒푒 (497)





푒 =12



휀 = 푒 푑푡 (4100)


푦 = 푣ω (4101)


푦 =푣ω (4102)





푦 = 푎α (4104)


푦 =푎α (4105)



























































(4126)


(4127)

)()middot()( GJG TX

(4128)

)()middot()( FJF TX

(4129)




























































휔 = 2 (51)

휉 = 1 (52)


퐾푝 = 휔 = 4 (53)


푑(푠) = 푠 + 4 middot 푠 + 4 (55)


푠 + 4 middot 푠 + 4 = 0 (56)


푑(푠) = (푠 + 2) middot (푠 + 2) (58)





σ = 20 (510)





퐾푑 = 24 (513)

퐾푝 = 84 (514)

퐾푖 = 80 (515)


24 middot 84 gt 80 (516)





푇 = 0001푠 (517)


퐾 =

⎣⎢⎢⎢⎢⎡24 0 0 0 0 00 24 0 0 0 00 0 24 0 0 00 0 0 24 0 00 0 0 0 24 00 0 0 0 0 24⎦

⎥⎥⎥⎥⎤

(518)

퐾 =

⎣⎢⎢⎢⎢⎡84 0 0 0 0 00 84 0 0 0 00 0 84 0 0 00 0 0 84 0 00 0 0 0 84 00 0 0 0 0 84⎦

⎥⎥⎥⎥⎤

(519)

퐾푖 =

⎣⎢⎢⎢⎢⎡80 0 0 0 0 00 80 0 0 0 00 0 80 0 0 00 0 0 80 0 00 0 0 0 80 00 0 0 0 0 80⎦

⎥⎥⎥⎥⎤

(520)







6 Simulacioacuten














th1 = 10ordm (61)

th2 = 15ordm (62)

th3 = 20ordm (63)

th4 = 5ordm (64)


th6 = 31ordm (66)


































퐾푑 = 20 (67)

퐾푝 = 100 (68)





퐾푑 = 200 (69)

퐾푝 = 10000 (610)














































퐾 =

⎣⎢⎢⎢⎢⎡33 0 0 0 0 00 33 0 0 0 00 0 33 0 0 00 0 0 33 0 00 0 0 0 33 00 0 0 0 0 33⎦

⎥⎥⎥⎥⎤

(611)


퐾 =

⎣⎢⎢⎢⎢⎡216 0 0 0 0 0

0 216 0 0 0 00 0 216 0 0 00 0 0 216 0 00 0 0 0 216 00 0 0 0 0 216⎦

⎥⎥⎥⎥⎤

(612)

퐾 =

⎣⎢⎢⎢⎢⎡400 0 0 0 0 0

0 400 0 0 0 00 0 400 0 0 00 0 0 400 0 00 0 0 0 400 00 0 0 0 0 400⎦

⎥⎥⎥⎥⎤

(613)


Figura 60





















9 Presupuesto




91 Hardware


kUTaTuCC Ti middot


Donde





Office 2010 2500 7 2 595 52083


Tabla 6


92 Software






100000 7 2 595 20825


Tabla 7






Tabla 8





Gasto Coste (euro)

Hardware 52083

Software 83300

Personal 929966




Total 1082395

IVA (21 ) 227303


Tabla 9









































































141 Eslabones























142 Articulaciones












A3 (θ3) +60 a -184ordm 69 ordms




Tabla 1

143 Compensadores




















Documentacioacuten

























Figura 15




1 -pi2 0500 1045 θ1

2 0 -1300 0 θ 2

3 -pi2 -0055 0 θ 3

4 pi2 0 -1025 θ 4

5 -pi2 0 0 θ 5

6 0 0 0290 θ 6

Tabla 2




PTTTTP iPi

QP

RQ

iR




PTP iii

i middot11 (22)


TTTTT Pi

QP

RQ

iR


(23)



iZiZiXiXi

i dDRaDRT (24)


)()middot( 111

iiZiiXi



푇 =



(26)







푅 (훼훽 훾) = 푅 (훼)푅 (훽)푅 (훾) =


0 0 1middot




(27)


푅 (αβ γ) =


(28)




(29)






푐표푠훽) (212)









푇 푇 푇 푇 푇 푇 (213)




푇 = 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) 푇(휃 ) (215)


(216)

(217)

1000010


1

1111

1111

01 d

aa

T

10000100


2222

2222

12

aa

T


(218)

(219)

(220)

(221)




10000010


3333

3333

23

aa

T

1000010

0cos0sin0sin0cos

4

44

44

34 dT


55

55

45

T

1000100

00cossin00sincos

6

66

66

56 dT










Figura 16


푙 = 푎 + 푙 (224)


푟 = 푃 + 푃 (225)








휃 =휋2



Figura 17





Figura 18






Figura 21


푟 = 푃 + 푃 (230)









휃 = 휋 minus 휑 + 휀 (233)





=0



(235)





Figura 22




Figura 23









Tabla 3






Figura 24



Figura 25






Figura 26










푦 = 푣휔 (246)



vn Jv 0 (247)

Jn

0 (248)



)(Jy (249)



JJ

J v (250)











n

iiinnn zkRkRk

1

01

01

0121


(253)





][ 01

01

00 nzzzJ (254)


푃 = 푣 (255)


i

n

i i

NN

vv

middot1

00

(256)


i

Nv

vJ

0

(257)



푣 = 푣 + 휔 times 푃 (258)







0

1

01

001 )(

i

iniv

zPPz

JJ

J

(261)


0

501

00

05

06

05

01

06

01

00

06

00 )()()(

zzzPPzPPzPPz

J

(262)

252 Singularidades

























dmrrI Ti

eslaboacuteni


Donde

푟 = [푥 푦 푧 1] (32)



I


2

2

2

(33)


dmzyI XX )( 22 (34)

dmzxIYY )( 22 (35)


dmyxIZZ )( 22 (36)




dmyzIYZ middot (39)


dmxxm middot (310)

dmyym middot (311)

dmzzm middot (312)


mzmymxm

zmIIIII

ymIIIII

xmIIIII

IZZYYXX

YZXZ

YZZZYYXX

XY

XZXYZZYYXX

2

2

2

(313)













Figura 28




휃




0ii r (315)


푇





푣 =푑푟푑푡





(318)


2)(21

iii vdmdK


(319)


(320)

El teacutermino iT

ii


(321)


(322)




ii




(324)

kj

i

j

Ti

kkki

Ti

ii

ii

ki


21

1

11

0

1

11

0

kj

i

j

Ti

kkki

Ti

ii

ii

ki


21

1

11

0

1

11

0

kj

i

j

Ti

kkki

i

ki


21

1

11

0

1

11

0

kj

n

i

i

j

i

k

Ti

kkkii


21

1 1 1

11

011

0

TZYX gggg ]0[



(325)






(327)


휏 =푑푑푡

middot퐿휃

minus퐿휃

(328)


(329)

Donde

n

ii

iii

n

ii rTgmPP

1

0

1middotmiddotmiddot

n

ii

iiikj

n

i

i

j

i

k

Ti

kkkii

jjj rTgmTQTITQTTrL

1

0

1 1 1

11

011


ikj

n

j

n

kijkj

n

jiji GhM




(330)


(331)


(332)



(333)

n

jip

Tpippjij dIdTrM

)max(middotmiddot

n

kjip

Tpip

p

pkijk dI

dd

Trh)max(

middotmiddot)(

pp

n

ipiPi rdgmG

1

middotmiddotmiddot

ijijTQT

d ij

jjij 0

middotmiddot 11

0


(334)




(335)






di

jjj

kkk

ik

kkj

jj

k

ij


)( 11

11

0

11

11

0

)()()middot( GHM


(336)



(337)



(338)



(339)



middotfriccioacuten



)()()()middot( FGHM


















)]()()()[(1 FGHM (344)



4 Control

























푡 =휔훼

(41)



푡 = 푡 (42)


∆휃 =12




푡 =∆휃휔

minus 푡 (44)


휃 =12



훼 = 훼 (47)




휔 = 휔 (49)

훼 = 0 (410)










42 Sensores











422 Resolvers



푒 = 푒 cos휃 (414)


푒 = 푒 sen휃 (415)















43 Actuadores












431 Servomotores













푃 = 푃 (419)






휔휔

=푟푟

(422)

432 Drivers










mecaacutenicos










































4421 Controlador PD













푒 =휏퐾푝

(432)








푑푑푡


푒 (434)


푑푑푡

푒푒 = 0 퐼















푠2 1 푀 middotKp



0

Tabla 4














휉 le 1 (446)





휔 =푘퐽

(447)


휔 lt휔2

(448)













훼 = 푀(휃) (451)

훽 = 퐻(휃휃 ) + 퐺(휃) + 퐹(휃휃 ) (452)



휃 = 휏 (453)




















푒 = 0 (461)







휀 = 푒 (462)


푑푑푡

휀푒푒


(463)





푑푑푡

휀푒푒


middot휀푒푒

(465)








풔 0 00 풔 00 0 풔


= 0 (470)


= 0 (471)





s3 1 Kp

s2 Kd Ki


0


퐾푑

0

Tabla 5



gt 0 (473)




퐾푑gt 0 (475)











444 Control Digital
















푎 = 휃 (480)

푎휃 푑

= 1 (481)



푎푒

= 퐾 (483)







푠 (485)



푠 cong2푇


(486)



푎푒





푎푒





middot 퐾푖 (489)



middot 퐾푖 (490)


middot 퐾푖 +푇2





푎 = 푎 + 퐾푝 +푇2














푦(푡) = [푋푌푍퐴퐵퐶] (496)


푒 =푒푒 (497)





푒 =12



휀 = 푒 푑푡 (4100)


푦 = 푣ω (4101)


푦 =푣ω (4102)





푦 = 푎α (4104)


푦 =푎α (4105)



























































(4126)


(4127)

)()middot()( GJG TX

(4128)

)()middot()( FJF TX

(4129)




























































휔 = 2 (51)

휉 = 1 (52)


퐾푝 = 휔 = 4 (53)


푑(푠) = 푠 + 4 middot 푠 + 4 (55)


푠 + 4 middot 푠 + 4 = 0 (56)


푑(푠) = (푠 + 2) middot (푠 + 2) (58)





σ = 20 (510)





퐾푑 = 24 (513)

퐾푝 = 84 (514)

퐾푖 = 80 (515)


24 middot 84 gt 80 (516)





푇 = 0001푠 (517)


퐾 =

⎣⎢⎢⎢⎢⎡24 0 0 0 0 00 24 0 0 0 00 0 24 0 0 00 0 0 24 0 00 0 0 0 24 00 0 0 0 0 24⎦

⎥⎥⎥⎥⎤

(518)

퐾 =

⎣⎢⎢⎢⎢⎡84 0 0 0 0 00 84 0 0 0 00 0 84 0 0 00 0 0 84 0 00 0 0 0 84 00 0 0 0 0 84⎦

⎥⎥⎥⎥⎤

(519)

퐾푖 =

⎣⎢⎢⎢⎢⎡80 0 0 0 0 00 80 0 0 0 00 0 80 0 0 00 0 0 80 0 00 0 0 0 80 00 0 0 0 0 80⎦

⎥⎥⎥⎥⎤

(520)







6 Simulacioacuten














th1 = 10ordm (61)

th2 = 15ordm (62)

th3 = 20ordm (63)

th4 = 5ordm (64)


th6 = 31ordm (66)


































퐾푑 = 20 (67)

퐾푝 = 100 (68)





퐾푑 = 200 (69)

퐾푝 = 10000 (610)














































퐾 =

⎣⎢⎢⎢⎢⎡33 0 0 0 0 00 33 0 0 0 00 0 33 0 0 00 0 0 33 0 00 0 0 0 33 00 0 0 0 0 33⎦

⎥⎥⎥⎥⎤

(611)


퐾 =

⎣⎢⎢⎢⎢⎡216 0 0 0 0 0

0 216 0 0 0 00 0 216 0 0 00 0 0 216 0 00 0 0 0 216 00 0 0 0 0 216⎦

⎥⎥⎥⎥⎤

(612)

퐾 =

⎣⎢⎢⎢⎢⎡400 0 0 0 0 0

0 400 0 0 0 00 0 400 0 0 00 0 0 400 0 00 0 0 0 400 00 0 0 0 0 400⎦

⎥⎥⎥⎥⎤

(613)


Figura 60





















9 Presupuesto




91 Hardware


kUTaTuCC Ti middot


Donde





Office 2010 2500 7 2 595 52083


Tabla 6


92 Software






100000 7 2 595 20825


Tabla 7






Tabla 8





Gasto Coste (euro)

Hardware 52083

Software 83300

Personal 929966




Total 1082395

IVA (21 ) 227303


Tabla 9





























Desarrollo de sistema de control para un manipulador de ...

Documents

Transcript of Desarrollo de sistema de control para un manipulador de ...