simulación y control de un robot manipulador
Post on 30-Jul-2015
119 Views
Preview:
TRANSCRIPT
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.4 Caracter��stica de fricci�on experimental del eje 5. . . . . . . . . 56
3.5 Caracter��stica de fricci�on experimental del eje 4. . . . . . . . . 56
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.
12 1 Introducci�on
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.
14 1 Introducci�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.
1.3 Descripci�on del robot industrial RM-10 15
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:
16 1 Introducci�on
� 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.
1.3 Descripci�on del robot industrial RM-10 17
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.
18 1 Introducci�on
� 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.
1.3 Descripci�on del robot industrial RM-10 19
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.
20 1 Introducci�on
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
22 1 Introducci�on
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
26 2 Cinem�atica del robot RM-10
�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
28 2 Cinem�atica del robot RM-10
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.
30 2 Cinem�atica del robot RM-10
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:
32 2 Cinem�atica del robot RM-10
�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 :
2.4 Problema cinem�atico inverso 33
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:
34 2 Cinem�atica del robot RM-10
�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)
2.4 Problema cinem�atico inverso 35
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
36 2 Cinem�atica del robot RM-10
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)
38 2 Cinem�atica del robot RM-10
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.
40 2 Cinem�atica del robot RM-10
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)
44 3 Din�amica del robot RM-10
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
3.2 Modelo Euler-Lagrange 45
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)
46 3 Din�amica del robot RM-10
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;
3.2 Modelo Euler-Lagrange 47
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.
48 3 Din�amica del robot RM-10
> 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)):
3.2 Modelo Euler-Lagrange 49
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:
50 3 Din�amica del robot RM-10
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:
52 3 Din�amica del robot RM-10
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.
54 3 Din�amica del robot RM-10
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:
56 3 Din�amica del robot RM-10
−1.5 −1 −0.5 0 0.5 1 1.5 2−60
−50
−40
−30
−20
−10
0
10
20
30
40
�
_q
Figura 3.4: Caracter��stica de fricci�on experimental del eje 5.
−2.5 −2 −1.5 −1 −0.5 0 0.5 1 1.5 2−30
−20
−10
0
10
20
30
�
_q
Figura 3.5: Caracter��stica de fricci�on experimental del eje 4.
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
58 3 Din�amica del robot RM-10
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
60 3 Din�amica del robot RM-10
+
- - -
� �_���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
62 3 Din�amica del robot RM-10
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
66 4 Control de robot RM-10
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
4.2 Control lineal 67
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
68 4 Control de robot RM-10
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
4.2 Control lineal 69
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
70 4 Control de robot RM-10
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.
4.2 Control lineal 71
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
72 4 Control de robot RM-10
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
4.2 Control lineal 73
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
74 4 Control de robot RM-10
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 Control lineal 75
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
Resultados de simulaci�on
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.
76 4 Control de robot RM-10
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
78 4 Control de robot RM-10
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
4.3 Control por Par Calculado 79
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
80 4 Control de robot RM-10
+
+
� 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)
4.3 Control por Par Calculado 81
+
+
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.
Resultados de simulaci�on
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
82 4 Control de robot RM-10
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
4.3 Control por Par Calculado 83
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
84 4 Control de robot RM-10
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
4.3 Control por Par Calculado 85
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
86 4 Control de robot RM-10
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.
4.3 Control por Par Calculado 87
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
88 4 Control de robot RM-10
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
4.3 Control por Par Calculado 89
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
90 4 Control de robot RM-10
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
98 B Tarjeta de conversi�on resolver-encoder
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.
100 B Tarjeta de conversi�on resolver-encoder
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
102 B Tarjeta de conversi�on resolver-encoder
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 Jm3 rwork[ 3]
#define Jm4 rwork[ 4]
#define Jm5 rwork[ 5]
#define Jm6 rwork[ 6]
#define a1 rwork[ 7] /* Distancias */
#define a2 rwork[ 8]
#define a3 rwork[ 9]
#define d3 rwork[10]
#define d4 rwork[11]
#define x1 rwork[12]
#define z1 rwork[13]
#define x2 rwork[14]
#define z2 rwork[15]
#define x3 rwork[16]
#define y3 rwork[17]
#define z3 rwork[18]
#define z4 rwork[19]
#define y5 rwork[20]
#define z6 rwork[21]
#define Izz1 rwork[22] /* Inercias */
#define Ixx2 rwork[23]
D C�odigo fuente funciones Simulink 107
#define Iyy2 rwork[24]
#define Izz2 rwork[25]
#define Ixx3 rwork[26]
#define Iyy3 rwork[27]
#define Izz3 rwork[28]
#define Ixx4 rwork[29]
#define Iyy4 rwork[30]
#define Izz4 rwork[31]
#define Ixx5 rwork[32]
#define Iyy5 rwork[33]
#define Izz5 rwork[34]
#define Ixx6 rwork[35]
#define Iyy6 rwork[36]
#define Izz6 rwork[37]
#define m1 rwork[38] /* Masas */
#define m2 rwork[39]
#define m3 rwork[40]
#define m4 rwork[41]
#define m5 rwork[42]
#define m6 rwork[43]
#define s2 rwork[44] /* Senos y cosenos */
#define c2 rwork[45]
#define s3 rwork[46]
#define c3 rwork[47]
#define s4 rwork[48]
#define c4 rwork[49]
#define s5 rwork[50]
#define c5 rwork[51]
#define s6 rwork[52]
#define c6 rwork[53]
#define s23 rwork[54]
#define c23 rwork[55]
#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 */
108 D C�odigo fuente funciones Simulink
#define bm2 rwork[68]
#define bm3 rwork[69]
#define bm4 rwork[70]
#define bm5 rwork[71]
#define bm6 rwork[72]
#define Red1 rwork[73]
#define Red2 rwork[74] /* Reductoras */
#define Red3 rwork[75]
#define Red4 rwork[76]
#define Red5 rwork[77]
#define Red6 rwork[78]
#define FC1 rwork[79] /* Friccin de Coulomb */
#define FC2 rwork[80]
#define FC3 rwork[81]
#define FC4 rwork[82]
#define FC5 rwork[83]
#define FC6 rwork[84]
#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 q2 xCont[ 1]
#define q3 xCont[ 2]
#define q4 xCont[ 3]
#define q5 xCont[ 4]
#define q6 xCont[ 5]
#define qp1 xCont[ 6]
#define qp2 xCont[ 7]
#define qp3 xCont[ 8]
#define qp4 xCont[ 9]
#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 */
D C�odigo fuente funciones Simulink 109
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
*
110 D C�odigo fuente funciones Simulink
*/
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 ;
D C�odigo fuente funciones Simulink 111
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;
112 D C�odigo fuente funciones Simulink
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 *rwork = ssGetRWork(S);
int_T *iwork = ssGetIWork(S);
real_T *xCont = ssGetContStates(S);
real_T g = mxGetPr(G(S))[0];
D C�odigo fuente funciones Simulink 113
/* 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
/*
114 D C�odigo fuente funciones Simulink
* mdlDerivatives - compute the derivatives
*
*/
static void mdlDerivatives(SimStruct *S)
{
int i,j, flagfric[6];
InputRealPtrsType uPtrs;
real_T *y;
real_T *rwork = ssGetRWork(S);
int_T *iwork = ssGetIWork(S);
real_T *xCont = ssGetContStates(S);
real_T *dx = ssGetdX(S);
real_T Enfric = mxGetPr(ENABLEFRIC(S))[0];
/* Declaraciones */
/*----------------------------------*/
static real_T g, forceac;
static
real_T k0, k1, k2, k3, k4, k5, k6, k7, k8, k9,
k10, k11, k12, k13, k14, k15, k16, k17, k18, k19,
k20, k21, k22, k23, k24, k25, k26;
static real_T m[7][7];
static real_T R1_1, R2_1, R2_2, R3_1, R3_2, R3_3, R4_1, R4_2, R4_3,
R4_4, R5_1, R5_2, R5_3, R5_4, R5_5, R6_1, R6_2, R6_3,
R6_4, R6_5, R6_6;
real_T 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);
y=ssGetOutputPortRealSignal(S,1);
D C�odigo fuente funciones Simulink 115
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;
116 D C�odigo fuente funciones Simulink
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+
D C�odigo fuente funciones Simulink 117
(((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 ;
if (mxGetPr(ENABLEFRIC(S))[0] == 1)
{
/* Vector de friccin */
if (Enfric==0.0)
{
for(i=0;i<6;i++)
fricc[i]=0.0;
}
else
118 D C�odigo fuente funciones Simulink
{
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;
}
D C�odigo fuente funciones Simulink 119
/* 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 */
120 D C�odigo fuente funciones Simulink
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 */
if (mxGetPr(ENABLEFRIC(S))[0] == 1)
{
for(i=0;i<6;i++)
if (ffric(i)==0)
dx[6+i]=x[i];
else
dx[6+i]=0.0;
}
else
{
for(i=0;i<6;i++)
dx[6+i]=x[i];
}
for(i=0;i<6;i++)
yant(i)=y[i];
}
static void mdlTerminate(SimStruct *S)
{
}
#ifdef MATLAB_MEX_FILE
#include "simulink.c"
D C�odigo fuente funciones Simulink 121
#else
#include "cg_sfun.h"
#endif
122 D C�odigo fuente funciones Simulink
/*
* 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
#define S_FUNCTION_LEVEL 2
#include <math.h>
#include "simstruc.h"
/* Variables del espacio de trabajo */
#define mE rwork[ 0]
#define Jm1 rwork[ 1]
#define Jm2 rwork[ 2]
#define Jm3 rwork[ 3]
#define Jm4 rwork[ 4]
#define Jm5 rwork[ 5]
#define Jm6 rwork[ 6]
#define a1 rwork[ 7]
#define a2 rwork[ 8]
#define a3 rwork[ 9]
#define d3 rwork[ 10]
#define d4 rwork[ 11]
#define x1 rwork[ 12]
#define z1 rwork[ 13]
#define x2 rwork[ 14]
#define z2 rwork[ 15]
#define x3 rwork[ 16]
#define y3 rwork[ 17]
#define z3 rwork[ 18]
#define z4 rwork[ 19]
#define y5 rwork[ 20]
#define z6 rwork[ 21]
#define Izz1 rwork[ 22]
#define Ixx2 rwork[ 23]
#define Iyy2 rwork[ 24]
#define Izz2 rwork[ 25]
#define Ixx3 rwork[ 26]
#define Iyy3 rwork[ 27]
D C�odigo fuente funciones Simulink 123
#define Izz3 rwork[ 28]
#define Ixx4 rwork[ 29]
#define Iyy4 rwork[ 30]
#define Izz4 rwork[ 31]
#define Ixx5 rwork[ 32]
#define Iyy5 rwork[ 33]
#define Izz5 rwork[ 34]
#define Ixx6 rwork[ 35]
#define Iyy6 rwork[ 36]
#define Izz6 rwork[ 37]
#define m1 rwork[ 38]
#define m2 rwork[ 39]
#define m3 rwork[ 40]
#define m4 rwork[ 41]
#define m5 rwork[ 42]
#define m6 rwork[ 43]
#define s2 rwork[ 44]
#define c2 rwork[ 45]
#define s3 rwork[ 46]
#define c3 rwork[ 47]
#define s4 rwork[ 48]
#define c4 rwork[ 49]
#define s5 rwork[ 50]
#define c5 rwork[ 51]
#define s6 rwork[ 52]
#define c6 rwork[ 53]
#define s23 rwork[ 54]
#define c23 rwork[ 55]
#define a1_2 rwork[ 56]
#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 Red1 rwork[67]
#define Red2 rwork[68]
#define Red3 rwork[69]
#define Red4 rwork[70]
#define Red5 rwork[71]
124 D C�odigo fuente funciones Simulink
#define Red6 rwork[72]
#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 */
static void mdlInitializeSizes(SimStruct *S)
{
ssSetNumContStates( S, 0);
ssSetNumDiscStates( S, 0);
if (!ssSetNumInputPorts(S, 2)) return; /*Entradas*/
ssSetInputPortWidth(S, 0, 6);
ssSetInputPortWidth(S, 1, 6);
ssSetInputPortDirectFeedThrough(S, 0, 1);
ssSetInputPortDirectFeedThrough(S, 1, 1);
if (!ssSetNumOutputPorts(S,1)) return; /*Salidas*/
ssSetOutputPortWidth(S, 0, 6); /*Par*/
ssSetNumSFcnParams(S, 1);
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
return; /*Si faltan parametros se da mensaje */
}
ssSetNumSampleTimes( S, 1); /* Numero de muestreos */
ssSetNumRWork( S, 73); /* 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);
}
D C�odigo fuente funciones Simulink 125
/*
* mdlInitializeSampleTimes - Inicializa muestreos *
*/
static void mdlInitializeSampleTimes(SimStruct *S)
{
ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
/*Automatico segun bloque preceden*/
ssSetOffsetTime(S, 0, 0.0);
}
#define MDL_INITIALIZE_CONDITIONS
/*
* mdlInitializeConditions - Inicializa estados y parametros del robot
*
*/
static void mdlInitializeConditions(SimStruct *S)
{
real_T *rwork = ssGetRWork(S);
int_T *iwork = ssGetIWork(S);
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 ;
126 D C�odigo fuente funciones Simulink
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;
D C�odigo fuente funciones Simulink 127
}
/*
* mdlOutputs - Calcula las salidas
*
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
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 *rwork = ssGetRWork(S);
int_T *iwork = ssGetIWork(S);
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*/
y=ssGetOutputPortRealSignal(S,0);
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;
128 D C�odigo fuente funciones Simulink
k8 = -Ixx2+Iyy2;
k9 = m6*a2*a3+m3*a2*x3+m5*a2*a3+m4*a2*a3;
k10 = -m5*y5_2-m6*z6_2-Izz5-Ixx6;
k11 = 2*m5*a1*a3+2*m3*x3*a1+2*m6*a1*a3+2*m4*a3*a1;
k12 = -m4*a2*d4-m4*a2*z4-m6*a2*d4-m3*a2*y3-m5*a2*d4;
k13 = -m5*y5_2-m6*z6_2-Ixx5-Iyy6-Izz4;
k14 = m4*d3*a3+m5*a3*d3+m6*a3*d3+m3*x3*d3+m3*x3*z3;
k15 = -2*m4*a3*d4-2*m4*a3*z4-2*m3*x3*y3-2*m6*d4*a3-2*m5*d4*a3;
k16 = 2*m2*x2*a1+2*m5*a2*a1+2*m6*a2*a1+2*m3*a2*a1+2*m4*a2*a1;
k17 = -m3*a2_2-m5*a2_2-m6*a2_2-m4*a2_2-m2*x2_2;
k18 = -2*m6*a1*d4-2*m4*a1*d4-2*m4*a1*z4-2*m5*a1*d4-2*m3*y3*a1;
k19 = m4*d3*z4+m4*d3*d4+m5*d3*d4+m3*y3*d3+m3*y3*z3+m6*d4*d3;
k20 = m4*d3*a2+m5*d3*a2+m3*d3*a2+m6*d3*a2+m2*x2*z2+m3*z3*a2;
k21 = -Iyy6+Izz6-m5*y5_2+Iyy5-Ixx5-m6*z6_2;
k22 = -Iyy5+Iyy4-Ixx4+m5*y5_2+m6*z6_2-Izz6+Izz5+Ixx6;
k23 = Ixx4+m4*z4_2+2*m4*z4*d4+m3*y3_2+m6*d4_2+m4*a3_2+Iyy5+m6*a3_2+Izz3+
m4*d4_2+m5*d4_2+m3*x3_2+m5*a3_2+Izz6;
k24 = Ixx4+m4*z4_2+2*m4*z4*d4+m3*y3_2+m6*d4_2+m4*a3_2+m5*a2_2+Iyy5+
m6*a3_2+Izz3+Izz2+m3*a2_2+m4*d4_2+m5*d4_2+m6*a2_2+m3*x3_2+m2*x2_2+
m4*a2_2+m5*a3_2+Izz6;
k25 = m3*a2_2-Iyy4+m4*a3_2+m6*a3_2-m4*d4_2+Iyy3-Izz5+Ixx5+Izz4+Iyy6-Ixx3
-Ixx6-2*m4*z4*d4+m6*a2_2+m5*a3_2+m4*a2_2-m4*z4_2-m6*d4_2+m3*x3_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));
D C�odigo fuente funciones Simulink 129
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+
130 D C�odigo fuente funciones Simulink
(-k7*c6*s6-k7*c5_2*c6*s6)*c4_2+(k7*s6_2-k7*c6_2)*c5*s4*c4+
k7*c6*s6)*QD(6);
C(1,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-
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(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+
D C�odigo fuente funciones Simulink 131
(-.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-
132 D C�odigo fuente funciones Simulink
.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-
D C�odigo fuente funciones Simulink 133
k21)*c5_2+k7*c6_2-k22)*s4*c4+k5*s4*s5-k7*c6*c5*s4_2*s6)*QD(2)+
(.5*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-
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*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(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+
((-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*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-
134 D C�odigo fuente funciones Simulink
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-
k21)*s5*c5*c4_2+(-k5*c5-k7*c6*s5*s6*s4)*c4+(-k7*c6_2+k21)*s5*c5-
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+
((-k7*s6_2+k7*c6_2)*c5*c4_2+(-k7*c6*s6-k7*c5_2*c6*s6)*s4*c4+
(-.5*k7*c6_2+.5*k7*s6_2+.5*k1)*c5)*s23)*QD(1)+((k7*c6*s6+
k7*c5_2*c6*s6)*c4_2+(-k7*s6_2+k7*c6_2)*c5*s4*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+
D C�odigo fuente funciones Simulink 135
.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-
136 D C�odigo fuente funciones Simulink
.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+
.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)*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+
.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)*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+
D C�odigo fuente funciones Simulink 137
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-
138 D C�odigo fuente funciones Simulink
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);
}
}
static void mdlTerminate(SimStruct *S)
{
}
#ifdef MATLAB_MEX_FILE
#include "simulink.c"
#else
#include "cg_sfun.h"
#endif
D C�odigo fuente funciones Simulink 139
/*
* 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
#define S_FUNCTION_LEVEL 2
#include <math.h>
#include "simstruc.h"
/* Asigna parametros al espacio de trabajo */
/*********************************************/
/* De 0 a 35 Matriz de coef de polinomios */
#define A(i,j) rwork[6*i+j]
#define 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 */
static void mdlInitializeSizes(SimStruct *S)
140 D C�odigo fuente funciones Simulink
{
ssSetNumContStates( S, 0); /* number of continuous states */
ssSetNumDiscStates( S, 0); /* number of discrete states */
if (!ssSetNumInputPorts(S, 3)) return; /*Entradas*/
ssSetInputPortWidth(S, 0, 6); /*Posicion Destino*/
ssSetInputPortWidth(S, 1, 6); /*Posicion Actual */
ssSetInputPortWidth(S, 2, 1); /*Orden de Movimiento*/
ssSetInputPortDirectFeedThrough(S, 0, 1);
ssSetInputPortDirectFeedThrough(S, 1, 1);
ssSetInputPortDirectFeedThrough(S, 2, 1);
if (!ssSetNumOutputPorts(S,4)) return; /*Salidas*/
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*/
ssSetNumSFcnParams(S, 3);
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
return; /* Si faltan parametros se da mensaje */
}
ssSetNumSampleTimes( S, 1); /* Numero de muestreos */
ssSetNumRWork( S, 51); /* Vector de numeros reales */
ssSetNumIWork( S, 3); /* 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 el vector de tiempos *
*/
static void mdlInitializeSampleTimes(SimStruct *S)
{
real_T *TSample = mxGetPr(SAMPLE(S)); /* Puntero a parametro frecuencia */
ssSetSampleTime(S, 0, *TSample); /*Automatico segun bloque anterior*/
ssSetOffsetTime(S, 0, 0.0);
}
D C�odigo fuente funciones Simulink 141
#define MDL_INITIALIZE_CONDITIONS
/*
* mdlInitializeConditions - Inicializa estados y parametros del robot
*
*/
static void mdlInitializeConditions(SimStruct *S)
{
int i;
real_T *posini = mxGetPr(POS_INI(S)); /* Puntero a Vector POS_INI */
real_T *rwork = ssGetRWork(S);
for(i=0;i<6;i++)
Estado(i)=posini[i];
}
#define MDL_START
#if defined(MDL_START)
/* Funci\'{o}n: mdlStart
*
*/
static void mdlStart(SimStruct *S)
{
int i;
real_T *posini = mxGetPr(POS_INI(S)); /* Puntero a Vector POS_INI */
real_T *rwork = ssGetRWork(S);
int_T *iwork = ssGetIWork(S);
Time_ini= 0;
Moverant = 0;
for(i=0;i<6;i++)
Estado(i)=posini[i];
MOVING = 0;
}
#endif
142 D C�odigo fuente funciones Simulink
/*
* mdlOutputs - Calcula las salidas
*
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
int i,j;
real_T *y;
InputRealPtrsType uPtrs[3];
real_T *rwork = ssGetRWork(S);
int_T *iwork = ssGetIWork(S);
real_T *ptqmax = mxGetPr(QMAX(S));
uPtrs[0] = ssGetInputPortRealSignalPtrs(S,0);
uPtrs[1] = ssGetInputPortRealSignalPtrs(S,1);
uPtrs[2] = ssGetInputPortRealSignalPtrs(S,2);
if ((Moverant==0)&&(MOVER==1)&&(MOVING==0)) /* 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;
D C�odigo fuente funciones Simulink 143
for(i=0;i<6;i++)
{
Estado(i)=0;
for(j=0;j<6;j++)
{
Estado(i)+=A(i,j)*pow(Time,j);
}
y = ssGetOutputPortRealSignal(S,1);
y[i]=0;
for(j=0;j<5;j++)
{
y[i]+=(j+1)*A(i,j+1)*pow(Time,j);
}
y = ssGetOutputPortRealSignal(S,2);
y[i]=0;
for(j=0;j<4;j++)
{
y[i]+=(j+2)*(j+1)*A(i,j+2)*pow(Time,j); /*Aceleracion*/
}
}
if (Time>=Tfinal)
MOVING=0;
}
else /* Si no hay movimiento velocidades nulas */
{
y = ssGetOutputPortRealSignal(S,1);
for(i=0;i<6;i++)
{
y[i]=0;
}
y = ssGetOutputPortRealSignal(S,2);
for(i=0;i<6;i++)
{
y[i]=0;
}
}
/* Asignamos las salidas del bloque */
y = ssGetOutputPortRealSignal(S,0);
for(i=0;i<6;i++)
{
y[i]=Estado(i);
}
144 D C�odigo fuente funciones Simulink
/* Indicaci\'{o}n fin de trayectoria */
y = ssGetOutputPortRealSignal(S,3);
if (MOVING==1) *y=0.0;
else *y=1;
Moverant=MOVER;
}
/*
* mdlTerminate - Se llama una vez finaliza la simulaci\'{o}n *
*/
static void mdlTerminate(SimStruct *S)
{
}
#ifdef MATLAB_MEX_FILE /* 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
D C�odigo fuente funciones Simulink 145
/*
* 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
#define S_FUNCTION_LEVEL 2
#include "simstruc.h"
#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_a2 mxGetPr(ssGetSFcnParam(S,2))[0]
#define RGEN_d3 mxGetPr(ssGetSFcnParam(S,3))[0]
#define RGEN_a3 mxGetPr(ssGetSFcnParam(S,4))[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_MIN_2 (double) -2.0944 /* -120 Grad */
#define LIM_ART_MAX_2 (double) +0.1745 /* 10 Grad */
#define LIM_ART_MIN_3 (double) -2.0944 /* -120 Grad */
#define LIM_ART_MAX_3 (double) +2.0944 /* +120 Grad */
#define LIM_ART_MIN_4 (double) -2.0944 /* -120 Grad */
#define LIM_ART_MAX_4 (double) +2.0944 /* +120 Grad */
#define LIM_ART_MIN_5 (double) -1.5708 /* - 90 Grad */
#define LIM_ART_MAX_5 (double) +1.5708 /* + 90 Grad */
#define LIM_ART_MIN_6 (double) -3.1416 /* -180 Grad */
#define LIM_ART_MAX_6 (double) +3.1416 /* +180 Grad */
#define FLOAT_EPSILON 1e-8
/* Asigna entradas */
146 D C�odigo fuente funciones Simulink
/*----------------------------------*/
#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 */
static void mdlInitializeSizes(SimStruct *S)
{
int i;
ssSetNumContStates( S, 0);/* N\'{u}mero de estados t continuo */
ssSetNumDiscStates( S, 0); /* N\'{u}mero de estados t discreto */
if (!ssSetNumInputPorts(S, 5)) return; /*Entradas*/
ssSetInputPortWidth(S, 0, 3); /*Vector n*/
ssSetInputPortWidth(S, 1, 3); /*Vector s*/
ssSetInputPortWidth(S, 2, 3); /*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);
if (!ssSetNumOutputPorts(S,3)) return; /*Salidas*/
ssSetOutputPortWidth(S, 0, 6); /*Posici\'{o}n Soluci\'{o}n*/
ssSetOutputPortWidth(S, 1, 1); /*Error trayectoria*/
ssSetOutputPortWidth(S, 2, 1); /*debug*/
ssSetNumSFcnParams(S, 7);
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
return; /* Si faltan parametros se da mensaje */
}
ssSetNumSampleTimes( S, 1); /* Numero de muestreos */
ssSetNumRWork( S, 0); /* Vector de numeros reales */
ssSetNumIWork( S, 0); /* Vector de numeros enteros */
ssSetNumPWork( S, 0); /* Vector de punteros */
ssSetNumModes( S, 0);
ssSetNumNonsampledZCs(S, 0);
ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE);
}
D C�odigo fuente funciones Simulink 147
static void mdlInitializeSampleTimes(SimStruct *S)
{
ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
ssSetOffsetTime(S, 0, 0.0);
}
#define MDL_INITIALIZE_CONDITIONS
/*
* mdlInitializeConditions - Inicializa estados y parametros del robot
*
*/
static void mdlInitializeConditions(SimStruct *S)
{
}
/*
* mdlOutputs - Calcula las salidas
*
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
int 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);
148 D C�odigo fuente funciones Simulink
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;
D C�odigo fuente funciones Simulink 149
}
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);
150 D C�odigo fuente funciones Simulink
}
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];
}
}
}
D C�odigo fuente funciones Simulink 151
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;
}
}
}
}
152 D C�odigo fuente funciones Simulink
/*Asignamos la salida*/
if (minnorm!=-1)
{
if (qNorm[minindex]<=1)
{
output = ssGetOutputPortRealSignal(S,0);
for(i=0;i<6;i++)
output[i]=qSol[minindex][i];
*outputerror=0;
}
else
{
output = ssGetOutputPortRealSignal(S,0);
for(i=0;i<6;i++)
output[i]=pact(i);
*outputerror=1; /* Posible Discontinuidad en la soluci\'{o}n */
}
}
else
{
output = ssGetOutputPortRealSignal(S,0);
for(i=0;i<6;i++)
output[i]=pact(i);
*outputerror=1; /* No hay soluci\'{o}n en la soluci\'{o}n */
}
}
else
{
output = ssGetOutputPortRealSignal(S,0);
for(i=0;i<6;i++)
output[i]=pact(i);
*outputerror=1; /* No hay soluci\'{o}n posible en el espacio de trabajo*/
}
}
static void mdlTerminate(SimStruct *S)
{
}
D C�odigo fuente funciones Simulink 153
#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
154 D C�odigo fuente funciones Simulink
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
top related