control de trayectoria de manipulador robótico de
DESCRIPTION
Se realiza un analisis de los diferentes controladores sintonizados por un PID, inicialmente en el espacio articular y luego en xyzphi para controlar la trayectoria del robotTRANSCRIPT
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
UNIVERSIDAD NACIONAL DE INGENIERÍA – Ingeniería Mecatrónica 2
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
UNIVERSIDAD NACIONAL DE INGENIERÍA – Ingeniería Mecatrónica 3
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
Hrot2=(0R2)xI2x(0R2)t
Hrot3=(0R3)xI3x(0R3)t
Hrot4=(0R4)xI4x(0R4)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
UNIVERSIDAD NACIONAL DE INGENIERÍA – Ingeniería Mecatrónica 4
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
UNIVERSIDAD NACIONAL DE INGENIERÍA – Ingeniería Mecatrónica 5
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.
UNIVERSIDAD NACIONAL DE INGENIERÍA – Ingeniería Mecatrónica 6
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
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
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.
UNIVERSIDAD NACIONAL DE INGENIERÍA – Ingeniería Mecatrónica 7
-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
Para valores de las ganancias:
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
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
Y el controlador PD:
u=diag([1000,990,800,800])*(eq)+diag([30,30,40,30])*(eqp)
Fig. 24 Trayectoria realizada por el efector final
UNIVERSIDAD NACIONAL DE INGENIERÍA – Ingeniería Mecatrónica 8
-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
UNIVERSIDAD NACIONAL DE INGENIERÍA – Ingeniería Mecatrónica 9
Fig. 28. Planos del eslabón con junta rotacional. Eslabón 1
Fig. 29. Planos del eslabón con junta rotacional. Eslabón 2
Fig. 30. Planos del eslabón con junta rotacional. Eslabón 3
Fig. 31. Planos del eslabón con junta rotacional. Eslabón 4
ALCANCES DEL MANIPULADOR ROBÓTICO
Fig. 32. Alcances del manipulador
UNIVERSIDAD NACIONAL DE INGENIERÍA – Ingeniería Mecatrónica 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 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: