control experimental del pendubot y compensacion …bdigital.ula.ve/storage/pdf/42288.pdf2...
TRANSCRIPT
CONTROL EXPERIMENTAL DEL PENDUBOT
Y
COMPENSACION DE FRICCION
Autor : José Ignacio Hidalgo Barroeta
Profesor Guía : Pablo Lischinsky
Proyecto de Grado presentado ante la Ilustre Universidad de Los Andes
como requisito final para optar al título de Ingeniero de Sistemas.
UNIVERSIDAD DE LOS ANDES
FACULTAD DE INGENIERIA
ESCUELA DE INGENIERIA DE SISTEMAS
(Julio, 2000)
Reconocimiento
2
AGRADECIMIENTO
En primer lugar quiero expresar mis palabras de agradecimiento al
profesor Pablo Lischinsky de quien adquirí importantes conocimientos bajo
su tutoría.
Al Laboratorio de Control por facilitarme el equipo necesario para
llevar adelante mi trabajo.
Para Alfredo quien estuvo pendiente y colaborador en todo
momento.
A mis amigos Janeyra y José Antonio por su valiosa colaboración
tanto en la impresión como en la organización de la tesis.
Reconocimiento
3
RESUMEN
La presente tesis se desarrolla en dos partes principales. La primera
de ellas está orientada al control experimental del Pendubot a través de
ciertos esquemas implementados por el fabricante, mientras que la segunda
parte está dedicada a la compensación dinámica de fricción.
En consecuencia, se realiza en primer término un estudio de la
conexión del hardware así como de las ecuaciones dinámicas que describen al
Pendubot.
Posteriormente se analizan, desde el punto de vista teórico, el método
de Linealización Parcial de Realimentación utilizado para levantar a las
articulaciones desde su posición de equilibrio estable hasta el punto inestable
deseado (problema de oscilación), así como también el de Realimentación
Completa de los Estados, usado para el control de equilibrio de las
articulaciones (problema de balanceo).
Reconocimiento
4
Dichos métodos de control son verificados experimentalmente a
través de dos programas. Uno de ellos levanta y equilibra las articulaciones
en el punto inestable ( / , )q q1 22= − =π π , mientras que el otro lo hace en el
punto inestable ( / , )q q1 22 0= =π .
Para el modelo de fricción se identifican, en primer lugar, los
parámetros estáticos utilizando un controlador PI de velocidad angular para
el motor DC solamente (sin las articulaciones), luego son identificados los
parámetros dinámicos a partir de cierta experiencia realizada en lazo abierto.
Una vez que se tiene identificado el modelo dinámico de fricción, se
realizan pruebas tanto en el motor solo, utilizando controladores PID de
posición, así como en el Pendubot con la finalidad de comprobar la
efectividad del modelo encontrado.
Descriptores : Cota :
*
Robótica - Investigaciones - Pruebas TJ211
Sistemas de Control no Lineales H5
Reconocimiento
5
TABLA DE CONTENIDO
INTRODUCCION 11
CAPITULO 1. EL PENDUBOT 13
1.1 El Sistema . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
1.2 Descripción del Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
1.3 Características del Pendubot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
CAPITULO 2. DINAMICA Y CONTROL DEL PENDUBOT 17
2.1 Dinámica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.2 Identificación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
2.3 Control del Pendubot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
2.3.1 Control para equilibrar las articulaciones alrededor de un punto
de equilibrio (Balancing Control) . . . . . . . . . . . . . . . . . . . . . . . . 27
2.3.2 Control de oscilación (Swing up Control) . . . . . . . . . . . . . . . . . 34
CAPITULO 3. CONTROL EXPERIMENTAL DEL PENDUBOT 39
3.1 Precauciones de Seguridad . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
3.2 Control Medio (Balmid) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
3.3 Control Vertical (Baltop) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
Reconocimiento
6
CAPITULO 4. COMPENSACION DE FRICCION 49
4.1 Modelo Dinámico de Fricción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.2 Estimación de Parámetros Estáticos . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
4.2.1 Control PI de velocidad angular para motor DC . . . . . . . . . . . . 58
4.2.2 Identificación de parámetros estáticos de fricción . . . . . . . . . . . 66
4.3 Estimación de Parámetros Dinámicos . . . . . . . . . . . . . . . . . . . . . . . . . . 72
4.4 Compensación Dinámica de Fricción . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
4.5 Resultados Experimentales . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
4.5.1 Control PID de posición angular . . . . . . . . . . . . . . . . . . . . . . . . . 78
4.5.2 Control PID con prealimentación de aceleración . . . . . . . . . . . . 83
4.5.3 Compensación de fricción en el Pendubot . . . . . . . . . . . . . . . . . 87
CONCLUSIONES 94
RECOMENDACIONES 96
REFERENCIAS 97
APENDICE A. Identificación de Parámetros 100
A.1 Archivos M de Matlab para identificar los parámetros de
Pendubot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
APENDICE B. Linealización de Ecuaciones Dinámicas 103
APENDICE C. Programas Fuentes en C 110
C.1 Compilación de los programas fuentes . . . . . . . . . . . . . . . . . . . . . . . 110
Reconocimiento
7
C.2 Programa fuente “baltop.c” usado para levantar
ambas articulaciones verticalmente . . . . . . . . . . . . . . . . . . . . . . . . . . 112
C.3 Programa fuente “balmid.c” usado para controlar
las articulaciones en la posición media . . . . . . . . . . . . . . . . . . . . . . . .121
C.4 Programa fuente “motormod.c” utilizado para controlar
la velocidad angular del servomotor DC de imán permanente . . . .130
C.5 Programa fuente “Codresol.c” utilizado para calcular
los parámetros dinámicos de fricción . . . . . . . . . . . . . . . . . . . . . . . . . 135
C.6 Programa fuente “Contpid.c” utilizado para realizar
compensación de fricción dinámica del servomotor . . . . . . . . . . . . . 139
C.7 Programa fuente “Precomp.c” utilizado para realizar compensación
de fricción dinámica del motor con un PID precompensado . . . . . . 145
C.8 Programa fuente “Comptop.c” utilizado para realizar
compensación de fricción dinámica del Pendubot . . . . . . . . . . . . . . 152
C.9 Gráficas de respuesta para el control PI
de velocidad angular del servomotor DC . . . . . . . . . . . . . . . . . . . . . . 162
Reconocimiento
8
INDICE DE FIGURAS
1.1 Pendubot en su posición de equilibrio estable . . . . . . . . . . . . . . . . . . . . . 14
1.2 Esquema de conexión del Pendubot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.1 Sistema de coordenadas del Pendubot . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.2 Control de linealización parcial de realimentación . . . . . . . . . . . . . . . . . 37
3.1 Fotografía del Pendubot en la posición media . . . . . . . . . . . . . . . . . . . . 44
3.2 Control de oscilación y balanceo en la posición Media . . . . . . . . . . . . . 45
3.3 Fotografía del Pendubot en posición arriba . . . . . . . . . . . . . . . . . . . . . . 47
3.4 Control de oscilación y balanceo en la posición vertical . . . . . . . . . . . . . 48
4.1 Corte transversal de un motor DC . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
4.2 Diagrama general de la compensación de fricción . . . . . . . . . . . . . . . . . 51
4.3 Parámetros principales de la fricción estática . . . . . . . . . . . . . . . . . . . . . 52
4.4 a) Deformación de las “cerdas” elásticas, b) Detalle de analogía resorte
amortiguador que modela la dinámica de fricción . . . . . . . . . . . . . . . . . . . . 53
4.5 Diagrama de motor DC . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
4.6 Método del trapecio para la integral del error . . . . . . . . . . . . . . . . . . . . . 61
4.7 Datos experimentales de fricción en el motor DC . . . . . . . . . . . . . . . . . . 69
4.8 Representación continua de datos de fricción . . . . . . . . . . . . . . . . . . . . . 70
4.9 Zoom de fricción positiva y negativa . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
4.10 Ajuste de curva de datos experimentales . . . . . . . . . . . . . . . . . . . . . . . . 72
4.11 Gráfica de micro-desplazamiento y torque aplicado . . . . . . . . . . . . . . . 75
Reconocimiento
9
4.12 Trayectoria de referencia y respuesta . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
4.13 Posición de referencia y respuesta del motor . . . . . . . . . . . . . . . . . . . . 81
4.14 Error de posición . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82
4.15 Torque aplicado por el controlador PID y fricción estimada . . . . . . . . 83
4.16 Señales de referencia y respuesta sobrepuestas . . . . . . . . . . . . . . . . . . . 84
4.17 a) Respuesta de posición sin compensación de fricción. b) Respuesta de
posición con compensación de fricción . . . . . . . . . . . . . . . . . . . . . . . . 85
4.18 Error de posición . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86
4.19 Torque aplicado por el control PID precompensado y
fricción estimada . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87
4.20 Posición angular de articulación 1 sin compensación (sc)
y con compensación (cc) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88
4.21 Acercamiento de la posición angular de la articulación 1, tanto
con compensación de fricción (cc) como sin ella (sc) . . . . . . . . . . . . . 89
4.22 Posición angular de articulación 2 sin compensación (sc) y con
compensación de fricción (cc) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90
4.23 a) Velocidad angular de articulación 1 sin compensación (sc) y
con compensación (cc). b) Velocidad angular de articulación 2 sin
compensación (sc) y con compensación de fricción (cc) . . . . . . . . . . 91
4.24 Torque aplicado por el control sin compensación y
con compensación respectivamente . . . . . . . . . . . . . . . . . . . . . . . . . . . 92
4.25 Fricción estimada para la compensación del Pendubot . . . . . . . . . . . . 93
4.26 Fotografía del Pendubot con compensación de fricción . . . . . . . . . . . . 93
Reconocimiento
10
INDICE DE TABLAS
4.1 Datos experimentales del Control PI de velocidad angular . . . . . . . . . . 67
4.2 Parámetros estimados del modelo estático de fricción . . . . . . . . . . . . . . 71
4.3 Parámetros dinámicos estimados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
Reconocimiento
11
INTRODUCCION
Los robot subactuados son aquellos cuyos grados de libertad son
superiores al número de actuadores que poseen. Las características y
complejidad que envuelven a estos sistemas les han merecido años de
estudios con el objetivo de probar y desarrollar diversos métodos que sean
efectivos en el control de los mismos.
El Pendubot pertenece a esta clase de sistemas ya que puede verse
como un robot de dos grados de libertad subactuado (el control se realiza a
través de un motor DC), cuyo movimiento se desarrolla en el plano vertical,
pero también posee las características para ser considerado como un péndulo
simple controlado a través de la articulación motorizada.
En él se pueden implementar numerosos métodos para lograr un
correcto control de sus articulaciones, tanto para lograr determinadas
trayectorias de oscilación como para balancearlas (equilibrar) alrededor de los
Reconocimiento
12
diversos puntos de equilibrio inestable que posee. Un ejemplo de ello son los
controladores que han sido suministrados por el fabricante, los cuales
afrontan los problemas de oscilación (levantar las articulaciones según una
determinada trayectoria) y balance (equilibrar las articulaciones alrededor de
un punto de equilibrio), utilizando un método de control diferente para cada
caso.
Sin embargo, el modelo matemático en el cual se basan no toma en
cuenta a la fricción que se produce entre los cojinetes y el eje del motor DC ya
que los métodos de identificación utilizados tienen dificultad para
identificarla correctamente. Esto trae como consecuencia que los controles
sean menos precisos, presentándose tanto errores de posición como ciclos
límites de considerable amplitud principalmente en aquellas configuraciones
en las cuales se requiere mantener ambas articulaciones balanceadas
alrededor de un punto de equilibrio inestable.
Por consiguiente, es necesario realizar la identificación del modelo de
fricción dinámica con la finalidad de incluir la compensación en los
controladores desarrollados por el fabricante.
Reconocimiento
13
CAPITULO 1
EL PENDUBOT
1.1 EL SISTEMA
El Pendubot (Péndulo RoBot) es un sistema electromecánico
conformado por dos articulaciones rígidas (figura 1.1), interconectadas entre
si a través de un eje o junta de revolución la cual permite un movimiento de
360° entre ellas. El otro extremo de la articulación 1 está acoplada
directamente al eje de un motor DC a través del cual se efectúa el control,
mientras que la articulación 2 es de movimiento libre (no motorizada)
respecto de la primera, lo cual hace que el Pendubot pueda considerarse
como un brazo robótico planar de dos grados de libertad sub-actuado.
Reconocimiento
14
Figura 1.1 Pendubot en su posición de equilibrio estable.
Por otro lado, la articulación 2 (no motorizada) puede verse como un
péndulo simple cuyo movimiento es controlado por el accionar de la
articulación 1.
1.2 DESCRIPCION DEL HARDWARE
Las articulaciones del Pendubot son de aluminio con 0.635 cm de
espesor cada una. La articulación 1 posee 15.24 cm de longitud la cual está
acoplada directamente al eje de un motor DC de 90V de imán permanente
Reconocimiento
15
controlado por corriente, mientras que la articulación 2 es de 22.86 cm de
longitud. La articulación 1 incluye en su otro extremo el soporte para la junta
de revolución de la articulación 2. El eje que une ambas articulaciones (el
codo del brazo) alberga también un codificador óptico de 1250 pulsos/rev de
resolución que junto a otro similar acoplado en el eje del motor DC (el
hombro del brazo) constituyen los sensores que proporcionan la
realimentación de las posiciones angulares de las respectivas articulaciones
(figura 1.2). El motor es manejado a través de un servo amplificador en modo
torque y ajustado para una ganancia de 1V entrada = 1.2 Amps salida.
El componente final del hardware del Pendubot es su controlador, el
cual consiste en una PC compatible IBM que alberga una tarjeta D/A
conectada al servo amplificador encargado de manejar al motor DC, una
tarjeta de interfaz para los codificadores ópticos y una tarjeta adicional que
funciona como cronómetro (timer) y que permite obtener períodos de
muestreo suficientemente rápidos de menos de 10ms necesarios para
controlar este sistema.
Reconocimiento
16
Figura 1.2 Esquema de conexión del Pendubot
1.3 CARACTERISTICAS DEL PENDUBOT
El Pendubot presenta una serie de características interesantes que
pueden ser usadas en el campo de la investigación y la educación. A través de
él se pueden implementar diversos paradigmas ó métodos de control tales
como control lineal, control no lineal, control óptimo, control adaptativo y
robusto, lógica difusa, control inteligente, control híbrido y muchos otros, así
como desarrollar aplicaciones de identificación, compensación de fricción,
control de oscilación ó trayectoria, control de balanceo, etc.
Reconocimiento
17
Reconocimiento
18
Reconocimiento
19
Reconocimiento
20
Reconocimiento
21
Reconocimiento
22
Reconocimiento
23
Reconocimiento
24
Reconocimiento
25
Reconocimiento
26
Reconocimiento
27
Reconocimiento
28
Reconocimiento
29
Reconocimiento
30
Reconocimiento
31
Reconocimiento
32
Reconocimiento
33
Reconocimiento
34
Reconocimiento
35
Reconocimiento
36
Reconocimiento
37
Reconocimiento
38
Reconocimiento
39
Reconocimiento
40
Reconocimiento
41
CAPITULO 3
CONTROL EXPERIMENTAL DEL PENDUBOT
El fabricante ha suministrado una serie de controladores para
levantar y balancear el Pendubot en diferentes puntos de equilibrio
inestables. Este capítulo describe el control experimental del Pendubot para
dos de las configuraciones mas usadas como son la posición media
( / , )q q1 22= − =π π y la posición invertida de ambas articulaciones
( / , )q q1 22 0= =π .
Como se mencionó en el capítulo 2, éstos controladores no toman en
cuenta el torque producido por la fricción en el eje del motor DC, sin
embargo, ésta será tratada posteriormente en el capítulo 4.
Todos los controladores se han realizado para correr bajo el ambiente
DOS ó en una ventana DOS de Windows. Sin embargo, para que el
Reconocimiento
42
controlador pueda correr en una ventana debe realizarse previamente una
modificación del archivo PIF (Program Interface File) correspondiente al
ejecutable (.EXE) del programa. Esta condición es necesaria debido a la
demanda de tiempo de procesador que se requiere para controlar
adecuadamente al Pendubot, por tanto el sistema operativo, como es el caso
de Windows, no debe ocuparlo en otras actividades.
Los PIF son archivos que contienen cierta información que
caracterizan al archivo ejecutable. Para modificarlos se pulsa con el botón
derecho del mouse sobre el nombre del programa .EXE, luego se pulsa
Propiedades donde se deben realizar los siguientes cambios :
♦ En la pestaña de Pantalla marcar el modo de pantalla completa.
♦ En la pestaña de Programa introducir el directorio de Trabajo en el cual se
desea guardar los archivos de datos M que se graficarán en Matlab, luego
marcar la opción de cerrar el programa cuando se sale.
♦ En la pestaña de Misceláneas eliminar la opción de Permitir Protector de
Pantalla con el objeto de no interrumpir la ejecución del controlador.
Reconocimiento
43
Otra opción para correr los programas controladores es utilizar la
Interfaz Gráfica del Usuario GUI (Graphical User Interface) suministrada con
el Pendubot, la cual resulta mas cómoda y amigable a la hora de probar los
programas ya que se puede utilizar en el ambiente de Windows, además no
existe la necesidad de escribir repetidamente el nombre del programa con sus
respectivos parámetros como es el caso del ambiente DOS. Esta se encuentra
en Windows bajo Programas/Pendubot. Sin embargo, cuando se utiliza esta
ventana para controlar el Pendubot se debe cerrar cualquier aplicación,
incluido Matlab.
3.1 PRECAUCIONES DE SEGURIDAD
Es importante señalar algunas precauciones que deben tomarse en
cuenta a la hora de poner en funcionamiento al Pendubot para evitar posibles
daños tanto a los componentes del mismo como a la persona que lo manipule.
El movimiento libre del Pendubot tiene un comportamiento de
péndulo doble el cual se conoce de poseer una respuesta caótica, por tanto, si
las articulaciones se dejan oscilar libremente a partir de un punto lejos de su
posición de equilibrio estable, su movimiento puede ser tan violento que
puede resultar en daños tanto al cable del codificador y ejes de las uniones
Reconocimiento
44
como a la persona que trate de detenerlo cuando se encuentre girando
violentamente.
Otras medidas de seguridad son :
* Después de efectuar cualquier control de equilibrio del Pendubot, es
recomendable sujetar la articulación 2 antes de soltar el switch manual de
seguridad para evitar que caigan libremente.
* No se debe colocar las manos ó cualquier otra parte del cuerpo sobre
la trayectoria del Pendubot mientras está operando (a pesar de estar
balanceando) ya que puede tener una respuesta imprevista.
* Evitar dejar caer las articulaciones desde la posición invertida, ya que
puede resultar, a la larga, en daños de los codificadores.
* Cualquier conexión ó desconexión tanto de las tarjetas de interfaz
como de cables de datos y codificadores deben realizarse con el Pendubot y la
PC apagados.
3.2 CONTROL MEDIO ( BALMID )
Para controlar el Pendubot en la posición media ( / , )q q1 22= − =π π se
ejecuta el programa BALMID.EXE bajo DOS ó en la GUI bajo Windows. En
caso de ejecutarlo bajo DOS, se introduce en la línea de comandos el nombre
Reconocimiento
45
del programa seguido de los parámetros que utilizará el controlador para
levantar a las articulaciones, las cuales deben llegar suavemente a la posición
de equilibrio para poder ser capturadas por el control de balance.
Como se mencionó en el capítulo 2, los controladores están divididos
en dos partes. La primera de ellas se encarga de oscilar ó levantar las
articulaciones hasta la posición de equilibrio, utilizando un control diseñado
por el método de linealización parcial de realimentación. Este controlador
introduce energía al sistema a través de una oscilación senoidal inicial, de tal
forma que la articulación 2 pueda alcanzar su punto de equilibrio q2 = π .
La trayectoria de la articulación 1 es controlada a través de un PD
(3.1) con prealimentación de la aceleración la cual le define un determinado
movimiento, al mismo tiempo se produce una excitación de la dinámica
interna que produce la oscilación de la articulación 2 para llevarla hasta la
posición de equilibrio deseada y así poder ser balanceada por el segundo
control el cual utiliza el método de realimentación completa del vector de
estados.
El control de la trayectoria es
v q K q q K q qdd
dp
d1 1 1 1 1 1= + − + −&& ( & & ) ( ) (3.1)
Reconocimiento
46
con una trayectoria definida por q ampl sin td1 = * ( * )ω .
Por consiguiente, el programa BALMID.EXE está compilado con
valores calculados para las ganancias Kp y Kd , así como ampl yω de la señal
senoidal, sin embargo, en algunas ocasiones las articulaciones no llegan al
punto de equilibrio en forma apropiada por lo que el control de balance no
captura la articulación 2. Para solventar este problema se debe introducir,
después del nombre del programa, un nombre para el archivo donde se
guardarán los datos de respuesta del sistema seguido de otros valores para
las ganancias ( Kp , Kd ) , amplitud y frecuencia (ampl, ω ) de la señal senoidal
tales que estén cercanos a los valores originales, por ejemplo : BALMID
datos.m 150 21.2 4.5 1.28.
*******************************************************************************
*
FOTO DE PENDUBOT EN BALMID
(Figura 3.1 Fotografía del Pendubot en mid)
*******************************************************************************
*
Reconocimiento
47
Una vez recolectados los datos de respuesta del sistema, se pueden
graficar en Matlab. La figura 3.2 presenta la respuesta y el torque aplicado
para balancear el Pendubot en la posición ( / , )q q1 22= − =π π .
Reconocimiento
48
Figura 3.2 Control de oscilación y balanceo en la posición Media
3.3 CONTROL VERTICAL (BALTOP)
La forma de levantar las articulaciones hasta la posición de equilibrio
utilizado por este controlador difiere del control BALMID, ya que no se
utiliza una senoidal para la oscilación inicial de la articulación 2.
La trayectoria la define un escalón q1 2= π / , de forma tal que las
articulaciones se muevan directamente desde su posición de equilibrio
estable ( / , )q q1 22 0= − =π hasta la posición inestable ( / , )q q1 22 0= =π .
La velocidad con la cual se realiza esta operación no debe ser
demasiado lenta que traiga como consecuencia que la articulación 2 no
levante, ó demasiado rápida de forma tal que esta pase por el punto de
equilibrio con una velocidad que no permita ser capturada por el control de
balance. Por consiguiente, los valores que tengan las ganancias Kp y Kd
Reconocimiento
49
deben ser apropiados para que la articulación 2 llegue de forma precisa al
punto de equilibrio. Por ejemplo : BALTOP misdatos.m 45.5 8.9 0.25 0.0.
Una vez mas, en caso de que no llegue al punto de equilibrio en
forma apropiada, se puede variar las ganancias un poco hasta que produzca
el efecto deseado.
Como en este caso no se ha tomado en cuenta la fricción, trae como
consecuencia de que las articulaciones presenten un movimiento de vaivén
de gran amplitud (ciclo límite). Este efecto es disminuido cuando le es
agregado al control una señal senoidal de alta frecuencia lo que produce una
vibración de las articulaciones pero eliminando en gran medida la inclinación
de las mismas.
Este efecto no se presenta en el caso del control medio porque la
fuerza de gravedad actúa a favor de la articulación 1, la cual se encuentra en
la posición de equilibrio estable, permitiendo que permanezca inmóvil,
mientras que en el control vertical arriba, la fuerza de gravedad trata de
empujar ambas articulaciones hacia abajo.
******************************************************************************
FOTO PENDUBOT CON CONTROL BALTOP
Reconocimiento
50
(Figura 3.3 Fotografía del Pendubot en posición de balance arriba)
******************************************************************************
Los resultados experimentales se presentan en la figura 3.4
Reconocimiento
51
Figura 3.4 Control de oscilación y balanceo en la posición vertical
Reconocimiento
52
CAPITULO 4
COMPENSACION DE FRICCION
4.1 MODELO DINAMICO DE FRICCION
La fricción es un fenómeno que se presenta entre superficies en
contacto y que al mismo tiempo exista un movimiento relativo entre ellas, lo
cual produce una fuerza de roce ó fricción que actúa oponiéndose al
desplazamiento relativo de los respectivos cuerpos. Por tal razón, la fricción
está presente en todos los sistemas mecánicos lo cual conlleva a efectuar un
tratamiento adecuado de la misma para que no produzca efectos indeseados
en el comportamiento y desempeño de los sistemas.
En el caso del Pendubot, la fricción está presente, principalmente,
entre el eje del rotor y los bujes ó cojinetes del servomotor (figura 4.1). Los
efectos producidos por la fricción sobre este tipo de servomecanismos son
Reconocimiento
53
principalmente errores de posición, ciclos límites y pequeños movimientos
intermitentes indeseados del eje del servomotor a bajas velocidades. Estos
efectos se pueden reducir significativamente al aplicar un método adecuado
de compensación de fricción.
Los métodos de compensación de fricción, como el presentado en [5],
se basan en la incorporación de un modelo que predice la fricción real a partir
de mediciones tomadas del sistema, de tal forma que pueda aplicarse un
torque opuesto que compense el torque producido por la fricción real tal
como se muestra en la figura 4.2.
Reconocimiento
54
Figura 4.2 Diagrama general de la compensación de fricción
El modelo clásico de la fricción es una representación estática entre la
velocidad constante y el torque de fricción, el cual depende del signo de la
velocidad y otros elementos tales como : fricción estática o seca, Coulomb y
viscosa. La dificultad al utilizar este tipo de modelo (figura 4.3) se presenta
cuando la velocidad es cero, debido a que la fricción es multivaluada o no
está definida, pero también presenta limitaciones cuando la velocidad varía
en el tiempo, por tanto, es necesario utilizar un modelo que solvente estas
dificultades.
La fricción es un fenómeno bastante difícil de modelar en forma
exacta, sin embargo, en la actualidad existen varios modelos dinámicos, unos
Reconocimiento
55
con mejores características que otros, los cuales describe el comportamiento
de la fricción con una buena aproximación.
Figura 4.3 Parámetros principales de la fricción estática
El modelo utilizado en este proyecto es el propuesto en [5]. Este
modelo toma en cuenta las irregularidades (asperezas) que presentan las
superficies de contacto a un nivel microscópico, lo cual se visualiza como el
contacto entre dos cuerpos rígidos a través de pequeñas cerdas elásticas
(figura 4.4).
Cuando se aplica una fuerza tangencial, las cerdas se tuercen como
resortes lo cual representa la fuerza de fricción. Sin embargo, si la fuerza
Reconocimiento
56
aplicada es suficientemente grande, las pequeñas cerdas se doblaran de tal
manera que permite un deslizamiento entre las superficies de contacto.
Por consiguiente, el modelo está basado en el comportamiento
promedio de las cerdas, ya que este fenómeno es aleatorio debido a las
formas irregulares de las superficies y tomando en cuenta el hecho de que
ninguna superficie es exactamente igual a otra.
(a) (b)
Figura 4.4 a) Deformación de las “cerdas” elásticas, b) Detalle de
analogía resorte-amortiguador que modela la dinámica de fricción.
El doblamiento promedio de las cerdas es representado por la
variable z cuyo comportamiento es modelado por
Reconocimiento
57
dzdt
g qz= −&
&
( &)σ0 (4.1)
donde &q es la velocidad relativa entre las dos superficies.
El primer término de la ecuación (4.1) representa la deformación de
z , el cual es proporcional a la integral de la velocidad relativa. El segundo
término asegura que en estado estacionario ( & )z = 0 , el doblamiento z se
aproxima al valor
zq
qg q
g qqss = =
&
&( &)
( &)sgn( &)
σ σ0 0 (4.2)
La función g q( &) representa la característica estática de la fricción,
dada por la expresión :
g q eqv( &)&
= +−⎛
⎝⎜
⎞
⎠⎟
α α0 10
2
(4.3)
la cual es positiva y depende de varios factores tales como las propiedades
del material, lubricación y temperatura. Para fricciones típicas en los
cojinetes, g q( &) decrecerá monótonamente desde g( )0 cuando la velocidad
Reconocimiento
58
aumenta (ver figura 4.3). Esto corresponde al llamado efecto Stribeck. El
parámetro v0 [rad/seg] representa la velocidad del término correspondiente
al efecto Stribeck, representado en la ecuación (4.3) por una función
exponencial. Para una velocidad nula g( )0 0 1= +α α , y para valores de
velocidad &q v> 3 0 , g q( &) = α0 . El valor ( )α α0 1+ [Nm] representa la fricción
estática o seca y α0 [Nm] es la fricción de Coulomb.
Por tanto, el torque generado por la fricción es descrito a través de la
ecuación
F zdzdt
q= + +σ σ α0 1 2 & (4.4)
donde σ0 representa la rigidez y σ1 es el coeficiente de amortiguamiento
(ambos llamados parámetros dinámicos de la fricción), mientras que α2 &q es
el término proporcional a la velocidad relativa que toma en cuenta a la
fricción viscosa.
A partir de las ecuaciones (4.2) y (4.4), se obtiene la fricción en estado
estacionario dada por :
Reconocimiento
59
F z q e q qss ss
qv= + = +
⎛
⎝
⎜⎜
⎞
⎠
⎟⎟ +
−⎛
⎝⎜
⎞
⎠⎟
σ α α α α0 2 0 1 20
2
& sgn( &) &
&
(4.5)
La ecuación (4.5) corresponde a la representación estática de la
fricción dada por (4.3).
En resumen, el modelo del motor DC controlado en torque con
fricción es el siguiente :
& &&
( &)z q
qg q
z= −σ0
g q eqv( &)&
= +−⎛
⎝⎜
⎞
⎠⎟
α α0 10
2
(4.6)
F z z q= + +σ σ α0 1 2& &
J q F&& = −Γ
donde los parámetros :
♦ α α0 1+ [Nm] es la fricción estática
♦ v0 [rad/seg] es la velocidad de Stribeck
♦ α0 [Nm] representa el nivel de la fricción de Coulomb
♦ α2 [Nm/rad/s] es la fricción viscosa
Reconocimiento
60
♦ σ0 [Nm/rad] constante de rigidez del modelo de resorte
♦ σ1 [Nm/(rad/seg)] coeficiente de amortiguamiento
♦ J [ Kg m− 2 ] momento de inercia equivalente del motor
♦ Γ [Nm] par aplicado por el motor DC
Los parámetros estáticos de fricción son α α α0 1 2, , y v0 , mientras que
los parámetros dinámicos son σ0 y σ1 .
4.2 ESTIMACION DE PARAMETROS ESTATICOS
Los parámetros estáticos del modelo de fricción pueden ser
estimados a partir de las ecuaciones (4.5) y (4.6). En este caso, cuando la
velocidad es constante ( & )q ctte= , se anulan &&q y &z , por tanto, el torque que
debe aplicarse al servomotor iguala a la fricción en estado estacionario.
Γ = = +⎛
⎝
⎜⎜
⎞
⎠
⎟⎟ +
−⎛
⎝⎜
⎞
⎠⎟
F e q qss
qvα α α0 1 2
0
2&
sgn( &) & (4.7)
Reconocimiento
61
La expresión (4.7) indica que a velocidad constante, el torque
aplicado es igual a la fricción a dicha velocidad, la cual está caracterizada por
los cuatro parámetros estáticos α α α0 1 2, , y v0 .
Entonces, el método de estimación de dichos parámetros consiste en
utilizar un controlador PI de velocidad angular, de tal forma poder obtener
los torques correspondientes a diferentes velocidades de referencia, tanto
positivas como negativas. Es de hacer notar que los parámetros anteriores
pueden ser diferentes para velocidades positivas y negativas.
4.2.1 Control PI de Velocidad Angular para Motor DC.
Como se ha mencionado, el objetivo de diseñar este control es el de
construir la gráfica que relaciona la velocidad angular del servomotor con el
torque aplicado al mismo. La gráfica resultante representará el modelo de
fricción estática del servomotor.
Para diseñar el controlador PI de velocidad angular se recurre, en
primera instancia, al modelo matemático del motor DC.
En este caso se utiliza la función de transferencia que relaciona la
señal de salida (velocidad angular) con la señal de entrada (torque aplicado).
Reconocimiento
62
Tomando en cuenta el diagrama presentado en la figura 4.5, la
función de transferencia de un motor DC es la siguiente.
Figura 4.5 Diagrama de motor DC
Según la segunda ley de Newton
J &ω α ω= −Γ 2 (4.8)
donde
J = momento de inercia del eje del motor DC en Kg - m2
ω = velocidad angular rad s/
Γ= par aplicado al sistema N- m
por tanto, aplicando transformada de Laplace a (4.8) se obtiene la siguiente
función de transferencia :
Reconocimiento
63
ω
α( )( )ss JsΓ
=+1
2 (4.9)
donde λ α= J2
[seg] representa la constante de tiempo del motor.
Ahora bien, utilizando la hoja de especificaciones del servomotor DC
del Pendubot modelo 3B-9013182K fabricado por la Minnesota Electric
Technology (MET) se obtiene la siguiente función de transferencia
ω( )( ) ( . ) .ss sΓ
=× +−
14 237 10 0 002184 (4.10)
Es de notar que el valor del coeficiente de fricción viscosa no se
encuentra explícitamente definido en las especificaciones del fabricante, sin
embargo, la compañía suministra ciertas pruebas hechas al servomotor. Es a
partir de allí donde se dedujo un valor inicial aproximado para el diseño del
controlador PI de velocidad angular. Una vez que se obtienen resultados del
Reconocimiento
64
control de velocidad, fue posible encontrar un valor más exacto de este
coeficiente resultando en α2 = 0.00012 [Nm/(rad/s)], por tanto, la constante
de tiempo del motor es λ = 35308. [seg].
El período de muestreo Ts utilizado para el control PI es de 5 ms, es
decir, el mismo que el empleado por los controladores del Pendubot, por
consiguiente, se utilizará la versión continua del PI.
La ley de control del PI tiene la siguiente forma :
Γ = + ∫Kp e Ki edt* * (4.11)
donde e r= −ω ω representa el error de velocidad. Teniendo presente que el
sistema realiza solamente la realimentación de posición, por consiguiente, la
velocidad es estimada a través del método de diferencias finitas :
[ ]
ω =− −q qTs
k k 1
La integral del error (llamada de ahora en adelante ie) se aproxima
del siguiente modo (figura 4.6)
Reconocimiento
65
Figura 4.6 Método del trapecio para la integral del error
donde ek representa el error actual, mientras que ek −1 es el valor del error en
el instante anterior. Por consiguiente, como la integral representa el área bajo
la curva, se procede del siguiente modo :
area Ts eTs
e ek k k= + −− −* ( )1 12 (4.12)
lo cual puede escribirse
areaTs e Ts
e ekk k= + −−
−
22 2
11
( * )( ) (4.13)
resultando en
Reconocimiento
66
areaTs
e ek k= + −2 1( ) (4.14)
se nota claramente que la distancia Ts representa el período de muestreo, por
tanto, la integral del error puede escribirse del siguiente modo
ie ieTs
e ek k= + − −2 1( ) (4.15)
Con la integral del error dada por la ecuación (4.15), queda
completamente especificado el control (4.11).
Ahora solo queda establecer las ganancias Kp y Ki partiendo de una
dinámica deseada.
Utilizando la función de transferencia del motor
G sJs
( ) =+1
2α (4.16)
además de la función de transferencia del controlador PI
G sKp s Ki
sc ( )*
=+
(4.17)
Reconocimiento
67
y realizando la operación de lazo cerrado con (4.16) y (4.17), produce la
siguiente función de transferencia
G s J Kp s Ki
sKp
Js
KiJ
o ( )( * )
( )=+
++
+
1
2 2α (4.18)
lo que implica que el polinomio característico es
pc sKp
Js
KiJ
= ++⎛
⎝⎜⎞⎠⎟ +2 2α (4.19)
Luego se iguala a un polinomio característico cuya dinámica sea
impuesta convenientemente.
pc s sd n n= + +2 22( * * )δ ω ω (4.20)
Reconocimiento
68
igualando los coeficientes de los polinomios (4.19) con (4.20), además
definiendo KJ
= =1
2360 2. y ′ = =αα
22 0 28
J. se obtienen las expresiones para
las ganancias Kp y Ki en función de los parámetros ωn y δ .
KpK
n=−2 2* *δ ω α
(4.21)
KiK
n=ω 2
(4.22)
La dinámica deseada requiere que el sistema sea críticamente
amortiguado ( )δ = 1 , mientras que ωn fue seleccionado después de
experimentar con distintos valores, estableciéndose que los mejores valores
de Kp y Ki para que el sistema pudiera seguir velocidades de referencia
suficientemente pequeñas corresponden a un valor de ωn = 50 rad/s. Para
valores menores de ωn ocasiona que las ganancias Kp y Ki no sean lo
suficientemente altas para que el sistema siga velocidades de referencia muy
bajas , por el contrario para valores superiores a 50 rad/s, el sistema tiende a
la inestabilidad.
Reconocimiento
69
De esa forma quedan establecidas las ganancias del controlador PI a
partir de (4.21) y (4.22) resultando en Kp =0.0423 y Ki = 1.0592, además, de
(4.11) se obtiene la siguiente señal de control :
Γ = +0 0423 10592. * . *e ie (4.23)
donde ie es la integral del error dada por la expresión (4.15).
La implementación de este control puede apreciarse en el programa
“Motormod.c” cuyo código se reproduce en el Apéndice C, además de
algunos resultados del controlador para diferentes velocidades angulares de
referencia.
4.2.2 Identificación de Parámetros Estáticos de Fricción
Para determinar la gráfica de fricción se coloca una velocidad angular
de referencia para luego determinar el torque necesario para mantener dicha
velocidad constante. Cada par de valores, velocidad - torque, representará un
solo punto de la gráfica de fricción, tal como se presenta en la figura 4.3, por
tanto se prueban diferentes velocidades de referencia y se guarda en un
Reconocimiento
70
archivo tanto el torque aplicado como las velocidades angulares cuando se ha
alcanzado el estado estacionario.
En la tabla 4.1 se presentan los valores de torque que se obtuvo para
diferentes velocidades de referencia.
Tabla 4.1 Datos experimentales del Control PI de velocidad angular
Nombre Archivo
Velocidad Ref.
Velocidad (Media)
Torque (Media) Voltaje (Media)
V40_50 40.0 39.9985
0.0099 0.0205
V30_50 30.0 30.0001
0.0082 0.0171
V20_50 20.0 20.0010
0.0071 0.0148
V15_50 15.0 15.0000
0.0065 0.0136
V10_50 10.0 10.0007
0.0059 0.0123
V5_50 5.0 5.0002 0.0054 0.0112 V4_50 4.0 3.9997 0.0050 0.0104 V3_50 3.0 2.9979 0.0042 0.0088 V2p5_50 2.5 2.4993 0.0038 0.0080
Reconocimiento
71
V2_50 2.0 1.9998 0.0031 0.0065 V1p5_50 1.5 1.5007 0.0035 0.0073 V1_50 1.0 0.9975 0.0034 0.0071 V08_50 0.8 0.8009 0.0034 0.0071 V06_50 0.6 0.5998 0.0045 0.0093 V05_50 0.5 0.4987 0.0039 0.0080 V04_50 0.4 0.3990 0.0031 0.0065 V03_50 0.3 0.2987 0.0034 0.0070 V02_50 0.2 0.2002 0.0040 0.0083
V01_50 0.1 0.1000 0.0028 0.0059 V008_50 0.08 0.0796 0.0004 0.0008 V006_50 0.06 0.0605 0.0037 0.0077 V005_50 0.05 0.0505 0.0072 0.0015 V004_50 0.04 0.0392 0.0004 0.0008 V003_50 0.03 0.0295 0.0015 0.0031 V002_50 0.02 0.0192 0.0022 0.0047
V001_50 0.01 0.0098 0.0098 0.0203 V0008_50 0.008 0.0079 0.0029 0.0061 V0006_50 0.006 0.0060 0.0099 0.0206 V0005_50 0.005 0.0048 0.0028 0.0059 V0004_50 0.004 0.0038 0.0059 0.0112 V0002_50 0.002 0.0018 0.0053 0.0110 V0001_50 0.001 0.0010 0.0006 0.0013 Vn0001_50 -0.001 -
0.0008 -
0.0175 -
0.0363 Vn0002_50 -0.002 -
0.0017 -
0.0182 -
0.0378 Vn0003_50 -0.003 -
0.0025 -
0.0206 -
0.0428 Vn0004_50 -0.004 -
0.0039 -
0.0192 -
0.0399 Vn0005_50 -0.005 -
0.0045 -
0.0227 -
0.0472 Vn0006_50 -0.006 -
0.0057 -
0.0218 -
0.0453 Vn0008_50 -0.008 -
0.0070 -
0.0287 -
0.0597 Vn001_50 -0.01 -
0.0101 -
0.0229 -
0.0477 Vn002_50 -0.02 -
0.0193 -
0.0316 -
0.0658
Reconocimiento
72
Vn003_50 -0.03 -0.0303
-0.0322
-0.0669
Vn004_50 -0.04 -0.0393
-0.0340
-0.0708
Vn005_50 -0.05 -0.0505
-0.0327
-0.0680
Vn006_50 -0.06 -0.0584
-0.0242
-0.0503
Vn008_50 -0.08 -0.0795
-0.0240
-0.0498
Vn01_50 -0.1 -0.0994
-0.0335
-0.0696
Vn02_50 -0.2 -0.2007
-0.0301
-0.0626
Vn03_50 -0.3 -0.3006
-0.0299
-0.0621
Vn04_50 -0.4 -0.3997
-0.0304
-0.0633
Vn05_50 -0.5 -0.5005
-0.0309
-0.0643
Vn06_50 -0.6 -0.6010
-0.0303
-0.0630
Vn08_50 -0.8 -0.7985
-0.0314
-0.0654
Vn1_50 -1.0 -1.0022
-0.0309
-0.0643
Vn1p5_50 -1.5 -1.4977
-0.0316
-0.0658
Vn2_50 -2.0 -2.0003
-0.0315
-0.0656
Vn2p5_50 -2.5 -2.4991
-0.0318
-0.0661
Vn3_50 -3.0 -2.9990
-0.0319
-0.0664
Vn4_50 -4.0 -3.9995
-0.0322
-0.0669
Vn5_50 -5.0 -5.0000
-0.0323
-0.0673
Vn10_50 -10.0 -9.9995
-0.0329
-0.0685
Vn15_50 -15.0 - - -
Reconocimiento
73
14.9999 0.0335 0.0698 Vn20_50 -20.0 -
19.9996 -
0.0341 -
0.0709 Vn30_50 -30.0 -
30.0003 -
0.0349 -
0.0726 Vn40_50 -40.0 -
39.9993 -
0.0363 -
0.0756
Al graficar los datos anteriores utilizando Matlab se obtiene el
modelo de fricción correspondiente al servomotor del Pendubot (figura 4.7) .
Figura 4.7 Datos experimentales de fricción en el motor DC
Reconocimiento
74
Puede apreciarse que la fricción de Coulomb ó estática del
servomotor es menor cuando la velocidad angular es positiva, por tanto se
necesita un torque mayor para iniciar un movimiento de giro negativo.
La figura 4.8 presenta los mismos datos, pero graficados en forma
continua.
Figura 4.8 Representación continua de datos de fricción
La figura 4.9 muestran un acercamiento en la escala gráfica tanto de
valores positivos como negativos de la fricción del servomotor DC.
Reconocimiento
75
Figura 4.9 Zoom de fricción positiva y negativa
Una vez que se tiene la gráfica de fricción se seleccionan los
parámetros de forma tal que el modelo ajuste aproximadamente a los datos
experimentales.
La figura 4.10 muestra el ajuste hecho a los datos experimentales, la
cual tiene los parámetros presentados en la tabla 4.2.
Tabla 4.2 Parámetros estimados del modelo estático de fricción
Parámetros estáticos &q > 0
Parámetros estáticos &q < 0
α0 [Nm] 0.0028 0.0299 α1 [Nm] 0.0070 0.0041 α2 [Nm
s/rad] 0.00012 0.00012
v0 [rad/s] 0.0100 0.0400
Reconocimiento
76
La expresión para el modelo de fricción estática (ecuación 4.5) queda :
* Para velocidad angular positiva ( &q > 0 )
F EXPq
sign q qss = + −⎛⎝⎜
⎞⎠⎟
⎛
⎝⎜
⎞
⎠⎟ +0 0028 0 007
0 010 00012
2
. . *&
.( &) . * & (4.24)
* Para velocidad angular negativa ( &q < 0 )
F EXPq
sign q qss = + −⎛⎝⎜
⎞⎠⎟
⎛
⎝⎜
⎞
⎠⎟ +0 0299 0 0041
0 040 00012
2
. . *&
.( &) . * & (4.25)
Figura 4.10 Ajuste de curva (rojo continuo) de datos experimentales
4.3 ESTIMACION DE PARAMETROS DINAMICOS
Reconocimiento
77
El método anterior de estimación de parámetros estáticos no es
aplicable para determinar los parámetros dinámicos ya que no se puede
efectuar mediciones físicas del estado interno z (ecuación 4.6), además de la
fuerte dependencia no lineal entre la fricción y los parámetros dinámicos.
La estimación del parámetro σ0 se efectúa a partir de datos tomados
de cierta experiencia hecha al motor en lazo abierto, la cual resalta el efecto
que tienen los parámetros dinámicos sobre el sistema.
El objetivo es inducir u obtener, aplicando un torque en lazo abierto,
micro-movimientos (imperceptibles a simple vista) en el eje del servomotor
los cuales representan la resolución del codificador de posición.
Si para el sistema en reposo, le es aplicado una señal rampa u = C*t,
donde la pendiente C es suficientemente pequeña de forma tal que el torque
aumente lentamente en el tiempo, se producen los microdesplazamientos en
el eje del motor ocasionando que &&q = 0 , por tanto, a partir de la ecuación (4.6)
se tiene que Γ = F . Pero, debido a que se producen estos
microdezplazamientos, se asume a &q ≈ 0 y &z = 0 de tal forma que a partir de
Reconocimiento
78
(4.6) queda Γ = σ0 z , además, haciendo la hipótesis de que z q≈ (ver la figura
4.4 (b)) se obtiene que σ0 =Γq
.
Por consiguiente, una estimación del parámetro σ0 se obtiene a partir
de :
$σ0 ≈ΔΓΔq
(4.26)
donde ΔΓ Γ Γ= −( ) ( )T 0 es el torque necesario para producir un
microdesplazamiento Δq q T q= −( ) ( )0 .
Luego, el valor del parámetro σ1 se calcula a partir del siguiente
modelo linealizado del motor y fricción (ver ecuación 4.6) durante la fase de
microdesplazamientos ( & , )q z≈ ≈0 0 dado por :
J q q q&& ( ) &+ + + =σ α σ1 2 0 Γ (4.27)
Por consiguiente :
σ ξ σ α1 0 22= −J (4.28)
con ξ = 1 .
Reconocimiento
79
Para la estimación de los parámetros dinámicos del servomotor, se
empleó un programa en C llamado “Codresol.c” (Apéndice C) que envía una
señal de torque tipo rampa en lazo abierto con pendiente apropiada con el
objeto de graficar los micro-desplazamientos del eje del servomotor. Se
realizaron tres experiencias con pendientes diferentes para promediar el valor
del parámetro σ0 . La figura (4.11) presenta uno de los resultado
experimentales para una pendiente de torque C = 0.000495.
Figura 4.11 Gráfica de micro-desplazamiento y torque aplicado
A partir de los datos experimentales y las ecuaciones (4.26) y (4.28) se
obtienen las siguientes estimaciones de los parámetros dinámicos.
Reconocimiento
80
Tabla 4.3 Parámetros dinámicos estimados
Parámetros [dimensión]
Valor estimado
$σ0 [Nm/rad] 0.7818 $σ1 [Nm/(rad/seg)] 0.0363
4.4 COMPENSACION DINAMICA DE FRICCION
Como se ha mencionado, la fricción está dada por :
& &&
( &)z q
qg q
z= −σ0
g q eqv( &)&
= +−⎛
⎝⎜
⎞
⎠⎟
α α0 10
2
(4.29)
F z z q= + +σ σ α0 1 2& &
El esquema de compensación de fricción está basado en un
observador para el estado interno z, de tal forma que :
Reconocimiento
81
$& &&
( &)$ ,
$ $ & &
z qq
g qz ke k
F z z q
= − − >
= + +
σ
σ σ α
0
0 1 2
0 (4.30)
donde e q qr= − es el error de posición, qr la posición de referencia. Su
estabilidad en lazo cerrado está demostrada en [3].
Para el cálculo del observador de fricción, se integra numéricamente
la ecuación diferencial presentada en (4.29), utilizando el método del trapecio
según la expresión :
( )y yT
f x y f x yk k k k k k= + +− − −1 1 12( , ) ( , )
Por tanto, la integración para el observador es la siguiente :
z zT
g qz q
qg q
zk k kk
kk k
k
kk= + − + −
⎛
⎝⎜⎜
⎞
⎠⎟⎟− −
−
−−1 1 0
1
11 02
( &&
( & )) ( &
&
( & ))σ σ (4.31)
así
Reconocimiento
82
z
zT
g qz q
T qg q
k
k kk
kk k
k
k
=
+ − +⎛
⎝⎜⎜
⎞
⎠⎟⎟
+
− −−
−−1 1 0
1
11
0
2
12
&&
( & )&
&
( & )
σ
σ
(4.32)
La ecuación (4.32) permite calcular zk a partir de zk−1 , &qk−1 y &qk .
Utilizando la ecuación de fricción estática presentada en (4.29)
conjuntamente con la fricción dinámica dada por (4.30) y (4.32) se logra la
compensación de fricción del Pendubot.
4.5 RESULTADOS EXPERIMENTALES
Para verificar la compensación dinámica de fricción se realizaron tres
clases de experiencias. La dos primera tienen la finalidad de comprobar la
efectividad de la compensación solamente con el motor DC (se desmontaron
ambas articulaciones del Pendubot), utilizando para ello un control PID de
posición angular, luego un PID precompensado en aceleración, mientras que
en la tercera experiencia se efectúa la compensación de fricción en el
Pendubot.
4.5.1 Control PID de posición angular.
La ley de control del PID es :
Reconocimiento
83
u t Kp eq Ki eq dt Kddeqdt
( ) * * *= + +∫ (4.33)
donde eq q qd= − es el error de posición.
La señal de posición de referencia está compuesta por el producto de
dos senoidales con frecuencias f1= 0.025 Hz (período de 40 seg.) y f2=0.25 Hz
(período de 4 seg.) y amplitud de 5 [rad] como se muestra a continuación.
q t t td ( ) * sen( ) * sen( )= 5 1 2ω ω (4.34)
donde ω1 = 2πf1 y ω2 = 2πf2.
Las ganancias del control PID de posición utilizado son calculadas a
partir de (4.35) con los parámetros δ = 1, n = 1.2 y ωn = 20 rad/s .
( ) ( )( )sJ
Kd sKpJ
sKiJ
s s s nn n n3
22 2 21
2+ + + + = + + +α δω ω ω (4.35)
Reconocimiento
84
donde el valor de la fricción viscosa es α2 = 0.00012 Nm/(rad/s) y el momento
de inercia del eje del motor es J = 0.0004237 Kg m− 2 . Sin embargo, como se
realizará la compensación de fricción a partir del modelo dinámico
encontrado, la fricción viscosa no es tomada en cuenta para el cálculo de las
ganancias del controlador PID.
Por tanto, las ganancias para el controlador PID son las siguientes:
Kp = 0.57622, Ki = 4.06745, Kd = 0.02712 .
Así, a partir de (4.33) se obtiene el siguiente torque de control para la
posición angular del servomotor :
Γ = + +∫057622 4 06745 0 02712. * . * * . *eq eq dtdeqdt
(4.36)
La ganancia del observador para el estado interno z (ecuación 4.30) es
k = 0.0005 de forma tal poder disminuir el efecto de integración del error
debido al observador de fricción. A continuación (figura 4.12) se muestran
tanto la señal de referencia como la respuesta del sistema.
Reconocimiento
85
Figura 4.12 Trayectoria de referencia (azul) y respuesta (rojo)
La compensación de fricción es realizada a partir de los 20 segundos
de iniciado el control PID con el objeto de verificar el efecto producido sobre
el comportamiento del sistema. El código fuente de “Contpid.c” para realizar
este control se presenta en el Apéndice C.
Realizando un acercamiento de la gráfica de posición de referencia y
la respuesta del motor presentada en la figura 4.12 se puede apreciar la
mayor rapidez de respuesta del motor cuando se realiza la compensación de
fricción después de los 20 segundos (figura 4.13).
Reconocimiento
86
Figura 4.13 Posición de referencia (azul) y respuesta del motor (rojo).
Así, el error de posición disminuye levemente tal como se muestra en
la figura 4.14. Esto se debe a que la fricción es relativamente pequeña en los
cojinetes del servomotor DC.
Realizando una comparación numérica del error de posición antes y
después de realizar la compensación de fricción, arroja los siguientes
resultados :
* Sin compensación de fricción eq∑ = 19 9988. [rad]
* Con compensación de fricción eq∑ = 18 0930. [rad]
Lo cual es corroborado en la figura 4.14, donde se aprecia la
disminución en el error de posición a partir de los 20 seg.
Reconocimiento
87
Figura 4.14 Error de posición
La figura 4.15 presenta el torque aplicado por el control PID donde se
aprecia una disminución del mismo a partir del momento en que se realiza la
compensación de fricción.
Reconocimiento
88
Figura 4.15 Torque aplicado por el controlador PID
y fricción estimada
4.5.2 Control PID con prealimentación de aceleración.
En este caso, además de una referencia de posición se usan
referencias tanto para la velocidad como para la aceleración angular las
cuales se obtienen al derivar la posición de referencia dada por el producto de
las senoidales de (4.34). Por tanto, el torque de control está dada en la forma :
Γ = + − + − + −∫&& ( ) ( & &) ( )q Kp q q Kd q q Ki q qd d d d (4.37)
Reconocimiento
89
En este caso, la respuesta del motor sigue a la referencia con una
mayor precisión. A pesar de que la figura 4.16 presenta ambas señales
sobrepuestas, estas no se diferencian debido a la escala en la cual se
encuentran.
Figura 4.16 Señales de referencia (azul)
y respuesta (rojo) sobrepuestas.
Al realizar acercamientos de las señales presentadas anteriormente,
se puede apreciar, de una mejor forma, la efectividad del control PID con
prealimentación de aceleración.
Reconocimiento
90
La figura 4.17 presenta un acercamiento de la figura 4.16 para la
misma trayectoria de referencia antes y después de realizar la compensación
de fricción la cual se realiza a partir de los 20 segundos.
(a) (b)
Figura 4.17 a) Respuesta de posición sin compensación de fricción.
b) Respuesta de posición con compensación de fricción.
A continuación se presenta el error de posición (figura 4.18). En este
caso se logra una disminución significativa del error de posición respecto del
que se obtuvo con el controlador PID de posición. También se puede apreciar
claramente la disminución que experimenta a partir de los 20 seg., tiempo en
el cual se inicia la compensación de fricción.
Reconocimiento
91
En este caso, la comparación del error presenta los siguientes
resultados :
* Sin compensación de fricción eq∑ = 4 3377. [rad]
* Con compensación de fricción eq∑ = 3 4497. [rad]
Figura 4.18 Error de posición .
El torque calculado por el controlador PID precompensado, al igual
que el calculado por el PID, también experimenta una disminución en el
momento en que empieza la compensación de fricción a los 20 segundos. La
figura 4.19 presenta tanto el torque aplicado por el controlador como la
fricción estimada.
Reconocimiento
92
Figura 4.19 Torque aplicado por el control PID precompensado
y fricción estimada.
4.5.3 Compensación de fricción en el Pendubot.
La tercera experiencia tiene como objetivo verificar el efecto de la
compensación de fricción en el Pendubot. El control sobre el cual se realizó la
compensación fue para la configuración q q1 22 0= =π / , (ambas
articulaciones arriba) ya que es allí donde tiene mayor incidencia el efecto de
la fricción sobre el eje del motor DC. El código fuente “Comptop.c” para
realizar este control se reproduce en el Apéndice C.
La figura 4.20 presenta la respuesta de posición angular de la
articulación 1, tanto con la compensación de fricción (cc) como sin ella (sc).
Reconocimiento
93
La posición media de la articulación 1 sin compensación es de 1.5847
pero cuando se compensa la fricción es de 1.5759, lo cual representa un valor
mas cercano al teórico (π/2 = 1.5708), pero lo mas importante es que la
amplitud de la oscilación de la articulación experimenta una disminución, tal
como se muestra en el acercamiento de la figura 4.21.
Figura 4.20 Posición angular de articulación 1 sin compensación en
azul (sc) y con compensación de fricción en rojo (cc).
Reconocimiento
94
Figura 4.21 Acercamiento de la posición angular de la articulación 1,
tanto con compensación de fricción (cc) como sin ella (sc).
La figura 4.22 presenta la respuesta de posición angular de la
articulación 2 tanto con la compensación de fricción (en rojo) como sin ella (en
azul).
La respuesta es mucho mas precisa cuando se lleva a cabo la
compensación de fricción ya que es de -0.0058 rad, mientras que sin
compensación es de -0.0151 rad.
Reconocimiento
95
Figura 4.22 Posición angular de articulación 2 sin compensación (sc)
y con compensación de fricción (cc).
La figura 4.23 presenta las velocidades de ambas articulaciones tanto
con la compensación como sin ella.
Para la articulación 1, la velocidad angular es -0.0042 rad/seg sin
compensación de fricción (en azul), mientras que al realizar la compensación
es de 0.0015 rad/seg (en rojo). Por otra parte, la velocidad angular de la
articulación 2 es 0.0060 rad/seg sin compensación (azul) y de -0.0015 rad/seg
con compensación (rojo).
Reconocimiento
96
(a) (b)
Figura 4.23 a)Velocidad angular de articulación 1 sin compensación
(sc) y con compensación (cc). b) Velocidad angular de articulación 2 sin
compensación (sc) y con compensación de fricción (cc).
El torque aplicado por el control de realimentación del vector de
estados se presenta en la figura 4.24.
Cuando no es compensada la fricción, el control aplica un torque en
estado estacionario de -0.0085 Nm, mientras que al aplicar la compensación,
el torque disminuye a -0.0040 Nm, lo cual concuerda mejor con el torque
teórico de cero Nm.
Reconocimiento
97
Figura 4.24 Torque aplicado por el control sin compensación (azul) y
con compensación de fricción (rojo) respectivamente.
La fricción estimada para realizar la compensación es presentada en
la figura 4.25. Esta es aplicada en el momento en el cual son capturadas
ambas articulaciones por el control de balance ó equilibrio en la posición
vertical invertida ( / , )q q1 22 0= =π .
Reconocimiento
98
Figura 4.25 Fricción estimada para la compensación del Pendubot.
A continuación se muestra el Pendubot controlando ambas
articulaciones en la posición de equilibrio inestable ( / , )q q1 22 0= =π con la
adición de la compensación dinámica de fricción en el eje del motor DC.
Reconocimiento
99
CONCLUSIONES
A pesar de que la fricción es relativamente pequeña sobre el eje del
servomotor, el torque producido por ella debe tomarse en cuenta para el
diseño de controladores en los cuales se requieran ciertos niveles de
desempeño.
Esto fue demostrado a partir de los resultado arrojados por las
pruebas realizadas, las cuales ponen de manifiesto un incremento en la
precisión de los controladores cuando se incluye la compensación de fricción.
Si se toma en cuenta los tres tipos de experiencias efectuadas, tanto
con el motor DC como con el Pendubot, puede decirse que el modelo
dinámico de fricción efectúa una estimación bastante aproximada a la fricción
real presente en el eje del motor.
Reconocimiento
100
Las pruebas realizadas al motor ilustran perfectamente la
disminución del error de posición cuando la compensación de fricción se
lleva a cabo, tanto en regulación (balanceo del Pendubot) como en
seguimiento de trayectorias. Por consiguiente, este modelo dinámico puede
ser utilizado en posteriores diseños de controladores en los cuales se requiera
una compensación de fricción.
Sin embargo, es bueno destacar que a pesar de que el movimiento de
vaivén del Pendubot disminuye al compensar la fricción, este no puede
eliminarse por completo. La posible razón es que las señales de posición que
se miden en el sistema a través de los codificadores son utilizadas para
estimar la velocidad, en consecuencia, los cálculos tanto para el control como
para la compensación de fricción están sometidas a ruido y errores de
cuantización ya que los codificadores poseen resolución finita. Esto conlleva a
que los parámetros identificados tanto para el modelo dinámico del Pendubot
como para el modelo dinámico de fricción representan meras
aproximaciones de los reales, sin embargo, lo mismo sucede para cualquier
sistema real. Esto quiere decir que las experiencias realizadas en la presente
tesis han arrojado resultados positivos en cuanto a los objetivos planteados.
Reconocimiento
101
RECOMENDACIONES
El Pendubot ha demostrado ser un sistema con capacidad de
convertirse en una importante herramienta en el área de control, así que, es
necesario sacar el mayor provecho de ello.
Como se ha mencionado en oportunidades anteriores, en él se
pueden implementar varios esquemas de control. Por ejemplo, podría
implementarse la compensación de fricción utilizando un esquema
adaptativo. Otro punto interesante podría ser el control del Pendubot a través
de la tarjeta dSPACE la cual posee un DSP (Digital Signal Procesor) que
permite el control y ajuste de parámetros en tiempo real. Con ello se
superaría el inconveniente de ocupar exclusivamente el procesador del PC
cuando se realiza el control del Pendubot.
En general, es recomendable aprovechar todas la características que
ofrece el Pendubot para implementar los diferentes tipos o esquemas de
control.
Reconocimiento
102
REFERENCIAS
[1] Asada y Slotine, “Robot Analysis and Control”, John Wiley & Sons,
New York, pp. 93, 1986.
[2] Block, D.J., “Mechanical Design and Control of the Pendubot”, MS.
Thesis, University of Illinois at Urbana-Champaign, 1996.
[3] Canudas de Wit, C., Olsson, H., Astrom, K.J., Lischinsky, P., “A New
Model for Control of Systems with Friction”, IEEE TAC. Vol. 40, No.
3, pp. 419 - 424, March 1995.
[4] Craig, J., “Introduction to Robotics”, Addinson-Wesley, Mass., pp.
159, 1986.
Reconocimiento
103
[5] Lischinsky, P., “Compensation de Frottement et Commande en
Position d’un Robot Hydraulique Industriel”, Thése préparée au
sein du Laboratorie d’Automatique de Grenoble, pp. 47-108, 1997.
[6] Mechatronic Systems, Inc. “Pendubot Model P-2. User’s Manual”.
Reconocimiento
104
APENDICES
Reconocimiento
105
APENDICE A. IDENTIFICACION DE PARAMETROS
A.1 Archivo m de Matlab para identificar los parámetros del Pendubot.
La identificación de los parámetros del Pendubot a partir del método
de la Ecuación de Energía presentado en el capítulo II es implementado con
un programa desarrollado en Matlab. A continuación se presenta el archivo
M resultante. Es importante recordar que previamente se requiere el archivo
donde se ha guardado el torque, posición angular y velocidad angular del
sistema real, los cuales han sido recolectados al introducir al Pendubot una
señal escalón en lazo abierto.
Este programa no incluye el trabajo realizado por la fricción, pues
como ya se ha mencionado, el método no es bueno en la identificación de los
parámetros de fricción.
clear dL1 dL2 dL3 dL4 DL Itq taudq1
g=9.804;
dL1 = (.5*dq1.^2);
dL2 = (.5*dq1.^2 + dq1.*dq2 + .5*dq2.^2);
Reconocimiento
106
dL3 = (cos(q2).*(dq1.^2 + dq1.*dq2)) + ((g*sin(q1+q2))/(6.0*0.0254));
dL4 = (g*sin(q1));
taudq1 = tau.*dq1;
for i = 1:(length(dL1)-10),
DL(i,1) = dL1(i+10)-dL1(1);
DL(i,2) = dL2(i+10)-dL2(1);
DL(i,3) = dL3(i+10)-dL3(1);
DL(i,4) = dL4(i+10)-dL4(1);
% taking advantage of the fact that theta3 = m2*l1*lc2 and theta5 = m2*lc2
% since theta5 = theta3/l1 (l1 known) I only have to identify 4 parameters
%DL(i,5) = dL5(i+10)-dL5(1);
% this was an attempt at identifing friction also
% this identification method did not work well with friction terms added
%DL(i,6) = trapz(t(1:i+10,1),abs(dq1(1:i+10,1)));
%DL(i,7) = trapz(t(1:i+10,1),abs(dq2(1:i+10,1)));
%DL(i,8) = trapz(t(1:i+10,1),dq1(1:i+10,1).^2);
%DL(i,9) = trapz(t(1:i+10,1),dq2(1:i+10,1).^2);
Itq(i,1) = trapz(time(1:i+10,1),taudq1(1:i+10,1));
end
theta = nnls(DL,Itq);
theta(5) = theta(3)/(0.0254*6.0)
% you can also solve least squares like this in Matlab
%theta2 = DL\Itq
�
Reconocimiento
107
Utilizando este procedimiento de identificación se obtienen los
siguientes parámetros,
Θ120 0308= . Vs
Θ220 0106= . Vs
Θ320 0095= . Vs
Θ420 2087= . /Vs m
Θ520 0630= . /Vs m
Reconocimiento
108
APENDICE B.
LINEALIZACION DE ECUACIONES
DINAMICAS
Cuando se requiere linealizar las ecuaciones dinámicas no lineales del
Pendubot con el objeto de diseñar un controlador lineal alrededor de un
punto de equilibrio deseado, se puede emplear el siguiente programa hecho
en Matlab, el cual representa la forma simbólica de las expresiones de la
linealización del Pendubot. Requiere el vector de parámetros identificados
del sistema, así como los puntos deseados de equilibrio. Regresa A, B y ur.
function [A,B,ur] = linpendu(Theta,q1,q2)
%
% Format: [A,B,ur] = linpendu(Theta,q1,q2)
%
% This function finds the linear model for the pendubot about the
operating
Reconocimiento
109
% point {q1,0,q2,0,Theta(4)*g*cos(q1)}. This function uses SI units
% so g is equal to 9.804 m/s^2.
%
% Inputs:
% 1. Theta: vector containing pendubots parameters:
% Theta(1) = m1*lc1^2 + m2*l1^2 + I1
% Theta(2) = m2*lc2^2 + I2
% Theta(3) = m2*l1*lc2
% Theta(4) = m1*lc1 + m2*l1
% Theta(5) = m2*lc2
%
% 2. q1 and q2 are the desired operating points and their sum should
follow:
% |q1 + q2| = pi/2
%
% Outputs:
% 1. A and B the matrices in the linear equation for the Pendubot
% delta_xdot = A*delta_x + B*delta_tau;
% 2. ur: Torque needed to hold linkage at operating point
allok = 1;
if nargin ~= 3
allok = 0;
disp('linpendu needs 3 input arguments');
end
if nargout ~= 3
Reconocimiento
110
allok = 0;
disp('linpendu needs 3 output variables');
end
if allok == 1
[m,n] = size(Theta);
if ((m ~= 5 | n ~= 1) & (n ~= 5 | m ~= 1))
allok = 0;
disp('The dimension of Theta is incorrect; Theta must have 5
elements');
end
end
if allok == 1
[m,n] = size(q1);
if (m ~= 1 | n ~= 1)
allok = 0;
disp('The dimension of q1 is incorrect; q1 must be a scaler');
end
end
if allok == 1
[m,n] = size(q2);
if (n ~= 1 | m ~= 1)
allok = 0;
disp('The dimension of q2 is incorrect; q2 must be a scaler');
end
end
if allok == 1
Reconocimiento
111
if (abs(abs(q1 + q2) - pi/2) > .1)
allok = 0;
disp('Your operating point does not follow the rule |q1 + q2| =
pi/2');
end
end
if allok == 1
g=9.804;
X1 = Theta(1);
X2 = Theta(2);
X3 = Theta(3);
X4 = Theta(4);
X5 = Theta(5);
ur=X4*g*cos(q1);
den1=2*X1*X2-X3^2-X3^2*cos(2*q2);
df2dx1=(g*(2*X2*X4*sin(q1)-X3*X5*sin(q1)-
X3*X5*sin(q1+2*q2)))/den1;
equ1=X1*X2-(X3^2)/2-(X3^2*cos(2*q2))/2;
equ2=X2*ur-
g*(X2*X4*cos(q1)+(X3*X5*cos(q1))/2+(X3*X5*cos(q1+2*q2))/2);
df2dx3=-((X3^2*equ2*sin(2*q2))/equ1^2)+
(2*g*X3*X5*sin(q1+2*q2))/(-den1);
df4dx1=(g*X5*(X1+X2+2*X3*cos(q2))*sin(q1+q2)-
g*(X2+X3*cos(q2))*(X4*sin(q1)+X5*sin(q1+q2)))/equ1;
equ3=g*X5*(X1+X2+2*X3*cos(q2))*cos(q1+q2);
equ4=(X2+X3*cos(q2))*(-ur+g*X4*cos(q1)+g*X5*cos(q1+q2));
Reconocimiento
112
equ5=(g*X3*X4*sin(q1-q2))/2+X3*ur*sin(q2)-(g*X3*X4*sin(q1+q2))/2;
equ6=g*(X1*X5*sin(q1+q2)+X3*X5*sin(q1+2*q2));
df4dx3=-(X3^2*(-equ3+equ4)*sin(2*q2))/equ1^2+(equ5+equ6)/equ1;
df2du=X2/equ1;
df4du=(-X2-X3*cos(q2))/equ1;
A(2,1) = df2dx1;
A(2,3) = df2dx3;
A(4,1) = df4dx1;
A(4,3) = df4dx3;
B(2,1) = df2du;
B(4,1) = df4du;
A(1,2) = 1;
A(3,4) = 1;
else
disp('Error in variables entered type "help linpendu" for more info');
end
�
Este programa es una implementación del método de linealización de
Taylor.
f x u f x ufx
x xfu
u ua a r rx u
rx u
rr r r r
( , ) ( , ) ( ) ( ), ,
= + − + −∂∂
∂∂
u g xr r= Θ4 1cos( )
Reconocimiento
113
x xr r1 3 2+ = π
∂∂
∂∂
∂∂
∂∂
∂∂
fx
fx
fx
fx
fx
=
⎡
⎣
⎢⎢⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥⎥⎥
0 1 0 0
0 0
0 0 0 1
0 0
2
1
2
3
4
1
4
3
donde
[ ]∂
∂
f
x
g sin x r sin x r sin x r x r
x r
2
1
2 2 4 1 3 5 1 3 5 1 2 3
2 1 2 32
32
2 3
=− − +
− −
Θ Θ Θ Θ Θ Θ
Θ Θ Θ Θ
( ) ( ) ( )
cos( )
∂
∂
f
x
sin xr
ur
g xr
g xr
xr
sin xr
g xr
xr
xr
2
3
32 2
3 2 2 4 1
3 5 1
2
1 2
3
2
2
3
22
3
2
2
32 2
3
3 5 12
3
2
1 2
32
2
32 2
3
2
2=
− − +
− −
+
−
+
− −
+
⎡
⎣⎢⎢
⎤
⎦⎥⎥
⎡
⎣
⎢⎢
⎤
⎦
⎥⎥
⎡
⎣⎢
⎤
⎦⎥
⎡
⎣⎢⎢
⎤
⎦⎥⎥
Θ Θ Θ Θ
Θ Θ
Θ Θ
Θ Θ
Θ
Θ Θ
Θ Θ
Θ Θ
( ) cos( )
cos( )
cos( )
( )
cos( )
cos( )
+
+
− + +
23 5 1
23
21 2 3
232 2
3
g sin xr
xr
xr
Θ Θ
Θ Θ Θ Θ
( )
cos( )
[ ] [ ][ ]∂
∂
f
x
g sin xr
xr
xr
xr
g xr
sin xr
sin xr
xr
xr
4
1
5 1 3 1 22
3 3
1 2
32
2
32 2
3
2
2 3 3 4 1 5 1 3
1 2
32
2
32 2
3
2
=
+ + +
− −
−
+ + +
− −
Θ Θ Θ Θ
Θ Θ
Θ Θ
Θ Θ Θ Θ
Θ Θ
Θ Θ
( ) cos( )
cos( )
cos( ) ( ) ( )
cos( )
Reconocimiento
114
[ ][ ] [ ][ ]∂
∂
f
x
sin xr
g xr
xr
xr
xr
sin xr
xr
ur
g xr
g xr
xr
xr
4
3
32 2
3 5 1 3 1 22
3 3
1 2
32
2
32 2
3
2
2
32 2
3 2 3 3 4 1 5 1 3
1 2
32
2
32 2
3
2
2=
+ + +
− −
−
+ − + + +
− −
+
⎡
⎣⎢⎢
⎤
⎦⎥⎥
⎡
⎣⎢⎢
⎤
⎦⎥⎥
Θ Θ Θ Θ Θ
Θ Θ
Θ Θ
Θ Θ Θ Θ Θ
Θ Θ
Θ Θ
( ) cos( ) cos( )
cos( )
( ) cos( ) cos( ) cos( )
cos( )
g sin xr
xr
ursin x
r
g sin xr
xr
xr
g sin xr
xr
g sin xr
xr
xr
Θ Θ
Θ
Θ Θ
Θ Θ
Θ Θ
Θ Θ Θ Θ
Θ Θ
Θ Θ
3 4 1 3
2 3 3
3 4 1 3
2
1 2
32
2
32 2
3
2
1 2 1 2 3 5 12
3
1 2
32
2
32 2
3
2
( )
( )
( )
cos( )
( ) ( )
cos( )
−
+ −
+
− −
+
+ + +
− −
∂∂
∂∂
∂∂
fu
fu
fu
=
⎡
⎣
⎢⎢⎢⎢⎢
⎤
⎦
⎥⎥⎥⎥⎥
0
0
2
4
donde
∂
∂
f
u xr
2 2
1 2
32
2
32 2
3
2
=
− −
Θ
Θ Θ
Θ Θ cos( )
∂
∂
f
u
xr
xr
4 2 3 3
1 2
32
2
32 2
3
2
=
− −
− −
Θ Θ
Θ Θ
Θ Θ
cos( )
cos( )
Reconocimiento
115
APENDICE C.
PROGRAMAS FUENTES EN C.
C.1 Compilación de los programas fuentes.
La compilación de los programas es realizada con el compilador de 16
bits de Borland C++ (Ver. 5.0), la cual se realiza en el sistema operativo DOS
ya que los programas deben correr bajo este ambiente.
El primer paso es instalar ó establecer las variables del ambiente DOS
para la compilación de Borland C++. Para ello se debe correr un archivo
llamado “SETMSI BAT”. Este archivo, suministrado por el fabricante, posee
las especificaciones basadas en el lenguaje Microsoft C++, sin embargo, para
este trabajo de grado se instaló Borland C++, por lo que fue necesario
realizarle pequeñas modificaciones de adaptación.
Una vez que se ha corrido el archivo “SETMSI BAT” (el cual se
encuentra en el directorio C :\MSI\ ) se va al directorio en el cual se
encuentra el programa fuente que se desea compilar. Este directorio debe
Reconocimiento
116
contener, además del código fuente, “MSMAKE” el cual es un archivo por
lotes que llama al programa ejecutable del compilador con sus respectivas
directivas. Este archivo también fue modificado para este trabajo de grado, de
forma tal que funcionara correctamente con Borland C++. El contenido de
“MSMAKE” es el siguiente.
Bcc - Ic :\msi\include -Ic :\msvc\include -tD -ml -f87 %1.c
A continuación se explica el significado de este archivo.
Bcc : Bcc.exe es el programa compilador de 16 bits de Borland C++.
-Ic : Directiva de compilación para incluir el directorio que se encuentra
a continuación de ella (en este caso los “Include” tanto de “MSI” como de
“MSVC” donde se encuentra Borland C++).
-tD : Directiva que indica que el programa fuente se compilará para un
archivo DOS.EXE
-ml : Directiva para compilar usando el modelo de memoria grande.
-f87 : Directiva para generar código de punto flotante 8087.
%1.c : Es reemplazado automáticamente por el nombre del programa
que se desea compilar.
Reconocimiento
117
Por tanto, para compilar se introduce en la línea de comandos del DOS
“msmake <nombre del programa>”. El archivo por lotes llamará entonces a
“Bcc” para compilar y crear así el .EXE del programa controlador.
C.2 Programa fuente “baltop.c” usado para levantar ambas
articulaciones verticalmente.
// baltop.c // This executable swings up and balances the second link of the Pendubot // in the 'top' position // Usage: // At the command line type: // baltop <datafilename> <Kp> <Kd> <dither_ampl> <offset> // <datafilename> is the filename desired to save the response data // <Kp> is the desired value for the Swing Up controls Kp gain // <Kd> is the desired value for the Swing Up controls Kd gain // <dither_ampl> is the amplitude of the open loop dither signal added to // balancing control's output. // <offset> is the value added to the balancing control's output to // compensate for offsets in the balancing position. i.e. if // links are favoring a side you can add a small offset output to // get the links to balance closer to the operating point. // example: baltop mydata.m 50.0 8.8 0.0 0.0 /*****************************************************************************/ /* */ /* MECHATRONIC SYSTEMS INC */ /* */ /* Control Algorithm Starter Shell */ /* */ /* Read through all the comments given is this file to get started in */ /* developing your own control algorithm for the Pendubot. */
Reconocimiento
118
/* Also read through the comments in the given include files to help in your*/ /* understanding of the given source code */ /* For help in getting started you can look at the controllers supplied. */ /* It is always easiest to learn by example. */ /* balmid.c, baltop.c, tgainsch.c, mgainsch.c, etc */ /* */ /* To COMPILE use Microsoft C/C++ ver 7.0 or higher. */ /* First setup your DOS environment variables for MS C. i.e. use the given */ /* batch file setmsi.bat */ /* Then type at the dos prompt "msmake <filename> (with out the ".c") */ /* and the msmake batch file will compile and link your source code */ /* For the batch file to work correctly you will need to have set the */ /* appropriate environment variables for command line compiling. Your */ /* compiler documentation covers this and readme.1st talks about this. */ /* Msmake.bat compiles the code with these flags: */ /* cl -c -AL -FPi87 <your filename>.c */ /* link /STACK:0x8000 <your filename>,<your filename>,, */ /* */ /* Defines: */ /* SAMPLE: Desired sample rate in seconds: range 0.001-0.016seconds */ /* */ /* NUM_COMMANDLINE_PAR: This is the number of input parameters. */ /* This relates to the number of parameters you need to type at the command*/ /* line or to specify in your */ /* "ctl" file in the "msdev.exe" executable. If this number of parameters*/ /* is not entered at the command line the input parameters will be ignored*/ /* i.e. with BALMID: 'balmid data.m 150.0 21.0 4.5 1.15' */ /* NUM_COMMANDLINE_PAR is set to 5 */ /* !!! NOTE: The way this source file is written the first command line */ /* Parameter is always the data filename. */ /* */ /* NUM_POINTS: Number of data points you wish to save to file. Can't be */ /* greater than 8000. I don't suggest using 8000 data points though. */ /* Loading 8000 points into Matlab takes quite a bit of time. */ /* */ /* POINT_INCREMENT: Time interval at which data is collected. */ /* Units: (Sample Periods). In other words, if POINT_INCREMENT = 4 */ /* data will be saved every fourth sample period. */ /* */ /* INIT_RAD_ENC1 & INIT_RAD_ENC2: These are the rest positions of the */ /* Pendubot's links. These defines are used in the conversion from */ /* encoder counts to radians. They are put here just in case your control*/ /* needs to start at a different position than the hanging rest position. */
Reconocimiento
119
/* In most cases these parameters should not be changed and your control */ /* should start at the rest position. Even if you are just balancing */ /* the links, it is better to initialize the encoders in the hanging */ /* position and then tell the user to move the links to the balancing */ /* position and then start the control. */ /* */ /* */ /*****************************************************************************/ #define SAMPLE 0.005 /* sample period between .001 and .016 */ #define NUM_COMMANDLINE_PAR 5 /* here enter the number of variables you want to get */ /* at the command line */ #define NUM_POINTS 1000 /* number of data points to write to a data M-file */ /* must be less than 8000 */ #define POINT_INCREMENT 1 /* After POINT_INCREMENT time interval the */ /* the savedata function will save a data point */ /* to be save in datafile at the end of the control */ /* i.e. with SAMPLE = .005 and POINT_INCREMENT = 3 */ /* data will be save every .015 seconds */ #define PI 3.1415926 #define HALFPI 1.5707963 #define G 9.804 /* acceleration of gravity m/s^2 */ #define INIT_RAD_ENC1 -HALFPI /* Pendubot hanging position initial conditions */ #define INIT_RAD_ENC2 0.0 #include "devproj.h" /* Beginning of the main program */ void main(argc, argv) int argc; char *argv[]; { /* define and initialize variables here */ /* I start you out with the variables you are more than likely going to */ /* need when controlling the Pendubot, states x1-x4 and past values */ /* time and output u and data filename buffer */ float x1k=0.0; float x3k=0.0; float x1old = 0.0;
Reconocimiento
120
float x3old = 0.0; float x2k=0.0,x4k=0.0; float x2old1,x2old2; float x4old1,x4old2; float u,t; char datafile[13]; int catch; int pd; float K1=-16.4615,K2=-3.1287; /* balancing control gains */ float K3=-16.2422,K4=-2.0658; /* u = -Kx */ float Kp=50; /* swing up outer loop control gains */ float Kd=8.7; float P[5] = {0.0308,0.0106,0.0095,0.2087,0.0630}; /* identified parameters */ float d11,d12or21,d22; /* Partial feedback linearization gains */ float c11,c12,c21; float phi[2]; float v1,dbar,c1bar,c2bar,gbar,h; float dither_w=20.0,dither_ampl=0.25; /* dither input to get ride of some */ /* friction effects */ /* offset is a open loop voltage that is added to the balancing control */ /* to eliminate balance offset, i.e. not balancing exactly at the operating point */ float offset=0.0; /* Set DAC output to zero initially */ zeroDAC(); /* see devproj.h for a description of pause_message */ pause_message("Make sure the links are at rest and then press any key\n"); /* see devproj.h for a description of init_boards */ init_boards(); /* determine if the command line input should be processed */ /* and if so assign command line input to the appropriate variables */ /* the first command line input is always the data filename */ if (argc == (NUM_COMMANDLINE_PAR + 1)) { /* Allows user to enter gains at command prompt */ if (strlen(argv[1]) > 13) { printf("Improper data file name...Setting datafile name to default:data.m"); sprintf(datafile,"data.m"); } else { sprintf(datafile,"%s",argv[1]); } /* endif */ /* assign input to other desired variables here */
Reconocimiento
121
/* for example: "Kp = (float) atof(argv[2]);" */ Kp = (float) atof(argv[2]); Kd = (float) atof(argv[3]); dither_ampl = (float) atof(argv[4]); offset = (float) atof(argv[5]); } else { /* if no command line input or not the correct number assign a default filename */ sprintf(datafile,"data.m"); } /* endif */ /* print the input parameters to check they were received correctly */ printf("datafile = %s, Kp = %f, Kd = %f, dither_ampl = %f, offset = %f\n",datafile,Kp,Kd,dither_ampl,offset); /* Check that gains entered are within limits */ /* Note these limits are just checking for gross errors in the gains */ /* For example, negative Kd and Kd gains which are obviously unstable */ /* If you set all gains at their maximum limits the controller will NOT work */ /* You can of course tighten these gains limits if you want for your own */ /* Controllers. */ if ((Kp < 0.0) || (Kp > 500.0)) { printf("The Value Entered for Kp is outside of its limit (0.0<Kp<500.0) Exiting!\n"); myexit(); } /* endif */ if ((Kd < 0.0) || (Kd > 100.0)) { printf("The Value Entered for Kd is outside of its limit (0.0<Kd<100.0) Exiting!\n"); myexit(); } /* endif */ if ((dither_ampl < 0.0) || (dither_ampl > 1.0)) { printf("The Value Entered for dither_ampl is outside of its limit (0.0<dither_ampl<1.0) Exiting!\n"); myexit(); } /* endif */ if ((offset < -3.0) || (offset > 3.0)) { printf("The Value Entered for offset is outside of its limit (-3.0<offset<3.0) Exiting!\n"); myexit(); } /* endif */ x1old = -HALFPI; /* initialize position and velocity variables */ x3old = 0.0;
Reconocimiento
122
x2old1 = 0.0; x2old2 = 0.0; x4old1 = 0.0; x4old2 = 0.0; t = 0.0; u = 0.0; pd = 1; catch = 0; pause_message("Please hold man switch and press any key\n"); printf("Press Any Key to Stop Control\n"); /* start the timer counting */ /* see timerdb.h */ arm_counter(); /* start of the continuous control loop */ do { /* read the position of the links */ /* see encodrbd.h */ read_encoders(&x1k,&x3k); /* this is a precaution to check if the first link is moving too far */ /* away from its normal operating area. This helps prevent the first */ /* from swinging multiple times out of control. You can change this */ /* to whatever safety limits you feel appropriate. All safty_check does */ /* is see if x1k is within the limits given. If x1k is outside the limits */ /* DAC 0 is set to zero and the program is exited. */ safety_check(x1k,(float) PI,(float) -PI); /* calculate raw velocity */ x2k = (x1k-x1old)/SAMPLE; /* calculate velocity */ x4k = (x3k-x3old)/SAMPLE; /* filter velocity with an average */ x2k = (x2k+x2old1+x2old2)/3.0; /* average last 3 velocities to get */ x4k = (x4k+x4old1+x4old2)/3.0; /* ride of quantization noise */ /* put your control algorithm here */ /* this is the part that is up to you, with the states and past states given */ /* produce your control algorithm that outputs a control effort "u" in the */ /* range of + or - 10V. */ if (!catch) { /* if links have not come in range to balance */
Reconocimiento
123
if (fabs(x1k-HALFPI) < .10) { if (fabs(x3k) < .20) { u = -K1*(x1k-HALFPI) - K2*x2k - K3*x3k - K4*x4k; /* calculate balancing control */ if (fabs(u) < 6) { /* if balancing control check is not too large */ pd = 0; /* switch from swing up control to balance */ catch = 1; } /* endif */ } /* endif */ } /* endif */ } /* endif */ if (pd) { /* if swing up control is still in use */ d11 = P[0] + P[1] + 2*P[2]*cos(x3k); d12or21 = P[1] + P[2]*cos(x3k); d22 = P[1]; /* Calculate Lagrangian */ /* dynamics */ h = -P[2]*sin(x3k); c11 = h*x4k; c12 = h*x4k + h*x2k; c21 = -h*x2k; phi[0] = P[3]*G*cos(x1k) + P[4]*G*cos(x1k + x3k); phi[1] = P[4]*G*cos(x1k + x3k); dbar = d11 - (d12or21*d12or21/d22); /* Partial Feedback Linearization */ c1bar = c11 - (d12or21*c21/d22); c2bar = c12; gbar = phi[0] - (d12or21*phi[1]/d22); /* outer loop control */ v1 = -Kd*(x2k) + Kp*(HALFPI-x1k); /* inner loop control */ u = dbar*v1 + c1bar*x2k + c2bar*x4k + gbar; } else { /* perform balancing control */ u = -K1*(x1k-HALFPI) - K2*x2k - K3*x3k - K4*x4k; if (fabs(x1k-HALFPI) < .2) { u = u + dither_ampl*sin(dither_w*t) + offset; /* add dither signal */ } /* endif */ if (fabs(u) > 6) { /* if control gets too large switch back to */ /* swing up control which is servoing around */ /* a set point */ u = 0.0; /* set control to zero */ pd = 1;
Reconocimiento
124
} /* endif */ } /* endif */ /* limit output */ if (u > 9.95) u = 9.95; if (u < -9.95) u = -9.95; /* output control effort */ /* see dacbd.h */ out_DAC0(u); /* keep track of past states */ x2old2 = x2old1; /* save old positions and velocities */ x2old1 = x2k; x4old2 = x4old1; x4old1 = x4k; x1old = x1k; x3old = x3k; /* save up to 8 variables */ /* for the Matlab plotting function in msdev to work correctly the */ /* first 6 states must be t x1k x2k x3k x4k u and in that order */ /* the last two variables can be whatever you like and the Matlab */ /* plotting function ignores them. Of course if you want to do */ /* own Matlab plotting feel free to assign what ever variables you */ /* like to these 8 positions */ /* see devproj.h */ savedata(t,x1k,x2k,x3k,x4k,u,0.0,0.0); /* savedata needs 8 pars */ /* that is the reason for the zeros */ /* increment time */ t = t+SAMPLE; /* I use this to watch if a correct sample rate is being produced */ /* This routine toggles DIO output pin 0 each sample period. You */ /* can scope the output pin to monitor the sample rate created */ /* see timerbd.h */ UpdateDIO_Output_Pin0(); /* This function wait for the sample period to expire before returning */ /* See timerbd.h */ WaitForSample();
Reconocimiento
125
} /* Continue controlling until any key is hit */ while (!kbhit()); /* continue control until any key hit */ /* send zero to the DAC output */ /* see dacbd.h */ zeroDAC(); /* stop counter */ /* see timerbd.h */ disarm_counter(); /* write saved data to the filename specified at the command line */ /* see devproj.h */ savedatafile(datafile); /* write saved data to the file "dontremv.m" used in the Matlab plotting command */ /* see devproj.h */ savedataforMatlabplotting("c:\\matlab\\"); /* Free the memory allocated to save data */ /* see devproj.h */ myfree(); } /* end of program */ �
C.3 Programa fuente “balmid.c” usado para controlar las
articulaciones en la posición media.
// balmid.c // This executable swings up and balances the second link of the Pendubot // in the 'mid' position // Usage: // At the command line type: // balmid <datafilename> <Kp> <Kd> <w> <ampl> // <datafilename> is the filename desired to save the response data // <Kp> is the desired value for the Swing Up controls Kp gain
Reconocimiento
126
// <Kd> is the desired value for the Swing Up controls Kd gain // <w> is the frequency of the initial swing up pump // <ampl> is the amplitude of the initial swing up pump // example: balmid mydata.m 150.0 21.0 4.5 1.165 /*****************************************************************************/ /* */ /* MECHATRONIC SYSTEMS INC */ /* */ /* Control Algorithm Starter Shell */ /* */ /* Read through all the comments given is this file to get started in */ /* developing your own control algorithm for the Pendubot. */ /* Also read through the comments in the given include files to help in your*/ /* understanding of the given source code */ /* For help in getting started you can look at the controllers supplied. */ /* It is always easiest to learn by example. */ /* balmid.c, baltop.c, tgainsch.c, mgainsch.c, etc */ /* */ /* To COMPILE use Microsoft C/C++ ver 7.0 or higher. */ /* First setup your DOS environment variables for MS C. i.e. use the given */ /* batch file setmsi.bat */ /* Then type at the dos prompt "msmake <filename> (with out the ".c") */ /* and the msmake batch file will compile and link your source code */ /* For the batch file to work correctly you will need to have set the */ /* appropriate environment variables for command line compiling. Your */ /* compiler documentation covers this and readme.1st talks about this. */ /* Msmake.bat compiles the code with these flags: */ /* cl -c -AL -FPi87 <your filename>.c */ /* link /STACK:0x8000 <your filename>,<your filename>,, */ /* */ /* Defines: */ /* SAMPLE: Desired sample rate in seconds: range 0.001-0.016seconds */ /* */ /* NUM_COMMANDLINE_PAR: This is the number of input parameters. */ /* This relates to the number of parameters you need to type at the command*/ /* line or to specify in your */ /* "ctl" file in the "msdev.exe" executable. If this number of parameters*/ /* is not entered at the command line the input parameters will be ignored*/ /* i.e. with BALMID: 'balmid data.m 150.0 21.0 4.5 1.15' */ /* NUM_COMMANDLINE_PAR is set to 5 */ /* !!! NOTE: The way this source file is written the first command line */ /* Parameter is always the data filename. */
Reconocimiento
127
/* */ /* NUM_POINTS: Number of data points you wish to save to file. Can't be */ /* greater than 8000. I don't suggest using 8000 data points though. */ /* Loading 8000 points into Matlab takes quite a bit of time. */ /* */ /* POINT_INCREMENT: Time interval at which data is collected. */ /* Units: (Sample Periods). In other words, if POINT_INCREMENT = 4 */ /* data will be saved every fourth sample period. */ /* */ /* INIT_RAD_ENC1 & INIT_RAD_ENC2: These are the rest positions of the */ /* Pendubot's links. These defines are used in the conversion from */ /* encoder counts to radians. They are put here just in case your control*/ /* needs to start at a different position than the hanging rest position. */ /* In most cases these parameters should not be changed and your control */ /* should start at the rest position. Even if you are just balancing */ /* the links, it is better to initialize the encoders in the hanging */ /* position and then tell the user to move the links to the balancing */ /* position and then start the control. */ /* */ /* */ /*****************************************************************************/ #define SAMPLE 0.005 /* sample period between .001 and .016 */ #define NUM_COMMANDLINE_PAR 5 /* here enter the number of variables you want to get */ /* at the command line */ #define NUM_POINTS 1000 /* number of data points to write to a data M-file */ /* must be less than 8000 */ #define POINT_INCREMENT 1 /* After POINT_INCREMENT time interval the */ /* the savedata function will save a data point */ /* to be save in datafile at the end of the control */ /* i.e. with SAMPLE = .005 and POINT_INCREMENT = 3 */ /* data will be save every .015 seconds */ #define PI 3.1415926 #define HALFPI 1.5707963 #define G 9.804 /* acceleration of gravity m/s^2 */ #define INIT_RAD_ENC1 -HALFPI #define INIT_RAD_ENC2 0.0 #include "devproj.h"
Reconocimiento
128
void main(argc, argv) /* This program swing's the Pendubot from its hanging */ /* position to its middle position and balances it there */ /* middle position, joint1 = -1.57,joint2 = 3.14 */ int argc; char *argv[]; { /* define and initialize variables here */ /* I start you out with the variables you are more than likely going to */ /* need when controlling the Pendubot, states x1-x4 and past values */ /* time and output u and data filename buffer */ float x1k=0.0; float x3k=0.0; float x1old = 0.0; float x3old = 0.0; float x2k=0.0,x4k=0.0; float x2old1,x2old2; float x4old1,x4old2; float u,t; char datafile[13]; int catch; int pd; float K1=7.246,K2=0.7509; /* balancing control gains */ float K3=10.1643,K4=1.2591; /* u=-Kx */ float Kp=150.0; /* swing up outer loop control gains */ float Kd=21.0; float P[5] = {0.0308,0.0106,0.0095,0.2087,0.0630}; /* identified parameters */ float d11,d12or21,d22; /* Partial feedback linearization gains */ float c11,c12,c21; float phi[2]; float v1,dbar,c1bar,c2bar,gbar,h; float q1d[3],ts; float w=4.5,ampl=1.165; /* swing up trajectory u=ampl*sin(w*t) */ /* Set DAC output to zero initially */ zeroDAC(); /* see devproj.h for a description of pause_message */ pause_message("\n\nMake sure the links are at rest and then press any key\n"); /* see devproj.h for a description of init_boards */ init_boards(); /* determine if the command line input should be processed */ /* and if so assign command line input to the appropriate variables */ /* the first command line input is always the data filename */
Reconocimiento
129
if (argc == (NUM_COMMANDLINE_PAR + 1)) { /* Allows user to enter swing up gains at command prompt */ if (strlen(argv[1]) > 13) { printf("Improper data file name...Setting datafile name to default:data.m"); sprintf(datafile,"data.m"); } else { sprintf(datafile,"%s",argv[1]); } /* endif */ /* assign input to other desired variables here */ /* for example: "Kp = (float) atof(argv[2]);" */ Kp = (float) atof(argv[2]); Kd = (float) atof(argv[3]); w = (float) atof(argv[4]); ampl = (float) atof(argv[5]); } else { /* if no command line input or not the correct number assign a default filename */ sprintf(datafile,"data.m"); } /* endif */ /* print the input parameters to check they were received correctly */ printf("datafile = %s,Kp = %.4f, Kd = %.4f, w = %.4f, ampl = %.4f\n",datafile,Kp,Kd,w,ampl); /* Check that gains entered are within limits */ /* Note these limits are just checking for gross errors in the gains */ /* For example, negative Kd and Kd gains which are obviously unstable */ /* If you set all gains at their maximum limits the controller will NOT work */ /* You can of course tighten these gains limits if you want for your own */ /* Controllers. */ if ((Kp < 0.0) || (Kp > 500.0)) { printf("The Value Entered for Kp is outside of its limit (0.0<Kp<500.0) Exiting!\n"); myexit(); } /* endif */ if ((Kd < 0.0) || (Kd > 100.0)) { printf("The Value Entered for Kd is outside of its limit (0.0<Kd<100.0) Exiting!\n"); myexit(); } /* endif */ if ((w <= 0.0) || (w > 50.0)) { printf("The Value Entered for w is outside of its limit (0.0<=w<50.0) Exiting!\n"); myexit();
Reconocimiento
130
} /* endif */ if ((ampl < 0.0) || (ampl > 1.57)) { printf("The Value Entered for ampl is outside of its limit (0.0<ampl<1.57) Exiting!\n"); myexit(); } /* endif */ x1old = -HALFPI; /* initialize position and velocity variables */ x3old = 0.0; x2old1 = 0.0; x2old2 = 0.0; x4old1 = 0.0; x4old2 = 0.0; pd = 1; catch = 0; t = 0.0; u = 0.0; ts = 2*PI/w; /* time in swing up control that the trajectory is changed */ /* from a sinusoidal to a step */ pause_message("Please hold man switch and press any key\n"); printf("Press Any Key to Stop Control\n"); /* start the timer counting */ /* see timerdb.h */ arm_counter(); /* start of the continuous control loop */ do { /* read the position of the links */ /* see encodrbd.h */ read_encoders(&x1k,&x3k); /* this is a precaution to check if the first link is moving too far */ /* away from its normal operating area. This helps prevent the first */ /* from swinging multiple times out of control. You can change this */ /* to whatever safety limits you feel appropriate. All safty_check does */ /* is see if x1k is within the limits given. If x1k is outside the limits */ /* DAC 0 is set to zero and the program is exited. */ safety_check(x1k,(float) PI,(float) -PI);
Reconocimiento
131
/* calculate raw velocity */ x2k = (x1k-x1old)/SAMPLE; /* calculate velocity */ x4k = (x3k-x3old)/SAMPLE; /* filter velocity with an average */ x2k = (x2k+x2old1+x2old2)/3.0; /* average last 3 velocities to get */ x4k = (x4k+x4old1+x4old2)/3.0; /* ride of numerical noise */ /* put your control algorithm here */ /* this is the part that is up to you, with the states and past states given */ /* produce your control algorithm that outputs a control effort "u" in the */ /* range of + or - 10V. */ if (!catch) { /* if links have not come in range to balance */ if (fabs(x1k+HALFPI) < .20) { if (fabs(x3k-PI) < .30) { u = -K1*(x1k+HALFPI) - K2*x2k - K3*(x3k-PI) - K4*x4k; /* calculate balancing control */ if (fabs(u) < 8.0) { /* if balancing control check is not too large */ pd = 0; /* switch from swing up control to balance */ catch = 1; } /* endif */ } /* endif */ } /* endif */ } /* endif */ if (pd) { /* if swing up control is still in use */ d11 = P[0] + P[1] + 2*P[2]*cos(x3k); d12or21 = P[1] + P[2]*cos(x3k); d22 = P[1]; /* Calculate Lagrangian */ /* dynamics */ h = -P[2]*sin(x3k); c11 = h*x4k; c12 = h*x4k + h*x2k; c21 = -h*x2k; phi[0] = P[3]*G*cos(x1k) + P[4]*G*cos(x1k + x3k); phi[1] = P[4]*G*cos(x1k + x3k); dbar = d11 - (d12or21*d12or21/d22); /* Partial Feedback Linearization */ c1bar = c11 - (d12or21*c21/d22); c2bar = c12; gbar = phi[0] - (d12or21*phi[1]/d22); if (t<ts) { /* Trajectory for Pendubot to follow for swing up */
Reconocimiento
132
q1d[0] = ampl*sin(w*t)-HALFPI; q1d[1] = w*ampl*cos(w*t); q1d[2] = -w*w*ampl*sin(w*t); } else { q1d[0] = -HALFPI; q1d[1] = 0.0; q1d[2] = 0.0; } /* endif */ /* outer loop control */ v1 = q1d[2] + Kd*(q1d[1] - x2k) + Kp*(q1d[0]-x1k); /* inner loop control */ u = dbar*v1 + c1bar*x2k + c2bar*x4k + gbar; } else { /* perform balancing control */ u = -K1*(x1k+HALFPI) - K2*x2k - K3*(x3k-PI) - K4*x4k; if (fabs(u) > 8) { /* if control gets too large switch back to */ /* swing up control which is servoing around */ /* a set point */ u = 0.0; /* set control to zero */ pd = 1; } /* endif */ } /* endif */ /* limit output */ if (u > 9.95) u = 9.95; if (u < -9.95) u = -9.95; /* output control effort */ /* see dacbd.h */ out_DAC0(u); /* keep track of past states */ x2old2 = x2old1; /* save old positions and velocities */ x2old1 = x2k; x4old2 = x4old1; x4old1 = x4k; x1old = x1k; x3old = x3k; /* save up to 8 variables */ /* for the Matlab plotting function in msdev to work correctly the */ /* first 6 states must be t x1k x2k x3k x4k u and in that order */
Reconocimiento
133
/* the last two variables can be whatever you like and the Matlab */ /* plotting function ignores them. Of course if you want to do */ /* own Matlab plotting feel free to assign what ever variables you */ /* like to these 8 positions */ /* see devproj.h */ savedata(t,x1k,x2k,x3k,x4k,u,0.0,0.0); /* savedata needs 8 pars */ /* that is the reason for the zeros */ /* increment time */ t = t+SAMPLE; /* I use this to watch if a correct sample rate is being produced */ /* This routine toggles DIO output pin 0 each sample period. You */ /* can scope the output pin to monitor the sample rate created */ /* see timerbd.h */ UpdateDIO_Output_Pin0(); /* This function wait for the sample period to expire before returning */ /* See timerbd.h */ WaitForSample(); } /* Continue controlling until any key is hit */ while (!kbhit()); /* continue control until any key hit */ /* send zero to the DAC output */ /* see dacbd.h */ zeroDAC(); /* stop counter */ /* see timerbd.h */ disarm_counter(); /* write saved data to the filename specified at the command line */ /* see devproj.h */ savedatafile(datafile); /* write saved data to the file "dontremv.m" used in the Matlab plotting command */ /* see devproj.h */ savedataforMatlabplotting("c:\\matlab\\"); /* Free the memory allocated to save data */ /* see devproj.h */
Reconocimiento
134
myfree(); } /* end of program */ �
C.4 Programa fuente “motormod.c” utilizado para controlar la
velocidad angular del servomotor DC de imán permanente.
// Motormod.c // Este programa controla la velocidad angular del motor DC // utilizando un controlador PI. Versión modificada /*****************************************************************************/ /* */ /* PROYECTO DE GRADO (José I. Hidalgo) */ /* */ /* Definiciones: /* SAMPLE: Rango de muestreo deseado en segundos:rango 0.001-0.016seg */ /* */ /* NUM_COMMANDLINE_PAR: Número de parámetros de entrada. */ /* */ /* NUM_POINTS: Número de datos que se desean guardar en el archivo. */ /* */ /* POINT_INCREMENT: Intervalo de tiempo para recolectar los datos */ /* Unidades: (Períodos de Muestreo). Por ejemplo, si POINT_INCREMENT = 4 */ /* los datos se guardarán cada cuatro períodos. */ /* */ /* INIT_RAD_ENC1 & INIT_RAD_ENC2: Se refiere a las posiciones iniciales */ /* de los codificadores. */ /* */ /* */
Reconocimiento
135
/*****************************************************************************/ #define SAMPLE 0.005 /* período de muestreo entre .001 y .016 */ #define NUM_COMMANDLINE_PAR 4 /* número de variables a obtener */ /* en la línea de comandos (nombre archivo, */ /* velocidad angular de referencia wr, frec. */ /* natural de oscilación(wn) y factor de amort(si)*/ #define NUM_POINTS 1000 /*número de puntos a escribir al M-File */ /* debe ser menor de 8000 */ #define POINT_INCREMENT 4 /* los datos se guardan cada 0.02 seg */ /* es decir, cada 4 períodos de muestreo */ #define PI 3.1415926 #define INIT_RAD_ENC1 0.0 /* condición inicial del codificador1 del motor */ #define INIT_RAD_ENC2 0.0 #include "devproj.h" /* archivo .h que contiene los drivers de las tarjetas */ extern unsigned _stklen = 32768; /* definición de tamaño de stack */ /* Inicio del Programa Principal*/ void main(argc, argv) int argc; char *argv[]; { /* definición e inicialización de variables */ float x1k=0.0; /*posición angular del codificador1 (motor) */ float x1old = 0.0; float x3k = 0.0; /* posición para codificador2 (no necesario) */ float wk=0.0; /* velocidad angular del codificador1 */ float wold1,wold2; /* velocidades angulares anteriores */ float uk,t; /* torque y tiempo */ float U; /* voltaje de control enviado por la computadora */ char datafile[13]; float ek,eold; /* señal de error entre wr(referencia) y wk(salida motor)*/ float ie; /* integral del error */ float kp,ki; /* ganacias proporcional e integral */ float wn = 10; /* frec. natural de oscilación (10 por defecto) */ float si = 0.8; /* fact. de amort. del sistema (0.8 por defecto) */ float k,b; /* ganancia del sistema y fricción */ float wr = 10.0; /* variable para velocidad angular de referencia */ /* tiene 10 rad/seg por defecto */
Reconocimiento
136
float kamp = 1.2; /* ganancia del amplificador */ float kt = 0.4006; /* constante de torque */ /* Salida del DAC en cero inicialmente */ zeroDAC(); /* Inicializar tarjetas */ init_boards(); if (argc == (NUM_COMMANDLINE_PAR + 1)) { /* permite al usuario introducir */ /* nombre de archivo de datos, */ /* velocidad de referencia, wn y si */ if (strlen(argv[1]) > 13) { printf("Nombre incorrecto...Se coloca nombre por defecto:data.m"); sprintf(datafile,"data.m"); } else { sprintf(datafile,"%s",argv[1]); } /* endif */ /* asignar segunda entrada a velocidad angular de referencia */ wr=(float)atof(argv[2]); /* asignar tercera entrada a frec. nat. de oscilación (wn) */ wn=(float)atof(argv[3]); /* asignar cuarta entrada a fact. de amort. (si) */ si=(float)atof(argv[4]); } else { /* si no se introduce nada en la linea de comando ó no es el número */ /* correcto, asignar nombre de archivo por defecto */ sprintf(datafile,"data.m"); } /* endif */ /* presentar los parámetros de entrada para verificar que fueron */ /* recibidos correctamente */ printf("datafile = %s, wr = %f, wn = %f, si = %f\n\n",datafile,wr,wn,si); /* verificar que la velocidad angular wr esté entre los límites */ if ((wr < -40.0) || (wr > 40.0)) { printf("Los valores de wr están fuera de los límites (-40.0<wr<40.0) Exiting!\n"); myexit(); } /* endif */ k = 2360.2; /* ganancia del sistema =1/J , donde J=0.0004237 */
Reconocimiento
137
b = 0.28; /* fricción ( b = 0.00012/0.0004237 ) */ kp = (2*si*wn-b)/k; /* cálculo de la ganancia de control proporcional */ ki = (wn*wn)/k; /* cálculo de la ganancia de control integral */ /* inicializar variables */ x1old = 0.0; /* posición angular del decodificador del motor */ wold1 = 0.0; /* velocidades angulares anteriores */ wold2 = 0.0; t = 0.0; /* tiempo */ uk = 0.0; /* torque */ ek = 0.0; /* señal de error entre wr y wk */ eold = 0.0; /* señal de error anterior */ ie = 0.0; /* integral del error */ U = 0.0; /* voltaje de control enviado por la tarjeta DA */ pause_message("\nPresione switch de Pendubot y presione cualquier tecla\n"); printf("Presione cualquier tecla para detener el control\n"); /* inicio de conteo del cronómetro (timer) */ /* see timerdb.h */ arm_counter(); /* inicio del lazo contínuo de control */ do { /* leer posición angular del eje del motor DC */ read_encoders(&x1k,&x3k); /* cálculo de la velocidad */ wk = (x1k-x1old)/SAMPLE; /* filtrar velocidad utilizando un promedio */ wk = (wk+wold1+wold2)/3.0; /* promedio de las 3 últimas velocidades*/ ek = wr-wk; /* señal error entre la referencia y la salida del sistema */ ie = ie + SAMPLE/2*(ek + eold); /* integral del error */ uk = kp*ek + ki*ie; /* torque de control */ U = uk/(kamp*kt); /* voltaje de control */ /* límites de salida */ if (U>9.95) U = 9.95; if (U<-9.95) U = -9.95; /* salida de la señal de control */
Reconocimiento
138
out_DAC0(U); /* mantener los estados pasados */ wold2 = wold1; /* guardar velocidades anteriores */ wold1 = wk; x1old = x1k; /* posición angular */ eold = ek; /* señal de error */ /* guardar las variables para graficar en Matlab */ savedata(t,wk,wr,uk,U,ek,ie,0.0); /* savedata necesita 8 parámetros */ /* incremento de tiempo */ t = t+SAMPLE; UpdateDIO_Output_Pin0(); /* esta función espera que el período de muestreo expire antes de retornar*/ WaitForSample(); } /* Continuar controlando hasta que se presione una tecla */ while (!kbhit()); /* envía cero a la salida del DAC */ zeroDAC(); /* detener el contador */ disarm_counter(); /* escribir los datos guardados al nombre de archivo especificado */ savedatafile(datafile); /* escribe los datos guardados en el archivo "dontremv.m" usado en el */ /* comando de plotting de Matlab */ savedataforMatlabplotting("c:\\matlab\\"); /* libera memoria reservada para guardar datos */ myfree(); } /* fin del programa */ �
Reconocimiento
139
C.5 Programa fuente “Codresol.c” utilizado para calcular los
parámetros dinámicos de fricción.
// Codresol.c // Este programa envía una señal rampa,con cierta pendiente, al motor DC // para determinar la resolución del codificador /*****************************************************************************/ /* */ /* PROYECTO DE GRADO (José I. Hidalgo) */ /* */ /* Definiciones: /* SAMPLE: Rango de muestreo deseado en segundos:rango 0.001-0.016seg */ /* */ /* NUM_COMMANDLINE_PAR: Número de parámetros de entrada. */ /* */ /* NUM_POINTS: Número de datos que se desean guardar en el archivo. */ /* */ /* POINT_INCREMENT: Intervalo de tiempo para recolectar los datos */ /* Unidades: (Períodos de Muestreo). Por ejemplo, si POINT_INCREMENT = 4 */ /* los datos se guardarán cada cuatro períodos. */ /* */ /* INIT_RAD_ENC1 & INIT_RAD_ENC2: Se refiere a las posiciones iniciales */ /* de los codificadores. */ /* */ /* */ /*****************************************************************************/ #define SAMPLE 0.005 /* período de muestreo entre .001 y .016 */ #define NUM_COMMANDLINE_PAR 2 /* parámetros a introducir en la */ /* línea de comandos (nombre del archivo.m /* y la pendiente "C" de la */ /* señal de torque tau = Ct) */ #define NUM_POINTS 1000 /*número de puntos a escribir al M-File */ /* debe ser menor de 8000 */
Reconocimiento
140
#define POINT_INCREMENT 6 /* los datos se guardan cada 0.03 seg */ /* es decir, cada 6 períodos de muestreo, lo que */ /* lleva aproximadamente 30 seg. */ #define PI 3.1415926 #define INIT_RAD_ENC1 0.0 /* condición inicial del codificador1 del motor */ #define INIT_RAD_ENC2 0.0 #include "devproj.h" /* archivo .h que contiene los drivers de las tarjetas */ extern unsigned _stklen = 32768; /* definición de tamaño de stack */ /* Inicio del Programa Principal*/ void main(argc, argv) int argc; char *argv[]; { /* definición e inicialización de variables */ float x1k=0.0; /*posición angular del codificador1 (motor) */ float x1old = 0.0; float x3k = 0.0; /* posición para codificador2 (no necesario) */ float tau,t,C; /* torque,tiempo y pendiente de la señal de torque */ float U; /* voltaje de control enviado por la computadora */ char datafile[13]; float kamp = 1.2; /* constante del amplificador */ float kt = 0.4006; /* constante de torque */ /* Salida del DAC en cero inicialmente */ zeroDAC(); /* Inicializar tarjetas */ init_boards(); if (argc == (NUM_COMMANDLINE_PAR + 1)) { /* permite al usuario introducir */ /* nombre de archivo de datos y */ /* pendiente de la señal de torque*/ if (strlen(argv[1]) > 13) { printf("Nombre incorrecto...Se coloca nombre por defecto:data.m"); sprintf(datafile,"data.m"); } else { sprintf(datafile,"%s",argv[1]); } /* endif */
Reconocimiento
141
/* asignar entrada a la pendiente C de la señal de torque */ C =(float)atof(argv[2]); } else { /* si no se introduce nada en la linea de comando ó no es el número */ /* correcto, asignar nombre de archivo por defecto */ sprintf(datafile,"data.m"); } /* endif */ /* presentar los parámetros de entrada para verificar que fueron */ /* recibidos correctamente */ printf("datafile = %s, C = %f\n\n",datafile,C); /* verificar que la velocidad angular wr esté entre los límites */ /* inicializar variables */ t = 0.0; /* tiempo */ tau = 0.0; /* torque */ pause_message("\nPresione switch de Pendubot y presione cualquier tecla\n"); printf("Presione cualquier tecla para detener el programa\n"); /* inicio de conteo del cronómetro (timer) */ /* see timerdb.h */ arm_counter(); /* inicio del lazo contínuo de control */ do { /* leer posición angular del eje del motor DC */ read_encoders(&x1k,&x3k); /* señal de torque */ tau = C*t; /* señal de voltaje hacia la tarjeta DA */ U = tau/(kamp*kt); /* límites de salida */ if (U>9.95) U = 9.95; if (U<-9.95) U = -9.95; /* salida de la señal de control */ out_DAC0(U);
Reconocimiento
142
/* guardar las variables para graficar en Matlab */ savedata(t,tau,U,x1k,C,0.0,0.0,0.0); /* savedata necesita 8 parámetros */ /* mantener los estados pasados */ x1old = x1k; /* posición angular */ uold = uk; /* torques */ /* incremento de tiempo */ t = t+SAMPLE; UpdateDIO_Output_Pin0(); /* esta función espera que el período de muestreo expire antes de retornar*/ WaitForSample(); } /* Continuar controlando hasta que se presione una tecla */ while (!kbhit()); /* envía cero a la salida del DAC */ zeroDAC(); /* detener el contador */ disarm_counter(); /* escribir los datos guardados al nombre de archivo especificado */ savedatafile(datafile); /* escribe los datos guardados en el archivo "dontremv.m" usado en el */ /* comando de plotting de Matlab */ savedataforMatlabplotting("c:\\matlab\\"); /* libera memoria reservada para guardar datos */ myfree(); } /* fin del programa */ �
Reconocimiento
143
C.6 Programa fuente “Contpid.c” utilizado para realizar
compensación de fricción dinámica del servomotor, con un PID.
// contpid.c // Este programa efectúa un control PID para compensar fricción // utilizando solamente el motor DC. 12/6/00 /******************************************************************
*/ /* */ /* PROYECTO DE GRADO (José I. Hidalgo) */ /* */ /* Definiciones: /* SAMPLE: Rango de muestreo deseado en segundos:rango 0.001-0.016seg
*/ /* */ /* NUM_COMMANDLINE_PAR: Número de parámetros de entrada. */ /* */ /* NUM_POINTS: Número de datos que se desean guardar en el archivo. */ /* */ /* POINT_INCREMENT: Intervalo de tiempo para recolectar los datos */ /* Unidades: (Períodos de Muestreo). Por ejemplo, si POINT_INCREMENT =
4*/ /* los datos se guardarán cada cuatro períodos. */ /* */ /* INIT_RAD_ENC1 & INIT_RAD_ENC2: Se refiere a las posiciones iniciales
*/ /* de los codificadores. */ /* */ /* */ /******************************************************************
*/ #define SAMPLE 0.005 /* período de muestreo entre .001 y .016 */ #define NUM_COMMANDLINE_PAR 1 /* parámetros a introducir en la */ /* línea de comandos (nombre del archivo.m ) */ #define NUM_POINTS 1000 /*número de puntos a escribir al M-File */ /* debe ser menor de 8000 */ #define POINT_INCREMENT 8 /* los datos se guardan cada 0.04 seg */
Reconocimiento
144
/* es decir, cada 8 períodos de muestreo, lo que */ /* lleva aproximadamente 40 seg. */ #define PI 3.1415926 #define INIT_RAD_ENC1 0.0 /* condición inicial del codificador1 del motor */ #define INIT_RAD_ENC2 0.0 #define per_med (SAMPLE/2) #define resol_cod 0.001257 /* resolución del codificador */ #define resol_vel (resol_cod/SAMPLE) /* resolución de velocidad */ #include "devproj.h" /* archivo .h que contiene los drivers de las tarjetas */ extern unsigned _stklen = 32768; /* definición de tamaño de stack */ /* Inicio del Programa Principal*/ void main(argc, argv) int argc; char *argv[]; { /* definición e inicialización de variables */ float x1k=0.0; /*posición angular del codificador1 (motor) */ float x1old = 0.0; float x3k = 0.0; /* posición para codificador2 (no necesario) */ float x2k = 0.0; /* velocidad angular del eje del motor */ float x2old1,x2old2; float t; float amp_r_sin = 5.0; /* amplitud de la señal senoidal de referencia */ float f1 = 0.025; /* frecuencia 1 de la señal senoidal */ float f2 = 0.25; /* frecuencia 2 de la señal senoidal */ float w1 = 0.1570796; /* 2*pi*f1 */ float w2 = 1.570796; float up=0.0,ud=0.0,ui=0.0; /* señal proporcional,derivativa e integral */ float u,uv; char datafile[13]; float kamp = 1.2; /* constante del amplificador */ float kt = 0.4006; /* constante de torque */ float alfa_i = 1.2; float psi = 1.0; float wn = 20.0; float kp=0.0,ki=0.0,kd=0.0; float e_q=0.0,ref_q=0.0; float e_qold = 0.0; float ie_q=0.0; /* integral del error de posición */ float alfp0 = 0.0028; float alfp1 = 0.007;
Reconocimiento
145
float alfp2 = 0.00012; float vop = 0.01; float alfn0 = 0.0299; float alfn1 = 0.0041; float alfn2 = 0.00012; float von = -0.04; float gv; float z=0.0,zp=0.0; float zold,zpold; float f_zp=0.0,f_z=0.0,f_v=0.0; float SGM0 = 0.7818; float SGM1 = 0.0363; /* SGM1 = 2*sqrt(SGM0*J)-alfp2 */ float a1=0.0,a2=0.0,a3=0.0,a4=0.0; float f_est,f_est_v; float k_obs = 0.0001; float v,vold; /* variable auxiliar de velocidad */ /* Salida del DAC en cero inicialmente */ zeroDAC(); /* Inicializar tarjetas */ init_boards(); if (argc == (NUM_COMMANDLINE_PAR + 1)) { /*permite al usuario
introducir /* nombre de archivo de datos y */ /* pendiente de la señal de torque*/ if (strlen(argv[1]) > 13) { printf("Nombre incorrecto...Se coloca nombre por defecto:data.m"); sprintf(datafile,"data.m"); } else { sprintf(datafile,"%s",argv[1]); } /* fin del if */ } else { /* si no se introduce nada en la linea de comando ó no es el número */ /* correcto, asignar nombre de archivo por defecto */ sprintf(datafile,"data.m"); } /* fin del if */ /* presentar los parámetros de entrada para verificar que fueron */ /* recibidos correctamente */ printf("datafile = %s\n\n",datafile);
Reconocimiento
146
/* cálculo de las ganancias del PID */ kp = (wn*wn+2*psi*alfa_i*wn*wn)/2360.2; ki = (alfa_i*wn*wn*wn)/2360.2; kd = (2*psi*wn+alfa_i*wn-0.28)/2360.2; /* inicializar variables */ t = 0.0; /* tiempo */ u = 0.0; /* señal de control */ uv = 0.0; /* voltaje de control */ x2k = 0.0; /* velocidad angular del eje del motor DC */ x2old1 = 0.0; x2old2 = 0.0; x1k = 0.0; /* posición angular del eje del motor DC */ x1old = 0.0; f_est = 0.0; /* fricción estimada en Nm */ f_est_v = 0.0; /* fricción estimada en voltios */ z = 0.0; /* estado interno de fricción */ zp = 0.0; /* derivada del estado interno */ zold = 0.0; zpold = 0.0; v = 0.0; /* variable auxiliar de velocidad angular */ e_q = 0.0; e_qold = 0.0; pause_message("\nPresione switch de Pendubot y presione cualquier tecla\n"); printf("Presione cualquier tecla para detener el programa\n"); /* inicio de conteo del cronómetro (timer) */ /* ver timerdb.h */ arm_counter(); /* inicio del lazo contínuo de control */ do { /* referencia de posición */ a1 = w1*t; a2 = w2*t; a3 = sin(a1); a4 = sin(a2); ref_q = amp_r_sin*a3*a4; /* leer posición angular del eje del motor DC */ read_encoders(&x1k,&x3k);
Reconocimiento
147
/* error de posición en radianes */ e_q = ref_q - x1k; /* velocidad angular del eje del motor DC */ x2k = (x1k-x1old)/SAMPLE; x2k = (x2k+x2old1+x2old2)/3.0; /* integral del error de posición */ ie_q += e_q*SAMPLE; /* control proporcional */ up = kp*e_q; /* control derivativo */ ud = -kd*x2k; /* control integral */ ui = ki*ie_q; if (t <= 20.0){ /* señal de control PID */ u = up + ui + ud; uv = u/(kamp*kt); /* control sin compensación de fricción */ }else{ u = up + ui + ud; /* control en unidades de torque */ uv = u/(kamp*kt); /* control en unidades de voltaje */ v = x2k; /* asignar velocidad a variable auxiliar */ /* cálculo de la fricción */ if (v > 0.0){ gv = alfp0+alfp1*exp(-((v*v)/(vop*vop))); }else{ gv = alfn0+alfn1*exp(-((v*v)/(von*von))); } /* fin del if */ /*if (fabs(v)<= resol_vel){ zp = 0.0; v = 0.0; }else { */
z = (zold + per_med*(zpold + v - k_obs*e_qold))/(1 + per_med*SGM0*fabs(v)/gv); zp = v - SGM0*fabs(v)*z/gv + k_obs*e_q; /*} fin del if */ f_z = SGM0*z; f_zp = SGM1*zp; f_v = alfp2*v; f_est = f_z + f_zp + f_v; /* estimación de la fricción */ f_est_v = f_est/(kamp*kt);
Reconocimiento
148
/* señal de control con compensación */ uv = uv + f_est_v; } /* fin del if */ /* límites de salida */ if (uv > 2.0) uv = 2.0; if (uv < -2.0) uv = -2.0; /* salida de la señal de control */ out_DAC0(uv); /* mantener los estados pasados */ x1old = x1k; /* posición angular */ x2old2 = x2old1; x2old1 = x2k; zold = z; zpold = zp; e_qold = e_q; /* guardar las variables para graficar en Matlab */
savedata(t,uv,f_est_v,x1k,e_q,ref_q,x2k,f_est); /* savedata necesita 8 parámetros */ /* incremento de tiempo */ t = t+SAMPLE; UpdateDIO_Output_Pin0(); /* esta función espera que el período de muestreo expire antes de retornar*/ WaitForSample(); } /* Continuar controlando hasta que se presione una tecla */ while (!kbhit()); /* envía cero a la salida del DAC */ zeroDAC(); /* detener el contador */ disarm_counter(); /* escribir los datos guardados al nombre de archivo especificado */
Reconocimiento
149
savedatafile(datafile); /* escribe los datos guardados en el archivo "dontremv.m" usado en el */ /* comando de plotting de Matlab */ savedataforMatlabplotting("c:\\matlab\\"); /* libera memoria reservada para guardar datos */ myfree(); } /* fin del programa */ �
C.7 Programa fuente “Precomp.c” utilizado para realizar
compensación de fricción dinámica del motor con un PID precompensado.
// precomp.c // Este programa efectúa un control PID precompensado para compensar fricción // utilizando solamente el motor DC. 4/7/00 /*****************************************************************************/ /* */ /* PROYECTO DE GRADO (José I. Hidalgo) */ /* */ /* Definiciones: /* SAMPLE: Rango de muestreo deseado en segundos:rango 0.001-0.016seg */ /* */ /* NUM_COMMANDLINE_PAR: Número de parámetros de entrada. */ /* */ /* NUM_POINTS: Número de datos que se desean guardar en el archivo. */ /* */ /* POINT_INCREMENT: Intervalo de tiempo para recolectar los datos */ /* Unidades: (Períodos de Muestreo). Por ejemplo, si POINT_INCREMENT = 4 */ /* los datos se guardarán cada cuatro períodos. */ /* */ /* INIT_RAD_ENC1 & INIT_RAD_ENC2: Se refiere a las posiciones iniciales */ /* de los codificadores. */ /* */ /* */
Reconocimiento
150
/*****************************************************************************/ #define SAMPLE 0.005 /* período de muestreo entre .001 y .016 */ #define NUM_COMMANDLINE_PAR 1 /* parámetros a introducir en la */ /* línea de comandos (nombre del archivo.m ) */ #define NUM_POINTS 1000 /*número de puntos a escribir al M-File */ /* debe ser menor de 8000 */ #define POINT_INCREMENT 8 /* los datos se guardan cada 0.04 seg */ /* es decir, cada 8 períodos de muestreo, lo que */ /* lleva aproximadamente 40 seg. */ #define PI 3.1415926 #define INIT_RAD_ENC1 0.0 /* condición inicial del codificador1 del motor */ #define INIT_RAD_ENC2 0.0 #define per_med (SAMPLE/2) #define resol_cod 0.001257 /* resolución del codificador */ #define resol_vel (resol_cod/SAMPLE) /* resolución de velocidad */ #include "devproj.h" /* archivo .h que contiene los drivers de las tarjetas */ extern unsigned _stklen = 32768; /* definición de tamaño de stack */ /* Inicio del Programa Principal*/ void main(argc, argv) int argc; char *argv[]; { /* definición e inicialización de variables */ float x1k=0.0; /*posición angular del codificador1 (motor) */ float x1old = 0.0; float x3k = 0.0; /* posición para codificador2 (no necesario) */ float x2k = 0.0; /* velocidad angular del eje del motor */ float x2old1,x2old2; float t; float amp_r_sin = 5.0; /* amplitud de la señal senoidal de referencia */ float f1 = 0.025; /* frecuencia 1 de la señal senoidal */ float f2 = 0.25; /* frecuencia 2 de la señal senoidal */ float w1 = 0.1570796; /* 2*pi*f1 */ float w2 = 1.570796; float up=0.0,ud=0.0,ui=0.0; /* señal proporcional,derivativa e integral */ float u,uv; char datafile[13]; float kamp = 1.2; /* constante del amplificador */
Reconocimiento
151
float kt = 0.4006; /* constante de torque */ float alfa_i = 1.2; float psi = 1.0; float wn = 20.0; float kp=0.0,ki=0.0,kd=0.0; float e_q=0.0,ref_q=0.0; float e_qold = 0.0; float ie_q=0.0; /* integral del error de posición */ float ref_qp=0.0,ref_qpp=0.0; /* ref. de velocidad y aceleración */ float e_qp=0.0; /* error de velocidad */ float alfp0 = 0.0028; float alfp1 = 0.007; float alfp2 = 0.00012; float vop = 0.01; float alfn0 = 0.0299; float alfn1 = 0.0041; float alfn2 = 0.00012; float von = -0.04; float gv; float z=0.0,zp=0.0; float zold,zpold; float f_zp=0.0,f_z=0.0,f_v=0.0; float SGM0 = 0.7818; float SGM1 = 0.0363; /* SGM1 = 2*sqrt(SGM0*J)-alfp2 */ float a1=0.0,a2=0.0,a3=0.0,a4=0.0,a5=0.0,a6=0.0; float f_est,f_est_v; float k_obs = 0.0005; float v,vold; /* variable auxiliar de velocidad */ float uacc = 0.0; float J = 0.0004237; /* momento de inercia del eje del motor Kg-m^2*/ /* Salida del DAC en cero inicialmente */ zeroDAC(); /* Inicializar tarjetas */ init_boards(); if (argc == (NUM_COMMANDLINE_PAR + 1)) { /* permite al usuario introducir */ /* nombre de archivo de datos y */ /* pendiente de la señal de torque*/ if (strlen(argv[1]) > 13) { printf("Nombre incorrecto...Se coloca nombre por defecto:data.m");
Reconocimiento
152
sprintf(datafile,"data.m"); } else { sprintf(datafile,"%s",argv[1]); } /* fin del if */ } else { /* si no se introduce nada en la linea de comando ó no es el número */ /* correcto, asignar nombre de archivo por defecto */ sprintf(datafile,"data.m"); } /* fin del if */ /* presentar los parámetros de entrada para verificar que fueron */ /* recibidos correctamente */ printf("datafile = %s\n\n",datafile); /* cálculo de las ganancias del PID precompensado */ kp = (2*psi*alfa_i+1)*wn*wn; ki = alfa_i*wn*wn*wn; kd = (2*psi+alfa_i)*wn; /* inicializar variables */ t = 0.0; /* tiempo */ u = 0.0; /* señal de control */ uv = 0.0; /* voltaje de control */ x2k = 0.0; /* velocidad angular del eje del motor DC */ x2old1 = 0.0; x2old2 = 0.0; x1k = 0.0; /* posición angular del eje del motor DC */ x1old = 0.0; f_est = 0.0; /* fricción estimada en Nm */ f_est_v = 0.0; /* fricción estimada en voltios */ z = 0.0; /* estado interno de fricción */ zp = 0.0; /* derivada del estado interno */ zold = 0.0; zpold = 0.0; v = 0.0; /* variable auxiliar de velocidad angular */ e_q = 0.0; e_qold = 0.0; uacc = 0.0; pause_message("\nPresione switch de Pendubot y presione cualquier tecla\n"); printf("Presione cualquier tecla para detener el programa\n");
Reconocimiento
153
/* inicio de conteo del cronómetro (timer) */ /* ver timerdb.h */ arm_counter(); /* inicio del lazo contínuo de control */ do { /* referencia de posición, velocidad y aceleración */ a1 = w1*t; a2 = w2*t; a3 = sin(a1); a4 = sin(a2); a5 = cos(a1); a6 = cos(a2); ref_q = amp_r_sin*a3*a4; ref_qp = amp_r_sin*(a6*a3*w2+w1*a5*a4); ref_qpp = amp_r_sin*((w2*w2+w1*w1)*(-a3*a4)+2*w1*w2*a5*a6); /* leer posición angular del eje del motor DC */ read_encoders(&x1k,&x3k); /* error de posición en radianes */ e_q = ref_q - x1k; /* velocidad angular del eje del motor DC */ x2k = (x1k-x1old)/SAMPLE; x2k = (x2k+x2old1+x2old2)/3.0; /* error de velocidad */ e_qp = ref_qp - x2k; /* integral del error de posición */ ie_q += e_q*SAMPLE; /* control de aceleración */ uacc = J*ref_qpp; /* control proporcional */ up = J*kp*e_q; /* control derivativo */ ud = J*kd*e_qp; /* control integral */ ui = J*ki*ie_q; if (t <= 20.0){ /* señal de control PID */ u = uacc + up + ui + ud;
Reconocimiento
154
uv = u/(kamp*kt); /* control sin compensación de fricción */ }else{ u = uacc + up + ui + ud; /* control en unidades de torque */ uv = u/(kamp*kt); /* control en unidades de voltaje */ v = x2k; /* asignar velocidad a variable auxiliar */ /* cálculo de la fricción */ if (v > 0.0){ gv = alfp0+alfp1*exp(-((v*v)/(vop*vop))); }else{ gv = alfn0+alfn1*exp(-((v*v)/(von*von))); } /* fin del if */ /*if (fabs(v)<= resol_vel){ zp = 0.0; v = 0.0; }else { */ z = (zold + per_med*(zpold + v - k_obs*e_qold))/(1 + per_med*SGM0*fabs(v)/gv); zp = v - SGM0*fabs(v)*z/gv + k_obs*e_q; /*} fin del if */ f_z = SGM0*z; f_zp = SGM1*zp; f_v = alfp2*v; f_est = f_z + f_zp + f_v; /* estimación de la fricción */ f_est_v = f_est/(kamp*kt); /* señal de control con compensación */ uv = uv + f_est_v; } /* fin del if */ /* límites de salida */ if (uv > 2.0) uv = 2.0; if (uv < -2.0) uv = -2.0; /* salida de la señal de control */ out_DAC0(uv); /* mantener los estados pasados */ x1old = x1k; /* posición angular */
Reconocimiento
155
x2old2 = x2old1; x2old1 = x2k; zold = z; zpold = zp; e_qold = e_q; /* guardar las variables para graficar en Matlab */ savedata(t,uv,f_est_v,x1k,x2k,ref_q,ref_qp,e_q); /* savedata necesita 8 parámetros */ /* incremento de tiempo */ t = t+SAMPLE; UpdateDIO_Output_Pin0(); /* esta función espera que el período de muestreo expire antes de retornar*/ WaitForSample(); } /* Continuar controlando hasta que se presione una tecla */ while (!kbhit()); /* envía cero a la salida del DAC */ zeroDAC(); /* detener el contador */ disarm_counter(); /* escribir los datos guardados al nombre de archivo especificado */ savedatafile(datafile); /* escribe los datos guardados en el archivo "dontremv.m" usado en el */ /* comando de plotting de Matlab */ savedataforMatlabplotting("c:\\matlab\\"); /* libera memoria reservada para guardar datos */ myfree(); } /* fin del programa */ �
Reconocimiento
156
C.8 Programa fuente “Comptop.c” utilizado para realizar
compensación de fricción dinámica del Pendubot.
// comptop.c (compensación arriba) // Este programa efectúa la compensación de fricción en el motor DC del // Pendubot. Se ejecuta primero un control que lleva las articulaciones // de la posición inicial estable de -Pi/2 para la articulación 1 y 0 para // la articulación 2 hasta la posición inestable de Pi/2 de la articulación 1 // y 0 para la 2. La fricción es adicionada como torque actuando sobre el eje // del motor, es decir, sobre la junta 1 (hombro del brazo), lo cual produce un // control mucho más preciso de balanceo en -Pi/2,0. // Uso: // En la línea de comandos introducir: // comptop <nombrearchivo> <Kp> <Kd> // <nombrearchivo> es el nombre de archivo donde guardaremos los datos // <Kp> es el valor de la ganancia Kp para la oscilación inicial que lleva las // articulaciones al punto de equilibrio inestable. // <Kd> valor de la ganancia Kd utilizado en el control de la oscilación inicial // // // ejemplo: comptop misdatos.m 50.0 8.8 /******************************************************************
*/ /* */ /* */ /* PROYECTO DE GRADO PENDUBOT (José Ignacio Hidalgo B.) */ /* */ /* SAMPLE: Período de muestreo deseado: rango 0.001-0.016segundos */ /* */ /* NUM_COMMANDLINE_PAR: This is the number of input parameters. */ /* This relates to the number of parameters you need to type at the command*/ /* line or to specify in your */ /* "ctl" file in the "msdev.exe" executable. If this number of parameters*/ /* is not entered at the command line the input parameters will be ignored*/ /* i.e. with BALMID: 'balmid data.m 150.0 21.0 4.5 1.15' */ /* NUM_COMMANDLINE_PAR is set to 5 */ /* !!! NOTE: The way this source file is written the first command line */ /* Parameter is always the data filename. */ /* */
Reconocimiento
157
/* NUM_POINTS: Number of data points you wish to save to file. Can't be */ /* greater than 8000. I don't suggest using 8000 data points though. */ /* Loading 8000 points into Matlab takes quite a bit of time. */ /* */ /* POINT_INCREMENT: Time interval at which data is collected. */ /* Units: (Sample Periods). In other words, if POINT_INCREMENT = 4 */ /* data will be saved every fourth sample period. */ /* */ /* INIT_RAD_ENC1 & INIT_RAD_ENC2: These are the rest positions of the*/ /* Pendubot's links. These defines are used in the conversion from */ /* encoder counts to radians. They are put here just in case your control*/ /* needs to start at a different position than the hanging rest position. */ /* In most cases these parameters should not be changed and your control */ /* should start at the rest position. Even if you are just balancing */ /* the links, it is better to initialize the encoders in the hanging */ /* position and then tell the user to move the links to the balancing */ /* position and then start the control. */ /* */ /* */ /******************************************************************
/ #define SAMPLE 0.005 /* período de muestreo entre .001 and .016 */
#define NUM_COMMANDLINE_PAR 3 /* número de variables que se desean obtener */
/* en la línea de comandos */ #define NUM_POINTS 1000 /* número de datos que se guardarán en archivo M
*/ /* debe ser menor a 8000 */ #define POINT_INCREMENT 4 /* la función savedata guarda los datos en el
*/ /* archivo M despues del número de muestras definidas */ /* por POINT_INCREMENT. ej. para SAMPLE = .005 y /* POINT_INCREMENT = 3 , los datos se guardan cada .015 segundos */ #define PI 3.1415926 #define HALFPI 1.5707963 #define G 9.804 /* aceleración de la gravedad m/s^2 */
#define INIT_RAD_ENC1 -HALFPI /* condiciones iniciales de posición del Pendubot*/
#define INIT_RAD_ENC2 0.0 #define resol_cod 0.001257 /* resolución de los codificadores */ #define resol_vel (resol_cod/SAMPLE) /* resolución de velocidad */ #define per_med (SAMPLE/2) /* período de muestreo entre 2 */
Reconocimiento
158
#include "devproj.h" extern unsigned _stklen = 32768; /* definición del tamaño del stack para
programa*/ /* inicio del programa principal */ void main(argc, argv) int argc; char *argv[]; { /* definición e inicialización de variables */ float x1k=0.0; float x3k=0.0; float x1old = 0.0; float x3old = 0.0; float x2k=0.0,x4k=0.0; float x2old1,x2old2; float x4old1,x4old2; float u,t; char datafile[13]; int catch; int pd; float K1=-16.4615,K2=-3.1287; /* ganancias para control de balanceo */ float K3=-16.2422,K4=-2.0658; /* u = -Kx. ganancias del 6/6/00 */ float Kp=50; /* ganancias para lazo externo del control de las oscilaciones*/ float Kd=8.7; float P[5] = {0.0308,0.0106,0.0095,0.2087,0.0630}; /* parámetros identificados float d11,d12or21,d22; /* ganancias de la linealización parcial de */ float c11,c12,c21; /* realimentación */ float phi[2]; float v1,dbar,c1bar,c2bar,gbar,h; /* parámetros estáticos identificados para el modelo de fricción */ float alfp0=0.0028; float alfn0=0.0299; float alfp1=0.007; float alfn1=0.0041; float alfp2=0.00012; float alfn2=0.00012; float vop=0.01; float von=-0.04; float z = 0.0; /* estado interno z */ float zold = 0.0; /* derivada del estado interno */ float zpold = 0.0; float zp = 0.0; /* derivada de estado z */
Reconocimiento
159
float gv; /* función de fricción estática */ float f_z = 0.0; float f_zp = 0.0; float f_v = 0.0; /* fricción viscosa */ float f_est = 0.0; /* fricción estimada */ float f_est_v = 0.0; /* fricción estimada en voltios */ float SGM0 = 0.7818; /* valor estimado del parámetro sigma 0 */ float SGM1 = 0.0363; /* SGM1 = 2*sqrt(SGM0*J) - alfp2; */ float k_obs = 0.0001; /* ganancia del observador para el error de posición */ float e_q = 0.0; /* error de posición */ float e_qold = 0.0; float ka = 1.2; /* ganancia del amplificador que maneja al motor DC */ float kt = 0.4006; /* constante de torque */ float J = 0.0004237; /* momento de inercia del motor */ float v = 0.0; /* variable auxiliar de velocidad */ /* colocar salida del DAC en cero inicialmente */ zeroDAC(); /* see devproj.h for a description of pause_message */
pause_message("Asegurese que las articulaciones esten en reposo y presione una tecla\n");
/* see devproj.h for a description of init_boards */ init_boards(); /* determine if the command line input should be processed */ /* and if so assign command line input to the appropriate variables */ /* the first command line input is always the data filename */
if (argc == (NUM_COMMANDLINE_PAR + 1)) { /* Permite al usuario introducir ganancias en la línea de comandos */
if (strlen(argv[1]) > 13) { printf("Nombre de archivo incorrecto...se coloca nombre por
defecto:data.m"); sprintf(datafile,"data.m"); } else { sprintf(datafile,"%s",argv[1]); } /* fin if */ /* asignar entradas a las otras variables */ /* por ejemplo: "Kp = (float) atof(argv[2]);" */ Kp = (float) atof(argv[2]); Kd = (float) atof(argv[3]); } else {
Reconocimiento
160
/* si no hay entrada en la línea de comandos ó no es el número correcto */ /* asignar nombre de archivo por defecto */ sprintf(datafile,"data.m"); } /* fin if */ /* presentar por pantalla los parámetros de entrada para verificar que fueron */ /* recibidos correctamente */ printf("datafile = %s, Kp = %f, Kd = %f,",datafile,Kp,Kd); /* verificar que las ganancias introducidas están dentro de los límites permitidos if ((Kp < 0.0) || (Kp > 500.0)) {
printf("The Value Entered for Kp is outside of its limit (0.0<Kp<500.0) Exiting!\n"); myexit(); } /* fin if */ if ((Kd < 0.0) || (Kd > 100.0)) {
printf("The Value Entered for Kd is outside of its limit (0.0<Kd<100.0) Exiting!\n"); myexit(); } /* fin if */ x1old = -HALFPI; /* inicializar variables de posición y velocidad */ x3old = 0.0; x2old1 = 0.0; x2old2 = 0.0; x4old1 = 0.0; x4old2 = 0.0; t = 0.0; u = 0.0; pd = 1; catch = 0; f_z = 0.0; f_zp = 0.0; f_v = 0.0; f_est = 0.0; f_est_v = 0.0; e_q = 0.0; e_qold = 0.0; pause_message("\nMantenga swicht Pendubot y presione cualquier tecla\n"); printf("Presione Cualquier Tecla Para Detener el Control\n"); /* iniciar conteo de cromómetro */
Reconocimiento
161
/* ver timerdb.h */ arm_counter(); /* iniciar el lazo de control contínuo */ do { /* leer las posiciones de las articulaciones */ /* ver encodrbd.h */ read_encoders(&x1k,&x3k); /* this is a precaution to check if the first link is moving too far */ /* away from its normal operating area. This helps prevent the first */ /* from swinging multiple times out of control. You can change this */ /* to whatever safety limits you feel appropriate. All safty_check does */ /* is see if x1k is within the limits given. If x1k is outside the limits */ /* DAC 0 is set to zero and the program is exited. */ safety_check(x1k,(float) PI,(float) -PI); x2k = (x1k-x1old)/SAMPLE; /* calcular velocidad */ x4k = (x3k-x3old)/SAMPLE; /* filtrar velocidad utilizando un promedio */ x2k = (x2k+x2old1+x2old2)/3.0; /* promedio últimas tres velocidades */ x4k = (x4k+x4old1+x4old2)/3.0; /* para reducir ruido de cuantización */ if (!catch) { /* si las articulaciones no han sido capturadas */ if (fabs(x1k-HALFPI) < .10) { if (fabs(x3k) < .20) { u = -K1*(x1k-HALFPI) - K2*x2k - K3*x3k - K4*x4k; /* calcular control de balanceo*/ if (fabs(u) < 6) { /* si la verificación del control de balanceo no es*/
pd = 0; /* muy grande cambiar de oscilación a balanceo */ catch = 1; } /* fin if */ } /* fin if */ } /* fin if */ } /* fin if */ if (pd) { /* si control de oscilación está todavia en uso */ d11 = P[0] + P[1] + 2*P[2]*cos(x3k); d12or21 = P[1] + P[2]*cos(x3k); d22 = P[1]; /* Calcular dinámica Lagrariana */ h = -P[2]*sin(x3k);
Reconocimiento
162
c11 = h*x4k; c12 = h*x4k + h*x2k; c21 = -h*x2k; phi[0] = P[3]*G*cos(x1k) + P[4]*G*cos(x1k + x3k); phi[1] = P[4]*G*cos(x1k + x3k);
dbar = d11 - (d12or21*d12or21/d22); /* Linealización parcial */ c1bar = c11 - (d12or21*c21/d22); /* de realimentación*/ c2bar = c12; gbar = phi[0] - (d12or21*phi[1]/d22); /* control del lazo externo */ v1 = -Kd*(x2k) + Kp*(HALFPI-x1k); /* control del lazo interno*/ u = dbar*v1 + c1bar*x2k + c2bar*x4k + gbar; } else { /* ejecutar control de balanceo */ u = -K1*(x1k-HALFPI) - K2*x2k - K3*x3k - K4*x4k; e_q = x1k - HALFPI; /* error de posición de articulación 1 */ z = 0.0; zp = 0.0; v = x2k; if (fabs(x1k-HALFPI) < .2) { /* estimación dinámica de fricción */ /* if (v > 0.0) { */ gv = alfp0+alfp1*exp(-((v*v)/(vop*vop))); /* sentido antihorario */ /*}else { gv = alfn0+alfn1*exp(-((v*v)/(von*von))); /* sentido horario */ /*} /*fin if */ /*if (fabs(v) <= resol_vel){ zp = 0.0; v = 0.0; }else { */
z = (zold + per_med*(zpold + v + k_obs*e_qold))/(1 + per_med*SGM0*fabs(v)/gv); zp = v - SGM0*fabs(v)*z/gv - k_obs*e_q; /*} fin if */ f_z = SGM0*z; f_zp = SGM1*zp; f_v = alfp2*v; f_est = f_z + f_zp + f_v; /* estimación de la fricción */ f_est_v = f_est/(ka*kt);
Reconocimiento
163
/* agregar fricción al control de balanceo */ u = u + f_est_v ; } /* fin del if */ if (fabs(u) > 6) {/*si control se torna demasiado grande cambiar de regreso */ /*a control de oscilación which is servoing around */ /* a set point */ u = 0.0; /* colocar control a cero */ pd = 1; } /* fin if */ } /* fin if */ /* limitar salida */ if (u > 6.5) u = 6.5; if (u < -6.5) u = -6.5; /* salida señal de control */ /* ver dacbd.h */ out_DAC0(u); /* mantener estados pasados */ x2old2 = x2old1; /* guardar posiciones y velocidades anteriores */ x2old1 = x2k; x4old2 = x4old1; x4old1 = x4k; x1old = x1k; x3old = x3k; zold = z; zpold = zp; e_qold = e_q; /* guardar hasta 8 variables */ /* para graficar en Matlab */ /* Los primeros 6 estados deben ser t x1k x2k x3k x4k u y en ese orden */ /* las dos últimas variables pueden ser cualquier otra variable que se */ /* desee. */ /* ver devproj.h */
Reconocimiento
164
savedata(t,x1k,x2k,x3k,x4k,u,f_est_v,e_q); /* savedata necesita 8 parámetros /* es la razón de los ceros */ /* incremento del tiempo */ t = t+SAMPLE; /* I use this to watch if a correct sample rate is being produced */ /* This routine toggles DIO output pin 0 each sample period. You */ /* can scope the output pin to monitor the sample rate created */ /* see timerbd.h */ UpdateDIO_Output_Pin0(); /* esta función espera hasta que termine período de muestreo */ /* ver timerbd.h */ WaitForSample(); } /* Continuar controlando hasta que se presione cualquier tecla */ while (!kbhit()); /* enviar cero a la salida del DAC */ /* ver dacbd.h */ zeroDAC(); /* detener contador */ /* ver timerbd.h */ disarm_counter(); /* escribir los datos tomados al archivo especificado en la línea de comandos*/ /* ver devproj.h */ savedatafile(datafile); /*write saved data to the file "dontremv.m" used in the Matlab plotting
command /* ver devproj.h */ savedataforMatlabplotting("c:\\matlab\\"); /* liberar memoria reservada para guardar datos */ /* ver devproj.h */ myfree(); } /* fin del programa */ �
Reconocimiento
165
C.9 Gráficas de respuesta para el control PI de velocidad angular del
servomotor DC.
A continuación se presentan algunas gráficas de velocidad angular para
ciertos valores de referencia, tales como : 2, 1, 0.1, 0.05, -2,-1,-0.1,-0.05.
Reconocimiento