You are on page 1of 9

UNIVERSIDAD NACIONAL DE INGENIERIA

FACULTAD DE INGENIERIA MECANICA


ESCUELA PROFESIONAL DE INGENIERÍA MECATRÓNICA

DISEÑO E IMPLEMENTACIÓN DE UN CONTROLADOR


DIFUSO PD+I PARA EL CONTROL DE UN
MANIPULADOR ROBÓTICO 4GDL

IMPLEMENTATION AND DESIGN OF FUZZY CONTROLLER FPD+I FOR A


ROBOTIC MIPULATOR 4DOF

Autores:
Cercado Cuya Julio
Céspedes Tapia Max
Vargas Carbajal Maxwell
Surichaqui Reyes Ángel

Estudiantes de la Escuela Profesional de Ingeniería Mecatrónica de la


Universidad Nacional de Ingeniería

RESUMEN

En el presente trabajo se presenta el diseño e implementación de un controlador


para un brazo robótico de 4 grados de libertad con tres articulaciones
rotacionales y una prismática, la estrategia de control a utilizar se basa en
lógica difusa, implementando un controlador PD+I difuso el cual también
trabajará en conjunto con un controlador de torque computado para asegurar un
mejor seguimiento de las trayectorias.

OBJETIVO

• Probar la eficiencia del controlador difuso aplicado al control de robots

• Comparar el desempeño del controlador difuso frente a otras estrategias


de control para manipuladores robóticos
UNIVERSIDAD NACIONAL DE INGENIERIA

FACULTAD DE INGENIERIA MECANICA


ESCUELA PROFESIONAL DE INGENIERÍA MECATRÓNICA

Parámetros del Robot


Manipulador de 4 grados de libertad, Cálculo de la dinámica y
posee 3 juntas rotativas y una cinemática del manipulador
prismática. robot.

El cálculo de la cinemática se obtiene


directamente de los parámetros de
Denavit-Hartemberg, usando la
siguiente matriz de transformación.

Tii+1=Cθi-CαiSθiSαiSθiaiCθiSθiCαiCθi-
SαiCθiaiSθi0SαiCαidi0001

Se usa es estándar Denavit-Hartemberg El cálculo de la dinámica se puede


para la ubicación espacial de las juntas. realizar usando alguno de los 2
siguientes métodos: el método de
Lagrange-Euler y el método de Newton-
Euler.

Tanto para el cálculo de la cinemática y


dinámica se uso el toolbox ‘hemero’ de
Matlab el cual utiliza el método
mencionado de cinemática y Lagrange-
Euler para la dinámica.

A continuación se mostrara el código


fuente para el cálculo de la dinámica
con la librería hemero:

Se inicializa con el total de variables a


usar en modo simbólico.
Fig 1: Asignación de sistemas de
coordenadas

De acuerdo a la figura se obtienen los


parámetros Denavit-Hartemberg

Eslab
α a θ d
ón
q
1 π2 0 L1 Se define la estructura L con los
1 parámetros del Denavit- Hartemberg.
L q
2 0 0
2 2
q
3 -π2 0 0
3
4 0 0 0 q4
UNIVERSIDAD NACIONAL DE INGENIERIA

FACULTAD DE INGENIERIA MECANICA


ESCUELA PROFESIONAL DE INGENIERÍA MECATRÓNICA

estados completa tal como se muestra


en la figura:

Se ingresan los valores a la estructura


L de momento de inercia ‘I’ masa ‘m’
y posición del centro de masa ‘C’ para Fig 2: Control del Motor DC
la obtención de la dinámica.
Los parámetros del motor usados son:
Definición de las variables de las
juntas, definimos a L como el objeto Ra=2Ω
robot y calculamos la cinemática. Con La=1.8mH
el comando fkine. J=8×10-3kgm2
B=4.94×10-3Nm/(rad/s)
Kv=0.3334V /(rad/s)
Kt=0.3334Nm/A

Cálculo de las matrices de la dinámica


del robot con el objeto robot y la
variable Q4 (juntas). CONTROLADOR FPD+I

Estructura del controlador difuso

El controlador difuso no es un
controlador en base a cálculos
matemáticos sino en base a las reglas e
Modelamiento de los Actuadores inferencias que gobiernan este
controlador sin embargo existe el
Las ecuaciones diferenciales que cálculo pero en el procesamiento de
gobiernan la dinámica y parte eléctrica estas reglas definido en 3 etapas
del motor se muestran a continuación
en forma de espacio de estados: • Bloque pre-procesamiento.
• Controlador difuso.
x=-Ra/La-K/LaK/J-b/Jx+1/La0u • Bloque post-procesamiento.
y=K0x+0u
En la siguiente figura se muestra la
Donde: estructura de el controlador difuso con
sus 3 etapas mencionadas
x=x1x2 anteriormente.
x1:corriente
x2:voltaje
y:Torque

En el motor se realizó un control de


torque mediante realimentación de
UNIVERSIDAD NACIONAL DE INGENIERIA

FACULTAD DE INGENIERIA MECANICA


ESCUELA PROFESIONAL DE INGENIERÍA MECATRÓNICA

Fig3. Estructura del controlador difuso

A continuación procederemos a explicar


brevemente cada uno de los bloques.
 Maquina de inferencia difusa:
Bloque pre-procesamiento: es una se encarga de juntar las pertenencias ya
entrada tipo crisp para acomodar la evaluadas según las reglas propuestas
entrada a este bloque anteriormente en el controlador según operador and u
pasa por un procesador que entregue or.
las señales correctas para el ingreso al
controlador fuzzy como por ejemplo un de(t
escalamiento del error. )
N N ZE PS P
Bloque controlador difuso: este B S B
bloque realiza el procesamiento N N N NS NS Z
completo de la señal de entrada tipo
B B B E
crisp y la lleva a una señal de salida
también tipo crisp dentro de este bloque N N N NS ZE P
se encuentran los siguientes sub- S B S S
bloques. e(t) Z N N ZE PS P
 Bloque Fuzzificacion: en este E S S S
bloque se realiza el cruce con el P N Z PS PS P
conjunto difuso y el grado de S S E B
pertenencia que este tenga. Mejor dicho P Z P PS PB P
evalúa la entrada según la partición B E S B
difusa de las funciones de membrecía de
entrada.
 Bloque base de reglas: está  Bloque defuzzificador: ya
conformada por sentencias las cuales teniendo juntas las pertenecías una
definen el tipo de operación que se sobrepuesta ala otra se tiene que llevar
realiza con la entrada crisp y el grado de el conjunto difuso resultan de a un
pertenencia anteriormente evaluados. numero crisp según los métodos ya
La forma básica de estas sentencias es estudiados como el centro de gravedad.
de la forma
Bloque post-procesamiento: el
IF x es A and y es B THEN z es C.
numero crisp ya obtenido para nuestro
caso será llevado a una suma con la
componente de salida integradora y
ambas ya sumadas serán amplificadas
parea utilizarlas de señal de control para
nuestra planta en este caso nuestro
motor.
UNIVERSIDAD NACIONAL DE INGENIERIA

FACULTAD DE INGENIERIA MECANICA


ESCUELA PROFESIONAL DE INGENIERÍA MECATRÓNICA

Fig4: Entrada y señal de control en un lazo


cerrado para nuestra planta.

Diseño del controlador FPD + I Fig 5: Estructura del controlador PID

Nos basaremos en el diseño de un


control PID lineal ya sintonizado por Transfiriendo ganancias PID a
método de tanteo FPD+I
ϖn=10 Es importante obtener las ganancias de
ζ=1 PID antes de diseñar el controlador fuzzy
ki=1 para tener una salida del estado que
nos interesa y poder formar nuestras
El controlador tendrá una función de reglas según esta.
transferencia de la forma:
U(s)E(s)=ϖn2+kis+2ζϖns Las entradas al controlador fuzzy FPD
son el error y la derivada del error. Junto
El cual tiene como equivalente: con la variable integral del error el cual
se dejara libre y no se diseñara dentro
U(s)E(s)=Kp+Kis+KD*s del controlador por lo cual no tendrá una
base de reglas, tan solo se sumara ala
salida de la señal de control del FPD y
Ganancias del controlador PID: ambos de amplificaran con una
ganancia de salida. Para la mejor visión
Kp=100 de lo que se trata de decir en lo anterior
Ki=1 se ilustra el modelo el controlador que
KD=16 se implementara para la planta.

Fig 6. Estructura del controlador difuso


FPD+I
UNIVERSIDAD NACIONAL DE INGENIERIA

FACULTAD DE INGENIERIA MECANICA


ESCUELA PROFESIONAL DE INGENIERÍA MECATRÓNICA

+ I (GE, GCE, GIE, GU) a continuación se


muestra las salida con el PID de nuestra
Entonces visualizando la imagen experiencia con sus actuadores:
anterior queda mas claro el tipo de
controlador que se quiere diseñar Como se muestra en la figura anterior se
entonces las reglas se hacen en base a presenta un sobre impulso al inicio en
el error y la derivada del error según la base a esto se diseñara las reglas y con
salida de el estado que nos interese en estas ganancias del PID se obtendrá las
nuestro caso la velocidad del motor ganancias del FPD + I.

Basados en Jantzen y según la obtención


de este controlador FPD + I se relaciona
la ganancia de el controlador PID antes
obtenido y las ganancias de la entrada
del error y derivada del error y la salida
de control estas ganancias se relacionan
de la siguiente manera.

Sea:

U(s)E(s)=Kp(1+Kis*Kp+KDKp*s)

Entonces: Fig 7: Estructura del controlador FPD + I

U(s)E(s)=Kp(1+1s*Ti+TD*s)

Teniendo lo anterior se define como


ganancia de entrada y salida lo
siguiente.

GE*GU=Kp
Fig 8: Controlador FPD + I para cada motor
GCEGE=TD

GIEGE=Ti

El valor inicial de GE se dará por la


siguiente ecuación:
GE=1max(yd-y) Resultados de la Simulación

Se probó el seguimiento de diferentes


PID prototipo para el control tipos de trayectorias que se muestran a
FPD+I continuación.

Para el diseño del controlador FPD + I se Trayectoria 1


tiene que tener un modelo prototipo PID Cuerva Sobre Superficie Cónica
para poder observar la salida de esta y
tener una cierta guía para el ajuste de
las reglas y para el diseño de las
ganancia de entrada y de salida del FPD
UNIVERSIDAD NACIONAL DE INGENIERIA

FACULTAD DE INGENIERIA MECANICA


ESCUELA PROFESIONAL DE INGENIERÍA MECATRÓNICA
Control FPD+I - Trayectoria Control FPD+I - Torques de Control
20
Referencia Torque 1
Trayectoria Real Torque 2
15 Torque 3
0.8
Torque 4

0.6 10

Torque
0.4 5
z

0.2
0

0
0 -5

0.2
1.4
0.4 -10
1.2 0 1 2 3 4 5 6 7 8 9 10
0.6
1 t
0.8 y
x
0.1
Control FPD+I - Error en el espacio "q" La integral del módulo del vector error
Error 1 es:
0.05 Error 2 0.2119
Error 3
Error

0 Error 4

-0.05 Trayectoria 2
-0.1
0 1 2 3 5 4 6 7 8 9 10 Circunferencia en el espacio
t
Control FPD+I - Módulo del Vector Error en el Espacio de Trabajo Control FPD+I - Trayectoria
0.04
Referencia
0.03 Trayectoria Real
Error

0.02 0.8

0.01
0.6
0
0 1 2 3 4 5 6 7 8 9 10
t 0.4
z

0.2

Control FPD+I - Torques de referencia 0


60 0
Torque 1 0.2
50 Torque 2
0.4 1.1
Torque 3
40 Torque 4 0.6 1
0.8 0.9 y
x
30

20
Torque

10

-10

-20

-30
0 1 2 3 4 5 6 7 8 9 10
t
UNIVERSIDAD NACIONAL DE INGENIERIA

FACULTAD DE INGENIERIA MECANICA


ESCUELA PROFESIONAL DE INGENIERÍA MECATRÓNICA
Control FPD+I - Error en el espacio "q"
0.1
Trayectoria 3
Error 1
0.05 Error 2 Linea Recta en el Espacio
Error 3
Error

0 Control FPD+I - Trayectoria


Error 4
Referencia
-0.05
Trayectoria Real

-0.1
0 1 2 3 45 6 7 8 9 10
t 0.8
Control FPD+I - Módulo del Vector Error en el Espacio de Trabajo
0.08
0.6

0.06

z
0.4
Error

0.04
0.2
0.02

0 0
0 1 2 3 4 5 6 7 8 9 10 0
t
Control FPD+I - Torques de referencia 1.2
80 0.5
Torque 1 1
Torque 2 1 0.8
x y
60 Torque 3 Control FPD+I - Error en el espacio "q"
Torque 4 0.02
Error 1
40 0.01 Error 2
Error 3
Error

0 Error 4
Torque

20
-0.01

-0.02
0 0 1 2 3 54 6 7 8 9 10
t
Control FPD+I - Módulo del Vector Error en el Espacio de Trabajo
-20 0.02

0.015
-40
Error

0 1 2 3 4 5 6 7 8 9 10
Control FPD+I - Torques
t de Control
30 0.01
Torque 1
25 Torque 2 0.005
Torque 3 0 1 2 3 4 5 6 7 8 9 10
20 Torque 4 t
Control FPD+I - Torques de referencia
60
15
Torque 1
50 Torque 2
10
Torque

Torque 3
40 Torque 4
5

0 30

-5 20
Torque

-10 10

-15 0
0 1 2 3 4 5 6 7 8 9 10
t
-10

La integral del módulo del vector error -20


es:
0.4774 -30
0 1 2 3 4 5 6 7 8 9 10
t
UNIVERSIDAD NACIONAL DE INGENIERIA

FACULTAD DE INGENIERIA MECANICA


ESCUELA PROFESIONAL DE INGENIERÍA MECATRÓNICA
Control FPD+I - Torques de Control
20
1. El torque de control es menor que el
Torque 1
Torque 2 torque de referencia debido a la
ganancia del sistema de motores.
15 Torque 3
Torque 4

10
2. La sintonía de las ganancias GU a la
salida de los controladores fuzzy
Torque

5
debe ser propia y distinta para cada
junta, además esta sintonía es
0
necesaria para obtener mejor
-5
performance en el error.

-10
0 1 2 3 4 5 6 7 8 9 10
t
La integral del módulo del vector error
es:
0.1236 REFERENCIAS

[1 Shabana A. “Computational
Conclusiones ] Dynamics” Editorial: JOHN WILEY
& SONS, LTD. (2ª Edition)
1. El desempeño del control Fuzzy PD [2 M. Sc. Ricardo Rodríguez
mas integrador depende de las ] Bustinza. Publicación Titulada:
trayectorias a seguir: “Diseño e Implementación de un
• Trayectorias cíclicas el control difuso PID basado en la
comportamiento del error es el tarjeta de Adquisición de Datos
mismo [3 Jan Jantzen. “Foundations of Fuzzy
• Trayectorias cíclicas y crecientes, el ] Control”. Wiley, 2007.
error tiende a crecer suavemente. [4 Ollero Baturone “Robótica;
• Trayectorias lineales el error tiende ] manipuladores y robots móviles”
Marcombo, S.A.2001
a decrecer suavemente.