Tarea No. 2: Controlador cinematico del robot manipulador Antropomorco de 3GDL y ecuaciones del manipulador Satnford.

J. M. Ochoa Nuez, D. Gonzalez Novoa, J.F. Salomon Rodriguez

Topicos de Robotica II j Departamento de Robtica Universidad Politcnica de Guanajuato

Resumen

El manipulador se puede controlar segn nuestras necesidades se deben considerar la posicin del manipulador as como la trayectoria deseada o la posicin nal segn la informacin que podamos obtener de nuestro entorno de los elementos anteriores es como se disea el controlador.

Introduccin

En esta prctica se desarrollan las ecuaciones dinmicas que describen a un robot manipulador con tres grados de libertad sin efectos de gravedad el cual se simula su respuesta considerando algunos factores que estn en las ecuaciones caractersticas que lo representan de forma simple, las excuaciones que se utilizaran en este reporte son las que describen la cinematica del manipulador y se les denomina comunmente como la matriz Jacobiana.

Objetivo

Realizar una simulacin numrica de un seguimiento de trayectoria con un tiempo dado usando archivos m.le y bloques de Simulink.

3
3.1

Desarrollo y anlisis de resultados

Robot Antropomorco de 3GDL

El robot Stanforn consta de 3 eslabones, el eslabon 1 (l1 ) es jo y los eslabones l2 y l3 son moviles. Las ecuaciones que describen la cinematica de movimiento del manipuladro son las siguientes:

x = l2 cos( 1 ) cos( 2 ) + l3 cos( 1 ) cos( y = l2 sin( 1 ) cos( 2 ) + l3 cos( 1 ) cos( z = l1 + l2 sin( 2 ) + l3 sin( 2 + 3 )

+ 2+
2

3) 3)

(1)

Para obtener las ecuaciones del controlador, hay que ontener la cinematica diferencial directa del manipulador, esto se hacer derivando las ec(1) para que quede de la forma que se muestra a continuacion:

b11 b12 b13 b21 b22 b23 b31 b32 b33

= l2 sin( 1 ) cos( 2 ) l3 sin( 1 ) sin( 2 + 3 ) = l2 cos( 1 ) sin( 2 ) l3 cos( 1 ) sin( 2 + 3 ) = l3 cos( 1 ) sin( 2 + 3 ) = l2 cos( 1 ) cos( 2 ) l3 sin( 1 ) cos( 2 + 3 ) = l2 sin( 1 ) sin( 2 ) l3 cos( 1 ) sin( 2 + 3 ) = l3 cos( 1 ) sin( 2 + 3 ) = 0 = l2 cos( 2 ) + l3 cos( 2 + 3 ) = l3 cos( 2 + 3 )

3 2 x _ b11 4 y 5 = 4 b21 _ z _ b31

b12 b22 b32

32 _ 3 b13 1 b23 5 4 _ 2 5 _3 b33

(2)

la ec(2) se puede respresentar de anera escalar como se muestra a continuacion en la ec() x = J_ _

(3)

De la ec(3) despejamos _ para obtener la cinematica diferencial de velicidad y como _ es nuestra entrada de control, entonces podemos deducir que _ = u, la ecuacion de nuestro controlador, quedad dada de la siguiente manera, representada en forma escalar:
1

u=J

(4)

donde es la dimanica del error y J es la matriz jacobiana, representada matricialmente, la escuacion del controlador, es como se muestra a continuacion: 2 donde b11 J = 4 b21 b31 2 b12 b22 b32 3 b13 b23 5 b33 3 u1 4 u2 5 = J u3 2 xd k(x _ 4 y k(y) _ zd k(z _ 3 xd) yd 5 zd)

(5)

(6)

Ahora que ya tenemos la ecuacion del controlador, se hara la simulacion en Simulink, utilizando bloques y archivos m.le. se representaran las ec(5) y (3). El primer paso es realizar los archivos m.le de las ecuaciones del controlador y de la cinematica diferencial directa, con esto obtendremos nuestras x reales y la entrada de control respectivamente. En primera estacncia, queremos que nuestra manipulador siga una trayectoria deseada, esta la vamos a generar en bloques de Simulink como se muestar en la Figura(1)

Figura 1. Diagrama de bloques de la trayectoria deseada. La trayectoria deseada es una espiral como se muestra a continuacion en la Figura(2)

Figura 2. Trayectoria deseada El diagrama de bloques de Simulink del controlador es el que se muestra en la Figura(3)

Figura 3. diagrama de bloques de Simulink del controlador cinematico diferencial de velocidad El bloque naranja de la Figura(??) contiene el controlador cinematico de velocidad del manipuldor, esta contenido en un archivo m.le, el cual contiene el siguiente codigo: function salida = controladorfunction4(w) %constantes k=50; % ganancia del controlador %entradas al sistema l2=w(1); l3=w(2); teta1=w(3); teta2=w(4); teta3=w(5); xr=w(6); yr=w(7); zr=w(8); dxd=w(9); dyd=w(10); dzd=w(11); xd=w(12); yd=w(13); zd=w(14); %elementos de la matriz Jacobiana b11= -((l2*sin(teta1)*cos(teta2))+(l3*sin(teta1)*sin(teta2+teta3))); b12= -((l2*cos(teta1)*sin(teta2))+(l3*cos(teta1)*sin(teta2+teta3))); b13= -(l3*cos(teta1)*sin(teta2+teta3)); b21= (l2*cos(teta1)*cos(teta2))-(l3*sin(teta1)*cos(teta2+teta3)); 4

b22= -((l2*sin(teta1)*sin(teta2))+(l3*cos(teta1)*sin(teta2+teta3))); b23= -l3*cos(teta1)*sin(teta2+teta3); b31= 0; b32= (l2*cos(teta2))+(l3*cos(teta2+teta3)); b33= l3*cos(teta2+teta3); %matriz Jacobiana J=[ b11 b12 b13; b21 b22 b23; b31 b32 b33]; %matriz de ganacias del controlador n=[(dxd-k*(xr-xd));(dyd-k*(yr-yd));(dzd-k*(zr-zd))]; %controlador dteta= Jnn; salida= dteta; El bloque de color crema contiene la cinematica diferencial directa, de la cual se obtiene la trayectoria real, esta contenida en un archivo m.le, el cual contiene el siguiente codigo: function salida = treal(g) %entradas al sistema teta1=g(1); teta2=g(2); teta3=g(3); dteta1=g(4); dteta2=g(5); dteta3=g(6); l2=g(7); l3=g(8); %elementos de la matriz Jacobiana b11= -((l2*sin(teta1)*cos(teta2))+(l3*sin(teta1)*sin(teta2+teta3))); b12= -((l2*cos(teta1)*sin(teta2))+(l3*cos(teta1)*sin(teta2+teta3))); b13= -(l3*cos(teta1)*sin(teta2+teta3)); b21= (l2*cos(teta1)*cos(teta2))-(l3*sin(teta1)*cos(teta2+teta3)); b22= -((l2*sin(teta1)*sin(teta2))+(l3*cos(teta1)*sin(teta2+teta3))); b23= -l3*cos(teta1)*sin(teta2+teta3); b31= 0; b32= (l2*cos(teta2))+(l3*cos(teta2+teta3)); b33= l3*cos(teta2+teta3); %matriz Jacobiana J=[ b11 b12 b13; b21 b22 b23; b31 b32 b33]; %dteta dteta=[dteta1;dteta2;dteta3]; %cinematica de velocidad x=J*dteta; salida = x; Ahora con estos elementos podemos realizar la simulacion del desempeo del controlador cinematico diferencial de velocidad del manipulador Stanford, y los resultados de la simulacion son lo siguientes:

Figure 1: Figura 5. y real vs y deseada

Figura 4. x real vs x deseada

Figurra 6. z real vs z deseada En la Figuras (4),(5) y (6) se muestra el desempeo del controlador el cual tiene una ganancia proporcional k=50, como podemos observar, el controlador si sigue la trayectoria deseada em x, y , z, esto se puede ver mas claramente en la siguiente Figuras:

Figura 7. Trayectoria real

Figura 8. trayectoria real vs trayectoria deseada Como podemos observar en la Figura(8) se ve ma claramente como el controlador siguie la trayectoria que se pripuso, la cual es una espiral.

3.2

Robot Estanford.

el robot estanford presenta 3 eslabones l1 ; l2 y l3 , con 3 grados de libertad, la cinematica direscta esta dada por la siguiente ecuacion: x = cos( 1 )(cos 2 (d2 + d3 )) y = sin( 1 )(cos 2 (d2 + d3 )) z = sin 2 (d2 + d3 ) + d1 (7)

La representacion usual del modelo matematico con escuaciones de la cinematica del robot tiene la forma como se muestra en la ec(??) x = J_ (8)

derivando de la ec(7) obtenemos,x,y y z las cuales representan x de la ec(??), donde J es la matriz Jacobiana, _ _ _ _ la cual contiene los siguientes elementos:

b11 b12 b13 b21 b22 b23 b31 b32 b33

= sin 1 (cos 2 (d2 + d3 )) = cos 1 (cos 2 (d2 + d3 )) = 0 = cos 1 (sin 2 (d2 + d3 )) = sin 1 (sin 2 (d2 + d3 )) = cos 2 (d2 + d3 ) + d1 = cos 1 (cos 2 (d2 + d3 )) = sin 1 (cos 2 (d2 + d3 )) = sin 2 (d2 + 1) 8

b11 J = 4 b21 b31

b12 b22 b32

3 b13 b23 5 b33

(9)

La ec(9) representa a la matriz Jacobiana la cual contiene los checientes se la cinematica diferencial directa, estos cohecientes tienen informacion de la cinematica del manipulador, decriben sus movimientos. Ahora, como _ es la entrada de control del sistema, vamos a depejara para obtener la cinematica diferencial inversa y asi disea el controlador cinematico de velocidad: x _ _ , el diseo del controlador esta dado de la siguiente forma: De la ec(10), sabiedo que u = u=J donde: = xd _ k(x xd) (12)
1

_ =J

(10)

(11)

la ec(12) representa el controlador, donde k es igual a la k prporcional, osea la ganacia del controlador.xd es la trayectoria deseada sustituyendo la ec(12) en la ec(11), obtenemos el controlador cinematico de velocidad: u=J
1

(xd _

k(x

xd))

(13)

El contrlolador de manera matricial esta representado de la siguiente manera: 3 2 3 2 u1 xd k(x xd) _ 4 u2 5 = J 1 4 yd k(y yd) 5 _ u3 zd k(z zd) _

(14)

La ec(14) es el controlador cinematico de velocidad, las salidas u1 ; u2 y u3 ahora serian las entradas de control, las cuales se sustiyuirian en la ec(??) y asi se obtendria la trayectosia real del manipulador.

Conclusiones generales

Con las simulaciones se puede predecir el comportamiento de los sistemas sus ecuaciones caracterizan cuantos factores quieras incluir tanto externos como internos aceleracin carga perturbaciones el manipulador se desplaza en el plano XYZ este se podra utilizar en varios procesos como la soldadura por arco. La cinematica de un manipulador es muy util para el diseo de los controladores de posicion y velocidad de los actuadores de este, puesto que con la informacion que nos brinda es posible hacer un controlador cinematico de velocidad, como se hizo en la simulacion del robot antropomorco de 3GDL.

Referencias

References
[1] Notas de Tpicos II. Seguimiento de trayectorias Esteban Chvez Conde.