Control de trayectoria de manipulador robótico de

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 ek 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. LAGRANGE-EULER

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

simechanics 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


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:

Control de trayectoria de manipulador robótico de

Documents

Transcript of Control de trayectoria de manipulador robótico de