Control de trayectoria de manipulador robótico de 4 GDL

UNIVERSIDAD NACIONAL DE INGENIERÍA – Ingeniería Mecatrónica 1

Abstact— El presente trabajo muestra el diseño y análisis de

controladores para un brazo robótico MITSUBISHI de 4

grados de libertad. Los controladores analizados son de 3

tipos; controladores clásicos tipo PID, controladores por

Torque Computado los cuales son optimizados empleando

Algoritmos Genéticos y finalmente un controlador Neuro

Fuzzy, los controles se comparan entre el modelo ideal

obtenido de la dinámica de LaGrange con los del

SimMechanics del SolidWorks, que nos brinda un modela más

real a comparación del anterior.

Index Terms— Diseño, SimMechanics, Controladores, PID,

Torque computado.

I. INTRODUCCION

N la actualidad el tema de los robots para aplicaciones

industriales ha alcanzado gran impacto en países

tecnológicamente más avanzados como por ejemplo

Alemania, en nuestro país tenemos muy poca aplicación de los

robots industriales. Estos robots instalados en la industria

manufacturera, están hoy en día firmemente asentados, con

tecnologías bien desarrolladas y una amplia gama de

fabricantes. El trabajo que realizan estos robots está destinado

básicamente a manipular piezas o herramientas en un entorno

industrial, por lo que se le denomina robot industrial

manipulador.

Fig. 1 Aplicación como manipulador

II. MODELO DEL ROBOT

Para realizar el control se ha escogido un robot de 4GDL: 4

rotacionales. Para todas las medidas y cálculos se usó el

Sistema Internacional de Unidades.

Fig. 2. Similitud del brazo humano al robótico

Para el diseño del robot se realizó un modelo utilizando CAD1

el cual puede observarse en la figura 2. Todas las piezas

fueron diseñadas en SolidWorks y los planos pueden verse en

el Apéndice A.

El cálculo de la dinámica se realizó usando el método de

Lagrange-Euler, el cual es ampliamente discutido en [2] y [3],

la dinámica del robot que puede ser expresada de la siguiente

forma:

Donde H es la matriz de inercia, C la matriz de Coriolis y

G la matriz de Gravedad. Los parámetros físicos del robot

como distancias centros de masa e inercias fueron escogidos

según los datos obtenidos en el programa SolidWorks y

replanteados para un manejo más sencillo. También es posible

utilizar distintos toolbox de MatLab como el Robotics

Toolbox [6] o métodos iterativos para su cálculo como los

descritos en [5]. Se realizó un modelo en Simulink para la

simulación de la dinámica de nuestro robot.

Fig. 3. Diseño del Robot

CONTROL DE TRAYECTORIA DE MANIPULADOR

ROBÓTICO MITSUBISHI

Camarena Quinto Juan E., Novoa Oliveros Erland F., Vilca Gavidea Celso M.

Estudiantes de Ingeniera Mecatrónica

Universidad Nacional de Ingeniera Lima-Perú

Facultad de Ingeniería Mecánica

E


III. DINAMICA POR LAGRANGE

Datos del robot:

Gráfico:

Fig. 4. Vista frontal del robot para hallar el DH

Dimensiones:

L1=0.307m L2=0.250m

L3=0.383m t=0.02m

Masas:

m1=26.72009 kg

m2=22.44353 kg

m3=11.48022 kg

m4=1.44073 kg

Tabla de Dénabith Hártenberg (DH)

Articulación θ d a α

1 q1 L1 t π/2

2 q2 0 L2 0

3 q3+π/2 0 0 π/2

4 q4 L3 0 0

III.A Hallando el tensor de inercia del

robot H(q)

1er

Paso: Matriz de transformación

2do

Paso: Posición de los centros de masa de cada eslabón

respecto al referencial XkYkZk

3er

Paso: Posición de los centros de masa de cada eslabón

respecto al referencial X0Y0Z0

4to

Paso: Matrices de Inercia de los centros de masas de cada

eslabón con respecto al referencial XkYkZk

Tabla DH

0T1=DH(θ1,d1,a1,α1)

1T2=DH(θ2,d2,a2,α2)

2T3=DH(θ3,d3,a3,α3)

3T4=DH(θ4,d4,a4,α4)

0T1=0T1

0T2=0T1*1T2

0T3=0T1*1T2*2T3

0T4=0T1*1T2*2T3*3T4

0R1=0T1(1:3,1:3)

0R2=0T2(1:3,1:3)

0R3=0T3(1:3,1:3)

0R4=0T4(1:3,1:3)

0P1=0T1(1:3,4)

0P2=0T2(1:3,4)

0P3=0T3(1:3,4)

0P4=0T4(1:3,4)

ΔCk 0Rk 0Pk

C1=0P1+0R1*ΔC1

C2=0P2+0R2*ΔC2

C3=0P3+0R3*ΔC3

C4=0P4+0R4*ΔC4


5to

Paso: Tensor Inercial Rotacional de cada eslabón.

6to

Paso: Jacobiano de velocidades lineales del centro de masa

de cada eslabón

7mo

Paso: Jacobiano de velocidades angulares de cada eslabón

Donde:

Observación: Jacobiano total de cada eslabón:

8vo

Paso: Tensor Inercial Total de cada eslabón

9no

Paso: Tensor Inercial Total del robot

III.B Algoritmo para Hallar

1

er paso: Derivada de Matriz

2do

paso: Vector de velocidad articular

0Rk Ik

Hrot1=(0R1)xI1x(0R1)t




Ck qk

Jv1=[δC1/δq1 0 0 0 ]t

Jv2=[δC2/δq1 δC2/δq2 0 0 ]t

Jv3=[δC3/δq1 δC3/δq2 δC3/δq3 0 ]t

Jv4=[δC4/δq1 δC4/δq2 δC4/δq3 δC4/δq4]t

εk uzk-1

Jw1=[ε1*uz

0 0 0 0 ]t

Jw2=[ε1*uz

0 ε2*uz1 0 0 ]t

Jw3=[ε1*uz

0 ε2*uz1 ε3*uz

2 0 ]t

Jw4=[ε1*uz

0 ε2*uz1 ε3*uz

2 ε4*uz3 ]t

mk Jvk Hrotk Jw

k

H1=(Jv1)txm1x(Jv

1)+ (Jw1)txHrot1x(Jw

1)H2=(Jv

2)txm2x(Jv2)+ (Jw

2)txHrot2x(Jw2)

H3=(Jv3)txm3x(Jv

3)+ (Jw3)txHrot3x(Jw

3)H4=(Jv

4)txm4x(Jv4)+ (Jw

4)txHrot4x(Jw4)

Hk

H=H1+H2+H3+H4


0 2 4 6-1

0

1

2

3

t

q1

0 2 4 6-1

-0.5

0

0.5

t

q2

0 2 4 6-2

-1.5

-1

-0.5

0

0.5

t

q2

0 2 4 6-0.1

-0.05

0

0.05

0.1

t

q4

3er

paso: Vector de Coriolis

III.C Algoritmo para hallar

1er

paso: Energía Potencial

Donde:

2do

paso: Derivada de energía potencial

Es un vector de tamaño

IV. MODELADO DE LA

DINAMICA EN SIMULINK

Ahora conociendo el algoritmo de las matrices H, C, h se

prosigue a implementar esto en Simulink.

Sabemos que:

Donde:

Entonces para implementar usar:

Figura5: Planta de Dinámica Lagrangiana (ideal).

V. ANALISIS DE

CONTROLADOR PID

Para valores de las ganancias:

Kp=diag([100,100,60,6])

Ki= diag([30,50,90,3])

Kd=diag([56,64,24,.8])

Para la referencia: q= [1 0 -.8 0]

V.A Control Proporcional (P)

Fig. 6 Esquema de controlador Proporcional

Fig. 7 Salida de las articulaciones P


0 2 4 6-50

0

50

100

t

T1

0 2 4 6-50

0

50

100

t

T2

0 2 4 6-60

-40

-20

0

20

t

T3

0 2 4 6-10

-5

0

5x 10

-3

t

T4

PD

PD+G

0 2 4 60

0.2

0.4

0.6

0.8

1

t

q1

0 2 4 6-0.6

-0.4

-0.2

0

0.2

t

q2

0 2 4 6-1

-0.8

-0.6

-0.4

-0.2

0

t

q3

0 2 4 60

0.2

0.4

0.6

0.8

1

t

q4

PD

PD+G

0 2 4 60

0.5

1

1.5

t

q1

0 2 4 6-0.4

-0.2

0

0.2

0.4

t

q2

0 2 4 6-1

-0.5

0

t

q3

0 2 4 6-5

0

5

10x 10

-4

t

q4

V.B Control PD

Fig. 8 Esquema de controlador P-D

V.C Control PD compensación de G

Fig. 9 Esquema de controlador PD compensando G

Fig. 10 Salida de las articulaciones (rad)

Fig. 11 Torques necesarias (N.m)

Haciendo una comparación entre los diferentes controladores

anteriores se concluye el controlador V.A es insuficiente ya

que necesita amortiguar las oscilaciones lo que se consigue

con los controladores V.B y V.C, nótese que una articulación

complicada es q2 ya que en el giro coordinado que se está

realizando la tercera articulación está afectada en gran medida

por la gravedad y esto se corrige al compensar G como se

observa en la Fig. 10 comparando los controladores V.B y

V.C. También se debe considerar el hecho que el robot es del

SimMechanics por ende considera varias no linealidades como

fricción entre las articulaciones.

Hasta el momento se ha conseguido buen desempeño con el

controlador V.C, El controlador PID lo usamos para variar

algunos parámetros dependiendo de nuestra aplicación, por

ejemplo el tiempo de establecimiento (ts) tomando un criterio

de 4% se puede notar que es menor.

V.D Control PID

Fig. 12 Esquema de controlador PID

Fig. 13 Salida de las articulaciones PID (rad)

El integrador sirve para disminuir (tender a cero) los errores

en estado estacionario por lo que q2 tiende a ser cero sin

necesidad de compensar la gravedad como se observa en la

Fig.13, y los Torques necesarios para este controlador PID son

similares a los de la Fig. 11.


0 1 2 3 4 5-0.2

00.2

error x

t

ex

0 1 2 3 4 5-0.1

00.1

error y

t

ey

0 1 2 3 4 5-0.5

00.5

error z

t

ez

0 1 2 3 4 5-202

error q4 (rad)

t

eq4

LaGrange

Simechanics

LaGrange

Simechanics

LaGrange

Simechanics

LaGrange

Simechanics

0 1 2 3 4 5-20

020

Torque 1

t

T1

0 1 2 3 4 50

50100

Torque 2

t

T2

0 1 2 3 4 5-100

0100

Torque 3

t

T3

0 1 2 3 4 5-10

010

Torque 4

t

T4

LaGrange

Simechanics

LaGrange

Simechanics

LaGrange

Simechanics

LaGrange

Simechanics

0 1 2 3 4 50

0.51

Salida x

t

x

0 1 2 3 4 50

0.10.2

Salida y

t

y

0 1 2 3 4 50

0.20.4

Salida z

t

z

0 1 2 3 4 5012

ANGULO q4 (rad)

t

q4

LaGrange

Simechanics

LaGrange

Simechanics

LaGrange

Simechanics

LaGrange

Simechanics

VI. CONTROL PID DE POSICION


Kp=diag([100,100,60,6])

Ki= diag([30,50,90,3])

Kd=diag([56,64,24,.8])

VI.A Planta LaGrange

Fig. 14 Esquema de control PID ideal de posición

VI.A Planta SimMechanics

Fig. 15 Esquema de control PID real de posición

Para la referencia [.5 .1 .3 pi/2]' obtenemos:

Fig. 16 Salida real del XYZ

Fig. 17 Torque para llegar a la posición de referencia.

Fig. 18 Torque para llegar a la posición de referencia.

Fig. 19 Movimiento del Robot SimMechanics

Con los valores de las ganancias elegidas la planta se controla

adecuadamente y el tiempo de establecimiento aumenta esto

es prudente para disminuir los torques de actuación, los cuales

según la figs. 17 son siempre menores a 100N.m. Los

resultados obtenidos en ambos casos sin compensación de

gravedad son parecidos como era de esperarse.


-1

-0.5

0

0.5

1

-1

-0.5

0

0.5

10.302

0.304

0.306

0.308

0.31

z

TRAYECTORIA

xy

-1

-0.5

0

0.5

1

-1

-0.5

0

0.5

10.1

0.2

0.3

0.4

0.5

z

TRAYECTORIA REAL

xy

0 10 20-5

0

5

10

t

T1

0 10 200

200

400

600

t

T2

0 10 200

50

100

150

t

T3

0 10 20-4

-2

0

2x 10

-3

t

T4

VII. CONTROL DE TRAYECTORIA


Kp=diag([100,100,60,6])

Ki= diag([30,50,90,3])

Kd=diag([56,64,24,.8])

VII.A Torque computado

Planta controlada MITSUBISHI

Fig. 20 Esquema del controlador por Torque Computado

Fig. 21 Esquema del control de trayectoria

Para la trayectoria dada: function y = fcn(u)

y=zeros(4,1);

y(1)=0.63*cos(u);%0.653

y(2)=0.63*sin(u);

y(3)=0.002*cos(5*u)+0.305;%0.305

y(4)=0.1*u;

Fig. 22 Trayectoria realizada por el efector final

Fig. 20 Torque necesario para seguir la trayectoria deseada

VII.B Luh Walker


Para la trayectoria dada: function r=Trayectoria(t)

r=zeros(3,3);

R=0.62;%0.653

x=R*cos(t);

y=R*sin(t);

z=-0.01*t+0.305;%0.305

Y el controlador PD:

u=diag([1000,990,800,800])*(eq)+diag([30,30,40,30])*(eqp)



-1

-0.5

0

0.5

1

-1

-0.5

0

0.5

10.1

0.2

0.3

0.4

0.5

z

COMPARACION DE TRAYECTORIAS

xy

0 5 10 15-20

0

20

40

60

t

T1

0 5 10 15-200

0

200

400

600

t

T2

0 5 10 15-50

0

50

100

150

t

T3

0 5 10 15-10

-5

0

5x 10

-3

t

T4

Fig. 25 Torque empleado en el Luh-Walker

Fig. 26 Error en la trayectoria real

REFERENCES

[1] John H. Holland, “Adaptation in Natural and Artificial Systems”, 1975

[2] Barrientos, Peñin, Balaguer, Aracil. “Fundamentos de Robótica”, McGraw-Hill, 1977

[3] Craig, John J, “Robótica”, Ed. Pearson Education, 2006 [4] Katsuhiko Ogata, “Ingeniera de Control Moderna”, Pearson, 2003, 4ta

edicion

[5] Ahmed A. Shabana, “Computational Dynamics”, Editorial John Wiley & Sons.

[6] Mathworks, “Robotics Toolbox de Matlab”

[7] Mathworks, “Fuzzy Logic Toolbox de Matlab”

APPENDIX A

PLANOS

Fig. 27. Planos del eslabón con junta rotacional. Eslabón 0






ALCANCES DEL MANIPULADOR ROBÓTICO

Fig. 32. Alcances del manipulador

ALGORITMO LUH WALKER


Los controladores PID (Proporcional Integral Derivativo) son

controladores que tienen una alta performance en sistemas

lineales o en sistemas no lineales en un determinado punto de

operación, inicialmente se planteó el uso de controladores de

este tipo dada su simplicidad.

Un controlador PID es un mecanismo de control por

realimentación que corrige el error entre la salida deseada y la

obtenida por la planta, su ley de control es la siguiente:

El controlador por torque computado con linealización utiliza

un controlador PID o PD el cual es multiplicado por la matriz

de Inercia adicionalmente se estiman los torques generados

por la Gravedad y Coriolis los cuales son agregados, esto

alivia bastante al controlador PID, la ley de control sería la

siguiente:


CONTROLADOR RNA

Tenemos:

Considerando los términos de fricción F(q&) , y los torques

perturbadores , podemos completar la ecuación (8) de la

siguiente manera

Simplificando la notación, haciendo:

Queda:

Que es la ecuación dinámica directa del manipulador, que da

los torques aplicados en función de las posiciones, velocidades

y aceleraciones articulares.

Estructura del Controlador

En la Figura 34 se aprecia la estructura del controlador

propuesto. La señal de control , está dada por:

La acción del controlador PD, está dada por

Siendo e el vector error de posición, dado por la diferencia

entre la posición deseada qd, y la real q. O sea – ,

e es el vector error de velocidad. Kp es la matriz diagonal con

las ganancias proporcionales de cada articulación y Kv es la

matriz diagonal con las ganancias derivativas.


Donde será la función dinámica inversa aprendida por la

red neuronal. Evidentemente, en el caso ideal, si la

aproximación de la RN a la dinámica inversa del robot fuera

sin errores, la salida del controlador sería cero ya que para una

trayectoria qd la RN generaría los torques ideales para que la

trayectoria real q siga a la deseada.

En el caso no ideal, el controlador PD compensará los errores

producidos en el modelaje de la RN. Siguiendo a [Velthuis et

al. 1996], se puede interpretar a la señal de salida del

controlador PD como una medida del error en el modelo

obtenido por la red neuronal.

Estructuras de aprendizaje de la dinámica

inversa

Siguiendo a [Patiño 1995] se entrena a la red neuronal en dos

etapas. En la primera se logra un ajuste grueso de los

parámetros de la RN utilizando la estructura de aprendizaje

generalizado [Psaltis et al 1988] representada en la Figura 3.

El aprendizaje es „offline‟ y realizado a partir de patrones de

entrenamiento obtenidos de las características de lazo cerrado

de la planta [Omatu et al. 1996]. Esto es, se hace funcional

robot con el control PD extrayéndose el conjunto de datos de

entrenamiento de la red a partir de la observación de su

comportamiento. Esta forma de aprendizaje es similar al usado

en reconocimiento de patrones.


Figura 35. Estructura de Aprendizaje Generalizado

Figura 36. Estructura de Aprendizaje por Error de Realimentación

Control de trayectoria de manipulador robótico de 4 GDL

Documents

Transcript of Control de trayectoria de manipulador robótico de 4 GDL