P. 1
Simulación y Control de un robot manipulador

Simulación y Control de un robot manipulador

|Views: 85|Likes:
Published by Jota Carreño

More info:

Published by: Jota Carreño on Sep 01, 2012
Copyright:Attribution Non-commercial

Availability:

Read on Scribd mobile: iPhone, iPad and Android.
download as PDF, TXT or read online from Scribd
See more
See less

07/11/2013

pdf

text

original

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

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

. . . . 64 4. . . . . . . . . . . .5 2. . .1 3. . . . . . . . . .3 3. . . . . . . . Generacion de trayectorias lineales. . . . . . . Situacion de los sistemas de coordenadas . . . . . 3. . Diagrama de bloques asociado al modelo . . . . 1. . . . .3 Seguimiento en posicion para control tipo PD . . . . . . . . . . . . . .2 Diagrama Simulink para la simulacion de un controlador PD . . . . . . . . . 2. . . .2 2. . . . . . .7 Convenio Denavit-Hartenberg . . . . . . . . . . Caracter stica estatica de friccion teorica . . . . Diagrama de bloques Simulink para la simulacion del manipulador . . . . . . . . . . . . . . . . . . . . . . . 4. . . . . .1 Estructura general de un robot industrial . .7 Esquema de interface con el robot RM-10 . . . . . . . . Vectores de orientacion en la herramienta del manipulador Soluciones para las tres ultimas articulaciones . 66 4. . . . . . 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. . 3. . . . 3. . . . . . . . . . . . . . . . . . . . . . . . . . Caracter stica de friccion experimental del eje 4. . . . . . . . . . . Caracter stica de friccion experimental del eje 5. . . . .5 3. . . . . . . . . . . . . .9 Evolucion de las posiciones de las articulaciones 5 y 6 . .4 Vista general del robot industrial RM-10 . . .3 2. . . n 1. . . 1. . .1 Esquema de control lineal tipo PD . .5 Se~ales proporcionadas por los resolvers .2 Brazo mecanico del robot industrial RM-10 con indicacion de sus movimientos posibles . 1. . .6 3. . . 1. . . . . . . .6 Representacion esquematica de un servoampli cador . . . Trayectorias articulares polinomicas de 5 orden . . 67 5 . . . .1 2.4 2. . . . .4 3. . . . . Caracter stica de friccion experimental del eje 6. . .8 Diagrama de bloques para la simulacion del modelo en bucle abierto . . . . . . . . . . . . . . . . . . . . . . . . . . .3 Control en un robot industrial . . . . . . . .2 3. . . . .6 3. . . .Indice de Figuras 1. . . . . . . . . .10 Evolucion de las velocidades de las articulaciones 5 y 6 . . . . . . .

. . . .7 Velocidades para control tipo PD en los ejes 4. . . . . . .16 Diagrama Simulink para la simulacion de un controlador Par Calculado . . . . . 4. . . . . . . . . . .1 Etapa de entrada digital de la tarjeta E/S digitales . 4. . . . . . . 4. . . . . . n 4. . . . . . . .18 Error de seguimiento en posicion para control tipo Par Calculado 4. . . . .4 Error de seguimiento en posicion para control tipo PD . . . . .6 Seguimiento en posicion para control tipo PD en los ejes 4. . . . . n 5y6 . . . . 5 y 6 . . . . . . n 4. . . . . . . . . . . . . . . . . . .20 Seguimiento en posicion para control tipo par calculado en los ejes 4. . .8 Error de seguimiento en posicion para control tipo PD en los ejes 4. . . . . . .19 Se~ales de control para regulador tipo Par Calculado . . . . .2 Etapa de salida digital de la tarjeta E/S digitales . .5 Se~ales de control para regulador tipo PD .14 Bucle de linealizacion y desacoplo . n 4. . . . . . . . . . . .23 Se~al de control para control tipo par calculado en los ejes 4. . . . . . . .2 Conectores presentes en la tarjeta CRE . . . 4. . 5 y 6 4. . . . A. . . . . . .11 Seguimiento en posicion para control tipo PID . . . . . . . . B. . . . . 5 y 6 . . .22 Error de seguimiento en posicion para control tipo par calculado en los ejes 4. . . . . . . . 4. . . . . . . . . B. . .1 Diagrama de bloques de la tarjeta CRE . .13 Se~ales de control para regulador tipo PID . . . . . . . . . . . 5 y 6 . . . . 4. . . . . . . . . . . . . . . . . . .6 INDICE DE FIGURAS 4.4 Funcionamiento interno del circuito integrado AD2S90 . 4.17 Seguimiento en posicion para control tipo Par Calculado . . . . . . . . . . . . . . . . . . B. . . . 5 y 6 . . .21 Velocidades para control tipo par calculado en los ejes 4. . . . . . . 5 y6 . . .3 Esquema del ltro activo para la se~al de referencia . . . . . . . . 4. 4. . . 4. . . . . . . . . . . . . . n B. . . . . .10 Diagrama Simulink para la simulacion de un controlador PID 4. . . . . . . . . . . . . n 4. . A. . . . 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 . 4. . . . . . . .12 Error de seguimiento en posicion para control tipo PID .9 Se~al de control para control tipo PD en los ejes 4. . . 4.15 Esquema completo de control por par calculado . . . . . . . . 5 y 6 . . . . .

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

que a su vez son: Manuales.2. dirigidos por un operario. pero que repiten siempre lo mismo. A principios de los 60 Unimation realiza los primeros proyectos de robots industriales. En 1967 instala un conjunto de robots en una factor a de General Motors. como los siguientes: 1. Secuenciales. los laboratorios de la Universidad de Stanford y del MIT acometen la tarea de controlar un robot mediante un computador.8 1 Introduccion Por esto. 5. con el objetivo de dise~ar una maquina exible. 1. En 1950 se dise~an manipuladores amo-esclavo para manejar materian les radiactivos. instalando el primero en 1961. se patento en 1956 un manipulador programable que fue la semilla para la robotica industrial. 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. 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. Manipuladores. fundamentalmente en el sector automovil stico. adaptable al n entorno de trabajo. capaces de tomar decisiones adecuadas a cada situacion. Tres a~os desn pues se inicia la implantacion de robots industriales en Europa.2 Clasi cacion de los robots industriales La evolucion a dado origen a una serie de tipos de robots. naciendo as los robots inteligentes. 4. 3. En la historia de la Robotica Industrial se pueden distinguir varias etapas segun el nivel de desarrollo: 1. 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. 2. . son sistemas mecanicos en los que se puede gobernar los movimientos. En 1970. Secuenciales.

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

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

El controlador ademas de realizar todas estas tareas es necesario que supervise el funcionamiento del conjunto. .2 Introduccion a la Robotica Industrial 3 11 Brazo 1 Antebrazo 6 2 5 Muñeca 4 Figura 1. actuando adecuadamente en el caso que algun componente del sistema fallase. como por ejemplo sistemas de vision o sensores de esfuerzos.1. de esta manera es posible controlar como el robot se desplaza de un punto a otro. 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.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. Es el usuario normalmente quien especi ca que tipo de trayectorias debe seguir el robot.

como por ejemplo garras de sujeccion. taladros.. La ventaja de los . Los sensores de posicion que aparecen en los robots industriales son: Potenciometros.. Resolvers. Para medir la velocidad se pueden usar dinamos tacometricas o bien sensores inductivos. adicionalmente tambien pueden llevar sensores de velocidad. etc. Encoders. si bien esta se puede estimar a partir de la medida de la posicion. seguido de los resolvers. maquinas de soldar.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. Existe gran variedad de herramientas. Inductosyn. En robots industriales cuyos ejes son de rotacion los sensores de posicion mas usados son los encoders.12 qref q_ref 1 Introduccion q Control Servos Control Trayectorias q_ Figura 1. 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.

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

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

5 2 2.1.5 4 x 10 4. 1.5 −3 0 S −5 0 0.5 2 2.5 4 x 10 4.5 1 1.5 3 3.5 1 1. Tension referencia: 10V pp Relacion de transformacion: 0.5 4 x 10 4.5 Corriente maxima de entrada: 10 mA.3.5 2 2. 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 15 C 0 −5 5 0 0. Las formas de estas se~ales son idealmente n como las de la gura 1. La se~al de referencia es proporcionada a cada resolver por su corresponn diente ampli cador de potencia.5 1 1.5 3 3.5 −3 Figura 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.3 Descripcion del robot industrial RM-10 10 5 Ref 0 −5 −10 5 0 0.5 3 3.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: .5. Las caracter sticas electricas mas importantes de los resolvers son: Frecuencia portadora: 3500 Hz.

Ademas el rack controlador posee tarjetas con interfases a distintos perifericos: Puertos serie. Tarjetas de entrada y salida digital. Electronica de potencia En la parte delantera inferior del armario de control se encuentran los dispositivos de potencia del robot. Dispositivos de almacenamiento. estando toda la labor de control centralizada en una unica tarjeta procesadora basada en un procesador Motorola 68020 a 25 Mhz. 1 Introduccion Rack controlador El rack controlador se encuentra ubicado en la parte delantera superior del armario.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. La energ a electrica es suministrada al armario controlador en forma trifasica con una tension nominal de 380V. . El sistema esta basado en bus VME. Servoampli cadores. Rack controlador. Zona de entrada y salidas.

de habilitar los servoampli cadores de modo que estos puedan recibir consignas. . Tambien aparecen en esta zona dos contactores: Contactor CP: Es el contactor principal que proporciona tension a la fuente de alimentacion de los servomotores. Un contacto auxiliar de este rele activa la fuente AL2. Fuente no regulada formada por el transformador TR3 y el puente de diodos PD. 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. Zona de entrada y salidas La parte posterior del armario alberga tarjetas de entradas y salidas digitales.Se encarga. En concreto existen las siguientes fuentes: Fuente AL1 +24V con salida regulada. por lo que deja a los servoampli cadores totalmente operativos. 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 . 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.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. 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. Fuente AL2. Este contactor se cierra cuando el usuario pulsa el boton START del panel frontal del armario.1.

estos son: Pulsador de emergencia del panel frontal del armario. IFR . excitar la bobina del contactor CF (contactor de frenos). Las salidas U1 y U2 estan condicionadas a que no este activado ningun dispositivo hardware de emergencia. Pulsador de emergencia de la pistola de programacion. Tienen interes las siguientes entradas: EM6 .Se activa cuando la fuente de alimentacion de los servoampli cadores se sobrecalienta. 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.Se pone a nivel bajo cuando se activa un contacto de alarma como los descritos anteriormente. Finales de carrera presentes en todas las articulaciones del robot excepto en la articulacion 6. IN8 .Esta salida activa la entrada de limitacion de par o velocidad de los servoampli cadores. IN9 . as como para activar y desactivar indicadores luminosos.Se activa cuando la resistencia de disipacion de la fuente de alimentacion se funde. IN10 .Se activa cuando el contactor de frenos se cierra liberandolos. n .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. Otras salidas se encargan de actuar sobres las servovalvulas de aire comprimido para la herramienta del manipulador. 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. 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.18 1 Introduccion U2 . CR5 .

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. de modo que la consigna se traduce en el porcentaje de velocidad maxima de la con gurada para ese motor. . 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. La alimentacion de potencia de los servoampli cadores son un bus de corriente continua de 300V que proporciona la fuente de alimentacion. La corriente maxima para cada motor viene determinada por un modulo de optimizacion insertado en cada servoampli cador.1.6: Representacion esquematica de un servoampli cador Los servoampli cadores pueden funcionar en modo corriente o en modo velocidad. En el primer modo el servoampli cador cierra un bucle de corriente mediante sensores de efecto Hall. En este modo la consigna se traduce en el porcentaje de la corriente maxima admisible que se hace circular por el estator del motor. Dicha velocidad maxima se con gura de entre un rango de valores discretos y se a na con un potenciometro de escalado. En modo velocidad el servoampli cador cierra un bucle de velocidad sobre el bucle de corriente. La conmutacion se realiza en funcion de la posicion del rotor que es realimentada por el resolver.

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. Numerosos perifericos de entrada y salida. 1. 4 Mbyte de memoria DRAM. 1.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. Adicionalmente es posible ajustar un o set de velocidad. 3 Tarjetas conversoras de resolver a encoder incremental (CRE). Adicionalmente se instalo mas hardware ya sea comercial o desarrollado a medida para el presente proyecto.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. ubicado en los laboratorios del Departamento de Ingenier a de Sistemas y Automatica de la Universidad de Sevilla. Una tarjeta controladora de la rma dSPACE modelo DS1103.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. Subsistema DSP esclavo de Texas Instrument TMS320F240 para funciones avanzadas de entrada salida. Una tarjeta de entradas y salidas digitales.

Para la implementacion de los algoritmos de control en la tarjeta controladora. 7 L neas de encoder incremental.8 s. 1 Interface para bus CAN.2 y la ToolBox de simulacion SIMULINK 2.0.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. { Entradas tipo Input Capture.2 Software de desarrollo Para el dise~o y simulacion de controladores se usa como herramienta MATn LAB v5. 4 canales de entrada analogicas de 12 bits y 0. { Entrada de fuentes de interrupciones y relojes externos.0. 32 se~ales digitales con gurables como entrada o salida. { UART con gurable como RS-232 o RS-422.1. n UART con gurable como RS-232 o RS-422. Sobre las tarjetas CRE y la tarjeta de se~ales digitales se puede encontrar n mas informacion en los apendices B y A respectivamente. Microcontrolador esclavo TMS320F240: { 16 canales de entrada analogicas de 10 bits y 6 s. de tiempo de { { { { { { conversion. { Salida para generacion PWM. { Salidas tipo Output Compare.4. 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. 8 canales de salidas analogicas de 14 bits y 6 s. 1.

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.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. 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.22 1 Introduccion Time Interface Library proporcionada por la rma dSPACE junto con la tarjeta. 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.7: Esquema de interface con el robot RM-10 . 1.4.

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

1 Tii. c ic i.1 i i. . y as de esta manera crear una matriz con los parametros cinematicos.2 muestra la colocacion de los distintos sistemas de coordenadas. 1 Figura 2. Segun el convenio adoptado en la gura 2. di 1 Yi Zi Xi i ai.24 i.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. c i. Zi. 1 vendra dada por la expresion 2. =6 s s 4 c is i.1 la matriz de transformacion de la articulacion i a la articulacion i . La gura 2. 1 Yi.1) .2. 1 ci . dic i.1 2 Cinematica del robot RM-10 Zi. 1 1 ai. 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. a partir de dicha gura se obtiene la tabla 2.dis i. 6 s i c i. 0 0 0 1 1 1 1 1 1 1 1 1 2 3 7 7 5 1 (2.s i. .s i 0 ai. i i.

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 .

s 0 6 0 T = 6 .s .4) c .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.2) 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.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.s .1 1 2 Cinematica del robot RM-10 ai.5) .1 y la tabla de parametros cinematicos 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.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.26 i.s 0 6 0 T = 6 .2 se obtienen las seis matrices de transformacion del robot: c .3) c .1: Parametros cinematicos 1 2 3 4 5 6 1 2 3 3 4 Usando la matriz 2.

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.s 0 0 6 0 7 T = 6 s c0 .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.2.s .9) 2.1 0 7 4 0 05 0 0 0 1 5 5 4 5 5 5 2 (2.0c 1 4 0 0 0 0 6 6 5 6 6 6 2 0 0 0 1 3 7 7 5 (2.6) c . En este caso la posicion viene dada por la situacion del sistema de coordenadas solidario a la garra (sistema de referencia G). Para la orientacion se .s 0 6 0 T = 6 .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.

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.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.12) . 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.3. 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 .28 2 Cinematica del robot RM-10 s a n Figura 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. s s s ay = .18) (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. c c c ) .c (s c c . c c s c . c c c ) . .a s . c s c c + c c s 1 6 23 4 5 23 5 (2.3 Problema cinematico directo 29 1 23 4 6 1 4 5 6 1 4 6 ny = s c (c c c .17) (2.s (c c s + s c ) .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 .21) (2. c c 23 4 5 23 5 Y para la posicion: xG = a c + a c c + a c c . d c s + dGax 1 1 2 1 2 3 1 23 3 1 4 1 23 (2.22) (2.13) (2. d s s + dGay 1 1 2 1 2 3 1 23 3 1 4 1 23 zG = .23) yG = a s + a s c + a s c + d c . s s c s + s c c sy = s s (s s . s s ) + s c s 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 = . a s .c (c c s + s c ) .14) nz = . d c + dGaz + dB 2 2 3 23 4 23 La implementacion software de estas ecuaciones se realiza en la funcion pcd.2. d s . c s s 1 23 4 5 23 5 1 4 5 1 4 5 az = s c s .15) (2.16) (2.c en forma de bloque para su uso en diagrama Simulink.

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

28) (2.s (T ).26 tenemos la ecuacion: . 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.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.27 obtenemos: .28 en la ecuacion 2.4) del sistema de ecuaciones 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 ).2.26) A partir del elemento (2.25) Realizamos la operacion: c 6 .29) Sustituyendo 2.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. 7 G z5 1 0 1 6 3 1 (2. T B (T G).s x + c y = d 1 1 3 (2.

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

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

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

=. = .4: Soluciones para las tres ultimas articulaciones .2. (c s s + c c ) sy + s s sz s = . s c ) nx .47) Con lo que se puede resolver el angulo 6 6 6 como: 6 = atan2(s c ) (2. 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. (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 = .1) aparecen las ecuaciones: 35 c = . s c ) sx . (c c s .1) y (3.4 Problema cinematico inverso En los elementos (1. (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. 4 5 6 (2.

c calcula las soluciones posibles.c) para diagrama de bloques de Simulink.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. De las soluciones que quedan se elige la mas cercana a la posicion actual de robot. Ademas recibe como parametros las distancias que de nen la cinematica del manipulador. de las cuales descarta las que se quedan fuera del espacio de trabajo del robot. La funcion recibe como entradas: ~ 1.4.2 Implementacion software El problema cinematico inverso se ha inplementado como una funcion S (la funcion cineinv. Las trayectorias son generadas en tiempo real a partir de la posicion actual del manipulador. 2. Vector de variables articulares correspondientes a la posicion actual del robot. Se consigue que el movimiento de todas las .~ y ~ .5. 2. ns a 3.36 2 Cinematica del robot RM-10 2. Y se tienen como salidas: 1. Vector de variables articulares solucion 2. Vector posicion cartesiana de la herramienta P .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. Vectores de orientacion de la herramienta ~ . La funcion cineinv. Se~al de error en caso de que no haya solucion en el espacio de trabajo n del robot. 2.

2 0. La velocidad articular maxima alcanzada se produce en t = T=2 y toma el valor: 15 f .8 1 1.8 2 00 0 −1 −2 0 0.8 2 00.4 1.2 1.8 2 1 Figura 2.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. i) a = T6 ( f .2.5 0 1 0 0.2 1. T ( f .2 1.8 1 t 1. o) (2.52) max = 8 T 3 3 4 4 5 5 .6 0.50) Los coe cientes de los polinomios son calculados en funcion de las condiciones de contorno.5 Generacion de trayectorias 37 articulaciones comience y termine a la vez.4 0.4 0. obteniendose: 10 a = T ( f .4 0. El tipo de trayectoria usado es un polinomio de quinto orden tal y como puede verse en al gura 2.5 0 2 0 0.6 0.6 1.8 1 1.5 1 0.4 1.2 0. as como un movimiento suave de las articulaciones.6 1.2 0.6 0. i 0 (2.4 1. i) 15 a = . 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.6 1.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.

c. El bloque genera una secuencia de posiciones y velocidades de referencia. la posicion actual del manipulador y la se~al digital que activa la generacion n de la trayectoria a cada anco. traj52. La generacion de este tipo de trayectorias esta implementada en la funcion Simulink. 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. En primer lugar se dispone de el bloque trajlin52. que a su vez se descompone en distintas tareas espec cas. Esta tarea es realizada por un subbloque Simulink.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. 2. La posicion inicial del espacio cartesiano se obtiene mediante el bloque .6: Generacion de trayectorias lineales.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.5. La funcion recibe como parametro un vector de velocidades articulares maximas.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 entradas la posicion deseada.

yaw a partir de la parte rotacional de la matriz de transformacion obtenida con las ecuaciones 2. sy cr cy spcr + sy sr 4 sy cp sy sp sr + cy cr sy sp cr .23.c realizando los calculos del problema cinematico directo.c que se encarga de resolver la cinematica inversa del manipulador segun el procedimiento descrito en el punto 2.54) En el caso de c = 0. 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.5 Generacion de trayectorias 39 pcdcar.55) Las consignas en el espacio cartesiano generadas son enviadas al bloque cineinv. una solucion del problema cinematico inverso.4.2.21 a 2.90 =0 =0 = atan2(r r ) = .12 a 2. . Ademas este bloque obtiene la orientacion de la herramienta del manipulador en terminos de los angulos roll. habra que jar y calcular de la forma: = 90 = . 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 . pitch y yaw.1. proporcionando referencias en el espacio articular al controlador. cy sr . Si c 6= 0: = atan2(r r ) q = atan2(.r r +r ) = atan2(r r ) 32 33 31 2 11 2 21 21 11 (2.20.53) A partir de esta matriz se pueden deducir los angulos de roll. En este caso el sistema genera una alarma bloqueando el manipulador.sp cpsr cp cr 3 5 (2. o sea a partir de las ecuaciones 2. pitch.atan2(r r ) 12 22 12 22 (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. estas formulaciones pueden resultan mas ine cientes computacionalmente ya que existen numerosas operaciones del tipo multiplicacion por ceros. Este tipo de operaciones no se producen con una formulacion simbolica del problema ya que todas estas operaciones se realizan a priori. La obtencion de las ecuaciones de movimiento se puede obtener mediante las ecuaciones de la dinamica clasica o bien mediante la mecanica lagrangiana.Cap tulo 3 Dinamica del robot RM-10 3. En general se pueden generar soluciones numericas o bien soluciones simbolicas. Todas estas manipulaciones se pueden realizar mediante herramientas de calculo simbolico como MAPLEV.1 Introduccion Antes de abordar el problema de control del manipulador es necesario obtener un modelo dinamico. Si bien las soluciones numericas resultan mas compactas en su formulacion mediante algoritmos como el de Newton-Euler. 41 . tambien lo es para comprender el problema de control de los manipuladores. El modelo dinamico no solo es util para poder simular el comportamiento del sistema. ganando mas aun en e ciencia.

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 .3) donde D(q) es la matriz de inercias del sistema.4) .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. @L = Q i dt @ q_i @qi i = 1 :::n (3.U (3. 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.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. 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. las fuerzas generalizadas son equivalentes a los pares aplicados a cada articulacion. L se conoce como lagrangiana del sistema y se calcula como: L=T .42 3 Dinamica del robot RM-10 3. siendo esta simetrica y de nida positiva. En este caso las variables elegidas como grados de libertad son las variables del espacio articular del manipulador.

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

Bm friccion viscosa del motor. m par electrico desarrollando por el motor. qm posicion del eje motor.44 3 Dinamica del robot RM-10 El par motor es proporcional a la corriente que recorre los devanados del estator.10) (3. L par de carga que ofrece el brazo manipulador. por lo que interesa en primer lugar calcular los . Jm inercia del eje motor.8) Kt constante de par del motor.5. Si se sustituye la expresion 3.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. por lo que se puede poner: Kt Im = Jmqm + Bm q_m + donde: L (3.6 en las ecuaciones 3.12) 2 2 Los terminos de Coriolis se pueden calcular como funcion de los dij segun se ve en la expresion 3.7 obtenemos: (D(q) + JmR )q + (C (q q_) + Bm R )q_ + F (q q_) + G(q) = Kt ImR (3.8 se tiene: (3.9) (3. 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.

15) Identi cando esta ecuacion con la ecuacion 3.18) .13. Sustituyendo estas expresiones en 3.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.3.2 Modelo Euler-Lagrange 45 elementos de la matriz de inercias expresandolos como funcion de las matrices de transformacion del manipulador. 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.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.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. 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.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.3 obtenemos la expresion para la matriz de inercias: D(q) = n X i T (mi JvTci Jvci + J!i IiJ!i ) (3.

1].1.m].m]:=diff(Mat n.46 Donde: 8 < 3 Dinamica del robot RM-10 (Tij ).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.x) for n from 1 to 4 do for m from 1 to 4 do Mat n. > Dvect := proc(Vect) Vel:=matrix(4.1) for n from 1 to 4 do Vel n. 0 > T := proc(p) M:=array(identity.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. > Dmatrix := proc(Mat.t) od RETURN(Vel) end . ~ si j k 1 J!ij = : i ~ 0 si j > i (3.1.4..1]:=diff(Vect n.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.1.1. r .0.1.1..4..n]:=vaux k.6) for n from 1 to i do vaux:=matrix(4.1.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.3.3. Vectores centros de masa ~ci .4) J:=array(sparse. 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..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. 0.. > calJacw:= proc(i) A(0):=array(identity.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.

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

5 aprovechando la simetr a lo podemos poner como: n X n @dkj .k)*QD(i)'.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).. 1 @dij )q_ q_ = 1 X( @dkj + @dki . > C := proc(k. @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.j)) od od Los elementos del vector de pares gravitatorios se calculan mediante el tercer termino de la expresion 3. o lo que es lo mismo con las instrucciones que siguen a continuacion: .'i'=1.j.3.j) resu:=sum('Cor(i.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.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.qd).5.j]:=eval(C(i. El segundo termino de la ecuacion 3.

> Vtot:=simplify(sum('V(k)'.. 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 ) . simp) Vtot := .50 3 Dinamica del robot RM-10 > V:=proc(i) m(i)*g*evalm(T(i)&*rcm(i)) 3.siderels. Las distancias que aparecen en las matrices de transformacion son las que aparecen en la tabla 3. 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.theta i]) od: 3. . > 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 ) . g (m(5) y5 + m(6) z6) cos(%1) cos( 5 ) + m(1) g z1 .6). V := proc(i) m(i)?g?evalm((T(i)) & (rcm(i))) end 3 1 Calculamos la energia potencial total del brazo manipulador.1 Entre los parametros dinamicos necesarios para la caracterizacion de un modelo tenemos los siguientes: Tensores de inercia. ademas esto permite poder aplicar tecnicas que se basan en el modelo del manipulador con mas exito.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.1] end Calculo de los terminos de energia potencial. 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.'k'=1.

Coe cientes de reduccion.3.3.1: Parametros cinematicos del robot. 3. Centros de gravedad. permitiendo tener as una matriz diagonal en cada enlace. Se hace la suposicion de que hay simetr a en los tres ejes. 3.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. 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.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. Parametros de friccion.3. Las inercias de los ejes se obtiene a partir de las hojas de catalogo: .3 Parametros del modelo del robot 51 dB 950 a 260 a 710 a 150 d 40 d 650 dH 150 Tabla 3. 1 2 3 3 4 Distancias en mm. Masas. Con la misma herramienta informatica se estimaron los centros de gravedad y las masas de los distintos elementos. Inercias de los ejes de los motores.

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: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.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 .

Kt (Nm=A:) Imax (A:) 1 0.3.47 15 4 0.31 30 Tabla 3.52 10 2 0.235 Tabla 3.05 2 5.05 3 1.235 6 0.3. Los coe cientes de las reductoras tienen los valores de 3.58 4 0.475 5 0.2: Inercia asociada a los ejes de los motores.52 10 3 0. Motor Eje Ratio Reduccion 1 121 2 153 3 105 4 59 5 80 6 50 Tabla 3.40 26 5 0.3 Parametros del modelo del robot Motor Eje Inercia (kg=cm ) 1 5.21) vTOT = (Fv + Fvm R )q De este modo se puede identi car la friccion total como se vera mas adelante. con la que es posible conocer el par aplicado por los motores a partir de la intensidad que circula.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.31 30 6 0.4: Caracter sticas electricas de los motores 2 2 Cada motor tiene asociada una constante de par. .

22) = 23 Kt Imax 10 Los valores que se tienen para los distintos ejes son los de la tabla 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. si u es la se~al de control: n p u (3.3.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.4. .3 Parametros de friccion Por medio de la realizacion de experimentos es posible estimar algunos parametros de friccion. El resto de articulaciones se mantiene a su valor de cero. como son la friccion viscosa y la friccion de Coulomb. Si se mantiene una frecuencia baja para la se~al n inyectada el par aplicado se usa basicamente en vencer la friccion. que se ha supuesto simetrica. 30 20 10 Fc Fv f 0 −10 −20 −30 −4 −3 −2 −1 q_ 0 1 2 3 4 Figura 3.2.2: Caracter stica estatica de friccion teorica de manera que se pueda estimar una caracter stica estatica como la de la gura 3. De manera que el par aplicado por los motores vale.

que se calculan a partir de los parametros elementales y permiten escribir las ecuaciones del estilo: . En la curva obtenida para el eje 5.4. En la gura 3. Se ve que existe un fenomeno de histeresis en la curva. 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. la gura 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.3. por lo que puede ocurrir que sea demasiado costoso computacionalmente hablando a la hora de realizar simulaciones o realizar control. Mediante manipulacion simbolica es posible obtener un conjunto de parametros ki algo mas reducido.3 se puede ver el resultado obtenido para la articulacion 6 del manipulador.3: Caracter stica de friccion experimental del eje 6.4 Minimizacion de los parametros Las ecuaciones dinamicas del manipulador comprenden cientos de terminos. Sabiendo esto y suponiendo los niveles de Coulomb simetricos se puede deducir su valor. 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.

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:

La implementacion se realiza mediante dos bloques S-Function programados en C.6 para facilitar la compilacion del las funciones.7. Como opciones de simulacion.6: Distribucion de terminos en las ecuaciones del manipulador simplicadas. En las gura 3. En las guras 3.6 Simulaciones del modelo Con objeto de comprobar si el modelo se comporta segun lo previsto se simula el sistema en bucle abierto. 3. 3.5 Implementacion informatica El modelo del manipulador de la gura 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. El bloque rm10modelcor.9y 3. En el bloque rm10model.6 se encuentra disponible en Simulink para la relalizacion de simulaciones. es posible "desactivar" tanto los terminos de Coriolis y centr petos como los efectos de friccion. el subsistema se presenta en la gura 3.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.c implementa el bloque C ( _) de la gura 3.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.8 se tiene el diagrama Simulink para la simulacion del sistema en bucle abierto.10 se presentan los resultados .

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

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.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.3.

5 4 4.5 1 1.5 2 2.5 5 Figura 3.5 4 4.62 2 3 Dinamica del robot RM-10 1 5 0 −1 −2 0 0.5 2 2.5 3 3.5 t 3 3.5 1 1.5 4 4.10: Evolucion de las velocidades de las articulaciones 5 y 6 .5 4 4.5 2 2.5 1 1.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 t 3 3.5 2 2.5 5 50 6 0 _ Par Velocidad −50 0 0.5 5 8 6 4 6 2 0 −2 0 0.

1) Las ecuaciones son claramente no lineales pero ademas estan acopladas por los terminos no diagonales de las matrices D y C.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.Cap tulo 4 Control de robot RM-10 4.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 .2 Control lineal 4.2. debido a que los terminos diagonales de D y C dependen de la posicion y velocidades de las otras articulaciones. 4.

En el l mite de esta situacion el sistema se convertir a en 6 sistema lineales desacoplados. 2 4.2...2 Control tipo PD Aplicando los expuesto anteriormente se pueden dise~ar controladores lin neales para cada articulacion.2) Los coe cientes Jei y Bei son inercias y fricciones viscosa efectivas del conjunto. 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.1 se obtiene: ..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. Las inercias dependen aun de la posicion del manipulador. Con lo que se obtiene un conjunto de sistemas: u Km1 = Je1 + Be1 _ + .. . Si se calcula la funcion de transferencia en bucle cerrado del diagrama de la gura 4. u Km6 = Je6 + Be6 _ + 1 1 1 6 6 6 p1 p6 (4. El sistema real tendera mas o menos a esta situacion en funcion de los valores concretos de las magnitudes f sicas del manipulador.

dado que las articulaciones 2 .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.9 2666 152 3 20.3) Se pueden dise~ar las constantes Kv y Kp atendiendo a criterios de estan bilidad.3 120 53.1 81 1.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 .2 Control lineal Art. Las frecuencias !n se eligen segun la velocidad deseada en la articulacion compatible con la saturacion de los actuadores.3 con la expresion 4.7 35 64. Be (4. Si identi camos el denominador con la ecuacion caracter stica de un sistema de segundo orden: s + 2 !n + ! = 0 2 2 (4.06 150 40.4: !n = KpJKm e r + = Be 2JKv Km ! e n (4.83 6 0.2 33 0.2 150 64. Las variaciones son mas importantes en las articulaciones 1 y 2.1: Ganancias del controlador PD 65 GBCPD (s) = r = KpKm + Kv Kms s Je + (Be + Kv Km )s + KpKm 2 (4.4 3749 249 2 150 35 68.33 Tabla 4.1 395 22 4 0.4) Si identi cando el denominador de la expresion 4. Je !n Km Kp Kv 1 227 30 54.4.66 5 0.4 69 0.

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

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

5 3 0.04 6 0.5 2 2.5 3 Figura 4.4: Error de seguimiento en posicion para control tipo PD .5 1 tiempo 1.01 0.005 1 −0.02 0.68 4 Control de robot RM-10 0.02 e e e 6 5 0.5 1 tiempo 1.015 −0.02 0 0.01 3 −0.01 4 5 0 −0.05 0.005 e e e 2 3 1 0 2 −0.02 0 0.015 0.01 4 −0.03 0.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.2 Control lineal 69 1.5 3 Figura 4.5 2 2.5 u u u 5 4 6 0 −0.5 2 2.5 0 0.5 6 1 5 0.5 1 tiempo 1.4.5 4 −1 −1.5: Se~ales de control para regulador tipo PD n .

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

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

1 0 −0.4 1.6 0.2 1 0.4 1.6 qd1 0.2 0 0.8 tiempo 1 1.2 0.8 −1 −1.72 4 Control de robot RM-10 1.2 −0.8 0.7 0.2 0 0.8 tiempo 1 1.2 1.4 0.8 2 0.2 0.2 1.2 0.8 2 Figura 4.5 qd2 0.6 0.8 tiempo 1 1.6 1.8 2 0 −0.4 1.4 0.4 qd3 −0.4 0.6 1.7: Velocidades para control tipo PD en los ejes 4.6 −0.6 0. 5 y 6 .2 1.3 0.9 0.2 0.6 1.4 0.8 0.4 0.1 0 0.2 0.2 0 −0.6 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.

Un primer bucle mas interno que linealiza y desacopla el sistema.25 0. y un segundo bucle externo que puede ser un control lineal que estabiliza el sistema resultante. Es por ello que en las tecnicas de control se empiece a tener en cuenta la naturaleza no lineal del modelo del robot. aunque en muchos robots industriales de accionamiento indirecto porporcionan las prestaciones buscadas. Logicamente las prestaciones que se podran conseguir con un control lineal estan limitadas. Si se aplica un par de la forma: = F ( _) + C ( _) + G( ) + M ( ) 0 (4.25 1] Referencia Ref K RM−10 Articular Kd Model_RM10 Velocidad [0 −50 −50 0 −20 0] Perturbacion adicional Step Figura 4. La tecnica del par calculado basicamente consiste en el uso de dos bucles de realimentacion.9) .10: Diagrama Simulink para la simulacion de un controlador PID 4.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.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.4 −0. En el caso de que los robot no sean de accionamiento indirecto el acoplamiento entre las distintas articulaciones cobra importancia.

5 2 2.8 0.5 3 Figura 4.5 3 1 0.5 0 0.6 5 5 4 6 0.5 1 tiempo 1.5 0 3 2 1 3 −0.4 0.2 1 6 0.4 0 0.11: Seguimiento en posicion para control tipo PID .2 4 −0.5 2 −1 −1.4.2 0 −0.5 2 2.3 Control por Par Calculado 77 1.5 1 tiempo 1.

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 1 tiempo 1.12: Error de seguimiento en posicion para control tipo PID .5 2 2.5 2 2.5 3 6 4 3 2 1 3 1 2 e e e 0 −2 −4 −6 0 0.5 1 tiempo 1.5 3 Figura 4.

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

Si igualamos la ecuacion 4.10) El doble integrador resultante se puede estabilizar con controladores lineales como por ejemplo PD o PID.10 se la obtiene la ecuacion diferencial para el error: e + Kv e + Kpe = 0 _ (4.14: Bucle de linealizacion y desacoplo Igualando con la ecuacion dinamica del modelo nos queda: 0= (4. q_ _ (4.13) .15.80 4 Control de robot RM-10 0 M( ) + + _ C ( _) + F ( _) + G( ) Figura 4.12) y las matrices Kv y Kp son matrices diagonales. 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. tal y como se ve en la gura 4.11 con la ecuacion 4.11) donde: e = qd . q e = q_d .

16. En un primer caso se puede usar un bucle de . 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.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.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.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.4. n Resultados de simulacion La simulacion del controlador por par calculado se realiza mediante el esquema Simulink de la gura 4.

!n Kp Kv 1 30 900 60 2 35 1225 70 3 35 1225 70 4 120 81 1.33 Tabla 4.66 5 150 69 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.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.3: Ganancias del bucle externo tipo PD realimentacion tipo PD.16: Diagrama Simulink para la simulacion de un controlador Par Calculado Art.25 0. cuyas constantes se dise~an segun la tabla siguiente n 4.4 −0.83 6 150 33 0.3 .

5 2 2.3 Control por Par Calculado 83 1 6 0.5 2 −1 −1.2 0 −0.6 5 0.5 0 3 2 1 3 −0.5 1 tiempo 1.5 3 tiempo 1 0.4 0 0.5 2 2.5 1 1.5 0 0.8 0.4.4 5 4 6 0.17: Seguimiento en posicion para control tipo Par Calculado .5 3 Figura 4.2 4 −0.

5 3 2 1.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 1 tiempo 1.5 1 0 2 −0.5 3 1 e e e 2 1 3 0.5 3 Figura 4.5 2 2.5 1 tiempo 1.18: Error de seguimiento en posicion para control tipo Par Calculado .5 2 2.5 −1 0 0.

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

86 4 Control de robot RM-10 Resultados experimentales En las guras 4. .21 se representan las se~ales obtenidas en la experin mentacion del algoritmo de control de par calculado. 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. 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.20 a 4. la mejora con respecto al controlador lineal no es muy signi cativa.

5 3 3.5 tiempo 2 2.6 0.5 4 0.1 0 0 0.4 0.5 4 Figura 4.2 0.5 1 1.5 4 0.5 tiempo 2 2.2 0.8 0.3 0.5 1 1.7 0 0.3 1 1 r −0.5 3 3.5 tiempo 2 2.2 −0.5 1 1.4 −0. 5 y 6 .4 2 2 r 0.6 0.3 0.5 3 3.7 0.5 −0.7 0.6 −0.5 0.3 Control por Par Calculado 0 87 −0.4.5 3 3 r 0.1 0 0 0.20: Seguimiento en posicion para control tipo par calculado en los ejes 4.1 −0.

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

5 1 1.5 tiempo 2 2.005 −0.005 0 2 e −0.015 −0.5 tiempo 2 2.5 tiempo 2 2.5 3 3.015 0.01 0. 5 y 6 .5 4 0.005 −0.22: Error de seguimiento en posicion para control tipo par calculado en los ejes 4.5 4 0.5 4 Figura 4.01 0.01 −0.5 3 3.5 1 1.01 −0.5 1 1.5 3 3.3 Control por Par Calculado x 10 8 −3 89 6 4 2 1 e 0 −2 −4 −6 0 0.02 0 0.02 0 0.4.015 −0.005 0 3 e −0.

1 0 0.5 3 tiempo 2 2.02 −0.06 −0.05 0 0.15 0.5 4 0.2 0.1 −0.5 3 3.04 u −0.1 u 0.5 1 tiempo 2 2.08 −0.1 u 0.05 0 −0.05 0 −0.12 0 0.5 4 0.5 1 1.5 4 Figura 4.02 4 Control de robot RM-10 0 −0.25 0.5 1 1.5 3 3.90 0.5 1 1.15 0.2 0.5 2 tiempo 2 2.5 3 3. 5 n y6 .23: Se~al de control para control tipo par calculado en los ejes 4.05 −0.

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

. 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. En las tablas 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.1: Conexionado entradas digitales Robot.1 y A. y por parte de la tarjeta controladora dSPACE se precisa de +5V para alimentar toda la logica TTL.2 se encuentra detallada el uso de cada se~al y la numeracion usada para el conexionado.92 Vcc A Tarjeta de E/S digital OUT D IN R 1 2 1 OUT D 1 2 IC 1 Figura A. n La tarjeta precisa de alimentacion de +24V por parte del armario de System-Robot para poder activar los reles.

2: Conexionado salidas digitales . 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.

94 A Tarjeta de E/S digital .

1 la funcion de cada uno de sus 95 . Filtrado/ Ajuste de fase Conversión RE Amplificación OptoAislación Conmutación Aislación Analógica Figura B. emulando las se~ales de un encoder inn cremental de 1024 pulso por vuelta.2 se representa la disposicion f sica de los distintos conectores que dispone la tarjeta y en la tabla B. El diagrama de bloques funcional de la tarjeta se puede en la gura B. La tarjeta incorpora algunas funciones adicionales para facilitar la incorporacion del nuevo controlador.1: Diagrama de bloques de la tarjeta CRE En la gura B.1.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.

Permiten la lectura de las se~ales del resolver y de la referencia. 5. 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. 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. P4: Conmutacion de se~ales analogica y salida de la misma. n . P1/P2: Conectores de entrada y salida de resolver. 2.2: Conectores presentes en la tarjeta CRE La funcion de cada conector es la que sigue: 1. P5: Conector de alimentacion de lado del PC. 3. n 4. P6: Conector de encoder incremental.96 pines. P7: Entrada analogica proveniente del la tarjeta controladora. 6. P3: Conector de alimentacion del lado del armario de control. B.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.

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 .

1 resulta ser: 1 Vo = . Este n problema no aparece con las dos senoides inducidas ya que el resolver tiene una relacion de transformacion de 0. 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. R2 R1 + C1 IC1 Figura B.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 . El ltro permite ajustar tambien su ganancia una vez introducido el desfase.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.5. n Este desfase es necesario compensarlo ya que resulta excesivo para que la conversion de la etapa siguiente funcione correctamente. Para ello se introduce en la se~al de referencia un ltro activo de primer orden basado en n ampli cador operacional. 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. que permite ajustar el desfase introducido por el resolver.3: Esquema del ltro activo para la se~al de referencia n La funcion de transferencia del circuito de la gura B.

dando la posicion de salida en dos formatos digitales con una resolucion de 12 bits .4: Funcionamiento interno del circuito integrado AD2S90 El circuito realiza la conversion con una tecnica de tracking. CONTADOR REVERSIBLE OSCILADOR CONTROLADO POR TENSIÓN DIR CS LATCH SCLK DATOS INTERFACE SERIE Figura B. y a partir de este error incrementa o decrementa el contador hasta reducir el error a cero. 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.2 Conversion resolver a encoder 99 B. Basicamente el circuito calcula una se~al proporcional al error entre el angulo real y el n angulo almacenado en un contador reversible. Uno de los formatos es tipo encoder incremental con niveles de tension TTL.2 Conversion resolver a encoder La coversion se basa en el circuito integrado AD2S90 de Analog Devices. Este circuito permite realizar la conversion a partir de las se~ales de referencia. n y el seno y coseno inducidos.B. B. . REF SENO+ SENOCOSENO+ COSENOÁNGULO θ MULTIPLICADOR -SIN( θ-φ) DETECTOR DE FASE + INTEGRADOR VEL ÁNGULO φ NMC A B NM CLKOUT LÓGICA DE DECODIF .

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 .

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] .h> #include "simstruc.h" D Codigo fuente funciones Simulink Modelo para el robot manipulador RM-10 Implementa la matriz de inercias.106 /* * * * * * * * */ #define S_FUNCTION_NAME rm10model #define S_FUNCTION_LEVEL 2 #include <math.c". gravedad y friccin Los efectos de coriolis se aaden mediante el archivo S-Function "rm10modelcor.

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 */ .

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.2) /* Habilitar friccin */ .1) /* Aceleracion de la gravedad */ ENABLEFRIC(S) ssGetSFcnParam(S.0) /* Posicion inicial del robot del robot*/ G(S) ssGetSFcnParam(S.

0) ssSetOptions(S. 0. 1) S.3)) return /*Salidas*/ ssSetOutputPortWidth(S. 0) /*Entradas*/ if (!ssSetNumInputPorts(S. 6) /*Velocidad*/ /*Velocidad*/ ssSetNumSFcnParams(S. 3) if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { return } ssSetNumSampleTimes( ssSetNumRWork( ssSetNumIWork( ssSetNumPWork( S.D Codigo fuente funciones Simulink 109 static void mdlInitializeSizes(SimStruct *S) { ssSetNumContStates( ssSetNumDiscStates( S. 0.Inicializa estados y parametros del robot * . 0) ssSetNumNonsampledZCs(S. S. 0. 0. 0. 6) /*Posicion*/ ssSetOutputPortWidth(S. SS_OPTION_EXCEPTION_FREE_CODE) } /* * mdlInitializeSampleTimes .Inicializa muestreos */ static void mdlInitializeSampleTimes(SimStruct *S) { ssSetSampleTime(S. 1. 103) S. 2. 0) if (!ssSetNumOutputPorts(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.0) } /*Automatico segun bloque anterior*/ #define MDL_INITIALIZE_CONDITIONS /* * mdlInitializeConditions . 0. 6) ssSetInputPortDirectFeedThrough(S. 1)) return ssSetInputPortWidth(S. 12) S. CONTINUOUS_SAMPLE_TIME) ssSetOffsetTime(S. 6) ssSetOutputPortWidth(S.

650 0.00044 0.000017 0.040 0.00014 0.260 0.000041 0.000017 121 153 105 59 80 50 0.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.710 0.0839 -0.150 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*/ .0832 0.00044 0.

0236 0.0201 0.0317 0.6442 0.0027 -0.8301 0.0100 0.36 5.0050 38.0198 0.0086 0.1717 0.1202 0.5202 1.65 51.1325 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.050 1.5 /*Masas*/ /*Inercias*/ 111 /*Friccin viscosa total*/ .9982 1.4349 3.2656 3.89 7.0246 0.1 33.00 40 30 20 10 6 4.800 84.7995 0.0100 0.1876 0.1945 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.Calcula las salidas * */ static void mdlOutputs(SimStruct *S.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] .0 ffric(i)=1.0 } } /* * mdlOutputs . int_T tid) { int i.

s2*s3 y=ssGetOutputPortRealSignal(S.2) for (i=0 i<6 i++) y i] = FLAG_DEBUG(i) } #define MDL_DERIVATIVES /* .0 } for (i=0 i<2 i++) { 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 .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.

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

0) { for(i=0 i<6 i++) fricc i]=0.0 } else .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 } .0) { FLAG_DEBUG(i) = 1 fricc i]=-Coulomb(i)+y i]*rwork 67+i] } } } } } else { for(i=0 i<6 i++) fricc i]=0.0) { fricc i]=Coulomb(i)+y i]*rwork 67+i] FLAG_DEBUG(i) = 3 } else if (y i]<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).0)) { ffric(i)=1 if(y i]==0.fabs(M(i)-fG i]-forceac)) } else { ffric(i)=0 if (y i]>0.001)||(yant(i)*y i]<0.118 { for(i=0 i<6 i++) { D Codigo fuente funciones Simulink if ((fabs(y i])<0.

D Codigo fuente funciones Simulink /* Par acelerador */ /*----------------------------------*/ for(i=0 i<6 i++) f i] = M(i) .fricc i] .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 */ .

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.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" .

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

/* 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] .122 /* * * * * * */ #define S_FUNCTION_NAME rm10modelcor #define S_FUNCTION_LEVEL 2 #include <math.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.h> #include "simstruc.

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] .

1) ssSetNumRWork( S. 6) ssSetInputPortDirectFeedThrough(S. 1.1)) return ssSetOutputPortWidth(S. 6) ssSetInputPortWidth(S. 6) /*Salidas*/ /*Par*/ ssSetNumSFcnParams(S. 1) if (!ssSetNumOutputPorts(S. 1) ssSetNumPWork( S. 1) if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { return } ssSetNumSampleTimes( S. 1. 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) } .j) ENABLE(S) (*uPtrs 1] i-1]) mc i-1] j-1] ssGetSFcnParam(S. 0. 1) ssSetInputPortDirectFeedThrough(S. S. 0) 0) /*Entradas*/ if (!ssSetNumInputPorts(S. 0) ssSetNumModes( S. 2)) return ssSetInputPortWidth(S. 73) ssSetNumIWork( 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. 0) ssSetNumNonsampledZCs(S.0) /* Flag para desactivar bloque */ static void mdlInitializeSizes(SimStruct *S) { ssSetNumContStates( ssSetNumDiscStates( S. 0.

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.5*k11+k2*c4*s5)*s23-k8*c2*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+.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.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*k16*s2)*QD(2)+((((k7*c6_2k21)*s5*c5+k4*s5)*c4-k7*c6*s5*s6*s4-k5*c5+.1)=((((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*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*k18)*c23+(((-k7*c6_2+k21)*s5*c5-k4*s5)*c4.

5*k21)*c5_2+ .5*k7*c6_2+.5*k7*c6_2)*s4_2+k6*s4*s5+(-.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*k1-.5*k1+.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*k21)*c5_2+.5*k7*c6_2-.5*k21)*c5_2-.5*k18)*c23+(((-k7*c6_2+k21)*s5*c5-k4*s5)*c4.5*k7*c6_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*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)+ (((.5*k15+k5*c5+k7*c6*s5*s6*s4)*s23_2+((-k9+k3*c4*s5)*c2+(k17*s3k12+k3*c5)*s2-.5*k13)*s23*QD(4)+((k7*c6*c5*s6*c4+((.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)*c4_2 -2*k7*c6*c5*s4*s6*c4+((-.5*k7*c6_2+.5*k21)*c5_2+(-.5*k22+.3)=((((k7*c6_2-k21)*s5*c5+k4*s5)*c4-k7*c6*s5*s6*s4-k5*c5+ .5*k22+ .5*k21)*s5_2+.5*k21)*s5_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+.5*k7*c6_2+.5*k7*c6_2.5*k18)*c23+ (((-k7*c6_2+k21)*s5*c5-k4*s5)*c4-.5*k1+.2)=((((k7*c6_2-k21)*s5*c5+k4*s5)*c4-k7*c6*s5*s6*s4-k5*c5+.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*k7*c6_2+.5*k7*c6_2-.5*k22.5*k13)*s23*QD(4)+((k7*c6*c5*s6*c4+((.5*k21)*c5_2+ .5*k21)*c5_2-.5*k7*c6_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*k10+.5*k22-.5*k10+.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*k7*c6_2)*s4_2+k6*s4*s5+(-.5*k7*s6_2)*c5)*s23)*QD(6) C(1.5*k7*c6_2-.5*k7*c6_2)*c4_2-2*k7*c6*c5*s4*s6*c4+((-.5*k21)*c5_2+.5*k21)*c5_2+ (-.5*k11+k2*c4*s5)*s23-k8*c2*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*k1+.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*k21)*c5_2+(-.5*k22+ .5*k1)*s5*s4)*s23)*QD(6) C(1.5*k7*c6_2+.5*k21)*c5_2-.5*k7*c6_2+.5*k21)*s5_2+.5*k10+ .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*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*k21)*c5_2+ .5*k22+ .5*k10+ .5*k22-.5*k1- .5*k7*c6_2-.5*k13)*s23*QD(2)+(((.5*k7*c6_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*k21)*s5_2+.D Codigo fuente funciones Simulink (-.5*k7*c6_2-.5*k7*c6_2-.5*k7*c6_2)*s4_2+k6*s4*s5+(-.5*k21)*c5_2+ .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*k1+.5*k7*s6_2)*c5)*s23)*QD(6) C(1.5*k7*c6_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*k7*c6_2)*c4_2-2*k7*c6*c5*s4*s6*c4+((-.5*k7*c6_2+.5*k1+.5*k7*c6_2)*s4_2+k6*s4*s5+(-.5*k7*c6_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*k21)*c5_2-k4*c5+ (.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*k21)*s5_2+.5*k7*c6_2)*c4_2-2*k7*c6*c5*s4*s6*c4+((-.5*k21)*c5_2-k4*c5+(.5*k7*c6_2+.5*k7*c6_2+.5*k7*c6_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*k1-.5*k21)*c5_2+(-.5*k21)*c5_2+ .5*k21)*s5_2+.5*k7*s6_2)*c5)*s23)*QD(2)+(((-.5*k21)*c5_2-.5*k7*c6_2+.5*k10+.5*k7*c6_2-.5*k7*c6_2.5*k1+.5*k7*s6_2)*c5*c4-k7*c6*s4*s6)*s23)*QD(6) C(1.5*k7*c6_2+.5*k7*c6_2+.5*k7*c6_2-.5*k10+ .5*k7*s6_2+.5*k22-.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*s6_2)*s5*c23+((-.

5*k3*c2*c23*s4*s5+.5*k7*s6_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*k21)*c5_2+k4*c5+(-.5*k7*c6_2)*s4_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+.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*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*k21)*c5_2-.5*k7*c6_2+ .5*k7*c6_2.5*k21)*c5_2+.5*k7*c6_2+.5*k21)*c5_2+ .1)=((((-k7*c6_2+k21)*s5*c5-k4*s5)*c4-.5*k7*s6_2)*c5*c4-k7*c6*s4*s6)*s23)*QD(5) C(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*s5-k7*c6*c5*s4_2*s6)*QD(4)+((-.5*k22-.5*k7*s6_2)*s5*c23+ ((-.4)=((((k7*c6_2-k21)*s5*c5+k4*s5)*c4-k7*c6*s5*s6*s4)*c23+(((.5*k22+.5*k15)*s23_2+((k9-k3*c4*s5)*c2+ (-k3*c5-k17*s3+k12)*s2+.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*k1+.5*k3*s2*c4*c5+.5*k7*c6_2)*c4_2+(k5*s5-2*k7*c6*c5*s4*s6)*c4+((-.5*k7*c6_2-.5*k11-k2*c4*s5)*s23+k8*c2*s2+ .132 D Codigo fuente funciones Simulink .5*k7*c6_2.5*k7*c6_2)*c4_2+(k5*s5-2*k7*c6*c5*s4*s6)*c4+ ((-.5*k18)*c23+(((k7*c6_2-k21)*s5*c5+k4*s5)*c4k7*c6*s5*s6*s4-k5*c5+.5*k3*c2*s5)*s23.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*k7*c6_2+.5*k7*c6_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*k13-.5*k1)*s5*s4)*s23)*QD(4)+((-.5*k22-.5*k22+ .5*k7*c6_2)*s4_2+(.5*k7*c6_2-.5*k1)*c5)*s23)*QD(6) C(2.5*k3*s2*s4*s23*s5+.5*k16*s2)*QD(1)+((((k7*c6_2-k21)*s5*c5+k4*s5)*c4k7*c6*s5*s6*s4)*c23+(((.5*k21)*c5_2+.5*k3*s5*s2.5*k21)*c5_2+.5*k21)*s5_2-.5*k13-.5*k7*c6_2.5*k7*c6_2.5*k3*c2*c4*c5)*c23+(-.5*k1+.5*k7*c6_2)*s23+k3*s2*c4*s5)*QD(4)+(((.5*k7*c6_2-.5*k3*c5*c4*c3+.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*c6_2+.5*k7*c6_2+ .5*k21)*c5_2-.5*k7*s6_2.5*k7*c6_2+.

5*k21)*s5_2.5*k3*s5*s2-.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*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)+((-.5*k7*c6_2-.5*k7*s6_2+.5*k7*c6_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*k7*c6_2+.5*k7*s6_2+.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-.5*k22-.5*k3*s2*c4*c5+ .5*k10+ .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*s6_2-.5*k21)*s5_2+.5*k7*c6_2-.5*k7*s6_2+.5*k7*s6_2-.5*k7*c6_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)*QD(5)+(((-.5*k7*c6_2)*s4_2+(.5*k7*c6_2+.5*k3*c2*c23*s4*s5+.5*k7*c6_2)*s5*c4+k7*s5*c5*s4*c6*s6)*QD(4)+(k7*c4*c6*s6+ (-.5*k1+.5*k22+ .5*k7*c6_2-.5*k18+k17*s2*c3+k2*c5)*c23+(((k7*c6_2k21)*s5*c5+k4*s5)*c4-k7*c6*s5*s6*s4-k5*c5+.5*k21)*c5_2+k4*c5+ (-.5*k3*c2*s5)*s23-.5*k7*c6_2+.5)=(((.5*k7*c6_2.5*k7*c6_2+ .5*k7*c6_2+.5*k7*c6_2)*s5*c4+ k7*s5*c5*s4*c6*s6)*QD(6) C(2.5*k7*c6_2-.5*k1)*s5*c4k7*s5*c5*s4*c6*s6)*c23+((-k7*s6_2+k7*c6_2)*c5*c4_2+(-k7*c6*s6- .5*k7*s6_2+.5*k3*c5*c4*c3+.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*k21)*c5_2-.5*k10+.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*k15)*s23_2+((k9k3*c4*s5)*c2+(-k3*c5-k17*s3+k12)*s2+.5*k13-.5*k21)*c5_2-k4*c5+(.5*k7*c6_2)*s4)*QD(5)+((.5*k7*c6_2+.5*k3*c2*c4*c5)*c23+(-.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*k1)*c5*s4)*QD(6) C(2.5*k7*c6_2+ .5*k1-.5*k21)*c5_2+ .6)=(((-.5*k7*c6_2-.5*k21)*c5_2+k4*c5+(-.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*k21)*c5_2-k4*c5 +(.1)=((((-k7*c6_2+k21)*s5*c5-k4*s5)*c4-.5*k7*c6_2-.5*k1)*c5*s4)*QD(5) C(3.5*k3*s2*s4*s23*s5+.5*k21)*c5_2+.5*k1.5*k21)*s5_2-.5*k7*c6_2)*s23)*QD(4)+(((.5*k7*c6_2)*c4_2+(k5*s5-2*k7*c6*c5*s4*s6)*c4+((-.5*k7*c6_2+.

5*k7*c6_2+.5*k3*c2*c4*c5)*c23+(-.5*k7*c6_2+.5*k7*c6_2)*s5*c4+k7*s5*c5*s4*c6*s6)*QD(4)+(k7*c4*c6*s6+(-.5*k21)*s5_2+.6)=(((.5*k3*c2*s5-.5*k7*c6_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*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*k10+.5*k21)*c5_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*k21)*c5_2+.5*k22.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)=(((.5*k3*s5*s2-.5*k21)*s5_2.5*k7*c6_2)*s4*QD(4)+(-k5*c5-k7*c6*s5*s6*s4k4*s5*c4)*QD(5)+(k7*c4*c6*s6+(-.5*k22+.5*k21)*s5_2+.5*k10-.5*k3*c2*s5-.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*k3*c2*c23*s4*s5+.134 D Codigo fuente funciones Simulink k7*c5_2*c6*s6)*s4*c4+(-.5*k7*s6_2.5*k7*c6_2+.5*k7*c6_2-.5*k3*s2*c4*c5)*s23+.5*k3*s2*s4*s23*s5-.5*k7*c6_2-.5*k1+ .5*k3*c2*c4*c5)*c23+ (-.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*k7*c6_2)*s4_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*k1+.5*k13.5*k7*s6_2)*c5)*s23)*QD(6) C(3.5*k3*s2*s4*s23*s5.5*k1+ .5*k3*c5*c4*c3+.5*k1+.5*k7*c6_2-.5*k7*c6_2-.5*k7*c6_2.5*k7*c6_2)*c4_2+(k5*s52*k7*c6*c5*s4*s6)*c4+((-.5*k3*c2*c23*s4*s5+.5*k7*c6_2)*s4*QD(5)+((-.5*k7*s6_2)*c5*s4)*QD(6) C(3.5*k3*s2*c4*c5)*s23+ .5*k7*c6_2.5*k3*c5*c4*c3+.5*k7*c6_2+.5*k7*c6_2)*s23)*QD(1)+(.5*k10+.5*k21)*c5_2+k4*c5+(-.5*k3*s5*s2.5*k1-.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*k21)*c5_2-k4*c5+ (.5*k7*s6_2+.5*k1+.4)=((((k7*c6_2-k21)*s5*c5+k4*s5)*c4-k7*c6*s5*s6*s4)*c23+ (((.5*k21)*c5_2-k4*c5+(.5*k7*s6_2-.5*k7*c6_2)*s5*c4+k7*s5*c5*s4*c6*s6)*QD(6) C(3.5*k7*c6_2+.5*k21)*c5_2-.2)=((-k9+k3*c4*s5)*s2*c23+(k9-k3*c4*s5)*c2*s23+(-k12+k3*c5)*c3)*QD(2)+ (.

5*k7*c6_2+.5*k7*c6_2+ .5*k7*c6_2+.5*k21)*s5_2-.5*k7*c6_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*k22.5*k22.5*k21)*c5_2+.5)=((-k7*c6_2+k21)*s5*c5*c23+(((-.5*k1)*s5*c4+ k7*s5*c5*s4*c6*s6)*QD(6) C(4.5*k21)*s5_2-.5*k22+.5*k13)*s23)*QD(3)+((-k7*c6_2+k21)*s5*c5*c23+ (((-.5*k13+.5*k22-.5*k7*c6_2)*s4_2+(-.D Codigo fuente funciones Simulink .5*k21)*c5_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)*s4_2+(-.5*k7*c6_2+.5*k7*s6_2+.5*k21)*c5_2-.5*k13)*s23)*QD(1)+(-.5*k22)*c4_2+(-k5*s5+ 2*k7*c6*c5*s4*s6)*c4+((.5*k21)*c5_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*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*k21)*c5_2+(.5*k21)*s5_2-.5*k22+ .5*k10)*s4)*QD(5)+((.5*k1-.5*k7*c6_2+.5*k10)*s4)*QD(5)+((-.5*k7*c6_2-.5*k7*c6_2)*s4_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*k3*s2*s4*s23*s5+.5*k3*c2*c23*s4*s5.5*k7*c6_2.5*k21)*c5_2-.5*k7*c6_2+.2)=((((-k7*c6_2+k21)*s5*c5-k4*s5)*c4+k7*c6*s5*s6*s4)*c23+ (((-.5*k7*c6_2-.5*k1+.5*k7*c6_2.5*k21)*c5_2-.5*k22+.5*k21)*c5_2+.5*k21)*c5_2+(.5*k3*c2*c23*s4*s5-.5*k21)*c5_2+ .5*k7*c6_2-.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*s6_2)*c5*s4)*QD(5) C(4.5*k7*c6_2-.5*k7*s6_2)*s5*s4)*s23)*QD(6) C(4.5*k7*c6_2.5*k7*c6_2)*s4_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.5*k7*c6_2+.5*k7*c6_2+ .5*k7*c6_2+.5*k7*c6_2+.5*k3*s2*s4*s23*s5+ .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+ 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*k7*s6_2)*s5*c4+k7*s5*c5*s4*c6*s6)*QD(6) C(4.5*k7*c6_2-.5*k21)*c5_2-.5*k7*c6_2- .5*k21)*c5_2-.5*k22+ .5*k21)*c5_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*k21)*c5_2+.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*k7*c6_2)*c4_2+ (2*k7*c6*c5*s4*s6-k5*s5)*c4+((.5*k21)*c5_2+(.5*k7*c6_2+.

5*k3*c5*c4*c3-.5*k3*s2*c4*c5)*s23-.5*k7*c6_2+.5*k21)*s5_2-.5*k10)*c4+k7*c6*c5*s4*s6)*s23)*QD(1)+ (-k7*c6*c5*s6*c4+((-.5*k21)*s5_2+.5*k10+ .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*k21)*c5_2+(-.5*k7*c6_2+ .5*k3*s5*s2)*c23+ (.5*k21)*s5_2+.5*k7*c6_2.5*k7*s6_2+ .5*k21)*c5_2-k4*c5+(.5*k7*c6_2+.5*k3*c2*c4*c5-.5*k7*c6_2)*s4)*QD(4)+ (k7*c4*c6*s6+(.3)=(((-.5*k10+.5*k7*s6_2+.5*k7*c6_2+.5*k1.5*k7*c6_2-.5*k1)*s5*c4+k7*s5*c5*s4*c6*s6)*QD(3)+(-k7*c6*s6+ k7*c5_2*c6*s6)*QD(4)+(-.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*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*k21)*s5_2-.5*k7*c6_2-.136 D Codigo fuente funciones Simulink .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*k7*s6_2)*c5*c4k7*c6*s4*s6)*s23)*QD(6) C(5.5*k7*s6_2+ .5*k21)*c5_2+ (-.5*k7*c6_2+ .5*k7*s6_2+.5*k21)*s5_2+.5*k7*c6_2+.5*k7*c6_2-.5*k7*c6_2+.5*k21)*c5_2-k4*c5+(.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*k7*c6_2-.5*k21)*c5_2-k4*c5+(.5*k10+ .5*k3*c5*c4*c3-.5*k10)*s4)*QD(3)+(k7*c6_2-k21)*s5*c5*QD(4)-k7*QD(5)*c6*c5*s6+ (-.5*k21)*c5_2+(.5*k21)*s5_2-.5*k3*c2*c4*c5-.5*k1-.6)=((k7*c6*s6-k7*c5_2*c6*s6)*c23+(k7*s5*c5*c4*c6*s6+(-.5*k10)*s4)*QD(2)+(-k7*c6*c5*s6*c4+ ((-.5*k10+ .5*k7*c6_2-.5*k10+ .5*k1-.5*k7*c6_2-.5*k3*s2*c4*c5)*s23-.5*k3*c2*s5+ .5*k10+.5*k7*c6_2+.5*k7*c6_2+.5*k21)*s5_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*k7*c6_2-.5*k1)*s5*QD(6) C(4.5*k21)*c5_2+(.5*k7*c6_2+.5*k7*s6_2)*s5*c23+((.5*k7*c6_2+.5*k7*s6_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)+((.2)=(((-.5*k3*c2*s5+.5*k7*c6_2)*c4-k7*c6*c5*s4*s6)*s23)*QD(4)+((.5*k7*c6_2.5*k3*s5*s2)*c23+(.5*k7*c6_2-.5*k21)*c5_2-k4*c5+(.5*k7*s6_2)*c5*s4)*QD(6) C(5.5*k7*c6_2-.5*k1)*s5*QD(5) C(5.5*k7*c6_2+.5*k21)*s5_2+ .5*k1)*s5*s4)*s23)*QD(1)+((-.5*k7*c6_2+.5*k1)*s5*c4+k7*s5*c5*s4*c6*s6)*QD(2)+((-.5*k7*c6_2-.5*k21)*s5_2+.5*k7*c6_2+.

5*k7*c6_2)*s5*QD(6) C(5.5*k7*s6_2)*c5*c4-k7*c6*s4*s6)*s23)*QD(1)+(k7*c4*c6*s6+ (.5*k1-.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*k1.5*k7*s6_2-.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*k7*c6_2-.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)*s5*c23+((.5*k7*c6_2+.5*k7*s6_2)*s5*c4- .5*k7*s6_2.5*k1+.5*k7*c6_2)*s4)*QD(4)+(k7*c4*c6*s6+ (.5*k7*s6_2+ .5*k21)*s5_2+.5*k1-.5*k7*c6_2+.5*k7*s6_2)*c5)*s23)*QD(3)+((-k7*c6*s6+k7*c5_2*c6*s6)*c23+ (-k7*s5*c5*c4*c6*s6+(-.2)=(((-.5*k21)*c5_2+(-.5*k7*s6_2)*c5*s4)*QD(3)+(-.5*k7*c6_2+ .5*k1+.5*k7*c6_2+.5*k7*c6_2-.5*k21)*s5_2+.5*k7*c6_2)*s5*QD(4)+k7*QD(5)*c6*s6 C(6.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*c6_2+.5*k7*s6_2-.5*k7*c6_2-.5*k1-.5*k7*c6_2.5*k1+.5*k1+.5*k7*c6_2-.5*k7*c6_2)*s4)*QD(3)+(-k7*c6_2+k21)*s5*c5*QD(4)+(-.5*k1+.5*k7*c6_2)*s4)*QD(2)+(k7*c6*c5*s6*c4+ ((.6)=((.5*k7*c6_2+.5*k1+.5*k10+.5*k1-.5)=k7*QD(6)*c6*s6 C(5.5*k21)*s5_2+.5*k7*c6_2+ .4)=((k7*c6_2-k21)*s5*c5*c23+(((.5*k7*c6_2)*c5*s4)*QD(5) C(6.5*k7*s6_2.5*k7*s6_2)*c5*s4)*QD(2)+(k7*c4*c6*s6+ (.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*k10+.5*k10+.5*k1-.5*k21)*c5_2+(-.5*k7*c6_2+.5*k7*c6_2-.5*k7*s6_2)*c5)*s23)*QD(2)+(((-.5*k7*s6_2.5*k1-.5*k1+ .3)=(((-.5*k1+.5*k1.5*k7*c6_2-.5*k7*s6_2-.5*k21)*c5_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*c6_2+.5*k7*c6_2-.5*k7*c6_2)*s5*c23+ ((-.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.5*k7*s6_2)*s5*c4k7*c6*s5*c5*s4*s6)*QD(4)+(-k7*c4*c6*s6+(-.5*k7*c6_2)*c5*c4+k7*c6*s4*s6)*s23)*QD(5) C(6.5*k1)*s5*s4)*s23)*QD(4)+((-.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)*c4-k7*c6*c5*s4*s6)*s23)*QD(1)+ (k7*c6*c5*s6*c4+((.5*k1+.5*k10+ .5*k7*s6_2-.5*k21)*c5_2+(-.5*k7*c6_2+ .5*k7*s6_2)*c5*s4)*QD(6) 137 C(5.5*k7*s6_2-.5*k21)*s5_2+.5*k7*c6_2-.5*k1-.

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

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) .1) /* Posicion inicial del robot*/ #define SAMPLE(S) ssGetSFcnParam(S. y aceleraciones final e inicial nulas.h> #include "simstruc.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. Autor: Carlos Perez Fernandez 139 * fecha: 30-4-99 */ #define S_FUNCTION_NAME traj52 #define S_FUNCTION_LEVEL 2 #include <math.0) /* Velocidades m\'{a}ximas */ #define POS_INI(S) ssGetSFcnParam(S.D Codigo fuente funciones Simulink /* * * * * * Generacion de trajectoria articular Robot RM-10 Polinomio de 5 grado con velocidad inicial y final nula.h" /* Asigna parametros al espacio de trabajo */ /*********************************************/ /* De 0 a 35 Matriz de coef de polinomios */ #define A(i.

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

3))*(QF(i)-QACT(i)) A(i.3)=(10/pow(Tfinal.0 for(i=0 i<6 i++) { TimeArt(i)=fabs((15. int_T tid) { int i.0)=QACT(i) A(i.0*(QF(i)-QACT(i)))/(8.4))*(QF(i)-QACT(i)) 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.142 /* * mdlOutputs .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.2)=0 A(i.4)=(-15/pow(Tfinal.5)=(6/pow(Tfinal.1)=0 A(i.1) uPtrs 2] = ssGetInputPortRealSignalPtrs(S.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.0) uPtrs 1] = ssGetInputPortRealSignalPtrs(S.Calcula las salidas * */ D Codigo fuente funciones Simulink static void mdlOutputs(SimStruct *S.

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

144 D Codigo fuente funciones Simulink /* Indicaci\'{o}n fin de trayectoria */ y = ssGetOutputPortRealSignal(S.0 else *y=1 Moverant=MOVER } /* * mdlTerminate .c" #else #include "cg_sfun.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.h" #endif /* Is this file being compiled as a MEX-file? */ /* MEX-file interface mechanism */ /* Code generation registration function */ .

1416 /* -135 Grad */ /* +135 Grad */ /* /* /* /* /* /* /* /* /* /* -120 10 -120 +120 -120 +120 .0944 -1.1745 -2.h" #include "math.5708 -3.5708 +1.3572 (double) (double) (double) (double) (double) (double) (double) (double) (double) (double) 1e-8 */ -2.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. Autor: Carlos Perez Fernandez 145 * Fecha: 11-Julio-1999 */ #define S_FUNCTION_NAME cineinv #define S_FUNCTION_LEVEL 2 #include "simstruc.0944 +0.3572 (double) +2.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.4)) 0] mxGetPr(ssGetSFcnParam(S.0944 +2.0944 -2.1)) 0] mxGetPr(ssGetSFcnParam(S.0)) 0] mxGetPr(ssGetSFcnParam(S.0944 +2.h" #include "stdlib.3)) 0] mxGetPr(ssGetSFcnParam(S.5)) 0] mxGetPr(ssGetSFcnParam(S.14159265358979 (double) -2.2)) 0] mxGetPr(ssGetSFcnParam(S.6)) 0] 3.90 + 90 -180 +180 Grad Grad Grad Grad Grad Grad Grad Grad Grad Grad */ */ */ */ */ */ */ */ */ */ #define FLOAT_EPSILON /* Asigna entradas .

3. 1) 0) 0) 0) 0) 0) /* /* /* /* Numero Vector Vector Vector de de de de muestreos */ numeros reales */ numeros enteros */ punteros */ ssSetOptions(S. 3) /*Vector n*/ ssSetInputPortWidth(S. 0. S.3)) return /*Salidas*/ ssSetOutputPortWidth(S. 3) ssSetInputPortWidth(S. ssSetNumIWork( S. i. 0) /* N\'{u}mero de estados t continuo */ 0) /* N\'{u}mero de estados t discreto */ if (!ssSetNumInputPorts(S. 6) /*Vector a*/ /*Posici\'{o}n xyz*/ /*Posici\'{o}n Actual*/ for(i=0 i<5 i++) ssSetInputPortDirectFeedThrough(S. ssSetNumRWork( S. 1) if (!ssSetNumOutputPorts(S. 2. 1. 5)) return /*Entradas*/ ssSetInputPortWidth(S. ssSetNumModes( S. 0. 3) ssSetInputPortWidth(S. SS_OPTION_EXCEPTION_FREE_CODE) } . 1. ssSetNumPWork( S. 4. 3) /*Vector s*/ ssSetInputPortWidth(S. 6) /*Posici\'{o}n Soluci\'{o}n*/ ssSetOutputPortWidth(S. 7) if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { return /* Si faltan parametros se da mensaje */ } ssSetNumSampleTimes( S. ssSetNumNonsampledZCs(S. 2. 1) /*Error trayectoria*/ ssSetOutputPortWidth(S. 1) /*debug*/ ssSetNumSFcnParams(S.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.

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

atan2(RGEN_d3.x) . -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 .atan2(RGEN_d3. sqrt(dtemp)) qSol 2] 0]=atan2(y.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.x) .2) for (i=0 i<5 i++) uPtrs i] = ssGetInputPortRealSignalPtrs(S.148 D Codigo fuente funciones Simulink real_T *debug=ssGetOutputPortRealSignal(S.

RGEN_d4) .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 .atan2(A 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].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].

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 } } } } .

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

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

154 D Codigo fuente funciones Simulink .

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

You're Reading a Free Preview

Download
scribd
/*********** DO NOT ALTER ANYTHING BELOW THIS LINE ! ************/ var s_code=s.t();if(s_code)document.write(s_code)//-->