UNIVERSIDAD NACIONAL DE INGENIERA Ingeniera Mecatrnica

CONTROL DE TRAYECTORIA DE MANIPULADOR ROBTICO MITSUBISHI

Camarena Quinto Juan E., Novoa Oliveros Erland F., Vilca Gavidea Celso M. Estudiantes de Ingeniera Mecatrnica Universidad Nacional de Ingeniera Lima-Per Facultad de Ingeniera Mecnica

Abstact El presente trabajo muestra el diseo y anlisis de

II.

MODELO DEL ROBOT

controladores para un brazo robtico MITSUBISHI de 4 grados de libertad. Los controladores analizados son de 3 tipos; controladores clsicos tipo PID, controladores por Torque Computado los cuales son optimizados empleando Algoritmos Genticos y finalmente un controlador Neuro Fuzzy, los controles se comparan entre ek modelo ideal obtenido de la dinmica de LaGrange con los del SimMechanics del SolidWorks, que nos brinda un modela ms real a comparacin del anterior.
Index Terms Diseo, SimMechanics, Controladores, PID, Torque computado.

Para realizar el control se ha escogido un robot de 4GDL: 4 rotacionales. Para todas las medidas y clculos se us el Sistema Internacional de Unidades.

Fig. 2. Similitud del brazo humano al robtico Para el diseo del robot se realiz un modelo utilizando CAD1 el cual puede observarse en la figura 2. Todas las piezas fueron diseadas en SolidWorks y los planos pueden verse en el Apndice A. El clculo de la dinmica se realiz usando el mtodo de Lagrange-Euler, el cual es ampliamente discutido en [2] y [3], la dinmica del robot que puede ser expresada de la siguiente forma:

I.

INTRODUCCION

la actualidad el tema de los robots para aplicaciones industriales ha alcanzado gran impacto en pases tecnolgicamente ms avanzados como por ejemplo Alemania, en nuestro pas tenemos muy poca aplicacin de los robots industriales. Estos robots instalados en la industria manufacturera, estn hoy en da firmemente asentados, con tecnologas bien desarrolladas y una amplia gama de fabricantes. El trabajo que realizan estos robots est destinado bsicamente a manipular piezas o herramientas en un entorno industrial, por lo que se le denomina robot industrial manipulador.
N

Donde H es la matriz de inercia, C la matriz de Coriolis y G la matriz de Gravedad. Los parmetros fsicos del robot como distancias centros de masa e inercias fueron escogidos segn los datos obtenidos en el programa SolidWorks y replanteados para un manejo ms sencillo. Tambin es posible utilizar distintos toolbox de MatLab como el Robotics Toolbox [6] o mtodos iterativos para su clculo como los descritos en [5]. Se realiz un modelo en Simulink para la simulacin de la dinmica de nuestro robot.

Fig. 3. Diseo del Robot Fig. 1 Aplicacin como manipulador

UNIVERSIDAD NACIONAL DE INGENIERA Ingeniera Mecatrnica

III. LAGRANGE-EULER
Datos del robot: Grfico:
0T 1T 2T 3T 1=DH(1,d1,a1,1) 2=DH(2,d2,a2,2) 3=DH(3,d3,a3,3) 4=DH(4,d4,a4,4)

Tabla DH

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

2 1T 2* T 3 0 0 1T 2 T 4= 0T 1* 1T 2* 2T 3* 3T 4 0 0 T 3= 0 T 1*

0T T 2=

1=

0T T 1*

0R

1=

2 2 0R =0T (1:3,1:3) R3=0T3(1:3,1:3) 0 4 4 0R =0T (1:3,1:3)

0T

1(1:3,1:3)

0P 0 0

1=

P 2= P 3=

0T 0T

1(1:3,4) 2(1:3,4)

3 0T (1:3,4) 4= T4(1:3,4) 0P 0

Tabla de Dnabith Hrtenberg (DH)

3er Paso: Posicin de los centros de masa de cada eslabn respecto al referencial X0Y0Z0

Articulacin
1 2 3 4

q1 q2 q3+/2 q4

d
L1 0 0 L3

a
t L2 0 0

/2 0 /2 0

Ck 0Rk

0P

III.A Hallando el tensor de inercia del robot h(q) 1er Paso: Matriz de transformacin

C1=0P1+0R1*C1 C2=0P2+0R2*C2 C3=0P3+0R3*C3 C4=0P4+0R4*C4

4to Paso: Matrices de Inercia de los centros de masas de cada eslabn con respecto al referencial XkYkZk

2do Paso: Posicin de los centros de masa de cada eslabn respecto al referencial XkYkZk

UNIVERSIDAD NACIONAL DE INGENIERA Ingeniera Mecatrnica

5to Paso: Tensor Inercial Rotacional de cada eslabn.

Observacin: Jacobiano total de cada eslabn:

0R

Ik

8vo Paso: Tensor Inercial Total de cada eslabn

Hrot1=(0R1)xI1x(0R1)t Hrot2=(0R2)xI2x(0R2)t Hrot3=(0R3)xI3x(0R3)t Hrot4=(0R4)xI4x(0R4)t

mk

Jvk

Hrotk

Jwk

6to Paso: Jacobiano de velocidades lineales del centro de masa de cada eslabn

H1=(Jv1)txm1x(Jv1)+ (Jw1)txHrot1x(Jw1) H2=(Jv2)txm2x(Jv2)+ (Jw2)txHrot2x(Jw2) H3=(Jv3)txm3x(Jv3)+ (Jw3)txHrot3x(Jw3) H4=(Jv4)txm4x(Jv4)+ (Jw4)txHrot4x(Jw4)

9no Paso: Tensor Inercial Total del robot

Ck

qk

Jv1=[C1/q1 0 0 0 ]t 2=[C /q Jv 0 0 ]t 2 1 C2/q2 3=[C /q Jv 0 ]t 3 1 C3/q2 C3/q3 4=[C /q t Jv 4 1 C4/q2 C4/q3 C4/q4]

Hk

H=H1+H2+H3+H4
7mo Paso: Jacobiano de velocidades angulares de cada eslabn III.B Algoritmo para Hallar 1er paso: Derivada de Matriz

Donde:

uzk-1
2do paso: Vector de velocidad articular

Jw1=[1*uz0 Jw2=[1*uz0 Jw3=[1*uz0 Jw4=[1*uz0

0 2*uz1 2*uz1 2*uz1

0 0 3*uz2 3*uz2

0 ]t 0 ]t 0 ]t 4*uz3 ]t

UNIVERSIDAD NACIONAL DE INGENIERA Ingeniera Mecatrnica

3er paso: Vector de Coriolis

III.C Algoritmo para hallar 1er paso: Energa Potencial Figura5: Planta de Dinmica Lagrangiana (ideal).

Donde:

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] 2do paso: Derivada de energa potencial V.A Control Proporcional (P)

Es un vector de tamao

Fig. 6 Esquema de controlador Proporcional

IV.

MODELADO DE LA DINAMICA EN SIMULINK

q1

3 2 1 0 -1 0 2 t 0.5 0 4 6

0.5 0

Ahora conociendo el algoritmo de las matrices H, C, h se prosigue a implementar esto en Simulink. Sabemos que:

q2
-0.5 -1 0

2 t

Donde:

0.1 0.05

q2

q4
0 2 t 4 6

-0.5 -1 -1.5

0 -0.05 -0.1 0 2 t 4 6

Entonces para implementar usar:

-2

Fig. 7 Salida de las articulaciones P

UNIVERSIDAD NACIONAL DE INGENIERA Ingeniera Mecatrnica

V.B Control PD

Fig. 8 Esquema de controlador P-D V.C Control PD compensacin de G

Haciendo una comparacin 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, ntese que una articulacin complicada es q2 ya que en el giro coordinado que se est realizando la tercera articulacin 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. Tambin se debe considerar el hecho que el robot es del simechanics por ende considera varias no linealidades como friccin entre las articulaciones. Hasta el momento se ha conseguido buen desempeo con el controlador V.C, El controlador PID lo usamos para variar algunos parmetros dependiendo de nuestra aplicacin, por ejemplo el tiempo de establecimiento (ts) tomando un criterio de 4% se puede notar que es menor.

V.D Control PID Fig. 9 Esquema de controlador PD compensando G

1 0.8 0.2 0

q1

q2
0 2 t 4 6

0.6 0.4 0.2 0

-0.2 -0.4 -0.6 0 2 t 1 0.8 PD PD+G

1.5 1 0.4 0.2

Fig. 12 Esquema de controlador PID

0 -0.2

q3

-0.6 -0.8 -1 0 2 t 4 6

q4

-0.4

0.6 0.4

q1

q2
0.5 0 0 2 t 0 4 6

0.2 0 0 2 t 4 6

0 -0.2 -0.4 0 x 10
-4

Fig. 10 Salida de las articulaciones (rad)

100 50 100

2 t

10 5

q3

T1

T2

-0.5

q4
0 0 2 t 4 6 -5 0

50 0

0 -50

-1

2 t

-50

2 t

2 t

20 0

5 0

x 10

-3

Fig. 13 Salida de las articulaciones PID (rad)

PD PD+G

T4

-20 -40 -60 0 2 t 4 6

-5 -10

2 t

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.

T3

Fig. 11 Torques necesarias (N.m)

UNIVERSIDAD NACIONAL DE INGENIERA Ingeniera Mecatrnica

VI. CONTROL PID DE POSICION

T1

Torque 1 20 0 -20 LaGrange Simechanics 5 4

2 t Torque 2

Para valores de las ganancias: Kp=diag([100,100,60,6]) Ki= diag([30,50,90,3]) Kd=diag([56,64,24,.8]) VI.A Planta LaGrange
10 0 -10 100 50 0

T2

2 t Torque 3

LaGrange Simechanics 5 4

100 0 -100

T3

2 t Torque 4

LaGrange Simechanics 5 4

T4

2 t

LaGrange Simechanics 5 4

Fig. 17 Torque para llegar a la posicin de referencia.

error x 0.2 0 -0.2

ex

2 t error y

LaGrange Simechanics 5 4

Fig. 14 Esquema de control PID ideal de posicin

0.1 0 -0.1

ey

2 t error z

LaGrange Simechanics 5 4

VI.A Planta SimMechanics

ez

0.5 0 -0.5

LaGrange Simechanics 5 4

t error q4 (rad) 2 0 -2

eq4

2 t

LaGrange Simechanics 5 4

Fig. 18 Torque para llegar a la posicin de referencia.

Fig. 15 Esquema de control PID real de posicin

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

Salida x 1 0.5 0

2 t Salida y

LaGrange Simechanics 5 4

0.2 0.1 0

2 t Salida z

LaGrange Simechanics 5 4

0.4 0.2 0

Fig. 19 Movimiento del Robot SimMechanics

LaGrange Simechanics 5 4

3 t ANGULO q4 (rad)

2 1 0

2 t

LaGrange Simechanics 5 4

Fig. 16 Salida real del XYZ

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 actuacin, los cuales segn la figs. 17 son siempre menores a 100N.m. Los resultados obtenidos en ambos casos sin compensacin de gravedad son parecidos como era de esperarse.

q4

UNIVERSIDAD NACIONAL DE INGENIERA Ingeniera Mecatrnica

VII. CONTROL DE TRAYECTORIA

10 600 400

Para valores de las ganancias:

T1

5 0 -5

T2
200 0 0 10 t 20 0 x 10
-3

Kp=diag([100,100,60,6]) Ki= diag([30,50,90,3]) Kd=diag([56,64,24,.8])

VII.A Torque computado
T3

10 t

20

150 100 50

2 0

T4
-2 -4 0 0 10 t 20 0

Planta controlada MITSUBISHI

10 t 20

Fig. 20 Torque necesario para seguir la trayectoria deseada

VII.B Luh Walker

Fig. 20 Esquema del controlador por Torque Computado

Fig. 23 Trayectoria realizada por el efector final 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

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;
TRAYECTORIA

Y el controlador PD:
u=diag([1000,990,800,800])*(eq)+diag([30,30,40,30])*(eqp)
TRAYECTORIA REAL

0.5
0.31

0.4

0.308

0.306

z
0.304 0.302 1 0.5 0 -0.5 y -1 -1 -0.5 x 0.5 0 1

0.3

0.2 0.1 1 0.5 0 -0.5 y -1 -1 -0.5 x 0.5 0 1

Fig. 22 Trayectoria realizada por el efector final

Fig. 24 Trayectoria realizada por el efector final

UNIVERSIDAD NACIONAL DE INGENIERA Ingeniera Mecatrnica

60 40

600 400

T1

20 0 -20 0 5 t 150 100 10 15

T2

200 0 -200 0 x 10
-3

5 t

10

15

5 0

T3

50 0 -50 0 5 t 10 15

T4
-5 -10 0

5 t

10

15

Fig. 25 Torque empleado en el Luh-Walker

COMPARACION DE TRAYECTORIAS

0.5

0.4

REFERENCES
z
0.3 0.2 0.1 1 0.5 0 -0.5 y -1 -1 -0.5 x 0.5 0 1

[1] [2] [3] [4] [5] [6] [7]

John H. Holland, Adaptation in Natural and Artificial Systems, 1975 Barrientos, Pein, Balaguer, Aracil. Fundamentos de Robtica, McGraw-Hill, 1977 Craig, John J, Robtica, Ed. Pearson Education, 2006 Katsuhiko Ogata, Ingeniera de Control Moderna, Pearson, 2003, 4ta edicion Ahmed A. Shabana, Computational Dynamics, Editorial John Wiley & Sons. Mathworks, Robotics Toolbox de Matlab Mathworks, Fuzzy Logic Toolbox de Matlab

Fig. 26 Error en la trayectoria real APPENDIX A PLANOS

Fig. 27. Planos del eslabn con junta rotacional. Eslabn 0

UNIVERSIDAD NACIONAL DE INGENIERA Ingeniera Mecatrnica

Fig. 28. Planos del eslabn con junta rotacional. Eslabn 1 Fig. 31. Planos del eslabn con junta rotacional. Eslabn 4

ALCANCES DEL MANIPULADOR ROBTICO

Fig. 29. Planos del eslabn con junta rotacional. Eslabn 2

Fig. 32. Alcances del manipulador

Fig. 30. Planos del eslabn con junta rotacional. Eslabn 3

UNIVERSIDAD NACIONAL DE INGENIERA Ingeniera Mecatrnica

10

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 operacin, inicialmente se plante el uso de controladores de este tipo dada su simplicidad. Un controlador PID es un mecanismo de control por realimentacin 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 linealizacin 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 sera la siguiente: