Departamento de Ingenier a de Sistemas y Automatica Escuela Superior de Ingenieros Universidad de Sevilla

Simulacion y control de un robot manipulador.
Autor: Carlos Perez Fernandez. Tutor: Francisco Rodr guez Rubio Sevilla, Noviembre de 1999.

2

Indice General
1 Introduccion
1.1 Contenido y objetivos del proyecto . . . . . . . . . 1.2 Introduccion a la Robotica Industrial . . . . . . . . 1.2.1 Evolucion historica de la robotica industrial 1.2.2 Clasi cacion de los robots industriales . . . 1.2.3 Estructura de un robot industrial . . . . . . 1.3 Descripcion del robot industrial RM-10 . . . . . . . 1.3.1 Brazo manipulador . . . . . . . . . . . . . . 1.3.2 Armario de control . . . . . . . . . . . . . . 1.4 Equipo para el desarrollo del proyecto . . . . . . . . 1.4.1 Equipo hardware . . . . . . . . . . . . . . . 1.4.2 Software de desarrollo . . . . . . . . . . . . 1.4.3 Interface con armario de control . . . . . . . 2.1 2.2 2.3 2.4 Introduccion . . . . . . . . . . . . . . . . . . . . . . Matrices de transformacion . . . . . . . . . . . . . . Problema cinematico directo . . . . . . . . . . . . . Problema cinematico inverso . . . . . . . . . . . . . 2.4.1 Resolucion anal tica de la cinematica inversa 2.4.2 Implementacion software . . . . . . . . . . . 2.5 Generacion de trayectorias . . . . . . . . . . . . . . 2.5.1 Trayectorias articulares . . . . . . . . . . . . 2.5.2 Trayectorias lineales . . . . . . . . . . . . . 3.1 Introduccion . . . . . . . . . . . . . . . . . . . . . 3.2 Modelo Euler-Lagrange . . . . . . . . . . . . . . . 3.3 Parametros del modelo del robot . . . . . . . . . 3.3.1 Tensores de inercia y centros de gravedad . 3.3.2 Caracter sticas de los motores y reductoras 3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 7 7 8 9 13 14 15 20 20 21 22

7

2 Cinematica del robot RM-10

23

23 24 27 30 30 36 36 36 38 41 42 50 51 51

3 Dinamica del robot RM-10

41

99 . . . . . .3 Parametros de friccion . . . . . . . . . . INDICE GENERAL . . . . . . . . . . . . . . . . . . . 4.2 Control lineal . .2. . . . .1 Introduccion . . . . . . . . . . . n Esquemas electricos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .3. .3 B.5 B. . . . .1 Justi cacion . . . . . . . . . . . . . . . .2. . . .3 Control tipo PID . . . . . . . . . . . . . . . . . . . . . . . . . . .6 Simulaciones del modelo . .2. . . . . . . . . 63 63 63 63 64 75 76 Apendices A Tarjeta de E/S digital B Tarjeta de conversion resolver-encoder B. . . . . . . . . . . . . . . . . . . . . . . . . . . 3. . . . . . . . . . . 4. . Conversion resolver a encoder . . . 4. . 3. . . . . . . . . . . . Ampli cador adaptador . . . . . . . . . . . . . 91 .4 Minimizacion de los parametros 3. . . . . . . . .2 B. . . . . . . .2 Control tipo PD . . . 96 . . . . Optoaislacion de se~ales digitales n Se~ales de control . 54 55 59 59 4 Control de robot RM-10 4. . . . . . . . . . . . . . . . . . . . . . . .4 B. . . . . . . .1 B. . .5 Implementacion informatica . 100 . . .4 3. . .3 Control por Par Calculado . . . . . . . . . .6 Filtros y Ajuste de fase . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99 . . . . . . . . . . . . . . . . 100 . . 4. . . . . . . . 101 91 95 C Esquemas electricos de electronica de potencia D Codigo fuente funciones Simulink 103 105 . . . 4. . . .

9 Evolucion de las posiciones de las articulaciones 5 y 6 . . . . Situacion de los sistemas de coordenadas . . . . . .3 Seguimiento en posicion para control tipo PD . . . Vectores de orientacion en la herramienta del manipulador Soluciones para las tres ultimas articulaciones . . . .8 Diagrama de bloques para la simulacion del modelo en bucle abierto . . . . . . . . . . . . . . . . . . . . .5 3. .7 Convenio Denavit-Hartenberg . 3. . . .1 3. . .Indice de Figuras 1. . Trayectorias articulares polinomicas de 5 orden . .2 Diagrama Simulink para la simulacion de un controlador PD . . . . . . . . . . . . Diagrama de bloques asociado al modelo . 2. . . . . . . 4. . . . . . . . . . . . . . . . . . 1. . . . . 66 4. . . .2 2. . . . . . . . 3. . . . . .2 Brazo mecanico del robot industrial RM-10 con indicacion de sus movimientos posibles . .2 3. . . . . . . 1. .1 2. . . . . . . . . . . . .6 Representacion esquematica de un servoampli cador . Generacion de trayectorias lineales. 64 4. . . 3. . .3 Control en un robot industrial . . . . . . . . . . . . .7 Esquema de interface con el robot RM-10 .4 3. . . . . . .10 Evolucion de las velocidades de las articulaciones 5 y 6 . . . . . . . . . . . . 10 11 12 14 15 19 22 24 25 28 35 37 38 52 54 55 56 56 60 61 61 62 62 Caracter sticas de los elementos del brazo del manipulador . . . . . . . 1.1 Estructura general de un robot industrial . . . . Caracter stica de friccion experimental del eje 5. . . . Diagrama de bloques Simulink para la simulacion del manipulador .4 2. . . . Caracter stica estatica de friccion teorica . . . . . . . . . Caracter stica de friccion experimental del eje 4. . Caracter stica de friccion experimental del eje 6. . . . . . . . . . . . . . . . . . . . . . . .5 2. . 1. . . . . . . .3 3. . . . . . . . . . . . . . . . . . .1 Esquema de control lineal tipo PD . . . . . . . . .3 2. . . . . 67 5 . . .6 3. 1. . . . . . . . . . . . . . . .5 Se~ales proporcionadas por los resolvers . . . . . . . . . . . . . .4 Vista general del robot industrial RM-10 . n 1. . . .6 3. . . . . . . . . . . . . . . . . . . .

. . . . 5 y 6 . . . . . . . . . . . . . . . . . . . . . . . 4. . . . . . . . . . . . 4. . . . . . . n 4.22 Error de seguimiento en posicion para control tipo par calculado en los ejes 4. . . . . . . 4. 5 y 6 4. . . .14 Bucle de linealizacion y desacoplo . . .11 Seguimiento en posicion para control tipo PID . . . . . . . . . . .15 Esquema completo de control por par calculado . . . . . . 4. . . . . . .21 Velocidades para control tipo par calculado en los ejes 4. 5 y 6 . . .4 Funcionamiento interno del circuito integrado AD2S90 . 4. . . . . . . . . . .16 Diagrama Simulink para la simulacion de un controlador Par Calculado . . 5 y 6 . . . .23 Se~al de control para control tipo par calculado en los ejes 4. . . . . . . . . . . .3 Esquema del ltro activo para la se~al de referencia . . . . . 4. . .13 Se~ales de control para regulador tipo PID . . . . . . . . . 4. .1 Etapa de entrada digital de la tarjeta E/S digitales . . . . . . . . A. . 5 y 6 .17 Seguimiento en posicion para control tipo Par Calculado . . . . . . . . . . . . . . . . . .2 Etapa de salida digital de la tarjeta E/S digitales . . . . . . . . .6 INDICE DE FIGURAS 4.10 Diagrama Simulink para la simulacion de un controlador PID 4. . . . . 5 y 6 . . . . . . . . . . . . . . . . . . . . . . B. . . . . 5 y6 .1 Diagrama de bloques de la tarjeta CRE .20 Seguimiento en posicion para control tipo par calculado en los ejes 4. B. .12 Error de seguimiento en posicion para control tipo PID . . . . . . .8 Error de seguimiento en posicion para control tipo PD en los ejes 4. . . . . . . . . . . . . . . . . . . 68 69 71 72 73 74 76 77 78 79 80 81 82 83 84 85 87 88 89 90 91 92 95 96 98 99 . n B. . . . . 4. . . . . n 4. .6 Seguimiento en posicion para control tipo PD en los ejes 4. . . 4. . . . . . . . . . B. .19 Se~ales de control para regulador tipo Par Calculado . .4 Error de seguimiento en posicion para control tipo PD .2 Conectores presentes en la tarjeta CRE . . . . . . . . . . . . . . . . . . . . . . . n 5y6 . . . .5 Se~ales de control para regulador tipo PD . n 4.9 Se~al de control para control tipo PD en los ejes 4. 4. n 4. . . . . . . 4. . .7 Velocidades para control tipo PD en los ejes 4. . . . A. . . . . .18 Error de seguimiento en posicion para control tipo Par Calculado 4. . . . . . . . 4. . . . .

2. ubicado en los laboratorios del Departamento de Ingenier a de Sistemas y Automatica de la Universidad de Sevilla.1 Contenido y objetivos del proyecto En el presente proyecto se trata de sustituir el controlador del robot industrial System-Robot RM-10. n 1.1 Evolucion historica de la robotica industrial La necesidad de aumentar la productividad y mejorar la calidad de los productos. 7 . dominante en las primeras decadas del siglo XX. conservando unicamente la electronica de potencia del manipulador y realizando la interfase electrica con las se~ales del manipulador. ha hecho insu ciente la automatizacion industrial r gida. En la actualidad. que estaba destinada a la fabricacion de grandes cantidades de una gama peque~a de productos. n mas de la mitad de los productos que se fabrican corresponden a lotes de pocas unidades. por un sistema de control basado en un ordenador PC an trion con una tarjeta controladora con multiples entrada-salida.2 Introduccion a la Robotica Industrial 1. Este sistema pretende sustituir los algoritmos de control de los motores y la generacion de trayectorias del manipulador.Cap tulo 1 Introduccion 1.

que a su vez son: Manuales. instalando el primero en 1961. En la historia de la Robotica Industrial se pueden distinguir varias etapas segun el nivel de desarrollo: 1. se patento en 1956 un manipulador programable que fue la semilla para la robotica industrial. A partir de 1980 los avances en informatica y el perfecionamiento de los sensores permiten integrar cada vez mas al robot en el entorno que le rodea. Tres a~os desn pues se inicia la implantacion de robots industriales en Europa. En 1950 se dise~an manipuladores amo-esclavo para manejar materian les radiactivos. A principios de los 60 Unimation realiza los primeros proyectos de robots industriales. pero con elementos que permiten modi car la secuencia. Desde este a~o hasta 1980 gracias a los avances en microelecn tronica y al auge de la industria automovil stica se produce un aumento notable en el parque de robots. como los siguientes: 1.2. Secuenciales. fundamentalmente en el sector automovil stico. En 1967 instala un conjunto de robots en una factor a de General Motors. 3. Manipuladores. con el objetivo de dise~ar una maquina exible.2 Clasi cacion de los robots industriales La evolucion a dado origen a una serie de tipos de robots. En 1970. 5. Secuenciales. dirigidos por un operario. naciendo as los robots inteligentes. adaptable al n entorno de trabajo. los laboratorios de la Universidad de Stanford y del MIT acometen la tarea de controlar un robot mediante un computador. 1. pero que repiten siempre lo mismo. son sistemas mecanicos en los que se puede gobernar los movimientos. . 2. 4.8 1 Introduccion Por esto. capaces de tomar decisiones adecuadas a cada situacion. En el a~o 1975 con la aplicacion de los microprocesadores se transforn man las caracter sticas de los robots pasando a ser mas compactos y baratos.

2. denominandose estas enlaces y articulaciones que permiten el movimiento relativo entre ellas. diferenciandose en que poseen una mayor interaccion con el entorno que lo rodea. aunque estos ultimos no siempre se encuentran presentes. alimentadores.1. desde simples entradas y salidas digitales hasta buses de campo. Normalmente esta dividido en partes r gidas. 5. tal y como se ve en la gura 1. con los que normalmente es necesario coordinarse para realizar alguna tarea por lo que los robots industriales suelen incorporar algun tipo de sistema de comunicacion. en general nos podemos encontrar las siguientes partes: 1. mediante el uso de sensores avanzados. Robot programables textualmente: En este tipo de robots. 4.1. Sensores de posicion y velocidad. se programan o -line mediante un lenguaje de programacion espec co 4. vision o inteligencia arti cial 1.3 Estructura de un robot industrial En un robot industrial. 2. Robots inteligentes: Son analogos a los anteriores. Robots de aprendizaje gestual: El operario ense~a al robot mediante n pistolas de programacion o bien moviendo el robot. Brazo mecanico El brazo mecanico es el conjunto de elementos mecanicos que permiten el movimiento de la herramienta del robot. 3. . 3. Actuadores.2 Introduccion a la Robotica Industrial 9 2. Brazo mecanico o manipulador que dispone en su extremo de una herramienta. Ademas el robot industrial se puede encontrar dotado de toda una serie de automatismos tales como cintas transportadoras. automatas programables etc. Sensores avanzados. Controlador.

el numero de grados de libertas del robot sera igual al numero de articulaciones. lo que permite usar los tres primeros para jar la posicion y los otros tres para jar la orientacion en el espacio de la herramienta. Por lo general los robots industriales suelen tener 6 grados de libertad. pero normalmente se suelen usar de rotacion o prismaticas. generando las se~ales de control n adecuadas para realizar los movimientos deseados. por ejemplo lazos de control PID. Controlador El controlador del sistema se encarga de procesar toda la informacion que llega de los sensores y del usuario mismo.2. .1: Estructura general de un robot industrial A semejanza del brazo humano al primer enlace se le denomina brazo. en el nivel mas bajo se encuentran los algoritmos de control de los actuadores. antebrazo y al resto mu~eca. Velocidad Brazo manipulador Figura 1. Existen distintos tipos de articulaciones. ambas de un grado de libertad. al segundo enlace. tal y como se ve en la gura n 1. El controlador suele estar organizado de un modo jerarquico.10 1 Introduccion Sensores de información Herramienta Actuadores Controlador Sensores Posición. Siendo todas las articulaciones de un grado de libertad.

. de esta manera es posible controlar como el robot se desplaza de un punto a otro. El controlador ademas de realizar todas estas tareas es necesario que supervise el funcionamiento del conjunto. Ademas del control de trayectorias es posible encontrar a este nivel lazos de control con lecturas de sensores mas especializados. esta parte se encarga de generar las referencias para cada eje adecuadas al control de mas bajo nivel. como por ejemplo sistemas de vision o sensores de esfuerzos.2: Brazo mecanico del robot industrial RM-10 con indicacion de sus movimientos posibles En un nivel mas alto se encuentra el generador de trayectorias.1.2 Introduccion a la Robotica Industrial 3 11 Brazo 1 Antebrazo 6 2 5 Muñeca 4 Figura 1. Es el usuario normalmente quien especi ca que tipo de trayectorias debe seguir el robot. actuando adecuadamente en el caso que algun componente del sistema fallase.

etc. maquinas de soldar. En robots industriales cuyos ejes son de rotacion los sensores de posicion mas usados son los encoders. taladros. si bien esta se puede estimar a partir de la medida de la posicion. Para medir la velocidad se pueden usar dinamos tacometricas o bien sensores inductivos. Los sensores de posicion que aparecen en los robots industriales son: Potenciometros. Existe gran variedad de herramientas.. La ventaja de los .12 qref q_ref 1 Introduccion q Control Servos Control Trayectorias q_ Figura 1.. Inductosyn. Sensores Para poder cerrar los bucles de control es preciso que por lo menos el robot manipulador disponga de sensores de posicion en cada una de sus articulaciones. seguido de los resolvers. Resolvers. Encoders.3: Control en un robot industrial Herramienta En la mu~eca del manipulador se coloca una herramienta con la que el man nipulador realiza las tareas que el usuario programe. adicionalmente tambien pueden llevar sensores de velocidad. como por ejemplo garras de sujeccion.

3 Descripcion del robot industrial RM-10 13 encoders frente a los resolvers son que sus se~ales son digitales haciendolos n mas robusto ante entornos de ruidos. Motores de corriente continua con o sin escobillas Motores de corriente alterna. en funcion de la energ a que usen: Neumaticos. Estos actuadores pueden actuar a traves de reductoras y correas de transmision o bien directamente en las articulaciones del robot. aunque cada vez mas se usan los motores sin escobillas. 1. los motores electricos mas comunmente usados son: Motores paso a paso.1. En estos la conmutacion de la corriente se produce en una electronica externa basandose en la medida de la posicion del rotor del motor. Los mas usados son los motores de corriente continua convencionales. Esta ofrece la unica interfaz de usuario para poder hacer uso manual del robot. un armario de control y una pistola de programacion. Actuadores Los actuadores en un robot industrial pueden ser. . sin embargo los resolver son capaces de dar medidas mas precisas que los encoders. Estos robots se denominan respectivamente de accionamiento indirecto y directo. En la actualidad la mayor a de los robots que se fabrican poseen actuadores electricos ya que son los mas adecuados para el control. Hidraulicos Electricos.3 Descripcion del robot industrial RM-10 El robot industrial RM-10 esta compuesto de un brazo manipulador. as como para su programacion.

Los servomotores son de corriente continua sin escobillas. todas ellas de rotacion.4: Vista general del robot industrial RM-10 1. siempre que no se viole ningun l mite angular de alguna articulacion. con una tension continua de alimentacion de 310V. Los ejes de las articulaciones estan todos acoplados a los motores mediante reductoras. esto permite bloquear el brazo en cualquier posicion. Los motores incorporan un freno electrico que se libera alimentandolo con una tension continua de 24V. El devanado estatorico de los motores es trifasico y el rotor esta construido con imanes permanentes de alta densidad.3. mas conocido como tecnolog a brushless. excepto las articulaciones de la mu~eca 5 y 6 que precisan de correas de transmision. estando colocadas en el mismo eje de giro.14 ARMARIO 12 12 12 1 Introduccion BRAZO MANIPULADOR PISTOLA 1234 1234 1234 1234 Figura 1.. n ya que dado el volumen de los motores estos se encuentran ubicados en el interior del antebrazo. lo cual le con ere la posibilidad de orientar la herramienta en cualquier posicion. En el estator de los motores se dispone de una proteccion termica mediante una resistencia NTC y en caso de sobrecalentamiento el servoampli cador del motor se encarga de reducir la intensidad que circula por los devanados del estator.1 Brazo manipulador El brazo manipulador del robot industrial RM-10 posee 6 articulaciones. .

5 4 x 10 4.5 −3 Figura 1.5 1 1.5 4 x 10 4.5 3 3.5 Corriente maxima de entrada: 10 mA. Las caracter sticas electricas mas importantes de los resolvers son: Frecuencia portadora: 3500 Hz.5 2 2. Tension referencia: 10V pp Relacion de transformacion: 0.5.5 4 x 10 4.5 2 2.2 Armario de control En el armario de control del robot RM-10 de System Robot nos encontramos 3 zonas mas o menos de nidas: . La se~al de referencia es proporcionada a cada resolver por su corresponn diente ampli cador de potencia.3 Descripcion del robot industrial RM-10 10 5 Ref 0 −5 −10 5 0 0.5 3 3.3. sino tambien al mismo servoampli cador ya que es necesaria la posicion para poder conmutar de forma electronica la tension continua del motor entre sus distintas fases.5 −3 0 S −5 0 0. Las formas de estas se~ales son idealmente n como las de la gura 1.5 −3 15 C 0 −5 5 0 0.5 2 2.5 1 1.5: Se~ales proporcionadas por los resolvers n Cada motor dispone de un resolver que permite realimentar la posicion no solo al controlador del robot.5 1 1. 1.1.5 3 3.

16 Electronica de potencia. Esta tension se reduce mediante un autotransformador trifasico a 220V. uno de ellos usado para la pistola de manejo y programacion. El procesador se encuentra conectado con los servoampli cadores a traves de un bus interno por el cual se transmiten las se~ales de control y el procen sador obtiene informacion acerca del estado de los distintos servoampli cadores pudiendo actuar en consecuencia. Electronica de potencia En la parte delantera inferior del armario de control se encuentran los dispositivos de potencia del robot. La energ a electrica es suministrada al armario controlador en forma trifasica con una tension nominal de 380V. Dispositivos de almacenamiento. El sistema esta basado en bus VME. Servoampli cadores. estando toda la labor de control centralizada en una unica tarjeta procesadora basada en un procesador Motorola 68020 a 25 Mhz. Rack controlador. Ademas el rack controlador posee tarjetas con interfases a distintos perifericos: Puertos serie. 1 Introduccion Rack controlador El rack controlador se encuentra ubicado en la parte delantera superior del armario. . Zona de entrada y salidas. Tarjetas de entrada y salida digital.

Un contacto auxiliar de este rele activa la fuente AL2.3 Descripcion del robot industrial RM-10 17 En esta zona se encuentran distintas fuentes de alimentacion que convierten la tension alterna en distintos niveles de tension continua necesarias para los distintos dispositivos. a traves del rele auxiliar AUX1. Este contactor se cierra cuando el usuario pulsa el boton START del panel frontal del armario. Fuente AL2. Proporciona una tension de +24V necesaria para alimentar los frenos de los motores brushless. Se usa para la alimentacion de reles auxiliares y tarjetas de entradas y salidas digitales. Fuente no regulada formada por el transformador TR3 y el puente de diodos PD. Tambien proporciona una alimentacion simetrica de 12V necesaria para el funcionamiento de las se~ales de los sern voampli cadores. De entre todas las salidas las siguientes tienen un interes especial de cara a integrar la nueva tarjeta controladora: U1 . En concreto existen las siguientes fuentes: Fuente AL1 +24V con salida regulada.1. Proporciona +5V para la alimentacion de las distintas tarjetas del rack VME. Contactor CF: Cuando se cierra se alimentan los frenos de los motores brushless permitiendo el movimiento de estos. Se disponen de 32 salidas digitales a rele con contactos libres de potencial de las cuales 13 se encuentran ocupadas por el propio controlador y el resto se encuentran libres para el usuario.Se encarga. Zona de entrada y salidas La parte posterior del armario alberga tarjetas de entradas y salidas digitales. Tambien aparecen en esta zona dos contactores: Contactor CP: Es el contactor principal que proporciona tension a la fuente de alimentacion de los servomotores. de habilitar los servoampli cadores de modo que estos puedan recibir consignas. por lo que deja a los servoampli cadores totalmente operativos. .

Esta salida activa la entrada de limitacion de par o velocidad de los servoampli cadores. El armario dispone tambien de 48 entradas digitales de las cuales 15 se encuentran ocupadas por el propio controlador quedando el resto libres para el usuario.18 1 Introduccion U2 . Otras salidas se encargan de actuar sobres las servovalvulas de aire comprimido para la herramienta del manipulador.Se activa cuando la resistencia de disipacion de la fuente de alimentacion se funde. IN9 . Pulsador de emergencia de la pistola de programacion.Se activa cuando la fuente de alimentacion de los servoampli cadores se sobrecalienta. Tienen interes las siguientes entradas: EM6 .Se pone a nivel bajo cuando se activa un contacto de alarma como los descritos anteriormente. CR5 . IN10 . IFR . n . IN8 . Las salidas U1 y U2 estan condicionadas a que no este activado ningun dispositivo hardware de emergencia. Finales de carrera presentes en todas las articulaciones del robot excepto en la articulacion 6. Basicamente las entradas que usa el controlador tienen la funcion de captar pulsaciones en el panel frontal del armario o bien comunicar el funcionamiento anomalo de algun equipo.Se activa cuando el contactor de frenos se cierra liberandolos. estos son: Pulsador de emergencia del panel frontal del armario.Su funcion es la de a traves del rele auxiliar AUX2.Esta se~al se activa cuando alguna de las tres fases de alin mentacion trifasica de la fuente de alimentacion se pierde. as como para activar y desactivar indicadores luminosos. excitar la bobina del contactor CF (contactor de frenos). Por lo tanto mientras no se cierre el contactor principal mediante la pulsacion de START en el panel frontal del armario esta se~al se mantiene activada.

La conmutacion se realiza en funcion de la posicion del rotor que es realimentada por el resolver.3 Descripcion del robot industrial RM-10 19 Servoampli cadores El servoampli cador del motor brushless proporciona la conmutacion de la corriente del motor de manera electronica manteniendo un angulo de par de 90 de modo que el motor da un par maximo para una corriente dada en cada momento. Para modi car la corriente se regula la tension en bornas del motor mediante PWM. En este modo la consigna se traduce en el porcentaje de la corriente maxima admisible que se hace circular por el estator del motor. . de modo que la consigna se traduce en el porcentaje de velocidad maxima de la con gurada para ese motor. La alimentacion de potencia de los servoampli cadores son un bus de corriente continua de 300V que proporciona la fuente de alimentacion.1. En modo velocidad el servoampli cador cierra un bucle de velocidad sobre el bucle de corriente.6: Representacion esquematica de un servoampli cador Los servoampli cadores pueden funcionar en modo corriente o en modo velocidad. Puente de transistores de potencia + A B C Conmutador electrónico /A /B /C /A /B /C N S A B C Servomotor Brushless Posición del rotor Resolver Figura 1. Dicha velocidad maxima se con gura de entre un rango de valores discretos y se a na con un potenciometro de escalado. En el primer modo el servoampli cador cierra un bucle de corriente mediante sensores de efecto Hall. La corriente maxima para cada motor viene determinada por un modulo de optimizacion insertado en cada servoampli cador.

.4 Equipo para el desarrollo del proyecto El equipo fundamental en el que se basa el proyecto es el robot industrial RM-10 de la rma italiana System-Robot. Adicionalmente se instalo mas hardware ya sea comercial o desarrollado a medida para el presente proyecto. Numerosos perifericos de entrada y salida.4. La tarjeta controladora se encuentra instalada en un slot ISA del ordenador personal y tiene como caracter sticas mas importantes: Procesador principal Motorola PowerPC 604e a 333MHz en el que se ejecutan los algoritmos de control. ubicado en los laboratorios del Departamento de Ingenier a de Sistemas y Automatica de la Universidad de Sevilla. Adicionalmente es posible ajustar un o set de velocidad.20 1 Introduccion El bucle de velocidad se cierra con un controlador tipo PI o P cuyas ganancias se ajustan mediante potenciometros alojados en el interior del servoampli cador. 3 Tarjetas conversoras de resolver a encoder incremental (CRE). Subsistema DSP esclavo de Texas Instrument TMS320F240 para funciones avanzadas de entrada salida. 1. 4 Mbyte de memoria DRAM. 1.1 Equipo hardware Ademas del robot industrial RM-10 se cuenta con los siguientes elementos: Un ordenador PC compatible con microprocesador PENTIUM II 350MHz y 128Mb de memoria RAM. Una tarjeta de entradas y salidas digitales. Una tarjeta controladora de la rma dSPACE modelo DS1103.

se usa la ToolBox Real Time WorkShop junto con la librer a Real . Los bloques de Simulink tipo S-Function se han programado en lenguaje C y compilado con el compilador de l nea de comandos proporcionado por Microsoft Visual 5. { Salidas tipo Output Compare. { Entradas tipo Input Capture.1.4 Equipo para el desarrollo del proyecto 21 Las entradas de la tarjeta DS1103 se encuentran organizadas de las siguiente forma: Procesador Motorola PowerPC 604e: { 16 canales de entrada analogicas de 16 bits y 4 s. 4 canales de entrada analogicas de 12 bits y 0.8 s. 7 L neas de encoder incremental. n UART con gurable como RS-232 o RS-422.2 y la ToolBox de simulacion SIMULINK 2. { Entrada de fuentes de interrupciones y relojes externos.0. Microcontrolador esclavo TMS320F240: { 16 canales de entrada analogicas de 10 bits y 6 s.4. 32 se~ales digitales con gurables como entrada o salida.2 Software de desarrollo Para el dise~o y simulacion de controladores se usa como herramienta MATn LAB v5. Sobre las tarjetas CRE y la tarjeta de se~ales digitales se puede encontrar n mas informacion en los apendices B y A respectivamente. Para la implementacion de los algoritmos de control en la tarjeta controladora. 8 canales de salidas analogicas de 14 bits y 6 s. 1 Interface para bus CAN.0. 1. de tiempo de { { { { { { conversion. { UART con gurable como RS-232 o RS-422. { Salida para generacion PWM.

4. Trace 1103 con el que se capturan en el PC an trion la evolucion temporal de variables del programa que se ejecuta en la tarjeta. 1.22 1 Introduccion Time Interface Library proporcionada por la rma dSPACE junto con la tarjeta. En particular los programas usados son: Cockpit 1103 con el que se dise~an interfaces de usuario con las que es n posible cambiar en tiempo real parametros del controlador. La tarjeta controladora viene acompa~ada con software para el intercamn bio de datos en tiempo real entre el programa que se ejecutan en la tarjeta controladora y el PC an trion.7: Esquema de interface con el robot RM-10 .3 Interface con armario de control Conversión Resolver a Encoder BRAZO MANIPULADOR Aislamiento señales analógicas Electrónica de potencia Señales Digitales Figura 1.

Distancia de enlace di Distancia entre los ejes xi. 2. Longitud de enlace ai.Cap tulo 2 Cinematica del robot RM-10 2. medido sobre el eje xi. y xi medida sobre el eje zi. con el criterio que aparece en la g. Distancia entre los ejes zi y zi. y xi medido sobre zi . La notacion utilizada para el estudio de la cinematica del robot RM-10 es la de Denavit-Hartenberg 3]. medida sobre el eje xi.1 Introduccion El estudio de la cinematica es fundamental para el desarrollo del sistema de control de un robot. as como la plani cacion de trayectorias del robot. En este cap tulo se realizara un modelo cinematico del robot. angulo de la articulacion i angulo entre los ejes xi.1 en la que a cada grado de libertad se le asocian 4 magnitudes que lo de nen completamente: angulo de enlace i. . angulo entre los ejes zi. y zi. que permitira relacionar el espacio de las variables articulares con el espacio cartesiano. 1 1 1 1 1 1 1 1 1 Adoptando el convenio de la gura 2. . asociamos a cada enlace un sistema de coordenadas solidario al mismo y a partir de los 4 parametros 23 .1.

2 muestra la colocacion de los distintos sistemas de coordenadas.1 i i.24 i.dis i.2.1 la matriz de transformacion de la articulacion i a la articulacion i . y as de esta manera crear una matriz con los parametros cinematicos.1: Convenio Denavit-Hartenberg de nidos anteriormente se pueden construir matrices de transformacion que relacionan la orientacion y posicion de dos sistemas de coordenadas contiguos. La gura 2.s i 0 ai. c ic i. a partir de dicha gura se obtiene la tabla 2. 2. di 1 Yi Zi Xi i ai. Zi. 1 Yi. 1 ci .s i. 6 s i c i.1) . c i. 1 Figura 2. i i. dic i.1 2 Cinematica del robot RM-10 Zi. 1 1 ai. Segun el convenio adoptado en la gura 2. =6 s s 4 c is i. 0 0 0 1 1 1 1 1 1 1 1 1 2 3 7 7 5 1 (2.2 Matrices de transformacion Previamente a la obtencion de matrices de transformacion hay que situar f sicamente los sistemas de coordenadas en el robot. 1 vendra dada por la expresion 2. . .1 Tii.

2 Matrices de transformacion 25 X6 a4 d3 X3 Z3 Y6 Y3 Z4 Y4 YG Z1 Y 1 X2 X1 a2 Z2 Y2 a3 ZG d4 X4 XG dG X5 Z6 Z5 Y5 dB ZB Y XB Figura 2.2.2: Situacion de los sistemas de coordenadas .

3) c .0c 1 4 0 0 0 0 2 2 1 2 2 2 2 a 0 0 1 a 0 d 1 3 7 7 5 1 (2.5) .4) c .2 se obtienen las seis matrices de transformacion del robot: c .s 6 s T = 6 1 c0 4 0 0 3 2 3 3 3 2 3 0 0 1 0 3 7 7 5 2 3 (2.s .s 0 6 0 T = 6 .0c 1 4 0 0 0 0 4 4 3 4 4 4 2 a d 0 1 3 7 7 5 3 4 (2.s 0 6 0 T = 6 .2) c .1 1 2 Cinematica del robot RM-10 ai.s 6 s T = 6 1 c0 4 0 0 1 0 1 1 1 2 1 0 0 1 0 0 07 7 05 1 3 (2.s .1 y la tabla de parametros cinematicos 2.26 i.1: Parametros cinematicos 1 2 3 4 5 6 1 2 3 3 4 Usando la matriz 2. i di 1 0 0 0 2 -90 a 0 3 0 a d 4 -90 a d 5 90 0 0 6 -90 0 0 Tabla 2.

En este caso la posicion viene dada por la situacion del sistema de coordenadas solidario a la garra (sistema de referencia G).s 0 6 0 T = 6 .9) 2.6) c .2.7) Si queremos usar como origen de posiciones el sistema de coordenadas B necesitamos la matriz de transformacion del sistema 0 al sistema B: 1 6 0 T =6 0 4 0 5 6 2 0 1 0 0 0 0 1 0 0 0 dB 1 3 7 7 5 (2.s 0 0 6 0 7 T = 6 s c0 .3 Problema cinematico directo Las ecuaciones del problema cinematico directo ligan la posicion y orientacion de la herramienta del manipulador con las variables articulares del mismo respecto a un sistema de coordenadas jo en el espacio.s .0c 1 4 0 0 0 0 6 6 5 6 6 6 2 0 0 0 1 3 7 7 5 (2.1 0 7 4 0 05 0 0 0 1 5 5 4 5 5 5 2 (2. Para la orientacion se .3 Problema cinematico directo 27 3 c .8) Normalmente el punto que se desea posicionar el extremo nal de la herramienta del robot con lo que tambien necesitaremos una matriz de transformacion entre el sistema 6 y el sistema de coordenadas situado en la garra: 1 6 0 T =6 0 4 0 5 6 2 0 1 0 0 0 0 1 0 0 0 dG 1 3 7 7 5 (2.

Las relaciones matematicas se obtienen facilmente multiplicado ordenadamente las matrices de transformacion desde la base hasta la herramienta de la forma: B TG = T B T T T T T T TG 0 0 1 1 2 2 3 3 4 4 5 5 6 6 (2.11) El juego de ecuaciones que se obtienen para la orientacion de la herramienta es: nx = c c (c c c .3.28 2 Cinematica del robot RM-10 s a n Figura 2.12) .10) Del resultado de este producto se obtiene la matriz: nx 6 ny B TG = 6 n 4 z 0 2 sx sy sz 0 ax ay az 0 xG yG zG 1 3 7 7 5 (2.3: Vectores de orientacion en la herramienta del manipulador utiliza la terna de vectores ~ ~ ~ cuya orientacion en el espacio puede verse nsa en la gura 2. s s ) + c c s s + s s c c + s c s 1 6 23 4 5 23 5 1 23 4 6 1 4 5 6 1 4 6 (2.

s c s c + c s c s + c c c 1 6 23 5 23 4 5 1 23 4 6 1 4 5 6 1 4 6 (2.c (c c s + s c ) . .s (c c s + s c ) .15) (2. c c s c . d s s + dGay 1 1 2 1 2 3 1 23 3 1 4 1 23 zG = .13) (2.23) yG = a s + a s c + a s c + d c .21) (2.19) (2. c s ) + s s s 6 23 4 5 23 5 1 6 23 5 23 4 5 1 23 4 6 1 4 5 6 23 4 6 sx = c s (s s .c (s c c . c c 23 4 5 23 5 Y para la posicion: xG = a c + a c c + a c c . d s . c s s 1 23 4 5 23 5 1 4 5 1 4 5 az = s c s .3 Problema cinematico directo 29 1 23 4 6 1 4 5 6 1 4 6 ny = s c (c c c .a s .20) 1 4 6 sz = s (c s + s c c ) + s s c 6 23 5 23 4 5 1 23 4 5 23 5 23 4 6 ax = .22) (2. a s . d c s + dGax 1 1 2 1 2 3 1 23 3 1 4 1 23 (2. La funcion recibe como entrada un vector de variables articulares y realiza los calculos necesarios para devolver la posicion cartesiana y los vectores de orientacion de la herramienta. s s s ay = .c en forma de bloque para su uso en diagrama Simulink. s s ) + s c s s .18) (2.17) (2.16) (2. c c c ) .2.14) nz = . c s c c + c c s 1 6 23 4 5 23 5 (2. d c + dGaz + dB 2 2 3 23 4 23 La implementacion software de estas ecuaciones se realiza en la funcion pcd. s s c s + s c c sy = s s (s s . c c c ) .

El conjunto de ecuaciones que se tienen que resolver son de 2. 2. se usa algun metodo de resolucion numerica si bien este metodo es el mas costoso en tiempo de calculo para una computadora.~ ns y ~ no son independientes y ademas son unitarios. el conjunto de variables articulares que la satisfacen.12 a 2. si bien existen soluciones que se descartan debido a las limitaciones que impone el espacio de trabajo del manipulador. en los que se intenta buscar una solucion anal tica basada en la geometr a del robot aplicando relaciones trigonometricas. As mismo. El problema cinematico a inverso en general no tiene una solucion unica.4. Numericos. A continuacion se expone una solucion anal tica usando metodos aritmeticos.4 Problema cinematico inverso En muchas ocasiones interesara dar consignas al robot en el espacio cartesiano. buscan una solucion anal tica manipulado las ecuaciones usando las matrices de transformacion.30 2 Cinematica del robot RM-10 2. esto implica que debamos conocer dado una posicion y orientacion de la herramienta del manipulador. Aritmeticos.23.1 Resolucion anal tica de la cinematica inversa Partimos de una matriz 2. es necesario resolver este problema para la generacion de trayectorias en el espacio cartesiano del robot. dejando solamente las matrices de transformacion que dependen de un grado de libertad: . 5 y 6 respecto al sistema de referencia 0. A partir de la posicion podemos obtener la posicion de la interseccion de los ejes 4. o sea de una orientacion y una posicion de la herramienta del manipulador. De estas 12 ecuaciones nos podr amos quedar con 6 ya que los vectores ~ .11. Existen distintos metodos de resolver la ecuaciones: Geometricos.

29) Sustituyendo 2.4) del sistema de ecuaciones 2.25) Realizamos la operacion: c 6 . T = 6 0 4 0 1 0 1 1 0 6 2 1 s c 0 0 1 1 0 0 1 0 0 0 0 1 32 nx 7 6 ny 76 5 4 nz 0 sx sy sz 0 ax ay az 0 x y 7=T 7 z5 1 3 1 6 (2.s (T ).26 tenemos la ecuacion: .27 obtenemos: . T B (T G).27) Para resolver el angulo 1 se realiza un cambio de variables: x = rcos( ) y = rsen( ) = atan2(y x) p r = x + y2 2 (2.26) A partir del elemento (2.s x + c y = d 1 1 3 (2.28 en la ecuacion 2.24) Tendremos que resolver: nx 6 ny T =6 n 4 z 0 0 6 2 sx sy sz 0 ax ay az 0 x y 7=T T T T T T 7 z5 1 0 1 1 2 2 3 3 4 4 5 3 5 6 (2.2.28) (2.4 Problema cinematico inverso 31 nx 6 ny T =6 n 4 z 0 0 6 2 sx sy sz 0 ax ay az 0 x y 7 = (T B ). 7 G z5 1 0 1 6 3 1 (2.

Para resolver premultiplicamos la expresion 2.34) Esta ecuacion tiene la misma forma que 2.s a .4) son: 1 c x+s y =c a +a +c a .27 por lo que la solucion sera: 3 = atan2(a d ) . a = c a .35) En este caso se tienen 4 soluciones para .4) y (2.33) Elevando ambas expresiones al cuadrado y sumandolas obtenemos tras hacer simpli caciones: (c x + s y . atan2(d q 3 x +y . atan2(A 3 4 3 p d +a +A ) 4 2 3 2 2 1 2 (2.31) Si deshacemos el cambio de variable con las expresiones 2. c d .32) Debido a la ra z cuadrada tendremos dos soluciones para la articulacion .rs c + rc s = rsen( . s a 1 1 2 2 1 23 23 3 4 23 2 2 23 4 3 (2.29 podemos obtener una expresion para : 1 = atan2(y x) .26 tambien podemos resolver la articulacion 3. a ) + z . Del bloque de ecuaciones de 2. d ) r 2 3 2 (2. ) = d 1 1 1 3 (2.s d z = . a . s d = A 2a 1 1 1 2 2 2 2 4 2 3 2 2 3 3 3 4 (2. las ecuaciones de los elementos (1. ya que tiene 2 soluciones y la solucion de depende de la solucion de A.32 2 Cinematica del robot RM-10 . d .26 por las inversas de T y T : 3 1 2 2 3 .30) Con lo que: .d ) 2 2 3 2 (2. = atan2( dr 1 1 r 3 1.

2. de cuya resolucion se obtiene: s .3) y (2.(2 xa+ c . + .s s c 0 1 1 23 1 23 .xcxa )ys .s . s s y .a c =a .s z.4 Problema cinematico inverso 2 33 32 cc 6 .s 0 1 23 1 23 1 sc .c s x .3): c c ax + s c ay .c s 6 4 . a c 1 23 1 23 3 2 3 0 0 23 nx a s + s a 7 6 ny 76 5 4 nz . 2as)ya (. 23 23 3 (2.38) 3 Con las que obtenemos la suma de los angulos obtenemos 4 soluciones posibles para el angulo : 2 2 2 y . c z + a s + s a = d 1 23 1 23 23 1 23 2 3 1 23 1 23 23 1 23 3 2 23 23 3 4 (2.c s .36 tambien podemos extraer las ecuaciones de los elementos (1.4): c c x+s c y. a ) + (s a . y conocido 3 = atan2(s c ) .s ax + c ay = s s 1 23 1 23 23 1 1 4 5 4 5 (2.39) Del bloque de ecuaciones 2.4) y (2.37) Las expresiones 2. d )+ s y ) 2 s ya + + z 23 3 2 4 1 1 1 2 3 1 2 3 1 2 2 1 1 1 1 1 2 1 2 23 2 3 3 1 1 1 3 2 4 1 2 2 1 1 1 1 1 1 2 2 1 2 2 2 (2.aa c + za ) zs y (2 2 c + + ( c + a ) xc ys a z c = c x +a(2 xs y .d 1 0 2 3 sx sy sz 0 ax ay az 0 x y 7 = T (2.c 23 . s az = . s = c x (+ a xs dy ).a c .36) 7 z5 1 3 6 3 De este conjunto de ecuaciones nos quedamos con las que forman los elementos (1. ( + .40) De las que se puede resolver : 4 .37 forman un sistema de ecuaciones en el que tenemos como incognitas unicamente a c y s ..a c .

(c s c . nos sobra un grado de libertad.3) y (3.45) Con lo que se puede resolver el angulo como: = atan2(s c ) 5 5 5 5 Se procede de forma analoga para el angulo : 6 nx 6 ny (T ). supervisando el valor del coseno de se acerca a la unidad: 5 4 6 5 c = .36 por la inversa de T nx 6 ny (T ). cuando el se acerque a cero se puede dejar constante y orientar la herramienta con .41) En el caso de que se anulase no podr amos resolver segun la ecuacion anterior. c s ) ay + c s az c = .46) .43) De este conjunto de ecuaciones se puede extraer los elementos (1. debido a que el manipulador se encuentra en una con guracion singular en la que los ejes 4 y 6 estan alineados.s ax + c ay .c s ax . s c ay + s az ) 1 1 1 23 1 23 23 5 4 4 (2. 6 n 4 z 0 0 4 1 2 sx sy sz 0 ax ay az 0 x y 7=T 7 z5 1 3 4 6 (2. (c c c + s s ) ax . c az 5 1 23 1 23 23 (2.c s ax .3).3): s = . Esto se puede hacer usando la ecuacion (3. s s ay . por tanto.34 2 Cinematica del robot RM-10 = atan2(. para conseguir la orientacion deseada de los vectores ~ y ~. Para n s evitar problemas. 6 n 4 z 0 0 5 1 2 sx sy sz 0 ax ay az 0 x y 7=T 7 z5 1 3 5 6 (2. c az 5 1 23 4 1 4 23 1 4 1 4 4 23 5 1 23 1 23 23 (2.44) (2.42) 3 4 Para resolver de la forma: 5 premultiplicamos la ecuacion 2.c c ax . s s ay .

(c c s .49) Con lo que en total obtenemos 8 soluciones para el problema cinematico inverso.48) Para las tres ultimas articulaciones existe una segunda solucion que se calcula como: 0 0 5 0 4 6 = .4: Soluciones para las tres ultimas articulaciones . (c s s + c c ) sy + s s sz s = .47) Con lo que se puede resolver el angulo 6 6 6 como: 6 = atan2(s c ) (2. s c ) sx . X Y 4 X Z 5 X Z 5 6 4 4 Y ~ a 5 a==a Z 6 6 ~ n ~ s Y Y 4 6 X Y 5 Z X 4 4 Z 6 ~ s ~ a ~ n sol2 Z 5 Y 6 X 5 Figura 2. =. s c ) nx .1) y (3. (c s s + c c ) ny + s s nz 6 1 23 4 1 4 23 4 1 1 4 4 23 6 1 23 4 1 4 23 4 1 1 4 4 23 (2.1) aparecen las ecuaciones: 35 c = .4 Problema cinematico inverso En los elementos (1. 4 5 6 (2. = . (c c s .2.

La funcion recibe como entradas: ~ 1. 2. Se~al de error en caso de que no haya solucion en el espacio de trabajo n del robot. De las soluciones que quedan se elige la mas cercana a la posicion actual de robot. La funcion cineinv.5. 2. Vectores de orientacion de la herramienta ~ . Vector posicion cartesiana de la herramienta P . Ademas recibe como parametros las distancias que de nen la cinematica del manipulador.4.c) para diagrama de bloques de Simulink.5 Generacion de trayectorias Para realizar un movimiento ordenado de las articulaciones se ha implementado la posibilidad de generar trayectoria articulares y en el espacio cartesiano.1 Trayectorias articulares Este tipo de trayectorias es el mas usado normalmente cuando no es necesario especi car con presicion el camino y orientacion que debe seguir la herramienta del manipulador.2 Implementacion software El problema cinematico inverso se ha inplementado como una funcion S (la funcion cineinv. Se consigue que el movimiento de todas las .c calcula las soluciones posibles. ns a 3. Vector de variables articulares solucion 2. Las trayectorias son generadas en tiempo real a partir de la posicion actual del manipulador. de las cuales descarta las que se quedan fuera del espacio de trabajo del robot.~ y ~ .36 2 Cinematica del robot RM-10 2. Vector de variables articulares correspondientes a la posicion actual del robot. Y se tienen como salidas: 1. 2.

i) a = T6 ( f .4 1.4 0.6 0.5 0 1 0 0. La velocidad articular maxima alcanzada se produce en t = T=2 y toma el valor: 15 f .5 0 2 0 0.5: Trayectorias articulares polinomicas de 5 orden Con este orden de polinomio es posible especi car tanto velocidades como aceleraciones nulas al comienzo y al nal de trayectoria.50) Los coe cientes de los polinomios son calculados en funcion de las condiciones de contorno.51) Donde f y i son las posiciones articulares nal e inicial respectivamente y T es el tiempo total durante el que se genera la trayectoria. obteniendose: 10 a = T ( f .2 1.8 2 00.52) max = 8 T 3 3 4 4 5 5 . El tipo de trayectoria usado es un polinomio de quinto orden tal y como puede verse en al gura 2.2 0.6 1.4 1.5 Generacion de trayectorias 37 articulaciones comience y termine a la vez.4 0.2 0.8 1 t 1.8 2 00 0 −1 −2 0 0. as como un movimiento suave de las articulaciones.6 0.6 1. T ( f .5 1 0.6 0. i 0 (2.8 1 1. i) 15 a = .8 2 1 Figura 2. de este modo las ecuaciones de la trayectoria quedan: (t) = i + a t + a t + a t 0 (t) = 3a t + 4a t + 5a t 00 (t) = 6a t + 12a t + 20a t 3 3 4 4 5 3 3 2 4 3 5 4 4 2 5 5 3 (2.8 1 1.4 0.2 1.2 0.4 1. o) (2.2 1.2.6 1.

c que se encarga de realizar la trayectoria lineal en terminos de las variables x y z roll pitch yaw referidos a los ejes inerciales situados en la base del manipulador.38 Enable 2 Cinematica del robot RM-10 du/dt n 3 qd eul2tr s a p pact q 1 q Error 2 error PCI 2 Pfinal 1 Move pcdcar xyz−rpy act Mover p Qf m Traylin 3 Qact Figura 2.6: Generacion de trayectorias lineales. La posicion inicial del espacio cartesiano se obtiene mediante el bloque . La funcion recibe como entradas la posicion deseada.2 Trayectorias lineales Para hacer tareas que requieran cierta precision es preciso que los manipuladores sean capaces de realizar trayectorias lineales. La funcion recibe como parametro un vector de velocidades articulares maximas. que a su vez se descompone en distintas tareas espec cas. de esta manera la funcion puede calcular el tiempo necesario que tiene que durar la generacion de la trayectoria sin que se viole ninguna de estas velocidades.5. 2. En primer lugar se dispone de el bloque trajlin52.c. Esta tarea es realizada por un subbloque Simulink. El bloque genera una secuencia de posiciones y velocidades de referencia. La generacion de este tipo de trayectorias esta implementada en la funcion Simulink. la posicion actual del manipulador y la se~al digital que activa la generacion n de la trayectoria a cada anco. traj52.

cy sr .sp cpsr cp cr 3 5 (2.4. En este caso el sistema genera una alarma bloqueando el manipulador. Si c 6= 0: = atan2(r r ) q = atan2(.c realizando los calculos del problema cinematico directo.r r +r ) = atan2(r r ) 32 33 31 2 11 2 21 21 11 (2. Este bloque genera una se~al de error en el caso de que no encontrase n para algun punto de la trayectoria en el espacio cartesiano. pitch y yaw. La relacion entre la parte rotacional de la matriz de transformacion y los angulos r p y y viene dada de la forma: 2 cy cp cy spsr .53) A partir de esta matriz se pueden deducir los angulos de roll.5 Generacion de trayectorias 39 pcdcar. una solucion del problema cinematico inverso. habra que jar y calcular de la forma: = 90 = .c que se encarga de resolver la cinematica inversa del manipulador segun el procedimiento descrito en el punto 2.12 a 2. . proporcionando referencias en el espacio articular al controlador.54) En el caso de c = 0.90 =0 =0 = atan2(r r ) = . sy cr cy spcr + sy sr 4 sy cp sy sp sr + cy cr sy sp cr . Ademas este bloque obtiene la orientacion de la herramienta del manipulador en terminos de los angulos roll. pitch.atan2(r r ) 12 22 12 22 (2.55) Las consignas en el espacio cartesiano generadas son enviadas al bloque cineinv. o sea a partir de las ecuaciones 2.1.23.2. yaw a partir de la parte rotacional de la matriz de transformacion obtenida con las ecuaciones 2.20.21 a 2.

40 2 Cinematica del robot RM-10 .

Ademas es posible simpli car las soluciones simbolicas mediante un analisis de los terminos de lo que esta compuesta.Cap tulo 3 Dinamica del robot RM-10 3. El modelo dinamico no solo es util para poder simular el comportamiento del sistema. estas formulaciones pueden resultan mas ine cientes computacionalmente ya que existen numerosas operaciones del tipo multiplicacion por ceros. 41 . ganando mas aun en e ciencia. Si bien las soluciones numericas resultan mas compactas en su formulacion mediante algoritmos como el de Newton-Euler. La obtencion de las ecuaciones de movimiento se puede obtener mediante las ecuaciones de la dinamica clasica o bien mediante la mecanica lagrangiana. Este tipo de operaciones no se producen con una formulacion simbolica del problema ya que todas estas operaciones se realizan a priori. tambien lo es para comprender el problema de control de los manipuladores.1 Introduccion Antes de abordar el problema de control del manipulador es necesario obtener un modelo dinamico. Todas estas manipulaciones se pueden realizar mediante herramientas de calculo simbolico como MAPLEV. En general se pueden generar soluciones numericas o bien soluciones simbolicas.

En este caso las variables elegidas como grados de libertad son las variables del espacio articular del manipulador. las fuerzas generalizadas son equivalentes a los pares aplicados a cada articulacion. siendo esta simetrica y de nida positiva.42 3 Dinamica del robot RM-10 3. L se conoce como lagrangiana del sistema y se calcula como: L=T .1) Siendo n el numero de grados de libertad del manipulador y qi las variables asociadas a cada uno de los grados de libertad de cada manipulador.2 Modelo Euler-Lagrange El modelo Euler-Lagrange se basa en aplicar las ecuaciones de la mecanica lagrangiana ya conocidas con la forma: d @L .2) Donde T es la energ a cinetica total del sistema mecanico y U su energ a potencial. Al elegir estas variables como grados de libertad.3) donde D(q) es la matriz de inercias del sistema. La energ a potencial se puede escribir en terminos de matrices de transformacion de la forma: U= Donde: n X i mi Ti ~ci r 0 (3.4) . La energ a cinetica del manipulador se puede expresar de la forma: n 1 q_T D(q)q_ = 1 X d q_ q_ T=2 2 i j ij i j (3. @L = Q i dt @ q_i @qi i = 1 :::n (3.U (3.

5) Hasta ahora no se ha considerado los fenomenos de friccion en el modelo. esta a su vez en 3. si se a~aden en forma matricial se pueden escribir de la forma: n D(q)q + C (q q_)q_ + F (q q_) + G(q) = Los terminos que aparecen en las ecuaciones de movimiento son: (3.3. ~ci es el vector posicion del centro de gravedad del enlace i respecto al r sistema de referencia i. Por tanto. @q = @q @q i k k k k = 1 ::: n (3. Ti es la matriz de transformacion del sistema de referencia 0 al sistema de referencia i. Las ecuaciones 3. G(q) son los pares gravitatorios asociados a la masa de cada enlace. 2 @dij )q_i q_j .6 modelan la dinamica del brazo manipulador. se puede escribir para los actuadores en forma vectorial: m = Jm qm + Bmq_m + L (3.6) D(q)q representa a las fuerzas inerciales de cada enlace debidas a la aceleracion. que en el caso mas sencillo se puede modelar como una friccion viscosa. la dinamica de estos se reduce a la ecuacion mecanica del eje del motor. F (q q_) representa a los pares de friccion en cada articulacion. Sustituyendo la expresion de la energ a cinetica en 3.2 Modelo Euler-Lagrange 0 43 mi es la masa del enlace i. Se pueden a~adir tambien la dinamica de los actuadores.1 y desarrollando las derivadas se obtiene: n X j dkj qj + n X ij 1 @U ( @dkj . C (q q_)q_ representa a los pares de Coriolis y los correspondientes a los terminos centr fugos. Suponiendo que los motores n tienen una constante electrica despreciable.7) .2.

qm posicion del eje motor.5.10) (3.12) 2 2 Los terminos de Coriolis se pueden calcular como funcion de los dij segun se ve en la expresion 3.6 en las ecuaciones 3.9) (3.44 3 Dinamica del robot RM-10 El par motor es proporcional a la corriente que recorre los devanados del estator. por lo que interesa en primer lugar calcular los .11) RKtIm = Jm R q + Bm R q_ + 2 2 donde R es una matriz diagonal con los coe cientes de reduccion mayores que uno. Jm inercia del eje motor. Si existen reductoras entre el manipulador y los motores se dispone ademas de las relaciones: qm = Rq q_m = Rq_ = RL sustituyendo en la expresion 3. L par de carga que ofrece el brazo manipulador. m par electrico desarrollando por el motor.7 obtenemos: (D(q) + JmR )q + (C (q q_) + Bm R )q_ + F (q q_) + G(q) = Kt ImR (3. Bm friccion viscosa del motor.8) Kt constante de par del motor. por lo que se puede poner: Kt Im = Jmqm + Bm q_m + donde: L (3. Si se sustituye la expresion 3.8 se tiene: (3.

3.3 obtenemos la expresion para la matriz de inercias: D(q) = n X i T (mi JvTci Jvci + J!i IiJ!i ) (3.13. La energ a cinetica en general se puede tambien expresar como la suma de la energ a cinetica de rotacion y la de traslacion: T= n X i 1 m vT v + 1 !T I! 2 i ci ci 2 i i (3. se obtiene: n 1 q_T X(m J T J + J T I J )q_ T=2 i vci vci !i i !i i (3.14) La matriz jacobiano para las velocidades angulares debe estar expresada en el sistema de referencia de cada enlace ya que estos son los ejes en los que se expresan los tensores de inercia.13) Las velocidades lineales de los centros de masa y las velocidades angulares de rotacion pueden ser expresadas en funcion de las variables articulares por medio de matrices jacobiano de la forma: vci = Jvci q_ !i = J!i q_ (3.18) . Sustituyendo estas expresiones en 3.17) El jacobiano de las velocidades de rotacion se forma tambien mediante una matriz por columnas de la forma: J!i = J!i : : : J!i 1 6 3 6 (3.2 Modelo Euler-Lagrange 45 elementos de la matriz de inercias expresandolos como funcion de las matrices de transformacion del manipulador.15) Identi cando esta ecuacion con la ecuacion 3.16) El jacobiano de las velocidades de traslacion se calcula como una matriz formada por las columnas: Jvci = h @ (Ti0~ci ) r @q1 ::: @ (Ti0~ci ) r @q6 i 3 6 (3.

1].4.m].19) Los calculos para obtener la matriz de inercias se pueden implementar en MapleV tal y como se muestra a continuacion: Calcula la matriz T i.1]:=diff(Vect n.1.. > Dvect := proc(Vect) Vel:=matrix(4.46 Donde: 8 < 3 Dinamica del robot RM-10 (Tij ).1) for n from 1 to 4 do Vel n. > Dmatrix := proc(Mat. ~ si j k 1 J!ij = : i ~ 0 si j > i (3..x) od od RETURN(Mat) end Dmatrix := proc(Mat x) localn m for n to 4 do for m to 4 do Mat n m := di (Mat n m x) od od end RETURN(Mat ) Deriva los elementos de un vector respecto al tiempo.1.m]:=diff(Mat n.4) to p do M:=evalm(M&*A(k)) od RETURN(M) end for k from 1 T := proc(p) localM k M := array(identity 1::4 1::4) for k to p do M := evalm(M & (A(k))) od RETURN(M ) end Deriva los elementos de una matriz.t) od RETURN(Vel) end .x) for n from 1 to 4 do for m from 1 to 4 do Mat n. 0 > T := proc(p) M:=array(identity.

0]) for m from n+1 to i do vaux:=evalm(evalm(1/A(m))&*vaux) od for k from 1 to 3 do J k. r ..n]:=vaux k.1. Jacv := proc(Vect ) localJ n m end J := matrix(3 6) for n to 3 do for m to 6 do Jn m := di (Vect n RETURN(J ) 1 m ) od od Calcula el jacobiano J!i de la velocidad angular del solido i.6) for n from 1 to i do vaux:=matrix(4.2 Modelo Euler-Lagrange Dvect := proc(Vect ) localVel n Vel := matrix(4 1) for n to 4 do Vel n 1 := di (Vect n 1 t) od RETURN(Vel ) 47 end Calcula el jacobiano Jvci de la velocidad del centro de masa. Vectores centros de masa ~ci .4) J:=array(sparse.1..1.3. 0..0.1] od od RETURN(J) end calJacw := proc(i) localJ n vaux m k A(0) := array(identity 1::4 1::4) J := array(sparse 1::3 1::6) for n to i do od end vaux := matrix(4 1 0 0 1 0]) for m from n + 1 to i do vaux := evalm((evalm(1=A(m))) & vaux ) od for k to 3 do Jk n := vaux k 1 od RETURN(J ) Caracter sticas de los elementos.4.1.3. > calJacw:= proc(i) A(0):=array(identity..1.1.

evalm(Drot+Dr i])): od: Inercia a~adida por los motores. a la matriz D(q). y i]. > Dtras:=0: > for i from 1 to 6 do Jvc i]:=eval(Jacv(evalm(T(i)&* rcm(i)))): Dt i]:=map(simplify. Izz p]]): Calcula la matriz D(q). z i]. Dt i]. Jm 2]*R 2]^2.Jacw i]. n > Dmot:=linalg diag](Jm 1]*R 1]^2.simp): Dr i]:=evalm(evalm(transpose(Jacw i])&* J(i)&* Jacw i])) Drot:=map(simplify. Drot es la parte de la matriz D debida a la rotacion. aportes de cada enlace a la matriz D(q).g.0. aportes de cada enlace por traslacion del c. evalm(m i]* evalm(transpose(Jvc i])&* Jvc i]))) Dt i]:=map(simplify.1. 0. rotacion y la a~adida n por los motores. > J:= p -> matrix(3. Dtras es la parte de la matriz D debida a la translacion de los c. > Inertia:= map(simplify.d. Jm 5]*R 5]^2. siderels. Iyy p].48 3 Dinamica del robot RM-10 > rcm:= i -> matrix(4. A continuacion se calcula los jacobianos de las velocidades de traslacion de los centros de gravedad. 0.d. eval(calJacw(i))) Jacw i]:=map(simplify. 3 . siderels. Jm 4]*R 4]^2. Jm 6]*R 6]^2): La matriz D(q) sera la suma de la de traslacion. 0 . 0. evalm(Dtras+Dt i])): od: A continuacion se calcula los jacobianos de las velocidades angulares de los elementos.evalm(Dtras+Drot+Dmot)): . x i].1]): Tensores de inercia Ji. Ixx p]. Jm 3]*R 3]^2.simp) Dtras:=map(simplify.g. > Drot:=0: > for i from 1 to 6 do Jacw i]:=map(simplify. 0.

6) RETURN(resu) end C := proc(k j ) localresu resu := sum('Cor(i j k) QD(i)' 'i' = 1::6) RETURN(resu ) end > for i from 1 to 6 do for j from 1 to 6 do MC i.j)) od od Los elementos del vector de pares gravitatorios se calculan mediante el tercer termino de la expresion 3. 1 @dij )q_ q_ = 1 X( @dkj + @dki .'i'=1.5 aprovechando la simetr a lo podemos poner como: n X n @dkj ..k)*QD(i)'.j.20) 2 i j ijk i j Las instrucciones de MapleV necesarias para obtener la matriz C (q q_) son las siguientes: Calcula los elementos de la matriz de coriolis C(q.qd).3.5.j) resu:=sum('Cor(i. El segundo termino de la ecuacion 3.j]:=eval(C(i. o lo que es lo mismo con las instrucciones que siguen a continuacion: . > C := proc(k.2 Modelo Euler-Lagrange 49 Los elementos de la matriz C (q q_) se pueden calcular a parir de la los elementos de la matr z D(q). @dij )q_ q_ = ( 2 @qk i j 2 i j @qi @qj @qk i j i j @qi n 1 X c q_ q_ = (3.

Las distancias que aparecen en las matrices de transformacion son las que aparecen en la tabla 3.6). .1] end Calculo de los terminos de energia potencial. ademas esto permite poder aplicar tecnicas que se basan en el modelo del manipulador con mas exito. g (m(6) d4 + z4 m(4) + d4 m(4) + y3 m(3) + m(5) d4) cos(%1) %1 := 2 + 3 Calculamos las derivadas para cada eje. g (m(5) a3 + a3 m(4) + x3 m(3) + m(6) a3) sin(%1) + g (m(5) y5 + m(6) z6) cos( 4 ) sin(%1) sin( 5 ) .. g (m(5) y5 + m(6) z6) cos(%1) cos( 5 ) + m(1) g z1 . simp) Vtot := . De entre los parametros que aparecen en las ecuaciones de movimiento del robot aparecen parametros puramente cinematicos como son las distancias y parametros dinamicos asociados a casa uno de los enlaces del robot.1 Entre los parametros dinamicos necesarios para la caracterizacion de un modelo tenemos los siguientes: Tensores de inercia.siderels.'k'=1. > for i from 1 to 6 do phi i]:=diff(Vtot.g (x2 m(2) + a2 m(3) + a2 m(4) + m(5) a2 + m(6) a2) sin( 2 ) . V := proc(i) m(i)?g?evalm((T(i)) & (rcm(i))) end 3 1 Calculamos la energia potencial total del brazo manipulador. > Vtot:=simplify(sum('V(k)'.50 3 Dinamica del robot RM-10 > V:=proc(i) m(i)*g*evalm(T(i)&*rcm(i)) 3.theta i]) od: 3.3 Parametros del modelo del robot Para poder simular el comportamiento del robot es necesario disponer de un conjunto de parametros lo mas preciso posible.

Con la misma herramienta informatica se estimaron los centros de gravedad y las masas de los distintos elementos. Las inercias de los ejes se obtiene a partir de las hojas de catalogo: . Coe cientes de reduccion. 3.3. Inercias de los ejes de los motores.3.3 Parametros del modelo del robot 51 dB 950 a 260 a 710 a 150 d 40 d 650 dH 150 Tabla 3. 3.3. Parametros de friccion.1: Parametros cinematicos del robot. Se hace la suposicion de que hay simetr a en los tres ejes. Las matrices de inercias se calculan respecto al sistema de coordenadas solidario a cada enlace y tambien se calcula en el origen del mismo sistema. permitiendo tener as una matriz diagonal en cada enlace.1 Tensores de inercia y centros de gravedad Dada la di cultad que entra~a la medida experimental de las componentes n del tensor de inercias se opto por realizar un modelo gra co aproximado mediante una herramienta de dise~o gra co que fuese capaz de calcular una n aproximacion de los tensores. Masas.2 Caracter sticas de los motores y reductoras Los parametros que aparecen en las ecuaciones dinamicas del manipulador son las inercias de los ejes y la friccion asociada a los motores. Centros de gravedad. 1 2 3 3 4 Distancias en mm.

0:194 3 5 Y m = 84 3 Y Z m = 34 4 X 2 3 0:02 0 0 J = 4 0 0:008 0 5 0 0 0:031 5 X 2 3 0:006 0 0 J = 4 0 0:007 0 5 0 0 0:005 6 Z 2 3 0 rc5 = 4 0:031 5 0 Z Y 2 3 0 rc6 = 4 0 5 0:07 Y m =7 5 m =7 5 Figura 3.52 3 Dinamica del robot RM-10 Z Y 2 3 0:741 0 0 J = 4 0 1:208 0 5 0 0 1:132 1 2 3 0:265 0 0 J = 4 0 3:434 0 5 0 0 3:520 2 2 X rc2 = 4 0:083 0 .1: Caracter sticas de los elementos del brazo del manipulador .0:083 3 5 X Z 2 3 0:228 rc2 = 4 0 5 0:187 m = 39 1 m = 51 2 Y X 2 3 1:020 0 0 J =4 0 1 0 5 0 0 1:644 3 2 3 0:830 0 0 J = 4 0 0:8 0 5 0 0 0:120 4 X Z 0:172 rc3 = 4 0:025 5 0:003 2 3 2 rc4 = 4 0 0 .

Los coe cientes de las reductoras tienen los valores de 3.40 26 5 0.31 30 6 0.52 10 2 0.3. con la que es posible conocer el par aplicado por los motores a partir de la intensidad que circula.31 30 Tabla 3.4: Caracter sticas electricas de los motores 2 2 Cada motor tiene asociada una constante de par.05 3 1.21) vTOT = (Fv + Fvm R )q De este modo se puede identi car la friccion total como se vera mas adelante.58 4 0.05 2 5.2: Inercia asociada a los ejes de los motores. Kt (Nm=A:) Imax (A:) 1 0.3: Coe cientes de reduccion 2 53 La friccion viscosa de los motores aparece tambien en las ecuaciones dinamicas afectado de un factor R a~adiendose a la friccion viscosa de la n articulacion de forma que el par de friccion viscosa ser a: _ (3.475 5 0.3.3 Parametros del modelo del robot Motor Eje Inercia (kg=cm ) 1 5.47 15 4 0.52 10 3 0. Motor Eje Ratio Reduccion 1 121 2 153 3 105 4 59 5 80 6 50 Tabla 3.235 Tabla 3. .235 6 0.

si u es la se~al de control: n p u (3.2: Caracter stica estatica de friccion teorica de manera que se pueda estimar una caracter stica estatica como la de la gura 3. Para estimar la caracter stica se proporciona al motor una se~al triangular n como orden de par y se estima la velocidad del eje a partir de la lectura del sensor de posicion.3.3 Parametros de friccion Por medio de la realizacion de experimentos es posible estimar algunos parametros de friccion. . 3. que se ha supuesto simetrica.22) = 23 Kt Imax 10 Los valores que se tienen para los distintos ejes son los de la tabla 3.4. 30 20 10 Fc Fv f 0 −10 −20 −30 −4 −3 −2 −1 q_ 0 1 2 3 4 Figura 3. Si se mantiene una frecuencia baja para la se~al n inyectada el par aplicado se usa basicamente en vencer la friccion. El resto de articulaciones se mantiene a su valor de cero. De manera que el par aplicado por los motores vale.54 3 Dinamica del robot RM-10 El valor de la intensidad es proporcional a la se~al de control aplicada a los n servoampli cadores de manera que el valor maximo se corresponde en cada servoampli cador con una intensidad maxima.2. como son la friccion viscosa y la friccion de Coulomb.

la gura 3. que se calculan a partir de los parametros elementales y permiten escribir las ecuaciones del estilo: .3: Caracter stica de friccion experimental del eje 6. Sabiendo esto y suponiendo los niveles de Coulomb simetricos se puede deducir su valor. Se ve que existe un fenomeno de histeresis en la curva. por lo que puede ocurrir que sea demasiado costoso computacionalmente hablando a la hora de realizar simulaciones o realizar control.4 Minimizacion de los parametros Las ecuaciones dinamicas del manipulador comprenden cientos de terminos.4. En la curva obtenida para el eje 5. Mediante manipulacion simbolica es posible obtener un conjunto de parametros ki algo mas reducido. En la gura 3.3. se observa una asimetr a grande en el nivel de Coulomb debido a la fuerza de gravedad que favorece el arranque en uno de los sentido y lo perjudica en otro.3 se puede ver el resultado obtenido para la articulacion 6 del manipulador. 3.4 Minimizacion de los parametros 30 55 20 10 0 −10 −20 −30 −4 −3 −2 −1 q_ 0 1 2 3 4 Figura 3. esto es debido a los efectos de la aceleracion y tambien a que en realidad la friccion viscosa en distinta si se gira en sentido contrario.

56

3 Dinamica del robot RM-10

40

30

20

10

0

−10

−20

−30

−40

−50

−60 −1.5

−1

−0.5

0

q_

0.5

1

1.5

2

Figura 3.4: Caracter stica de friccion experimental del eje 5.
30 20

10

0

−10

−20

−30 −2.5

−2

−1.5

−1

−0.5

q_

0

0.5

1

1.5

2

Figura 3.5: Caracter stica de friccion experimental del eje 4.

3.4 Minimizacion de los parametros = k q +k c q +k c q +::: ... = :::
1 1 2 2 2 3 23 3

57

1

6

(3.23)

El algoritmo se encuentra en el archivo modelo.mws, que aplicado a todos los elementos de la matriz de inercias permite obtener 26 constantes que permiten escribir las ecuaciones de una forma mas compacta. Ademas estas constantes tambien aparecen en los terminos de coriolis y de gravedad. Algunas de estas constantes son:

k k k k k k k k k k k k k k k k k k k k k k

1 2 3 4 5 6 7 8 9

10 12 13 14 15 16 17 18 19 20 21 22 23

= = = = = = = = = = = = = = = = = = = = = =

;Izz
m m m m m
5 6 5

a +m z +m a +m d +m d +m ;Ixx + Iyy
5 1 2 6 5 3 6 6 4 5 5 3 2

y a y z y

6

6

5 6

5 6

z y z y z

6 5 6 5

6

a a a d d

1 2 3

4 3

Iyy 6 ; Ixx 6 m5 a2 a3 + m3 a2 x3 + m6 a2 a3 + m4 a2 a3 ;m5 y52 ; m6 z6 2 ; Izz 5 ; Ixx 6
4 2 4 6 2 4 5 2 4 3 2 5 5 5 2 6 6 2 4 6

2

;m a d ; m a d ; m a d ; m a y ; m a z ;Ixx ; m y ; m z ; Izz ; Iyy
3 4 2 3 3 3 3 3 3 4 3 3 5 3 3 6 3 3 2 2 2 4 2 2 3 2 2 5 2 2 6 2 2 5 4 3 3 3 3 6 4 3 4 3 4

4

m x z +m x d +m d a +m a d +m a d ;m x ; m a ; m a ; m a ; m a ;2 m d a ; 2 m x y ; 2 m d a ; 2 m a d ; 2 m a z 2m a a +2m a a +2m a a +2m a a +2m x a ;2 m y a ; 2 m a d ; 2 m a d ; 2 m a d ; 2 m a z m d a +m d a +m d a +m x z +m z a +m d a m d d +m y z +m y d +m d d +m d z +m d d m y + m z + Iyy ; Izz ; Iyy + Ixx m y ; Iyy + Iyy + m z + Ixx ; Ixx ; Izz + Izz m a + m d + m z + Izz + 2 m z d + Izz + m d + + Ixx + m a + Iyy + m x + m y + m d + m a
4 3 4 4 2 1 5 2 1 3 2 1 6 2 1 2 2 1 3 3 1 5 1 4 4 1 4 6 1 4 4 1 4 5 3 2 4 3 2 3 3 2 2 2 2 3 3 2 6 3 2 6 4 3 3 3 3 3 3 3 5 4 3 4 3 4 4 3 4 5 5 2 6 6 2 6 6 5 5 5 5 2 5 4 6 6 2 6 4 6 5 6 3 2 5 4 2 4 4 2 3 4 4 4 6 4 4 2 4 4 3 2 5 3 3 2 3 3 2 6 4 2 5 3 2

58

3 Dinamica del robot RM-10 = m a + m a + m d + m z + Izz + m x + 2 m z d + + Izz + m d + Ixx + m a + Iyy + m x + Izz + m y + +m d +m a +m a +m a +m a = ;Ixx ; 2 m z d ; Ixx + Iyy + Iyy + m x + m a + + m a + Izz + m x ; m y + m a ; m d + m a ; ; m d + m a + Ixx ; Iyy + m a ; Izz ; m z ; m d + +m a = m a + m d + m z + m z + Ixx + m x + Izz + + 2 m z d + m a + Ixx + m d + m z + Iyy + m a + + m a + m a + Izz + m z + m d + Ixx + m y + +m y +m d +m d +m a +2m z d +m d + +m a +m x +m a +m d +m a
6 3 2 4 2 2 5 4 2 4 4 2 3 2 2 2 4 4 4 6 4 4 2 4 4 3 2 5 3 3 2 2 3 3 2 6 4 2 6 2 2 5 3 2 5 2 2 3 2 2 3 4 4 4 6 6 3 2 2 2 4 2 2 3 2 2 4 3 3 2 3 3 2 5 3 2 5 4 2 5 2 2 6 4 2 6 3 2 5 4 6 2 2 5 4 4 2 4 4 2 4 3 2 4 2 2 5 4 2 6 6 2 4 4 2 3 2 2 2 1 4 4 4 6 1 2 6 4 4 2 2 2 2 4 2 1 2 5 1 2 3 1 2 5 3 3 2 3 3 2 2 3 3 2 5 5 2 6 4 2 5 3 2 4 1 2 3 3 3 4 3 2 6 2 2 1 1 2 5 2 2 6 3 2 3 2 2

k

24

k

25

k

26

Calculando previamente las constantes es posible mininimizar el numero de calculos, obteniendose una distribucion de terminos segun la tabla siguiente: Inercia Coriolis/Centrif Gravedad Total 1 96 296 0 392 2 63 173 18 254 3 51 152 13 216 4 27 128 2 157 5 25 123 4 152 6 6 86 0 92 Tabla 3.5: Distribucion de terminos en las ecuaciones del manipulador. Se ve que es en el apartado de pares de coriolis y centr fugos donde se acumulan la mayor a de las operaciones. Es posible no obstante reducir el numero de terminos mediante comparacion del valor numerico de los coe cientes, usando las velocidades maximas asociadas a cada articulacion. Eliminando los terminos con un coe ciente menor que el 5% del coe ciente mayor. Si se realiza esta simpli cacion la cantidad de terminos pasa a ser:

6: Distribucion de terminos en las ecuaciones del manipulador simplicadas. En el bloque rm10model. En las guras 3. El bloque rm10modelcor. 3.6 se encuentra disponible en Simulink para la relalizacion de simulaciones. Como opciones de simulacion.c se realizan todas las operaciones para calcular las derivadas de los estados de manera que Simulink los integre segun el metodo de integracion elegido. es posible "desactivar" tanto los terminos de Coriolis y centr petos como los efectos de friccion.8 se tiene el diagrama Simulink para la simulacion del sistema en bucle abierto.6 Simulaciones del modelo Con objeto de comprobar si el modelo se comporta segun lo previsto se simula el sistema en bucle abierto.3. En las gura 3.10 se presentan los resultados .7.5 Implementacion informatica El modelo del manipulador de la gura 3.5 Implementacion informatica 59 Inercia Coriolis/Centrif Gravedad Total 1 96 27 0 123 2 63 26 18 107 3 51 17 13 81 4 27 45 2 74 5 25 61 4 90 6 6 12 0 18 Tabla 3. 3. el subsistema se presenta en la gura 3. La implementacion se realiza mediante dos bloques S-Function programados en C.c implementa el bloque C ( _) de la gura 3.9y 3. El par que se aplica a la entrada es el par gravitatorio mas una se~al n senoidal de baja frecuencia con objeto de tener un equilibrio en la posicion inicial y ver los per les de velocidades creados por la friccion.6 para facilitar la compilacion del las funciones.

60 3 Dinamica del robot RM-10 + - M( ) C ( _) F ( _) G( ) 1 s _ 1 s Figura 3.6: Diagrama de bloques asociado al modelo de la simulacion del diagrama 3. .8.

3.8: Diagrama de bloques para la simulacion del modelo en bucle abierto .6 Simulaciones del modelo 61 1 In1 rm10model 1 Out1 2 Out2 rm10modelcor 5 m6 Figura 3.7: Diagrama de bloques Simulink para la simulacion del manipulador vect_gravedad S−Function 0 Mux RM−10 Model_RM10 Par Adicional Mux Posición Velocidad Figura 3.

5 3 3.5 t 3 3.5 1 1.5 4 4.5 4 4.5 2 2.5 5 50 6 0 _ Par Velocidad −50 0 0.5 1 1.5 4 4.5 1 1.5 2 2.5 4 4.5 5 8 6 4 6 2 0 −2 0 0.10: Evolucion de las velocidades de las articulaciones 5 y 6 .5 5 Figura 3.5 t 3 3.5 2 2.62 2 3 Dinamica del robot RM-10 1 5 0 −1 −2 0 0.5 1 1.5 3 3.5 5 Figura 3.9: Evolucion de las posiciones de las articulaciones 5 y 6 50 5 0 _ −50 0 0.5 2 2.

1 Justi cacion En el cap tulo anterior se vio que las ecuaciones que modelan la dinamica del manipulador son: (D(q) + Jm R )q + (C (q q_) + R )q_ + F (q q_) + G(q) = KtIm R 2 2 (4.2 Control lineal 4. 4.2.1 Introduccion En problema fundamental en el control de robot es tratar que el extremo nal de la herramienta siga una trayectoria deseada lo mas elmente posible independientemente de las perturbaciones y condiciones de carga a las que pueda estar sometido el manipulador. 63 .1) Las ecuaciones son claramente no lineales pero ademas estan acopladas por los terminos no diagonales de las matrices D y C.Cap tulo 4 Control de robot RM-10 4. debido a que los terminos diagonales de D y C dependen de la posicion y velocidades de las otras articulaciones.

.1: Esquema de control lineal tipo PD Ahora bien al introducir la dinamica de los motores y reductoras aparecen en los terminos diagonales cantidades afectadas por R lo cual hace que domine la dinamica de los motores frente a la del propio manipulador. En el l mite de esta situacion el sistema se convertir a en 6 sistema lineales desacoplados.2) Los coe cientes Jei y Bei son inercias y fricciones viscosa efectivas del conjunto. u Km6 = Je6 + Be6 _ + 1 1 1 6 6 6 p1 p6 (4. . Si se calcula la funcion de transferencia en bucle cerrado del diagrama de la gura 4.. Con lo que se obtiene un conjunto de sistemas: u Km1 = Je1 + Be1 _ + ... 2 4.1 se obtiene: . El sistema real tendera mas o menos a esta situacion en funcion de los valores concretos de las magnitudes f sicas del manipulador.2. Las inercias dependen aun de la posicion del manipulador. asumiendo que el manipulador se comporta desacopladamente y que los efectos de acoplo debidos a pares gravitatorios y elementos no diagonales de las matrices C y M son perturbaciones al sistema.64 p 4 Control de robot RM-10 _d + - e Kp + Kv s + + Km Je +Be s _d 1 s Figura 4.2 Control tipo PD Aplicando los expuesto anteriormente se pueden dise~ar controladores lin neales para cada articulacion.

4) Si identi cando el denominador de la expresion 4.3 con la expresion 4.33 Tabla 4.06 150 40.6) Km Km Dado que las inercias e caces var an con la posicion del manipulador se adopta como criterio conservador el dise~ar las ganancias para los valores n maximos de las inercias e caces que se alcanzan dentro del espacio de trabajo del manipulador.4 69 0.1 395 22 4 0.7 35 64.3 120 53.9 2666 152 3 20.5) Si se impone = 1 e imponemos un cierto !n a cada articulacion tendremos expresiones para las ganancias del controlador de la forma: Kp = ! Je Kv = 2Je!n .4: !n = KpJKm e r + = Be 2JKv Km ! e n (4.2 Control lineal Art.66 5 0.1 81 1.3) Se pueden dise~ar las constantes Kv y Kp atendiendo a criterios de estan bilidad. Las variaciones son mas importantes en las articulaciones 1 y 2. Las frecuencias !n se eligen segun la velocidad deseada en la articulacion compatible con la saturacion de los actuadores.83 6 0.4.4 3749 249 2 150 35 68.2 150 64. Je !n Km Kp Kv 1 227 30 54. dado que las articulaciones 2 .1: Ganancias del controlador PD 65 GBCPD (s) = r = KpKm + Kv Kms s Je + (Be + Kv Km )s + KpKm 2 (4. Si identi camos el denominador con la ecuacion caracter stica de un sistema de segundo orden: s + 2 !n + ! = 0 2 2 (4.2 33 0. Be (4.

Resultados de simulacion La simulacion del controlador PD se realiza mediante el esquema Simulink de la gura 4.2: Diagrama Simulink para la simulacion de un controlador PD .1 se tienen los valores calculados para cada articulacion.2.25 1] Referencia Ref K RM−10 Articular Kd Model_RM10 Velocidad 0 −50 −50 0 −20 0] Perturbacion adicional Step Figura 4. Las perturbaciones que mas afectan al funcionamiento del controlador son el par gravitatorio y la friccion estatica simulada. En la tabla 4. que dada la ausencia de efecto integral provocan la existencia de errores en regimen permanente. [ref] 1 Error seg Control qact q K Kp [ref] u Posicion 1 Move qd [−1 −pi/6 0.4 −0.66 4 Control de robot RM-10 de la base mueven mucha mas masa se adoptan frecuencias naturales mas bajas.25 0.

5 1 tiempo 1.5 3 1 0.5 2 2.2 4 −0.4 0 0.8 0.5 2 2.6 5 0.2 Control lineal 67 1 6 0.4.5 0 3 2 1 3 −0.5 2 −1 −1.3: Seguimiento en posicion para control tipo PD .5 3 Figura 4.5 1 tiempo 1.4 5 4 6 0.5 0 0.2 0 −0.

03 0.68 4 Control de robot RM-10 0.02 0.04 6 0.015 −0.005 1 −0.5 2 2.5 3 0.01 4 −0.02 0 0.5 1 tiempo 1.01 0.02 e e e 6 5 0.05 0.4: Error de seguimiento en posicion para control tipo PD .01 3 −0.5 3 Figura 4.015 0.01 4 5 0 −0.02 0 0.5 1 tiempo 1.5 2 2.005 e e e 2 3 1 0 2 −0.

5 0 0.5: Se~ales de control para regulador tipo PD n .2 Control lineal 69 1.5 1 tiempo 1.4.5 u u u 5 4 6 0 −0.5 2 2.5 2 2.5 3 2 1 1 0 −1 u u u 2 1 3 −2 −3 3 −4 2 −5 −6 0 0.5 1 tiempo 1.5 6 1 5 0.5 3 Figura 4.5 4 −1 −1.

9 muestran los resultados de aplicar el control PD a los ejes 4. . El controlador se ve degradado debido a las no linealidades presentes. Dado que en estos ejes la gravedad ni las masas puestas en juego son dominantes. Las ganancias usadas en la implementacion son algo menores que las calculadas por la aparicion de oscilaciones.70 4 Control de robot RM-10 Resultados experimentales Las guras de 4. aunque fenomenos de elasticidad estan presentes dado que las reductoras de los ejes 5 y 6 son del tipo harmonic drive. la no linealidad dominante es la caracter stica de friccion.6 a 4. 5 y 6 del manipulador.

8 2 0 −0.4 0.8 tiempo 1 1.6 0.1 −0.6 0.3 0.3 0.4 −0.1 0 0 0.5 0.7 0.2 0.6 1.2 1.6 0.1 0 0 0.6 1.6 1.8 0.2 1.5 −0.2 1.1 0. 5 y 6 .2 0.2 0.4 0.4 0.8 tiempo 1 1.8 2 Figura 4.2 3 3 r −0.4 1.6 0 0.4 1.2 0.7 0.6 0.4.4 1.5 1 1 r 0.8 tiempo 1 1.2 Control lineal 71 0.2 0.4 2 2 r 0.4 0.6: Seguimiento en posicion para control tipo PD en los ejes 4.3 −0.6 0.8 2 0.

2 1.6 0.2 0.4 0.9 0.6 1.2 0 −0.8 2 Figura 4.8 −1 −1.4 0.2 −0.8 0.4 qd3 −0.2 0.2 0 0.6 1.2 0.4 0.5 qd2 0.8 0.4 1.6 0.8 2 0 −0.6 1.2 0.7: Velocidades para control tipo PD en los ejes 4.2 1 0.8 tiempo 1 1.8 2 0.4 1.6 0.8 tiempo 1 1.2 0.6 qd1 0.4 1.6 0.6 −0.4 0.3 0.2 1.1 0 −0.72 4 Control de robot RM-10 1.7 0. 5 y 6 .2 1.1 0 0.2 0 0.8 tiempo 1 1.4 0.

4.2 Control lineal
x 10 8
−3

73

7

6

5

1

e

4

3

2

1

0 0 0.2 0.4 0.6 0.8

tiempo
1

1.2

1.4

1.6

1.8

2

x 10 6

−3

5

4

2

e

3

2

1

0 0 0.2 0.4 0.6 0.8

tiempo
1

1.2

1.4

1.6

1.8

2

0

−0.002

−0.004

−0.006

−0.008

e
−0.01 −0.012 −0.014 −0.016 −0.018 0 0.2 0.4 0.6 0.8

3

tiempo

1

1.2

1.4

1.6

1.8

2

Figura 4.8: Error de seguimiento en posicion para control tipo PD en los ejes 4, 5 y 6

74

4 Control de robot RM-10

0.2

0.15

0.1

u
0.05 0 −0.05 0 0.2 0.2 0.4 0.6 0.8

1

tiempo
1

1.2

1.4

1.6

1.8

2

0.15

0.1

0.05

0

u
−0.05 −0.1 −0.15 −0.2 −0.25 0 0 0.2 0.4 0.6 0.8

2

tiempo
1

1.2

1.4

1.6

1.8

2

−0.05

−0.1

−0.15

−0.2

3

u

−0.25

−0.3

−0.35

−0.4

−0.45

−0.5 0 0.2 0.4 0.6 0.8

tiempo
1

1.2

1.4

1.6

1.8

2

Figura 4.9: Se~al de control para control tipo PD en los ejes 4, 5 y 6 n

4.2 Control lineal

75

4.2.3 Control tipo PID
Con objeto de eliminar errores en regimen permanente se puede a~adir un n efecto integral tal. La funcion de transferencia en bucle cerrado para el conjunto resulta ser:

GBCPID (s) =

r

K s s + K Ki = s J + (BpKmK+ Kv)Km+ K K ms + K K e e + v Km s p m m i
2 3 2

(4.7)

Aplicando el criterio de estabilidad de Routh-Hurwitz al denominador de la expresion anterior se obtiene una cota para la ganancia Ki :

Ki < Kp(B + Km Kv ) J

(4.8)

Las cotas para las ganancias Ki que se obtienen a partir de la tabla 4.1 tienen los siguientes valores: Articulacion Ki 1 224960 2 186680 3 27690 4 19510 5 20950 6 10060 Tabla 4.2: Cotas maximas para las ganancias Ki

Resultados de simulacion
La simulacion del controlador PID se realiza mediante el esquema Simulink de la gura 4.10. En las guras se observa que las perturbaciones al funcionamiento del sistema controlado son atenuadas mas rapidamente que en el caso PD, consiguiendo una reduccion de los errores en regimen permanente. Aunque tambien se observa que los transitorios se han empeorado apareciendo comportamientos oscilatorios.

25 0.3 Control por Par Calculado En el control lineal desacoplado no se consideran las interacciones que existen entre las distintas articulaciones asumiendo que la planta es completamente lineal y considerando los efectos de las no linealidades como la friccion y la gravedad como perturbaciones que son atenuadas por la realimentacion del controlador. y un segundo bucle externo que puede ser un control lineal que estabiliza el sistema resultante.9) . Es por ello que en las tecnicas de control se empiece a tener en cuenta la naturaleza no lineal del modelo del robot.25 1] Referencia Ref K RM−10 Articular Kd Model_RM10 Velocidad [0 −50 −50 0 −20 0] Perturbacion adicional Step Figura 4. En el caso de que los robot no sean de accionamiento indirecto el acoplamiento entre las distintas articulaciones cobra importancia. Un primer bucle mas interno que linealiza y desacopla el sistema. Logicamente las prestaciones que se podran conseguir con un control lineal estan limitadas.4 −0. La tecnica del par calculado basicamente consiste en el uso de dos bucles de realimentacion.76 1 [ref] 1 s qact q 4 Control de robot RM-10 K Ki Control [ref] K u Posicion Kp 1 Move qd [−1 −pi/6 0.10: Diagrama Simulink para la simulacion de un controlador PID 4. Si se aplica un par de la forma: = F ( _) + C ( _) + G( ) + M ( ) 0 (4. aunque en muchos robots industriales de accionamiento indirecto porporcionan las prestaciones buscadas.

5 2 2.5 1 tiempo 1.3 Control por Par Calculado 77 1.2 0 −0.5 1 tiempo 1.5 0 3 2 1 3 −0.5 2 2.4.8 0.5 3 1 0.5 2 −1 −1.6 5 5 4 6 0.2 1 6 0.4 0.4 0 0.11: Seguimiento en posicion para control tipo PID .2 4 −0.5 0 0.5 3 Figura 4.

78 4 Control de robot RM-10 14 x 10 −3 12 6 10 8 6 5 e e e 5 4 6 4 2 0 −2 −4 4 −6 0 x 10 −3 0.5 2 2.12: Error de seguimiento en posicion para control tipo PID .5 1 tiempo 1.5 3 Figura 4.5 3 6 4 3 2 1 3 1 2 e e e 0 −2 −4 −6 0 0.5 2 2.5 1 tiempo 1.

3 Control por Par Calculado 79 2 1.5 3 Figura 4.13: Se~ales de control para regulador tipo PID n .4.5 5 u u u 4 5 6 0 −0.5 1 tiempo 1.5 2 2.5 1 6 0.5 4 −1 −1.5 3 3 2 1 1 0 −1 u u u 2 1 3 −2 3 −3 −4 2 −5 −6 −7 0 0.5 2 2.5 1 tiempo 1.5 0 0.

q_ _ (4. q e = q_d .11 con la ecuacion 4. Bucle externo tipo PD En este caso se elige como ley de control para el bucle externo la ley: 0=q _ d + K p e + Kv e (4.13) .10 se la obtiene la ecuacion diferencial para el error: e + Kv e + Kpe = 0 _ (4. tal y como se ve en la gura 4.14: Bucle de linealizacion y desacoplo Igualando con la ecuacion dinamica del modelo nos queda: 0= (4.11) donde: e = qd . Si igualamos la ecuacion 4.80 4 Control de robot RM-10 0 M( ) + + _ C ( _) + F ( _) + G( ) Figura 4.15.10) El doble integrador resultante se puede estabilizar con controladores lineales como por ejemplo PD o PID.12) y las matrices Kv y Kp son matrices diagonales.

16.14) Normalmente se ja a 1 para tener una respuesta no sobreoscilante y !n atendiendo a criterios de resonancia estructural y saturacion de los actuadores. En un primer caso se puede usar un bucle de .3 Control por Par Calculado 0 81 1 s 1 s _ qr qdr qddr Controlador Lineal Cont 0 M( ) + + _ C ( _) + F ( _) + G( ) Figura 4. Normal mente son mas bajas en las articulaciones de la base y mas altas en las articulaciones de la mu~eca. Identi cando la ecuacion del error con los parametros caracter sticos de un sistema de segundo orden se tiene: Kp = !n 2 Kv = 2 !n (4.15: Esquema completo de control por par calculado Con Kv y Kp se puede modi car la dinamica del error y obtener el comportamiento deseado. n Resultados de simulacion La simulacion del controlador por par calculado se realiza mediante el esquema Simulink de la gura 4.4.

!n Kp Kv 1 30 900 60 2 35 1225 70 3 35 1225 70 4 120 81 1.16: Diagrama Simulink para la simulacion de un controlador Par Calculado Art.33 Tabla 4.66 5 150 69 0.3 .4 −0.3: Ganancias del bucle externo tipo PD realimentacion tipo PD.83 6 150 33 0.5 1 Referencia Ref qdd QD Articular PID Velocidad Model_RM10 [0 100 100 0 30 0] vect_friccion S−Function In1 Out1 In2 Coriolis vect_gravedad S−Function3 Figura 4. cuyas constantes se dise~an segun la tabla siguiente n 4.25 0.82 [ref] 1 4 Control de robot RM-10 [ref] qact q Qref Q Control u Out1 1 Move qd QDref mat_inercia S−Function1 K R Kt Imax RM−10 Posicion 5 −pi/4 −0.

5 0 0.2 4 −0.5 2 2.17: Seguimiento en posicion para control tipo Par Calculado .2 0 −0.6 5 0.8 0.5 3 tiempo 1 0.5 1 1.4 5 4 6 0.4.5 1 tiempo 1.3 Control por Par Calculado 83 1 6 0.5 2 2.5 0 3 2 1 3 −0.5 3 Figura 4.5 2 −1 −1.4 0 0.

5 2 2.5 1 tiempo 1.5 1 0 2 −0.5 2 2.84 4 Control de robot RM-10 12 x 10 −3 6 10 8 6 5 4 e e e 5 4 6 2 0 −2 −4 −6 4 −8 0 x 10 −3 0.5 3 Figura 4.5 −1 0 0.5 1 tiempo 1.18: Error de seguimiento en posicion para control tipo Par Calculado .5 3 1 e e e 2 1 3 0.5 3 2 1.

5 4 −1 −1.5 1 tiempo 1.5 u u u 5 4 6 0 −0.5 1 tiempo 1.5 6 1 5 0.4.19: Se~ales de control para regulador tipo Par Calculado n .5 2 2.3 Control por Par Calculado 85 1.5 3 2 1 1 0 −1 u u u 2 1 3 −2 −3 3 −4 2 −5 −6 0 0.5 0 0.5 3 Figura 4.5 2 2.

86 4 Control de robot RM-10 Resultados experimentales En las guras 4. A la vista de los resultados obtenidos se ve que el factor principal que empeora el comportamiento del sistema es la caracter stica de friccion por lo que usando modelos de friccion mas completos ser a posible mejorar las prestaciones del sistema. . Dado que el acoplamiento entre estas ultimas articulaciones es bastante reducido no solo ya por la presencia de reductoras si no tambien por el hecho de que los efectos gravitatorios y acoplamientos de la matriz de inercias son muy reducidos.20 a 4. la mejora con respecto al controlador lineal no es muy signi cativa.21 se representan las se~ales obtenidas en la experin mentacion del algoritmo de control de par calculado.

4 0.3 1 1 r −0.5 tiempo 2 2.1 0 0 0.4 2 2 r 0.3 0.1 0 0 0.2 −0.2 0.6 0.5 3 3.5 3 3.7 0.3 0. 5 y 6 .5 1 1.1 −0.5 4 0.5 3 3.5 4 Figura 4.5 0.2 0.5 4 0.6 0.4 −0.7 0 0.3 Control por Par Calculado 0 87 −0.5 1 1.5 1 1.7 0.5 3 3 r 0.5 tiempo 2 2.6 −0.4.20: Seguimiento en posicion para control tipo par calculado en los ejes 4.5 tiempo 2 2.8 0.5 −0.

6 qd2 0.5 1 1.5 tiempo 2 2.6 qd3 0.2 0.5 3 3.5 3 3.5 4 1 0.21: Velocidades para control tipo par calculado en los ejes 4.2 0.2 0 −0.8 −1 0 1.5 3 3.2 qd1 −0.88 4 Control de robot RM-10 0.5 1 1.4 0.4 −0.2 0 1.8 0.2 0 −0.8 0.5 4 Figura 4.4 0.2 0 0.2 0 −0.5 4 1 0.5 1 1. 5 y 6 .5 tiempo 2 2.6 −0.4 0.5 tiempo 2 2.

01 0.3 Control por Par Calculado x 10 8 −3 89 6 4 2 1 e 0 −2 −4 −6 0 0.5 tiempo 2 2.5 3 3.5 1 1.5 4 0.01 −0.01 −0.5 tiempo 2 2.5 1 1. 5 y 6 .01 0.5 4 Figura 4.015 0.4.5 3 3.005 0 3 e −0.5 1 1.5 3 3.005 0 2 e −0.015 −0.02 0 0.005 −0.5 4 0.22: Error de seguimiento en posicion para control tipo par calculado en los ejes 4.02 0 0.015 −0.005 −0.5 tiempo 2 2.

5 1 tiempo 2 2.12 0 0.5 1 1.5 1 1.05 −0.06 −0.15 0.15 0.5 2 tiempo 2 2.5 3 3.5 4 Figura 4.2 0.5 3 tiempo 2 2.5 3 3.05 0 0.1 u 0.02 4 Control de robot RM-10 0 −0.05 0 −0.25 0.5 3 3.1 −0.2 0. 5 n y6 .5 4 0.23: Se~al de control para control tipo par calculado en los ejes 4.1 u 0.05 0 −0.5 4 0.02 −0.04 u −0.1 0 0.08 −0.90 0.5 1 1.

Todas las entradas y salidas estan optoaisladas y realizan una conversion de tension de 0-24V a niveles TTL. as como captar se~ales n de alarma de distintas fuentes. Vcc R 1 2 IN R R 3 IC 2 D OUT 1 IC 1 Figura A. se encuentra situada f sicamente en la parte posterior el armario controlador de System 91 . en el armario y la tarjeta controladora respectivamente.2 y A.1 respectivamente.1: Etapa de entrada digital de la tarjeta E/S digitales La tarjeta dispone de 8 salidas digitales y 8 entradas digitales.Apendice A Tarjeta de E/S digital Esta tarjeta permite a la tarjeta controladora interactuar con la parte electromecanica del armario de control de System Robot. La con guracion de las etapas de salida y entrada se puede ver en las guras A.

y por parte de la tarjeta controladora dSPACE se precisa de +5V para alimentar toda la logica TTL.92 Vcc A Tarjeta de E/S digital OUT D IN R 1 2 1 OUT D 1 2 IC 1 Figura A.1 y A.2: Etapa de salida digital de la tarjeta E/S digitales Entrada 1 2 3 4 5 6 7 8 Descripcion Pin dSPACE Emergencia General P2B18 Contactor de frenos electricos cerrado P2A18 Sobrecalentamiento fuente alimentacion de servos P2B02 Resistencia de disipacion fundida P2A02 Perdida de fase en fuente de alimentacion servos P2B19 No usada P2A19 No usada P2B03 No usada P2A03 Tabla A. En las tablas A. El conjunto de todas las se~ales se recoge en un solo mazo de cables n que tiene como destino el par de conectores de se~ales digitales de la tarjeta n controladora dSPACE.1: Conexionado entradas digitales Robot. .2 se encuentra detallada el uso de cada se~al y la numeracion usada para el conexionado. n La tarjeta precisa de alimentacion de +24V por parte del armario de System-Robot para poder activar los reles.

se~ales control dSPACE-SystemRobot n No Usada No Usada No Usada No Usada Descripcion Pin dSPACE P2B20 P2A20 P2B04 P2A04 P2B21 P2A21 P2B5 P2A5 Tabla A.A Tarjeta de E/S digital 93 Salida 1 2 3 4 5 6 7 8 Habilitacion de los servo ampli cadores Cerrar contactor de frenos electromecanicos Limitacion de se~al de control al 10% n Conmut.2: Conexionado salidas digitales .

94 A Tarjeta de E/S digital .

Filtrado/ Ajuste de fase Conversión RE Amplificación OptoAislación Conmutación Aislación Analógica Figura B. La tarjeta incorpora algunas funciones adicionales para facilitar la incorporacion del nuevo controlador.1. El diagrama de bloques funcional de la tarjeta se puede en la gura B.1: Diagrama de bloques de la tarjeta CRE En la gura B.Apendice B Tarjeta de conversion resolver-encoder La tarjeta de conversion resolver a encoder (CRE) permite la lectura de un sensor de posicion tipo resolver.1 la funcion de cada uno de sus 95 .2 se representa la disposicion f sica de los distintos conectores que dispone la tarjeta y en la tabla B. emulando las se~ales de un encoder inn cremental de 1024 pulso por vuelta.

P5: Conector de alimentacion de lado del PC. P1/P2: Conectores de entrada y salida de resolver. P3: Conector de alimentacion del lado del armario de control. 2. 1 1 2 3 4 2 B Tarjeta de conversion resolver-encoder 3 4 5 6 1 2 P4 P3 P5 3 4 P7 P1 P2 1 1 2 •• •• •• P6 2 Figura B. n . 5. Permiten la lectura de las se~ales del resolver y de la referencia. Es necesario tener dos n conectores ya que no se puede interrumpir las se~ales de resolver que n llegan al servoampli cador debido a que este las necesita para la logica de conmutacion de los puentes de transistores de potencia. P7: Entrada analogica proveniente del la tarjeta controladora. 6. 3. B. P4: Conmutacion de se~ales analogica y salida de la misma. P6: Conector de encoder incremental.96 pines.1 Filtros y Ajuste de fase Como etapa previa a la conversion de las se~ales de los resolvers se ltran n las se~ales seno y coseno mediante ltros pasivos RC. n 4.2: Conectores presentes en la tarjeta CRE La funcion de cada conector es la que sigue: 1.

B.1 Filtros y Ajuste de fase 97 P1/P2-1 Se~al seno (+) n P1/P2-2 Resistencia NTC Motor (+) P1/P2-3 Se~al coseno (-) n P1/P2-4 Referecia resolver (+) P1/P2-5 Resistencia NTC Motor (-) P1/P2-6 Se~al seno (-) n P1/P2-7 Pantalla P1/P2-8 Se~al coseno (+) n P1/P2-9 Referencia resolver (-) P3-1 Tension Alimentacion -12V armario P3-2/3 GND Alimentacion armario P3-4 Tension Alimentacion +12V P4-1 Alimentacion Rele Conmutacion +24V P4-2 Alimentacion Rele Conmutacion GND P4-3 Se~al de control a servoampli cador (+) n P4-4 Se~al de control a servoampli cador (-) n P4-5 Se~al de control de control System Robot (-) n P4-6 Se~al de control de control System Robot (+) n P5-1 Tension Alimentacion -12V PC P5-2/3 GND Alimentacion PC P5-4 Tension Alimentacion +12V PC P6-1 Tension Alimentacion +5V PC P6-2 GND Alimentacion PC P6-3 Encoder incremental pulso A P6-4 Encoder incremental pulso B P6-5/6 No usadas P7-1 Se~al de control tarjeta controladora dSPACE (-) n P7-2 Se~al de control tarjeta controladora dSPACE (+) n Tabla B.1: Conexiones de la tarjeta CRE .

3: Esquema del ltro activo para la se~al de referencia n La funcion de transferencia del circuito de la gura B.5. Esto es necesario ya que el circuito de conversion de la etapa siguiente esta dise~ado para tensiones de entrada senoidales de 2VRMS como maximo n y la se~al de referencia que suministra el servoampli cador es de 10VPP que n equivale a 3:53VRMS por lo que el ltro ademas debe atenuar esta se~al. Para ello se introduce en la se~al de referencia un ltro activo de primer orden basado en n ampli cador operacional.1 resulta ser: 1 Vo = . R (B. esto en realidad no es as n ya que las bobinas de los resolver distan de ser ideales y como resultado las se~ales inducidas seno y coseno presentan un desfase electrico apreciable. n Este desfase es necesario compensarlo ya que resulta excesivo para que la conversion de la etapa siguiente funcione correctamente. El ltro permite ajustar tambien su ganancia una vez introducido el desfase. Este n problema no aparece con las dos senoides inducidas ya que el resolver tiene una relacion de transformacion de 0. R2 R1 + C1 IC1 Figura B. que permite ajustar el desfase introducido por el resolver.1) Vi R 1+R C s 2 1 2 2 Por lo que el desfase solo depende de R y una vez jada esta se puede ajustar la ganancia mediante la resistencia R 2 1 .98 B Tarjeta de conversion resolver-encoder Idealmente las se~ales inducidas en los devanados jos de los resolvers n estan en fase con las se~al aplicada de referencia.

Basicamente el circuito calcula una se~al proporcional al error entre el angulo real y el n angulo almacenado en un contador reversible. n y el seno y coseno inducidos. REF SENO+ SENOCOSENO+ COSENOÁNGULO θ MULTIPLICADOR -SIN( θ-φ) DETECTOR DE FASE + INTEGRADOR VEL ÁNGULO φ NMC A B NM CLKOUT LÓGICA DE DECODIF . Uno de los formatos es tipo encoder incremental con niveles de tension TTL. B. CONTADOR REVERSIBLE OSCILADOR CONTROLADO POR TENSIÓN DIR CS LATCH SCLK DATOS INTERFACE SERIE Figura B.4: Funcionamiento interno del circuito integrado AD2S90 El circuito realiza la conversion con una tecnica de tracking.B. Este circuito permite realizar la conversion a partir de las se~ales de referencia.2 Conversion resolver a encoder 99 B. y a partir de este error incrementa o decrementa el contador hasta reducir el error a cero. dando la posicion de salida en dos formatos digitales con una resolucion de 12 bits .2 Conversion resolver a encoder La coversion se basa en el circuito integrado AD2S90 de Analog Devices. el otro formato disponible es una linea serie. .3 Ampli cador adaptador Esta etapa es necesaria ya que las salidas de encoder incremental del AD2S90 no son capaces de suministrar su ciente corriente para activar la etapa de optoaislamiento.

100

B Tarjeta de conversion resolver-encoder

Vel. Maxima (/s) Reduccion Frec. Maxima (kHz.) 90 121 30.976 90 153 39.168 90 105 26.880 360 59 60.416 200 80 45.551 360 50 51.2 Tabla B.2: Frecuencias maximas de conmutacion de las se~ales encoder n La solucion adoptada consiste en intercalar ampli cadores operacionales en modo de seguidor de tension de manera que estos si sean capaces de suministrar la intensidad requerida a la etapa siguiente. El requisito fundamental de esta etapa es que los ampli cadores elegidos sean capaces de conmutar lo su cientemente rapido como para que las se~ales de emulacion de encoder n no se vean distorsionadas. Las frecuencias maximas que deben tolerar se pueden estimar a partir de las velocidades maximas de las articulaciones proporcionadas por el fabricante System Robot. Teniendo en cuenta las reductoras y que el encoder da 1024 pulsos por vuelta se obtienen la tabla de frecuencias maximas B.2

B.4 Optoaislacion de se~ales digitales n
Para evitar cualquier peligro de derivacion de tensiones peligrosas hacia la tarjeta controladora, las se~ales A y B de los encoders se aslan debidamente n mediante el uso de optoacopladores. Los optoacopladores seleccionados disponen de salida Smitch-Trigger permitiendo la alta velocidad de conmutacion necesaria

B.5 Se~ales de control n
Las se~ales de control generadas por los algoritmos de control pasan por esta n tarjeta donde la etapa de aislamiento analogico se encarga de proteger a la tarjeta controladora.

B.6 Esquemas electricos

101

El aislamiento de las se~ales analogicas esta basado en el circuito integran do ISO122P de la rma Burr-Brown que permite aislar se~ales con un rango n 10V y con un ancho de banda de 50kHz. El esquema de conexion se puede ver en los esquemas electricos que se adjuntan. La tarjeta incorpora tambien un rele que conmmuta entre las se~ales de n control proporcionadas por el controlador de System Robot y las generadas por el nuevo controlador dSPACE. La conmutacion se realiza mediante la aplicacion de +24V en las bornas de conexion del conector P4-1/2.

B.6 Esquemas electricos

102

B Tarjeta de conversion resolver-encoder

Apendice C Esquemas electricos de electronica de potencia 103 .

104 C Esquemas electricos de electronica de potencia .

Apendice D Codigo fuente funciones Simulink 105 .

gravedad y friccin Los efectos de coriolis se aaden mediante el archivo S-Function "rm10modelcor.106 /* * * * * * * * */ #define S_FUNCTION_NAME rm10model #define S_FUNCTION_LEVEL 2 #include <math.h> #include "simstruc.c".h" D Codigo fuente funciones Simulink Modelo para el robot manipulador RM-10 Implementa la matriz de inercias. autor: Carlos Perez Fernandez fecha: 30-4-99 /* Variables del espacio de trabajo */ #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define mE Jm1 Jm2 Jm3 Jm4 Jm5 Jm6 a1 a2 a3 d3 d4 x1 z1 x2 z2 x3 y3 z3 z4 y5 z6 Izz1 Ixx2 rwork rwork rwork rwork rwork 0] 1] 2] 3] 4] /* Inercias motores */ rwork 5] rwork 6] rwork 7] rwork 8] rwork 9] rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork 10] 11] 12] 13] 14] 15] 16] 17] 18] 19] 20] 21] 22] /* Distancias */ /* Inercias */ rwork 23] .

D Codigo fuente funciones Simulink #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define Iyy2 Izz2 Ixx3 Iyy3 Izz3 Ixx4 Iyy4 Izz4 Ixx5 Iyy5 Izz5 Ixx6 Iyy6 Izz6 m1 m2 m3 m4 m5 m6 s2 c2 s3 c3 s4 c4 s5 c5 s6 c6 s23 c23 a1_2 a2_2 a3_2 d3_2 d4_2 x2_2 x3_2 y3_2 z4_2 y5_2 z6_2 bm1 rwork 24] rwork 25] rwork 26] rwork 27] rwork 28] rwork 29] rwork 30] rwork 31] rwork 32] rwork 33] rwork 34] rwork 35] rwork 36] rwork 37] rwork 38] /* Masas */ rwork 39] rwork 40] rwork 41] rwork 42] rwork 43] rwork 44] /* Senos y cosenos */ rwork 45] rwork 46] rwork 47] rwork 48] rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork 49] 50] 51] 52] 53] 54] 55] 56] /* Distancias al cuadrado*/ 57] 58] 59] 60] 61] 62] 63] 64] 65] 66] 107 rwork 67] /* Friccin viscosa */ .

2) /* Habilitar friccin */ .1) /* Aceleracion de la gravedad */ ENABLEFRIC(S) ssGetSFcnParam(S.0) /* Posicion inicial del robot del robot*/ G(S) ssGetSFcnParam(S.108 #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define bm2 bm3 bm4 bm5 bm6 Red1 Red2 Red3 Red4 Red5 Red6 FC1 FC2 FC3 FC4 FC5 FC6 Coulomb(i) rwork 68] rwork 69] rwork 70] rwork 71] rwork 72] rwork 73] rwork 74] rwork 75] rwork 76] rwork 77] rwork 78] D Codigo fuente funciones Simulink /* Reductoras */ rwork 79] /* Friccin de Coulomb */ rwork 80] rwork 81] rwork 82] rwork 83] rwork 84] rwork i+79] #define yant(i) #define ffric(i) #define FLAG_INIT #define rwork 85+i] rwork 91+i] iwork 0] FLAG_DEBUG(i) rwork 97+i] /* Entradas y estados */ #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define M(i) q1 q2 q3 q4 q5 q6 qp1 qp2 qp3 qp4 qp5 qp6 (*uPtrs i]) xCont xCont xCont xCont xCont xCont 0] 1] 2] 3] 4] 5] xCont 6] xCont 7] xCont 8] xCont 9] xCont 10] xCont 11] POS_INI(S) ssGetSFcnParam(S.

6) ssSetOutputPortWidth(S. 0) if (!ssSetNumOutputPorts(S.Inicializa muestreos */ static void mdlInitializeSampleTimes(SimStruct *S) { ssSetSampleTime(S. 0. 0. 0.0) } /*Automatico segun bloque anterior*/ #define MDL_INITIALIZE_CONDITIONS /* * mdlInitializeConditions . 103) S. 0.3)) return /*Salidas*/ ssSetOutputPortWidth(S. 6) /*Posicion*/ ssSetOutputPortWidth(S. SS_OPTION_EXCEPTION_FREE_CODE) } /* * mdlInitializeSampleTimes . 0) /*Entradas*/ if (!ssSetNumInputPorts(S. 1) 0) /* Numero de muestreos */ /* Vector de numeros reales */ /* Vector de numeros enteros */ /* Vector de punteros */ /*Si faltan parametros se da mensaje */ ssSetNumModes( S. 1) S. 0) ssSetOptions(S. 0. 3) if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { return } ssSetNumSampleTimes( ssSetNumRWork( ssSetNumIWork( ssSetNumPWork( S. 12) S. CONTINUOUS_SAMPLE_TIME) ssSetOffsetTime(S. S. 1. 0.D Codigo fuente funciones Simulink 109 static void mdlInitializeSizes(SimStruct *S) { ssSetNumContStates( ssSetNumDiscStates( S. 6) ssSetInputPortDirectFeedThrough(S. 1)) return ssSetInputPortWidth(S.Inicializa estados y parametros del robot * . 6) /*Velocidad*/ /*Velocidad*/ ssSetNumSFcnParams(S. 2. 0) ssSetNumNonsampledZCs(S.

040 0.00014 0.260 0.0839 -0.000041 0.710 0.00044 0.110 */ D Codigo fuente funciones Simulink static void mdlInitializeConditions(SimStruct *S) { int i real_T int_T real_T real_T *rwork = ssGetRWork(S) *iwork = ssGetIWork(S) *xCont = ssGetContStates(S) *posini = mxGetPr(POS_INI(S)) /* Puntero a Vector POS_INI */ /* Posiciones y velocidades iniciales */ for (i=0 i<ssGetNumContStates(S) i++) xCont i] = 0.2286 /*Distancias constantes*/ /*Reductoras*/ /* Flag de inicializacin a cero */ /* Masa opcional garra Adicional */ /*Inercias motores*/ /*Centros de masa*/ .0 for(i=0 i<6 i++) xCont i] = posini i] FLAG_INIT mE Jm1 Jm2 Jm3 Jm4 Jm5 Jm6 Red1 Red2 Red3 Red4 Red5 Red6 a1 a2 a3 d3 d4 x1 z1 x2 = = = = = = = = = = = = = = = = = = = = = = 0 0 0.000017 0.0832 0.150 0.650 0.000017 121 153 105 59 80 50 0.00044 0.

0100 0.0236 0.0246 0.65 51.0086 0.1717 0.1202 0.2656 3.5202 1.9982 1.0317 0.0100 0.0027 -0.36 5.0201 0.0050 38.1945 0.5 /*Masas*/ /*Inercias*/ 111 /*Friccin viscosa total*/ .89 7.6442 0.8301 0.D Codigo fuente funciones Simulink z2 x3 y3 z3 z4 y5 z6 Izz1 Ixx2 Iyy2 Izz2 Ixx3 Iyy3 Izz3 Ixx4 Iyy4 Izz4 Ixx5 Iyy5 Izz5 Ixx6 Iyy6 Izz6 m1 m2 m3 m4 m5 m6 bm1 bm2 bm3 bm4 bm5 bm6 = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = 0.4349 3.7995 0.00 40 30 20 10 6 4.0198 0.800 84.1876 0.1 33.050 1.1325 0.

j real_T *y real_T *rwork = ssGetRWork(S) int_T *iwork = ssGetIWork(S) real_T *xCont = ssGetContStates(S) real_T g = mxGetPr(G(S)) 0] .112 FC1 FC2 FC3 FC4 FC5 FC6 = = = = = = 60 50 40 30 24 14 D Codigo fuente funciones Simulink /*Friccion coulomb*/ /* Distancias al cuadrado a1_2 a2_2 a3_2 d3_2 d4_2 x2_2 x3_2 y3_2 z4_2 y5_2 z6_2 = = = = = = = = = = = a1*a1 a2*a2 a3*a3 d3*d3 d4*d4 x2*x2 x3*x3 y3*y3 z4*z4 y5*y5 z6*z6 */ for(i=0 i<6 i++) { yant(i)=0.0 } } /* * mdlOutputs .0 ffric(i)=1. int_T tid) { int i.Calcula las salidas * */ static void mdlOutputs(SimStruct *S.

s2*s3 y=ssGetOutputPortRealSignal(S.i) for(j=0 j<6 j++) { y j] = xCont (i*6)+j] } } 113 /* Calculo de senos y cosenos s2 = sin(q2) s3 = sin(q3) s4 = sin(q4) s5 = sin(q5) s6 = sin(q6) c2 c3 c4 c5 c6 = = = = = cos(q2) cos(q3) cos(q4) cos(q5) cos(q6) */ /*----------------------------------*/ s23 = s2*c3 + s3*c2 c23 = c2*c3 .0 } for (i=0 i<2 i++) { y=ssGetOutputPortRealSignal(S.2) for (i=0 i<6 i++) y i] = FLAG_DEBUG(i) } #define MDL_DERIVATIVES /* .D Codigo fuente funciones Simulink /* Calcula salida a partir de Estados */ if (mxGetPr(ENABLEFRIC(S)) 0] == 1) { for (i=0 i<6 i++) if (ffric(i)==1) xCont i+6]=0.

k12. fP_2. R3_2. R4_3. k9.sig 6] s2_2. fP_6 fG 6] fricc 6]. R5_5. k2. s23_2. R2_1. k24. R3_3. k22. R5_4.1) .0) y=ssGetOutputPortRealSignal(S. R6_4. k11.114 D Codigo fuente funciones Simulink * mdlDerivatives . c5_2. k13. c3_2. c6_2. R6_1. z_5. k16. k20. z_3. k25. z_2. fP_4. s4_2. R6_3. forceac static real_T k0. R4_4. R2_2. R5_3. k26 static real_T m 7] 7] static real_T R1_1. R3_1. k6. k21. k7. z_6 x 6] fP_1. c4_2. R4_1. k17. R4_2. R6_2. k1. R6_6 real_T real_T real_T real_T real_T real_T real_T f 6] z_1. flagfric 6] InputRealPtrsType uPtrs real_T *y real_T *rwork = ssGetRWork(S) int_T *iwork = ssGetIWork(S) real_T *xCont = ssGetContStates(S) real_T *dx = ssGetdX(S) real_T Enfric = mxGetPr(ENABLEFRIC(S)) 0] /* Declaraciones */ /*----------------------------------*/ static real_T g. s5_2. z_4. k15. k18. k23.compute the derivatives * */ static void mdlDerivatives(SimStruct *S) { int i. k3. c2_2. fP_3. R6_5. c23_2 /*Puntero a vector de pares*/ uPtrs = ssGetInputPortRealSignalPtrs(S. k8. R5_2. k14. k19.j. s3_2. k4. k10. s6_2. k5. R5_1. fP_5.

D Codigo fuente funciones Simulink 115 if (FLAG_INIT == 0) { /* Calculo de parametros */ /*----------------------------------*/ g k1 k2 k3 k4 k5 k6 k7 k8 = mxGetPr(G(S)) 0] = -Izz6 = m5*y5*a1+m6*z6*a1 = m5*y5*d4+m6*z6*d4 = -m5*y5*d3-m6*z6*d3 = m5*y5*a3+m6*z6*a3 = m5*a2*y5+m6*a2*z6 = -Ixx2+Iyy2 = -Ixx6+Iyy6 k9 = m3*a2*x3+m6*a2*a3+m4*a2*a3+m5*a2*a3 k10 = -m5*y5_2-m6*z6_2-Izz5-Ixx6 k11 = 2*m4*a3*a1+2*m5*a1*a3+2*m3*x3*a1+2*m6*a1*a3 k12 = -m5*a2*d4-m6*a2*d4-m3*a2*y3-m4*a2*z4-m4*a2*d4 k13 = -Ixx5-Izz4-Iyy6-m5*y5_2-m6*z6_2 k14 = m3*x3*d3+m3*x3*z3+m4*d3*a3+m5*a3*d3+m6*a3*d3 k15 = 2*m3*a2*a1+2*m2*x2*a1+2*m6*a2*a1+2*m4*a2*a1+2*m5*a2*a1 k16 = -2*m4*a1*d4-2*m4*a1*z4-2*m5*a1*d4-2*m6*a1*d4-2*m3*y3*a1 k17 = -2*m4*a3*z4-2*m4*a3*d4-2*m5*d4*a3-2*m6*d4*a3-2*m3*x3*y3 k18 k19 k20 k21 k22 k23 = = = = = = -m6*a2_2-m2*x2_2-m3*a2_2-m4*a2_2-m5*a2_2 m3*y3*z3+m3*y3*d3+m5*d4*d3+m6*d4*d3+m4*d3*d4+m4*d3*z4 m5*d3*a2+m3*d3*a2+m6*d3*a2+m4*d3*a2+m3*z3*a2+m2*x2*z2 -Ixx5-Iyy6+Izz6+Iyy5-m6*z6_2-m5*y5_2 Iyy4-Ixx4+Ixx6-Iyy5+Izz5-Izz6+m6*z6_2+m5*y5_2 2*m4*z4*d4+m4*d4_2+Izz3+m3*x3_2+m3*y3_2+m6*a3_2+m6*d4_2+ m4*a3_2+Ixx4+m4*z4_2+Iyy5+m5*a3_2+m5*d4_2+Izz6 k24 = m5*a2_2+2*m4*z4*d4+m4*d4_2+Izz3+m4*a2_2+m3*x3_2+m3*y3_2+m6*a3_2+ m6*d4_2+m4*a3_2+m2*x2_2+Ixx4+m4*z4_2+Izz2+m3*a2_2+Iyy5+m5*a3_2+ m5*d4_2+Izz6+m6*a2_2 k25 = -m6*d4_2+m6*a2_2+m6*a3_2+m4*a3_2-m4*z4_2-m4*d4_2+Ixx5+Iyy3-Iyy4+ m2*x2_2+Izz4-Ixx3+m3*x3_2+m3*a2_2+m4*a2_22*m4*z4*d4-m3*y3_2-Ixx6+m5*a3_2+m5*a2_2-m5*d4_2-Izz5+Iyy6 k26 = m5*a2_2+m4*d3_2+m4*a1_2+m3*d3_2+m3*a1_2+m1*x1*x1+Izz1+ 2*m4*z4*d4+m4*d4_2+Ixx3+m3*z3*z3+m4*a2_2+m6*a1_2+m3*y3_2+ 2*m3*z3*d3+m5*a1_2+m5*d3_2+m6*d4_2+m2*z2*z2+m2*a1_2+ m2*x2_2+Iyy4+m4*z4_2+m6*d3_2+Ixx2+m3*a2_2+Izz5+m5*d4_2+ m6*z6_2+Ixx6+m5*y5_2+m6*a2_2 .

116 FLAG_INIT = 1 } /* Matriz de inercias D Codigo fuente funciones Simulink /* Inicializacion realizada */ */ /*----------------------------------*/ s2_2 c2_2 s3_2 c3_2 s4_2 c4_2 c5_2 s5_2 c6_2 s6_2 c23_2 s23_2 = c2*c2 = c2*c2 = s3*s3 = c3*c3 = s4*s4 = c4*c4 = c5*c5 = s5*s5 = c6*c6 = s6*s6 = c23*c23 = s23*s23 (((k21+k8*c6_2)*c5_2+k22+k8*c6_2)*c4_2+(-2*k8*s6*c6*c5*s42*k5*s5)*c4+(k21+k8*c6_2)*c5_2-2*k3*c5+k25-2*k8*c6_2)*c23_2+ ((((-2*k21-2*k8*c6_2)*s5*c5+2*k3*s5)*c4+2*k8*c6*s5*s6*s4+ k17-2*k5*c5)*s23+(-2*k6*c4*s5+2*k9)*c2+(-2*k18*s3-2*k6*c5+ 2*k12)*s2-2*k2*c4*s5+k11)*c23+(-2*k2*c5+k16)*s23+k7*c2_2+ k15*c2+k18*c3_2+(-2*k6*c5+2*k12)*s3+((-k21-k8*c6_2)*c5_2k22-k8*c6_2)*c4_2+2*k8*c6*c5*s4*s6*c4-2*k4*s5*s4+k26+ 2*k3*c5+k8*c6_2+Jm1*Red1*Red1 (-k8*c6*s5*s6*c4+((-k21-k8*c6_2)*s5*c5+k3*s5)*s4-k4*c5+ k19)*c23+(-2*k8*c6*c5*c4_2*s6+(((-k21-k8*c6_2)*c5_2-k22k8*c6_2)*s4+k4*s5)*c4+k14+k8*s6*c6*c5+k5*s4*s5)*s23+(k20+ k6*s5*s4)*s2 (-2*k6*c4*s5+2*k9)*c2*c23+(-2*k6*c4*s5+2*k9)*s2*s23+ (-2*k6*c5+2*k12)*s3+((k21+k8*c6_2)*c5_2+k22+k8*c6_2)*c4_2+ (-2*k8*s6*c6*c5*s4-2*k5*s5)*c4+(-k21-k8*c6_2)*c5_2+2*k3*c5+ k24+Jm2*Red2*Red2 (-k8*c6*s5*s6*c4+((-k21-k8*c6_2)*s5*c5+k3*s5)*s4-k4*c5+ k19)*c23+(-2*k8*c6*c5*c4_2*s6+(((-k21-k8*c6_2)*c5_2-k22k8*c6_2)*s4+k4*s5)*c4+k14+k8*s6*c6*c5+k5*s4*s5)*s23 (k9-k6*c4*s5)*c2*c23+(k9-k6*c4*s5)*s2*s23+(-k6*c5+k12)*s3+ ((k21+k8*c6_2)*c5_2+k22+k8*c6_2)*c4_2+(-2*k8*s6*c6*c5*s42*k5*s5)*c4+(-k21-k8*c6_2)*c5_2+2*k3*c5+k23 ((k21+k8*c6_2)*c5_2+k22+k8*c6_2)*c4_2+(-2*k8*s6*c6*c5*s42*k5*s5)*c4+(-k21-k8*c6_2)*c5_2+2*k3*c5+k23+Jm3*Red3*Red3 (k5*c4*s5+k4*s4*s5+(-k21-k8*c6_2)*c5_2+k13+k8*c6_2)*c23+ m 1] 1] = m 1] 2] = m 2] 2] = m 1] 3] = m 2] 3] = m 3] 3] = m 1] 4] = .

D Codigo fuente funciones Simulink (((k21+k8*c6_2)*s5*c5-k3*s5)*c4-k8*s6*s5*c6*s4)*s23+ k2*c4*s5+k6*s5*c2*c4 k6*s5*s4*s3+k8*c6*s5*s6*c4+((k21+k8*c6_2)*s5*c5-k3*s5)*s4 k8*c6*s5*s6*c4+((k21+k8*c6_2)*s5*c5-k3*s5)*s4 (k21+k8*c6_2)*c5_2-k13-k8*c6_2+Jm4*Red4*Red4 (k5*s4*c5-k8*s6*c6*s5-k4*c4*c5)*c23+(-k8*s6*c6*c5*c4+ (-k3*c5+k10-k8*c6_2)*s4+k4*s5)*s23+k6*c2*c5*s4+k2*c5*s4 -k6*c2*c23*s5-k6*s2*s23*s5-k6*c5*c4*s3+(k3*c5+k8*c6_2k10)*c4-k8*s6*c6*c5*s4-k5*s5 (k3*c5+k8*c6_2-k10)*c4-k8*s6*c6*c5*s4-k5*s5 k8*s6*c6*s5 -k10+k8*c6_2+Jm5*Red5*Red5 k1*c23*c5-k1*s5*c4*s23 -k1*s5*s4 -k1*s5*s4 -k1*c5 0 -k1+Jm6*Red6*Red6 117 m 2] 4] = m 3] 4] = m 4] 4] = m 1] 5] = m 2] 5] = m 3] 5] = m 4] 5] = m 5] 5] = m 1] 6] = m 2] 6] = m 3] 6] = m 4] 6] = m 5] 6] = m 6] 6] = /* Par gravitatorio */ /*----------------------------------*/ fG 0] = 0 fG 1] = g*(s23*m6*z6*c5+s23*m5*y5*c5+s23*m4*d4+s23*m6*d4+s23*m5*d4+ s23*m4*z4+s23*m3*y3-c23*m3*x3-c23*m6*a3-c23*m5*a3+ c23*m5*y5*c4*s5+c23*m6*z6*c4*s5-c23*m4*a3)-g*(m5*a2+m6*a2+ m4*a2+x2*m2+m3*a2)*c2 fG 2] = g*(s23*m6*z6*c5+s23*m5*y5*c5+s23*m4*d4+s23*m6*d4+s23*m5*d4+ s23*m4*z4+s23*m3*y3-c23*m3*x3-c23*m6*a3-c23*m5*a3+ c23*m5*y5*c4*s5+c23*m6*z6*c4*s5-c23*m4*a3) fG 3] = g*(-s23*m5*y5*s4*s5-s23*m6*z6*s4*s5) fG 4] = g*(c23*m6*z6*s5+c23*m5*y5*s5+s23*m5*y5*c4*c5+s23*m6*z6*c4*c5) fG 5] = 0 if (mxGetPr(ENABLEFRIC(S)) 0] == 1) { /* Vector de friccin */ if (Enfric==0.0 } else .0) { for(i=0 i<6 i++) fricc i]=0.

fabs(M(i)-fG i]-forceac)) } else { ffric(i)=0 if (y i]>0.0 } .0) ffric(i)=0 FLAG_DEBUG(i) = 2 forceac=0 for (j=0 j<6 j++) forceac+=m i] j]*dx j] forceac-=m i] i]*dx i] if ((M(i)-fG i]-forceac)>=0) sig i]=1 else sig i]=-1 if (fabs(M(i)-fG i]-forceac)<Coulomb(i)) ffric(i)=1 fricc i]=sig i]*min(Coulomb(i).001)||(yant(i)*y i]<0.0) { fricc i]=Coulomb(i)+y i]*rwork 67+i] FLAG_DEBUG(i) = 3 } else if (y i]<0.0)) { ffric(i)=1 if(y i]==0.118 { for(i=0 i<6 i++) { D Codigo fuente funciones Simulink if ((fabs(y i])<0.0) { FLAG_DEBUG(i) = 1 fricc i]=-Coulomb(i)+y i]*rwork 67+i] } } } } } else { for(i=0 i<6 i++) fricc i]=0.

fG i] 119 /* Se resuelve la acelara cion con una des composicin */ /* de Cholesky de la matriz M para resolver el sistema */ /* de ecuaciones tau_acel=M*qdd */ R1_1 = R2_1 = R2_2 = R3_1 = R3_2 = R3_3 = R4_1 = R4_2 = R4_3 = R4_4 = R5_1 = R5_2 = R5_3 = R5_4 = R5_5 R6_1 R6_2 R6_3 R6_4 R6_5 R6_6 = = = = = = = sqrt(m 1] 1]) m 1] 2]/R1_1 sqrt(m 2] 2]-R2_1*R2_1) m 1] 3]/R1_1 (m 2] 3]-R2_1*R3_1)/R2_2 sqrt(m 3] 3]-R3_1*R3_1-R3_2*R3_2) m 1] 4]/R1_1 (m 2] 4]-R2_1*R4_1)/R2_2 (m 3] 4]-R3_1*R4_1-R3_2*R4_2)/R3_3 sqrt(m 4] 4]-R4_1*R4_1-R4_2*R4_2-R4_3*R4_3 m 1] 5]/R1_1 (m 2] 5]-R2_1*R5_1)/R2_2 (m 3] 5]-R3_1*R5_1-R3_2*R5_2)/R3_3 (m 4] 5]-R4_1*R5_1-R4_2*R5_2-R4_3*R5_3)/R4_4 sqrt(m 5] 5]-R5_1*R5_1-R5_2*R5_2-R5_3*R5_3-R5_4*R5_4) m 1] 6]/R1_1 (m 2] 6]-R2_1*R6_1)/R2_2 (m 3] 6]-R3_1*R6_1-R3_2*R6_2)/R3_3 (m 4] 6]-R4_1*R6_1-R4_2*R6_2-R4_3*R6_3)/R4_4 (-R5_1*R6_1-R5_2*R6_2-R5_3*R6_3-R5_4*R6_4)/R5_5 sqrt(m 6] 6]-R6_1*R6_1-R6_2*R6_2-R6_3*R6_3-R6_4*R6_4-R6_5*R6_5) /* Primer sistema triangular */ z_1 z_2 z_3 z_4 z_5 z_6 = = = = = = f 0]/R1_1 (f 1]-R2_1*z_1)/R2_2 (f 2]-R3_1*z_1-R3_2*z_2)/R3_3 (f 3]-R4_1*z_1-R4_2*z_2-R4_3*z_3)/R4_4 (f 4]-R5_1*z_1-R5_2*z_2-R5_3*z_3-R5_4*z_4)/R5_5 (f 5]-R6_1*z_1-R6_2*z_2-R6_3*z_3-R6_4*z_4-R6_5*z_5)/R6_6 /* Segundo sistema triangular */ .D Codigo fuente funciones Simulink /* Par acelerador */ /*----------------------------------*/ for(i=0 i<6 i++) f i] = M(i) .fricc i] .

120 x 5] = x 4] = x 3] = x 2] = x 1] = x 0] = D Codigo fuente funciones Simulink z_6/R6_6 (z_5-R6_5*x 5])/R5_5 (z_4-R5_4*x 4]-R6_4*x 5])/R4_4 (z_3-R4_3*x 3]-R5_3*x 4]-R6_3*x 5])/R3_3 (z_2-R3_2*x 2]-R4_2*x 3]-R5_2*x 4]-R6_2*x 5])/R2_2 (z_1-R2_1*x 1]-R3_1*x 2]-R4_1*x 3]-R5_1*x 4]-R6_1*x 5])/R1_1 /* Velocidades */ dx 0] = xCont 6] dx 1] = xCont 7] dx 2] dx 3] dx 4] dx 5] = xCont 8] = xCont 9] = xCont 10] = xCont 11] /* Aceleraciones */ if (mxGetPr(ENABLEFRIC(S)) 0] == 1) { for(i=0 i<6 i++) if (ffric(i)==0) dx 6+i]=x i] else dx 6+i]=0.c" .0 } else { for(i=0 i<6 i++) dx 6+i]=x i] } for(i=0 i<6 i++) yant(i)=y i] } static void mdlTerminate(SimStruct *S) { } #ifdef MATLAB_MEX_FILE #include "simulink.

h" #endif 121 .D Codigo fuente funciones Simulink #else #include "cg_sfun.

122 /* * * * * * */ #define S_FUNCTION_NAME rm10modelcor #define S_FUNCTION_LEVEL 2 #include <math.h> #include "simstruc.h" autor: Carlos Perez Fernandez fecha: 30-4-99 D Codigo fuente funciones Simulink Calcula el par de Coriolis debido a las velocidades de los distintos elementos del robot. /* Variables del espacio de trabajo */ #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define mE Jm1 Jm2 Jm3 Jm4 Jm5 Jm6 a1 a2 a3 d3 d4 x1 z1 x2 z2 x3 y3 z3 z4 y5 z6 Izz1 Ixx2 Iyy2 Izz2 Ixx3 Iyy3 rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork 0] 1] 2] 3] 4] 5] 6] 7] 8] 9] 10] 11] 12] 13] 14] 15] 16] 17] 18] 19] 20] 21] 22] 23] 24] 25] 26] 27] .

D Codigo fuente funciones Simulink #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define Izz3 Ixx4 Iyy4 Izz4 Ixx5 Iyy5 Izz5 Ixx6 Iyy6 Izz6 m1 m2 m3 m4 m5 m6 s2 c2 s3 c3 s4 c4 s5 c5 s6 c6 s23 c23 a1_2 a2_2 a3_2 d3_2 d4_2 x2_2 x3_2 y3_2 z4_2 y5_2 z6_2 Red1 Red2 Red3 Red4 Red5 rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork 28] 29] 30] 31] 32] 33] 34] 35] 36] 37] 38] 39] 40] 41] 42] 43] 44] 45] 46] 47] 48] 49] 50] 51] 52] 53] 54] 55] 56] 57] 58] 59] 60] 61] 62] 63] 64] 65] 66] 67] 68] 69] 70] 123 rwork 71] .

S.124 #define Red6 rwork 72] D Codigo fuente funciones Simulink #define FLAG_INIT iwork 0] */ /* Posicion */ /* Velocidad */ /* Entradas y estados /************************************/ #define Q(i) (*uPtrs 0] i-1]) #define #define #define QD(i) C(i. 1. 2)) return ssSetInputPortWidth(S.0) /* Flag para desactivar bloque */ static void mdlInitializeSizes(SimStruct *S) { ssSetNumContStates( ssSetNumDiscStates( S. 0. 0) ssSetNumModes( S. 0) ssSetNumNonsampledZCs(S. 73) ssSetNumIWork( S. 0) /* /* /* /* Numero Vector Vector Vector de de de de muestreos */ numeros reales */ numeros enteros */ punteros */ /*Si faltan parametros se da mensaje */ ssSetOptions(S. 0. SS_OPTION_EXCEPTION_FREE_CODE) } . 1) ssSetNumRWork( S.j) ENABLE(S) (*uPtrs 1] i-1]) mc i-1] j-1] ssGetSFcnParam(S. 0. 1. 6) ssSetInputPortDirectFeedThrough(S. 1) if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { return } ssSetNumSampleTimes( S. 6) /*Salidas*/ /*Par*/ ssSetNumSFcnParams(S. 1) ssSetNumPWork( S. 6) ssSetInputPortWidth(S. 1) if (!ssSetNumOutputPorts(S. 1) ssSetInputPortDirectFeedThrough(S. 0) 0) /*Entradas*/ if (!ssSetNumInputPorts(S.1)) return ssSetOutputPortWidth(S.

D Codigo fuente funciones Simulink
/* * mdlInitializeSampleTimes - Inicializa muestreos * */ static void mdlInitializeSampleTimes(SimStruct *S) { ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME) /*Automatico segun bloque preceden*/ ssSetOffsetTime(S, 0, 0.0) }

125

#define MDL_INITIALIZE_CONDITIONS /* * mdlInitializeConditions - Inicializa estados y parametros del robot * */ static void mdlInitializeConditions(SimStruct *S) { real_T int_T *rwork = ssGetRWork(S) *iwork = ssGetIWork(S) = 0 /* Flag de inicializaci\'{o}n a cero */

FLAG_INIT

a1 a2 a3 d3 d4 x1 z1 x2 z2 x3 y3 z3 z4 y5 z6

= = = = = = = = = = = = = = =

0.260 0.710 0.150 0.040 0.650 0.0839 -0.0832 0.2286 0.1876 0.1717 0.0246 0.0027 -0.1945 0.0317 0.050

/*Distancias constantes*/

/*Centros de masa*/

126
Izz1 Ixx2 Iyy2 Izz2 Ixx3 Iyy3 Izz3 Ixx4 Iyy4 Izz4 Ixx5 Iyy5 Izz5 Ixx6 Iyy6 Izz6 m1 m2 m3 m4 m5 m6 = = = = = = = = = = = = = = = = = = = = = = 1.1325 0.2656 3.4349 3.5202 1.0198 0.9982 1.6442 0.8301 0.7995 0.1202 0.0201 0.0086 0.0236 0.0100 0.0100 0.0050 38.65 51.800 84.1 33.89 7.36 5.00

D Codigo fuente funciones Simulink
/*Inercias*/

/*Masas*/

/* Distancias al cuadrado */ /*----------------------------------*/ a1_2 = a1*a1 a2_2 = a2*a2 a3_2 = a3*a3 d3_2 = d3*d3 d4_2 = d4*d4 x2_2 x3_2 y3_2 z4_2 y5_2 z6_2 = = = = = = x2*x2 x3*x3 y3*y3 z4*z4 y5*y5 z6*z6

D Codigo fuente funciones Simulink
} /* * mdlOutputs - Calcula las salidas * */ static void mdlOutputs(SimStruct *S, int_T tid) { double h = 0.5 int i,j real_T *y InputRealPtrsType uPtrs real_T 2]

127

s2_2, c2_2, s3_2, c3_2, s4_2, c4_2, s5_2, c5_2, s6_2, c6_2, s23_2, c23_2

real_T mc 6] 6] static real_T k1,k2,k3,k4,k5,k6,k7,k8,k9,k10,k11,k12,k13,k14,k15, k16,k17,k18,k19,k20,k21,k22,k23,k24,k25 real_T *rwork = ssGetRWork(S) int_T *iwork = ssGetIWork(S) real_T enable = mxGetPr(ENABLE(S)) 0] uPtrs uPtrs 0] = ssGetInputPortRealSignalPtrs(S,0) /*Puntero a vector posicion*/ 1] = ssGetInputPortRealSignalPtrs(S,1) /*Puntero a vector velocidad*/

y=ssGetOutputPortRealSignal(S,0) if (FLAG_INIT == 0) { /* Calculo de parametros */ /*----------------------------------*/ k1 k2 k3 k4 k5 k6 = -Izz6 = m5*y5*a1+m6*z6*a1 = m5*a2*y5+m6*a2*z6 = m5*y5*d4+m6*z6*d4 = m5*y5*a3+m6*z6*a3 = m5*y5*d3+m6*z6*d3

k7 = -Ixx6+Iyy6

128 D Codigo fuente funciones Simulink k8 = -Ixx2+Iyy2 k9 = m6*a2*a3+m3*a2*x3+m5*a2*a3+m4*a2*a3 k10 = -m5*y5_2-m6*z6_2-Izz5-Ixx6 k11 = 2*m5*a1*a3+2*m3*x3*a1+2*m6*a1*a3+2*m4*a3*a1 k12 = -m4*a2*d4-m4*a2*z4-m6*a2*d4-m3*a2*y3-m5*a2*d4 k13 = -m5*y5_2-m6*z6_2-Ixx5-Iyy6-Izz4 k14 = m4*d3*a3+m5*a3*d3+m6*a3*d3+m3*x3*d3+m3*x3*z3 k15 = -2*m4*a3*d4-2*m4*a3*z4-2*m3*x3*y3-2*m6*d4*a3-2*m5*d4*a3 k16 = 2*m2*x2*a1+2*m5*a2*a1+2*m6*a2*a1+2*m3*a2*a1+2*m4*a2*a1 k17 = -m3*a2_2-m5*a2_2-m6*a2_2-m4*a2_2-m2*x2_2 k18 = -2*m6*a1*d4-2*m4*a1*d4-2*m4*a1*z4-2*m5*a1*d4-2*m3*y3*a1 k19 = m4*d3*z4+m4*d3*d4+m5*d3*d4+m3*y3*d3+m3*y3*z3+m6*d4*d3 k20 = m4*d3*a2+m5*d3*a2+m3*d3*a2+m6*d3*a2+m2*x2*z2+m3*z3*a2 k21 = -Iyy6+Izz6-m5*y5_2+Iyy5-Ixx5-m6*z6_2 k22 = -Iyy5+Iyy4-Ixx4+m5*y5_2+m6*z6_2-Izz6+Izz5+Ixx6 k23 = Ixx4+m4*z4_2+2*m4*z4*d4+m3*y3_2+m6*d4_2+m4*a3_2+Iyy5+m6*a3_2+Izz3+ m4*d4_2+m5*d4_2+m3*x3_2+m5*a3_2+Izz6 k24 = Ixx4+m4*z4_2+2*m4*z4*d4+m3*y3_2+m6*d4_2+m4*a3_2+m5*a2_2+Iyy5+ m6*a3_2+Izz3+Izz2+m3*a2_2+m4*d4_2+m5*d4_2+m6*a2_2+m3*x3_2+m2*x2_2+ m4*a2_2+m5*a3_2+Izz6 k25 = m3*a2_2-Iyy4+m4*a3_2+m6*a3_2-m4*d4_2+Iyy3-Izz5+Ixx5+Izz4+Iyy6-Ixx3 -Ixx6-2*m4*z4*d4+m6*a2_2+m5*a3_2+m4*a2_2-m4*z4_2-m6*d4_2+m3*x3_2m3*y3_2+m5*a2_2+m2*x2_2-m5*d4_2 FLAG_INIT = 1 } /* Inicializacion realizada */ /* Calculo de senos y cosenos */ /*----------------------------------*/ s2 = sin(Q(2)) s3 = sin(Q(3)) s4 = sin(Q(4)) s5 = sin(Q(5)) s6 = sin(Q(6)) c2 c3 c4 c5 c6 = = = = = cos(Q(2)) cos(Q(3)) cos(Q(4)) cos(Q(5)) cos(Q(6)) s23 = sin(Q(2)+Q(3)) .

5*k15+k5*c5+ k7*c6*s5*s6*s4)*s23_2+((-k9+k3*c4*s5)*c2+(k17*s3-k12+k3*c5)*s2.D Codigo fuente funciones Simulink c23 = cos(Q(2)+Q(3)) s2_2 c2_2 s3_2 c3_2 s4_2 c4_2 c5_2 s5_2 c6_2 s6_2 c23_2 s23_2 = c2*c2 = c2*c2 = s3*s3 = c3*c3 = s4*s4 = c4*c4 = c5*c5 = s5*s5 = c6*c6 = s6*s6 = c23*c23 = s23*s23 129 /* Fuerzas centrifugas y de coriolis */ /*------------------------------------*/ C(1.1)=((((k7*c6_2-k21)*s5*c5+k4*s5)*c4-k7*c6*s5*s6*s4-k5*c5+ .5*k11+k2*c4*s5)*s23+(-k3*c5-k17*s3+k12)*c3)*QD(3)+ ((k7*c6*c5*c4_2*s6+((k7*c6_2-k21)*c5_2+k7*c6_2-k22)*s4*c4+ k5*s4*s5-k7*c6*c5*s4_2*s6)*c23_2+((-k7*c6*s5*s6*c4+((-k7*c6_2+ k21)*s5*c5-k4*s5)*s4)*s23+k2*s4*s5+k3*c2*s4*s5)*c23k7*c6*c5*c4_2*s6+(((-k7*c6_2+k21)*c5_2+k22-k7*c6_2)*s4+k6*s5)*c4+ k7*c6*c5*s4_2*s6)*QD(4)+(((k7*c6_2-k21)*s5*c5*c4_2+(-k5*c5k7*c6*s5*s6*s4)*c4+(k7*c6_2-k21)*s5*c5+k4*s5)*c23_2+((((k7*c6_2 -k21)*c5_2+k4*c5+(-k7*c6_2+k21)*s5_2)*c4+k5*s5k7*c6*c5*s4*s6)*s23-k3*c2*c4*c5-k2*c4*c5+k3*s5*s2)*c23+k2*s23*s5+ k3*s5*s3+(-k7*c6_2+k21)*s5*c5*c4_2+k7*c6*s5*s4*s6*c4-k4*s5+ k6*c5*s4)*QD(5)+(((k7*c6*s6+k7*c5_2*c6*s6)*c4_2+(-k7*s6_2+ k7*c6_2)*c5*s4*c4-2*k7*c6*s6+k7*c5_2*c6*s6)*c23_2+ (-2*k7*s5*c5*c4*c6*s6+(k7*s6_2-k7*c6_2)*s5*s4)*s23*c23+ .5*k18)*c23+(((-k7*c6_2+k21)*s5*c5-k4*s5)*c4-.5*k11+k2*c4*s5)*s23-k8*c2*s2-.5*k15+k5*c5+k7*c6*s5*s6*s4)*s23_2+((-k9+k3*c4*s5)*c2+(k17*s3k12+k3*c5)*s2-.5*k15)*c23_2+ ((((k7*c6_2-k21)*c5_2+k7*c6_2-k22)*c4_2+(2*k5*s52*k7*c6*c5*s4*s6)*c4+(k7*c6_2-k21)*c5_2+2*k4*c5-k25-2*k7*c6_2)*s23k2*c5-k17*s2*c3+.5*k18)*c23+(((-k7*c6_2+k21)*s5*c5-k4*s5)*c4.5*k15)*c23_2+((((k7*c6_2-k21)*c5_2+k7*c6_2-k22)*c4_2+(2*k5*s52*k7*c6*c5*s4*s6)*c4+(k7*c6_2-k21)*c5_2+2*k4*c5-k252*k7*c6_2)*s23+(-k3*c5-k17*s3+k12)*c2+(-k9+k3*c4*s5)*s2-k2*c5+ .5*k16*s2)*QD(2)+((((k7*c6_2k21)*s5*c5+k4*s5)*c4-k7*c6*s5*s6*s4-k5*c5+.

5*k7*c6_2-.5*k22+ .5*k21)*c5_2-.5*k21)*c5_2+.5*k1+.5*k1-.5*k21)*c5_2+(-.5*k7*c6_2-.5*k21)*c5_2+ (-.5*k15)*c23_2+((((k7*c6_2-k21)*c5_2+k7*c6_2-k22)*c4_2+(2*k5*s52*k7*c6*c5*s4*s6)*c4+(k7*c6_2-k21)*c5_2+2*k4*c5-k25-2*k7*c6_2)*s23k2*c5-k17*s2*c3+.5*k21)*c5_2+ .5*k13)*s23*QD(4)+((k7*c6*c5*s6*c4+((.5*k7*c6_2)*s4_2+k6*s4*s5+(-.5*k7*c6_2)*s4k6*s5)*c23+(-k7*c6*s5*c4_2*s6+((-k7*c6_2+k21)*s5*c5*s4k6*c5)*c4)*s23)*QD(5)+(((-.5*k7*c6_2+.5*k7*c6_2-.5*k18)*c23+ (((-k7*c6_2+k21)*s5*c5-k4*s5)*c4-.5*k7*c6_2)*c4_2 -2*k7*c6*c5*s4*s6*c4+((-.5*k22.5*k21)*c5_2+.2)=((((k7*c6_2-k21)*s5*c5+k4*s5)*c4-k7*c6*s5*s6*s4-k5*c5+.5*k15+k5*c5+k7*c6*s5*s6*s4)*s23_2+((-k9+k3*c4*s5)*c2+(k17*s3k12+k3*c5)*s2-.5*k7*c6_2+.5*k7*s6_2)*s5*c4k7*s5*c5*s4*c6*s6)*c23+((-k7*s6_2+k7*c6_2)*c5*c4_2+(-k7*c6*s6k7*c5_2*c6*s6)*s4*c4+(-.5*k10+.5*k13)*s23*QD(4)+((k7*c6*c5*s6*c4+((.5*k7*c6_2+.5*k7*c6_2)*c4_2-2*k7*c6*c5*s4*s6*c4+((-.5*k7*c6_2.5*k21)*c5_2-.5*k7*c6_2-.5*k7*c6_2-.130 D Codigo fuente funciones Simulink (-k7*c6*s6-k7*c5_2*c6*s6)*c4_2+(k7*s6_2-k7*c6_2)*c5*s4*c4+ k7*c6*s6)*QD(6) C(1.5*k7*c6_2)*s4_2+k6*s4*s5+(-.5*k10+.5*k21)*s5_2+.5*k21)*s5_2+.5*k7*c6_2)*s4-k6*s5)*c23+ (-k7*c6*s5*c4_2*s6+((-k7*c6_2+k21)*s5*c5*s4-k6*c5)*c4)*s23)*QD(5)+ (((-.5*k15)*c23_2+ ((((k7*c6_2-k21)*c5_2+k7*c6_2-k22)*c4_2+(2*k5*s52*k7*c6*c5*s4*s6)*c4+(k7*c6_2-k21)*c5_2+2*k4*c5-k25-2*k7*c6_2)*s23+ (-k3*c5-k17*s3+k12)*c2+(-k9+k3*c4*s5)*s2-k2*c5+.5*k22-.5*k7*s6_2)*s5*c4-k7*s5*c5*s4*c6*s6)*c23+ ((-k7*s6_2+k7*c6_2)*c5*c4_2+(-k7*c6*s6-k7*c5_2*c6*s6)*s4*c4+ .5*k7*c6_2+.5*k7*c6_2+.5*k7*c6_2.5*k15+k5*c5+ k7*c6*s5*s6*s4)*s23_2+((-k9+k3*c4*s5)*c2+(k17*s3-k12+k3*c5)*s2.5*k7*c6_2+.5*k11+k2*c4*s5)*s23+(-k3*c5-k17*s3+k12)*c3)*QD(1)+ ((2*k7*c6*c5*c4_2*s6+(((k7*c6_2-k21)*c5_2+k7*c6_2-k22)*s4k6*s5)*c4+k14+k5*s4*s5-k7*c6*c5*s6)*c23+(-k7*c6*s5*s6*c4+ ((-k7*c6_2+k21)*s5*c5-k4*s5)*s4-k6*c5-k19)*s23)*QD(2)+ ((2*k7*c6*c5*c4_2*s6+(((k7*c6_2-k21)*c5_2+k7*c6_2-k22)*s4k6*s5)*c4+k14+k5*s4*s5-k7*c6*c5*s6)*c23+(-k7*c6*s5*s6*c4+ ((-k7*c6_2+k21)*s5*c5-k4*s5)*s4-k6*c5-k19)*s23)*QD(3)+ (((.3)=((((k7*c6_2-k21)*s5*c5+k4*s5)*c4-k7*c6*s5*s6*s4-k5*c5+ .5*k7*s6_2)*c5)*s23)*QD(6) C(1.5*k22+.5*k7*c6_2+.5*k18)*c23+(((-k7*c6_2+k21)*s5*c5-k4*s5)*c4.5*k1+.5*k21)*c5_2+ .5*k11+k2*c4*s5)*s23-k8*c2*s2-.5*k7*c6_2-.5*k16*s2)*QD(1)+ ((2*k7*c6*c5*c4_2*s6+(((k7*c6_2-k21)*c5_2+k7*c6_2-k22)*s4k6*s5)*c4+k14+k5*s4*s5-k7*c6*c5*s6)*c23+(-k7*c6*s5*s6*c4+ ((-k7*c6_2+k21)*s5*c5-k4*s5)*s4-k6*c5-k19)*s23+(k3*s4*s5+ k20)*c2)*QD(2)+((2*k7*c6*c5*c4_2*s6+(((k7*c6_2-k21)*c5_2+ k7*c6_2-k22)*s4-k6*s5)*c4+k14+k5*s4*s5-k7*c6*c5*s6)*c23+ (-k7*c6*s5*s6*c4+((-k7*c6_2+k21)*s5*c5-k4*s5)*s4-k6*c5k19)*s23)*QD(3)+(((.

5*k7*c6_2+.5*k21)*c5_2-.5*k7*s6_2+.5*k13)*s23*QD(3)+((-k6*s5*c4-k5*s4*s5)*c23+ (k7*c6*s5*s6*c4+((k7*c6_2-k21)*s5*c5+k4*s5)*s4)*s23-k2*s4*s5k3*c2*s4*s5)*QD(4)+((k5*c4*c5-k6*c5*s4+(-k7*c6_2+k21)*s5*c5)*c23+ ((-.5*k7*c6_2+ .5*k7*c6_2)*s4_2+k6*s4*s5+(-.5*k7*c6_2)*c4_2-2*k7*c6*c5*s4*s6*c4+((-.5*k7*c6_2.5*k21)*s5_2+.5*k7*s6_2)*c5)*s23)*QD(2)+(((-.5*k7*c6_2+.5*k7*s6_2)*s5*c23+((-.5*k21)*s5_2+.5*k1+.5*k7*c6_2.5*k10+.5*k7*c6_2+.5*k1+.5*k21)*s5_2+.5*k21)*c5_2+ .5*k1.5*k21)*c5_2-k4*c5+ (.5*k1- .5*k7*c6_2+.5*k10+ .5*k7*c6_2-.5*k22+ .5*k7*c6_2+.5*k7*c6_2-.5*k21)*c5_2+ .5*k7*c6_2)*s4-k6*s5)*c23+(-k7*c6*s5*c4_2*s6+((-k7*c6_2+ k21)*s5*c5*s4-k6*c5)*c4)*s23)*QD(3)+((k5*c4*c5-k6*c5*s4+ (-k7*c6_2+k21)*s5*c5)*c23+((-.5*k22+ .5*k7*s6_2)*s5*c4-k7*s5*c5*s4*c6*s6)*c23+((-k7*s6_2 +k7*c6_2)*c5*c4_2+(-k7*c6*s6-k7*c5_2*c6*s6)*s4*c4+(-.5*k10+ .5*k22-.5*k21)*c5_2+ .5*k21)*c5_2+(-.5*k7*c6_2-.4)=((k7*c6*c5*c4_2*s6+((k7*c6_2-k21)*c5_2+k7*c6_2-k22)*s4*c4+ k5*s4*s5-k7*c6*c5*s4_2*s6)*c23_2+((-k7*c6*s5*s6*c4+((-k7*c6_2+ k21)*s5*c5-k4*s5)*s4)*s23+k2*s4*s5+k3*c2*s4*s5)*c23- 131 k7*c6*c5*c4_2*s6+(((-k7*c6_2+k21)*c5_2+k22-k7*c6_2)*s4+k6*s5)*c4+ k7*c6*c5*s4_2*s6)*QD(1)+(((.5*k1-.5*k7*c6_2.5*k7*c6_2)*c4*s23+k3*c2*c4*c5+ k2*c4*c5)*QD(4)+((-k5*s4*s5+k7*c6*c5*s6-k6*s5*c4)*c23+ (-k7*c6*s5*s6*c4-k6*c5+k4*s4*s5)*s23-k2*s4*s5-k3*c2*s4*s5)*QD(5)+ ((-.5)=(((k7*c6_2-k21)*s5*c5*c4_2+(-k5*c5-k7*c6*s5*s6*s4)*c4+(k7*c6_2k21)*s5*c5+k4*s5)*c23_2+((((k7*c6_2-k21)*c5_2+k4*c5+(-k7*c6_2+ k21)*s5_2)*c4+k5*s5-k7*c6*c5*s4*s6)*s23-k3*c2*c4*c5-k2*c4*c5+ k3*s5*s2)*c23+k2*s23*s5+k3*s5*s3+(-k7*c6_2+k21)*s5*c5*c4_2+ k7*c6*s5*s4*s6*c4-k4*s5+k6*c5*s4)*QD(1)+((k7*c6*c5*s6*c4+ ((.5*k7*c6_2+.5*k21)*c5_2-.5*k7*c6_2)*s4-k6*s5)*c23+(-k7*c6*s5*c4_2*s6+((-k7*c6_2+ k21)*s5*c5*s4-k6*c5)*c4)*s23)*QD(2)+((k7*c6*c5*s6*c4+ ((.5*k10+ .5*k21)*c5_2-k4*c5+(.5*k7*c6_2-.5*k21)*c5_2+ .5*k7*s6_2)*c5)*s23)*QD(6) C(1.5*k21)*s5_2+.5*k7*s6_2)*s5*c4-k7*s5*c5*s4*c6*s6)*c23+((-k7*s6_2+ k7*c6_2)*c5*c4_2+(-k7*c6*s6-k7*c5_2*c6*s6)*s4*c4+(-.5*k7*c6_2-.D Codigo fuente funciones Simulink (-.5*k21)*c5_2+(-.5*k7*c6_2-.5*k7*s6_2)*c5*c4-k7*c6*s4*s6)*s23)*QD(6) C(1.6)=(((k7*c6*s6+k7*c5_2*c6*s6)*c4_2+(-k7*s6_2+k7*c6_2)*c5*s4*c42*k7*c6*s6+k7*c5_2*c6*s6)*c23_2+(-2*k7*s5*c5*c4*c6*s6+(k7*s6_2k7*c6_2)*s5*s4)*s23*c23+(-k7*c6*s6-k7*c5_2*c6*s6)*c4_2+(k7*s6_2k7*c6_2)*c5*s4*c4+k7*c6*s6)*QD(1)+(((-.5*k7*c6_2+.5*k1+.5*k7*c6_2+.5*k7*c6_2-.5*k7*c6_2+.5*k1+.5*k1)*s5*s4)*s23)*QD(6) C(1.5*k7*c6_2+.5*k7*c6_2)*s4_2+k6*s4*s5+(-.5*k7*c6_2-.5*k22-.5*k7*c6_2)*c4_2-2*k7*c6*c5*s4*s6*c4+((-.5*k7*c6_2)*c4*s23+k3*c2*c4*c5+k2*c4*c5)*QD(5)+((-k7*c5_2*c6*s6+ k7*c6*s6)*c23+(k7*s5*c5*c4*c6*s6+(-.5*k7*c6_2-.5*k13)*s23*QD(2)+(((.

5*k7*c6_2)*s23+k3*s2*c4*s5)*QD(1)+ (k3*c2*c23*s4*s5+k3*s2*s4*s23*s5+k7*c6*c5*c4_2*s6+((k7*c6_2- .5*k7*s6_2+.5*k7*s6_2.5*k1)*s5*s4)*s23)*QD(4)+((-.132 D Codigo fuente funciones Simulink .1)=((((-k7*c6_2+k21)*s5*c5-k4*s5)*c4-.5*k21)*c5_2+ .5*k7*s6_2)*c5)*s23)*QD(3)+((-k7*c5_2*c6*s6+ k7*c6*s6)*c23+(k7*s5*c5*c4*c6*s6+(-.5*k21)*c5_2+.5*k7*c6_2.5*k7*c6_2)*c4_2+(k5*s5-2*k7*c6*c5*s4*s6)*c4+ ((-.5*k22-.5*k13-.5*k7*c6_2+.5*k21)*c5_2+.5*k3*c2*s5)*s23.5*k22-.5*k7*c6_2-.5*k3*s5*s3+(k7*c6_2-k21)*s5*c5*c4_2+(-k5*c5k7*c6*s5*s6*s4)*c4+(-k7*c6_2+k21)*s5*c5-k4*s5)*QD(5)+((k7*c6*s6+ k7*c5_2*c6*s6)*c4_2+(-k7*s6_2+k7*c6_2)*c5*s4*c4-k7*c5_2*c6*s6)*QD(6) C(2.5*k7*c6_2+.5*k11-k2*c4*s5)*s23+k8*c2*s2+ .5*k7*c6_2+ .5*k3*c2*c23*s4*s5+.5*k3*s2*s4*s23*s5+.5*k18)*c23+(((k7*c6_2-k21)*s5*c5+k4*s5)*c4k7*c6*s5*s6*s4-k5*c5+.5*k7*c6_2.5*k21)*c5_2+k4*c5+(-.5*k1+.5*k21)*c5_2+.5*k15+k5*c5+ k7*c6*s5*s6*s4)*c23_2+((((-k7*c6_2+k21)*c5_2+k22-k7*c6_2)*c4_2+ (2*k7*c6*c5*s4*s6-2*k5*s5)*c4+(-k7*c6_2+k21)*c5_2-2*k4*c5+ 2*k7*c6_2+k25)*s23+(k17*s3-k12+k3*c5)*c2+(k9-k3*c4*s5)*s2+ k2*c5-.5*k21)*s5_2-.5*k3*c2*c4*c5)*c23+(-.5*k7*c6_2+.5*k7*s6_2+ .5*k7*c6_2)*s23+k3*s2*c4*s5)*QD(4)+(((.5*k22+.5*k7*c6_2.5*k1+.5*k7*c6_2)*s4_2+(.4)=((((k7*c6_2-k21)*s5*c5+k4*s5)*c4-k7*c6*s5*s6*s4)*c23+(((.5*k21)*c5_2-.5*k1)*c5)*s23)*QD(6) C(2.2)=((k9-k3*c4*s5)*s2*c23+(-k9+k3*c4*s5)*c2*s23+(k12-k3*c5)*c3)*QD(3)+ (k3*c2*c23*s4*s5+k3*s2*s4*s23*s5+k7*c6*c5*c4_2*s6+((k7*c6_2k21)*c5_2+k7*c6_2-k22)*s4*c4+k5*s4*s5-k7*c6*c5*s4_2*s6)*QD(4)+ (-k3*c2*c23*c4*c5-k3*s2*c4*s23*c5+k3*s5*s3+(k7*c6_2k21)*s5*c5*c4_2+(-k5*c5-k7*c6*s5*s6*s4)*c4+(-k7*c6_2+k21)*s5*c5k4*s5)*QD(5)+((k7*c6*s6+k7*c5_2*c6*s6)*c4_2+(-k7*s6_2+ k7*c6_2)*c5*s4*c4-k7*c5_2*c6*s6)*QD(6) C(2.5*k7*c6_2-.5*k22+ .5*k21)*c5_2-.5*k10)*s4*c23+(-k7*c6*s5*c4_2*s6+(-k7*c6_2+k21)*s5*c5*s4*c4+ k5*s4*c5+k7*s6*c6*s5)*s23+k3*c5*s4*s2)*QD(5)+(((-.5*k15)*s23_2+((k9-k3*c4*s5)*c2+ (-k3*c5-k17*s3+k12)*s2+.5*k3*s5*s4*c3+ k7*c6*c5*c4_2*s6+((k7*c6_2-k21)*c5_2+k7*c6_2-k22)*s4*c4+ k5*s4*s5-k7*c6*c5*s4_2*s6)*QD(4)+((-.5*k7*c6_2.5*k16*s2)*QD(1)+((((k7*c6_2-k21)*s5*c5+k4*s5)*c4k7*c6*s5*s6*s4)*c23+(((.5*k3*c5*c4*c3+.5*k1)*s5*c4-k7*s5*c5*s4*c6*s6)*c23+((-k7*s6_2+ k7*c6_2)*c5*c4_2+(-k7*c6*s6-k7*c5_2*c6*s6)*s4*c4+(.5*k7*c6_2)*c4_2+(k5*s5-2*k7*c6*c5*s4*s6)*c4+((-.5*k7*c6_2+ .5*k13-.5*k7*c6_2)*s4_2+(.5*k7*c6_2+.5*k7*c6_2+.3)=((k9-k3*c4*s5)*s2*c23+(-k9+k3*c4*s5)*c2*s23+(k12-k3*c5)*c3)*QD(2)+ ((k9-k3*c4*s5)*s2*c23+(-k9+k3*c4*s5)*c2*s23+(k12-k3*c5)*c3)*QD(3)+ (.5*k7*s6_2)*s5*c23+ ((-.5*k3*s5*s2.5*k7*c6_2-.5*k7*s6_2)*c5*c4-k7*c6*s4*s6)*s23)*QD(5) C(2.5*k7*c6_2-.5*k3*s2*c4*c5+.

5*k21)*c5_2+k4*c5+(-.5*k10)*s4*c23+ (-k7*c6*s5*c4_2*s6+(-k7*c6_2+k21)*s5*c5*s4*c4+k5*s4*c5+ k7*s6*c6*s5)*s23)*QD(5)+(((-.5*k7*c6_2)*s5*c4+k7*s5*c5*s4*c6*s6)*QD(4)+(k7*c4*c6*s6+ (-.5*k7*c6_2+ .5*k1)*c5*s4)*QD(6) C(2.5*k7*c6_2-.5*k3*c5*c4*c3+.5*k21)*c5_2-k4*c5 +(.5*k1+.5*k7*c6_2-.5*k7*c6_2+.5*k3*s5*s4*c3+ k7*c6*c5*c4_2*s6+((k7*c6_2-k21)*c5_2+k7*c6_2-k22)*s4*c4+k5*s4*s5k7*c6*c5*s4_2*s6)*QD(3)+(k3*s5*c4*s3+((-k7*c6_2+k21)*s5*c5k4*s5)*c4+k7*c6*s5*s6*s4)*QD(4)+(k3*c5*s4*s3+((-.5*k7*c6_2-.5*k10+ .5*k3*c2*c23*s4*s5+.5*k21)*c5_2+ .5*k10+.5*k7*c6_2-.5*k7*c6_2.5*k7*s6_2-.5*k7*c6_2+.5*k7*s6_2+.5*k1)*s5*c4k7*s5*c5*s4*c6*s6)*c23+((-k7*s6_2+k7*c6_2)*c5*c4_2+(-k7*c6*s6- .5*k22-.5*k7*c6_2+ .5*k3*c2*s5)*s23-.5*k3*s5*s2-.5*k7*c6_2-.5*k21)*c5_2-.5*k7*c6_2-.5*k7*c6_2-.5*k7*c6_2+.5*k21)*s5_2+.5*k7*c6_2)*s4)*QD(4)+(-k5*c5k3*s23*c5*s2-k7*c6*s5*s6*s4-k3*c23*c5*c2-k4*s5*c4+ k3*s5*c4*s3)*QD(5)+(k7*c4*c6*s6+(-.5*k7*c6_2+.5*k7*s6_2)*c5)*s23)*QD(1)+((k7*c6*s6+ k7*c5_2*c6*s6)*c4_2+(-k7*s6_2+k7*c6_2)*c5*s4*c4k7*c5_2*c6*s6)*QD(2)+((k7*c6*s6+k7*c5_2*c6*s6)*c4_2+(-k7*s6_2+ k7*c6_2)*c5*s4*c4-k7*c5_2*c6*s6)*QD(3)+((.5*k1)*c5*s4)*QD(5) C(3.5*k7*c6_2)*s5*c4+ k7*s5*c5*s4*c6*s6)*QD(6) C(2.5*k7*c6_2)*s4_2+(.5*k7*s6_2+.5*k21)*s5_2+.5*k7*c6_2+.5*k13-.5*k21)*s5_2.5*k1.6)=(((-.5*k21)*c5_2+.5*k7*c6_2+.5*k3*s2*c4*c5+ .5*k3*c2*c4*c5)*c23+(-.5*k7*c6_2)*s4)*QD(5)+((.D Codigo fuente funciones Simulink 133 k21)*c5_2+k7*c6_2-k22)*s4*c4+k5*s4*s5-k7*c6*c5*s4_2*s6)*QD(2)+ (.5*k21)*c5_2-k4*c5+(.5*k7*s6_2+.5*k21)*c5_2+k4*c5+ (-.5*k15)*s23_2+((k9k3*c4*s5)*c2+(-k3*c5-k17*s3+k12)*s2+.5*k21)*s5_2-.5*k11-k2*c4*s5)*s23+(k17*s3k12+k3*c5)*c3)*QD(1)+((((k7*c6_2-k21)*s5*c5+k4*s5)*c4k7*c6*s5*s6*s4)*c23+(((.5*k3*s5*s3+(k7*c6_2k21)*s5*c5*c4_2+(-k5*c5-k7*c6*s5*s6*s4)*c4+(-k7*c6_2+k21)*s5*c5k4*s5)*QD(3)+(k3*c5*s4*s3+((-.5*k18+k17*s2*c3+k2*c5)*c23+(((k7*c6_2k21)*s5*c5+k4*s5)*c4-k7*c6*s5*s6*s4-k5*c5+.5*k7*c6_2-.5*k3*s2*s4*s23*s5+.5*k22+ .5*k7*c6_2)*s23)*QD(4)+(((.5*k7*c6_2-.5*k7*c6_2)*c4_2+(k5*s5-2*k7*c6*c5*s4*s6)*c4+((-.5*k1)*s5*c4-k7*s5*c5*s4*c6*s6)*c23+ ((-k7*s6_2+k7*c6_2)*c5*c4_2+(-k7*c6*s6-k7*c5_2*c6*s6)*s4*c4+ (-.5*k7*s6_2-.5*k15+k5*c5+ k7*c6*s5*s6*s4)*c23_2+((((-k7*c6_2+k21)*c5_2+k22-k7*c6_2)*c4_2+ (-2*k5*s5+2*k7*c6*c5*s4*s6)*c4+(-k7*c6_2+k21)*c5_2-2*k4*c5+ 2*k7*c6_2+k25)*s23-.5*k1-.5*k10)*s4*c23+(-k7*c6*s5*c4_2*s6+(-k7*c6_2+ k21)*s5*c5*s4*c4+k5*s4*c5+k7*s6*c6*s5)*s23+k3*c5*s4*s2)*QD(1)+ (-k3*c2*c23*c4*c5-k3*s2*c4*s23*c5+k3*s5*s3+(k7*c6_2k21)*s5*c5*c4_2+(-k5*c5-k7*c6*s5*s6*s4)*c4+(-k7*c6_2+k21)*s5*c5k4*s5)*QD(2)+((-.1)=((((-k7*c6_2+k21)*s5*c5-k4*s5)*c4-.5*k7*s6_2+.5)=(((.

5*k3*c5*c4*c3+.5*k7*c6_2-.5*k7*c6_2+.5*k3*c2*c4*c5)*c23+(-.5)=(((.2)=((-k9+k3*c4*s5)*s2*c23+(k9-k3*c4*s5)*c2*s23+(-k12+k3*c5)*c3)*QD(2)+ (.5*k7*c6_2)*s5*c4+k7*s5*c5*s4*c6*s6)*QD(6) C(3.5*k10-.5*k7*c6_2)*s5*c4+k7*s5*c5*s4*c6*s6)*QD(4)+(k7*c4*c6*s6+(-.5*k7*c6_2)*c4_2+(k5*s52*k7*c6*c5*s4*s6)*c4+((-.5*k7*c6_2+.5*k10+.5*k7*c6_2.3)=(k7*c6*c5*c4_2*s6+((k7*c6_2-k21)*c5_2+k7*c6_2-k22)*s4*c4+ k5*s4*s5-k7*c6*c5*s4_2*s6)*QD(4)+((k7*c6_2-k21)*s5*c5*c4_2+ (-k5*c5-k7*c6*s5*s6*s4)*c4+(-k7*c6_2+k21)*s5*c5-k4*s5)*QD(5)+ ((k7*c6*s6+k7*c5_2*c6*s6)*c4_2+(-k7*s6_2+k7*c6_2)*c5*s4*c4k7*c5_2*c6*s6)*QD(6) C(3.5*k7*c6_2-.5*k3*s5*s4*c3+k7*c6*c5*c4_2*s6+((k7*c6_2-k21)*c5_2+k7*c6_2k22)*s4*c4+k5*s4*s5-k7*c6*c5*s4_2*s6)*QD(2)+(k7*c6*c5*c4_2*s6+ ((k7*c6_2-k21)*c5_2+k7*c6_2-k22)*s4*c4+k5*s4*s5k7*c6*c5*s4_2*s6)*QD(3)+(((-k7*c6_2+k21)*s5*c5-k4*s5)*c4+ k7*c6*s5*s6*s4)*QD(4)+((-.5*k3*s2*s4*s23*s5.5*k22+.5*k21)*c5_2-k4*c5+(.5*k13.5*k3*s2*c4*c5)*s23+ .5*k3*c2*s5-.4)=((((k7*c6_2-k21)*s5*c5+k4*s5)*c4-k7*c6*s5*s6*s4)*c23+ (((.5*k7*c6_2+.5*k7*c6_2.5*k3*c5*c4*c3+.5*k7*s6_2-.5*k3*s5*s3+ (k7*c6_2-k21)*s5*c5*c4_2+(-k5*c5-k7*c6*s5*s6*s4)*c4+(-k7*c6_2+ k21)*s5*c5-k4*s5)*QD(5)+((k7*c6*s6+k7*c5_2*c6*s6)*c4_2+(-k7*s6_2+ k7*c6_2)*c5*s4*c4-k7*c5_2*c6*s6)*QD(6) C(3.5*k1+.5*k7*c6_2-.5*k21)*c5_2+.5*k21)*c5_2-k4*c5+ (.5*k3*c2*s5-.5*k7*s6_2)*c5)*s23)*QD(6) C(3.5*k7*s6_2)*c5*s4)*QD(6) C(3.5*k7*c6_2+.134 D Codigo fuente funciones Simulink k7*c5_2*c6*s6)*s4*c4+(-.5*k3*s5*s2-.5*k1+.5*k7*c6_2+.5*k7*s6_2+.5*k1-.5*k3*s2*c4*c5)*s23+.5*k10+.5*k22.5*k3*s2*s4*s23*s5-.5*k1+ .5*k21)*c5_2+.5*k3*s5*s2.5*k1+.5*k3*c2*c23*s4*s5+.5*k7*c6_2+.5*k21)*c5_2-.5*k7*c6_2+.6)=(((.5*k3*s5*s3+(k7*c6_2-k21)*s5*c5*c4_2+(-k5*c5k7*c6*s5*s6*s4)*c4+(-k7*c6_2+k21)*s5*c5-k4*s5)*QD(2)+((k7*c6_2k21)*s5*c5*c4_2+(-k5*c5-k7*c6*s5*s6*s4)*c4+(-k7*c6_2+k21)*s5*c5k4*s5)*QD(3)+((-.5*k21)*s5_2+.5*k7*c6_2)*s4_2+(.5*k1+ .5*k7*c6_2-.5*k7*c6_2)*s4*QD(5)+((-.5*k7*s6_2.5*k21)*s5_2+.5*k7*c6_2)*s4*c23+(-k7*c6*s5*c4_2*s6+(-k7*c6_2+ k21)*s5*c5*s4*c4+k7*s6*c6*s5+k5*s4*c5)*s23)*QD(1)+((.5*k21)*s5_2.5*k3*c2*c4*c5)*c23+ (-.5*k7*s6_2)*s5*c4-k7*s5*c5*s4*c6*s6)*c23+ ((-k7*s6_2+k7*c6_2)*c5*c4_2+(-k7*c6*s6-k7*c5_2*c6*s6)*s4*c4+ (-.5*k7*c6_2)*s4*QD(4)+(-k5*c5-k7*c6*s5*s6*s4k4*s5*c4)*QD(5)+(k7*c4*c6*s6+(-.5*k3*s5*s4*c3+ k7*c6*c5*c4_2*s6+((k7*c6_2-k21)*c5_2+k7*c6_2-k22)*s4*c4+ k5*s4*s5-k7*c6*c5*s4_2*s6)*QD(4)+((.5*k21)*c5_2+k4*c5+(-.5*k7*c6_2)*s23)*QD(1)+(.5*k1)*c5)*s23)*QD(1)+((k7*c6*s6+ k7*c5_2*c6*s6)*c4_2+(-k7*s6_2+k7*c6_2)*c5*s4*c4k7*c5_2*c6*s6)*QD(2)+((k7*c6*s6+k7*c5_2*c6*s6)*c4_2+(-k7*s6_2+ k7*c6_2)*c5*s4*c4-k7*c5_2*c6*s6)*QD(3)+((-.5*k3*c2*c23*s4*s5+.

5*k3*c2*c23*s4*s5.5*k7*c6_2+ .5*k21)*c5_2-.5*k22+ .5*k22+.5*k7*c6_2+.5*k22+ .5*k7*s6_2+.5*k3*s5*s4*c3-k7*c6*c5*c4_2*s6+((-k7*c6_2+k21)*c5_2+ k22-k7*c6_2)*s4*c4+k7*c6*c5*s4_2*s6-k5*s4*s5)*QD(2)+ (-k7*c6*c5*c4_2*s6+((-k7*c6_2+k21)*c5_2+k22-k7*c6_2)*s4*c4+ k7*c6*c5*s4_2*s6-k5*s4*s5)*QD(3)+(-k7*c6*c5*s6*c4+((-.5*k21)*s5_2-.5*k21)*s5_2-.5*k21)*c5_2+(.5*k22)*c4_2+(-k5*s5+ 2*k7*c6*c5*s4*s6)*c4+((.5*k7*c6_2)*s4_2+(-.5*k13)*s23)*QD(3)+((-k7*c6_2+k21)*s5*c5*c23+ (((-.5*k21)*c5_2+(.5*k7*c6_2-.5*k10)*s4)*QD(5)+((-.5*k22.5*k7*c6_2.5*k7*c6_2-.5*k7*s6_2)*c5*s4)*QD(5) C(4.5*k7*c6_2-.5*k7*c6_2-.5*k21)*c5_2-.5*k7*c6_2)*c4_2+ (2*k7*c6*c5*s4*s6-k5*s5)*c4+((.5*k7*c6_2-.5*k22+.5*k13)*s23)*QD(1)+(-.5*k21)*c5_2+(.5*k7*c6_2+.5*k21)*c5_2-.4)=(k7*c6_2-k21)*s5*c5*QD(5)+(-k7*c6*s6+k7*c5_2*c6*s6)*QD(6) C(4.5*k7*c6_2-.5*k22.5*k7*s6_2)*s5*s4)*s23)*QD(6) C(4.5*k3*s2*s4*s23*s5+ .5*k7*c6_2+ .5*k21)*c5_2+.5*k1+.5*k22-.1)=((-k7*c6*c5*c4_2*s6+((-k7*c6_2+k21)*c5_2+k22-k7*c6_2)*s4*c4+ k7*c6*c5*s4_2*s6-k5*s4*s5)*c23_2+((k7*c6*s5*s6*c4+((k7*c6_2k21)*s5*c5+k4*s5)*s4)*s23-k2*s4*s5-k3*c2*s4*s5)*c23+ 135 k7*c6*c5*c4_2*s6+(((k7*c6_2-k21)*c5_2+k7*c6_2-k22)*s4-k6*s5)*c4k7*c6*c5*s4_2*s6)*QD(1)+((((-k7*c6_2+k21)*s5*c5-k4*s5)*c4+ k7*c6*s5*s6*s4)*c23+(((-.5*k13+.5*k7*c6_2.5*k21)*c5_2+.5*k7*c6_2)*s23k3*s5*c4*s2)*QD(1)+(-k3*c2*c23*s4*s5-k3*s2*s4*s23*s5k7*c6*c5*c4_2*s6+((-k7*c6_2+k21)*c5_2+k22-k7*c6_2)*s4*c4+ k7*c6*c5*s4_2*s6-k5*s4*s5)*QD(2)+(-.5*k10)*c4+k7*c6*c5*s4*s6)*s23)*QD(5)+((k7*c6*s6k7*c5_2*c6*s6)*c23+(k7*s5*c5*c4*c6*s6+(-.5*k7*c6_2+.5*k7*c6_2)*c4_2+(2*k7*c6*c5*s4*s6-k5*s5)*c4+((.5*k7*c6_2.5*k7*c6_2+.5*k7*c6_2+.5*k7*c6_2+.5*k7*c6_2+.5*k7*c6_2.5*k7*c6_2)*s4_2+(-.5*k7*c6_2+.5*k7*c6_2+.5*k7*c6_2.5*k21)*c5_2+.D Codigo fuente funciones Simulink .5*k21)*s5_2-.5*k21)*c5_2-.5*k7*c6_2.5*k21)*c5_2-.5*k21)*c5_2+.5*k7*c6_2.3)=((((-k7*c6_2+k21)*s5*c5-k4*s5)*c4+k7*c6*s5*s6*s4)*c23+ (((-.5*k7*c6_2+.5)=((-k7*c6_2+k21)*s5*c5*c23+(((-.5*k21)*c5_2+ .5*k3*s5*s4*c3-k7*c6*c5*c4_2*s6+((-k7*c6_2+ k21)*c5_2+k22-k7*c6_2)*s4*c4+k7*c6*c5*s4_2*s6-k5*s4*s5)*QD(3)+ (-k7*c6*c5*s6*c4+((-.5*k7*c6_2)*s4_2+(-.5*k7*c6_2+.5*k7*c6_2)*c4_2+(2*k7*c6*c5*s4*s6-k5*s5)*c4+((.5*k21)*c5_2-.5*k7*c6_2+.5*k21)*c5_2+(.5*k7*s6_2)*s5*c4+k7*s5*c5*s4*c6*s6)*QD(6) C(4.5*k7*c6_2- .5*k13)*s23-k3*s5*c4*s2)*QD(2)+((((-k7*c6_2+k21)*s5*c5k4*s5)*c4+k7*c6*s5*s6*s4)*c23+(((-.5*k7*c6_2-.5*k3*c2*c23*s4*s5-.2)=((((-k7*c6_2+k21)*s5*c5-k4*s5)*c4+k7*c6*s5*s6*s4)*c23+ (((-.5*k21)*c5_2+ .5*k3*s2*s4*s23*s5+.5*k1)*s5*c4+ k7*s5*c5*s4*c6*s6)*QD(6) C(4.5*k7*c6_2-.5*k7*c6_2)*s4_2+(-.5*k1-.5*k10)*s4)*QD(5)+((.5*k7*c6_2+.

5*k7*c6_2+.5*k7*c6_2-.5*k10+ .3)=(((-.5*k7*c6_2+ .5*k7*c6_2)*c4-k7*c6*c5*s4*s6)*s23)*QD(4)+((.5*k10+.5*k3*s5*s3+ (-k7*c6_2+k21)*s5*c5*c4_2+(k5*c5+k7*c6*s5*s6*s4)*c4+(k7*c6_2k21)*s5*c5+k4*s5)*QD(2)+((-k7*c6_2+k21)*s5*c5*c4_2+(k5*c5+ .5*k3*c5*c4*c3-.5*k21)*c5_2-k4*c5+(.5*k21)*c5_2-k4*c5+(.2)=(((-.5*k10+ .5*k7*c6_2+.5*k1-.5*k7*c6_2-.5*k7*c6_2)*s4*c23+(k7*c6*s5*c4_2*s6+(k7*c6_2k21)*s5*c5*s4*c4-k7*c6*s5*s6-k5*s4*c5)*s23-k3*c5*s4*s2)*QD(1)+ (k3*c2*c23*c4*c5+k3*s2*c4*s23*c5-k3*s5*s3+(-k7*c6_2+ k21)*s5*c5*c4_2+(k5*c5+k7*c6*s5*s6*s4)*c4+(k7*c6_2-k21)*s5*c5+ k4*s5)*QD(2)+((.5*k7*c6_2+.5*k7*c6_2+.5*k7*s6_2+.1)=(((-k7*c6_2+k21)*s5*c5*c4_2+(k5*c5+k7*c6*s5*s6*s4)*c4+(-k7*c6_2+ k21)*s5*c5-k4*s5)*c23_2+((((-k7*c6_2+k21)*c5_2-k4*c5+(k7*c6_2k21)*s5_2)*c4-k5*s5+k7*c6*c5*s4*s6)*s23-k3*s5*s2+k2*c4*c5+ k3*c2*c4*c5)*c23-k2*s23*s5-k3*s5*s3+(k7*c6_2-k21)*s5*c5*c4_2k7*c6*s5*s4*s6*c4+k4*s5-k6*c5*s4)*QD(1)+(((-.5*k21)*s5_2+.5*k7*c6_2+.5*k7*c6_2.5*k7*s6_2)*s5*c23+((.5*k10)*c4+k7*c6*c5*s4*s6)*s23)*QD(1)+ (-k7*c6*c5*s6*c4+((-.5*k21)*s5_2+.5*k3*c2*c4*c5-.5*k3*c2*s5+ .5*k21)*c5_2-k4*c5+(.5*k7*c6_2-.5*k21)*s5_2+.5*k7*c6_2-.5*k21)*s5_2-.5*k7*c6_2+.5*k1)*s5*QD(5) C(5.5*k10)*s4)*QD(2)+(-k7*c6*c5*s6*c4+ ((-.5*k3*s2*c4*c5)*s23-.5*k21)*s5_2-.5*k7*c6_2+.5*k7*c6_2-.5*k3*s5*s2)*c23+ (.5*k1)*s5*c4+k7*s5*c5*s4*c6*s6)*QD(2)+((-.5*k10+ .5*k7*c6_2+ .5*k10+.5*k1)*s5*c4+k7*s5*c5*s4*c6*s6)*QD(3)+(-k7*c6*s6+ k7*c5_2*c6*s6)*QD(4)+(-.6)=((k7*c6*s6-k7*c5_2*c6*s6)*c23+(k7*s5*c5*c4*c6*s6+(-.5*k7*s6_2)*c5*s4)*QD(6) C(5.5*k21)*c5_2+(.5*k21)*c5_2-k4*c5+(.5*k7*c6_2+.5*k1)*s5*s4)*s23)*QD(1)+((-.5*k7*c6_2)*s4*c23+(k7*c6*s5*c4_2*s6+(k7*c6_2-k21)*s5*c5*s4*c4k7*c6*s5*s6-k5*s4*c5)*s23)*QD(3)+((k7*c6_2-k21)*s5*c5*c23+ (((.5*k10+ .5*k3*c2*s5+.5*k7*s6_2+.5*k7*s6_2+ .5*k3*s5*s3+(-k7*c6_2+ k21)*s5*c5*c4_2+(k5*c5+k7*c6*s5*s6*s4)*c4+(k7*c6_2-k21)*s5*c5+ k4*s5)*QD(3)+(k7*c6*c5*s6*c4+((.5*k21)*c5_2+(.5*k10)*s4)*QD(3)+(k7*c6_2-k21)*s5*c5*QD(4)-k7*QD(5)*c6*c5*s6+ (-.5*k21)*s5_2+.5*k3*c2*c4*c5-.5*k21)*s5_2+ .5*k7*c6_2-.5*k3*s5*s2)*c23+(.5*k21)*s5_2-.5*k7*c6_2-.5*k21)*c5_2+(-.5*k7*c6_2-.5*k1.5*k7*c6_2)*s4)*QD(4)+ (k7*c4*c6*s6+(.5*k7*s6_2)*c5*c4k7*c6*s4*s6)*s23)*QD(6) C(5.136 D Codigo fuente funciones Simulink .5*k7*c6_2+.5*k7*c6_2)*s4*c23+(k7*c6*s5*c4_2*s6+(k7*c6_2-k21)*s5*c5*s4*c4k7*c6*s5*s6-k5*s4*c5)*s23-k3*c5*s4*s2)*QD(2)+(((-.5*k21)*c5_2+ (-.5*k7*c6_2)*s4*c23+(k7*c6*s5*c4_2*s6+(k7*c6_2-k21)*s5*c5*s4*c4k7*c6*s5*s6-k5*s4*c5)*s23)*QD(1)+((.5*k21)*s5_2+.5*k7*c6_2+.5*k7*c6_2.5*k7*c6_2-.5*k3*s2*c4*c5)*s23-.5*k7*s6_2+ .5*k7*c6_2-.5*k7*c6_2+.5*k3*c5*c4*c3-.5*k1)*s5*QD(6) C(4.5*k7*s6_2+ .5*k7*c6_2+.5*k7*c6_2+.5*k1-.

5*k7*c6_2)*s5*c4+k7*c6*s5*c5*s4*s6)*c23+ ((k7*s6_2-k7*c6_2)*c5*c4_2+(k7*c6*s6+k7*c5_2*c6*s6)*s4*c4+ (.5*k21)*s5_2+.5*k7*c6_2+.5*k21)*s5_2+.5*k1+.5*k1-.5*k7*s6_2.5*k7*s6_2-.5*k7*c6_2)*s5*c4+k7*c6*s5*c5*s4*s6)*c23+((k7*s6_2k7*c6_2)*c5*c4_2+(k7*c6*s6+k7*c5_2*c6*s6)*s4*c4+(.5)=k7*QD(6)*c6*s6 C(5.5*k7*s6_2)*s5*c4- .5*k1+.5*k7*s6_2)*c5*s4)*QD(6) 137 C(5.5*k7*c6_2+.5*k1-.5*k21)*s5_2+.5*k1+.5*k7*c6_2+.5*k7*c6_2)*c5*c4+k7*c6*s4*s6)*s23)*QD(5) C(6.5*k10+.5*k7*c6_2)*s4)*QD(3)+(-k7*c6_2+k21)*s5*c5*QD(4)+(-.3)=(((-.5*k21)*c5_2+(-.5*k10+.5*k7*c6_2-.5*k21)*c5_2+(-.5*k7*s6_2)*s5*c23+((.5*k7*s6_2)*c5*s4)*QD(2)+(k7*c4*c6*s6+ (.5*k7*c6_2-.5*k7*c6_2-.5*k7*c6_2-.5*k7*s6_2-.5*k7*c6_2+ .5*k7*c6_2)*s5*QD(6) C(5.5*k7*c6_2+ .5*k1+.5*k7*c6_2)*s5*c4+k7*c6*s5*c5*s4*s6)*c23+ ((k7*s6_2-k7*c6_2)*c5*c4_2+(k7*c6*s6+k7*c5_2*c6*s6)*s4*c4+ (.5*k1+.5*k7*s6_2.6)=((.5*k7*c6_2)*s5*QD(4)+k7*QD(5)*c6*s6 C(6.5*k7*c6_2-.5*k7*c6_2-.5*k1+.5*k7*s6_2)*c5)*s23)*QD(3)+((-k7*c6*s6+k7*c5_2*c6*s6)*c23+ (-k7*s5*c5*c4*c6*s6+(-.5*k7*s6_2)*c5*s4)*QD(3)+(-.5*k21)*c5_2+(-.5*k7*s6_2)*c5)*s23)*QD(1)+((-k7*c6*s6k7*c5_2*c6*s6)*c4_2+(k7*s6_2-k7*c6_2)*c5*s4*c4+k7*c5_2*c6*s6)*QD(2)+ ((-k7*c6*s6-k7*c5_2*c6*s6)*c4_2+(k7*s6_2-k7*c6_2)*c5*s4*c4+ k7*c5_2*c6*s6)*QD(3)+((.5*k7*s6_2-.5*k10+ .5*k7*c6_2)*c5*s4)*QD(5) C(6.2)=(((-.5*k7*s6_2)*c5)*s23)*QD(2)+(((-.4)=((k7*c6_2-k21)*s5*c5*c23+(((.5*k7*c6_2+.D Codigo fuente funciones Simulink k7*c6*s5*s6*s4)*c4+(k7*c6_2-k21)*s5*c5+k4*s5)*QD(3)+ (k7*c6*c5*s6*c4+((.5*k7*s6_2+ .5*k21)*c5_2+(-.5*k7*c6_2-.5*k1+ .5*k7*s6_2.5*k7*c6_2)*s5*c23+ ((-.5*k7*c6_2)*c4-k7*c6*c5*s4*s6)*s23)*QD(1)+ (k7*c6*c5*s6*c4+((.5*k1+.5*k1-.5*k1-.5*k21)*s5_2+.5*k7*s6_2.5*k7*c6_2-.5*k1)*s5*s4)*s23)*QD(4)+((-.5*k7*c6_2+.5*k7*s6_2-.5*k1-.5*k7*s6_2)*c5)*s23)*QD(1)+((-k7*c6*s6k7*c5_2*c6*s6)*c4_2+(k7*s6_2-k7*c6_2)*c5*s4*c4+k7*c5_2*c6*s6)*QD(2)+ ((-k7*c6*s6-k7*c5_2*c6*s6)*c4_2+(k7*s6_2-k7*c6_2)*c5*s4*c4+ k7*c5_2*c6*s6)*QD(3)+((.5*k7*c6_2+.5*k7*c6_2+.5*k7*c6_2+ .5*k1-.5*k7*c6_2)*s4)*QD(4)+(k7*c4*c6*s6+ (.5*k7*c6_2)*s5*c4+k7*c6*s5*c5*s4*s6)*c23+((k7*s6_2k7*c6_2)*c5*c4_2+(k7*c6*s6+k7*c5_2*c6*s6)*s4*c4+(.5*k7*s6_2)*s5*c4k7*c6*s5*c5*s4*s6)*QD(4)+(-k7*c4*c6*s6+(-.5*k1-.5*k1+.1)=(((-k7*c6*s6-k7*c5_2*c6*s6)*c4_2+(k7*s6_2-k7*c6_2)*c5*s4*c4k7*c5_2*c6*s6+2*k7*c6*s6)*c23_2+(2*k7*s5*c5*c4*c6*s6+(-k7*s6_2+ k7*c6_2)*s5*s4)*s23*c23+(k7*c6*s6+k7*c5_2*c6*s6)*c4_2+(-k7*s6_2+ k7*c6_2)*c5*s4*c4-k7*c6*s6)*QD(1)+(((-.5*k7*s6_2-.5*k7*c6_2-.5*k7*s6_2)*c5*c4-k7*c6*s4*s6)*s23)*QD(1)+(k7*c4*c6*s6+ (.5*k1-.5*k7*c6_2.5*k1.5*k1.5*k7*c6_2)*s4)*QD(2)+(k7*c6*c5*s6*c4+ ((.5*k10+.5*k1-.

5*k7*c6_2)*c5*c4+k7*c6*s4*s6)*s23)*QD(1)+(-k7*c4*c6*s6+(-.5*k1+.5*k7*c6_2-.5*k7*s6_2+.h" #endif .5*k1.5*k1+ .4)=((-k7*c6*s6+k7*c5_2*c6*s6)*c23+(-k7*s5*c5*c4*c6*s6+(-.5*k7*c6_2-.5*k7*c6_2)*s5*c23+((-.5*k7*s6_2.5*k7*c6_2)*c5*s4)*QD(3)+(.138 D Codigo fuente funciones Simulink k7*c6*s5*c5*s4*s6)*QD(4)+(-k7*c4*c6*s6+(-.5*k1)*s5*QD(5) C(6.5*k7*s6_2-.5*k7*c6_2-.5*k1)*s5*QD(4)-k7*QD(5)*c6*s6 C(6.5*k1+ .5*k7*c6_2)*c5*s4)*QD(2)+(-k7*c4*c6*s6+(-.6)=0.0 i++) if (enable!=0.0) for(j=0 j<6 } } j++) y i]=y i]+C(i+1.0 /* Par total */ /*----------------------------------*/ for(i=0 i<6 { y i]=0.5*k7*s6_2+ .5*k7*c6_2-.5*k7*s6_2)*s5*c4-k7*c6*s5*c5*s4*s6)*QD(2)+((.5*k7*s6_2-.5*k7*s6_2-.5*k1)*s5*s4)*s23)*QD(1)+((.5*k7*s6_2)*s5*c4-k7*c6*s5*c5*s4*s6)*QD(3)+(-k7*c5_2*c6*s6+ k7*c6*s6)*QD(4)+(.c" #else #include "cg_sfun.5*k7*c6_2+ .5*k1.j+1)*QD(j+1) static void mdlTerminate(SimStruct *S) { } #ifdef MATLAB_MEX_FILE #include "simulink.5)=((-.5*k7*c6_2)*c5*s4)*QD(5) C(6.5*k1+.5*k7*s6_2.5*k7*s6_2+.5*k1+.

D Codigo fuente funciones Simulink /* * * * * * Generacion de trajectoria articular Robot RM-10 Polinomio de 5 grado con velocidad inicial y final nula.1) /* Posicion inicial del robot*/ #define SAMPLE(S) ssGetSFcnParam(S.2) /* Frecuencia */ /* Asigna entradas */ /*----------------------------------*/ #define QF(indice) (*uPtrs 0] indice]) #define QACT(indice) (*uPtrs 1] indice]) #define MOVER (*uPtrs 2] 0]) /* Posici\'{o}n Final */ /* Posici\'{o}n Actual */ /* Orden de Movimiento */ static void mdlInitializeSizes(SimStruct *S) . Autor: Carlos Perez Fernandez 139 * fecha: 30-4-99 */ #define S_FUNCTION_NAME traj52 #define S_FUNCTION_LEVEL 2 #include <math.j) rwork 6*i+j] #define #define #define #define #define #define #define Estado(i) Tfinal Time_ini Time TimeArt(i) Moverant MOVING rwork 36+i] rwork 42] rwork rwork rwork iwork iwork 43] 44] 45+i] 0] 1] /* Asigna par\'{a}metros */ #define QMAX(S) ssGetSFcnParam(S.h" /* Asigna parametros al espacio de trabajo */ /*********************************************/ /* De 0 a 35 Matriz de coef de polinomios */ #define A(i. y aceleraciones final e inicial nulas.h> #include "simstruc.0) /* Velocidades m\'{a}ximas */ #define POS_INI(S) ssGetSFcnParam(S.

SS_OPTION_EXCEPTION_FREE_CODE) } /* * mdlInitializeSampleTimes . 1) /*Salidas*/ /*Posicion Referencia*/ /*Velocidad Referencia*/ /*Aceleraci\'{o}n Referencia*/ /*Final de la trayectoria*/ ssSetNumSFcnParams(S.Inicializa el vector de tiempos * */ static void mdlInitializeSampleTimes(SimStruct *S) { real_T *TSample = mxGetPr(SAMPLE(S)) /* Puntero a parametro frecuencia */ ssSetSampleTime(S. 0. 0) 0) D Codigo fuente funciones Simulink /* number of continuous states */ /* number of discrete states */ if (!ssSetNumInputPorts(S. 1) S. 1) if (!ssSetNumOutputPorts(S. S. 6) ssSetOutputPortWidth(S. 3)) return /*Entradas*/ ssSetInputPortWidth(S. 0.4)) return ssSetOutputPortWidth(S. ssSetNumModes( S. 1) ssSetInputPortDirectFeedThrough(S. 0. 0. 2. 6) /*Posicion Destino*/ ssSetInputPortWidth(S. 6) ssSetOutputPortWidth(S. 2.0) } /*Automatico segun bloque anterior*/ . ssSetNumPWork( S. 1) ssSetInputPortDirectFeedThrough(S. 3) if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { return /* Si faltan parametros se da mensaje */ } ssSetNumSampleTimes( ssSetNumRWork( S. 0.140 { ssSetNumContStates( ssSetNumDiscStates( S. 1. ssSetOptions(S. *TSample) ssSetOffsetTime(S. 1. 2. 51) 3) 0) 0) 0) /* Numero de muestreos */ /* Vector de numeros reales */ /* Vector de numeros enteros */ /* Vector de punteros */ ssSetNumIWork( S. 6) ssSetOutputPortWidth(S. 3. 1. ssSetNumNonsampledZCs(S. 0. 1) /*Posicion Actual */ /*Orden de Movimiento*/ ssSetInputPortDirectFeedThrough(S. 6) ssSetInputPortWidth(S.

Inicializa estados y parametros del robot * */ static void mdlInitializeConditions(SimStruct *S) { int i real_T *posini = mxGetPr(POS_INI(S)) 141 /* Puntero a Vector POS_INI */ real_T *rwork = ssGetRWork(S) for(i=0 i<6 i++) Estado(i)=posini i] } #define MDL_START #if defined(MDL_START) /* Funci\'{o}n: mdlStart * */ static void mdlStart(SimStruct *S) { int i real_T *posini = mxGetPr(POS_INI(S)) real_T *rwork = ssGetRWork(S) int_T *iwork = ssGetIWork(S) /* Puntero a Vector POS_INI */ Time_ini= Moverant = 0 0 for(i=0 i<6 i++) Estado(i)=posini i] MOVING } #endif = 0 .D Codigo fuente funciones Simulink #define MDL_INITIALIZE_CONDITIONS /* * mdlInitializeConditions .

1)=0 A(i.5))*(QF(i)-QACT(i)) } MOVING = 1 } if (MOVING==1) { Time=ssGetT(S)-Time_ini .2) if ((Moverant==0)&&(MOVER==1)&&(MOVING==0)) { Time_ini=ssGetT(S) Tfinal=0.0*ptqmax i])) /* Tiempo para articulaci\'{o}n i */ if (TimeArt(i)>Tfinal) Tfinal=TimeArt(i) } for(i=0 i<6 i++) { /* Escogemos el mayor tiempo */ /* Capturamos el tiempo inicial */ /* Si detecta flanco */ /* Calculo coeficientes del polinomio */ A(i.0) uPtrs 1] = ssGetInputPortRealSignalPtrs(S.0)=QACT(i) A(i.4))*(QF(i)-QACT(i)) A(i.j real_T *y InputRealPtrsType uPtrs 3] real_T *rwork = ssGetRWork(S) int_T *iwork = ssGetIWork(S) real_T *ptqmax = mxGetPr(QMAX(S)) uPtrs 0] = ssGetInputPortRealSignalPtrs(S.4)=(-15/pow(Tfinal.3))*(QF(i)-QACT(i)) A(i.0*(QF(i)-QACT(i)))/(8.1) uPtrs 2] = ssGetInputPortRealSignalPtrs(S. int_T tid) { int i.5)=(6/pow(Tfinal.2)=0 A(i.3)=(10/pow(Tfinal.142 /* * mdlOutputs .0 for(i=0 i<6 i++) { TimeArt(i)=fabs((15.Calcula las salidas * */ D Codigo fuente funciones Simulink static void mdlOutputs(SimStruct *S.

0) for(i=0 i<6 i++) { y i]=Estado(i) } .2) for(i=0 i<6 i++) { y i]=0 } } /* Asignamos las salidas del bloque */ y = ssGetOutputPortRealSignal(S.1) for(i=0 i<6 i++) { y i]=0 } y = ssGetOutputPortRealSignal(S.1) y i]=0 for(j=0 j<5 j++) { y i]+=(j+1)*A(i.j) } } if (Time>=Tfinal) MOVING=0 } else { /* Si no hay movimiento velocidades nulas */ 143 /*Aceleracion*/ y = ssGetOutputPortRealSignal(S.D Codigo fuente funciones Simulink for(i=0 i<6 i++) { Estado(i)=0 for(j=0 j<6 j++) { Estado(i)+=A(i.j) } y = ssGetOutputPortRealSignal(S.j+2)*pow(Time.j) } y = ssGetOutputPortRealSignal(S.j)*pow(Time.j+1)*pow(Time.2) y i]=0 for(j=0 j<4 j++) { y i]+=(j+2)*(j+1)*A(i.

c" #else #include "cg_sfun.144 D Codigo fuente funciones Simulink /* Indicaci\'{o}n fin de trayectoria */ y = ssGetOutputPortRealSignal(S.h" #endif /* Is this file being compiled as a MEX-file? */ /* MEX-file interface mechanism */ /* Code generation registration function */ .Se llama una vez finaliza la simulaci\'{o}n * */ static void mdlTerminate(SimStruct *S) { } #ifdef MATLAB_MEX_FILE #include "simulink.3) if (MOVING==1) *y=0.0 else *y=1 Moverant=MOVER } /* * mdlTerminate .

1)) 0] mxGetPr(ssGetSFcnParam(S.6)) 0] 3.0)) 0] mxGetPr(ssGetSFcnParam(S.h" #include "math.h" /*Parametros geom\'{e}tricos */ #define RGEN_dB #define RGEN_a1 #define RGEN_a2 #define RGEN_d3 #define RGEN_a3 #define RGEN_d4 #define RGEN_dG #define PI #define LIM_ART_MIN_1 #define LIM_ART_MAX_1 #define #define #define #define #define #define #define #define #define #define LIM_ART_MIN_2 LIM_ART_MAX_2 LIM_ART_MIN_3 LIM_ART_MAX_3 LIM_ART_MIN_4 LIM_ART_MAX_4 LIM_ART_MIN_5 LIM_ART_MAX_5 LIM_ART_MIN_6 LIM_ART_MAX_6 mxGetPr(ssGetSFcnParam(S.1416 +3.90 + 90 -180 +180 Grad Grad Grad Grad Grad Grad Grad Grad Grad Grad */ */ */ */ */ */ */ */ */ */ #define FLOAT_EPSILON /* Asigna entradas .0944 +2.14159265358979 (double) -2.0944 -2.1745 -2. Autor: Carlos Perez Fernandez 145 * Fecha: 11-Julio-1999 */ #define S_FUNCTION_NAME cineinv #define S_FUNCTION_LEVEL 2 #include "simstruc.D Codigo fuente funciones Simulink /* * * * * * Cinem\'{a}tica inversa del Robot RM-10 Calcula las 8 posibles soluciones y elige la que mas se acerque a la posici\'{o}n actual.2)) 0] mxGetPr(ssGetSFcnParam(S.3572 (double) (double) (double) (double) (double) (double) (double) (double) (double) (double) 1e-8 */ -2.1416 /* -135 Grad */ /* +135 Grad */ /* /* /* /* /* /* /* /* /* /* -120 10 -120 +120 -120 +120 .0944 -1.0944 +0.3572 (double) +2.0944 +2.3)) 0] mxGetPr(ssGetSFcnParam(S.5708 -3.5)) 0] mxGetPr(ssGetSFcnParam(S.4)) 0] mxGetPr(ssGetSFcnParam(S.5708 +1.h" #include "stdlib.

7) if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { return /* Si faltan parametros se da mensaje */ } ssSetNumSampleTimes( S. 0. ssSetNumIWork( S. 1. 1) /*debug*/ ssSetNumSFcnParams(S. 5)) return /*Entradas*/ ssSetInputPortWidth(S. S. SS_OPTION_EXCEPTION_FREE_CODE) } . 1) if (!ssSetNumOutputPorts(S. i. 0) /* N\'{u}mero de estados t continuo */ 0) /* N\'{u}mero de estados t discreto */ if (!ssSetNumInputPorts(S. 2. ssSetNumRWork( S. 3) /*Vector s*/ ssSetInputPortWidth(S. ssSetNumModes( S. ssSetNumPWork( S. 3) ssSetInputPortWidth(S. 6) /*Vector a*/ /*Posici\'{o}n xyz*/ /*Posici\'{o}n Actual*/ for(i=0 i<5 i++) ssSetInputPortDirectFeedThrough(S. 1. 1) 0) 0) 0) 0) 0) /* /* /* /* Numero Vector Vector Vector de de de de muestreos */ numeros reales */ numeros enteros */ punteros */ ssSetOptions(S.3)) return /*Salidas*/ ssSetOutputPortWidth(S. 4.146 D Codigo fuente funciones Simulink /* Vector n /* Vector s /* Vector a */ */ */ /*----------------------------------*/ #define n(indice) (*uPtrs 0] indice]) #define s(indice) (*uPtrs 1] indice]) #define #define #define a(indice) (*uPtrs 2] indice]) p(indice) (*uPtrs 3] indice]) pact(indice) (*uPtrs 4] indice]) /* Posici\'{o}n xyz */ /* Posici\'{o}n actual */ static void mdlInitializeSizes(SimStruct *S) { int i ssSetNumContStates( ssSetNumDiscStates( S. 1) /*Error trayectoria*/ ssSetOutputPortWidth(S. 0. 2. ssSetNumNonsampledZCs(S. 6) /*Posici\'{o}n Soluci\'{o}n*/ ssSetOutputPortWidth(S. 3. 3) /*Vector n*/ ssSetInputPortWidth(S. 3) ssSetInputPortWidth(S.

0.s23 4] c4nf 4].0. LIM_ART_MAX_2.0.0.0. 0.0.0.0. LIM_ART_MIN_3.0.0.0.0. int_T tid) { int int real_T double double double double double double double double i.1) . LIM_ART_MIN_2. dep1 4].0.0. LIM_ART_MAX_4.0.0. LIM_ART_MAX_5. y.0. LIM_ART_MIN_5.0.0. LIM_ART_MAX_6} limit_inf 6]={LIM_ART_MIN_1.c2 4].s6nf 4] x.0. den 4] p=1 minnorm = 0 c1 4].-1.0.-1.s4nf 4].0. 0.-1. CONTINUOUS_SAMPLE_TIME) ssSetOffsetTime(S.c6nf 4].s2 4].0.c5nf 4].0. 0.s5nf 4].D Codigo fuente funciones Simulink 147 static void mdlInitializeSampleTimes(SimStruct *S) { ssSetSampleTime(S.Inicializa estados y parametros del robot * */ static void mdlInitializeConditions(SimStruct *S) { } /* * mdlOutputs .0.0.0. z /* Posici\'{o}n 06 */ limit_sup 6]={LIM_ART_MAX_1.s3 4].0.c23 4]. LIM_ART_MIN_4.0.0.0. LIM_ART_MAX_3.0.0.0.0. 0.-1. 0. k minindex = 0 *output dtemp.0.0.0.0.0.0} /* Matriz de soluciones */ /*Septima columna codigos de error*/ double qNorm 8]={-1.0.-1.Calcula las salidas * */ static void mdlOutputs(SimStruct *S. LIM_ART_MIN_6} double qSol 8] 7]={0.0.0.0.0) } #define MDL_INITIALIZE_CONDITIONS /* * mdlInitializeConditions .0.s1 4]. A 4].0.-1.0.0.c3 4].0.0.-1} InputRealPtrsType uPtrs 5] real_T *outputerror=ssGetOutputPortRealSignal(S.0.0. j.

2) for (i=0 i<5 i++) uPtrs i] = ssGetInputPortRealSignalPtrs(S.atan2(RGEN_d3.x) .i) /* Calculamos la posici\'{o}n de 0 a 6 */ x = p(0)-a(0)*RGEN_dG y = p(1)-a(1)*RGEN_dG z = p(2)-a(2)*RGEN_dG-RGEN_dB /* Calculo de la primera articulaci\'{o}n */ dtemp=x*x+y*y-RGEN_d3*RGEN_d3 if(dtemp>0) { qSol 0] 0]=atan2(y. -sqrt(dtemp)) qSol 1] 0]=qSol 0] 0] qSol 3] 0]=qSol 2] 0] for(i=0 i<4 i++) { c1 i]=cos(qSol i] 0]) s1 i]=sin(qSol i] 0]) dep1 i]=c1 i]*x+s1 i]*y-RGEN_a1 A i]=(dep1 i]*dep1 i]+z*z-RGEN_a2*RGEN_a2RGEN_d4*RGEN_d4-RGEN_a3*RGEN_a3)/(2*RGEN_a2) if ((qSol i] 0]<=LIM_ART_MIN_1)||(qSol i] 0]>=LIM_ART_MAX_1)) { qSol i] 6]=2 qSol i+4] 6]=2 } } /* Calculamos tercera articulacion */ for (i=0 i<4 i++) { if(qSol i] 6]==0) { dtemp=RGEN_d4*RGEN_d4+RGEN_a3*RGEN_a3-A i]*A i] if (dtemp<=0) { qSol i] 6]=1 /*Marcamos soluci\'{o}n no valida */ qSol i+4] 6]=1 . sqrt(dtemp)) qSol 2] 0]=atan2(y.x) .148 D Codigo fuente funciones Simulink real_T *debug=ssGetOutputPortRealSignal(S.atan2(RGEN_d3.

atan2(A i].D Codigo fuente funciones Simulink } else { qSol i] 2]=atan2(RGEN_a3. p*sqrt(dtemp)) s3 i]=sin(qSol i] 2]) c3 i]=cos(qSol i] 2]) p=p*(-1) } } } /* Calculamos articulaci\'{o}n 2 */ den 0]=c1 0]*c1 0]*x*x+(2*x*s1 0]*y-2*x*RGEN_a1)*c1 0] -2*s1 0]*y*RGEN_a1+RGEN_a1*RGEN_a1+z*z+s1 0]*s1 0]*y*y den 2]=c1 2]*c1 2]*x*x+(2*x*s1 2]*y-2*x*RGEN_a1)*c1 2] -2*s1 2]*y*RGEN_a1+RGEN_a1*RGEN_a1+z*z+s1 2]*s1 2]*y*y den 1]=den 0] den 3]=den 2] for(i=0 i<4 i++) { if (qSol i] 6]==0) { s23 i]=((s3 i]*RGEN_a2-RGEN_d4)*dep1 i]+ (-RGEN_a2*c3 i]-RGEN_a3)*z)/den i] c23 i]=((c3 i]*RGEN_a2+RGEN_a3)*dep1 i]+ (RGEN_a2*s3 i]-RGEN_d4)*z)/den i] qSol i] 1]=atan2(s23 i].c23 i])-qSol i] 2] for(j=0 j<3 j++) /* Copiamos soluciones */ qSol i+4] j]=qSol i] j] } } /* Calculamos la articulaci\'{o}n 4 */ for(i=0 i<4 i++) { if (qSol i] 6]==0) { c5nf i]=-c1 i]*s23 i]*a(0)-s1 i]*s23 i]*a(1)-c23 i]*a(2) if(c5nf i]==1) { qSol i] 3]=pact(3) 149 .RGEN_d4) .

c6nf i]) for(j=3 j<6 j++) /* Otras soluciones mu\~{n}eca */ { if (j==4) qSol i+4] j]=-qSol i] j] else qSol i+4] j]=PI+qSol i] j] } } } . -c1 i]*c23 i]*a(0)-s1 i]*c23 i]*a(1)+s23 i]*a(2)) } c4nf i]=cos(qSol i] 3]) s4nf i]=sin(qSol i] 3]) } } for(i=0 i<4 i++) { if (qSol i] 6]==0) { s5nf i]=-(a(0)*(c1 i]*c23 i]*c4nf i]+s1 i]*s4nf i])+ a(1)*(s1 i]*c23 i]*c4nf i]-c1 i]*s4nf i])+ a(2)*(-s23 i]*c4nf i])) qSol i] 4]=atan2(s5nf i].150 } else { D Codigo fuente funciones Simulink qSol i] 3]=atan2(-a(0)*s1 i]+a(1)*c1 i].c5nf i]) } } for(i=0 i<4 i++) { if (qSol i] 6]==0) { s6nf i]=n(0)*(-c1 i]*c23 i]*s4nf i]+s1 i]*c4nf i])+ n(1)*(-s1 i]*c23 i]*s4nf i]-c1 i]*c4nf i])+ n(2)*s23 i]*s4nf i] c6nf i]=s(0)*(-c1 i]*c23 i]*s4nf i]+s1 i]*c4nf i])+ s(1)*(-s1 i]*c23 i]*s4nf i]-c1 i]*c4nf i])+ s(2)*s23 i]*s4nf i] qSol i] 5]=atan2(s6nf i].

D Codigo fuente funciones Simulink for(i=0 i<8 i++) /* mantenemos \'{a}ngulos entre -PI y PI */ { for(j=1 j<6 j++) { if (qSol i] j]>PI) qSol i] j]=-1*(2*PI-qSol i] j]) if (qSol i] j]<-PI) qSol i] j]+=2*PI /* Comprobamos limites para eliminar soluciones */ if (qSol i] 6]==0) 151 { if ((qSol i] j]<=limit_inf j])||(qSol i] j]>=limit_sup j])) qSol i] 6]=2 } } } /* Elegimos la soluci\'{o}n m\'{a}s proxima a la posici\'{o}n actual */ minnorm=-1 for(i=0 i<8 i++) { if(qSol i] 6]==0) { qNorm i]=0 for(j=0 j<6 j++) qNorm i]+=(pact(j)-qSol i] j])*(pact(j)-qSol i] j]) qNorm i]=sqrt(qNorm i]) if (minnorm==-1) { minnorm=qNorm i] minindex=i } else { if (qNorm i]<minnorm) { minnorm=qNorm i] minindex=i } } } } .

152 D Codigo fuente funciones Simulink /*Asignamos la salida*/ if (minnorm!=-1) { if (qNorm minindex]<=1) { output = ssGetOutputPortRealSignal(S.0) for(i=0 i<6 i++) output i]=pact(i) *outputerror=1 /* No hay soluci\'{o}n en la soluci\'{o}n */ } } else { output = ssGetOutputPortRealSignal(S.0) for(i=0 i<6 i++) output i]=pact(i) *outputerror=1 /* No hay soluci\'{o}n posible en el espacio de trabajo*/ } } static void mdlTerminate(SimStruct *S) { } .0) for(i=0 i<6 i++) output i]=pact(i) *outputerror=1 /* Posible Discontinuidad en la soluci\'{o}n */ } } else { output = ssGetOutputPortRealSignal(S.0) for(i=0 i<6 i++) output i]=qSol minindex] i] *outputerror=0 } else { output = ssGetOutputPortRealSignal(S.

h" #endif 153 /* Is this file being compiled as a MEX-file? */ /* MEX-file interface mechanism */ /* Code generation registration function */ .c" #else #include "cg_sfun.D Codigo fuente funciones Simulink #ifdef MATLAB_MEX_FILE #include "simulink.

154 D Codigo fuente funciones Simulink .

1989. 1986. Gonzlez.r. 1998. Programa para el manejo y programacion del Robot RM10. M. Moog Gmbh. ~ M. System Robot.S.G. Analog Devices. Carlos J. J. System Robot RM10 Mantenaince Manual. 1989. 1990.r.Bibliograf a 1] 2] 3] 4] 5] 6] 7] 8] 9] 10] 11] 12] 13] 14] 15] An bal Ollero Baturone. The MIT Press. Apuntes de Robotica. Catalogo de Motores Tecnolog a Brushless. Moog Gmbh. 1989. Introduction to Robotics. Manual de Referencia de Robotic Toolbox.S. Li Slotine. McGrawHill. 1989. Moog Gmbh. Robots Manipulators. Lee K.A. Craig. Moog Gmbh.S Universidad de Sevilla.l. R. dSPACE Gmbh. John Wiley And Sons. Spong. 1995. Analog Devices. Peter I. s. Robot Dynamics and Control.T. PrenticeHall.l. P. 1993. Real Time Interface Implementation Guide. Corke. Addison Wesley. n E. Paul. 1981. Robotic Engineering.R. System Robot RM10 User Manual. AD2S90 DataSheet Revision D. Prentice Hall.C. Vidyasagar Mark W. Klafter. T. deteccion. vision e inteligencia. Ma~as Sanchez. J. 1992. Richard. Mathematics. 155 . Fu. Applied Nonlinear Control. Universidad de Sevilla. 1992.Negin R. 1996. T158-11X Controller User Manual. Chmielewski. Robotica: Control. And Integrated Approach. Programming and Control. C. s. 1999. System Robot.