Control de Trayectoria de Manipulador Robótico Mitsubishi PDF

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.
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.
Index Terms Diseo, SimMechanics, Controladores, PID,

Torque computado.
Fig. 2. Similitud del brazo humano al robtico
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
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:
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
III. LAGRANGE-EULER
Tabla DH
Datos del robot:

Grfico:
0T
1=DH(1,d1,a1,1)
2T
3=DH(3,d3,a3,3)
1T
2=DH(2,d2,a2,2)
3T
4=DH(4,d4,a4,4)
0T
T 2=
0T
T 1*
2
1T
2
3
0
0
1T * 2T
4
1
2
3
4
0T = 0T * 1T * 2T * 3T
0
T 3=
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
1=
0R
1=
0T
0
T 1*
1(1:3,1:3)
0P
2
2
0R =0T (1:3,1:3)
3= T3(1:3,1:3)
R
0
0
4
4
0R =0T (1:3,1:3)
1=
P 2=
0T
1(1:3,4)
2(1:3,4)
3
0T (1:3,4)
4
4
0P =0T (1:3,4)
P 3=
0T
3er Paso: Posicin de los centros de masa de cada eslabn

respecto al referencial X0Y0Z0
Tabla de Dnabith Hrtenberg (DH)
Articulacin
q1
L1
/2
q2
L2
q3+/2
/2
q4
L3
III.A Hallando el tensor de inercia del

robot h(q)
1er Paso: Matriz de transformacin
2do Paso: Posicin de los centros de masa de cada eslabn

respecto al referencial XkYkZk
Ck 0Rk
0P
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
5to Paso: Tensor Inercial Rotacional de cada eslabn.
0R
Ik
Observacin: Jacobiano total de cada eslabn:
8vo Paso: Tensor Inercial Total de cada eslabn
Hrot1=(0R1)xI1x(0R1)t
mk
6to Paso: Jacobiano de velocidades lineales del centro de masa

de cada eslabn
Jvk
Hrotk
Jwk
H1=(Jv1)txm1x(Jv1)+ (Jw1)txHrot1x(Jw1)
9no Paso: Tensor Inercial Total del robot
Ck
qk
Jv1=[C1/q1
0
0
0 ]t
2
Jv =[C2/q1 C2/q2
0
0 ]t
3
Jv =[C3/q1 C3/q2 C3/q3
0 ]t
4
Jv =[C4/q1 C4/q2 C4/q3 C4/q4]t
Hk
H=H1+H2+H3+H4
7mo Paso: Jacobiano de velocidades angulares de cada eslabn
III.B Algoritmo para Hallar
Donde:
1er paso: Derivada de Matriz
Jw1=[1*uz0
Jw2=[1*uz0
Jw3=[1*uz0
Jw4=[1*uz0
0
2*uz1
2*uz1
2*uz1
uzk-1
0
0
3*uz2
3*uz2
0 ]t
0 ]t
0 ]t
4*uz3 ]t
2do paso: Vector de velocidad articular
3er paso: Vector de Coriolis
III.C Algoritmo para hallar

1er paso: Energa Potencial
Figura5: Planta de Dinmica Lagrangiana (ideal).
V.
Donde:
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
MODELADO DE LA
DINAMICA EN SIMULINK
0.5
q2
-0.5
0
-1
-1
q2
Donde:
0.5
0.1
0.05
-0.5
q4
Ahora conociendo el algoritmo de las matrices H, C, h se

prosigue a implementar esto en Simulink.
Sabemos que:
q1
IV.
-1
-0.05
-1.5
Entonces para implementar usar:
-2
4
t
-0.1
2
t
Fig. 7 Salida de las articulaciones P
V.B Control PD
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.
Fig. 8 Esquema de controlador P-D

V.C Control PD compensacin de G
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.2
0
0.6
q2
q1
0.8
-0.2
0.4
-0.4
0.2
0
-0.6
-0.2
0.8
-0.4
0.6
-0.6
0.4
-0.8
0.2
0
Fig. 12 Esquema de controlador PID
PD
PD+G
1.5
0.4
0.2
q1
-1
4
t
q4
q3
q2
0.5
-0.2
t
0
Fig. 10 Salida de las articulaciones (rad)
-0.4
t
0
100
100
50
50
t
10
x 10
-4
0
-1
-50
-50
x 10
PD
PD+G
T4
-20
-5
-40
-10
0
4
t
4
t
-5
2
t
Fig. 13 Salida de las articulaciones PID (rad)
-3
T3
20
-60
q4
q3
T2
T1
5
-0.5
4
t
Fig. 11 Torques necesarias (N.m)
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.
VI. CONTROL PID DE POSICION
Torque 1
T1
20
0
-20
LaGrange
4Simechanics 5
LaGrange
4Simechanics 5
LaGrange
4Simechanics 5
LaGrange
4Simechanics 5
t
Torque 2
T2
100
50
0
Kp=diag([100,100,60,6])
Ki= diag([30,50,90,3])
Kd=diag([56,64,24,.8])
T3
t
Torque 3
100
0
-100
VI.A Planta LaGrange
2
t
Torque 4
T4
10
0
-10
2
t
Fig. 17 Torque para llegar a la posicin de referencia.
ex
error x
0.2
0
-0.2
LaGrange
4Simechanics 5
LaGrange
4Simechanics 5
LaGrange
4Simechanics 5
t
error y
ey
Fig. 14 Esquema de control PID ideal de posicin

0.1
0
-0.1
2
t
error z
ez
VI.A Planta SimMechanics
0.5
0
-0.5
eq4
t
error q4 (rad)
2
0
-2
LaGrange
4Simechanics 5
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
LaGrange
4Simechanics 5
LaGrange
4Simechanics 5
t
Salida y
0.2
0.1
0
q4
t
Salida z
0.4
0.2
0
2
1
0
Fig. 19 Movimiento del Robot SimMechanics
3
t
ANGULO q4 (rad)
LaGrange
4Simechanics 5
LaGrange
4Simechanics 5
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.
VII. CONTROL DE TRAYECTORIA

10
600
400
T2
T1
Kp=diag([100,100,60,6])
Ki= diag([30,50,90,3])
Kd=diag([56,64,24,.8])
-5
10
t
20
100
0
x 10
10
t
20
10
t
20
-3
T4
150
T3
VII.A Torque computado
200
50
-2
Planta controlada MITSUBISHI

0
10
t
-4
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:
Y el controlador PD:
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;
u=diag([1000,990,800,800])*(eq)+diag([30,30,40,30])*(eqp)
TRAYECTORIA REAL
TRAYECTORIA
0.5
0.31
0.4
0.308
0.3
0.306
0.2
0.304
0.1
1
0.302
1
0.5
0.5
1
0.5
0.5
-0.5
-1
-1
y
x
-0.5
-0.5
-0.5
-1
-1
60
600
40
400
T2
T1
20
200
0
-20
0
0
10
-200
15
t
5
100
15
x 10
10
15
-3
T4
T3
10
t
150
50
-5
0
-50
10
-10
15
Fig. 25 Torque empleado en el Luh-Walker
COMPARACION DE TRAYECTORIAS
0.5
0.4
REFERENCES
0.3
[1]
[2]
0.2
[3]
[4]
0.1
1
0.5
1
0.5
-0.5
y
-0.5
-1
-1
[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

ALCANCES DEL MANIPULADOR ROBTICO
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 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:
10

Control de Trayectoria de Manipulador Robótico Mitsubishi PDF

Cargado por

Información del documento

Título original

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

Control de Trayectoria de Manipulador Robótico Mitsubishi PDF

Cargado por

Copyright:

Formatos disponibles

UNIVERSIDAD NACIONAL DE INGENIERA Ingeniera Mecatrnica