Ejercicios Resueltos Cinematica Inversa_matlab

March 24, 2020 | Author: Anonymous | Category: Funciones trigonométricas, Rotación, Cinemática, Trayectoria, Vector Euclidiano
Share Embed Donate


Short Description

Download Ejercicios Resueltos Cinematica Inversa_matlab...

Description

EJERCICIO 2. Programe la cinemática inversa de un robot manipulador de 3 DOF, tal que realice las siguientes tareas: El robot seleccionado es del tipo RRR y RRR  y la cinemática inversa se obtuvo mediante el método geométrico, como sigue: Z2 Z1

Z0

Sz  Y 0  X0

1

 X1

r

P x

P y

La proyección de la herramienta sobre el plano  x-y,  x-y, y sobre el plano r-s permite calcular los ángulos 1, 2 y 3, utilizando diferentes propiedades trigonométricas, en las figuras se observan las proyecciones:

 Y 0

Z0

a3 S

r

P y

a2

1

P x

2

2

 X0

r

Realizando las diferentes operaciones trigonométricas con el fin de encontrar los ángulos por medio de la función arcotangente se obtuvo que:

 1

  A tan 2( px,  py )

 3

  A tan 2( D,

2

1  D )

 D  cos 3  D   D 

r 2  s 2  a22

2

 a3

2a 2 a3  p x2

2

2

2

2

  p y   p z   a 2  a3

2a 2 a3

 2

  A tan 2( r , s )   A tan 2((a 2  a3 cos 3 ), ( a3 sen 3 ))

 2

  A tan 2(

 p x2

2

  p y

,  p z  )   A tan 2((a2

 a3 cos 3 ), ( a3 sen 3 ))

Los parámetro de Denavit-Hartenberg utilizados para el robot seleccionado tipo RRR, fueron los calculados para la Tarea 1, a continuación se presentan:

n

ai  

di  

i  

1 2 3

0 a2 a3

d1 0 0

-90 0 0

θi   θ1 θ2 θ3

Para realizar los programas de cinemática inversa se utilizo el software MatLab y su Toolbox de Robótica. En el Toolbox se realizo la definición del robot para poder comprobar mediante simulación la cinemática inversa. Para definir un robot con sus diferentes parámetros se tiene el siguiente programa: L1=link([alfa1 a1 q10 d1]); Define el eslabón 1 L2=link([alfa2 a2 q20 d2]); Define el eslabón 2 L3=link([alfa3 a3 q30 d3]); Define el eslabón 3 ROBOTINA=robot({L1 L2 L3}); Asocia a una variable la configuración del robot

a. Un círculo sobre un plano a una distancia ZT. a1=0; a2=8; a3=8; alfa1=-pi/2; alfa2=0; alfa3=0; d1=13; d2=0; d3=0; q10=0; q20=0; q30=0;

Parámetros de entrada del Robot

Definición del robot L1=link([alfa1 a1 q10 d1]); Define el eslabón 1 L2=link([alfa2 a2 q20 d2]); Define el eslabón 2 L3=link([alfa3 a3 q30 d3]); Define el eslabón 3 ROBOTINA=robot({L1 L2 L3}); Asocia a una variable la configuración del robot R xo yo pz

Parámetros de entrada del Círculo Radio del círculo Distancia del centro del circulo al origen en x Distancia del centro del circulo al origen en y Posición deseada en z

i=1; for(x=0:1:360) beta=x*pi/180; px=xo+R*cos(beta); py=yo+R*sin(beta);

Ciclo de Cálculo Calculo del Círculo Angulo para calcular el circulo Posición deseada en x Posición deseada en y

Calculo de los Ángulos D=(px^2+py^2+pz^2-a2^2-a3^2)/(2*a2*a3); q1=atan2(py,px); q3=atan2(sqrt(1-D^2),D); q2=atan2(pz,sqrt(px^2+py^2))-atan2(a3*sin(q3),a2+a3*cos(q3)); q(i,:)=[q1,q2,q3];

Matriz de los vectores (q)

 Actualización datos del vector d (obtenido por la cinemática directa) dx=cos(q1)*(a2*cos(q2)+a3*cos(q2+q3)); dy=sin(q1)*(a2*cos(q2)+a3*cos(q2+q3)); dz=d1-a2*sin(q2)-a3*sin(q2+q3);

Dx(i,:)=[dx]; Dy(i,:)=[dy]; Dz(i,:)=[dz]; X(i,:)=[px];  Y(i,:)=[py]; Z(i,:)=[pz];

Matriz de los vectores (Dx, Dy, Dz)

Matriz de los vectores (px, py, pz)

i=i+1; end Rutina para graficar figure(1) grid plot3(X,Y,Z) Grafica de la trayectoria calculada figure(2) plot3(Dx,Dy,Dz) Grafica de la trayectoria realizada plot(ROBOTINA,q,'ROBOTINA','shadow','erase') Grafica del robot siguiendo la trayectoria

b. Un circulo en un plano inclinado



grados sobre el plano x-y.

Para calcular el circulo en un plano inclinado se utiliza el mismo programa, solo se modifica los parámetros de entrada y el ciclo de calculo, como sigue: Parámetros de entrada del círculo rotado

R xo yo zo

Radio del círculo Distancia del centro del circulo al origen en x Distancia del centro del circulo al origen en y Altura desde el origen hasta el centro del circulo en z

alfa=30*pi/180; a=R; b=R*cos(alfa); i=1; phi=0;

Angulo de inclinación del plano alrededor de Y  Radio mayor de la elipse Radio menor de la elipse Ciclo de cálculo

for(n=0:1:360) beta=n*pi/180; x = a*cos(beta); y = b*sin(beta);

Ecuación Paramétrica de una elipse

Transformación de Coordenadas px = cos(phi)*x - sin(phi)*y; py = sin(phi)*x + cos(phi)*y; px = px + xo; Posición deseada en x py = py + yo; Posición deseada en y pz = (zo-0.5*R*sin(sigma))+(R*sin(sigma))*(1-sin(beta)); Posición deseada en z Calculo de los Ángulos D=(px^2+py^2+pz^2-a2^2-a3^2)/(2*a2*a3); q1=atan2(py,px); q3=atan2(sqrt(1-D^2),D); q2=atan2(pz,sqrt(px^2+py^2))-atan2(a3*sin(q3),a2+a3*cos(q3)); q(i,:)=[q1,q2,q3];

Matriz de los vectores (q)

 Actualización datos del vector d (obtenido por la cinemática directa) dx=cos(q1)*(a2*cos(q2)+a3*cos(q2+q3)); dy=sin(q1)*(a2*cos(q2)+a3*cos(q2+q3)); dz=d1-a2*sin(q2)-a3*sin(q2+q3);

Dx(i,:)=[dx]; Dy(i,:)=[dy]; Dz(i,:)=[dz]; X(i,:)=[px];  Y(i,:)=[py]; Z(i,:)=[pz];

Matriz de los vectores (Dx, Dy, Dz)

Matriz de los vectores (px, py, pz)

i=i+1; end Rutina para graficar figure(1) grid plot3(X,Y,Z) Grafica de la trayectoria calculada figure(2) plot3(Dx,Dy,Dz) Grafica de la trayectoria realizada plot(ROBOTINA,q,'ROBOTINA','shadow','erase') Grafica del robot siguiendo la trayectoria

c. Un corte de manzana. Para calcular el circulo en un plano inclinado se utiliza el mismo programa, solo se modifican los parámetros de entrada y el ciclo de calculo, como sigue: Parámetros de entrada xo=; Distancia del centro del circulo al origen en x yo=; Distancia del centro del circulo al origen en y zo=; Distancia del centro del circulo al origen en z ro=; Radio mayor Ciclo de cálculo i=1; k=10; for(x=0:1:k*360)

Numero de vueltas Ecuación de círculo con radio variable

beta=x*pi/180; R=ro*sin(beta/(2*k)); px=xo+R*cos(beta); py=yo+R*sin(beta); pz=zo-2*sin(beta/(4*k));

Angulo para calcular el circulo Radio variable de la espiral Posición deseada en x Posición deseada en y Variación de la altura

Calculo de los Ángulos D=(px^2+py^2+pz^2-a2^2-a3^2)/(2*a2*a3); q1=atan2(py,px); q3=atan2(sqrt(1-D^2),D); q2=atan2(pz,sqrt(px^2+py^2))-atan2(a3*sin(q3),a2+a3*cos(q3)); q(i,:)=[q1,q2,q3];

Matriz de los vectores (q)

 Actualización datos del vector d (obtenido por la cinemática directa) dx=cos(q1)*(a2*cos(q2)+a3*cos(q2+q3)); dy=sin(q1)*(a2*cos(q2)+a3*cos(q2+q3)); dz=d1-a2*sin(q2)-a3*sin(q2+q3); Dx(i,:)=[dx]; Dy(i,:)=[dy]; Dz(i,:)=[dz]; X(i,:)=[px];  Y(i,:)=[py]; Z(i,:)=[pz]; i=i+1; end

Matriz de los vectores (Dx, Dy, Dz)

Matriz de los vectores (px, py, pz)

Rutina para graficar

figure(1) grid plot3(X,Y,Z) Grafica de la trayectoria calculada figure(2) plot3(Dx,Dy,Dz) Grafica de la trayectoria realizada plot(ROBOTINA,q,'ROBOTINA','shadow','erase') Grafica del robot siguiendo la trayectoria

d. Un corte de manzana pero que la orientación de los últimos 3 DOF sigan planos tangentes. Primero se calcula la cinemática inversa de un robot de seis grados de libertad. En este caso se utilizo un robot de tipo RRR, cuyos parámetros de Denavit-Hartenberg se observan a continuación:

n

ai  

di  

1 2 3 4 5 6

0 a2 a3 0 0 0

d1 0 0 0 0 d6

θi  

i  

-90 0 90 -90 -90 0

1 2 3 4 5 6

Para calcular la cinemática inversa se utilizo el método de desacople cinemático ya que los tres últimos ejes Z se interceptan en un punto. d6

a3 a2

Z3

Z6 Z4  X1  X2 Z2

Z1

 Y 1

d05

Z0

 Y 0  X0

 Y 2

dd0066

Z5

 X6

 Y 6

Por tanto: d 05



d 06

 d 6 R K 

 pc  d   d 6 R K 

Se calcula el vector al centro de la muñeca ( pc ) desplazado del lugar donde se desea tener el centro de la herramienta ( d  ), la distancia desde la herramienta hasta donde se interceptan los tres últimos ejes de Z, (distancia d  rotada según el origen al premultiplicarla por la matriz de rotación de Roll, Pitch, Yaw,  RK  ). 6

De esta forma se desacoplan los tres primeros ángulos de los tres últimos. Así los tres primeros ángulos se calculan utilizando como vector de posición, el que se calcula de acuerdo a:  px   xd   d 6 r 13  py

  yd   d 6 r 23

 pz    zd   d 6 r 33  px   xd   d 6 (s  s  + c  s   c  )  py

  yd   d 6 (-c s 

+ s  s  s  )

 pz    zd   d 6 (c  c  )

En general como datos de entrada se tienen: xd, Posición deseada en x yd, Posición deseada en y zd, Posición deseada en z , Ángulo de rotación deseado sobre el eje z , Ángulo de rotación deseado sobre el eje y   Ángulo de rotación deseado sobre el eje z Con estos datos se calculan entonces los ángulos trigonométrico, como se hizo en la tarea anterior.

1, 2

y

3

con el método

 1

  A tan 2( px,  py )

 3

  A tan 2( D,

2

1  D )

 D  cos 3  D   D 

r 2  s 2  a22

2

 a3

2a 2 a3  p x2

2

2

2

2

  p y   p z   a 2  a3

2a 2 a3

 2

  A tan 2( r , s )   A tan 2((a 2  a3 cos 3 ), ( a3 sen 3 ))

 2

  A tan 2(

 p x2

2

  p y

,  p z  )   A tan 2((a2

 a3 cos 3 ), ( a3 sen 3 ))

La cinemática inversa de los tres últimos ángulos se calcula de acuerdo a:

R 03 =





0



0

-s1

c2

-s2

0

c3

0

s3

s1

0

c1

s2

C2

0

s3

0

- c3

0

-1

0

0

0

1

0

1

0

c1

R 03 =

 R   R : 3 T 

6

 R3

c1c2c3 - c1s2s3

-s 1

c1c2s3 + c1c 3s2

c2c3s1 - s1s2s3

c 1

c2s1s3 + c3s1s2

-

c2s3 - c3s2

0

c2c3 - s2s3

c1c2c3 - c1s2s3

R 36 =

c2c3s1 - s1s2s3

-



-s1 c1

 R   R :



0



T ccθ

-scψ+csθsψ

ssψ+csθcψ

c2s1s3 + c3s1s2

scθ

ccψ+ssθsψ

-csψ+ssθcψ

c2c3 - s2s3

-sθ

csψ

cθcψ

0

c2s3 - c3s2

3 T 

6

 R3

c1c2s3 + c1c 3s2



U11

U12

U13

U21

U22

U23

U31

U32

U33

U11= cθc (c1c2c3-c1s2s3)+cθs (c2c3s1-s1s2s3)-sθ(-c2s3-c3s2) φ

U12= (c cψ+sθs sψ)(c2c3s1-s1s2s3)+c sψ(-c2s3-c3s2)+ (c1c2c3-c1s2s3)(-c sφ+c sθsψ) φ

φ

φ

φ

φ

U13= (s sψ+c  —cψsθ)(c1c2c3-c1s2s3)+cθcψ(-c2s3-c3s2)+ (c2c3s1-s1s2s3)(-c sψ+cψsθs ) φ

φ

φ

φ

U21= c1cθs - s1cθc φ

φ

U22= c1(c cψ+sθs sψ)-s1(-cψs +c sθsψ) φ

φ

φ

φ

U23= -s1(s sψ+c cψsθ)+c1(-c sψ+cψsθs ) φ

φ

φ

φ

U31= cθc (c1c2s3+c1c3s2)+cθs (c2s1s3+c3s1s2)-sθ(c2c3-s2s3) φ

φ

U32= (c2s1s3+c3s1s2)(c cψ+sθs sψ)+c sψ(c2c3-s2s3)+ (c1c2s3+c1c3s2)(-cψs +c sθsψ) φ

φ

φ

φ

φ

U33= (c1c2s3+c1c3s2)(s sψ+c cψsθ)+cθcψ(c2c3-s2s3)+ (c2s1s3+c3s1s2)(-c sψ+cψsθs ) φ

φ

φ

φ

 Ahora para calcular los ángulos 4, 5 y 6 se calculan los ángulos de Euler ,  y  a partir de la matriz U como se muestra a continuación:

2     A tan(u33 , 1  u33 )

    A tan(u13 , u 23 )     A tan(u31 , u32 )

Los ángulos de Euler corresponden a los ángulos siguiente manera:



de las articulaciones de la

 = 4  = 5  = 6

 Ahora para calcular la orientación que logre que los tres últimos DOF de libertad sigan planos tangentes, los datos que se darán como entrada a la cinemática inversa serán la posición por medio de xd, xy y xz de la misma forma que en el caso anterior (corte de manzana) y los ángulos de Roll, Pitch, Yaw se calcularan de manera tal que el vector a del efector final sea normal al plano tangente y el vector n sea paralelo al mismo. Dado que la trayectoria a seguir es la de corte de manzana, se tiene como trayectoria un círculo de radio variable, por tanto los ángulos de Roll, Pitch, Yaw para lograr la orientación tangente deseada se calculan como sigue:

 py 

    A tan( r 

 py r 

, 1

      A tan(

  px

 px r 

 py 2 r 2

, 1

 px 2 r 2

Parámetros de entrada xo=; Distancia del centro del circulo al origen en x yo=; Distancia del centro del circulo al origen en y zo=; Distancia del centro del circulo al origen en z ro=; Radio mayor Ciclo de cálculo i=1; k=10; for(x=0:1:k*360)

Numero de vueltas Ecuación de círculo con radio variable

beta=x*pi/180; Angulo para calcular el circulo R=ro*sin(beta/(2*k)); Radio variable de la espiral px=xo+R*cos(beta); Posición deseada en x py=yo+R*sin(beta); Posición deseada en y fi=0; Calculo ángulos de Euler teta=pi-atan2((px^2/R),sqrt(1-( px/R)^2)); Calculo ángulos de Euler si= atan2((py^2/R),sqrt(1-( py/R)^2)); Calculo ángulos de Euler pz=zo-2*sin(beta/(4*k)); Variación de la altura pxf=px-(d6*(sin(fi)*sin(si)+cos(fi)*cos(teta)*sin(si)) pyf=py-(d6*(-cos(fi)*sin(si)+sin(fi)* sin(teta)*sin(si)) pxf=pz-(d6*( cos(si)*cos(teta) Calculo de los Ángulos D=(pxf^2+pyf^2+pzf^2-a2^2-a3^2)/(2*a2*a3); q1=atan2(pyf,pxf); q3=atan2(sqrt(1-D^2),D); q2=atan2(pzf,sqrt(pxf^2+pyf^2))-atan2(a3*sin(q3),a2+a3*cos(q3)); u13=(sin(fi)*sin(si)+cos(fi)*cos(si)*sin(teta))*(cos(q1)*cos(q2)*cos(q3)-cos(q1) *sin(q2) *sin(q3)+cos(teta)*cos(si))*(-cos(q2)*sin(q3)-cos(q3)*sin(q2)+(cos(q2)*cos(q3)*sin(q1)sin(q1)*sin(q2)*sin(q3))*(-cos(fi)*sin(fi)+cos(fi)*sin(teta)*sin(fi)); u23=(-sin(q1)*(sin(fi)*sin(si)+cos(fi)*cos(si)*sin(teta))+ cos(q1)*(-cos(fi)*sin(fi)+cos(fi)*sin(teta)*sin(fi)); u31= cos(teta)*cos(si) *( cos(q1)*cos(q2)*sin(q3))+ cos(q1)*cos(q3)*sin(q2)) +cos(teta)*cos(fi)*(cos(q2)*sin(q1)*sin(q3)+cos(q3)*sin(q1)*sin(q2)* (-sin(teta))*(cos(q2)* cos(q3)- sin(q2)*sin(q3)); u32=(cos(q2)*sin(q1)*sin(q3)+ cos(q3)*sin(q1)*sin(q2))*(cos(fi)*cos(si)+ sin(teta)*sin(fi)*sin(si))+cos(fi)*sin(si)*(cos(q2)* cos(q3)sin(q2)*sin(q3))+( cos(q1)*cos(q2)*sin(q3)) +cos(q1)*cos(q3)*sin(q2))*(-cos(si)*sin(teta)+cos(fi)*sin(teta)*sin(si)); u33=((q1)*cos(q2)*sin(q3))+cos(q1)*cos(q3)*sin(q2))*(sin(fi)*sin(si)+ cos(fi)*cos(si)*sin(teta))+cos(teta)*cos(si)*(cos(q2)*cos(q3)- sin(q2)*sin(q3));

q4=atan2(u13,u23); q5=atan2(u33,sqrt(1-(u33^2)); q6=atan2(-u31,u32); Matriz de los vectores (q) q(i,:)=[q1,q2,q3]; X(i,:)=[px];  Y(i,:)=[py]; Z(i,:)=[pz];

Matriz de los vectores (px, py, pz)

i=i+1; end Rutina para graficar figure(1) grid plot3(X,Y,Z) Grafica de la trayectoria calculada figure(2) plot(ROBOTINA,q,'ROBOTINA','shadow','erase') Grafica del robot siguiendo la trayectoria

EJERCICIO 3 Encontrar la cinemática inversa de orientación de 3 DOF cuando en el último eje coordenado a6 es distinto de cero. d6

a3 a2

Z3

Z4

Z5

a6

 X1

 X6

 X2 Z2

Z1

 Y 1

 Y 2

Z6  Y 6

dd0066

d05

Z0

 Y 0  X0

Utilizando el mismo robot de tipo RRR de la tarea anterior, cuyos parámetros de Denavit-Hartenberg se cambian solo en la columna de las ai,  al agregar el valor de a6 como se observa a continuación:

n

ai  

di  

1 2 3 4 5 6

0 a2 a3 0 0 a6

d1 0 0 0 0 d6

i  

-90 0 90 -90 -90 0

θi   1 2 3 4 5 6

Para calcular la cinemática inversa se utilizo el método de desacople cinemático ya que los tres últimos ejes Z se interceptan en un punto. Por tanto: d 05



d 06

 dR K 

El vector d  indica la distancia del origen al centro de la muñeca, d  es el vector que indica la posición deseada de la herramienta, el calculo de la posición real se realiza teniendo en cuenta que la herramienta esta desplazada del centro de la muñeca y en esta caso la distancia d  se trasforma en un vector cuyos componentes son a6 0d 6 T  que premultiplica a la matriz de rotación de Roll, Pitch,  Yaw, RK  ). 5

6

0

0

6

De esta forma al desacoplar los tres primero ángulos de los tres últimos, los tres primeros ángulos se calculan de acuerdo a:  a6    d 05  d 06   R 0   d 6   px   xd   a6   py    yd  R  0         pz   zd  d 6 

Donde la matriz R es matriz de rotación de Roll, Pitch, Yaw: ccθ

-scψ+csθsψ

ssψ+csθcψ

scθ

ccψ+ssθsψ

-csψ+ssθcψ

-sθ

csψ

cθcψ

Como datos de entrada se tiene:

xd, posición deseada en x yd, posición deseada en x zd, posición deseada en x , ,  Ángulos de rotación deseados

 A parte del cambio generado en el vector de distancia de la herramienta a la muñeca el proceso para calcular los demás ángulos es el mismo del ejercicio anterior.

View more...

Comments

Copyright ©2017 KUPDF Inc.
SUPPORT KUPDF