Control de trayectoria de un robot esferico de tres grados de libertad (página 2)

El torque ejercido por el robot en su eje de articulación para el eslabón 1 es: T1=1/3*cos(q2)^2*(m2*L2^2+3*m3*r^2- 3*m3*r*L3+m3*L3^2)*ddq1- 2/3*cos(q2)*(m2*L2^2+3*m3*r^2- 3*m3*r*L3+m3*L3^2)*sin(q2)*dq2*dq1+1/3*cos(q2)^2*(6* m3*r-3*m3*L3)*dr*dq1

Para el eslabón 2 es: T2=(1/3*m2*L2^2+m3*r^2- m3*r*L3+1/3*m3*L3^2)*ddq2+1/3*cos(q2)*(m2*L2^2+3*m3 *r^2-3*m3*r*L3+m3*L3^2)*sin(q2)*dq1^2+(2*m3*r- m3*L3)*dr*dq2+1/2*g*cos(q2)*(m2*L2+2*m3*r-m3*L3)

La fuerza ejercida por el eslabón 3 hacia su eje de traslación es: F3=m3*ddr-1/2*m3*(2*r- L3)*(cos(q2)^2*dq1^2+dq2^2)+g*m3*sin(q2)

2) Control Monoarticular. Consiste en controlar cada motor independientemente, sin tomar en cuenta las fuerzas y torques que los demás eslabones ejercen sobre cada motor.

En Simulink están los siguientes bloques de izquierda a derecha:

UNIVERSIDAD NACIONAL DE INGENIERÍA •

•

•

•

• Analisis y Control de Robots

Generador de trayectorias cartesianas, una línea recta en el espacio.

Transformación de coordenadas cartesianas a articulares, usando la cinemática inversa

Control PID discreto para cada motor.

Modelamiento con bloques continuos para los tres motores.

Modelamiento del robot por dinámica directa que produce los torques y fuerzas perturbares. El esquema en Simulink es el siguiente:

2.1) Generador de Trayectorias Cartesianas. El extremo del robot recorrerá una línea recta desde el punto A al punto B, siendo el tiempo de simulación de 12 segundos.

PA = (0.65,0,0.9)m PB = (0.4,0.8,1.2)m Cálculo de velocidades: 0 . 4-0 . 65 12 0 . 8-0 12 1 . 2-0 . 9 12 Por tanto las ecuaciones que describen el movimiento en metros, aplicando la fórmula MRU distancia=velocidad*tiempo, son:

-6 –

Y PX = -0.020833*t +0.65 P = 0.066666*t P Z = 0.025*t +0.9 El bloque en simulink es: 2.2) Transformación de Coordenadas Cartesianas a Articulares.

La referencia que le debe llegar a cada motor debe estar en coordenadas articulares, por tanto se debe emplear la cinemática inversa para pasar coordenadas en metros a rad para el motor 1 y 2 y milímetros para el motor 3. Las ecuaciones que describen la cinemática inversa son obtenidas por métodos geométricos, estas son: ) ) 2 Y P PX Y P Z -L1 PX + P2 q2 = arctag(

P Z -L1 r = sin(q2) q1 = arctag( El bloque MatLab Function llama a la función Cinv.m que calcula las coordenadas articulares en cada iteración.

UNIVERSIDAD NACIONAL DE INGENIERÍA Analisis y Control de Robots

2..3) Control PID discreto para cada motor

Los tres controladores fueron diseñados por el método del Lugar Geométrico de las Raíces. El PID discreto tiene la siguiente forma: kC * (z -a) * (z -b) z*(z -1) C(Z) = Los parámetros de diseño escogidos para los tres motores son un tiempo de establecimiento de 0.2seg, un sobreimpulso máximo de 2% y un periodo de muestreo de 0.001seg. ante entradas escalón. Los controladores hallados son. =

= 3340 * (z -0.9943) * (z -0. 974767 ) z*(z -1) 12000 * (z -0. 976093) * (z-0.9186) z*(z-1) 3340 * (z -0.9943) * (z -0. 974767 ) z*(z -1) C2(Z)

C3(Z) C1(Z) = Los motores 1 y 2 reciben el error de posición en radianes y entregan voltaje, mientras que el motor 3 recibe el error en milímetros y entregan voltaje.

Cada control tiene su limitador de voltaje a +- 100vdc y su ZOH con un tiempo igual al tiempo de muestreo de 0.001seg. Abriendo el bloque “PID discreto” de color naranja, tenemos: 2.4) Modelamiento de los motores DC El motor de corriente continua empleado en cada articulación es:

Marca: Baldor Voltaje=100v TorqueNominal=4N*m Potencia: 0.8Hp

-6 –

Los motores de corriente continua fueron modelados con bloques de suma, multiplicación e integrales de acuerdo a sus parámetros principales. Reciben como entrada el voltaje en voltios y el torque perturbador en N*m, y las salidas son la posición en radianes, la velocidad en radianes por segundo en el eje del engranaje (no del piñón) y la corriente. Los parámetros del motor son:

Ra = 2? La =1.8mH J =8*10-3kg*m2 B = 4.94*10-3N *m/(rad /s) kV = 0.3334v/(rad /s) kt = 0.3334N *m/ A

Cada motor está conectado a una caja reductora cuyos factores de engranaje son:

N1 =8 N2 =12 N3 = 2 El bloque que usa simulink es este, se puede apreciar las entradas y salidas. En el bloque interno se puede ver las ecuaciones que describen el movimiento de un motor, la de armadura y la de torque, en forma de bloques.

UNIVERSIDAD NACIONAL DE INGENIERÍA Analisis y Control de Robots

2.5) Modelamiento del Robot La consigna de este bloque es obtener los torques perturbadores que produce el robot en cada articulación. Estos torques que produce el robot dependen de las posiciones, velocidades y aceleraciones que ocurren en cada articulación y en todo momento, por tanto se calculará la dinámica directa. El bloque MatLab Function llama a la función DIN.m para el calculo de la dinámica directa.

El desarrollo de la dinámica directa se basa en la siguiente expresión:

Ti F i = Hi(q) *q'' +Ci(q,q') +hi(q) Donde: • Hi(q) es el tensor de inercial del eslabón i, por tanto el torque que produce la expresión Hi(q) *q''solo produce torque o fuerza cuando hay aceleraciones en los eslabones. •

• Ci(q,q') representa el torque o fuerza de Coriolis o centrípetas y ocurre cuando hay velocidades en las articulaciones. hi(q)es el torque o fuerza ejercida por la gravedad en cada articulación, esta actúa en todo instante y depende de la posición de cada eslabón del robot. Se muestra el archivo DIN.m a continuación:

% tensores inerciales H11=1/3*cos(q2)^2*(m2*L2^2+3*m3*r^2- 3*m3*r*L3+m3*L3^2); H22=1/3*m2*L2^2+m3*r^2-m3*r*L3+1/3*m3*L3^2; %H33=m3; H33=I3;

% coriolis cor1=-2/3*cos(q2)*(m2*L2^2+3*m3*r^2- 3*m3*r*L3+m3*L3^2)*sin(q2)*dq2*dq1+1/3*cos(q2)^2*(6*m 3*r-3*m3*L3)*dr*dq1; cor2=1/3*cos(q2)*(m2*L2^2+3*m3*r^2- 3*m3*r*L3+m3*L3^2)*sin(q2)*dq1^2+(2*m3*r- m3*L3)*dr*dq2; %cor3=-1/2*m3*(2*r-L3)*(cos(q2)^2*dq1^2+dq2^2);

-6 –

UNIVERSIDAD NACIONAL DE INGENIERÍA -6 – cor3=0;

% efecto de la gravedad h1=0; h2=1/2*g*cos(q2)*(m2*L2+2*m3*r-m3*L3); %h3=g*m3*sin(q2); h3=0;

% rozamiento viscoso Fv1=0; Fv2=0; Fv3=B3*dq3;

% torques perturbadores que entran a los motores Tp1=1/N1*(H11*ddq1+cor1+h1+Fv1); Tp2=1/N2*(H22*ddq2+cor2+h2+Fv2); % sist. con tornillo, no coriolis, no gravedad, solo I3 y B3 Tp3=1/N3*(H33*ddq3+cor3+h3+Fv3);

Vemos en el código de arriba dos detalles muy importantes.

Primero, los torques perturbadores están divididos por el factor de la caja reductora de cada motor, es decir el motor recibe una fracción del torque perturbador que hay en cada eje del articulación.

Segundo, el motor del eslabón 3 recibe torque y no fuerza, esto es debido a que es eslabón 3 está formado por un tornillo sin fin y una nuez que se desplaza, por tanto los efectos de Coriolis y gravedad serán ceros, y el tensor inercial será constante, además hay que agregar el rozamiento viscoso que todo tornillo produce.

3) Resultados de la Simulación

El tiempo de simulación a usar será de tres segundos, que nos permitirá ver la evolución del error en el tiempo para los tres motores. Recordar que el error es medido y controlado respecto al eje de articulación de cada eslabón del robot y no respecto al eje del motor.

La gráfica del error cometido por el motor en la articulación 1 es como máximo 0.006 rad bastante pequeño y el error en régimen permanente es cero, debido a que el sistema es de tipo II puede hacer el error cero ante entradas tipo rampas. Analisis y Control de Robots

El error cometido por el motor en la articulación 2 en radianes es:

El error cometido por el motor en la articulación 3 en milímetros es:

4) Recomendaciones. Se recomienda diseñar los controladores PID discretos para que respondan en forma sobreamortiguada, es decir con un factor de amortiguamiento de 1 (e=1) o Mp=0 (máximo sobreimpulso), con esto se reducirán las oscilaciones que se ven en el motor 3 del eslabón de traslación.

5) Conclusiones •

• Aplicar un control monoarticular, sin tener presentes en el control las perturbaciones que produce el robot, es recomendable cuando se tengan motores con caja de engranajes, ya sea interna o externa a él, para que disminuyan el torque perturbador producido por el robot. También este método es aplicable cuando las masas no son considerables y las aceleraciones y velocidades no son muy altas.

Usar la simulación para el motor DC es una herramienta importante que acerca las

Analisis y Control de Robots UNIVERSIDAD NACIONAL DE INGENIERÍA -6 – •

• respuestas de simulación a las respuestas reales, siempre y cuando el robot sea construido con una mecánica fina y el puente de H para los motores esté muy bien diseñado.

Se analizó el control desde el punto de vista del motor, es decir se controla al motor, en el eje de salida de la articulación, y no al robot. Es mejor y más real obtener la posición desde el motor que desde el robot por dinámica inversa.

Para obtener menos error en el estado transitorio en los motores es necesario aplicar una aceleración tipo rampa. En este caso la aceleración es infinita para alcanzar la velocidad nominal.

6) Referencias [1] Anibal Ollero Baturone “ROBÓTICA Manipuladores y Robots Móviles” 2001

[2] Antonio Barrientos “Fundamentos de Robótica” Segunda edición.