Simulación y Control de Un Robot Manipulador

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.
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
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
4 3.3.3 Parametros de friccion . 3.4 Minimizacion de los parametros 3.5 Implementacion informatica . . 3.6 Simulaciones del modelo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
INDICE GENERAL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 55 59 59
4 Control de robot RM-10
4.1 Introduccion . . . . . . . . 4.2 Control lineal . . . . . . . 4.2.1 Justi cacion . . . . 4.2.2 Control tipo PD . . 4.2.3 Control tipo PID . 4.3 Control por Par Calculado
. . . . . .
. . . . . .
. . . . . .
63
63 63 63 64 75 76
Apendices
A Tarjeta de E/S digital B Tarjeta de conversion resolver-encoder
B.1 B.2 B.3 B.4 B.5 B.6 Filtros y Ajuste de fase . . . . . . Conversion resolver a encoder . . Ampli cador adaptador . . . . . Optoaislacion de se~ales digitales n Se~ales de control . . . . . . . . . n Esquemas electricos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
91
. 96 . 99 . 99 . 100 . 100 . 101
91 95
C Esquemas electricos de electronica de potencia D Codigo fuente funciones Simulink
103 105
Indice de Figuras
1.1 Estructura general de un robot industrial . . . . . . . . . . . . 1.2 Brazo mecanico del robot industrial RM-10 con indicacion de sus movimientos posibles . . . . . . . . . . . . . . . . . . . . . 1.3 Control en un robot industrial . . . . . . . . . . . . . . . . . . 1.4 Vista general del robot industrial RM-10 . . . . . . . . . . . . 1.5 Se~ales proporcionadas por los resolvers . . . . . . . . . . . . n 1.6 Representacion esquematica de un servoampli cador . . . . . . . . 1.7 Esquema de interface con el robot RM-10 . . . . . . . . . . . . 2.1 2.2 2.3 2.4 2.5 2.6 3.1 3.2 3.3 3.4 3.5 3.6 3.7 Convenio Denavit-Hartenberg . . . . . . . . . . . . . . . . Situacion de los sistemas de coordenadas . . . . . . . . . . Vectores de orientacion en la herramienta del manipulador Soluciones para las tres ultimas articulaciones . . . . . . . Trayectorias articulares polinomicas de 5 orden . . . . . . Generacion de trayectorias lineales. . . . . . . . . . . . . . . . . . . . . . . . . . 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 . . Caracter stica estatica de friccion teorica . . . . . . . . . . . . Caracter stica de friccion experimental del eje 6. . . . . . . . . Caracter stica de friccion experimental del eje 5. . . . . . . . . Caracter stica de friccion experimental del eje 4. . . . . . . . . Diagrama de bloques asociado al modelo . . . . . . . . . . . . Diagrama de bloques Simulink para la simulacion del manipulador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.8 Diagrama de bloques para la simulacion del modelo en bucle abierto . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.9 Evolucion de las posiciones de las articulaciones 5 y 6 . . . . . 3.10 Evolucion de las velocidades de las articulaciones 5 y 6 . . . .
4.1 Esquema de control lineal tipo PD . . . . . . . . . . . . . . . 64 4.2 Diagrama Simulink para la simulacion de un controlador PD . 66 4.3 Seguimiento en posicion para control tipo PD . . . . . . . . . 67 5
INDICE DE FIGURAS 4.4 Error de seguimiento en posicion para control tipo PD . . . . 4.5 Se~ales de control para regulador tipo PD . . . . . . . . . . . n 4.6 Seguimiento en posicion para control tipo PD en los ejes 4, 5 y6 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.7 Velocidades para control tipo PD en los ejes 4, 5 y 6 . . . . . . 4.8 Error de seguimiento en posicion para control tipo PD en los ejes 4, 5 y 6 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.9 Se~al de control para control tipo PD en los ejes 4, 5 y 6 . . . n 4.10 Diagrama Simulink para la simulacion de un controlador PID 4.11 Seguimiento en posicion para control tipo PID . . . . . . . . . 4.12 Error de seguimiento en posicion para control tipo PID . . . . 4.13 Se~ales de control para regulador tipo PID . . . . . . . . . . . n 4.14 Bucle de linealizacion y desacoplo . . . . . . . . . . . . . . . . 4.15 Esquema completo de control por par calculado . . . . . . . . 4.16 Diagrama Simulink para la simulacion de un controlador Par Calculado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.17 Seguimiento en posicion para control tipo Par Calculado . . . 4.18 Error de seguimiento en posicion para control tipo Par Calculado 4.19 Se~ales de control para regulador tipo Par Calculado . . . . . n 4.20 Seguimiento en posicion para control tipo par calculado en los ejes 4, 5 y 6 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.21 Velocidades para control tipo par calculado en los ejes 4, 5 y 6 4.22 Error de seguimiento en posicion para control tipo par calculado en los ejes 4, 5 y 6 . . . . . . . . . . . . . . . . . . . . . . 4.23 Se~al de control para control tipo par calculado en los ejes 4, n 5y6 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . A.1 Etapa de entrada digital de la tarjeta E/S digitales . . . . . . A.2 Etapa de salida digital de la tarjeta E/S digitales . . . . . . . B.1 Diagrama de bloques de la tarjeta CRE . . . . . . . . . . . . . B.2 Conectores presentes en la tarjeta CRE . . . . . . . . . . . . . B.3 Esquema del ltro activo para la se~al de referencia . . . . . . n B.4 Funcionamiento interno del circuito integrado AD2S90 . . . . 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
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, ubicado en los laboratorios del Departamento de Ingenier a de Sistemas y Automatica de la Universidad de Sevilla, por un sistema de control basado en un ordenador PC an trion con una tarjeta controladora con multiples entrada-salida. Este sistema pretende sustituir los algoritmos de control de los motores y la generacion de trayectorias del manipulador, conservando unicamente la electronica de potencia del manipulador y realizando la interfase electrica con las se~ales del manipulador. n
1.2 Introduccion a la Robotica Industrial

1.2.1 Evolucion historica de la robotica industrial
La necesidad de aumentar la productividad y mejorar la calidad de los productos, ha hecho insu ciente la automatizacion industrial r gida, dominante en las primeras decadas del siglo XX, que estaba destinada a la fabricacion de grandes cantidades de una gama peque~a de productos. En la actualidad, n mas de la mitad de los productos que se fabrican corresponden a lotes de pocas unidades. 7
1 Introduccion
Por esto, con el objetivo de dise~ar una maquina exible, adaptable al n entorno de trabajo, se patento en 1956 un manipulador programable que fue la semilla para la robotica industrial. En la historia de la Robotica Industrial se pueden distinguir varias etapas segun el nivel de desarrollo: 1. En 1950 se dise~an manipuladores amo-esclavo para manejar materian les radiactivos. 2. A principios de los 60 Unimation realiza los primeros proyectos de robots industriales, instalando el primero en 1961. En 1967 instala un conjunto de robots en una factor a de General Motors. Tres a~os desn pues se inicia la implantacion de robots industriales en Europa, fundamentalmente en el sector automovil stico. 3. En 1970, los laboratorios de la Universidad de Stanford y del MIT acometen la tarea de controlar un robot mediante un computador. 4. 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. 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. 5. 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, naciendo as los robots inteligentes, capaces de tomar decisiones adecuadas a cada situacion.
1.2.2 Clasi cacion de los robots industriales

La evolucion a dado origen a una serie de tipos de robots, como los siguientes: 1. Manipuladores, son sistemas mecanicos en los que se puede gobernar los movimientos, que a su vez son: Manuales, dirigidos por un operario. Secuenciales, pero que repiten siempre lo mismo. Secuenciales, pero con elementos que permiten modi car la secuencia.
2. Robots de aprendizaje gestual: El operario ense~a al robot mediante n pistolas de programacion o bien moviendo el robot. 3. Robot programables textualmente: En este tipo de robots, se programan o -line mediante un lenguaje de programacion espec co 4. Robots inteligentes: Son analogos a los anteriores, diferenciandose en que poseen una mayor interaccion con el entorno que lo rodea, mediante el uso de sensores avanzados, vision o inteligencia arti cial
1.2.3 Estructura de un robot industrial

En un robot industrial, tal y como se ve en la gura 1.1, en general nos podemos encontrar las siguientes partes: 1. Brazo mecanico o manipulador que dispone en su extremo de una herramienta. 2. Controlador. 3. Sensores de posicion y velocidad. 4. Actuadores. 5. Sensores avanzados, aunque estos ultimos no siempre se encuentran presentes. Ademas el robot industrial se puede encontrar dotado de toda una serie de automatismos tales como cintas transportadoras, alimentadores, automatas programables etc. 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, desde simples entradas y salidas digitales hasta buses de campo.
Brazo mecanico
El brazo mecanico es el conjunto de elementos mecanicos que permiten el movimiento de la herramienta del robot. Normalmente esta dividido en partes r gidas, denominandose estas enlaces y articulaciones que permiten el movimiento relativo entre ellas.
10
1 Introduccion
Sensores de informacin
Herramienta Actuadores
Controlador
Sensores
Posicin. Velocidad
Brazo manipulador
Figura 1.1: Estructura general de un robot industrial A semejanza del brazo humano al primer enlace se le denomina brazo, al segundo enlace, antebrazo y al resto mu~eca, tal y como se ve en la gura n 1.2. Existen distintos tipos de articulaciones, pero normalmente se suelen usar de rotacion o prismaticas, ambas de un grado de libertad. Siendo todas las articulaciones de un grado de libertad, el numero de grados de libertas del robot sera igual al numero de articulaciones. Por lo general los robots industriales suelen tener 6 grados de libertad, 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.
Controlador
El controlador del sistema se encarga de procesar toda la informacion que llega de los sensores y del usuario mismo, generando las se~ales de control n adecuadas para realizar los movimientos deseados. El controlador suele estar organizado de un modo jerarquico, en el nivel mas bajo se encuentran los algoritmos de control de los actuadores, por ejemplo lazos de control PID.

3
11
Brazo
1
Antebrazo
6 2 5
Mueca
Figura 1.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, esta parte se encarga de generar las referencias para cada eje adecuadas al control de mas bajo nivel, de esta manera es posible controlar como el robot se desplaza de un punto a otro. Es el usuario normalmente quien especi ca que tipo de trayectorias debe seguir el robot. Ademas del control de trayectorias es posible encontrar a este nivel lazos de control con lecturas de sensores mas especializados, como por ejemplo sistemas de vision o sensores de esfuerzos. El controlador ademas de realizar todas estas tareas es necesario que supervise el funcionamiento del conjunto, actuando adecuadamente en el caso que algun componente del sistema fallase.
12
qref q_ref
1 Introduccion
q
Control Servos
Control Trayectorias
q_
Figura 1.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, como por ejemplo garras de sujeccion, taladros, maquinas de soldar, etc...
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, adicionalmente tambien pueden llevar sensores de velocidad, si bien esta se puede estimar a partir de la medida de la posicion. Los sensores de posicion que aparecen en los robots industriales son: Potenciometros. Encoders. Resolvers. Inductosyn. Para medir la velocidad se pueden usar dinamos tacometricas o bien sensores inductivos. En robots industriales cuyos ejes son de rotacion los sensores de posicion mas usados son los encoders, seguido de los resolvers. La ventaja de los
1.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, sin embargo los resolver son capaces de dar medidas mas precisas que los encoders.
Actuadores
Los actuadores en un robot industrial pueden ser, en funcion de la energ a que usen: Neumaticos. Hidraulicos Electricos. Estos actuadores pueden actuar a traves de reductoras y correas de transmision o bien directamente en las articulaciones del robot. Estos robots se denominan respectivamente de accionamiento indirecto y directo. En la actualidad la mayor a de los robots que se fabrican poseen actuadores electricos ya que son los mas adecuados para el control, los motores electricos mas comunmente usados son: Motores paso a paso. Motores de corriente continua con o sin escobillas Motores de corriente alterna. Los mas usados son los motores de corriente continua convencionales, aunque cada vez mas se usan los motores sin escobillas. En estos la conmutacion de la corriente se produce en una electronica externa basandose en la medida de la posicion del rotor del motor.

El robot industrial RM-10 esta compuesto de un brazo manipulador, un armario de control y una pistola de programacion. Esta ofrece la unica interfaz de usuario para poder hacer uso manual del robot, as como para su programacion.
14
ARMARIO
12 12 12
1 Introduccion
BRAZO MANIPULADOR
PISTOLA
1234 1234 1234 1234
Figura 1.4: Vista general del robot industrial RM-10
1.3.1 Brazo manipulador

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

10 5 Ref 0 5 10 5 0 0.5 1 1.5 2 2.5 3 3.5 4 x 10 4.5
3
15
5 5
0.5
1.5
2.5
3.5
4 x 10
4.5
3
0.5
1.5
2.5
3.5
4 x 10
4.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, 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. Las caracter sticas electricas mas importantes de los resolvers son: Frecuencia portadora: 3500 Hz. Tension referencia: 10V
pp
Relacion de transformacion: 0,5 Corriente maxima de entrada: 10 mA. La se~al de referencia es proporcionada a cada resolver por su corresponn diente ampli cador de potencia. Las formas de estas se~ales son idealmente n como las de la gura 1.5.
1.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:
16 Electronica de potencia. Zona de entrada y salidas. Rack controlador. Servoampli cadores.
1 Introduccion
Rack controlador
El rack controlador se encuentra ubicado en la parte delantera superior del armario. El sistema esta basado en bus VME, estando toda la labor de control centralizada en una unica tarjeta procesadora basada en un procesador Motorola 68020 a 25 Mhz. 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. Ademas el rack controlador posee tarjetas con interfases a distintos perifericos: Puertos serie, uno de ellos usado para la pistola de manejo y programacion. Dispositivos de almacenamiento. 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. La energ a electrica es suministrada al armario controlador en forma trifasica con una tension nominal de 380V. Esta tension se reduce mediante un autotransformador trifasico a 220V.
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. En concreto existen las siguientes fuentes: Fuente AL1 +24V con salida regulada. Se usa para la alimentacion de reles auxiliares y tarjetas de entradas y salidas digitales. Fuente AL2. Proporciona +5V para la alimentacion de las distintas tarjetas del rack VME. Tambien proporciona una alimentacion simetrica de 12V necesaria para el funcionamiento de las se~ales de los sern voampli cadores. Fuente no regulada formada por el transformador TR3 y el puente de diodos PD. Proporciona una tension de +24V necesaria para alimentar los frenos de los motores brushless. Tambien aparecen en esta zona dos contactores: Contactor CP: Es el contactor principal que proporciona tension a la fuente de alimentacion de los servomotores. Este contactor se cierra cuando el usuario pulsa el boton START del panel frontal del armario. Un contacto auxiliar de este rele activa la fuente AL2, por lo que deja a los servoampli cadores totalmente operativos. 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 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. De entre todas las salidas las siguientes tienen un interes especial de cara a integrar la nueva tarjeta controladora: U1 - Se encarga, a traves del rele auxiliar AUX1, de habilitar los servoampli cadores de modo que estos puedan recibir consignas.
18
1 Introduccion U2 - Su funcion es la de a traves del rele auxiliar AUX2, excitar la bobina del contactor CF (contactor de frenos). CR5 - Esta salida activa la entrada de limitacion de par o velocidad de los servoampli cadores.
Otras salidas se encargan de actuar sobres las servovalvulas de aire comprimido para la herramienta del manipulador, as como para activar y desactivar indicadores luminosos. Las salidas U1 y U2 estan condicionadas a que no este activado ningun dispositivo hardware de emergencia, estos son: Pulsador de emergencia del panel frontal del armario. Pulsador de emergencia de la pistola de programacion. Finales de carrera presentes en todas las articulaciones del robot excepto en la articulacion 6. 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. Tienen interes las siguientes entradas: EM6 - Se pone a nivel bajo cuando se activa un contacto de alarma como los descritos anteriormente. IFR - Se activa cuando el contactor de frenos se cierra liberandolos. IN8 - Se activa cuando la fuente de alimentacion de los servoampli cadores se sobrecalienta. IN9 - Se activa cuando la resistencia de disipacion de la fuente de alimentacion se funde. IN10 - Esta se~al se activa cuando alguna de las tres fases de alin mentacion trifasica de la fuente de alimentacion se pierde. 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. n
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. La conmutacion se realiza en funcion de la posicion del rotor que es realimentada por el resolver. La alimentacion de potencia de los servoampli cadores son un bus de corriente continua de 300V que proporciona la fuente de alimentacion.
Puente de transistores de potencia + A B C Conmutador electrnico /A /B /C /A /B /C N S A B C Servomotor Brushless
Posicin del rotor Resolver
Figura 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. Para modi car la corriente se regula la tension en bornas del motor mediante PWM. En este modo la consigna se traduce en el porcentaje de la corriente maxima admisible que se hace circular por el estator del motor. La corriente maxima para cada motor viene determinada por un modulo de optimizacion insertado en cada servoampli cador. En modo velocidad el servoampli cador cierra un bucle de velocidad sobre el bucle de corriente, de modo que la consigna se traduce en el porcentaje de velocidad maxima de la con gurada para ese motor. Dicha velocidad maxima se con gura de entre un rango de valores discretos y se a na con un potenciometro de escalado.
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.
1.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. Adicionalmente se instalo mas hardware ya sea comercial o desarrollado a medida para el presente proyecto.
1.4.1 Equipo hardware

Ademas del robot industrial RM-10 se cuenta con los siguientes elementos: Un ordenador PC compatible con microprocesador PENTIUM II 350MHz y 128Mb de memoria RAM. Una tarjeta controladora de la rma dSPACE modelo DS1103. 3 Tarjetas conversoras de resolver a encoder incremental (CRE). Una tarjeta de entradas y salidas digitales. 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. 4 Mbyte de memoria DRAM. Numerosos perifericos de entrada y salida.
1.4 Equipo para el desarrollo del proyecto
21
Las entradas de la tarjeta DS1103 se encuentran organizadas de las siguiente forma: Procesador Motorola PowerPC 604e:
{ 16 canales de entrada analogicas de 16 bits y 4 s. de tiempo de { { { { { {

conversion. 4 canales de entrada analogicas de 12 bits y 0.8 s. 8 canales de salidas analogicas de 14 bits y 6 s. 32 se~ales digitales con gurables como entrada o salida. n UART con gurable como RS-232 o RS-422. 7 L neas de encoder incremental. 1 Interface para bus CAN.
Microcontrolador esclavo TMS320F240: { 16 canales de entrada analogicas de 10 bits y 6 s. { Entrada de fuentes de interrupciones y relojes externos. { Salida para generacion PWM. { Salidas tipo Output Compare. { Entradas tipo Input Capture. { 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.
1.4.2 Software de desarrollo

Para el dise~o y simulacion de controladores se usa como herramienta MATn LAB v5.2 y la ToolBox de simulacion SIMULINK 2.0. 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.0. Para la implementacion de los algoritmos de control en la tarjeta controladora, se usa la ToolBox Real Time WorkShop junto con la librer a Real
22
1 Introduccion
Time Interface Library proporcionada por la rma dSPACE junto con la tarjeta. 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. 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. Trace 1103 con el que se capturan en el PC an trion la evolucion temporal de variables del programa que se ejecuta en la tarjeta.
1.4.3 Interface con armario de control

Conversin Resolver a Encoder
BRAZO MANIPULADOR Aislamiento seales analgicas
Electrnica de potencia
Seales Digitales
Figura 1.7: Esquema de interface con el robot RM-10
Cap tulo 2 Cinematica del robot RM-10

2.1 Introduccion
El estudio de la cinematica es fundamental para el desarrollo del sistema de control de un robot. En este cap tulo se realizara un modelo cinematico del robot, que permitira relacionar el espacio de las variables articulares con el espacio cartesiano, as como la plani cacion de trayectorias del robot. La notacion utilizada para el estudio de la cinematica del robot RM-10 es la de Denavit-Hartenberg 3], con el criterio que aparece en la g. 2.1 en la que a cada grado de libertad se le asocian 4 magnitudes que lo de nen completamente: angulo de enlace i; angulo entre los ejes zi; y zi; medido sobre el eje xi; . Longitud de enlace ai; Distancia entre los ejes zi y zi; medida sobre el eje xi; . angulo de la articulacion i angulo entre los ejes xi; y xi medido sobre zi . Distancia de enlace di Distancia entre los ejes xi; y xi medida sobre el eje zi.
1 1 1 1 1 1 1 1 1
Adoptando el convenio de la gura 2.1, asociamos a cada enlace un sistema de coordenadas solidario al mismo y a partir de los 4 parametros 23
24
i;1 i
i;1
Zi;
Yi; Zi;
ai; di
Yi
Zi Xi
ai;
Figura 2.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.
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, y as de esta manera crear una matriz con los parametros cinematicos. La gura 2.2 muestra la colocacion de los distintos sistemas de coordenadas, a partir de dicha gura se obtiene la tabla 2.2. Segun el convenio adoptado en la gura 2.1 la matriz de transformacion de la articulacion i a la articulacion i ; 1 vendra dada por la expresion 2.1
Tii;
ci ;s i 0 ai; 6 s i c i; c ic i; ;s i; ;dis i; =6 s s 4 c is i; c i; dic i; i i; 0 0 0 1

1 1 1 1 1 1 1 1
3 7 7 5
(2.1)
2.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: Situacion de los sistemas de coordenadas
26
i;1
1
ai; 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.1: Parametros cinematicos

1 2 3 4 5 6 1 2 3 3 4
Usando la matriz 2.1 y la tabla de parametros cinematicos 2.2 se obtienen las seis matrices de transformacion del robot:
c ;s 6 s T = 6 1 c0 4 0 0
1 0 1 1 1
0 0 1 0
0 07 7 05 1
(2.2)
c ;s 0 6 0 T = 6 ;s ;0c 1 4 0 0 0 0
2 2 1 2 2 2
a 0 0 1 a 0 d 1
3 7 7 5
(2.3)
c ;s 6 s T = 6 1 c0 4 0 0
3 2 3 3 3
0 0 1 0
3 7 7 5
(2.4)
c ;s 0 6 0 T = 6 ;s ;0c 1 4 0 0 0 0
4 4 3 4 4 4
a d 0 1
3 7 7 5
3 4
(2.5)
2.3 Problema cinematico directo
27
3
c ;s 0 0 6 0 7 T = 6 s c0 ;1 0 7 4 0 05 0 0 0 1
5 5 4 5 5 5
(2.6)
c ;s 0 6 0 T = 6 ;s ;0c 1 4 0 0 0 0
6 6 5 6 6 6
0 0 0 1
3 7 7 5
(2.7)
Si queremos usar como origen de posiciones el sistema de coordenadas B necesitamos la matriz de transformacion del sistema 0 al sistema B: 1 6 0 T =6 0 4 0
5 6
0 1 0 0
0 0 1 0
0 0 dB 1
3 7 7 5
(2.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
0 1 0 0
0 0 1 0
0 0 dG 1
3 7 7 5
(2.9)

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. 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
28
a n
Figura 2.3: Vectores de orientacion en la herramienta del manipulador utiliza la terna de vectores ~ ~ ~ cuya orientacion en el espacio puede verse nsa en la gura 2.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.10)
Del resultado de este producto se obtiene la matriz:
nx 6 ny B TG = 6 n 4 z 0
sx sy sz 0
ax ay az 0
xG yG zG 1
3 7 7 5
(2.11)
El juego de ecuaciones que se obtienen para la orientacion de la herramienta es:
nx = c c (c c c ; 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.12)
29
1 23 4 6 1 4 5 6 1 4 6
ny = s c (c c c ; s s ) + s c s s ; c s c c + c c s
1 6 23 4 5 23 5
(2.13) (2.14)
nz = ;c (s c c ; c s ) + s s s
6 23 4 5 23 5 1 6 23 5 23 4 5 1 23 4 6 1 4 5 6
23 4 6
sx = c s (s s ; c c c ) ; c c s c ; s s c s + s c c sy = s s (s s ; c c c ) ; 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.15) (2.16) (2.17) (2.18) (2.19) (2.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 = ;c (c c s + s c ) ; s s s ay = ;s (c c s + s c ) ; c s s
1 23 4 5 23 5
1 4 5
1 4 5
az = s c s ; c c
23 4 5
23 5
Y para la posicion:
xG = a c + a c c + a c c ; d s ; d c s + dGax
1 1 2 1 2 3 1 23 3 1 4 1 23
(2.21) (2.22) (2.23)
yG = a s + a s c + a s c + d c ; d s s + dGay
1 1 2 1 2 3 1 23 3 1 4 1 23
zG = ;a s ; a s ; d c + dGaz + dB
2 2 3 23 4 23
La implementacion software de estas ecuaciones se realiza en la funcion pcd.c en forma de bloque para su uso en diagrama Simulink. 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.
30
2.4 Problema cinematico inverso

En muchas ocasiones interesara dar consignas al robot en el espacio cartesiano, esto implica que debamos conocer dado una posicion y orientacion de la herramienta del manipulador, el conjunto de variables articulares que la satisfacen. As mismo, es necesario resolver este problema para la generacion de trayectorias en el espacio cartesiano del robot. El conjunto de ecuaciones que se tienen que resolver son de 2.12 a 2.23. De estas 12 ecuaciones nos podr amos quedar con 6 ya que los vectores ~ ,~ ns y ~ no son independientes y ademas son unitarios. El problema cinematico a inverso en general no tiene una solucion unica, 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, en los que se intenta buscar una solucion anal tica basada en la geometr a del robot aplicando relaciones trigonometricas. Aritmeticos, buscan una solucion anal tica manipulado las ecuaciones usando las matrices de transformacion. Numericos, se usa algun metodo de resolucion numerica si bien este metodo es el mas costoso en tiempo de calculo para una computadora. A continuacion se expone una solucion anal tica usando metodos aritmeticos.
2.4.1 Resolucion anal tica de la cinematica inversa

Partimos de una matriz 2.11, o sea de una orientacion y una posicion de la herramienta del manipulador. A partir de la posicion podemos obtener la posicion de la interseccion de los ejes 4, 5 y 6 respecto al sistema de referencia 0, dejando solamente las matrices de transformacion que dependen de un grado de libertad:
31
nx 6 ny T =6 n 4 z 0
0 6
sx sy sz 0
ax ay az 0
x y 7 = (T B ); T B (T G); 7 G z5 1
0 1 6
(2.24)
Tendremos que resolver:
nx 6 ny T =6 n 4 z 0
0 6
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
5 6
(2.25)
Realizamos la operacion:
c 6 ;s (T ); T = 6 0 4 0
1 0 1 1 0 6
s c 0 0
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
1 6
(2.26)
A partir del elemento (2,4) del sistema de ecuaciones 2.26 tenemos la ecuacion:
;s x + c y = d
1 1
(2.27)
Para resolver el angulo
se realiza un cambio de variables:
x = rcos( ) y = rsen( ) = atan2(y x) p r = x + y2

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

2
33
32
cc 6 ;c s 6 4 ;s 0
1 23 1 23 1
sc ;s s c 0
1
1 23 1 23
;s ;c
23
;a c ; a c
1 23 1 23 3 2 3
0 0
23
nx a s + s a 7 6 ny 76 5 4 nz ;d 1 0
2 3
sx sy sz 0
ax ay az 0
x y 7 = T (2.36) 7 z5 1
3 6
De este conjunto de ecuaciones nos quedamos con las que forman los elementos (1,4) y (2,4):
c c x+s c y;s z;a c ;a c =a ;c s x ; s s y ; c z + a s + s a = d

1 23 1 23 23 1 23 2 3 1 23 1 23 23 1 23 3 2 23 23
3 4
(2.37)
Las expresiones 2.37 forman un sistema de ecuaciones en el que tenemos como incognitas unicamente a c y s , de cuya resolucion se obtiene:
s ; ( + ; + ; s = c x (+ a xs dy );xcxa )ys ; 2as)ya (;aa c + za ) zs y (2 2 c + + ( c + a ) xc ys a z c = c x +a(2 xs y ;(2 xa+ c ;; a ) + (s a ; 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.38)
3
Con las que obtenemos la suma de los angulos obtenemos 4 soluciones posibles para el angulo :
2 2
y , y conocido
3
= atan2(s c ) ;
23 23
(2.39)
Del bloque de ecuaciones 2.36 tambien podemos extraer las ecuaciones de los elementos (1,3) y (2,3):
c c ax + s c ay ; s az = ;c s ;s ax + c ay = s s
1 23 1 23 23 1 1
4 5 4 5
(2.40)
De las que se puede resolver :

4
34
2 Cinematica del robot RM-10 = atan2(;s ax + c ay ;c c ax ; s c ay + s az )

1 1 1 23 1 23 23 5 4
(2.41)
En el caso de que se anulase no podr amos resolver segun la ecuacion anterior, debido a que el manipulador se encuentra en una con guracion singular en la que los ejes 4 y 6 estan alineados, por tanto, para conseguir la orientacion deseada de los vectores ~ y ~, nos sobra un grado de libertad. Para n s evitar problemas, cuando el se acerque a cero se puede dejar constante y orientar la herramienta con . Esto se puede hacer usando la ecuacion (3,3), supervisando el valor del coseno de se acerca a la unidad:
5 4 6 5
c = ;c s ax ; s s ay ; c az
5 1 23 1 23 23
(2.42)
3 4
Para resolver de la forma:
premultiplicamos la ecuacion 2.36 por la inversa de T
nx 6 ny (T ); 6 n 4 z 0
0 4 1
sx sy sz 0
ax ay az 0
x y 7=T 7 z5 1
4 6
(2.43)
De este conjunto de ecuaciones se puede extraer los elementos (1,3) y (3,3):
s = ; (c c c + s s ) ax ; (c s c ; c s ) ay + c s az c = ;c s ax ; s s ay ; c az
5 1 23 4 1 4 23 1 4 1 4 4 23 5 1 23 1 23 23
(2.44) (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 ); 6 n 4 z 0
0 5 1
sx sy sz 0
ax ay az 0
x y 7=T 7 z5 1
5 6
(2.46)
2.4 Problema cinematico inverso En los elementos (1,1) y (3,1) aparecen las ecuaciones:
35
c = ; (c c s ; s c ) sx ; (c s s + c c ) sy + s s sz s = ; (c c s ; s c ) nx ; (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.47)
Con lo que se puede resolver el angulo

6 6
como:
6
= atan2(s c )
(2.48)
Para las tres ultimas articulaciones existe una segunda solucion que se calcula como:
0 0 5 0
4 6
= ; =; = ;
4 5 6
(2.49)
Con lo que en total obtenemos 8 soluciones para el problema cinematico inverso.

X Y
4
X Z
X Z
5
~ a
5
a==a
Z
6
~ n ~ s
Y
4
X Y
5
Z X
4
~ s ~ a ~ n
sol2
Figura 2.4: Soluciones para las tres ultimas articulaciones
36
2.4.2 Implementacion software

El problema cinematico inverso se ha inplementado como una funcion S (la funcion cineinv.c) para diagrama de bloques de Simulink. La funcion recibe como entradas:
~ 1. Vector posicion cartesiana de la herramienta P . 2. Vectores de orientacion de la herramienta ~ ,~ y ~ . ns a 3. Vector de variables articulares correspondientes a la posicion actual del robot.
Y se tienen como salidas: 1. Vector de variables articulares solucion 2. Se~al de error en caso de que no haya solucion en el espacio de trabajo n del robot. Ademas recibe como parametros las distancias que de nen la cinematica del manipulador. La funcion cineinv.c calcula las soluciones posibles, de las cuales descarta las que se quedan fuera del espacio de trabajo del robot. De las soluciones que quedan se elige la mas cercana a la posicion actual de robot.
2.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. Las trayectorias son generadas en tiempo real a partir de la posicion actual del manipulador.
2.5.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. Se consigue que el movimiento de todas las
37
articulaciones comience y termine a la vez, as como un movimiento suave de las articulaciones. El tipo de trayectoria usado es un polinomio de quinto orden tal y como puede verse en al gura 2.5
1 0.5
0 1
0.2
0.4
0.6
0.8
1.2
1.4
1.6
1.8
00.5
0 2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
00 0
1 2 0 0.2 0.4 0.6 0.8 1 t 1.2 1.4 1.6 1.8 2
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, 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
(2.50)
Los coe cientes de los polinomios son calculados en funcion de las condiciones de contorno, obteniendose: 10 a = T ( f ; i) 15 a = ; T ( f ; i) a = T6 ( f ; o) (2.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. La velocidad articular maxima alcanzada se produce en t = T=2 y toma el valor: 15 f ; i 0 (2.52) max = 8 T
3 3 4 4 5 5
38
Enable
du/dt
n
3 qd
eul2tr
s a p pact
1 q
Error
2 error
PCI 2 Pfinal 1 Move pcdcar

xyzrpy act Mover p Qf
Traylin 3 Qact
Figura 2.6: Generacion de trayectorias lineales. La generacion de este tipo de trayectorias esta implementada en la funcion Simulink, traj52.c. La funcion recibe como entradas la posicion deseada, la posicion actual del manipulador y la se~al digital que activa la generacion n de la trayectoria a cada anco. El bloque genera una secuencia de posiciones y velocidades de referencia. La funcion recibe como parametro un vector de velocidades articulares maximas, 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.
2.5.2 Trayectorias lineales

Para hacer tareas que requieran cierta precision es preciso que los manipuladores sean capaces de realizar trayectorias lineales. Esta tarea es realizada por un subbloque Simulink, que a su vez se descompone en distintas tareas espec cas. En primer lugar se dispone de el bloque trajlin52.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. La posicion inicial del espacio cartesiano se obtiene mediante el bloque
39
pcdcar.c realizando los calculos del problema cinematico directo, o sea a partir de las ecuaciones 2.21 a 2.23. Ademas este bloque obtiene la orientacion de la herramienta del manipulador en terminos de los angulos roll, pitch, yaw a partir de la parte rotacional de la matriz de transformacion obtenida con las ecuaciones 2.12 a 2.20. 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 ; sy cr cy spcr + sy sr 4 sy cp sy sp sr + cy cr sy sp cr ; cy sr ;sp cpsr cp cr
3 5
(2.53)
A partir de esta matriz se pueden deducir los angulos de roll, pitch y yaw. 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.54)
En el caso de c = 0, habra que jar y calcular de la forma: = 90 = ;90 =0 =0 = atan2(r r ) = ;atan2(r r )

12 22 12 22
(2.55)
Las consignas en el espacio cartesiano generadas son enviadas al bloque cineinv.c que se encarga de resolver la cinematica inversa del manipulador segun el procedimiento descrito en el punto 2.4.1. proporcionando referencias en el espacio articular al controlador. 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, una solucion del problema cinematico inverso. En este caso el sistema genera una alarma bloqueando el manipulador.
40
Cap tulo 3 Dinamica del robot RM-10

3.1 Introduccion
Antes de abordar el problema de control del manipulador es necesario obtener un modelo dinamico. El modelo dinamico no solo es util para poder simular el comportamiento del sistema, tambien lo es para comprender el problema de control de los manipuladores. En general se pueden generar soluciones numericas o bien soluciones simbolicas. Si bien las soluciones numericas resultan mas compactas en su formulacion mediante algoritmos como el de Newton-Euler, 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. Ademas es posible simpli car las soluciones simbolicas mediante un analisis de los terminos de lo que esta compuesta, ganando mas aun en e ciencia. Todas estas manipulaciones se pueden realizar mediante herramientas de calculo simbolico como MAPLEV. La obtencion de las ecuaciones de movimiento se puede obtener mediante las ecuaciones de la dinamica clasica o bien mediante la mecanica lagrangiana. 41
42
3.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 ; @L = Q i dt @ q_i @qi
i = 1 :::n
(3.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. Al elegir estas variables como grados de libertad, 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 ;U
(3.2)
Donde T es la energ a cinetica total del sistema mecanico y U su energ a potencial. En este caso las variables elegidas como grados de libertad son las variables del espacio articular del manipulador. 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.3)
donde D(q) es la matriz de inercias del sistema, siendo esta simetrica y de nida positiva. La energ a potencial se puede escribir en terminos de matrices de transformacion de la forma:
U=
Donde:
n X i
mi Ti ~ci r
0
(3.4)

0
43
mi es la masa del enlace i. Ti es la matriz de transformacion del sistema de referencia 0 al sistema de referencia i. ~ci es el vector posicion del centro de gravedad del enlace i respecto al r sistema de referencia i.
Sustituyendo la expresion de la energ a cinetica en 3.2, 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 ; @q = @q @q

i k k
k = 1 ::: n
(3.5)
Hasta ahora no se ha considerado los fenomenos de friccion en el modelo, 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.6)
D(q)q representa a las fuerzas inerciales de cada enlace debidas a la aceleracion. C (q q_)q_ representa a los pares de Coriolis y los correspondientes a los terminos centr fugos. F (q q_) representa a los pares de friccion en cada articulacion, que en el caso mas sencillo se puede modelar como una friccion viscosa. G(q) son los pares gravitatorios asociados a la masa de cada enlace.
Las ecuaciones 3.6 modelan la dinamica del brazo manipulador. Se pueden a~adir tambien la dinamica de los actuadores. Suponiendo que los motores n tienen una constante electrica despreciable, la dinamica de estos se reduce a la ecuacion mecanica del eje del motor. Por tanto, se puede escribir para los actuadores en forma vectorial:
m
= Jm qm + Bmq_m +
(3.7)
44
El par motor es proporcional a la corriente que recorre los devanados del estator, por lo que se puede poner:
Kt Im = Jmqm + Bm q_m +
donde:
(3.8)
Kt constante de par del motor. m par electrico desarrollando por el motor. Jm inercia del eje motor. qm posicion del eje motor. Bm friccion viscosa del motor. L par de carga que ofrece el brazo manipulador.
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.8 se tiene:
(3.9) (3.10) (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. Si se sustituye la expresion 3.6 en las ecuaciones 3.7 obtenemos: (D(q) + JmR )q + (C (q q_) + Bm R )q_ + F (q q_) + G(q) = Kt ImR (3.12)
2 2
Los terminos de Coriolis se pueden calcular como funcion de los dij segun se ve en la expresion 3.5, por lo que interesa en primer lugar calcular los
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.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.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. Sustituyendo estas expresiones en 3.13, 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.15)
Identi cando esta ecuacion con la ecuacion 3.3 obtenemos la expresion para la matriz de inercias:
D(q) =
n X i
T (mi JvTci Jvci + J!i IiJ!i )
(3.16)
El jacobiano de las velocidades de traslacion se calcula como una matriz formada por las columnas:
Jvci =
@ (Ti0~ci ) r @q1
:::
@ (Ti0~ci ) r @q6
i
3 6
(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
(3.18)
46 Donde:
8 <
3 Dinamica del robot RM-10 (Tij ); ~ si j k

1
J!ij = :
~ 0
si j > i
(3.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.
0
> T := proc(p) M:=array(identity,1..4,1..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.

> Dmatrix := proc(Mat,x) for n from 1 to 4 do for m from 1
to 4 do Mat n,m]:=diff(Mat n,m],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.

> Dvect := proc(Vect) Vel:=matrix(4,1) for n from 1 to 4 do
Vel n,1]:=diff(Vect n,1],t) od RETURN(Vel) end

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.

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 )
m ) od od
Calcula el jacobiano J!i de la velocidad angular del solido i.

> calJacw:= proc(i) A(0):=array(identity,1..4,1..4) J:=array(sparse,1..3,1..6) for n from 1 to i do 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 from 1 to 3 do J k,n]:=vaux k,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.

Vectores centros de masa ~ci . r
48

> rcm:= i -> matrix(4,1, x i], y i], z i],1]):
Tensores de inercia Ji.

> J:= p -> matrix(3, 3 , Ixx p], 0, 0, 0, Iyy p], 0 ,0, 0,
Izz p]]):
Calcula la matriz D(q).

A continuacion se calcula los jacobianos de las velocidades de traslacion de los centros de gravedad, aportes de cada enlace por traslacion del c.d.g. a la matriz D(q). Dtras es la parte de la matriz D debida a la translacion de los c.d.g.
> Dtras:=0: > for i from 1 to 6 do Jvc i]:=eval(Jacv(evalm(T(i)&*
rcm(i)))): Dt i]:=map(simplify, evalm(m i]* evalm(transpose(Jvc i])&* Jvc i]))) Dt i]:=map(simplify, Dt i], siderels,simp) Dtras:=map(simplify, evalm(Dtras+Dt i])): od:
A continuacion se calcula los jacobianos de las velocidades angulares de los elementos, aportes de cada enlace a la matriz D(q). Drot es la parte de la matriz D debida a la rotacion.
> Drot:=0: > for i from 1 to 6 do Jacw i]:=map(simplify,
eval(calJacw(i))) Jacw i]:=map(simplify,Jacw i], siderels,simp): Dr i]:=evalm(evalm(transpose(Jacw i])&* J(i)&* Jacw i])) Drot:=map(simplify, evalm(Drot+Dr i])): od:
Inercia a~adida por los motores. n

> Dmot:=linalg diag](Jm 1]*R 1]^2, Jm 2]*R 2]^2,
Jm 3]*R 3]^2, Jm 4]*R 4]^2, Jm 5]*R 5]^2, Jm 6]*R 6]^2):
La matriz D(q) sera la suma de la de traslacion, rotacion y la a~adida n por los motores.
> Inertia:= map(simplify,evalm(Dtras+Drot+Dmot)):
49
Los elementos de la matriz C (q q_) se pueden calcular a parir de la los elementos de la matr z D(q). El segundo termino de la ecuacion 3.5 aprovechando la simetr a lo podemos poner como:
n X n @dkj ; 1 @dij )q_ q_ = 1 X( @dkj + @dki ; @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.20) 2 i j ijk i j
Las instrucciones de MapleV necesarias para obtener la matriz C (q q_) son las siguientes:
Calcula los elementos de la matriz de coriolis C(q,qd).

> C := proc(k,j) resu:=sum('Cor(i,j,k)*QD(i)','i'=1..6) RETURN(resu) end
C :=
proc(k j ) localresu resu := sum('Cor(i j k) QD(i)' 'i' = 1::6) RETURN(resu ) end
> for i from 1 to 6 do for j from 1 to 6 do

MC i,j]:=eval(C(i,j)) od od
Los elementos del vector de pares gravitatorios se calculan mediante el tercer termino de la expresion 3.5, o lo que es lo mismo con las instrucciones que siguen a continuacion:
50

> V:=proc(i) m(i)*g*evalm(T(i)&*rcm(i)) 3,1] end
Calculo de los terminos de energia potencial.

V := proc(i) m(i)?g?evalm((T(i)) & (rcm(i))) end
3 1
Calculamos la energia potencial total del brazo manipulador.

> Vtot:=simplify(sum('V(k)','k'=1..6),siderels, simp)
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 ; g (m(5) a3 + a3 m(4) + x3 m(3) + m(6) a3) sin(%1) + g (m(5) y5 + m(6) z6) cos( 4 ) sin(%1) sin( 5 ) ; g (m(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.

> for i from 1 to 6 do phi i]:=diff(Vtot,theta i]) od:
3.3 Parametros del modelo del robot

Para poder simular el comportamiento del robot es necesario disponer de un conjunto de parametros lo mas preciso posible, ademas esto permite poder aplicar tecnicas que se basan en el modelo del manipulador con mas exito. 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. Las distancias que aparecen en las matrices de transformacion son las que aparecen en la tabla 3.1 Entre los parametros dinamicos necesarios para la caracterizacion de un modelo tenemos los siguientes: Tensores de inercia.
3.3 Parametros del modelo del robot
51
dB 950 a 260 a 710 a 150 d 40 d 650 dH 150 Tabla 3.1: Parametros cinematicos del robot.
1 2 3 3 4
Distancias en mm.
Centros de gravedad. Masas. Inercias de los ejes de los motores. Coe cientes de reduccion. Parametros de friccion.
3.3.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. 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. Se hace la suposicion de que hay simetr a en los tres ejes, permitiendo tener as una matriz diagonal en cada enlace. Con la misma herramienta informatica se estimaron los centros de gravedad y las masas de los distintos elementos.
3.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 inercias de los ejes se obtiene a partir de las hojas de catalogo:
52
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
rc2 = 4
0:083 0 ;0:083
3 5
X Z
2 3 0:228 rc2 = 4 0 5 0:187
m = 39
1
m = 51
2
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
0:172 rc3 = 4 0:025 5 0:003
rc4 = 4
0 0 ;0:194
3 5
m = 84
3
m = 34
4
2 3 0:02 0 0 J = 4 0 0:008 0 5 0 0 0:031

5
2 3 0:006 0 0 J = 4 0 0:007 0 5 0 0 0:005

6
2 3 0 rc5 = 4 0:031 5 0
Z Y
2 3 0 rc6 = 4 0 5 0:07
m =7
5
m =7
5
Figura 3.1: Caracter sticas de los elementos del brazo del manipulador
3.3 Parametros del modelo del robot Motor Eje Inercia (kg=cm ) 1 5.05 2 5.05 3 1.58 4 0.475 5 0.235 6 0.235 Tabla 3.2: Inercia asociada a los ejes de los motores. Motor Eje Ratio Reduccion 1 121 2 153 3 105 4 59 5 80 6 50 Tabla 3.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.21) vTOT = (Fv + Fvm R )q De este modo se puede identi car la friccion total como se vera mas adelante. Los coe cientes de las reductoras tienen los valores de 3.3. Kt (Nm=A:) Imax (A:) 1 0.52 10 2 0.52 10 3 0.47 15 4 0.40 26 5 0.31 30 6 0.31 30 Tabla 3.4: Caracter sticas electricas de los motores
2 2
Cada motor tiene asociada una constante de par, con la que es posible conocer el par aplicado por los motores a partir de la intensidad que circula.
54
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. De manera que el par aplicado por los motores vale, si u es la se~al de control: n p u (3.22) = 23 Kt Imax 10 Los valores que se tienen para los distintos ejes son los de la tabla 3.4.
3.3.3 Parametros de friccion

Por medio de la realizacion de experimentos es posible estimar algunos parametros de friccion, como son la friccion viscosa y la friccion de Coulomb,
30
20
10
Fc
Fv
10
20
30 4
q_
0
Figura 3.2: Caracter stica estatica de friccion teorica de manera que se pueda estimar una caracter stica estatica como la de la gura 3.2, que se ha supuesto simetrica. 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. Si se mantiene una frecuencia baja para la se~al n inyectada el par aplicado se usa basicamente en vencer la friccion. El resto de articulaciones se mantiene a su valor de cero.
3.4 Minimizacion de los parametros

30
55
20
10
10
20
30 4
q_
0
Figura 3.3: Caracter stica de friccion experimental del eje 6. En la gura 3.3 se puede ver el resultado obtenido para la articulacion 6 del manipulador. Se ve que existe un fenomeno de histeresis en la curva, 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. En la curva obtenida para el eje 5, la gura 3.4, 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. Sabiendo esto y suponiendo los niveles de Coulomb simetricos se puede deducir su valor.
3.4 Minimizacion de los parametros

Las ecuaciones dinamicas del manipulador comprenden cientos de terminos, 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, que se calculan a partir de los parametros elementales y permiten escribir las ecuaciones del estilo:
56
40
30
20
10
10
20
30
40
50
60 1.5
0.5
q_
0.5
1.5
Figura 3.4: Caracter stica de friccion experimental del eje 5.

30 20
10
10
20
30 2.5
1.5
0.5
q_
0.5
1.5
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
(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
5 6
5 6
z y z y z
6 5 6 5
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
;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
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
24
25
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:
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.6: Distribucion de terminos en las ecuaciones del manipulador simplicadas.
3.5 Implementacion informatica

El modelo del manipulador de la gura 3.6 se encuentra disponible en Simulink para la relalizacion de simulaciones, el subsistema se presenta en la gura 3.7. La implementacion se realiza mediante dos bloques S-Function programados en C. En el bloque rm10model.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. El bloque rm10modelcor.c implementa el bloque C ( _) de la gura 3.6 para facilitar la compilacion del las funciones. Como opciones de simulacion, es posible "desactivar" tanto los terminos de Coriolis y centr petos como los efectos de friccion.
3.6 Simulaciones del modelo

Con objeto de comprobar si el modelo se comporta segun lo previsto se simula el sistema en bucle abierto. 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. En las gura 3.8 se tiene el diagrama Simulink para la simulacion del sistema en bucle abierto. En las guras 3.9y 3.10 se presentan los resultados
60
+ -
M( ) C ( _) F ( _) G( )
Figura 3.6: Diagrama de bloques asociado al modelo de la simulacion del diagrama 3.8.
3.6 Simulaciones del modelo
61
1 In1 rm10model
1 Out1 2 Out2
rm10modelcor
5 m6
Figura 3.7: Diagrama de bloques Simulink para la simulacion del manipulador
vect_gravedad SFunction 0
Mux RM10 Model_RM10 Par Adicional Mux
Posicin
Velocidad
Figura 3.8: Diagrama de bloques para la simulacion del modelo en bucle abierto
62
2
1
5
0.5
1.5
2.5
3.5
4.5
8 6 4
6
2 0 2
0.5
1.5
2.5 t
3.5
4.5
Figura 3.9: Evolucion de las posiciones de las articulaciones 5 y 6

50
50
0.5
1.5
2.5
3.5
4.5
50
Par Velocidad
50 0 0.5 1 1.5 2 2.5 t 3 3.5 4 4.5 5
Figura 3.10: Evolucion de las velocidades de las articulaciones 5 y 6
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.
4.2 Control lineal

4.2.1 Justi cacion
En el cap tulo anterior se vio que las ecuaciones que modelan la dinamica del manipulador son: (D(q) + Jm R )q + (C (q q_) + R )q_ + F (q q_) + G(q) = KtIm R
2 2
(4.1)
Las ecuaciones son claramente no lineales pero ademas estan acopladas por los terminos no diagonales de las matrices D y C, debido a que los terminos diagonales de D y C dependen de la posicion y velocidades de las otras articulaciones. 63
64
p
_d
+ -
Kp + Kv s
+ +
Km Je +Be s
_d
Figura 4.1: Esquema de control lineal tipo PD Ahora bien al introducir la dinamica de los motores y reductoras aparecen en los terminos diagonales cantidades afectadas por R lo cual hace que domine la dinamica de los motores frente a la del propio manipulador. En el l mite de esta situacion el sistema se convertir a en 6 sistema lineales desacoplados. El sistema real tendera mas o menos a esta situacion en funcion de los valores concretos de las magnitudes f sicas del manipulador.
2
4.2.2 Control tipo PD

Aplicando los expuesto anteriormente se pueden dise~ar controladores lin neales para cada articulacion, 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. Con lo que se obtiene un conjunto de sistemas:
u Km1 = Je1 + Be1 _ + ... ... u Km6 = Je6 + Be6 _ +

1 1 1 6 6 6
p1 p6
(4.2)
Los coe cientes Jei y Bei son inercias y fricciones viscosa efectivas del conjunto. Las inercias dependen aun de la posicion del manipulador. Si se calcula la funcion de transferencia en bucle cerrado del diagrama de la gura 4.1 se obtiene:
4.2 Control lineal Art. Je !n Km Kp Kv 1 227 30 54.4 3749 249 2 150 35 68.9 2666 152 3 20.7 35 64.1 395 22 4 0.3 120 53.1 81 1.66 5 0.2 150 64.4 69 0.83 6 0.06 150 40.2 33 0.33 Tabla 4.1: Ganancias del controlador PD
65
GBCPD (s) =
KpKm + Kv Kms s Je + (Be + Kv Km )s + KpKm

2
(4.3)
Se pueden dise~ar las constantes Kv y Kp atendiendo a criterios de estan bilidad. Si identi camos el denominador con la ecuacion caracter stica de un sistema de segundo orden:
s + 2 !n + ! = 0
2 2
(4.4)
Si identi cando el denominador de la expresion 4.3 con la expresion 4.4:
!n = KpJKm e
+ = Be 2JKv Km !
e n
(4.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 ; Be (4.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. Las variaciones son mas importantes en las articulaciones 1 y 2. Las frecuencias !n se eligen segun la velocidad deseada en la articulacion compatible con la saturacion de los actuadores, dado que las articulaciones
2
66
de la base mueven mucha mas masa se adoptan frecuencias naturales mas bajas. En la tabla 4.1 se tienen los valores calculados para cada articulacion.
Resultados de simulacion
La simulacion del controlador PD se realiza mediante el esquema Simulink de la gura 4.2. Las perturbaciones que mas afectan al funcionamiento del controlador son el par gravitatorio y la friccion estatica simulada, que dada la ausencia de efecto integral provocan la existencia de errores en regimen permanente.
[ref] 1 Error seg Control
qact q
K Kp
[ref] u Posicion
Move
qd
[1 pi/6 0.4 0.25 0.25 1] Referencia
Ref
K RM10 Articular Kd Model_RM10 Velocidad
0 50 50 0 20 0] Perturbacion adicional Step
Figura 4.2: Diagrama Simulink para la simulacion de un controlador PD
4.2 Control lineal
67
1 6 0.8
0.6 5
0.4
5 4 6
0.2
0.2
0.4
0.5
tiempo
1.5
2.5
1 0.5
3
2 1 3
0.5 2
1.5
0.5
tiempo
1.5
2.5
Figura 4.3: Seguimiento en posicion para control tipo PD
68
0.05
0.04 6 0.03
0.02
e e e
5 0.01
0.01
0.02
0.5
tiempo
1.5
2.5
0.02
0.015
0.01
0.005
e e e
1 0 2 0.005
0.01
0.015
0.02
0.5
tiempo
1.5
2.5
Figura 4.4: Error de seguimiento en posicion para control tipo PD
4.2 Control lineal
69
1.5
6 1 5
0.5
u u u
5 4
0.5
4 1
1.5
0.5
tiempo
1.5
2.5
1 1 0
u u u
2 1
4 2 5
0.5
tiempo
1.5
2.5
Figura 4.5: Se~ales de control para regulador tipo PD n
70
Resultados experimentales
Las guras de 4.6 a 4.9 muestran los resultados de aplicar el control PD a los ejes 4, 5 y 6 del manipulador. El controlador se ve degradado debido a las no linealidades presentes. Dado que en estos ejes la gravedad ni las masas puestas en juego son dominantes, la no linealidad dominante es la caracter stica de friccion, aunque fenomenos de elasticidad estan presentes dado que las reductoras de los ejes 5 y 6 son del tipo harmonic drive. Las ganancias usadas en la implementacion son algo menores que las calculadas por la aparicion de oscilaciones.
4.2 Control lineal
71
0.8
0.7
0.6
0.5
r
0.4 0.3 0.2 0.1 0 0 0.7 0.2 0.4 0.6 0.8
tiempo
1
1.2
1.4
1.6
1.8
0.6
0.5
0.4
2 2
r
0.3 0.2 0.1 0 0 0.1 0.2 0.4 0.6 0.8
tiempo
1
1.2
1.4
1.6
1.8
0.1
0.2
3 3
r
0.3 0.4 0.5 0.6 0 0.2 0.4 0.6 0.8
tiempo
1
1.2
1.4
1.6
1.8
Figura 4.6: Seguimiento en posicion para control tipo PD en los ejes 4, 5 y 6
72
1.2
0.8
0.6 qd1 0.4 0.2 0 0.2 0 0.9 0.2 0.4 0.6 0.8
tiempo
1
1.2
1.4
1.6
1.8
0.8
0.7
0.6
0.5
qd2
0.4
0.3
0.2
0.1
0.1 0 0.2 0.2 0.4 0.6 0.8
tiempo
1
1.2
1.4
1.6
1.8
0.2
0.4 qd3 0.6 0.8 1 1.2 0 0.2 0.4 0.6 0.8
tiempo
1
1.2
1.4
1.6
1.8
Figura 4.7: Velocidades para control tipo PD en los ejes 4, 5 y 6
4.2 Control lineal

x 10 8
3
73
0 0 0.2 0.4 0.6 0.8
tiempo
1
1.2
1.4
1.6
1.8
x 10 6
0 0 0.2 0.4 0.6 0.8
tiempo
1
1.2
1.4
1.6
1.8
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
tiempo
1.2
1.4
1.6
1.8
Figura 4.8: Error de seguimiento en posicion para control tipo PD en los ejes 4, 5 y 6
74
0.2
0.15
0.1
u
0.05 0 0.05 0 0.2 0.2 0.4 0.6 0.8
tiempo
1
1.2
1.4
1.6
1.8
0.15
0.1
0.05
u
0.05 0.1 0.15 0.2 0.25 0 0 0.2 0.4 0.6 0.8
tiempo
1
1.2
1.4
1.6
1.8
0.05
0.1
0.15
0.2
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
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) =
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
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.
76
1 [ref] 1 s
qact q
K Ki
Control
[ref] K u Posicion Kp
Move
qd
[1 pi/6 0.4 0.25 0.25 1] Referencia
Ref
K RM10 Articular Kd Model_RM10 Velocidad
[0 50 50 0 20 0] Perturbacion adicional Step
Figura 4.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. Logicamente las prestaciones que se podran conseguir con un control lineal estan limitadas, aunque en muchos robots industriales de accionamiento indirecto porporcionan las prestaciones buscadas. En el caso de que los robot no sean de accionamiento indirecto el acoplamiento entre las distintas articulaciones cobra importancia. Es por ello que en las tecnicas de control se empiece a tener en cuenta la naturaleza no lineal del modelo del robot. La tecnica del par calculado basicamente consiste en el uso de dos bucles de realimentacion. Un primer bucle mas interno que linealiza y desacopla el sistema, y un segundo bucle externo que puede ser un control lineal que estabiliza el sistema resultante. Si se aplica un par de la forma: = F ( _) + C ( _) + G( ) + M ( ) 0 (4.9)
77
1.2
1 6 0.8
0.6 5
5 4 6
0.4
0.2
0.2
0.4
0.5
tiempo
1.5
2.5
1 0.5
3
2 1 3
0.5 2
1.5
0.5
tiempo
1.5
2.5
Figura 4.11: Seguimiento en posicion para control tipo PID
78
14
x 10
12
10
e e e
5 4
0 x 10
3
0.5
tiempo
1.5
2.5
3 2
1
3 1 2
e e e
0.5
tiempo
1.5
2.5
Figura 4.12: Error de seguimiento en posicion para control tipo PID
79
1.5
0.5
u u u
4 5
0.5
4 1
1.5
0.5
tiempo
1.5
2.5
1 1 0
u u u
2 1
2 3 3
4 2 5
0.5
tiempo
1.5
2.5
Figura 4.13: Se~ales de control para regulador tipo PID n
80
M( )
+ +
C ( _) + F ( _) + G( )
Figura 4.14: Bucle de linealizacion y desacoplo Igualando con la ecuacion dinamica del modelo nos queda:
0=
(4.10)
El doble integrador resultante se puede estabilizar con controladores lineales como por ejemplo PD o PID, tal y como se ve en la gura 4.15.
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.11)
donde:
e = qd ; q e = q_d ; q_ _
(4.12)
y las matrices Kv y Kp son matrices diagonales. Si igualamos la ecuacion 4.11 con la ecuacion 4.10 se la obtiene la ecuacion diferencial para el error:
e + Kv e + Kpe = 0 _
(4.13)

0
81
qr qdr qddr
Controlador Lineal
Cont
M( )
+ +
C ( _) + F ( _) + G( )
Figura 4.15: Esquema completo de control por par calculado Con Kv y Kp se puede modi car la dinamica del error y obtener el comportamiento deseado. 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. Normal mente son mas bajas en las articulaciones de la base y mas altas en las articulaciones de la mu~eca. n
La simulacion del controlador por par calculado se realiza mediante el esquema Simulink de la gura 4.16. En un primer caso se puede usar un bucle de
82
[ref] 1
[ref]
qact
Qref Q
Control u
Out1
Move
qd QDref
mat_inercia SFunction1
K R Kt Imax RM10
Posicion
5 pi/4 0.4 0.25 0.5 1 Referencia
Ref
qdd
QD
Articular
PID
Velocidad
Model_RM10
[0 100 100 0 30 0]
vect_friccion SFunction
In1 Out1 In2
Coriolis vect_gravedad SFunction3
Figura 4.16: Diagrama Simulink para la simulacion de un controlador Par Calculado Art. !n Kp Kv 1 30 900 60 2 35 1225 70 3 35 1225 70 4 120 81 1.66 5 150 69 0.83 6 150 33 0.33 Tabla 4.3: Ganancias del bucle externo tipo PD realimentacion tipo PD, cuyas constantes se dise~an segun la tabla siguiente n 4.3
83
1 6 0.8
0.6
0.4
5 4 6
0.2
0.2 4 0.4 0 0.5 1 1.5 2 2.5 3
tiempo
1 0.5
3
2 1 3
0.5 2
1.5
0.5
tiempo
1.5
2.5
Figura 4.17: Seguimiento en posicion para control tipo Par Calculado
84
12
x 10
6 10
e e e
5 4
6 4 8 0 x 10
3
0.5
tiempo
1.5
2.5
1.5
e e e
2 1
0.5 1
2 0.5
0.5
tiempo
1.5
2.5
Figura 4.18: Error de seguimiento en posicion para control tipo Par Calculado
85
1.5
6 1 5
0.5
u u u
5 4
0.5
4 1
1.5
0.5
tiempo
1.5
2.5
1 1 0
u u u
2 1
4 2 5
0.5
tiempo
1.5
2.5
Figura 4.19: Se~ales de control para regulador tipo Par Calculado n
86
Resultados experimentales
En las guras 4.20 a 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, la mejora con respecto al controlador lineal no es muy signi cativa. 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.

0
87
0.1
0.2
0.3
1 1
r
0.4 0.5 0.6 0.7 0 0.7 0.5 1 1.5
tiempo
2
2.5
3.5
0.6
0.5
0.4
2 2
r
0.3 0.2 0.1 0 0 0.8 0.5 1 1.5
tiempo
2
2.5
3.5
0.7
0.6
0.5
r
0.4 0.3 0.2 0.1 0 0 0.5 1 1.5
tiempo
2
2.5
3.5
Figura 4.20: Seguimiento en posicion para control tipo par calculado en los ejes 4, 5 y 6
88
0.4
0.2
0.2 qd1 0.4 0.6 0.8 1 0 1.2 0.5 1 1.5
tiempo
2
2.5
3.5
0.8
0.6 qd2 0.4 0.2 0 0.2 0 1.2 0.5 1 1.5
tiempo
2
2.5
3.5
0.8
0.6 qd3 0.4 0.2 0 0.2 0 0.5 1 1.5
tiempo
2
2.5
3.5
Figura 4.21: Velocidades para control tipo par calculado en los ejes 4, 5 y 6

x 10 8
3
89
2
1
e
0 2 4 6 0 0.5 1 1.5
tiempo
2
2.5
3.5
0.01
0.005
0.005
0.01
0.015
0.02 0 0.5 1 1.5
tiempo
2.5
3.5
0.015
0.01
0.005
0
3
e
0.005 0.01 0.015 0.02 0 0.5 1 1.5
tiempo
2.5
3.5
Figura 4.22: Error de seguimiento en posicion para control tipo par calculado en los ejes 4, 5 y 6
90
0.02
0.02
0.04
u
0.06 0.08 0.1 0.12 0 0.25 0.5 1 1.5
tiempo
2
2.5
3.5
0.2
0.15
0.1
u
0.05 0 0.05 0.1 0 0.2 0.5 1 1.5
tiempo
2
2.5
3.5
0.15
0.1
u
0.05 0 0.05 0 0.5 1 1.5
tiempo
2
2.5
3.5
Figura 4.23: Se~al de control para control tipo par calculado en los ejes 4, 5 n y6
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, as como captar se~ales n de alarma de distintas fuentes. Todas las entradas y salidas estan optoaisladas y realizan una conversion de tension de 0-24V a niveles TTL, en el armario y la tarjeta controladora respectivamente. La con guracion de las etapas de salida y entrada se puede ver en las guras A.2 y A.1 respectivamente.
Vcc R
1 2
IN
IC
OUT
IC
Figura A.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
92
Vcc
A Tarjeta de E/S digital
OUT D IN R
1 2
OUT D
1
IC
Figura 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. 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.1 y A.2 se encuentra detallada el uso de cada se~al y la numeracion usada para el conexionado. n La tarjeta precisa de alimentacion de +24V por parte del armario de System-Robot para poder activar los reles, y por parte de la tarjeta controladora dSPACE se precisa de +5V para alimentar toda la logica TTL.
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. 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.2: Conexionado salidas digitales
94
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, emulando las se~ales de un encoder inn cremental de 1024 pulso por vuelta. La tarjeta incorpora algunas funciones adicionales para facilitar la incorporacion del nuevo controlador. El diagrama de bloques funcional de la tarjeta se puede en la gura B.1.
Filtrado/ Ajuste de fase
Conversin RE
Amplificacin
OptoAislacin
Conmutacin
Aislacin Analgica
Figura B.1: Diagrama de bloques de la tarjeta CRE En la gura B.2 se representa la disposicion f sica de los distintos conectores que dispone la tarjeta y en la tabla B.1 la funcion de cada uno de sus 95
96 pines.
1 1 2 3 4 2
B Tarjeta de conversion resolver-encoder
6 1 2
P4 P3 P5
3 4
P7 P1 P2
1
1 2
P6
Figura B.2: Conectores presentes en la tarjeta CRE La funcion de cada conector es la que sigue: 1. P1/P2: Conectores de entrada y salida de resolver. Permiten la lectura de las se~ales del resolver y de la referencia. Es necesario tener dos n conectores ya que no se puede interrumpir las se~ales de resolver que n llegan al servoampli cador debido a que este las necesita para la logica de conmutacion de los puentes de transistores de potencia. 2. P3: Conector de alimentacion del lado del armario de control. 3. P4: Conmutacion de se~ales analogica y salida de la misma. n 4. P5: Conector de alimentacion de lado del PC. 5. P6: Conector de encoder incremental. 6. P7: Entrada analogica proveniente del la tarjeta controladora.
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. n
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
98
Idealmente las se~ales inducidas en los devanados jos de los resolvers n estan en fase con las se~al aplicada de referencia, esto en realidad no es as n ya que las bobinas de los resolver distan de ser ideales y como resultado las se~ales inducidas seno y coseno presentan un desfase electrico apreciable. n Este desfase es necesario compensarlo ya que resulta excesivo para que la conversion de la etapa siguiente funcione correctamente. Para ello se introduce en la se~al de referencia un ltro activo de primer orden basado en n ampli cador operacional, que permite ajustar el desfase introducido por el resolver. El ltro permite ajustar tambien su ganancia una vez introducido el desfase. 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. Este n problema no aparece con las dos senoides inducidas ya que el resolver tiene una relacion de transformacion de 0,5.
R2
R1 +
C1
IC1
Figura B.3: Esquema del ltro activo para la se~al de referencia n La funcion de transferencia del circuito de la gura B.1 resulta ser: 1 Vo = ; R (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
B.2 Conversion resolver a encoder
99
B.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, dando la posicion de salida en dos formatos digitales con una resolucion de 12 bits . Uno de los formatos es tipo encoder incremental con niveles de tension TTL, el otro formato disponible es una linea serie.
REF
SENO+ SENOCOSENO+ COSENONGULO MULTIPLICADOR
-SIN( -)
DETECTOR DE FASE + INTEGRADOR
VEL
NGULO NMC A B NM CLKOUT LGICA DE DECODIF . CONTADOR REVERSIBLE OSCILADOR CONTROLADO POR TENSIN DIR
CS
LATCH
SCLK DATOS
INTERFACE SERIE
Figura B.4: Funcionamiento interno del circuito integrado AD2S90 El circuito realiza la conversion con una tecnica de tracking. Basicamente el circuito calcula una se~al proporcional al error entre el angulo real y el n angulo almacenado en un contador reversible, y a partir de este error incrementa o decrementa el contador hasta reducir el error a cero.
B.3 Ampli cador adaptador

Esta etapa es necesaria ya que las salidas de encoder incremental del AD2S90 no son capaces de suministrar su ciente corriente para activar la etapa de optoaislamiento.
100
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
Apendice C Esquemas electricos de electronica de potencia
103
104
C Esquemas electricos de electronica de potencia
Apendice D Codigo fuente funciones Simulink
105
106
/* * * * * * * * */ #define S_FUNCTION_NAME rm10model #define S_FUNCTION_LEVEL 2 #include <math.h> #include "simstruc.h"
D Codigo fuente funciones Simulink

Modelo para el robot manipulador RM-10 Implementa la matriz de inercias, gravedad y friccin Los efectos de coriolis se aaden mediante el archivo S-Function "rm10modelcor.c". 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]

#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]
/* 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,0) /* Posicion inicial del robot del robot*/ G(S) ssGetSFcnParam(S,1) /* Aceleracion de la gravedad */ ENABLEFRIC(S) ssGetSFcnParam(S,2) /* Habilitar friccin */
109
static void mdlInitializeSizes(SimStruct *S) { ssSetNumContStates( ssSetNumDiscStates( S, 12) S, 0) /*Entradas*/
if (!ssSetNumInputPorts(S, 1)) return ssSetInputPortWidth(S, 0, 6) ssSetInputPortDirectFeedThrough(S, 0, 0)
if (!ssSetNumOutputPorts(S,3)) return /*Salidas*/ ssSetOutputPortWidth(S, 0, 6) /*Posicion*/ ssSetOutputPortWidth(S, 1, 6) ssSetOutputPortWidth(S, 2, 6) /*Velocidad*/ /*Velocidad*/
ssSetNumSFcnParams(S, 3) if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { return } ssSetNumSampleTimes( ssSetNumRWork( ssSetNumIWork( ssSetNumPWork( S, 1) S, 103) S, 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) ssSetNumNonsampledZCs(S, 0) ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE) } /* * mdlInitializeSampleTimes - Inicializa muestreos */ static void mdlInitializeSampleTimes(SimStruct *S) { ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME) ssSetOffsetTime(S, 0, 0.0) }
/*Automatico segun bloque anterior*/
#define MDL_INITIALIZE_CONDITIONS /* * mdlInitializeConditions - Inicializa estados y parametros del robot *
110
*/
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.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.00044 0.00044 0.00014 0.000041 0.000017 0.000017 121 153 105 59 80 50 0.260 0.710 0.150 0.040 0.650 0.0839 -0.0832 0.2286 /*Distancias constantes*/ /*Reductoras*/ /* Flag de inicializacin a cero */ /* Masa opcional garra Adicional */ /*Inercias motores*/
/*Centros de masa*/

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.1876 0.1717 0.0246 0.0027 -0.1945 0.0317 0.050 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 40 30 20 10 6 4.5 /*Masas*/ /*Inercias*/
111
/*Friccin viscosa total*/
112
FC1 FC2 FC3 FC4 FC5 FC6 = = = = = = 60 50 40 30 24 14

/*Friccion coulomb*/
/* Distancias al cuadrado a1_2 a2_2 a3_2 d3_2 d4_2 x2_2 x3_2 y3_2 z4_2 y5_2 z6_2 = = = = = = = = = = = a1*a1 a2*a2 a3*a3 d3*d3 d4*d4 x2*x2 x3*x3 y3*y3 z4*z4 y5*y5 z6*z6
*/
for(i=0 i<6 i++) { yant(i)=0.0 ffric(i)=1.0 }
} /* * mdlOutputs - Calcula las salidas * */ static void mdlOutputs(SimStruct *S, int_T tid) { int i,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]

/* 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.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 - s2*s3 y=ssGetOutputPortRealSignal(S,2) for (i=0 i<6 i++) y i] = FLAG_DEBUG(i) } #define MDL_DERIVATIVES /*
114
* mdlDerivatives - compute the derivatives * */ static void mdlDerivatives(SimStruct *S) { int i,j, 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, forceac static real_T
k0,
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, k26 static real_T m 7] 7]
static real_T 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
real_T real_T real_T real_T real_T real_T real_T
f 6] z_1, z_2, z_3, z_4, z_5, z_6 x 6] fP_1, fP_2, fP_3, fP_4, fP_5, fP_6 fG 6] fricc 6],sig 6] s2_2, c2_2, s3_2, c3_2, s4_2, c4_2, s5_2, c5_2, s6_2, c6_2, s23_2, c23_2
/*Puntero a vector de pares*/ uPtrs = ssGetInputPortRealSignalPtrs(S,0) y=ssGetOutputPortRealSignal(S,1)
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

/* 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] =

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

/* 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 */
120
x 5] = x 4] = x 3] = x 2] = x 1] = x 0] =

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

#else #include "cg_sfun.h" #endif
121
122
/* * * * * * */ #define S_FUNCTION_NAME rm10modelcor #define S_FUNCTION_LEVEL 2 #include <math.h> #include "simstruc.h" autor: Carlos Perez Fernandez fecha: 30-4-99

Calcula el par de Coriolis debido a las velocidades de los distintos elementos del robot.
/* Variables del espacio de trabajo */ #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define mE Jm1 Jm2 Jm3 Jm4 Jm5 Jm6 a1 a2 a3 d3 d4 x1 z1 x2 z2 x3 y3 z3 z4 y5 z6 Izz1 Ixx2 Iyy2 Izz2 Ixx3 Iyy3 rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork rwork 0] 1] 2] 3] 4] 5] 6] 7] 8] 9] 10] 11] 12] 13] 14] 15] 16] 17] 18] 19] 20] 21] 22] 23] 24] 25] 26] 27]

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

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

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

} /* * 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
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))

c23 = cos(Q(2)+Q(3)) s2_2 c2_2 s3_2 c3_2 s4_2 c4_2 c5_2 s5_2 c6_2 s6_2 c23_2 s23_2 = c2*c2 = c2*c2 = s3*s3 = c3*c3 = s4*s4 = c4*c4 = c5*c5 = s5*s5 = c6*c6 = s6*s6 = c23*c23 = s23*s23
129
/* Fuerzas centrifugas y de coriolis */ /*------------------------------------*/ C(1,1)=((((k7*c6_2-k21)*s5*c5+k4*s5)*c4-k7*c6*s5*s6*s4-k5*c5+ .5*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*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*k16*s2)*QD(2)+((((k7*c6_2k21)*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)*s23k2*c5-k17*s2*c3+.5*k18)*c23+(((-k7*c6_2+k21)*s5*c5-k4*s5)*c4.5*k15+k5*c5+k7*c6*s5*s6*s4)*s23_2+((-k9+k3*c4*s5)*c2+(k17*s3k12+k3*c5)*s2-.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+
130
(-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,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*k18)*c23+ (((-k7*c6_2+k21)*s5*c5-k4*s5)*c4-.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*k16*s2)*QD(1)+ ((2*k7*c6*c5*c4_2*s6+(((k7*c6_2-k21)*c5_2+k7*c6_2-k22)*s4k6*s5)*c4+k14+k5*s4*s5-k7*c6*c5*s6)*c23+(-k7*c6*s5*s6*c4+ ((-k7*c6_2+k21)*s5*c5-k4*s5)*s4-k6*c5-k19)*s23+(k3*s4*s5+ k20)*c2)*QD(2)+((2*k7*c6*c5*c4_2*s6+(((k7*c6_2-k21)*c5_2+ k7*c6_2-k22)*s4-k6*s5)*c4+k14+k5*s4*s5-k7*c6*c5*s6)*c23+ (-k7*c6*s5*s6*c4+((-k7*c6_2+k21)*s5*c5-k4*s5)*s4-k6*c5k19)*s23)*QD(3)+(((.5*k7*c6_2-.5*k21)*c5_2-.5*k22+ .5*k7*c6_2)*c4_2-2*k7*c6*c5*s4*s6*c4+((-.5*k7*c6_2+.5*k21)*c5_2+ .5*k22-.5*k7*c6_2)*s4_2+k6*s4*s5+(-.5*k7*c6_2+.5*k21)*c5_2+ .5*k7*c6_2-.5*k13)*s23*QD(4)+((k7*c6*c5*s6*c4+((.5*k7*c6_2.5*k21)*c5_2+(-.5*k7*c6_2+.5*k21)*s5_2+.5*k10+.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*k1+.5*k7*c6_2-.5*k7*s6_2)*s5*c4k7*s5*c5*s4*c6*s6)*c23+((-k7*s6_2+k7*c6_2)*c5*c4_2+(-k7*c6*s6k7*c5_2*c6*s6)*s4*c4+(-.5*k1-.5*k7*c6_2+.5*k7*s6_2)*c5)*s23)*QD(6) C(1,3)=((((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)*s23k2*c5-k17*s2*c3+.5*k18)*c23+(((-k7*c6_2+k21)*s5*c5-k4*s5)*c4.5*k15+k5*c5+k7*c6*s5*s6*s4)*s23_2+((-k9+k3*c4*s5)*c2+(k17*s3k12+k3*c5)*s2-.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*k7*c6_2-.5*k21)*c5_2-.5*k22+.5*k7*c6_2)*c4_2 -2*k7*c6*c5*s4*s6*c4+((-.5*k7*c6_2+.5*k21)*c5_2+.5*k22.5*k7*c6_2)*s4_2+k6*s4*s5+(-.5*k7*c6_2+.5*k21)*c5_2+.5*k7*c6_2.5*k13)*s23*QD(4)+((k7*c6*c5*s6*c4+((.5*k7*c6_2-.5*k21)*c5_2+ (-.5*k7*c6_2+.5*k21)*s5_2+.5*k10+.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*k1+.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*k7*c6_2+.5*k7*s6_2)*c5)*s23)*QD(6) C(1,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*c6_2-.5*k21)*c5_2-.5*k22+ .5*k7*c6_2)*c4_2-2*k7*c6*c5*s4*s6*c4+((-.5*k7*c6_2+.5*k21)*c5_2+ .5*k22-.5*k7*c6_2)*s4_2+k6*s4*s5+(-.5*k7*c6_2+.5*k21)*c5_2+ .5*k7*c6_2-.5*k13)*s23*QD(2)+(((.5*k7*c6_2-.5*k21)*c5_2-.5*k22+ .5*k7*c6_2)*c4_2-2*k7*c6*c5*s4*s6*c4+((-.5*k7*c6_2+.5*k21)*c5_2+ .5*k22-.5*k7*c6_2)*s4_2+k6*s4*s5+(-.5*k7*c6_2+.5*k21)*c5_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*k7*c6_2+.5*k21)*c5_2-k4*c5+(.5*k7*c6_2-.5*k21)*s5_2+.5*k10+ .5*k7*c6_2)*c4*s23+k3*c2*c4*c5+k2*c4*c5)*QD(5)+((-k7*c5_2*c6*s6+ k7*c6*s6)*c23+(k7*s5*c5*c4*c6*s6+(-.5*k7*s6_2+.5*k7*c6_2+ .5*k1)*s5*s4)*s23)*QD(6) C(1,5)=(((k7*c6_2-k21)*s5*c5*c4_2+(-k5*c5-k7*c6*s5*s6*s4)*c4+(k7*c6_2k21)*s5*c5+k4*s5)*c23_2+((((k7*c6_2-k21)*c5_2+k4*c5+(-k7*c6_2+ k21)*s5_2)*c4+k5*s5-k7*c6*c5*s4*s6)*s23-k3*c2*c4*c5-k2*c4*c5+ k3*s5*s2)*c23+k2*s23*s5+k3*s5*s3+(-k7*c6_2+k21)*s5*c5*c4_2+ k7*c6*s5*s4*s6*c4-k4*s5+k6*c5*s4)*QD(1)+((k7*c6*c5*s6*c4+ ((.5*k7*c6_2-.5*k21)*c5_2+(-.5*k7*c6_2+.5*k21)*s5_2+.5*k10+ .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-.5*k21)*c5_2+(-.5*k7*c6_2+.5*k21)*s5_2+.5*k10+ .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*k7*c6_2+.5*k21)*c5_2-k4*c5+ (.5*k7*c6_2-.5*k21)*s5_2+.5*k10+.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*k1+.5*k7*c6_2-.5*k7*s6_2)*s5*c23+((-.5*k1+.5*k7*c6_2.5*k7*s6_2)*c5*c4-k7*c6*s4*s6)*s23)*QD(6) C(1,6)=(((k7*c6*s6+k7*c5_2*c6*s6)*c4_2+(-k7*s6_2+k7*c6_2)*c5*s4*c42*k7*c6*s6+k7*c5_2*c6*s6)*c23_2+(-2*k7*s5*c5*c4*c6*s6+(k7*s6_2k7*c6_2)*s5*s4)*s23*c23+(-k7*c6*s6-k7*c5_2*c6*s6)*c4_2+(k7*s6_2k7*c6_2)*c5*s4*c4+k7*c6*s6)*QD(1)+(((-.5*k1+.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*k7*c6_2+.5*k7*s6_2)*c5)*s23)*QD(2)+(((-.5*k1+.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-
132

.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*k7*s6_2+.5*k7*c6_2+ .5*k1)*s5*s4)*s23)*QD(4)+((-.5*k1+.5*k7*c6_2-.5*k7*s6_2)*s5*c23+ ((-.5*k1+.5*k7*c6_2-.5*k7*s6_2)*c5*c4-k7*c6*s4*s6)*s23)*QD(5)
C(2,1)=((((-k7*c6_2+k21)*s5*c5-k4*s5)*c4-.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*k18)*c23+(((k7*c6_2-k21)*s5*c5+k4*s5)*c4k7*c6*s5*s6*s4-k5*c5+.5*k15)*s23_2+((k9-k3*c4*s5)*c2+ (-k3*c5-k17*s3+k12)*s2+.5*k11-k2*c4*s5)*s23+k8*c2*s2+ .5*k16*s2)*QD(1)+((((k7*c6_2-k21)*s5*c5+k4*s5)*c4k7*c6*s5*s6*s4)*c23+(((.5*k7*c6_2-.5*k21)*c5_2-.5*k22+ .5*k7*c6_2)*c4_2+(k5*s5-2*k7*c6*c5*s4*s6)*c4+((-.5*k7*c6_2+ .5*k21)*c5_2+.5*k22-.5*k7*c6_2)*s4_2+(.5*k7*c6_2-.5*k21)*c5_2+ .5*k13-.5*k7*c6_2)*s23+k3*s2*c4*s5)*QD(4)+(((.5*k7*c6_2.5*k21)*c5_2+k4*c5+(-.5*k7*c6_2+.5*k21)*s5_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+k3*c5*s4*s2)*QD(5)+(((-.5*k7*s6_2+ .5*k7*c6_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*s6_2.5*k7*c6_2+.5*k1)*c5)*s23)*QD(6) C(2,2)=((k9-k3*c4*s5)*s2*c23+(-k9+k3*c4*s5)*c2*s23+(k12-k3*c5)*c3)*QD(3)+ (k3*c2*c23*s4*s5+k3*s2*s4*s23*s5+k7*c6*c5*c4_2*s6+((k7*c6_2k21)*c5_2+k7*c6_2-k22)*s4*c4+k5*s4*s5-k7*c6*c5*s4_2*s6)*QD(4)+ (-k3*c2*c23*c4*c5-k3*s2*c4*s23*c5+k3*s5*s3+(k7*c6_2k21)*s5*c5*c4_2+(-k5*c5-k7*c6*s5*s6*s4)*c4+(-k7*c6_2+k21)*s5*c5k4*s5)*QD(5)+((k7*c6*s6+k7*c5_2*c6*s6)*c4_2+(-k7*s6_2+ k7*c6_2)*c5*s4*c4-k7*c5_2*c6*s6)*QD(6) C(2,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*k3*c2*c23*s4*s5+.5*k3*s2*s4*s23*s5+.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*k3*s5*s2.5*k3*c2*c4*c5)*c23+(-.5*k3*s2*c4*c5+.5*k3*c2*s5)*s23.5*k3*c5*c4*c3+.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,4)=((((k7*c6_2-k21)*s5*c5+k4*s5)*c4-k7*c6*s5*s6*s4)*c23+(((.5*k7*c6_2.5*k21)*c5_2-.5*k22+.5*k7*c6_2)*c4_2+(k5*s5-2*k7*c6*c5*s4*s6)*c4+ ((-.5*k7*c6_2+.5*k21)*c5_2+.5*k22-.5*k7*c6_2)*s4_2+(.5*k7*c6_2.5*k21)*c5_2+.5*k13-.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-
133
k21)*c5_2+k7*c6_2-k22)*s4*c4+k5*s4*s5-k7*c6*c5*s4_2*s6)*QD(2)+ (.5*k3*c2*c23*s4*s5+.5*k3*s2*s4*s23*s5+.5*k3*s5*s4*c3+ k7*c6*c5*c4_2*s6+((k7*c6_2-k21)*c5_2+k7*c6_2-k22)*s4*c4+k5*s4*s5k7*c6*c5*s4_2*s6)*QD(3)+(k3*s5*c4*s3+((-k7*c6_2+k21)*s5*c5k4*s5)*c4+k7*c6*s5*s6*s4)*QD(4)+(k3*c5*s4*s3+((-.5*k7*c6_2+ .5*k21)*c5_2-k4*c5+(.5*k7*c6_2-.5*k21)*s5_2+.5*k10+ .5*k7*c6_2)*s4)*QD(5)+((.5*k7*s6_2-.5*k1-.5*k7*c6_2)*s5*c4+ k7*s5*c5*s4*c6*s6)*QD(6) C(2,5)=(((.5*k7*c6_2-.5*k21)*c5_2+k4*c5+(-.5*k7*c6_2+.5*k21)*s5_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+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*k3*s5*s2-.5*k3*c2*c4*c5)*c23+(-.5*k3*s2*c4*c5+ .5*k3*c2*s5)*s23-.5*k3*c5*c4*c3+.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*k7*c6_2+.5*k21)*c5_2-k4*c5 +(.5*k7*c6_2-.5*k21)*s5_2+.5*k10+.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*k7*c6_2.5*k1)*c5*s4)*QD(6) C(2,6)=(((-.5*k7*s6_2+.5*k7*c6_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*k1+.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*k7*s6_2-.5*k1.5*k7*c6_2)*s5*c4+k7*s5*c5*s4*c6*s6)*QD(4)+(k7*c4*c6*s6+ (-.5*k7*s6_2+.5*k7*c6_2-.5*k1)*c5*s4)*QD(5) C(3,1)=((((-k7*c6_2+k21)*s5*c5-k4*s5)*c4-.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*k18+k17*s2*c3+k2*c5)*c23+(((k7*c6_2k21)*s5*c5+k4*s5)*c4-k7*c6*s5*s6*s4-k5*c5+.5*k15)*s23_2+((k9k3*c4*s5)*c2+(-k3*c5-k17*s3+k12)*s2+.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*k21)*c5_2-.5*k22+ .5*k7*c6_2)*c4_2+(k5*s5-2*k7*c6*c5*s4*s6)*c4+((-.5*k7*c6_2+ .5*k21)*c5_2+.5*k22-.5*k7*c6_2)*s4_2+(.5*k7*c6_2-.5*k21)*c5_2+ .5*k13-.5*k7*c6_2)*s23)*QD(4)+(((.5*k7*c6_2-.5*k21)*c5_2+k4*c5+ (-.5*k7*c6_2+.5*k21)*s5_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*s6_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-
134
k7*c5_2*c6*s6)*s4*c4+(-.5*k7*c6_2+.5*k1+.5*k7*s6_2)*c5)*s23)*QD(6) C(3,2)=((-k9+k3*c4*s5)*s2*c23+(k9-k3*c4*s5)*c2*s23+(-k12+k3*c5)*c3)*QD(2)+ (.5*k3*c2*c23*s4*s5+.5*k3*s2*s4*s23*s5-.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*k3*s5*s2-.5*k3*c2*c4*c5)*c23+ (-.5*k3*c2*s5-.5*k3*s2*c4*c5)*s23+.5*k3*c5*c4*c3+.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,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,4)=((((k7*c6_2-k21)*s5*c5+k4*s5)*c4-k7*c6*s5*s6*s4)*c23+ (((.5*k7*c6_2-.5*k21)*c5_2-.5*k22+.5*k7*c6_2)*c4_2+(k5*s52*k7*c6*c5*s4*s6)*c4+((-.5*k7*c6_2+.5*k21)*c5_2+.5*k22.5*k7*c6_2)*s4_2+(.5*k7*c6_2-.5*k21)*c5_2+.5*k13.5*k7*c6_2)*s23)*QD(1)+(.5*k3*c2*c23*s4*s5+.5*k3*s2*s4*s23*s5.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*k7*c6_2+.5*k21)*c5_2-k4*c5+ (.5*k7*c6_2-.5*k21)*s5_2+.5*k10+.5*k7*c6_2)*s4*QD(5)+((-.5*k1+ .5*k7*s6_2-.5*k7*c6_2)*s5*c4+k7*s5*c5*s4*c6*s6)*QD(6) C(3,5)=(((.5*k7*c6_2-.5*k21)*c5_2+k4*c5+(-.5*k7*c6_2+.5*k21)*s5_2.5*k10-.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*k3*s5*s2.5*k3*c2*c4*c5)*c23+(-.5*k3*c2*s5-.5*k3*s2*c4*c5)*s23+ .5*k3*c5*c4*c3+.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+.5*k21)*c5_2-k4*c5+(.5*k7*c6_2.5*k21)*s5_2+.5*k10+.5*k7*c6_2)*s4*QD(4)+(-k5*c5-k7*c6*s5*s6*s4k4*s5*c4)*QD(5)+(k7*c4*c6*s6+(-.5*k1+.5*k7*c6_2.5*k7*s6_2)*c5*s4)*QD(6) C(3,6)=(((.5*k7*c6_2+.5*k1-.5*k7*s6_2)*s5*c4-k7*s5*c5*s4*c6*s6)*c23+ ((-k7*s6_2+k7*c6_2)*c5*c4_2+(-k7*c6*s6-k7*c5_2*c6*s6)*s4*c4+ (-.5*k7*c6_2+.5*k7*s6_2+.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*k1+.5*k7*s6_2.5*k7*c6_2)*s5*c4+k7*s5*c5*s4*c6*s6)*QD(4)+(k7*c4*c6*s6+(-.5*k1+

.5*k7*c6_2-.5*k7*s6_2)*c5*s4)*QD(5) C(4,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+.5*k21)*c5_2+.5*k22.5*k7*c6_2)*c4_2+(2*k7*c6*c5*s4*s6-k5*s5)*c4+((.5*k7*c6_2.5*k21)*c5_2-.5*k22+.5*k7*c6_2)*s4_2+(-.5*k7*c6_2+.5*k21)*c5_2+ .5*k7*c6_2-.5*k13)*s23-k3*s5*c4*s2)*QD(2)+((((-k7*c6_2+k21)*s5*c5k4*s5)*c4+k7*c6*s5*s6*s4)*c23+(((-.5*k7*c6_2+.5*k21)*c5_2+.5*k22.5*k7*c6_2)*c4_2+(2*k7*c6*c5*s4*s6-k5*s5)*c4+((.5*k7*c6_2.5*k21)*c5_2-.5*k22+.5*k7*c6_2)*s4_2+(-.5*k7*c6_2+.5*k21)*c5_2+ .5*k7*c6_2-.5*k13)*s23)*QD(3)+((-k7*c6_2+k21)*s5*c5*c23+ (((-.5*k7*c6_2+.5*k21)*c5_2+(.5*k7*c6_2-.5*k21)*s5_2-.5*k7*c6_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*k1+.5*k7*c6_2.5*k7*s6_2)*s5*s4)*s23)*QD(6) C(4,2)=((((-k7*c6_2+k21)*s5*c5-k4*s5)*c4+k7*c6*s5*s6*s4)*c23+ (((-.5*k7*c6_2+.5*k21)*c5_2-.5*k7*c6_2+.5*k22)*c4_2+(-k5*s5+ 2*k7*c6*c5*s4*s6)*c4+((.5*k7*c6_2-.5*k21)*c5_2-.5*k22+ .5*k7*c6_2)*s4_2+(-.5*k7*c6_2+.5*k21)*c5_2-.5*k13+.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*k3*c2*c23*s4*s5.5*k3*s2*s4*s23*s5+.5*k3*s5*s4*c3-k7*c6*c5*c4_2*s6+((-k7*c6_2+ k21)*c5_2+k22-k7*c6_2)*s4*c4+k7*c6*c5*s4_2*s6-k5*s4*s5)*QD(3)+ (-k7*c6*c5*s6*c4+((-.5*k7*c6_2+.5*k21)*c5_2+(.5*k7*c6_2.5*k21)*s5_2-.5*k7*c6_2-.5*k10)*s4)*QD(5)+((.5*k1-.5*k7*c6_2+ .5*k7*s6_2)*s5*c4+k7*s5*c5*s4*c6*s6)*QD(6) C(4,3)=((((-k7*c6_2+k21)*s5*c5-k4*s5)*c4+k7*c6*s5*s6*s4)*c23+ (((-.5*k7*c6_2+.5*k21)*c5_2+.5*k22-.5*k7*c6_2)*c4_2+ (2*k7*c6*c5*s4*s6-k5*s5)*c4+((.5*k7*c6_2-.5*k21)*c5_2-.5*k22+ .5*k7*c6_2)*s4_2+(-.5*k7*c6_2+.5*k21)*c5_2+.5*k7*c6_2.5*k13)*s23)*QD(1)+(-.5*k3*c2*c23*s4*s5-.5*k3*s2*s4*s23*s5+ .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*c6_2+ .5*k21)*c5_2+(.5*k7*c6_2-.5*k21)*s5_2-.5*k7*c6_2.5*k10)*s4)*QD(5)+((-.5*k7*c6_2+.5*k7*s6_2+.5*k1)*s5*c4+ k7*s5*c5*s4*c6*s6)*QD(6) C(4,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+k21)*s5*c5*c23+(((-.5*k7*c6_2+.5*k21)*c5_2+(.5*k7*c6_2-
136

.5*k21)*s5_2-.5*k7*c6_2-.5*k10)*c4+k7*c6*c5*s4*s6)*s23)*QD(1)+ (-k7*c6*c5*s6*c4+((-.5*k7*c6_2+.5*k21)*c5_2+(.5*k7*c6_2.5*k21)*s5_2-.5*k7*c6_2-.5*k10)*s4)*QD(2)+(-k7*c6*c5*s6*c4+ ((-.5*k7*c6_2+.5*k21)*c5_2+(.5*k7*c6_2-.5*k21)*s5_2-.5*k7*c6_2.5*k10)*s4)*QD(3)+(k7*c6_2-k21)*s5*c5*QD(4)-k7*QD(5)*c6*c5*s6+ (-.5*k7*c6_2+.5*k7*s6_2+.5*k1)*s5*QD(6)
C(4,6)=((k7*c6*s6-k7*c5_2*c6*s6)*c23+(k7*s5*c5*c4*c6*s6+(-.5*k7*s6_2+ .5*k7*c6_2-.5*k1)*s5*s4)*s23)*QD(1)+((-.5*k7*c6_2+.5*k7*s6_2+ .5*k1)*s5*c4+k7*s5*c5*s4*c6*s6)*QD(2)+((-.5*k7*c6_2+.5*k7*s6_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+.5*k7*s6_2+.5*k1)*s5*QD(5) C(5,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*k21)*c5_2-k4*c5+(.5*k7*c6_2-.5*k21)*s5_2+.5*k10+ .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*c6_2+ .5*k21)*c5_2-k4*c5+(.5*k7*c6_2-.5*k21)*s5_2+.5*k10+ .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*k7*c6_2-.5*k21)*c5_2+(-.5*k7*c6_2+.5*k21)*s5_2+.5*k10+ .5*k7*c6_2)*c4-k7*c6*c5*s4*s6)*s23)*QD(4)+((.5*k7*c6_2+.5*k1.5*k7*s6_2)*s5*c23+((.5*k7*c6_2+.5*k1-.5*k7*s6_2)*c5*c4k7*c6*s4*s6)*s23)*QD(6) C(5,2)=(((-.5*k7*c6_2+.5*k21)*c5_2-k4*c5+(.5*k7*c6_2-.5*k21)*s5_2+ .5*k10+.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*k3*c2*c4*c5-.5*k3*s5*s2)*c23+(.5*k3*c2*s5+ .5*k3*s2*c4*c5)*s23-.5*k3*c5*c4*c3-.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+ (-.5*k7*c6_2+.5*k21)*s5_2+.5*k10+.5*k7*c6_2)*s4)*QD(4)+ (k7*c4*c6*s6+(.5*k7*c6_2+.5*k1-.5*k7*s6_2)*c5*s4)*QD(6) C(5,3)=(((-.5*k7*c6_2+.5*k21)*c5_2-k4*c5+(.5*k7*c6_2-.5*k21)*s5_2+.5*k10+ .5*k7*c6_2)*s4*c23+(k7*c6*s5*c4_2*s6+(k7*c6_2-k21)*s5*c5*s4*c4k7*c6*s5*s6-k5*s4*c5)*s23)*QD(1)+((.5*k3*c2*c4*c5-.5*k3*s5*s2)*c23+ (.5*k3*c2*s5+.5*k3*s2*c4*c5)*s23-.5*k3*c5*c4*c3-.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+

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+(-.5*k7*c6_2+ .5*k21)*s5_2+.5*k10+.5*k7*c6_2)*s4)*QD(4)+(k7*c4*c6*s6+ (.5*k7*c6_2+.5*k1-.5*k7*s6_2)*c5*s4)*QD(6)
137
C(5,4)=((k7*c6_2-k21)*s5*c5*c23+(((.5*k7*c6_2-.5*k21)*c5_2+(-.5*k7*c6_2+ .5*k21)*s5_2+.5*k10+.5*k7*c6_2)*c4-k7*c6*c5*s4*s6)*s23)*QD(1)+ (k7*c6*c5*s6*c4+((.5*k7*c6_2-.5*k21)*c5_2+(-.5*k7*c6_2+ .5*k21)*s5_2+.5*k10+.5*k7*c6_2)*s4)*QD(2)+(k7*c6*c5*s6*c4+ ((.5*k7*c6_2-.5*k21)*c5_2+(-.5*k7*c6_2+.5*k21)*s5_2+.5*k10+ .5*k7*c6_2)*s4)*QD(3)+(-k7*c6_2+k21)*s5*c5*QD(4)+(-.5*k1+ .5*k7*s6_2-.5*k7*c6_2)*s5*QD(6) C(5,5)=k7*QD(6)*c6*s6 C(5,6)=((.5*k7*c6_2+.5*k1-.5*k7*s6_2)*s5*c23+((.5*k7*c6_2+.5*k1.5*k7*s6_2)*c5*c4-k7*c6*s4*s6)*s23)*QD(1)+(k7*c4*c6*s6+ (.5*k7*c6_2+.5*k1-.5*k7*s6_2)*c5*s4)*QD(2)+(k7*c4*c6*s6+ (.5*k7*c6_2+.5*k1-.5*k7*s6_2)*c5*s4)*QD(3)+(-.5*k1+.5*k7*s6_2.5*k7*c6_2)*s5*QD(4)+k7*QD(5)*c6*s6 C(6,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*k1+.5*k7*s6_2.5*k7*c6_2)*s5*c4+k7*c6*s5*c5*s4*s6)*c23+((k7*s6_2k7*c6_2)*c5*c4_2+(k7*c6*s6+k7*c5_2*c6*s6)*s4*c4+(.5*k7*c6_2.5*k1-.5*k7*s6_2)*c5)*s23)*QD(2)+(((-.5*k1+.5*k7*s6_2.5*k7*c6_2)*s5*c4+k7*c6*s5*c5*s4*s6)*c23+((k7*s6_2k7*c6_2)*c5*c4_2+(k7*c6*s6+k7*c5_2*c6*s6)*s4*c4+(.5*k7*c6_2-.5*k1.5*k7*s6_2)*c5)*s23)*QD(3)+((-k7*c6*s6+k7*c5_2*c6*s6)*c23+ (-k7*s5*c5*c4*c6*s6+(-.5*k7*c6_2+.5*k7*s6_2+ .5*k1)*s5*s4)*s23)*QD(4)+((-.5*k1+.5*k7*s6_2-.5*k7*c6_2)*s5*c23+ ((-.5*k1+.5*k7*s6_2-.5*k7*c6_2)*c5*c4+k7*c6*s4*s6)*s23)*QD(5) C(6,2)=(((-.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-.5*k1-.5*k7*s6_2)*c5)*s23)*QD(1)+((-k7*c6*s6k7*c5_2*c6*s6)*c4_2+(k7*s6_2-k7*c6_2)*c5*s4*c4+k7*c5_2*c6*s6)*QD(2)+ ((-k7*c6*s6-k7*c5_2*c6*s6)*c4_2+(k7*s6_2-k7*c6_2)*c5*s4*c4+ k7*c5_2*c6*s6)*QD(3)+((.5*k7*c6_2-.5*k1-.5*k7*s6_2)*s5*c4k7*c6*s5*c5*s4*s6)*QD(4)+(-k7*c4*c6*s6+(-.5*k1+.5*k7*s6_2.5*k7*c6_2)*c5*s4)*QD(5) C(6,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-.5*k1-.5*k7*s6_2)*c5)*s23)*QD(1)+((-k7*c6*s6k7*c5_2*c6*s6)*c4_2+(k7*s6_2-k7*c6_2)*c5*s4*c4+k7*c5_2*c6*s6)*QD(2)+ ((-k7*c6*s6-k7*c5_2*c6*s6)*c4_2+(k7*s6_2-k7*c6_2)*c5*s4*c4+ k7*c5_2*c6*s6)*QD(3)+((.5*k7*c6_2-.5*k1-.5*k7*s6_2)*s5*c4-
138
k7*c6*s5*c5*s4*s6)*QD(4)+(-k7*c4*c6*s6+(-.5*k1+.5*k7*s6_2.5*k7*c6_2)*c5*s4)*QD(5) C(6,4)=((-k7*c6*s6+k7*c5_2*c6*s6)*c23+(-k7*s5*c5*c4*c6*s6+(-.5*k7*c6_2+ .5*k7*s6_2+.5*k1)*s5*s4)*s23)*QD(1)+((.5*k7*c6_2-.5*k1.5*k7*s6_2)*s5*c4-k7*c6*s5*c5*s4*s6)*QD(2)+((.5*k7*c6_2-.5*k1.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*k7*c6_2-.5*k7*s6_2+.5*k1)*s5*QD(5) C(6,5)=((-.5*k1+.5*k7*s6_2-.5*k7*c6_2)*s5*c23+((-.5*k1+.5*k7*s6_2.5*k7*c6_2)*c5*c4+k7*c6*s4*s6)*s23)*QD(1)+(-k7*c4*c6*s6+(-.5*k1+ .5*k7*s6_2-.5*k7*c6_2)*c5*s4)*QD(2)+(-k7*c4*c6*s6+(-.5*k1+ .5*k7*s6_2-.5*k7*c6_2)*c5*s4)*QD(3)+(.5*k7*c6_2-.5*k7*s6_2+ .5*k1)*s5*QD(4)-k7*QD(5)*c6*s6 C(6,6)=0.0
/* Par total */ /*----------------------------------*/ for(i=0 i<6 { y i]=0.0 i++)
if (enable!=0.0) for(j=0 j<6 } }
j++)
y i]=y i]+C(i+1,j+1)*QD(j+1)
static void mdlTerminate(SimStruct *S) { } #ifdef MATLAB_MEX_FILE #include "simulink.c" #else #include "cg_sfun.h" #endif

/* * * * * * Generacion de trajectoria articular Robot RM-10 Polinomio de 5 grado con velocidad inicial y final nula, y aceleraciones final e inicial nulas. Autor: Carlos Perez Fernandez
139
* fecha: 30-4-99 */ #define S_FUNCTION_NAME traj52 #define S_FUNCTION_LEVEL 2 #include <math.h> #include "simstruc.h"
/* Asigna parametros al espacio de trabajo
*/
/*********************************************/ /* De 0 a 35 Matriz de coef de polinomios */ #define A(i,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,0) /* Velocidades m\'{a}ximas */ #define POS_INI(S) ssGetSFcnParam(S,1) /* Posicion inicial del robot*/ #define SAMPLE(S) ssGetSFcnParam(S,2) /* Frecuencia */ /* Asigna entradas */ /*----------------------------------*/ #define QF(indice) (*uPtrs 0] indice]) #define QACT(indice) (*uPtrs 1] indice]) #define MOVER (*uPtrs 2] 0])
/* Posici\'{o}n Final */ /* Posici\'{o}n Actual */ /* Orden de Movimiento */
static void mdlInitializeSizes(SimStruct *S)
140
{ ssSetNumContStates( ssSetNumDiscStates( S, S, 0) 0)

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

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

for(i=0 i<6 i++) { Estado(i)=0 for(j=0 j<6 j++) { Estado(i)+=A(i,j)*pow(Time,j) } y = ssGetOutputPortRealSignal(S,1) y i]=0 for(j=0 j<5 j++) { y i]+=(j+1)*A(i,j+1)*pow(Time,j) } y = ssGetOutputPortRealSignal(S,2) y i]=0 for(j=0 j<4 j++) { y i]+=(j+2)*(j+1)*A(i,j+2)*pow(Time,j) } } if (Time>=Tfinal) MOVING=0 } else { /* Si no hay movimiento velocidades nulas */
143
/*Aceleracion*/
y = ssGetOutputPortRealSignal(S,1) for(i=0 i<6 i++) { y i]=0 } y = ssGetOutputPortRealSignal(S,2) for(i=0 i<6 i++) { y i]=0 } } /* Asignamos las salidas del bloque */ y = ssGetOutputPortRealSignal(S,0) for(i=0 i<6 i++) { y i]=Estado(i) }
144
/* Indicaci\'{o}n fin de trayectoria */ y = ssGetOutputPortRealSignal(S,3) if (MOVING==1) *y=0.0 else *y=1
Moverant=MOVER }
/* * mdlTerminate - Se llama una vez finaliza la simulaci\'{o}n * */ static void mdlTerminate(SimStruct *S) { } #ifdef MATLAB_MEX_FILE #include "simulink.c" #else #include "cg_sfun.h" #endif /* Is this file being compiled as a MEX-file? */ /* MEX-file interface mechanism */ /* Code generation registration function */

/* * * * * * 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.h" #include "math.h" #include "stdlib.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,0)) 0] mxGetPr(ssGetSFcnParam(S,1)) 0] mxGetPr(ssGetSFcnParam(S,2)) 0] mxGetPr(ssGetSFcnParam(S,3)) 0] mxGetPr(ssGetSFcnParam(S,4)) 0] mxGetPr(ssGetSFcnParam(S,5)) 0] mxGetPr(ssGetSFcnParam(S,6)) 0] 3.14159265358979 (double) -2.3572 (double) +2.3572 (double) (double) (double) (double) (double) (double) (double) (double) (double) (double) 1e-8 */ -2.0944 +0.1745 -2.0944 +2.0944 -2.0944 +2.0944 -1.5708 +1.5708 -3.1416 +3.1416 /* -135 Grad */ /* +135 Grad */ /* /* /* /* /* /* /* /* /* /* -120 10 -120 +120 -120 +120 - 90 + 90 -180 +180 Grad Grad Grad Grad Grad Grad Grad Grad Grad Grad */ */ */ */ */ */ */ */ */ */
#define FLOAT_EPSILON /* Asigna entradas
146

/* 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, S, 0) /* N\'{u}mero de estados t continuo */ 0) /* N\'{u}mero de estados t discreto */
if (!ssSetNumInputPorts(S, 5)) return /*Entradas*/ ssSetInputPortWidth(S, 0, 3) /*Vector n*/ ssSetInputPortWidth(S, 1, 3) /*Vector s*/ ssSetInputPortWidth(S, 2, 3) ssSetInputPortWidth(S, 3, 3) ssSetInputPortWidth(S, 4, 6) /*Vector a*/ /*Posici\'{o}n xyz*/ /*Posici\'{o}n Actual*/
for(i=0 i<5 i++) ssSetInputPortDirectFeedThrough(S, i, 1)
if (!ssSetNumOutputPorts(S,3)) return /*Salidas*/ ssSetOutputPortWidth(S, 0, 6) /*Posici\'{o}n Soluci\'{o}n*/ ssSetOutputPortWidth(S, 1, 1) /*Error trayectoria*/ ssSetOutputPortWidth(S, 2, 1) /*debug*/ ssSetNumSFcnParams(S, 7) if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { return /* Si faltan parametros se da mensaje */ } ssSetNumSampleTimes( S, ssSetNumRWork( S, ssSetNumIWork( S, ssSetNumPWork( S, ssSetNumModes( S, ssSetNumNonsampledZCs(S, 1) 0) 0) 0) 0) 0) /* /* /* /* Numero Vector Vector Vector de de de de muestreos */ numeros reales */ numeros enteros */ punteros */
147
static void mdlInitializeSampleTimes(SimStruct *S) { ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME) ssSetOffsetTime(S, 0, 0.0) } #define MDL_INITIALIZE_CONDITIONS /* * mdlInitializeConditions - Inicializa estados y parametros del robot * */ static void mdlInitializeConditions(SimStruct *S) { } /* * mdlOutputs - Calcula las salidas * */ static void mdlOutputs(SimStruct *S, int_T tid) { int int real_T double double double double double double double double i, j, k minindex = 0 *output dtemp, dep1 4], A 4], den 4] p=1 minnorm = 0 c1 4],s1 4],c2 4],s2 4],c3 4],s3 4],c23 4],s23 4] c4nf 4],s4nf 4],c5nf 4],s5nf 4],c6nf 4],s6nf 4] x, y, z /* Posici\'{o}n 06 */ limit_sup 6]={LIM_ART_MAX_1, LIM_ART_MAX_2, LIM_ART_MAX_3, LIM_ART_MAX_4, LIM_ART_MAX_5, LIM_ART_MAX_6} limit_inf 6]={LIM_ART_MIN_1, LIM_ART_MIN_2, LIM_ART_MIN_3, LIM_ART_MIN_4, LIM_ART_MIN_5, LIM_ART_MIN_6}
double qSol 8] 7]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0} /* Matriz de soluciones */ /*Septima columna codigos de error*/ double qNorm 8]={-1,-1,-1,-1,-1,-1,-1,-1} InputRealPtrsType uPtrs 5] real_T *outputerror=ssGetOutputPortRealSignal(S,1)
148
real_T *debug=ssGetOutputPortRealSignal(S,2) for (i=0 i<5 i++) uPtrs i] = ssGetInputPortRealSignalPtrs(S,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) - atan2(RGEN_d3, sqrt(dtemp)) qSol 2] 0]=atan2(y,x) - atan2(RGEN_d3, -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

} else { qSol i] 2]=atan2(RGEN_a3,RGEN_d4) - atan2(A i], 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
150
} else {
qSol i] 3]=atan2(-a(0)*s1 i]+a(1)*c1 i], -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],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] } } }

for(i=0 i<8 i++) /* mantenemos \'{a}ngulos entre -PI y PI */ { for(j=1 j<6 j++) { if (qSol i] j]>PI) qSol i] j]=-1*(2*PI-qSol i] j]) if (qSol i] j]<-PI) qSol i] j]+=2*PI /* Comprobamos limites para eliminar soluciones */ if (qSol i] 6]==0)
151
{ if ((qSol i] j]<=limit_inf j])||(qSol i] j]>=limit_sup j])) qSol i] 6]=2 } } } /* Elegimos la soluci\'{o}n m\'{a}s proxima a la posici\'{o}n actual */ minnorm=-1 for(i=0 i<8 i++) { if(qSol i] 6]==0) { qNorm i]=0 for(j=0 j<6 j++) qNorm i]+=(pact(j)-qSol i] j])*(pact(j)-qSol i] j]) qNorm i]=sqrt(qNorm i]) if (minnorm==-1) { minnorm=qNorm i] minindex=i } else { if (qNorm i]<minnorm) { minnorm=qNorm i] minindex=i } } } }
152
/*Asignamos la salida*/ if (minnorm!=-1) { if (qNorm minindex]<=1) { 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 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) { }

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

Simulación y Control de Un Robot Manipulador

Diunggah oleh

Informasi Dokumen

Judul Asli

Hak Cipta

Format Tersedia

Bagikan dokumen Ini

Bagikan atau Tanam Dokumen

Opsi Berbagi

Apakah menurut Anda dokumen ini bermanfaat?

Apakah konten ini tidak pantas?

Hak Cipta:

Format Tersedia

Simulación y Control de Un Robot Manipulador

Diunggah oleh

Hak Cipta:

Format Tersedia

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

Simulacion y control de un robot manipulador.

2 Cinematica del robot RM-10

3 Dinamica del robot RM-10

4 Control de robot RM-10

C Esquemas electricos de electronica de potencia D Codigo fuente funciones Simulink

Cap tulo 1 Introduccion

1.2 Introduccion a la Robotica Industrial

1.2.2 Clasi cacion de los robots industriales

1.2 Introduccion a la Robotica Industrial

1.2.3 Estructura de un robot industrial

1.2 Introduccion a la Robotica Industrial

Figura 1.3: Control en un robot industrial

1.3 Descripcion del robot industrial RM-10

1.3 Descripcion del robot industrial RM-10

1234 1234 1234 1234

Figura 1.4: Vista general del robot industrial RM-10

1.3.1 Brazo manipulador

1.3 Descripcion del robot industrial RM-10

1.3.2 Armario de control

16 Electronica de potencia. Zona de entrada y salidas. Rack controlador. Servoampli cadores.

1.3 Descripcion del robot industrial RM-10

Zona de entrada y salidas

1.3 Descripcion del robot industrial RM-10

Posicin del rotor Resolver

1.4 Equipo para el desarrollo del proyecto

1.4.1 Equipo hardware

1.4 Equipo para el desarrollo del proyecto

{ 16 canales de entrada analogicas de 16 bits y 4 s. de tiempo de { { { { { {

1.4.2 Software de desarrollo

1.4.3 Interface con armario de control

BRAZO MANIPULADOR Aislamiento seales analgicas

Figura 1.7: Esquema de interface con el robot RM-10

Cap tulo 2 Cinematica del robot RM-10

2 Cinematica del robot RM-10

2.2 Matrices de transformacion

ci ;s i 0 ai; 6 s i c i; c ic i; ;s i; ;dis i; =6 s s 4 c is i; c i; dic i; i i; 0 0 0 1

2.2 Matrices de transformacion

Figura 2.2: Situacion de los sistemas de coordenadas

2 Cinematica del robot RM-10

ai; 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.1: Parametros cinematicos

2.3 Problema cinematico directo

2.3 Problema cinematico directo

2 Cinematica del robot RM-10

Del resultado de este producto se obtiene la matriz:

El juego de ecuaciones que se obtienen para la orientacion de la herramienta es:

2.3 Problema cinematico directo

(2.15) (2.16) (2.17) (2.18) (2.19) (2.20)

(2.21) (2.22) (2.23)

2 Cinematica del robot RM-10

2.4 Problema cinematico inverso

2.4.1 Resolucion anal tica de la cinematica inversa

2.4 Problema cinematico inverso

Tendremos que resolver:

Para resolver el angulo

se realiza un cambio de variables:

x = rcos( ) y = rsen( ) = atan2(y x) p r = x + y2

Sustituyendo 2.28 en la ecuacion 2.27 obtenemos:

2 Cinematica del robot RM-10

2.4 Problema cinematico inverso

c c x+s c y;s z;a c ;a c =a ;c s x ; s s y ; c z + a s + s a = d

s ; ( + ; + ; s = c x (+ a xs dy );xcxa )ys ; 2as)ya (;aa c + za ) zs y (2 2 c + + ( c + a ) xc ys a z c = c x +a(2 xs y ;(2 xa+ c ;; a ) + (s a ; d )+ s y ) 2 s ya + + z

De las que se puede resolver :