Simulación y Control de un robot manipulador

Departamento de Ingenier��a de Sistemas y Autom�atica

Escuela Superior de Ingenieros

Universidad de Sevilla

Simulaci�on y control de un robot

manipulador.

Autor: Carlos P�erez Fern�andez.

Tutor: Francisco Rodr��guez Rubio

Sevilla, Noviembre de 1999.

2

�Indice General

1 Introducci�on 7

1.1 Contenido y objetivos del proyecto . . . . . . . . . . . . . . . 7

1.2 Introducci�on a la Rob�otica Industrial . . . . . . . . . . . . . . 7

1.2.1 Evoluci�on hist�orica de la rob�otica industrial . . . . . . 7

1.2.2 Clasi�caci�on de los robots industriales . . . . . . . . . 8

1.2.3 Estructura de un robot industrial . . . . . . . . . . . . 9

1.3 Descripci�on del robot industrial RM-10 . . . . . . . . . . . . . 13

1.3.1 Brazo manipulador . . . . . . . . . . . . . . . . . . . . 14

1.3.2 Armario de control . . . . . . . . . . . . . . . . . . . . 15

1.4 Equipo para el desarrollo del proyecto . . . . . . . . . . . . . . 20

1.4.1 Equipo hardware . . . . . . . . . . . . . . . . . . . . . 20

1.4.2 Software de desarrollo . . . . . . . . . . . . . . . . . . 21

1.4.3 Interface con armario de control . . . . . . . . . . . . . 22

2 Cinem�atica del robot RM-10 23

2.1 Introducci�on . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

2.2 Matrices de transformaci�on . . . . . . . . . . . . . . . . . . . . 24

2.3 Problema cinem�atico directo . . . . . . . . . . . . . . . . . . . 27

2.4 Problema cinem�atico inverso . . . . . . . . . . . . . . . . . . . 30

2.4.1 Resoluci�on anal��tica de la cinem�atica inversa . . . . . . 30

2.4.2 Implementaci�on software . . . . . . . . . . . . . . . . . 36

2.5 Generaci�on de trayectorias . . . . . . . . . . . . . . . . . . . . 36

2.5.1 Trayectorias articulares . . . . . . . . . . . . . . . . . . 36

2.5.2 Trayectorias lineales . . . . . . . . . . . . . . . . . . . 38

3 Din�amica del robot RM-10 41

3.1 Introducci�on . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

3.2 Modelo Euler-Lagrange . . . . . . . . . . . . . . . . . . . . . . 42

3.3 Par�ametros del modelo del robot . . . . . . . . . . . . . . . . 50

3.3.1 Tensores de inercia y centros de gravedad . . . . . . . . 51

3.3.2 Caracter��sticas de los motores y reductoras . . . . . . . 51

3

4 �INDICE GENERAL

3.3.3 Par�ametros de fricci�on . . . . . . . . . . . . . . . . . . 54

3.4 Minimizaci�on de los parametros . . . . . . . . . . . . . . . . . 55

3.5 Implementaci�on inform�atica . . . . . . . . . . . . . . . . . . . 59

3.6 Simulaciones del modelo . . . . . . . . . . . . . . . . . . . . . 59

4 Control de robot RM-10 63

4.1 Introducci�on . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

4.2 Control lineal . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

4.2.1 Justi�cacion . . . . . . . . . . . . . . . . . . . . . . . . 63

4.2.2 Control tipo PD . . . . . . . . . . . . . . . . . . . . . . 64

4.2.3 Control tipo PID . . . . . . . . . . . . . . . . . . . . . 75

4.3 Control por Par Calculado . . . . . . . . . . . . . . . . . . . . 76

Ap�endices 91

A Tarjeta de E/S digital 91

B Tarjeta de conversi�on resolver-encoder 95

B.1 Filtros y Ajuste de fase . . . . . . . . . . . . . . . . . . . . . . 96

B.2 Conversi�on resolver a encoder . . . . . . . . . . . . . . . . . . 99

B.3 Ampli�cador adaptador . . . . . . . . . . . . . . . . . . . . . 99

B.4 Optoaislaci�on de se~nales digitales . . . . . . . . . . . . . . . . 100

B.5 Se~nales de control . . . . . . . . . . . . . . . . . . . . . . . . . 100

B.6 Esquemas el�ectricos . . . . . . . . . . . . . . . . . . . . . . . . 101

C Esquemas el�ectricos de electr�onica de potencia 103

D C�odigo fuente funciones Simulink 105

�Indice de Figuras

1.1 Estructura general de un robot industrial . . . . . . . . . . . . 10

1.2 Brazo mec�anico del robot industrial RM-10 con indicaci�on de

sus movimientos posibles . . . . . . . . . . . . . . . . . . . . . 11

1.3 Control en un robot industrial . . . . . . . . . . . . . . . . . . 12

1.4 Vista general del robot industrial RM-10 . . . . . . . . . . . . 14

1.5 Se~nales proporcionadas por los resolvers . . . . . . . . . . . . 15

1.6 Representaci�on esquem�atica de un servoampli�cador . . . . . . . . 19

1.7 Esquema de interface con el robot RM-10 . . . . . . . . . . . . 22

2.1 Convenio Denavit-Hartenberg . . . . . . . . . . . . . . . . . . 24

2.2 Situaci�on de los sistemas de coordenadas . . . . . . . . . . . . 25

2.3 Vectores de orientaci�on en la herramienta del manipulador . . 28

2.4 Soluciones para las tres �ultimas articulaciones . . . . . . . . . 35

2.5 Trayectorias articulares polin�omicas de 5 orden . . . . . . . . 37

2.6 Generaci�on de trayectorias lineales. . . . . . . . . . . . . . . . 38

3.1 Caracter��sticas de los elementos del brazo del manipulador . . 52

3.2 Caracter��stica est�atica de fricci�on te�orica . . . . . . . . . . . . 54

3.3 Caracter��stica de fricci�on experimental del eje 6. . . . . . . . . 55



3.6 Diagrama de bloques asociado al modelo . . . . . . . . . . . . 60

3.7 Diagrama de bloques Simulink para la simulaci�on del manip-

ulador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

3.8 Diagrama de bloques para la simulaci�on del modelo en bucle

abierto . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

3.9 Evoluci�on de las posiciones de las articulaciones 5 y 6 . . . . . 62

3.10 Evoluci�on de las velocidades de las articulaciones 5 y 6 . . . . 62

4.1 Esquema de control lineal tipo PD . . . . . . . . . . . . . . . 64

4.2 Diagrama Simulink para la simulaci�on de un controlador PD . 66

4.3 Seguimiento en posici�on para control tipo PD . . . . . . . . . 67

5

6 �INDICE DE FIGURAS

4.4 Error de seguimiento en posici�on para control tipo PD . . . . 68

4.5 Se~nales de control para regulador tipo PD . . . . . . . . . . . 69

4.6 Seguimiento en posici�on para control tipo PD en los ejes 4, 5

y 6 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

4.7 Velocidades para control tipo PD en los ejes 4, 5 y 6 . . . . . . 72

4.8 Error de seguimiento en posici�on para control tipo PD en los

ejes 4, 5 y 6 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73

4.9 Se~nal de control para control tipo PD en los ejes 4, 5 y 6 . . . 74

4.10 Diagrama Simulink para la simulaci�on de un controlador PID 76

4.11 Seguimiento en posici�on para control tipo PID . . . . . . . . . 77

4.12 Error de seguimiento en posici�on para control tipo PID . . . . 78

4.13 Se~nales de control para regulador tipo PID . . . . . . . . . . . 79

4.14 Bucle de linealizaci�on y desacoplo . . . . . . . . . . . . . . . . 80

4.15 Esquema completo de control por par calculado . . . . . . . . 81

4.16 Diagrama Simulink para la simulaci�on de un controlador Par

Calculado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82

4.17 Seguimiento en posici�on para control tipo Par Calculado . . . 83

4.18 Error de seguimiento en posici�on para control tipo Par Calculado 84

4.19 Se~nales de control para regulador tipo Par Calculado . . . . . 85

4.20 Seguimiento en posici�on para control tipo par calculado en los

ejes 4, 5 y 6 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87

4.21 Velocidades para control tipo par calculado en los ejes 4, 5 y 6 88

4.22 Error de seguimiento en posici�on para control tipo par calcu-

lado en los ejes 4, 5 y 6 . . . . . . . . . . . . . . . . . . . . . . 89

4.23 Se~nal de control para control tipo par calculado en los ejes 4,

5 y 6 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90

A.1 Etapa de entrada digital de la tarjeta E/S digitales . . . . . . 91

A.2 Etapa de salida digital de la tarjeta E/S digitales . . . . . . . 92

B.1 Diagrama de bloques de la tarjeta CRE . . . . . . . . . . . . . 95

B.2 Conectores presentes en la tarjeta CRE . . . . . . . . . . . . . 96

B.3 Esquema del �ltro activo para la se~nal de referencia . . . . . . 98

B.4 Funcionamiento interno del circuito integrado AD2S90 . . . . 99

Cap��tulo 1

Introducci�on

1.1 Contenido y objetivos del proyecto

En el presente proyecto se trata de sustituir el controlador del robot indus-

trial System-Robot RM-10, ubicado en los laboratorios del Departamento

de Ingenier��a de Sistemas y Autom�atica de la Universidad de Sevilla, por

un sistema de control basado en un ordenador PC an�tri�on con una tarjeta

controladora con m�ultiples entrada-salida.

Este sistema pretende sustituir los algoritmos de control de los motores

y la generaci�on de trayectorias del manipulador, conservando �unicamente la

electr�onica de potencia del manipulador y realizando la interfase el�ectrica

con las se~nales del manipulador.

1.2 Introducci�on a la Rob�otica Industrial

1.2.1 Evoluci�on hist�orica de la rob�otica industrial

La necesidad de aumentar la productividad y mejorar la calidad de los pro-

ductos, ha hecho insu�ciente la automatizaci�on industrial r��gida, dominante

en las primeras d�ecadas del siglo XX, que estaba destinada a la fabricaci�on

de grandes cantidades de una gama peque~na de productos. En la actualidad,

m�as de la mitad de los productos que se fabrican corresponden a lotes de

pocas unidades.

7

8 1 Introducci�on

Por esto, con el objetivo de dise~nar una maquina exible, adaptable al

entorno de trabajo, se patent�o en 1956 un manipulador programable que fue

la semilla para la rob�otica industrial.

En la historia de la Rob�otica Industrial se pueden distinguir varias etapas

seg�un el nivel de desarrollo:

1. En 1950 se dise~nan manipuladores amo-esclavo para manejar materia-

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~nos des-

pu�es se inicia la implantaci�on de robots industriales en Europa, funda-

mentalmente 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~no 1975 con la aplicaci�on de los microprocesadores se transfor-

man las caracter��sticas de los robots pasando a ser m�as compactos y

baratos. Desde este a~no hasta 1980 gracias a los avances en microelec-

tr�onica 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 inform�atica y el perfecionamiento de los

sensores permiten integrar cada vez m�as al robot en el entorno que le

rodea, naciendo as�� los robots inteligentes, capaces de tomar decisiones

adecuadas a cada situaci�on.

1.2.2 Clasi�caci�on de los robots industriales

La evoluci�on a dado origen a una serie de tipos de robots, como los siguientes:

1. Manipuladores, son sistemas mec�anicos 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 se-

cuencia.

1.2 Introducci�on a la Rob�otica Industrial 9

2. Robots de aprendizaje gestual: El operario ense~na al robot mediante

pistolas de programaci�on o bien moviendo el robot.

3. Robot programables textualmente: En este tipo de robots, se progra-

man o�-line mediante un lenguaje de programaci�on espec��co

4. Robots inteligentes: Son an�alogos a los anteriores, diferenciandose en

que poseen una mayor interacci�on con el entorno que lo rodea, mediante

el uso de sensores avanzados, visi�on 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 mec�anico o manipulador que dispone en su extremo de una he-

rramienta.

2. Controlador.

3. Sensores de posici�on y velocidad.

4. Actuadores.

5. Sensores avanzados, aunque estos �ultimos no siempre se encuentran

presentes.

Adem�as el robot industrial se puede encontrar dotado de toda una serie de

automatismos tales como cintas transportadoras, alimentadores, aut�omatas

programables etc. con los que normalmente es necesario coordinarse para rea-

lizar alguna tarea por lo que los robots industriales suelen incorporar alg�un

tipo de sistema de comunicaci�on, desde simples entradas y salidas digitales

hasta buses de campo.

Brazo mec�anico

El brazo mec�anico es el conjunto de elementos mec�anicos que permiten

el movimiento de la herramienta del robot. Normalmente est�a dividido en

partes r��gidas, denomin�andose �estas enlaces y articulaciones que permiten el

movimiento relativo entre ellas.

10 1 Introducci�on

SensoresPosición.Velocidad

Controlador

Actuadores

Sensores deinformación

Herramienta

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~neca, tal y como se ve en la �gura

1.2.

Existen distintos tipos de articulaciones, pero normalmente se suelen usar

de rotaci�on o prism�aticas, ambas de un grado de libertad.

Siendo todas las articulaciones de un grado de libertad, el n�umero de gra-

dos de libertas del robot ser�a igual al n�umero de articulaciones. Por lo general

los robots industriales suelen tener 6 grados de libertad, lo que permite usar

los tres primeros para �jar la posici�on y los otros tres para �jar la orientaci�on

en el espacio de la herramienta.

Controlador

El controlador del sistema se encarga de procesar toda la informaci�on que

llega de los sensores y del usuario mismo, generando las se~nales de control

adecuadas para realizar los movimientos deseados. El controlador suele estar

organizado de un modo jer�arquico, en el nivel m�as bajo se encuentran los

algoritmos de control de los actuadores, por ejemplo lazos de control PID.

1.2 Introducci�on a la Rob�otica Industrial 11

Brazo

Antebrazo

Muñeca

�1

�2

�3

�4

�5

�6

Figura 1.2: Brazo mec�anico del robot industrial RM-10 con indicaci�on de sus

movimientos posibles

En un nivel m�as alto se encuentra el generador de trayectorias, esta parte

se encarga de generar las referencias para cada eje adecuadas al control de

m�as 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.

Adem�as del control de trayectorias es posible encontrar a este nivel lazos

de control con lecturas de sensores m�as especializados, como por ejemplo

sistemas de visi�on o sensores de esfuerzos.

El controlador adem�as de realizar todas estas tareas es necesario que

supervise el funcionamiento del conjunto, actuando adecuadamente en el caso

que alg�un componente del sistema fallase.


ControlTrayectorias

ControlServos

q

_q

qref

_qref

Figura 1.3: Control en un robot industrial

Herramienta

En la mu~neca del manipulador se coloca una herramienta con la que el ma-

nipulador realiza las tareas que el usuario programe. Existe gran variedad de

herramientas, como por ejemplo garras de sujecci�on, taladros, m�aquinas de

soldar, etc...

Sensores

Para poder cerrar los bucles de control es preciso que por lo menos el robot

manipulador disponga de sensores de posici�on en cada una de sus articula-

ciones, adicionalmente tambi�en pueden llevar sensores de velocidad, si bien

esta se puede estimar a partir de la medida de la posici�on.

Los sensores de posici�on que aparecen en los robots industriales son:

� Potenciometros.

� Encoders.

� Resolvers.

� Inductosyn.

Para medir la velocidad se pueden usar dinamos tacom�etricas o bien sensores

inductivos.

En robots industriales cuyos ejes son de rotaci�on los sensores de posici�on

m�as usados son los encoders, seguido de los resolvers. La ventaja de los

1.3 Descripci�on del robot industrial RM-10 13

encoders frente a los resolvers son que sus se~nales son digitales haci�endolos

m�as robusto ante entornos de ruidos, sin embargo los resolver son capaces de

dar medidas m�as precisas que los encoders.

Actuadores

Los actuadores en un robot industrial pueden ser, en funci�on de la energ��a

que usen:

� Neum�aticos.

� Hidr�aulicos

� El�ectricos.

Estos actuadores pueden actuar a trav�es de reductoras y correas de trans-

misi�on 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 actua-

dores el�ectricos ya que son los m�as adecuados para el control, los motores

el�ectricos m�as com�unmente usados son:

� Motores paso a paso.

� Motores de corriente continua con o sin escobillas

� Motores de corriente alterna.

Los m�as usados son los motores de corriente continua convencionales,

aunque cada vez m�as se usan los motores sin escobillas. En estos la con-

mutaci�on de la corriente se produce en una electr�onica externa bas�andose en

la medida de la posici�on del rotor del motor.

1.3 Descripci�on del robot industrial RM-10

El robot industrial RM-10 est�a compuesto de un brazo manipulador, un ar-

mario de control y una pistola de programaci�on. �Esta ofrece la �unica interfaz

de usuario para poder hacer uso manual del robot, as�� como para su progra-

maci�on.


121212

12341234

12341234

ARMARIO

PISTOLA

BRAZOMANIPULADOR

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 rotaci�on, lo cual le con�ere la posibilidad de orientar la herra-

mienta en cualquier posici�on, siempre que no se viole ning�un l��mite angular

de alguna articulaci�on.

Los ejes de las articulaciones est�an todos acoplados a los motores me-

diante reductoras, estando colocadas en el mismo eje de giro, excepto las

articulaciones de la mu~neca 5 y 6 que precisan de correas de transmisi�on,

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, m�as conocido

como tecnolog��a brushless, con una tensi�on continua de alimentaci�on de 310V.

El devanado estat�orico de los motores es trif�asico y el rotor est�a construido

con imanes permanentes de alta densidad. En el est�ator de los motores se

dispone de una protecci�on t�ermica 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 est�ator.

Los motores incorporan un freno el�ectrico que se libera aliment�andolo con

una tensi�on continua de 24V., esto permite bloquear el brazo en cualquier

posici�on.


0 0.5 1 1.5 2 2.5 3 3.5 4 4.5

x 10−3

−10

−5

0

5

10

Ref

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5

x 10−3

−5

0

5

C

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5

x 10−3

−5

0

5

S

Figura 1.5: Se~nales proporcionadas por los resolvers

Cada motor dispone de un resolver que permite realimentar la posici�on

no solo al controlador del robot, sino tambi�en al mismo servoampli�cador

ya que es necesaria la posici�on para poder conmutar de forma electr�onica

la tensi�on continua del motor entre sus distintas fases. Las caracter��sticas

el�ectricas m�as importantes de los resolvers son:

� Frecuencia portadora: 3500 Hz.

� Tensi�on referencia: 10Vpp

� Relaci�on de transformaci�on: 0,5

� Corriente m�axima de entrada: 10 mA.

La se~nal de referencia es proporcionada a cada resolver por su correspon-

diente ampli�cador de potencia. Las formas de estas se~nales son idealmente

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:


� Electr�onica de potencia.

� Zona de entrada y salidas.

� Rack controlador.

� Servoampli�cadores.

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 cen-

tralizada en una �unica tarjeta procesadora basada en un procesador Motorola

68020 a 25 Mhz.

El procesador se encuentra conectado con los servoampli�cadores a trav�es

de un bus interno por el cual se transmiten las se~nales de control y el proce-

sador obtiene informaci�on acerca del estado de los distintos servoampli�-

cadores pudiendo actuar en consecuencia.

Adem�as el rack controlador posee tarjetas con interfases a distintos

perif�ericos:

� Puertos serie, uno de ellos usado para la pistola de manejo y progra-

maci�on.

� Dispositivos de almacenamiento.

� Tarjetas de entrada y salida digital.

Electr�onica de potencia

En la parte delantera inferior del armario de control se encuentran los dis-

positivos de potencia del robot.

La energ��a el�ectrica es suministrada al armario controlador en forma

trif�asica con una tensi�on nominal de 380V. �Esta tensi�on se reduce mediante

un autotransformador trif�asico a 220V.


En esta zona se encuentran distintas fuentes de alimentaci�on que con-

vierten la tensi�on alterna en distintos niveles de tensi�on continua necesarias

para los distintos dispositivos.

En concreto existen las siguientes fuentes:

� Fuente AL1 +24V con salida regulada. Se usa para la alimentaci�on de

rel�es auxiliares y tarjetas de entradas y salidas digitales.

� Fuente AL2. Proporciona +5V para la alimentaci�on de las distintas tar-

jetas del rack VME. Tambi�en proporciona una alimentaci�on sim�etrica

de �12V necesaria para el funcionamiento de las se~nales de los ser-

voampli�cadores.

� Fuente no regulada formada por el transformador TR3 y el puente de

diodos PD. Proporciona una tensi�on 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 tensi�on a la

fuente de alimentaci�on de los servomotores. Este contactor se cierra

cuando el usuario pulsa el bot�on START del panel frontal del armario.

Un contacto auxiliar de este rel�e 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 rel�e 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 inter�es especial de cara

a integrar la nueva tarjeta controladora:

� U1 - Se encarga, a trav�es del rel�e auxiliar AUX1, de habilitar los ser-

voampli�cadores de modo que �estos puedan recibir consignas.


� U2 - Su funci�on es la de a trav�es del rel�e auxiliar AUX2, excitar la

bobina del contactor CF (contactor de frenos).

� CR5 - �Esta salida activa la entrada de limitaci�on de par o velocidad de

los servoampli�cadores.

Otras salidas se encargan de actuar sobres las servov�alvulas de aire compri-

mido para la herramienta del manipulador, as�� como para activar y desactivar

indicadores luminosos.

Las salidas U1 y U2 est�an condicionadas a que no este activado ning�un

dispositivo hardware de emergencia, �estos son:

� Pulsador de emergencia del panel frontal del armario.

� Pulsador de emergencia de la pistola de programaci�on.

� Finales de carrera presentes en todas las articulaciones del robot ex-

cepto en la articulaci�on 6.

El armario dispone tambi�en de 48 entradas digitales de las cuales 15 se

encuentran ocupadas por el propio controlador quedando el resto libres para

el usuario.

B�asicamente las entradas que usa el controlador tienen la funci�on de

captar pulsaciones en el panel frontal del armario o bien comunicar el fun-

cionamiento an�omalo de alg�un equipo.

Tienen inter�es 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 liber�andolos.

� IN8 - Se activa cuando la fuente de alimentaci�on de los servoampli�-

cadores se sobrecalienta.

� IN9 - Se activa cuando la resistencia de disipaci�on de la fuente de

alimentaci�on se funde.

� IN10 - Esta se~nal se activa cuando alguna de las tres fases de ali-

mentaci�on trif�asica de la fuente de alimentaci�on se pierde. Por lo tanto

mientras no se cierre el contactor principal mediante la pulsaci�on de

START en el panel frontal del armario esta se~nal se mantiene activada.


Servoampli�cadores

El servoampli�cador del motor brushless proporciona la conmutaci�on de la

corriente del motor de manera electr�onica manteniendo un �angulo de par de

90 de modo que el motor da un par m�aximo para una corriente dada en cada

momento. La conmutaci�on se realiza en funci�on de la posici�on del rotor que

es realimentada por el resolver.

La alimentaci�on de potencia de los servoampli�cadores son un bus de

corriente continua de 300V que proporciona la fuente de alimentaci�on.

Conmutadorelectrónico

N S

Posición del rotor Resolver

Servomotor BrushlessPuente de transistores de potencia

+

-

A

B BA

/C

/B

/A

C

/C/B/A

C

Figura 1.6: Representaci�on esquem�atica 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 corrien-

te mediante sensores de efecto Hall. Para modi�car la corriente se regula la

tensi�on en bornas del motor mediante PWM. En este modo la consigna se

traduce en el porcentaje de la corriente m�axima admisible que se hace cir-

cular por el est�ator del motor. La corriente m�axima para cada motor viene

determinada por un m�odulo de optimizaci�on 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 m�axima de la con�gurada para ese motor. Dicha velocidad m�axima

se con�gura de entre un rango de valores discretos y se a�na con un poten-

ci�ometro de escalado.


El bucle de velocidad se cierra con un controlador tipo PI o P cuyas

ganancias se ajustan mediante potenci�ometros 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 Autom�atica de la Universidad de

Sevilla.

Adicionalmente se instal�o m�as hardware ya sea comercial o desarrollado

a medida para el presente proyecto.

1.4.1 Equipo hardware

Adem�as 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 orde-

nador personal y tiene como caracter��sticas m�as 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 fun-

ciones 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 si-

guiente forma:

� Procesador Motorola PowerPC 604e:

{ 16 canales de entrada anal�ogicas de 16 bits y 4�s. de tiempo de

conversi�on.

{ 4 canales de entrada anal�ogicas de 12 bits y 0.8�s.

{ 8 canales de salidas anal�ogicas de 14 bits y 6�s.

{ 32 se~nales digitales con�gurables como entrada o salida.

{ 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 anal�ogicas de 10 bits y 6�s.

{ Entrada de fuentes de interrupciones y relojes externos.

{ Salida para generaci�on 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~nales digitales se puede encontrar

m�as informaci�on en los ap�endices B y A respectivamente.

1.4.2 Software de desarrollo

Para el dise~no y simulaci�on de controladores se usa como herramienta MAT-

LAB v5.2 y la ToolBox de simulaci�on 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 implementaci�on de los algoritmos de control en la tarjeta contro-

ladora, se usa la ToolBox Real Time WorkShop junto con la librer��a Real


Time Interface Library proporcionada por la �rma dSPACE junto con la

tarjeta.

La tarjeta controladora viene acompa~nada con software para el intercam-

bio de datos en tiempo real entre el programa que se ejecutan en la tarjeta

controladora y el PC an�tri�on. En particular los programas usados son:

� Cockpit 1103 con el que se dise~nan interfaces de usuario con las que es

posible cambiar en tiempo real par�ametros del controlador.

� Trace 1103 con el que se capturan en el PC an�tri�on la evoluci�on

temporal de variables del programa que se ejecuta en la tarjeta.

1.4.3 Interface con armario de control

BRAZOMANIPULADOR

Señales Digitales

Aislamiento señalesanalógicas

ConversiónResolver a Encoder

Electrónica depotencia

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

Cap��tulo 2

Cinem�atica del robot RM-10

2.1 Introducci�on

El estudio de la cinem�atica es fundamental para el desarrollo del sistema de

control de un robot. En este cap��tulo se realizar�a un modelo cinem�atico del

robot, que permitir�a relacionar el espacio de las variables articulares con el

espacio cartesiano, as�� como la plani�caci�on de trayectorias del robot.

La notaci�on utilizada para el estudio de la cinem�atica 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�1 �angulo entre los ejes zi�1 y zi�1 medido sobre el

eje xi�1 .

� Longitud de enlace ai�1 Distancia entre los ejes zi y zi�1 medida sobre

el eje xi�1.

� �angulo de la articulaci�on �i �angulo entre los ejes xi�1 y xi medido sobre

zi.

� Distancia de enlace di Distancia entre los ejes xi�1 y xi medida sobre

el eje zi.

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 par�ametros

23

24 2 Cinem�atica del robot RM-10

�i

�i�1

ai�1

ai�1

di

i� 1

i

Yi

Xi

Zi

Yi�1

Zi�1

Zi�1

Figura 2.1: Convenio Denavit-Hartenberg

de�nidos anteriormente se pueden construir matrices de transformaci�on que

relacionan la orientaci�on y posici�on de dos sistemas de coordenadas contiguos.

2.2 Matrices de transformaci�on

Previamente a la obtenci�on de matrices de transformaci�on hay que situar

f��sicamente los sistemas de coordenadas en el robot, y as�� de esta manera

crear una matriz con los par�ametros cinem�aticos. La �gura 2.2 muestra la

colocaci�on de los distintos sistemas de coordenadas, a partir de dicha �gura

se obtiene la tabla 2.2.

Seg�un el convenio adoptado en la �gura 2.1 la matriz de transformaci�on

de la articulaci�on i a la articulaci�on i� 1 vendr�a dada por la expresi�on 2.1

T i�1i =

2664

c�i �s�i 0 ai�1s�ic�i�1 c�ic�i�1 �s�i�1 �dis�i�1s�is�i�1 c�is�i�1 c�i�1 dic�i�1

0 0 0 1

3775 (2.1)

2.2 Matrices de transformaci�on 25

Z6 Z5

ZG

Z4

Z3

Z

Z

1

2

ZB

Y

Y

Y

Y

Y

Y

Y

Y

X

X

X

X

X

X

X

X

BB

B

1

1

2

2

3

3

5

5

6

6

4

4

G

G

d

a

a3

2

a4

d3

d

dG

4

Figura 2.2: Situaci�on de los sistemas de coordenadas


�i�1 ai�1 �i di

1 0 0 �1 0

2 -90 a1 �2 0

3 0 a2 �3 d3

4 -90 a3 �4 d4

5 90 0 �5 0

6 -90 0 �6 0

Tabla 2.1: Par�ametros cinem�aticos

Usando la matriz 2.1 y la tabla de par�ametros cinem�aticos 2.2 se obtienen

las seis matrices de transformaci�on del robot:

T 01 =

2664c1 �s1 0 0

s1 c1 0 0

1 0 1 0

0 0 0 1

3775 (2.2)

T 12 =

2664

c2 �s2 0 a10 0 1 0

�s2 �c2 0 0

0 0 0 1

3775 (2.3)

T 23 =

2664c3 �s3 0 a2s3 c3 0 0

1 0 1 d30 0 0 1

3775 (2.4)

T 34 =

2664

c4 �s4 0 a30 0 1 d4�s4 �c4 0 0

0 0 0 1

3775 (2.5)

2.3 Problema cinem�atico directo 27

T 45 =

2664c5 �s5 0 0

0 0 �1 0

s5 c5 0 0

0 0 0 1

3775 (2.6)

T 56 =

2664

c6 �s6 0 0

0 0 1 0

�s6 �c6 0 0

0 0 0 1

3775 (2.7)

Si queremos usar como origen de posiciones el sistema de coordenadas B

necesitamos la matriz de transformaci�on del sistema 0 al sistema B:

T 56 =

2664

1 0 0 0

0 1 0 0

0 0 1 dB0 0 0 1

3775 (2.8)

Normalmente el punto que se desea posicionar el extremo �nal de la he-

rramienta del robot con lo que tambi�en necesitaremos una matriz de trans-

formaci�on entre el sistema 6 y el sistema de coordenadas situado en la garra:

T 56 =

2664

1 0 0 0

0 1 0 0

0 0 1 dG0 0 0 1

3775 (2.9)

2.3 Problema cinem�atico directo

Las ecuaciones del problema cinem�atico directo ligan la posici�on y orientaci�on

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 posici�on viene dada por la situaci�on del sistema de coor-

denadas solidario a la garra (sistema de referencia G). Para la orientaci�on se


a

s

n

Figura 2.3: Vectores de orientaci�on en la herramienta del manipulador

utiliza la terna de vectores ~n;~s;~a cuya orientaci�on en el espacio puede verse

en la �gura 2.3.

Las relaciones matem�aticas se obtienen f�acilmente multiplicado ordenada-

mente las matrices de transformaci�on desde la base hasta la herramienta de

la forma:

TBG = TB

0 T01 T

12 T

23 T

34 T

45 T

56 T

6G (2.10)

Del resultado de este producto se obtiene la matriz:

TBG =

2664nx sx ax xGny sy ay yGnz sz az zG0 0 0 1

3775 (2.11)

El juego de ecuaciones que se obtienen para la orientaci�on de la herra-

mienta es:

nx = c1c6(c23c4c5 � s23s5) + c1c23s4s6 + s1s4c5c6 + s1c4s6 (2.12)

2.3 Problema cinem�atico directo 29

ny = s1c6(c23c4c5 � s23s5) + s1c23s4s6 � c1s4c5c6 + c1c4s6 (2.13)

nz = �c6(s23c4c5 � c23s5) + s23s4s6 (2.14)

sx = c1s6(s23s5 � c23c4c5)� c1c23s4c6 � s1s4c5s6 + s1c4c6 (2.15)

sy = s1s6(s23s5 � c23c4c5)� s1c23s4c6 + c1s4c5s6 + c1c4c6 (2.16)

sz = s6(c23s5 + s23c4c5) + s23s4c6 (2.17)

ax = �c1(c23c4s5 + s23c5)� s1s4s5 (2.18)

ay = �s1(c23c4s5 + s23c5)� c1s4s5 (2.19)

az = s23c4s5 � c23c5 (2.20)

Y para la posici�on:

xG = a1c1 + a2c1c2 + a3c1c23 � d3s1 � d4c1s23 + dGax (2.21)

yG = a1s1 + a2s1c2 + a3s1c23 + d3c1 � d4s1s23 + dGay (2.22)

zG = �a2s2 � a3s23 � d4c23 + dGaz + dB (2.23)

La implementaci�on software de estas ecuaciones se realiza en la funci�on

pcd.c en forma de bloque para su uso en diagrama Simulink. La funci�on

recibe como entrada un vector de variables articulares y realiza los c�alculos

necesarios para devolver la posici�on cartesiana y los vectores de orientaci�on

de la herramienta.


2.4 Problema cinem�atico inverso

En muchas ocasiones interesar�a dar consignas al robot en el espacio carte-

siano, esto implica que debamos conocer dado una posici�on y orientaci�on de

la herramienta del manipulador, el conjunto de variables articulares que la

satisfacen. As�� mismo, es necesario resolver este problema para la generaci�on

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 ~n,~s

y ~a no son independientes y adem�as son unitarios. El problema cinem�atico

inverso en general no tiene una soluci�on �unica, si bien existen soluciones que

se descartan debido a las limitaciones que impone el espacio de trabajo del

manipulador.

Existen distintos m�etodos de resolver la ecuaciones:

� Geom�etricos, en los que se intenta buscar una soluci�on anal��tica basada

en la geometr��a del robot aplicando relaciones trigonom�etricas.

� Aritm�eticos, buscan una soluci�on anal��tica manipulado las ecuaciones

usando las matrices de transformaci�on.

� Num�ericos, se usa alg�un m�etodo de resoluci�on num�erica si bien este

m�etodo es el mas costoso en tiempo de c�alculo para una computadora.

A continuaci�on se expone una soluci�on anal��tica usando m�etodos aritm�e-

ticos.

2.4.1 Resoluci�on anal��tica de la cinem�atica inversa

Partimos de una matriz 2.11, o sea de una orientaci�on y una posici�on de

la herramienta del manipulador. A partir de la posici�on podemos obtener la

posici�on de la intersecci�on de los ejes 4, 5 y 6 respecto al sistema de referencia

0, dejando solamente las matrices de transformaci�on que dependen de un

grado de libertad:

2.4 Problema cinem�atico inverso 31

T 06 =

2664nx sx ax x

ny sy ay y

nz sz az z

0 0 0 1

3775 = (TB

0 )�1TB

G (TG6 )

�1 (2.24)

Tendremos que resolver:

T 06 =

2664nx sx ax x

ny sy ay y

nz sz az z

0 0 0 1

3775 = T 0

1 T12 T

23 T

34 T

45 T

56 (2.25)

Realizamos la operaci�on:

(T 01 )

�1T 06 =

2664

c1 s1 0 0

�s1 c1 0 0

0 0 1 0

0 0 0 1

3775

2664nx sx ax x

ny sy ay y

nz sz az z

0 0 0 1

3775 = T 1

6 (2.26)

A partir del elemento (2,4) del sistema de ecuaciones 2.26 tenemos la

ecuaci�on:

�s1x+ c1y = d3 (2.27)

Para resolver el �angulo �1 se realiza un cambio de variables:

x = rcos(�)

y = rsen(�) (2.28)

� = atan2(y; x)

r =px2 + y2 (2.29)

Sustituyendo 2.28 en la ecuaci�on 2.27 obtenemos:


�rs1c� + rc1s� = rsen(�� 1) = d3 (2.30)

Con lo que:

�� 1 = atan2(d3

r;�r1� d23

r2) (2.31)

Si deshacemos el cambio de variable con las expresiones 2.29 podemos

obtener una expresi�on para �1:

�1 = atan2(y; x)� atan2(d3;�qx2 + y2 � d3

2) (2.32)

Debido a la ra��z cuadrada tendremos dos soluciones para la articulaci�on

�1. Del bloque de ecuaciones de 2.26 tambi�en podemos resolver la articulaci�on

3, las ecuaciones de los elementos (1,4) y (2,4) son:

c1x + s1y = c2a2 + a1 + c23a3 � s23d4

z = �s2a2 � c23d4 � s23a3 (2.33)

Elevando ambas expresiones al cuadrado y sum�andolas obtenemos tras

hacer simpli�caciones:

(c1x + s1y � a1)2+ z2 � a2

2 � d42 � a3

2

2 a2= c3a3 � s3d4 = A (2.34)

Esta ecuaci�on tiene la misma forma que 2.27 por lo que la soluci�on ser�a:

�3 = atan2(a3; d4)� atan2(A;�pd4

2 + a32 + A2) (2.35)

En este caso se tienen 4 soluciones para �3, ya que �1 tiene 2 soluciones y la

soluci�on de �3 depende de la soluci�on de A. Para resolver �2 premultiplicamos

la expresi�on 2.26 por las inversas de T 12 y T 2

3 :


2664

c1c23 s1c23 �s23 �a1c23 � a2c3�c1s23 �s1s23 �c23 a1s23 + s3a2�s1 c1 0 �d30 0 0 1

3775

2664nx sx ax x

ny sy ay y

nz sz az z

0 0 0 1

3775 = T 3

6 (2.36)

De este conjunto de ecuaciones nos quedamos con las que forman los

elementos (1,4) y (2,4):

c1c23x+ s1c23y � s23z � a1c23 � a2c3 = a3

�c1s23x� s1s23y � c23z + a1s23 + s3a2 = d4 (2.37)

Las expresiones 2.37 forman un sistema de ecuaciones en el que tenemos

como inc�ognitas �unicamente a c23 y s23, de cuya resoluci�on se obtiene:

s23 =(s3a2 � d4) (xc1 + ys1 � a1) + (�a2c3 � a3) z

c12x2 + (2 xs1y � 2 xa1) c1 � 2 s1ya1 + a12 + z2 + s12y2

c23 =(a2c3 + a3) (xc1 + ys1 � a1) + (s3a2 � d4) z

c12x2 + (2 xs1y � 2 xa1) c1 � 2 s1ya1 + a12 + z2 + s12y2(2.38)

Con las que obtenemos la suma de los �angulos �2 y �3, y conocido �3obtenemos 4 soluciones posibles para el �angulo �2:

�2 = atan2(s23; c23)� �3 (2.39)

Del bloque de ecuaciones 2.36 tambi�en podemos extraer las ecuaciones

de los elementos (1,3) y (2,3):

c1c23ax + s1c23ay � s23az = �c4s5�s1ax + c1ay = s4s5 (2.40)

De las que se puede resolver �4:


�4 = atan2(�s1ax + c1ay;�c1c23ax � s1c23ay + s23az) (2.41)

En el caso de que �5 se anulase no podr��amos resolver �4 seg�un la ecuaci�on

anterior, debido a que el manipulador se encuentra en una con�guraci�on

singular en la que los ejes 4 y 6 est�an alineados, por tanto, para conseguir la

orientaci�on deseada de los vectores ~n y ~s, nos sobra un grado de libertad. Para

evitar problemas, cuando el �5 se acerque a cero se puede dejar �4 constante y

orientar la herramienta con �6. Esto se puede hacer usando la ecuaci�on (3,3),

supervisando el valor del coseno de �5 se acerca a la unidad:

c5 = �c1s23ax � s1s23ay � c23az (2.42)

Para resolver �5 premultiplicamos la ecuaci�on 2.36 por la inversa de T 34

de la forma:

(T 04 )

�1

2664nx sx ax x

ny sy ay y

nz sz az z

0 0 0 1

3775 = T 4

6 (2.43)

De este conjunto de ecuaciones se puede extraer los elementos (1,3) y

(3,3):

s5 = � (c1c23c4 + s1s4) ax � (c23s1c4 � c1s4) ay + c4s23az

c5 = �c1s23ax � s1s23ay � c23az (2.44)

Con lo que se puede resolver el �angulo �5 como:

�5 = atan2(s5; c5) (2.45)

Se procede de forma an�aloga para el �angulo �6:

(T 05 )

�1

2664nx sx ax x

ny sy ay y

nz sz az z

0 0 0 1

3775 = T 5

6 (2.46)


En los elementos (1,1) y (3,1) aparecen las ecuaciones:

c6 = � (c1c23s4 � s1c4) sx � (c23s4s1 + c1c4) sy + s4s23sz

s6 = � (c1c23s4 � s1c4)nx � (c23s4s1 + c1c4)ny + s4s23nz (2.47)

Con lo que se puede resolver el �angulo �6 como:

�6 = atan2(s6; c6) (2.48)

Para las tres �ultimas articulaciones existe una segunda soluci�on que se

calcula como:

�04 = �4 � �

�05 = ��5�06 = �6 � � (2.49)

Con lo que en total obtenemos 8 soluciones para el problema cinem�atico

inverso.

sol2

X4

X4

Y4

Y4

Z4

Z4

X5

X5

Y5

Y5

Z5

Z5

X6

X6

Y6

Y6

Z6

Z6

~n

~n

~s

~s

~a

~a a==a

Figura 2.4: Soluciones para las tres �ultimas articulaciones


2.4.2 Implementaci�on software

El problema cinem�atico inverso se ha inplementado como una funci�on S (la

funci�on cineinv.c) para diagrama de bloques de Simulink. La funci�on recibe

como entradas:

1. Vector posici�on cartesiana de la herramienta ~P .

2. Vectores de orientaci�on de la herramienta ~n,~s y ~a.

3. Vector de variables articulares correspondientes a la posici�on actual del

robot.

Y se tienen como salidas:

1. Vector de variables articulares soluci�on

2. Se~nal de error en caso de que no haya soluci�on en el espacio de trabajo

del robot.

Adem�as recibe como par�ametros las distancias que de�nen la cinem�atica

del manipulador. La funci�on 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 m�as cercana a la posici�on actual de

robot.

2.5 Generaci�on de trayectorias

Para realizar un movimiento ordenado de las articulaciones se ha implementa-

do la posibilidad de generar trayectoria articulares y en el espacio cartesiano.

Las trayectorias son generadas en tiempo real a partir de la posici�on actual

del manipulador.

2.5.1 Trayectorias articulares

Este tipo de trayectorias es el m�as usado normalmente cuando no es nece-

sario especi�car con presici�on el camino y orientaci�on que debe seguir la

herramienta del manipulador. Se consigue que el movimiento de todas las

2.5 Generaci�on de trayectorias 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

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20

0.5

1

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20

0.5

1

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−2

−1

0

1

2

t

�

�0

�00

Figura 2.5: Trayectorias articulares polin�omicas 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 + a3t3 + a4t

4 + a5t5

�0(t) = 3a3t2 + 4a4t

3 + 5a5t4

�00(t) = 6a3t+ 12a4t2 + 20a5t

3 (2.50)

Los coe�cientes de los polinomios son calculados en funci�on de las condi-

ciones de contorno, obteniendose:

a3 =10

T 3(�f � �i)

a4 = � 15

T 4(�f � �i)

a5 =6

T 5(�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 m�axima alcanzada se produce en t = T=2 y toma el valor:

�0max =15

8

�f � �i

T(2.52)


3

qd

2

error

1

q

Qf

Mover

xyz−rpy act

p

Traylin

pcdcar

eul2tr

n

s

a

p

pact

q

Error

PCI

du/dt

m

Enable

3

Qact

2

Pfinal

1

Move

Figura 2.6: Generaci�on de trayectorias lineales.

La generaci�on de este tipo de trayectorias est�a implementada en la funci�on

Simulink, traj52.c. La funci�on recibe como entradas la posici�on deseada, la

posici�on actual del manipulador y la se~nal digital que activa la generaci�on

de la trayectoria a cada anco. El bloque genera una secuencia de posiciones

y velocidades de referencia. La funci�on recibe como par�ametro un vector de

velocidades articulares m�aximas, de esta manera la funci�on puede calcular el

tiempo necesario que tiene que durar la generaci�on de la trayectoria sin que

se viole ninguna de estas velocidades.

2.5.2 Trayectorias lineales

Para hacer tareas que requieran cierta precisi�on es preciso que los manipu-

ladores sean capaces de realizar trayectorias lineales.

Esta tarea es realizada por un subbloque Simulink, que a su vez se de-

scompone en distintas tareas espec��cas.

En primer lugar se dispone de el bloque trajlin52.cque se encarga de re-

alizar la trayectoria lineal en t�erminos de las variables x; y; z; roll; pitch; yaw

referidos a los ejes inerciales situados en la base del manipulador.

La posici�on inicial del espacio cartesiano se obtiene mediante el bloque

2.5 Generaci�on de trayectorias 39

pcdcar.c realizando los c�alculos del problema cinem�atico directo, o sea a partir

de las ecuaciones 2.21 a 2.23. Adem�as este bloque obtiene la orientaci�on de

la herramienta del manipulador en t�erminos de los �angulos roll, pitch, yaw a

partir de la parte rotacional de la matriz de transformaci�on obtenida con las

ecuaciones 2.12 a 2.20.

La relaci�on entre la parte rotacional de la matriz de transformaci�on y los

�angulos r; p y y viene dada de la forma:

24 cycp cyspsr � sycr cyspcr + sysrsycp syspsr + cycr syspcr � cysr�sp cpsr cpcr

35 (2.53)

A partir de esta matriz se pueden deducir los �angulos de roll, pitch y yaw.

Si c� 6= 0:

= atan2(r32; r33)

� = atan2(�r31;qr211 + r221)

� = atan2(r21; r11) (2.54)

En el caso de c� = 0, habr�a que �jar � y calcular de la forma:

� = 90 � = 0 = atan2(r12; r22)

� = �90 � = 0 = �atan2(r12; r22) (2.55)

Las consignas en el espacio cartesiano generadas son enviadas al bloque

cineinv.c que se encarga de resolver la cinem�atica inversa del manipulador

seg�un el procedimiento descrito en el punto 2.4.1. proporcionando referencias

en el espacio articular al controlador.

Este bloque genera una se~nal de error en el caso de que no encontrase

para alg�un punto de la trayectoria en el espacio cartesiano, una soluci�on

del problema cinem�atico inverso. En este caso el sistema genera una alarma

bloqueando el manipulador.


Cap��tulo 3

Din�amica del robot RM-10

3.1 Introducci�on

Antes de abordar el problema de control del manipulador es necesario obtener

un modelo din�amico. El modelo din�amico no solo es �util para poder simular

el comportamiento del sistema, tambi�en lo es para comprender el problema

de control de los manipuladores.

En general se pueden generar soluciones num�ericas o bien soluciones

simb�olicas. Si bien las soluciones num�ericas resultan m�as compactas en su

formulaci�on mediante algoritmos como el de Newton-Euler, estas formula-

ciones pueden resultan m�as ine�cientes computacionalmente ya que existen

numerosas operaciones del tipo multiplicaci�on por ceros. Este tipo de opera-

ciones no se producen con una formulaci�on simb�olica del problema ya que

todas estas operaciones se realizan a priori.

Adem�as es posible simpli�car las soluciones simb�olicas mediante un

an�alisis de los t�erminos de lo que esta compuesta, ganando m�as aun en e�cien-

cia. Todas estas manipulaciones se pueden realizar mediante herramientas de

c�alculo simb�olico como MAPLEV.

La obtenci�on de las ecuaciones de movimiento se puede obtener mediante

las ecuaciones de la din�amica cl�asica o bien mediante la mec�anica lagrangiana.

41

42 3 Din�amica del robot RM-10

3.2 Modelo Euler-Lagrange

El modelo Euler-Lagrange se basa en aplicar las ecuaciones de la mec�anica

lagrangiana ya conocidas con la forma:

d

dt

@L

@ _qi� @L

@qi= Qi i = 1; :::n (3.1)

Siendo n el n�umero 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 articulaci�on.

L se conoce como lagrangiana del sistema y se calcula como:

L = T � U (3.2)

Donde T es la energ��a cin�etica total del sistema mec�anico y U su energ��a

potencial.

En este caso las variables elegidas como grados de libertad son las vari-

ables del espacio articular del manipulador.

La energ��a cin�etica del manipulador se puede expresar de la forma:

T =1

2_qTD(q) _q =

1

2

nXi;j

dij _qi _qj (3.3)

donde D(q) es la matriz de inercias del sistema, siendo esta sim�etrica y de�ni-

da positiva.

La energ��a potencial se puede escribir en t�erminos de matrices de trans-

formaci�on de la forma:

U =

nXi

miT0i ~rci (3.4)

Donde:

3.2 Modelo Euler-Lagrange 43

� mi es la masa del enlace i.

� T 0i es la matriz de transformaci�on del sistema de referencia 0 al sistema

de referencia i.

� ~rci es el vector posici�on del centro de gravedad del enlace i respecto al

sistema de referencia i.

Sustituyendo la expresi�on de la energ��a cin�etica en 3.2, esta a su vez en

3.1 y desarrollando las derivadas se obtiene:

nXj

dkj�qj +

nXi;j

(@dkj

@qi� 1

2

@dij

@qk) _qi _qj �

@U

@qk= �k k = 1; :::; n (3.5)

Hasta ahora no se ha considerado los fen�omenos de fricci�on en el modelo,

si se a~naden en forma matricial se pueden escribir de la forma:

D(q)�q + C(q; _q) _q + F (q; _q) +G(q) = � (3.6)

Los t�erminos que aparecen en las ecuaciones de movimiento son:

� D(q)�q representa a las fuerzas inerciales de cada enlace debidas a la

aceleraci�on.

� C(q; _q) _q representa a los pares de Coriolis y los correspondientes a los

t�erminos centr��fugos.

� F (q; _q) representa a los pares de fricci�on en cada articulaci�on, que en

el caso m�as sencillo se puede modelar como una fricci�on viscosa.

� G(q) son los pares gravitatorios asociados a la masa de cada enlace.

Las ecuaciones 3.6 modelan la din�amica del brazo manipulador. Se pueden

a~nadir tambi�en la din�amica de los actuadores. Suponiendo que los motores

tienen una constante el�ectrica despreciable, la din�amica de �estos se reduce a

la ecuaci�on mec�anica del eje del motor. Por tanto, se puede escribir para los

actuadores en forma vectorial:

�m = Jm �qm +Bm _qm + �L (3.7)


El par motor es proporcional a la corriente que recorre los devanados del

est�ator, por lo que se puede poner:

Kt � Im = Jm�qm +Bm _qm + �L (3.8)

donde:

� Kt constante de par del motor.

� �m par el�ectrico desarrollando por el motor.

� Jm inercia del eje motor.

� qm posici�on del eje motor.

� Bm fricci�on viscosa del motor.

� �L par de carga que ofrece el brazo manipulador.

Si existen reductoras entre el manipulador y los motores se dispone

adem�as de las relaciones:

�qm = R�q

_qm = R _q (3.9)

� = R�L (3.10)

sustituyendo en la expresi�on 3.8 se tiene:

RKtIm = JmR2�q +BmR

2 _q + � (3.11)

donde R es una matriz diagonal con los coe�cientes de reducci�on mayores

que uno. Si se sustituye la expresi�on 3.6 en las ecuaciones 3.7 obtenemos:

(D(q) + JmR2)�q + (C(q; _q) +BmR

2) _q + F (q; _q) +G(q) = KtImR (3.12)

Los t�erminos de Coriolis se pueden calcular como funci�on de los dij seg�un

se ve en la expresi�on 3.5, por lo que interesa en primer lugar calcular los


elementos de la matriz de inercias expres�andolos como funci�on de las matrices

de transformaci�on del manipulador.

La energ��a cin�etica en general se puede tambi�en expresar como la suma

de la energ��a cin�etica de rotaci�on y la de traslaci�on:

T =

nXi

1

2miv

Tcivci +

1

2!Ti I!i (3.13)

Las velocidades lineales de los centros de masa y las velocidades angulares

de rotaci�on pueden ser expresadas en funci�on 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:

T =1

2_qT

nXi

(miJTvciJvci + JT!iIiJ!i) _q (3.15)

Identi�cando esta ecuaci�on con la ecuaci�on 3.3 obtenemos la expresi�on

para la matriz de inercias:

D(q) =

nXi

(miJTvciJvci + JT!iIiJ!i) (3.16)

El jacobiano de las velocidades de traslaci�on se calcula como una matriz

formada por las columnas:

Jvci =h

@(T 0

i~rci)

@q1: : :

@(T 0

i~rci )

@q6

i3�6

(3.17)

El jacobiano de las velocidades de rotaci�on se forma tambi�en mediante una

matriz por columnas de la forma:

J!i =�J!i1 : : : J!i6

�3�6

(3.18)


Donde:

J!ij =

8<:

(T ji )

�1~k si j � i

~0 si j > i

(3.19)

Los c�alculos para obtener la matriz de inercias se pueden implementar en

MapleV tal y como se muestra a continuaci�on:

Calcula la matriz T i0.

> T := proc(p) M:=array(identity,1..4,1..4); for k from 1to p do M:=evalm(M&*A(k)); od; RETURN(M) end;

T := proc(p)

localM; k;

M := array(identity ; 1::4; 1::4) ;

for k to pdoM := 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 1to 4 do Mat[n,m]:=diff(Mat[n,m],x) od od; RETURN(Mat); end;

Dmatrix := proc(Mat ; x)

localn; m;

forn to 4do form to 4doMatn;m := di�(Matn;m; x) odod ;

RETURN(Mat)

end

Deriva los elementos de un vector respecto al tiempo.

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


Dvect := proc(Vect)

localVel ; n;

Vel := matrix(4; 1) ; forn to 4doVeln; 1 := di�(Vectn; 1; t) od ;

RETURN(Vel)

end

Calcula el jacobiano Jvci de la velocidad del centro de masa.

Jacv := proc(Vect)

localJ; n; m;

J := matrix(3; 6) ;

forn to 3do form to 6doJn;m := di�(Vectn; 1; �m) odod ;

RETURN(J)

end

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 dovaux:=matrix(4,1,[0,0,1,0]); for m from n+1 to i dovaux:=evalm(evalm(1/A(m))&*vaux); od; for k from 1 to 3do 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) ;

forn to ido

vaux := matrix(4; 1; [0; 0; 1; 0]) ;

form fromn+ 1 to ido

vaux := evalm((evalm(1=A(m)))&� vaux ) od ;for k to 3doJk; n := vaux k; 1 od

od;

RETURN(J)

end

Caracter��sticas de los elementos.

Vectores centros de masa ~rci.


> 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 continuaci�on se calcula los jacobianos de las velocidades de traslaci�on de

los centros de gravedad, aportes de cada enlace por traslaci�on del c.d.g. a la

matriz D(q). Dtras es la parte de la matriz D debida a la translaci�on 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 continuaci�on 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 rotaci�on.

> 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~nadida por los motores.

> 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) ser�a la suma de la de traslaci�on, rotaci�on y la a~nadida

por los motores.

> Inertia:= map(simplify,evalm(Dtras+Drot+Dmot)):


Los elementos de la matriz C(q; _q) se pueden calcular a parir de la los ele-

mentos de la matr��z D(q). El segundo t�ermino de la ecuaci�on 3.5 aprovechan-

do la simetr��a lo podemos poner como:

nXi;j

(@dkj

@qi� 1

2

@dij

@qk) _qi _qj =

1

2

nXi;j

(@dkj

@qi+@dki

@qj� @dij

@qk) _qi _qj =

=1

2

nXi;j

cijk _qi _qj (3.20)

Las instrucciones de MapleV necesarias para obtener la m�atriz 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 doMC[i,j]:=eval(C(i,j)) od od;

Los elementos del vector de pares gravitatorios se calculan mediante el

tercer t�ermino de la expresi�on 3.5, o lo que es lo mismo con las instrucciones

que siguen a continuaci�on:


C�alculo de los terminos de energia potencial.

> V:=proc(i) m(i)*g*evalm(T(i)&*rcm(i))[3,1] end;

V := proc(i)m(i)?g?evalm((T(i))&� (rcm(i)))3; 1 end

Calculamos la energia potencial total del brazo manipulador.

> Vtot:=simplify(sum('V(k)','k'=1..6),siderels, simp);

Vtot := �g (x2m(2) + a2m(3) + a2m(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 + a3m(4) + x3m(3) + m(6) a3) sin(%1)

+ g (m(5) y5 +m(6) z6) cos(�4) sin(%1) sin(�5)

� g (m(6) d4 + z4m(4) + d4m(4) + y3m(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 Par�ametros del modelo del robot

Para poder simular el comportamiento del robot es necesario disponer de un

conjunto de par�ametros lo m�as preciso posible, adem�as esto permite poder

aplicar t�ecnicas que se basan en el modelo del manipulador con m�as �exito.

De entre los par�ametros que aparecen en las ecuaciones de movimiento del

robot aparecen par�ametros puramente cinem�aticos como son las distancias y

par�ametros din�amicos asociados a casa uno de los enlaces del robot.

Las distancias que aparecen en las matrices de transformaci�on son las que

aparecen en la tabla 3.1

Entre los par�ametros din�amicos necesarios para la caracterizaci�on de un

modelo tenemos los siguientes:

� Tensores de inercia.

3.3 Par�ametros del modelo del robot 51

Distancias en mm.

dB 950

a1 260

a2 710

a3 150

d3 40

d4 650

dH 150

Tabla 3.1: Par�ametros cinem�aticos del robot.

� Centros de gravedad.

� Masas.

� Inercias de los ejes de los motores.

� Coe�cientes de reducci�on.

� Par�ametros de fricci�on.

3.3.1 Tensores de inercia y centros de gravedad

Dada la di�cultad que entra~na la medida experimental de las componentes

del tensor de inercias se opt�o por realizar un modelo gr�a�co aproximado

mediante una herramienta de dise~no gr�a�co que fuese capaz de calcular una

aproximaci�on de los tensores. Las matrices de inercias se calculan respecto

al sistema de coordenadas solidario a cada enlace y tambi�en se calcula en el

origen del mismo sistema. Se hace la suposici�on de que hay simetr��a en los

tres ejes, permitiendo tener as�� una matriz diagonal en cada enlace. Con la

misma herramienta inform�atica se estimaron los centros de gravedad y las

masas de los distintos elementos.

3.3.2 Caracter��sticas de los motores y reductoras

Los par�ametros que aparecen en las ecuaciones din�amicas del manipulador

son las inercias de los ejes y la fricci�on asociada a los motores. Las inercias

de los ejes se obtiene a partir de las hojas de cat�alogo:


X

Y

Z

Z

X

Y

Z

Y

X

Y Z

X

Y

X

Z

Y

X

Z

J1 =

24 0:741 0 0

0 1:208 0

0 0 1:132

35

rc2 =

24 0:083

0

�0:083

35

m1 = 39

J2 =

24 0:265 0 0

0 3:434 0

0 0 3:520

35

rc2 =

24 0:228

0

0:187

35

m2 = 51

J3 =

24 1:020 0 0

0 1 0

0 0 1:644

35

rc3 =

24 0:172

0:025

0:003

35

m3 = 84

J4 =

24 0:830 0 0

0 0:8 0

0 0 0:120

35

rc4 =

24 0

0

�0:194

35

m4 = 34

J5 =

24 0:02 0 0

0 0:008 0

0 0 0:031

35

rc5 =

24 0

0:031

0

35

m5 = 7

J6 =

24 0:006 0 0

0 0:007 0

0 0 0:005

35

rc6 =

24 0

0

0:07

35

m5 = 7

Figura 3.1: Caracter��sticas de los elementos del brazo del manipulador

3.3 Par�ametros del modelo del robot 53

Motor Eje Inercia (kg=cm2)

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 Reducci�on

1 121

2 153

3 105

4 59

5 80

6 50

Tabla 3.3: Coe�cientes de reducci�on

La fricci�on viscosa de los motores aparece tambi�en en las ecuaciones

din�amicas afectado de un factor R2 a~nadi�endose a la fricci�on viscosa de la

articulaci�on de forma que el par de fricci�on viscosa ser��a:

�vTOT = (Fv + FvmR2) _q (3.21)

De este modo se puede identi�car la fricci�on total como se ver�a m�as 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 el�ectricas de los motores

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.


El valor de la intensidad es proporcional a la se~nal de control aplicada a los

servoampli�cadores de manera que el valor m�aximo se corresponde en cada

servoampli�cador con una intensidad m�axima. De manera que el par aplicado

por los motores vale, si u es la se~nal de control:

� =

p3

2KtImax

u

10(3.22)

Los valores que se tienen para los distintos ejes son los de la tabla 3.4.

3.3.3 Par�ametros de fricci�on

Por medio de la realizaci�on de experimentos es posible estimar algunos

par�ametros de fricci�on, como son la fricci�on viscosa y la fricci�on de Coulomb,

−4 −3 −2 −1 0 1 2 3 4−30

−20

−10

0

10

20

30

Fc

Fv

� f

_q

Figura 3.2: Caracter��stica est�atica de fricci�on te�orica

de manera que se pueda estimar una caracter��stica est�atica como la de la

�gura 3.2, que se ha supuesto sim�etrica.

Para estimar la caracter��stica se proporciona al motor una se~nal triangular

como orden de par y se estima la velocidad del eje a partir de la lectura

del sensor de posici�on. Si se mantiene una frecuencia baja para la se~nal

inyectada el par aplicado se usa b�asicamente en vencer la fricci�on. El resto

de articulaciones se mantiene a su valor de cero.

3.4 Minimizaci�on de los parametros 55

−4 −3 −2 −1 0 1 2 3 4−30

−20

−10

0

10

20

30

�

_q

Figura 3.3: Caracter��stica de fricci�on experimental del eje 6.

En la �gura 3.3 se puede ver el resultado obtenido para la articulaci�on

6 del manipulador. Se ve que existe un fen�omeno de hist�eresis en la curva,

esto es debido a los efectos de la aceleraci�on y tambi�en a que en realidad la

fricci�on 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 sim�etricos se puede deducir su valor.

3.4 Minimizaci�on de los parametros

Las ecuaciones din�amicas del manipulador comprenden cientos de t�erminos,

por lo que puede ocurrir que sea demasiado costoso computacionalmente

hablando a la hora de realizar simulaciones o realizar control.

Mediante manipulaci�on simb�olica es posible obtener un conjunto de

par�ametros ki algo m�as reducido, que se calculan a partir de los par�ametros

elementales y permiten escribir las ecuaciones del estilo:


−1.5 −1 −0.5 0 0.5 1 1.5 2−60

−50

−40

−30

−20

−10

0

10

20

30

40

�

_q


−2.5 −2 −1.5 −1 −0.5 0 0.5 1 1.5 2−30

−20

−10

0

10

20

30

�

_q


3.4 Minimizaci�on de los parametros 57

�1 = k1�q1 + k2c2�q2 + k3c23�q3 + : : :...

�6 = : : : (3.23)

El algoritmo se encuentra en el archivo modelo.mws, que aplicado a todos

los elementos de la matriz de inercias permite obtener 26 constantes que

permiten escribir las ecuaciones de una forma m�as compacta. Adem�as estas

constantes tambi�en aparecen en los t�erminos de coriolis y de gravedad.

Algunas de estas constantes son:

k1 = �Izz 6k2 = m5 y5 a1 +m6 z6 a1

k3 = m6 a2 z6 +m5 y5 a2

k4 = m5 y5 a3 +m6 z6 a3

k5 = m6 z6 d4 +m5 y5 d4

k6 = m5 y5 d3 +m6 z6 d3

k7 = �Ixx 2 + Iyy2

k8 = Iyy6 � Ixx 6

k9 = m5 a2 a3 +m3 a2 x3 +m6 a2 a3 +m4 a2 a3

k10 = �m5 y52 �m6 z6

2 � Izz 5 � Ixx 6

k12 = �m4 a2 d4 �m6 a2 d4 �m5 a2 d4 �m3 a2 y3 �m4 a2 z4

k13 = �Ixx 5 �m5 y52 �m6 z6

2 � Izz 4 � Iyy6

k14 = m3 x3 z3 +m3 x3 d3 +m4 d3 a3 +m5 a3 d3 +m6 a3 d3

k15 = �m2 x22 �m4 a2

2 �m3 a22 �m5 a2

2 �m6 a22

k16 = �2m5 d4 a3 � 2m3 x3 y3 � 2m6 d4 a3 � 2m4 a3 d4 � 2m4 a3 z4

k17 = 2m4 a2 a1 + 2m5 a2 a1 + 2m3 a2 a1 + 2m6 a2 a1 + 2m2 x2 a1

k18 = �2m3 y3 a1 � 2m5 a1 d4 � 2m4 a1 d4 � 2m6 a1 d4 � 2m4 a1 z4

k19 = m5 d3 a2 +m4 d3 a2 +m3 d3 a2 +m2 x2 z2 +m3 z3 a2 +m6 d3 a2

k20 = m6 d4 d3 +m3 y3 z3 +m3 y3 d3 +m5 d4 d3 +m4 d3 z4 +m4 d3 d4

k21 = m5 y52 +m6 z6

2 + Iyy6 � Izz 6 � Iyy5 + Ixx 5

k22 = m5 y52 � Iyy5 + Iyy4 +m6 z6

2 + Ixx 6 � Ixx 4 � Izz 6 + Izz 5

k23 = m6 a32 +m5 d4

2 +m4 z42 + Izz 3 + 2m4 z4 d4 + Izz 6 +m4 d4

2 +

+ Ixx 4 +m4 a32 + Iyy5 +m3 x3

2 +m3 y32 +m6 d4

2 +m5 a32


k24 = m6 a32 +m4 a2

2 +m5 d42 +m4 z4

2 + Izz 3 +m2 x22 + 2m4 z4 d4 +

+ Izz 6 +m4 d42 + Ixx 4 +m4 a3

2 + Iyy5 +m3 x32 + Izz 2 +m3 y3

2 +

+m6 d42 +m6 a2

2 +m5 a32 +m5 a2

2 +m3 a22

k25 = �Ixx 3 � 2m4 z4 d4 � Ixx 6 + Iyy6 + Iyy3 +m2 x22 +m4 a2

2 +

+m3 a22 + Izz 4 +m3 x3

2 �m3 y32 +m5 a3

2 �m5 d42 +m5 a2

2 ��m6 d4

2 +m6 a32 + Ixx 5 � Iyy4 +m6 a2

2 � Izz 5 �m4 z42 �m4 d4

2 +

+m4 a32

k26 = m4 a22 +m5 d4

2 +m6 z62 +m4 z4

2 + Ixx 3 +m2 x22 + Izz 1 +

+ 2m4 z4 d4 +m6 a12 + Ixx 6 +m4 d4

2 +m2 z22 + Iyy4 +m2 a1

2 +

+m5 a12 +m3 a1

2 + Izz 5 +m3 z32 +m3 d3

2 + Ixx 2 +m3 y32 +

+m5 y52 +m6 d4

2 +m5 d32 +m4 a1

2 + 2m3 z3 d3 +m4 d32 +

+m6 a22 +m1 x1

2 +m5 a22 +m6 d3

2 +m3 a22

Calculando previamente las constantes es posible mininimizar el n�umero

de c�alculos, obteniendose una distribuci�on de t�erminos seg�un la tabla siguien-

te:

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: Distribuci�on de t�erminos 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 t�erminos mediante comparaci�on del valor num�erico de los coe�-

cientes, usando las velocidades m�aximas asociadas a cada articulaci�on.

Eliminando los t�erminos con un coe�ciente menor que el 5% del coe�ciente

mayor. Si se realiza esta simpli�caci�on la cantidad de t�erminos pasa a ser:

3.5 Implementaci�on inform�atica 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: Distribuci�on de t�erminos en las ecuaciones del manipulador sim-

plicadas.

3.5 Implementaci�on inform�atica

El modelo del manipulador de la �gura 3.6 se encuentra disponible en

Simulink para la relalizaci�on de simulaciones, el subsistema se presenta en

la �gura 3.7. La implementaci�on se realiza mediante dos bloques S-Function

programados en C.

En el bloque rm10model.c se realizan todas las operaciones para c�alcular

las derivadas de los estados de manera que Simulink los integre segun el

m�etodo de integraci�on elegido.

El bloque rm10modelcor.c implementa el bloque C(�; _�) de la �gura 3.6

para facilitar la compilaci�on del las funciones.

Como opciones de simulaci�on, es posible "desactivar" tanto los t�erminos

de Coriolis y centr��petos como los efectos de fricci�on.

3.6 Simulaciones del modelo

Con objeto de comprobar si el modelo se comporta seg�un lo previsto se simula

el sistema en bucle abierto.

El par que se aplica a la entrada es el par gravitatorio m�as una se~nal

senoidal de baja frecuencia con objeto de tener un equilibrio en la posici�on

inicial y ver los per�les de velocidades creados por la fricci�on.

En las �gura 3.8 se tiene el diagrama Simulink para la simulaci�on del

sistema en bucle abierto. En las �guras 3.9y 3.10 se presentan los resultados


+

- - -

� �_��1s

1sM(�)

C(�; _�)

G(�)

F ( _�)

Figura 3.6: Diagrama de bloques asociado al modelo

de la simulaci�on del diagrama 3.8.

3.6 Simulaciones del modelo 61

2

Out2

1

Out1

5

m6

rm10modelcor

rm10model

1

In1

Figura 3.7: Diagrama de bloques Simulink para la simulaci�on del manipulador

Velocidad

vect_gravedad

S−Function

PosiciónMux

ParAdicional

MuxRM−10

Model_RM10

0

Figura 3.8: Diagrama de bloques para la simulaci�on del modelo en bucle

abierto


0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−2

−1

0

1

2

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−2

0

2

4

6

8

t

� 5

� 6

Figura 3.9: Evoluci�on de las posiciones de las articulaciones 5 y 6

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−50

0

50

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−50

0

50

t

ParVelocidad

_ � 5

_ � 6

Figura 3.10: Evoluci�on de las velocidades de las articulaciones 5 y 6

Cap��tulo 4

Control de robot RM-10

4.1 Introducci�on

En problema fundamental en el control de robot es tratar que el extremo

�nal de la herramienta siga una trayectoria deseada lo m�as �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 din�amica del

manipulador son:

(D(q) + JmR2)�q + (C(q; _q) +R2) _q + F (q; _q) +G(q) = KtImR (4.1)

Las ecuaciones son claramente no lineales pero adem�as est�an acopladas

por los t�erminos no diagonales de las matrices D y C, debido a que los

t�erminos diagonales de D y C dependen de la posici�on y velocidades de las

otras articulaciones.

63

64 4 Control de robot RM-10

+

-+

+ �_�d_�d e

�p

Kp +KvsKm

Je+Bes1s

Figura 4.1: Esquema de control lineal tipo PD

Ahora bien al introducir la din�amica de los motores y reductoras aparecen

en los t�erminos diagonales cantidades afectadas por R2 lo cual hace que

domine la din�amica de los motores frente a la del propio manipulador.

En el l��mite de esta situaci�on el sistema se convertir��a en 6 sistema lineales

desacoplados. El sistema real tender�a m�as o menos a esta situaci�on en funci�on

de los valores concretos de las magnitudes f��sicas del manipulador.

4.2.2 Control tipo PD

Aplicando los expuesto anteriormente se pueden dise~nar controladores li-

neales para cada articulaci�on, 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:

u1Km1= Je1

��1 +Be1_�1 + �p1

......

u6Km6= Je6

��6 +Be6_�6 + �p6

(4.2)

Los coe�cientes Jei y Bei son inercias y fricciones viscosa efectivas del

conjunto. Las inercias dependen a�un de la posici�on del manipulador.

Si se calcula la funci�on de transferencia en bucle cerrado del diagrama de

la �gura 4.1 se obtiene:

4.2 Control lineal 65

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

GBCPD(s) =�

�r=

KpKm +KvKms

s2Je + (Be +KvKm)s+KpKm

(4.3)

Se pueden dise~nar las constantes Kv y Kp atendiendo a criterios de esta-

bilidad. Si identi�camos el denominador con la ecuaci�on caracter��stica de un

sistema de segundo orden:

s2 + 2�!n + !2 = 0 (4.4)

Si identi�cando el denominador de la expresi�on 4.3 con la expresi�on 4.4:

!n =

rKpKm

Je; � =

Be +KvKm

2Je!n(4.5)

Si se impone � = 1 e imponemos un cierto !n a cada articulaci�on ten-

dremos expresiones para las ganancias del controlador de la forma:

Kp =!2Je

Km

; Kv =2Je!n � Be

Km

(4.6)

Dado que las inercias e�caces var��an con la posici�on del manipulador se

adopta como criterio conservador el dise~nar las ganancias para los valores

m�aximos de las inercias e�caces que se alcanzan dentro del espacio de trabajo

del manipulador. Las variaciones son m�as importantes en las articulaciones

1 y 2.

Las frecuencias !n se eligen seg�un la velocidad deseada en la articulaci�on

compatible con la saturaci�on de los actuadores, dado que las articulaciones


de la base mueven mucha m�as masa se adoptan frecuencias naturales m�as

bajas.

En la tabla 4.1 se tienen los valores calculados para cada articulaci�on.

Resultados de simulaci�on

La simulaci�on del controlador PD se realiza mediante el esquema Simulink

de la �gura 4.2. Las perturbaciones que m�as afectan al funcionamiento del

controlador son el par gravitatorio y la fricci�on est�atica simulada, que dada

la ausencia de efecto integral provocan la existencia de errores en r�egimen

permanente.

Velocidad

Step

[−1 −pi/6 0.4 −0.25 0.25 1]

Referencia

Posicion

Perturbacion adicional

u

RM−10

Model_RM10

K

Kp

K

Kd

[ref]

[ref]

Error seg Control

0 −50 −50 0 −20 0]

1

1

qact

Move

Ref

q

qd

Articular

Figura 4.2: Diagrama Simulink para la simulaci�on de un controlador PD


0 0.5 1 1.5 2 2.5 3−0.4

−0.2

0

0.2

0.4

0.6

0.8

1

6

5

4

� 4;�5;�6

tiempo

0 0.5 1 1.5 2 2.5 3

−1.5

−1

−0.5

0

0.5

1

2

3

� 1;�2;�3

tiempo

Figura 4.3: Seguimiento en posici�on para control tipo PD


0 0.5 1 1.5 2 2.5 3−0.02

−0.01

0

0.01

0.02

0.03

0.04

0.05

6

5

4

e 4;e5;e6

tiempo

0 0.5 1 1.5 2 2.5 3−0.02

−0.015

−0.01

−0.005

0

0.005

0.01

0.015

0.02

1

2

3

e 1;e2;e3

tiempo

Figura 4.4: Error de seguimiento en posici�on para control tipo PD


0 0.5 1 1.5 2 2.5 3−1.5

−1

−0.5

0

0.5

1

1.5

6

5

4

u4;u5;u6

tiempo

0 0.5 1 1.5 2 2.5 3−6

−5

−4

−3

−2

−1

0

1

2

1

2

3

u1;u2;u3

tiempo

Figura 4.5: Se~nales de control para regulador tipo PD


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 fricci�on, aunque fen�omenos de elasticidad est�an presentes

dado que las reductoras de los ejes 5 y 6 son del tipo harmonic drive. Las

ganancias usadas en la implementaci�on son algo menores que las calculadas

por la aparici�on de oscilaciones.


0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

� 1;�1r

tiempo

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20

0.1

0.2

0.3

0.4

0.5

0.6

0.7

� 2;�2r

tiempo

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−0.6

−0.5

−0.4

−0.3

−0.2

−0.1

0

0.1

� 3;�3r

tiempo

Figura 4.6: Seguimiento en posici�on para control tipo PD en los ejes 4, 5 y 6


0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−0.2

0

0.2

0.4

0.6

0.8

1

1.2

qd1

tiempo

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

qd2

tiempo

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−1.2

−1

−0.8

−0.6

−0.4

−0.2

0

0.2

qd3

tiempo

Figura 4.7: Velocidades para control tipo PD en los ejes 4, 5 y 6


0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20

1

2

3

4

5

6

7

8x 10

−3

e 1

tiempo

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20

1

2

3

4

5

6x 10

−3

e 2

tiempo

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−0.018

−0.016

−0.014

−0.012

−0.01

−0.008

−0.006

−0.004

−0.002

0

e 3

tiempo

Figura 4.8: Error de seguimiento en posici�on para control tipo PD en los ejes

4, 5 y 6


0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−0.05

0

0.05

0.1

0.15

0.2

u1

tiempo

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−0.25

−0.2

−0.15

−0.1

−0.05

0

0.05

0.1

0.15

0.2

u2

tiempo

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−0.5

−0.45

−0.4

−0.35

−0.3

−0.25

−0.2

−0.15

−0.1

−0.05

0

u3

tiempo

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


4.2.3 Control tipo PID

Con objeto de eliminar errores en regimen permanente se puede a~nadir un

efecto integral tal.

La funci�on de transferencia en bucle cerrado para el conjunto resulta ser:

GBCPID(s) =�

�r=

KpKms+KvKms2 +KmKi

s3Je + (Be +KvKm)s2 +KpKms+KmKi

(4.7)

Aplicando el criterio de estabilidad de Routh-Hurwitz al denominador de

la expresi�on anterior se obtiene una cota para la ganancia Ki:

Ki <Kp(B +KmKv)

J(4.8)

Las cotas para las ganancias Ki que se obtienen a partir de la tabla 4.1

tienen los siguientes valores:

Articulaci�on Ki

1 224960

2 186680

3 27690

4 19510

5 20950

6 10060

Tabla 4.2: Cotas m�aximas para las ganancias Ki


La simulaci�on del controlador PID se realiza mediante el esquema Simulink

de la �gura 4.10. En las �guras se observa que las perturbaciones al fun-

cionamiento del sistema controlado son atenuadas m�as r�apidamente que en

el caso PD, consiguiendo una reducci�on de los errores en r�egimen perma-

nente. Aunque tambi�en se observa que los transitorios se han empeorado

apareciendo comportamientos oscilatorios.


Velocidad

Step

[−1 −pi/6 0.4 −0.25 0.25 1]

Referencia

Posicion

Perturbacion adicional

u

RM−10

Model_RM10

K

Kp

K

Ki

K

Kd

s

1[ref]

[ref]

Control

[0 −50 −50 0 −20 0]

1

1

qact

Move

Ref

q

qd

Articular

Figura 4.10: Diagrama Simulink para la simulaci�on 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 fricci�on y

la gravedad como perturbaciones que son atenuadas por la realimentaci�on

del controlador. Logicamente las prestaciones que se podran conseguir con

un control lineal est�an 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 t�ecnicas de control se empiece a tener en cuenta la naturaleza

no lineal del modelo del robot.

La t�ecnica del par calculado b�asicamente consiste en el uso de dos bucles

de realimentaci�on. Un primer bucle m�as 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)

4.3 Control por Par Calculado 77

0 0.5 1 1.5 2 2.5 3−0.4

−0.2

0

0.2

0.4

0.6

0.8

1

1.2

6

5

4

� 4;�5;�6

tiempo

0 0.5 1 1.5 2 2.5 3

−1.5

−1

−0.5

0

0.5

1

2

3

� 1;�2;�3

tiempo

Figura 4.11: Seguimiento en posici�on para control tipo PID


0 0.5 1 1.5 2 2.5 3−6

−4

−2

0

2

4

6

8

10

12

14x 10

−3

6

5

4

e 4;e5;e6

tiempo

0 0.5 1 1.5 2 2.5 3

−6

−4

−2

0

2

4

6x 10

−3

1

3

e 1;e2;e3

tiempo

Figura 4.12: Error de seguimiento en posici�on para control tipo PID


0 0.5 1 1.5 2 2.5 3−1.5

−1

−0.5

0

0.5

1

1.5

2

6

5

4

u4;u5;u6

tiempo

0 0.5 1 1.5 2 2.5 3−7

−6

−5

−4

−3

−2

−1

0

1

2

3

1

2

3u1;u2;u3

tiempo

Figura 4.13: Se~nales de control para regulador tipo PID


+

+

� 0�

_�

M(�)

C(�; _�) + F ( _�) +G(�)

Figura 4.14: Bucle de linealizaci�on y desacoplo

Igualando con la ecuaci�on 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 = �qd +Kpe+Kv _e (4.11)

donde:

e = qd � q

_e = _qd � _q (4.12)

y las matrices Kv y Kp son matrices diagonales. Si igualamos la ecuaci�on

4.11 con la ecuaci�on 4.10 se la obtiene la ecuaci�on diferencial para el error:

�e +Kv _e+Kpe = 0 (4.13)


+

+

Cont

qddrqdrqr

1s

1s

� 0

� 0�

�_�

_�

M(�)

C(�; _�) + F ( _�) +G(�)

Controlador Lineal

Figura 4.15: Esquema completo de control por par calculado

Con Kv y Kp se puede modi�car la din�amica del error y obtener el com-

portamiento deseado. Identi�cando la ecuaci�on del error con los par�ametros

caracter��sticos de un sistema de segundo orden se tiene:

Kp = !2n; 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 saturaci�on de los

actuadores. Normal mente son mas bajas en las articulaciones de la base y

m�as altas en las articulaciones de la mu~neca.


La simulaci�on del controlador por par calculado se realiza mediante el esque-

ma Simulink de la �gura 4.16. En un primer caso se puede usar un bucle de


Velocidad

vect_gravedad

S−Function3

mat_inercia

S−Function1

vect_friccion

S−Function

5 −pi/4 −0.4 −0.25 0.5 1

Referencia

K

R Kt Imax

Posicion

Qref

Q

QDref

QD

Out1

PID

u

RM−10

Model_RM10

[ref]

[ref]

In1

In2Out1

Coriolis

Control

[0 100 100 0 30 0]

1

1

qact

Move

Ref

q

qd

qdd

Articular

Figura 4.16: Diagrama Simulink para la simulaci�on 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

realimentaci�on tipo PD, cuyas constantes se dise~nan seg�un la tabla siguiente

4.3


0 0.5 1 1.5 2 2.5 3−0.4

−0.2

0

0.2

0.4

0.6

0.8

1

6

5

4

� 4;�5;�6

tiempo

0 0.5 1 1.5 2 2.5 3

−1.5

−1

−0.5

0

0.5

1

2

3

� 1;�2;�3

tiempo

Figura 4.17: Seguimiento en posici�on para control tipo Par Calculado


0 0.5 1 1.5 2 2.5 3−8

−6

−4

−2

0

2

4

6

8

10

12x 10

−3

6

5

4

e 4;e5;e6

tiempo

0 0.5 1 1.5 2 2.5 3−1

−0.5

0

0.5

1

1.5

2x 10

−3

1

2

3

e 1;e2;e3

tiempo

Figura 4.18: Error de seguimiento en posici�on para control tipo Par Calculado


0 0.5 1 1.5 2 2.5 3−1.5

−1

−0.5

0

0.5

1

1.5

6

5

4

u4;u5;u6

tiempo

0 0.5 1 1.5 2 2.5 3−6

−5

−4

−3

−2

−1

0

1

2

1

2

3

u1;u2;u3

tiempo

Figura 4.19: Se~nales de control para regulador tipo Par Calculado


Resultados experimentales

En las �guras 4.20 a 4.21 se representan las se~nales obtenidas en la experi-

mentaci�on del algoritmo de control de par calculado. Dado que el acoplamien-

to entre estas ultimas articulaciones es bastante reducido no solo ya por la

presencia de reductoras si no tambi�en por el hecho de que los efectos gravita-

torios 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 fricci�on por

lo que usando modelos de fricci�on m�as completos ser��a posible mejorar las

prestaciones del sistema.


0 0.5 1 1.5 2 2.5 3 3.5 4−0.7

−0.6

−0.5

−0.4

−0.3

−0.2

−0.1

0

� 1;�1r

tiempo

0 0.5 1 1.5 2 2.5 3 3.5 40

0.1

0.2

0.3

0.4

0.5

0.6

0.7

� 2;�2r

tiempo

0 0.5 1 1.5 2 2.5 3 3.5 40

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

� 3;�3r

tiempo

Figura 4.20: Seguimiento en posici�on para control tipo par calculado en los

ejes 4, 5 y 6


0 0.5 1 1.5 2 2.5 3 3.5 4−1

−0.8

−0.6

−0.4

−0.2

0

0.2

0.4

qd1

tiempo

0 0.5 1 1.5 2 2.5 3 3.5 4−0.2

0

0.2

0.4

0.6

0.8

1

1.2

qd2

tiempo

0 0.5 1 1.5 2 2.5 3 3.5 4−0.2

0

0.2

0.4

0.6

0.8

1

1.2

qd3

tiempo

Figura 4.21: Velocidades para control tipo par calculado en los ejes 4, 5 y 6


0 0.5 1 1.5 2 2.5 3 3.5 4−6

−4

−2

0

2

4

6

8x 10

−3

e 1

tiempo

0 0.5 1 1.5 2 2.5 3 3.5 4−0.02

−0.015

−0.01

−0.005

0

0.005

0.01

e 2

tiempo

0 0.5 1 1.5 2 2.5 3 3.5 4−0.02

−0.015

−0.01

−0.005

0

0.005

0.01

0.015

e 3

tiempo

Figura 4.22: Error de seguimiento en posici�on para control tipo par calculado

en los ejes 4, 5 y 6


0 0.5 1 1.5 2 2.5 3 3.5 4−0.12

−0.1

−0.08

−0.06

−0.04

−0.02

0

0.02

u1

tiempo

0 0.5 1 1.5 2 2.5 3 3.5 4−0.1

−0.05

0

0.05

0.1

0.15

0.2

0.25

u2

tiempo

0 0.5 1 1.5 2 2.5 3 3.5 4−0.05

0

0.05

0.1

0.15

0.2

u3

tiempo

Figura 4.23: Se~nal de control para control tipo par calculado en los ejes 4, 5

y 6

Ap�endice A

Tarjeta de E/S digital

Esta tarjeta permite a la tarjeta controladora interactuar con la parte elec-

tromec�anica del armario de control de System Robot, as�� como captar se~nales

de alarma de distintas fuentes. Todas las entradas y salidas est�an optoais-

ladas y realizan una conversi�on de tensi�on de 0-24V a niveles TTL, en el

armario y la tarjeta controladora respectivamente.

La con�guraci�on de las etapas de salida y entrada se puede ver en las

�guras A.2 y A.1 respectivamente.

Vcc

R1

R2

R3

IC1

IC2

D1

OUTIN

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 encuen-

tra situada f��sicamente en la parte posterior el armario controlador de System

91

92 A Tarjeta de E/S digital

Vcc

R1

IC1

D1

D2

OUT1

OUT2IN

Figura A.2: Etapa de salida digital de la tarjeta E/S digitales

Entrada Descripci�on Pin dSPACE

1 Emergencia General P2B18

2 Contactor de frenos el�ectricos cerrado P2A18

3 Sobrecalentamiento fuente alimentaci�on de servos P2B02

4 Resistencia de disipaci�on fundida P2A02

5 Perdida de fase en fuente de alimentaci�on servos P2B19

6 No usada P2A19

7 No usada P2B03

8 No usada P2A03

Tabla A.1: Conexionado entradas digitales

Robot. El conjunto de todas las se~nales se recoge en un solo mazo de cables

que tiene como destino el par de conectores de se~nales digitales de la tarjeta

controladora dSPACE. En las tablas A.1 y A.2 se encuentra detallada el uso

de cada se~nal y la numeraci�on usada para el conexionado.

La tarjeta precisa de alimentaci�on de +24V por parte del armario de

System-Robot para poder activar los reles, y por parte de la tarjeta contro-

ladora dSPACE se precisa de +5V para alimentar toda la l�ogica TTL.

A Tarjeta de E/S digital 93

Salida Descripci�on Pin dSPACE

1 Habilitaci�on de los servo ampli�cadores P2B20

2 Cerrar contactor de frenos electromec�anicos P2A20

3 Limitaci�on de se~nal de control al 10% P2B04

4 Conmut. se~nales control dSPACE-SystemRobot P2A04

5 No Usada P2B21

6 No Usada P2A21

7 No Usada P2B5

8 No Usada P2A5

Tabla A.2: Conexionado salidas digitales

94 A Tarjeta de E/S digital

Ap�endice B

Tarjeta de conversi�on

resolver-encoder

La tarjeta de conversi�on resolver a encoder (CRE) permite la lectura de

un sensor de posici�on tipo resolver, emulando las se~nales de un encoder in-

cremental de 1024 pulso por vuelta. La tarjeta incorpora algunas funciones

adicionales para facilitar la incorporaci�on del nuevo controlador.

El diagrama de bloques funcional de la tarjeta se puede en la �gura B.1.

Filtrado/Ajuste de fase

Conversión RE

Amplificación OptoAislación

AislaciónAnalógica

Conmutación

Figura B.1: Diagrama de bloques de la tarjeta CRE

En la �gura B.2 se representa la disposici�on f��sica de los distintos conec-

tores que dispone la tarjeta y en la tabla B.1 la funci�on de cada uno de sus

95

96 B Tarjeta de conversi�on resolver-encoder

pines.

P2P1

P3

P4

P5

P6

P7

• •• •• •

4

3

2

14321 5

4

3

2

16

1

2

1 2

Figura B.2: Conectores presentes en la tarjeta CRE

La funci�on de cada conector es la que sigue:

1. P1/P2: Conectores de entrada y salida de resolver. Permiten la lectura

de las se~nales del resolver y de la referencia. Es necesario tener dos

conectores ya que no se puede interrumpir las se~nales de resolver que

llegan al servoampli�cador debido a que �este las necesita para la l�ogica

de conmutaci�on de los puentes de transistores de potencia.

2. P3: Conector de alimentaci�on del lado del armario de control.

3. P4: Conmutaci�on de se~nales anal�ogica y salida de la misma.

4. P5: Conector de alimentaci�on de lado del PC.

5. P6: Conector de encoder incremental.

6. P7: Entrada anal�ogica proveniente del la tarjeta controladora.

B.1 Filtros y Ajuste de fase

Como etapa previa a la conversi�on de las se~nales de los resolvers se �ltran

las se~nales seno y coseno mediante �ltros pasivos RC.

B.1 Filtros y Ajuste de fase 97

P1/P2-1 Se~nal seno (+)

P1/P2-2 Resistencia NTC Motor (+)

P1/P2-3 Se~nal coseno (-)

P1/P2-4 Referecia resolver (+)

P1/P2-5 Resistencia NTC Motor (-)

P1/P2-6 Se~nal seno (-)

P1/P2-7 Pantalla

P1/P2-8 Se~nal coseno (+)

P1/P2-9 Referencia resolver (-)

P3-1 Tensi�on Alimentaci�on -12V armario

P3-2/3 GND Alimentaci�on armario

P3-4 Tensi�on Alimentaci�on +12V

P4-1 Alimentaci�on Rel�e Conmutaci�on +24V

P4-2 Alimentaci�on Rel�e Conmutaci�on GND

P4-3 Se~nal de control a servoampli�cador (+)

P4-4 Se~nal de control a servoampli�cador (-)

P4-5 Se~nal de control de control System Robot (-)

P4-6 Se~nal de control de control System Robot (+)

P5-1 Tensi�on Alimentaci�on -12V PC

P5-2/3 GND Alimentaci�on PC

P5-4 Tensi�on Alimentaci�on +12V PC

P6-1 Tensi�on Alimentaci�on +5V PC

P6-2 GND Alimentaci�on PC

P6-3 Encoder incremental pulso A

P6-4 Encoder incremental pulso B

P6-5/6 No usadas

P7-1 Se~nal de control tarjeta controladora dSPACE (-)

P7-2 Se~nal de control tarjeta controladora dSPACE (+)

Tabla B.1: Conexiones de la tarjeta CRE


Idealmente las se~nales inducidas en los devanados �jos de los resolvers

est�an en fase con las se~nal aplicada de referencia, esto en realidad no es as��

ya que las bobinas de los resolver distan de ser ideales y como resultado las

se~nales inducidas seno y coseno presentan un desfase el�ectrico apreciable.

Este desfase es necesario compensarlo ya que resulta excesivo para que

la conversi�on de la etapa siguiente funcione correctamente. Para ello se in-

troduce en la se~nal de referencia un �ltro activo de primer orden basado en

ampli�cador operacional, que permite ajustar el desfase introducido por el

resolver.

El �ltro permite ajustar tambi�en su ganancia una vez introducido el des-

fase. Esto es necesario ya que el circuito de conversi�on de la etapa siguiente

esta dise~nado para tensiones de entrada senoidales de 2VRMS como m�aximo

y la se~nal de referencia que suministra el servoampli�cador es de 10VPP que

equivale a 3:53VRMS por lo que el �ltro adem�as debe atenuar esta se~nal. Este

problema no aparece con las dos senoides inducidas ya que el resolver tiene

una relaci�on de transformaci�on de 0,5.

R1

R2

C1

IC1+

-

Figura B.3: Esquema del �ltro activo para la se~nal de referencia

La funci�on de transferencia del circuito de la �gura B.1 resulta ser:

Vo

Vi= �R2

R1

1

1 +R2C2s(B.1)

Por lo que el desfase solo depende de R2 y una vez �jada esta se puede

ajustar la ganancia mediante la resistencia R1

B.2 Conversi�on resolver a encoder 99

B.2 Conversi�on resolver a encoder

La coversi�on se basa en el circuito integrado AD2S90 de Analog Devices. Este

circuito permite realizar la conversi�on a partir de las se~nales de referencia,

y el seno y coseno inducidos, dando la posici�on de salida en dos formatos

digitales con una resoluci�on de 12 bits . Uno de los formatos es tipo encoder

incremental con niveles de tensi�on TTL, el otro formato disponible es una

linea serie.

MULTIPLICADOR

DETECTOR DEFASE

+INTEGRADOR

OSCILADORCONTROLADOPOR TENSIÓN

CONTADORREVERSIBLE

LÓGICA DEDECODIF .

LATCH

INTERFACE SERIE

SENO+

SENO-

COSENO+

COSENO-

NMC

A

B

NM

CS

SCLK

DATOS

REF

VEL

DIR

CLKOUT

ÁNGULOφ

ÁNGULOθ

-SIN( θ-φ)

Figura B.4: Funcionamiento interno del circuito integrado AD2S90

El circuito realiza la conversi�on con una t�ecnica de tracking. Basicamente

el circuito calcula una se~nal proporcional al error entre el �angulo real y el

angulo almacenado en un contador reversible, y a partir de este error incre-

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


Vel. M�axima (/s) Reducci�on Frec. M�axima (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 m�aximas de conmutaci�on de las se~nales encoder

La soluci�on adoptada consiste en intercalar ampli�cadores operacionales

en modo de seguidor de tensi�on de manera que estos si sean capaces de sumi-

nistrar 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 r�apido como para que las se~nales de emulaci�on de encoder

no se vean distorsionadas. Las frecuencias m�aximas que deben tolerar se

pueden estimar a partir de las velocidades m�aximas 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 m�aximas B.2

B.4 Optoaislaci�on de se~nales digitales

Para evitar cualquier peligro de derivaci�on de tensiones peligrosas hacia la

tarjeta controladora, las se~nales A y B de los encoders se aslan debidamente

mediante el uso de optoacopladores.

Los optoacopladores seleccionados disponen de salida Smitch-Trigger per-

mitiendo la alta velocidad de conmutaci�on necesaria

B.5 Se~nales de control

Las se~nales de control generadas por los algoritmos de control pasan por esta

tarjeta donde la etapa de aislamiento anal�ogico se encarga de proteger a la

tarjeta controladora.

B.6 Esquemas el�ectricos 101

El aislamiento de las se~nales anal�ogicas esta basado en el circuito integra-

do ISO122P de la �rma Burr-Brown que permite aislar se~nales con un rango

�10V y con un ancho de banda de 50kHz. El esquema de conexi�on se puede

ver en los esquemas el�ectricos que se adjuntan.

La tarjeta incorpora tambi�en un rel�e que conmmuta entre las se~nales de

control proporcionadas por el controlador de System Robot y las generadas

por el nuevo controlador dSPACE. La conmutaci�on se realiza mediante la

aplicaci�on de +24V en las bornas de conexi�on del conector P4-1/2.

B.6 Esquemas el�ectricos


Ap�endice C

Esquemas el�ectricos de

electr�onica de potencia

103

104 C Esquemas el�ectricos de electr�onica de potencia

Ap�endice D

C�odigo fuente funciones

Simulink

105

106 D C�odigo 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

*/

#define S_FUNCTION_NAME rm10model

#define S_FUNCTION_LEVEL 2

#include <math.h>

#include "simstruc.h"

/* Variables del espacio de trabajo */

#define mE rwork[ 0]

#define Jm1 rwork[ 1] /* Inercias motores */

#define Jm2 rwork[ 2]





#define a1 rwork[ 7] /* Distancias */

#define a2 rwork[ 8]


#define d3 rwork[10]

#define d4 rwork[11]

#define x1 rwork[12]

#define z1 rwork[13]




#define y3 rwork[17]



#define y5 rwork[20]


#define Izz1 rwork[22] /* Inercias */

#define Ixx2 rwork[23]


#define Iyy2 rwork[24]

#define Izz2 rwork[25]













#define m1 rwork[38] /* Masas */

#define m2 rwork[39]





#define s2 rwork[44] /* Senos y cosenos */

#define c2 rwork[45]

#define s3 rwork[46]










#define a1_2 rwork[56] /* Distancias al cuadrado*/

#define a2_2 rwork[57]

#define a3_2 rwork[58]

#define d3_2 rwork[59]

#define d4_2 rwork[60]

#define x2_2 rwork[61]

#define x3_2 rwork[62]

#define y3_2 rwork[63]

#define z4_2 rwork[64]

#define y5_2 rwork[65]

#define z6_2 rwork[66]

#define bm1 rwork[67] /* Friccin viscosa */


#define bm2 rwork[68]





#define Red1 rwork[73]

#define Red2 rwork[74] /* Reductoras */





#define FC1 rwork[79] /* Friccin de Coulomb */

#define FC2 rwork[80]





#define Coulomb(i) rwork[i+79]

#define yant(i) rwork[85+i]

#define ffric(i) rwork[91+i]

#define FLAG_INIT iwork[0]

#define FLAG_DEBUG(i) rwork[97+i]

/* Entradas y estados */

#define M(i) (*uPtrs[i])

#define q1 xCont[ 0]






#define qp1 xCont[ 6]




#define qp5 xCont[10]

#define qp6 xCont[11]

#define POS_INI(S) ssGetSFcnParam(S,0) /* Posicion inicial del robot del robot*/

#define G(S) ssGetSFcnParam(S,1) /* Aceleracion de la gravedad */

#define ENABLEFRIC(S) ssGetSFcnParam(S,2) /* Habilitar friccin */


static void mdlInitializeSizes(SimStruct *S)

{

ssSetNumContStates( S, 12);

ssSetNumDiscStates( S, 0);

if (!ssSetNumInputPorts(S, 1)) return; /*Entradas*/

ssSetInputPortWidth(S, 0, 6);

ssSetInputPortDirectFeedThrough(S, 0, 0);

if (!ssSetNumOutputPorts(S,3)) return; /*Salidas*/

ssSetOutputPortWidth(S, 0, 6); /*Posicion*/

ssSetOutputPortWidth(S, 1, 6); /*Velocidad*/

ssSetOutputPortWidth(S, 2, 6); /*Velocidad*/

ssSetNumSFcnParams(S, 3);

if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {

return; /*Si faltan parametros se da mensaje */

}

ssSetNumSampleTimes( S, 1); /* Numero de muestreos */

ssSetNumRWork( S, 103); /* Vector de numeros reales */

ssSetNumIWork( S, 1); /* Vector de numeros enteros */

ssSetNumPWork( S, 0); /* Vector de punteros */

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); /*Automatico segun bloque anterior*/

ssSetOffsetTime(S, 0, 0.0);

}

#define MDL_INITIALIZE_CONDITIONS

/*

* mdlInitializeConditions - Inicializa estados y parametros del robot

*


*/

static void mdlInitializeConditions(SimStruct *S)

{

int i;

real_T *rwork = ssGetRWork(S);

int_T *iwork = ssGetIWork(S);

real_T *xCont = ssGetContStates(S);

real_T *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 = 0; /* Flag de inicializacin a cero */

mE = 0 ;/* Masa opcional garra Adicional */

Jm1 = 0.00044 ; /*Inercias motores*/

Jm2 = 0.00044 ;

Jm3 = 0.00014 ;

Jm4 = 0.000041 ;

Jm5 = 0.000017 ;

Jm6 = 0.000017 ;

Red1 = 121 ; /*Reductoras*/

Red2 = 153 ;

Red3 = 105 ;

Red4 = 59 ;

Red5 = 80 ;

Red6 = 50 ;

a1 = 0.260 ; /*Distancias constantes*/

a2 = 0.710 ;

a3 = 0.150 ;

d3 = 0.040 ;

d4 = 0.650 ;

x1 = 0.0839 ; /*Centros de masa*/

z1 = -0.0832 ;

x2 = 0.2286 ;


z2 = 0.1876 ;

x3 = 0.1717 ;

y3 = 0.0246 ;

z3 = 0.0027 ;

z4 = -0.1945 ;

y5 = 0.0317 ;

z6 = 0.050 ;

Izz1 = 1.1325 ; /*Inercias*/

Ixx2 = 0.2656 ;

Iyy2 = 3.4349 ;

Izz2 = 3.5202 ;

Ixx3 = 1.0198 ;

Iyy3 = 0.9982 ;

Izz3 = 1.6442 ;

Ixx4 = 0.8301 ;

Iyy4 = 0.7995 ;

Izz4 = 0.1202 ;

Ixx5 = 0.0201 ;

Iyy5 = 0.0086 ;

Izz5 = 0.0236 ;

Ixx6 = 0.0100 ;

Iyy6 = 0.0100 ;

Izz6 = 0.0050 ;

m1 = 38.65 ; /*Masas*/

m2 = 51.800 ;

m3 = 84.1 ;

m4 = 33.89 ;

m5 = 7.36 ;

m6 = 5.00 ;

bm1 = 40; /*Friccin viscosa total*/

bm2 = 30;

bm3 = 20;

bm4 = 10;

bm5 = 6;

bm6 = 4.5;


FC1 = 60 ; /*Friccion coulomb*/

FC2 = 50 ;

FC3 = 40 ;

FC4 = 30 ;

FC5 = 24 ;

FC6 = 14 ;

/* 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 = x2*x2;

x3_2 = x3*x3;

y3_2 = y3*y3;

z4_2 = z4*z4;

y5_2 = y5*y5;

z6_2 = 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 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];

}

}

/* Calculo de senos y cosenos */

/*----------------------------------*/

s2 = sin(q2);

s3 = sin(q3);

s4 = sin(q4);

s5 = sin(q5);

s6 = sin(q6);

c2 = cos(q2);

c3 = cos(q3);

c4 = cos(q4);

c5 = cos(q5);

c6 = 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

/*


* mdlDerivatives - compute the derivatives

*

*/

static void mdlDerivatives(SimStruct *S)

{

int i,j, flagfric[6];

InputRealPtrsType uPtrs;

real_T *y;




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 f[6];

real_T z_1, z_2, z_3, z_4, z_5, z_6;

real_T x[6];

real_T fP_1, fP_2, fP_3, fP_4, fP_5, fP_6;

real_T fG[6];

real_T fricc[6],sig[6];

real_T 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);



if (FLAG_INIT == 0) {

/* Calculo de parametros */

/*----------------------------------*/

g = mxGetPr(G(S))[0];

k1 = -Izz6;

k2 = m5*y5*a1+m6*z6*a1;

k3 = m5*y5*d4+m6*z6*d4;

k4 = -m5*y5*d3-m6*z6*d3;

k5 = m5*y5*a3+m6*z6*a3;

k6 = m5*a2*y5+m6*a2*z6;

k7 = -Ixx2+Iyy2;

k8 = -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 = -m6*a2_2-m2*x2_2-m3*a2_2-m4*a2_2-m5*a2_2;

k19 = m3*y3*z3+m3*y3*d3+m5*d4*d3+m6*d4*d3+m4*d3*d4+m4*d3*z4;

k20 = m5*d3*a2+m3*d3*a2+m6*d3*a2+m4*d3*a2+m3*z3*a2+m2*x2*z2;

k21 = -Ixx5-Iyy6+Izz6+Iyy5-m6*z6_2-m5*y5_2;

k22 = Iyy4-Ixx4+Ixx6-Iyy5+Izz5-Izz6+m6*z6_2+m5*y5_2;

k23 = 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_2-

2*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;


FLAG_INIT = 1; /* Inicializacion realizada */

}

/* Matriz de inercias */

/*----------------------------------*/

s2_2 = c2*c2;

c2_2 = c2*c2;

s3_2 = s3*s3;

c3_2 = c3*c3;

s4_2 = s4*s4;

c4_2 = c4*c4;

c5_2 = c5*c5;

s5_2 = s5*s5;

c6_2 = c6*c6;

s6_2 = s6*s6;

c23_2 = c23*c23;

s23_2 = s23*s23;

m[1][1] = (((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+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_2-

k22-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;

m[1][2] = (-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-k22-

k8*c6_2)*s4+k4*s5)*c4+k14+k8*s6*c6*c5+k5*s4*s5)*s23+(k20+

k6*s5*s4)*s2;

m[2][2] = (-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;

m[1][3] = (-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-k22-

k8*c6_2)*s4+k4*s5)*c4+k14+k8*s6*c6*c5+k5*s4*s5)*s23;

m[2][3] = (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*s4-

2*k5*s5)*c4+(-k21-k8*c6_2)*c5_2+2*k3*c5+k23;

m[3][3] = ((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+k23+Jm3*Red3*Red3;

m[1][4] = (k5*c4*s5+k4*s4*s5+(-k21-k8*c6_2)*c5_2+k13+k8*c6_2)*c23+


(((k21+k8*c6_2)*s5*c5-k3*s5)*c4-k8*s6*s5*c6*s4)*s23+

k2*c4*s5+k6*s5*c2*c4;

m[2][4] = k6*s5*s4*s3+k8*c6*s5*s6*c4+((k21+k8*c6_2)*s5*c5-k3*s5)*s4;

m[3][4] = k8*c6*s5*s6*c4+((k21+k8*c6_2)*s5*c5-k3*s5)*s4;

m[4][4] = (k21+k8*c6_2)*c5_2-k13-k8*c6_2+Jm4*Red4*Red4;

m[1][5] = (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;

m[2][5] = -k6*c2*c23*s5-k6*s2*s23*s5-k6*c5*c4*s3+(k3*c5+k8*c6_2-

k10)*c4-k8*s6*c6*c5*s4-k5*s5;

m[3][5] = (k3*c5+k8*c6_2-k10)*c4-k8*s6*c6*c5*s4-k5*s5;

m[4][5] = k8*s6*c6*s5;

m[5][5] = -k10+k8*c6_2+Jm5*Red5*Red5;

m[1][6] = k1*c23*c5-k1*s5*c4*s23;

m[2][6] = -k1*s5*s4;

m[3][6] = -k1*s5*s4;

m[4][6] = -k1*c5;

m[5][6] = 0;

m[6][6] = -k1+Jm6*Red6*Red6;

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


{

/* Vector de friccin */

if (Enfric==0.0)

{

for(i=0;i<6;i++)

fricc[i]=0.0;

}

else


{

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

/* 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 = sqrt(m[1][1]);

R2_1 = m[1][2]/R1_1;

R2_2 = sqrt(m[2][2]-R2_1*R2_1);

R3_1 = m[1][3]/R1_1 ;

R3_2 = (m[2][3]-R2_1*R3_1)/R2_2;

R3_3 = sqrt(m[3][3]-R3_1*R3_1-R3_2*R3_2);

R4_1 = m[1][4]/R1_1;

R4_2 = (m[2][4]-R2_1*R4_1)/R2_2;

R4_3 = (m[3][4]-R3_1*R4_1-R3_2*R4_2)/R3_3;

R4_4 = sqrt(m[4][4]-R4_1*R4_1-R4_2*R4_2-R4_3*R4_3;

R5_1 = m[1][5]/R1_1 ;

R5_2 = (m[2][5]-R2_1*R5_1)/R2_2;

R5_3 = (m[3][5]-R3_1*R5_1-R3_2*R5_2)/R3_3;

R5_4 = (m[4][5]-R4_1*R5_1-R4_2*R5_2-R4_3*R5_3)/R4_4;

R5_5 = sqrt(m[5][5]-R5_1*R5_1-R5_2*R5_2-R5_3*R5_3-R5_4*R5_4);

R6_1 = m[1][6]/R1_1 ;

R6_2 = (m[2][6]-R2_1*R6_1)/R2_2 ;

R6_3 = (m[3][6]-R3_1*R6_1-R3_2*R6_2)/R3_3 ;

R6_4 = (m[4][6]-R4_1*R6_1-R4_2*R6_2-R4_3*R6_3)/R4_4;

R6_5 = (-R5_1*R6_1-R5_2*R6_2-R5_3*R6_3-R5_4*R6_4)/R5_5;

R6_6 = 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 = f[0]/R1_1;

z_2 = (f[1]-R2_1*z_1)/R2_2;

z_3 = (f[2]-R3_1*z_1-R3_2*z_2)/R3_3;

z_4 = (f[3]-R4_1*z_1-R4_2*z_2-R4_3*z_3)/R4_4;

z_5 = (f[4]-R5_1*z_1-R5_2*z_2-R5_3*z_3-R5_4*z_4)/R5_5;

z_6 = (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 */


x[5] = z_6/R6_6;

x[4] = (z_5-R6_5*x[5])/R5_5;

x[3] = (z_4-R5_4*x[4]-R6_4*x[5])/R4_4;

x[2] = (z_3-R4_3*x[3]-R5_3*x[4]-R6_3*x[5])/R3_3;

x[1] = (z_2-R3_2*x[2]-R4_2*x[3]-R5_2*x[4]-R6_2*x[5])/R2_2;

x[0] = (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] = xCont[8];

dx[3] = xCont[9];

dx[4] = xCont[10];

dx[5] = xCont[11];

/* Aceleraciones */


{

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


/*

* Calcula el par de Coriolis debido a las velocidades

* de los distintos elementos del robot.

*

* autor: Carlos Perez Fernandez

* fecha: 30-4-99

*/

#define S_FUNCTION_NAME rm10modelcor


#include <math.h>


/* Variables del espacio de trabajo */

#define mE rwork[ 0]










#define d3 rwork[ 10]

#define d4 rwork[ 11]

#define x1 rwork[ 12]

#define z1 rwork[ 13]




#define y3 rwork[ 17]



#define y5 rwork[ 20]


#define Izz1 rwork[ 22]

#define Ixx2 rwork[ 23]

#define Iyy2 rwork[ 24]















#define m1 rwork[ 38]






#define s2 rwork[ 44]

#define c2 rwork[ 45]











#define a1_2 rwork[ 56]



#define d3_2 rwork[ 59]

#define d4_2 rwork[ 60]

#define x2_2 rwork[ 61]

#define x3_2 rwork[ 62]

#define y3_2 rwork[ 63]

#define z4_2 rwork[ 64]

#define y5_2 rwork[ 65]

#define z6_2 rwork[ 66]








#define FLAG_INIT iwork[0]

/* Entradas y estados */

/************************************/

#define Q(i) (*uPtrs[ 0][i-1]) /* Posicion */

#define QD(i) (*uPtrs[ 1][i-1]) /* Velocidad */

#define C(i,j) mc[i-1][j-1]

#define ENABLE(S) ssGetSFcnParam(S,0)

/* Flag para desactivar bloque */


{

ssSetNumContStates( S, 0);

ssSetNumDiscStates( S, 0);







ssSetOutputPortWidth(S, 0, 6); /*Par*/



return; /*Si faltan parametros se da mensaje */

}








}


/*

* mdlInitializeSampleTimes - Inicializa muestreos *

*/


{

ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);

/*Automatico segun bloque preceden*/


}


/*


*

*/


{



FLAG_INIT = 0; /* Flag de inicializaci\'{o}n a cero */

a1 = 0.260 ; /*Distancias constantes*/

a2 = 0.710 ;

a3 = 0.150 ;

d3 = 0.040 ;

d4 = 0.650 ;

x1 = 0.0839 ; /*Centros de masa*/

z1 = -0.0832 ;

x2 = 0.2286 ;

z2 = 0.1876 ;

x3 = 0.1717 ;

y3 = 0.0246 ;

z3 = 0.0027 ;

z4 = -0.1945 ;

y5 = 0.0317 ;

z6 = 0.050 ;


Izz1 = 1.1325 ; /*Inercias*/

Ixx2 = 0.2656 ;

Iyy2 = 3.4349 ;

Izz2 = 3.5202 ;

Ixx3 = 1.0198 ;

Iyy3 = 0.9982 ;

Izz3 = 1.6442 ;

Ixx4 = 0.8301 ;

Iyy4 = 0.7995 ;

Izz4 = 0.1202 ;

Ixx5 = 0.0201 ;

Iyy5 = 0.0086 ;

Izz5 = 0.0236 ;

Ixx6 = 0.0100 ;

Iyy6 = 0.0100 ;

Izz6 = 0.0050 ;

m1 = 38.65 ; /*Masas*/

m2 = 51.800 ;

m3 = 84.1 ;

m4 = 33.89 ;

m5 = 7.36 ;

m6 = 5.00 ;

/* 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 = x2*x2;

x3_2 = x3*x3;

y3_2 = y3*y3;

z4_2 = z4*z4;

y5_2 = y5*y5;

z6_2 = z6*z6;


}

/*


*

*/


{

double h = 0.5;

int i,j;

real_T *y;

InputRealPtrsType uPtrs[ 2];

real_T 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 enable = mxGetPr(ENABLE(S))[0];

uPtrs[ 0] = ssGetInputPortRealSignalPtrs(S,0);

/*Puntero a vector posicion*/

uPtrs[ 1] = ssGetInputPortRealSignalPtrs(S,1);

/*Puntero a vector velocidad*/


if (FLAG_INIT == 0) {

/* Calculo de parametros */

/*----------------------------------*/

k1 = -Izz6;

k2 = m5*y5*a1+m6*z6*a1;

k3 = m5*a2*y5+m6*a2*z6;

k4 = m5*y5*d4+m6*z6*d4;

k5 = m5*y5*a3+m6*z6*a3;

k6 = m5*y5*d3+m6*z6*d3;

k7 = -Ixx6+Iyy6;


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_2-

m3*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 = cos(Q(2));

c3 = cos(Q(3));

c4 = cos(Q(4));

c5 = cos(Q(5));

c6 = cos(Q(6));

s23 = sin(Q(2)+Q(3));


c23 = cos(Q(2)+Q(3));

s2_2 = c2*c2;

c2_2 = c2*c2;

s3_2 = s3*s3;

c3_2 = c3*c3;

s4_2 = s4*s4;

c4_2 = c4*c4;

c5_2 = c5*c5;

s5_2 = s5*s5;

c6_2 = c6*c6;

s6_2 = s6*s6;

c23_2 = c23*c23;

s23_2 = s23*s23;

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

2*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(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*s5-

2*k7*c6*c5*s4*s6)*c4+(k7*c6_2-k21)*c5_2+2*k4*c5-k25-2*k7*c6_2)*s23-

k2*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*s3-

k12+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)*c23-

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(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)*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(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+


(-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*s5-

2*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)*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*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*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,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*s5-

2*k7*c6*c5*s4*s6)*c4+(k7*c6_2-k21)*c5_2+2*k4*c5-k25-2*k7*c6_2)*s23-

k2*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*s3-

k12+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)*s4-


((-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)*s4-


((-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-

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

k3*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_2-

k21)*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*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+(-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*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-


.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)*c4-

k7*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)*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(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_2-

k21)*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_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(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*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(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-


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

k7*c6*c5*s4_2*s6)*QD(3)+(k3*s5*c4*s3+((-k7*c6_2+k21)*s5*c5-

k4*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_2-


k4*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_2-


k4*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*c5-

k3*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+


(-.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*c4-

k7*c5_2*c6*s6)*QD(2)+((k7*c6*s6+k7*c5_2*c6*s6)*c4_2+(-k7*s6_2+

k7*c6_2)*c5*s4*c4-k7*c5_2*c6*s6)*QD(3)+((.5*k7*s6_2-.5*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_2-

k21)*s5*c5+k4*s5)*c4-k7*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+(k17*s3-

k12+k3*c5)*c3)*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+(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*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(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*c4-

k7*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*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(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_2-

k22)*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*s5-

k7*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*c5-

k7*c6*s5*s6*s4)*c4+(-k7*c6_2+k21)*s5*c5-k4*s5)*QD(2)+((k7*c6_2-


k4*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*s4-

k4*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+


(-.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*c4-

k7*c5_2*c6*s6)*QD(2)+((k7*c6*s6+k7*c5_2*c6*s6)*c4_2+(-k7*s6_2+

k7*c6_2)*c5*s4*c4-k7*c5_2*c6*s6)*QD(3)+((-.5*k1+.5*k7*s6_2-

.5*k7*c6_2)*s5*c4+k7*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_2-

k21)*s5*c5+k4*s5)*s4)*s23-k2*s4*s5-k3*c2*s4*s5)*c23+

k7*c6*c5*c4_2*s6+(((k7*c6_2-k21)*c5_2+k7*c6_2-k22)*s4-k6*s5)*c4-

k7*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*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(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*s6-

k7*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)*s23-

k3*s5*c4*s2)*QD(1)+(-k3*c2*c23*s4*s5-k3*s2*s4*s23*s5-

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)+(-.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-


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

k21)*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_2-

k7*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*c4-

k7*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+


k7*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*c4-

k7*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_2-

k21)*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+


k7*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_2-

k21)*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);

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

k7*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_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(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(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*s6-

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

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,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*s6-

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


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; i++)

{

y[i]=0.0;

if (enable!=0.0)

for(j=0; j<6; j++)

y[i]=y[i]+C(i+1,j+1)*QD(j+1);

}

}


{

}

#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

* fecha: 30-4-99

*/

#define S_FUNCTION_NAME traj52


#include <math.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 Estado(i) rwork[36+i]

#define Tfinal rwork[ 42]

#define Time_ini rwork[ 43]

#define Time rwork[ 44]

#define TimeArt(i) rwork[ 45+i]

#define Moverant iwork[ 0]

#define MOVING iwork[ 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]) /* Posici\'{o}n Final */

#define QACT(indice) (*uPtrs[1][indice]) /* Posici\'{o}n Actual */

#define MOVER (*uPtrs[2][0]) /* Orden de Movimiento */



{

ssSetNumContStates( S, 0); /* number of continuous states */

ssSetNumDiscStates( S, 0); /* number of discrete states */


ssSetInputPortWidth(S, 0, 6); /*Posicion Destino*/

ssSetInputPortWidth(S, 1, 6); /*Posicion Actual */

ssSetInputPortWidth(S, 2, 1); /*Orden de Movimiento*/





ssSetOutputPortWidth(S, 0, 6); /*Posicion Referencia*/

ssSetOutputPortWidth(S, 1, 6); /*Velocidad Referencia*/

ssSetOutputPortWidth(S, 2, 6); /*Aceleraci\'{o}n Referencia*/

ssSetOutputPortWidth(S, 3, 1); /*Final de la trayectoria*/



return; /* Si faltan parametros se da mensaje */

}








}

/*

* mdlInitializeSampleTimes - Inicializa el vector de tiempos *

*/


{

real_T *TSample = mxGetPr(SAMPLE(S)); /* Puntero a parametro frecuencia */

ssSetSampleTime(S, 0, *TSample); /*Automatico segun bloque anterior*/


}



/*


*

*/


{

int i;



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;




Time_ini= 0;

Moverant = 0;

for(i=0;i<6;i++)

Estado(i)=posini[i];

MOVING = 0;

}

#endif


/*


*

*/


{

int i,j;

real_T *y;

InputRealPtrsType uPtrs[3];



real_T *ptqmax = mxGetPr(QMAX(S));

uPtrs[0] = ssGetInputPortRealSignalPtrs(S,0);



if ((Moverant==0)&&(MOVER==1)&&(MOVING==0)) /* Si detecta flanco */

{

Time_ini=ssGetT(S); /* Capturamos el tiempo inicial */

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) /* Escogemos el mayor tiempo */

Tfinal=TimeArt(i);

}

for(i=0;i<6;i++) /* 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[i]=0;

for(j=0;j<4;j++)

{

y[i]+=(j+2)*(j+1)*A(i,j+2)*pow(Time,j); /*Aceleracion*/

}

}

if (Time>=Tfinal)

MOVING=0;

}

else /* Si no hay movimiento velocidades nulas */

{


for(i=0;i<6;i++)

{

y[i]=0;

}


for(i=0;i<6;i++)

{

y[i]=0;

}

}

/* Asignamos las salidas del bloque */


for(i=0;i<6;i++)

{

y[i]=Estado(i);

}


/* Indicaci\'{o}n fin de trayectoria */


if (MOVING==1) *y=0.0;

else *y=1;

Moverant=MOVER;

}

/*

* mdlTerminate - Se llama una vez finaliza la simulaci\'{o}n *

*/


{

}

#ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */

#include "simulink.c" /* MEX-file interface mechanism */

#else

#include "cg_sfun.h" /* Code generation registration function */

#endif


/*

* 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

* Fecha: 11-Julio-1999

*/

#define S_FUNCTION_NAME cineinv



#include "math.h"

#include "stdlib.h"

/*Parametros geom\'{e}tricos */

#define RGEN_dB mxGetPr(ssGetSFcnParam(S,0))[0]

#define RGEN_a1 mxGetPr(ssGetSFcnParam(S,1))[0]


#define RGEN_d3 mxGetPr(ssGetSFcnParam(S,3))[0]


#define RGEN_d4 mxGetPr(ssGetSFcnParam(S,5))[0]

#define RGEN_dG mxGetPr(ssGetSFcnParam(S,6))[0]

#define PI 3.14159265358979

#define LIM_ART_MIN_1 (double) -2.3572 /* -135 Grad */

#define LIM_ART_MAX_1 (double) +2.3572 /* +135 Grad */


#define LIM_ART_MAX_2 (double) +0.1745 /* 10 Grad */





#define LIM_ART_MIN_5 (double) -1.5708 /* - 90 Grad */

#define LIM_ART_MAX_5 (double) +1.5708 /* + 90 Grad */



#define FLOAT_EPSILON 1e-8

/* Asigna entradas */


/*----------------------------------*/

#define n(indice) (*uPtrs[0][indice]) /* Vector n */

#define s(indice) (*uPtrs[1][indice]) /* Vector s */

#define a(indice) (*uPtrs[2][indice]) /* Vector a */

#define p(indice) (*uPtrs[3][indice]) /* Posici\'{o}n xyz */

#define pact(indice) (*uPtrs[4][indice]) /* Posici\'{o}n actual */


{

int i;

ssSetNumContStates( S, 0);/* N\'{u}mero de estados t continuo */

ssSetNumDiscStates( S, 0); /* N\'{u}mero de estados t discreto */


ssSetInputPortWidth(S, 0, 3); /*Vector n*/

ssSetInputPortWidth(S, 1, 3); /*Vector s*/

ssSetInputPortWidth(S, 2, 3); /*Vector a*/

ssSetInputPortWidth(S, 3, 3); /*Posici\'{o}n xyz*/

ssSetInputPortWidth(S, 4, 6); /*Posici\'{o}n Actual*/

for(i=0;i<5;i++)

ssSetInputPortDirectFeedThrough(S, i, 1);


ssSetOutputPortWidth(S, 0, 6); /*Posici\'{o}n Soluci\'{o}n*/

ssSetOutputPortWidth(S, 1, 1); /*Error trayectoria*/

ssSetOutputPortWidth(S, 2, 1); /*debug*/



return; /* Si faltan parametros se da mensaje */

}








}



{

ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);


}


/*


*

*/


{

}

/*


*

*/


{

int i, j, k;

int minindex = 0;

real_T *output;

double dtemp, dep1[4], A[4], den[4];

double p=1;

double minnorm = 0;

double c1[4],s1[4],c2[4],s2[4],c3[4],s3[4],c23[4],s23[4];

double c4nf[4],s4nf[4],c5nf[4],s5nf[4],c6nf[4],s6nf[4];

double x, y, z; /* Posici\'{o}n 06 */

double 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};

double 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);


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_a2-

RGEN_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);


}

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)

{

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;

}

}

}

}


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

{


for(i=0;i<6;i++)

output[i]=pact(i);

*outputerror=1; /* Posible Discontinuidad en la soluci\'{o}n */

}

}

else

{


for(i=0;i<6;i++)

output[i]=pact(i);

*outputerror=1; /* No hay soluci\'{o}n en la soluci\'{o}n */

}

}

else

{


for(i=0;i<6;i++)

output[i]=pact(i);

*outputerror=1; /* No hay soluci\'{o}n posible en el espacio de trabajo*/

}

}


{

}


#ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */

#include "simulink.c" /* MEX-file interface mechanism */

#else

#include "cg_sfun.h" /* Code generation registration function */

#endif


Bibliograf��a

[1] An��bal Ollero Baturone. Apuntes de Rob�otica. Universidad de Sevilla, 1995.

[2] Peter I. Corke. Manual de Referencia de Robotic Toolbox. 1986.

[3] J. J. Craig. Introduction to Robotics. Addison Wesley, 1989.

[4] Analog Devices. AD2S90 DataSheet Revision D. Analog Devices, 1999.

[5] dSPACE Gmbh. Real Time Interface Implementation Guide. 1998.

[6] Moog Gmbh. T158-11X Controller User Manual. Moog Gmbh, 1990.

[7] Moog Gmbh. Cat�alogo de Motores Tecnolog��a Brushless. Moog Gmbh, 1993.

[8] C.S.G. Lee K.S. Fu, R.C. Gonzlez. Rob�otica: Control, detecci�on, visi�on e inteligencia.

McGrawHill, 1989.

[9] M. Vidyasagar Mark W. Spong. Robot Dynamics and Control. John Wiley And Sons,

1989.

[10] Richard. P. Paul. Robots Manipulators. Mathematics, Programming and Control. The

MIT Press, 1981.

[11] M.~Negin R.R. Klafter, T.A. Chmielewski. Robotic Engineering, And Integrated Ap-

proach. PrenticeHall, 1989.

[12] Carlos J. Ma~nas S�anchez. Programa para el manejo y programaci�on del Robot RM10.

E.T.S Universidad de Sevilla, 1996.

[13] Li Slotine. Applied Nonlinear Control. Prentice Hall.

[14] s.r.l. System Robot. System Robot RM10 Mantenaince Manual. 1992.

[15] s.r.l. System Robot. System Robot RM10 User Manual. 1992.

155

Documents

Simulación y Control de un robot manipulador