Upload
jecamarenaq
View
392
Download
0
Embed Size (px)
DESCRIPTION
Modelamiento dinámico de manipulador robótico de 4 GDL y análisis de diferentes controladores en espacio articular y espacio carteciano, PID, Torque Computado, Luh Walker, RNA. Empleando La Grange, Newton Euler para los modelamientos.
Citation preview
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
UNIVERSIDAD NACIONAL DE INGENIERÍA – Ingeniería Mecatrónica 2
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
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
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.
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
Fig. 28. Planos del eslabón con junta rotacional. Eslabón 1
Fig. 29. Planos del eslabón con junta rotacional. Eslabón 2
UNIVERSIDAD NACIONAL DE INGENIERÍA – Ingeniería Mecatrónica 9
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
ALGORITMO LUH WALKER
Fig. 33. 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:
UNIVERSIDAD NACIONAL DE INGENIERÍA – Ingeniería Mecatrónica 10
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.
Fig. 34. Alcances del manipulador
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.
UNIVERSIDAD NACIONAL DE INGENIERÍA – Ingeniería Mecatrónica 11
Figura 35. Estructura de Aprendizaje Generalizado
Figura 36. Estructura de Aprendizaje por Error de Realimentación