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 la cinemática inversa se obtuvo mediante

el método geométrico, como sigue:

La proyección de la herramienta sobre el plano 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:

Realizando las diferentes operaciones trigonométricas con el fin de encontrar los

ángulos por medio de la función arcotangente se obtuvo que:

S 2

a3

a2

2

Z0

r

1

r

Y0

X0

Py

Px

1

Z2

Py

Y0

Sz

Px X1 X0

Z0

r

Z1

))(),cos((2tan),(2tan

))(),cos((2tan),(2tan

2

2

cos

)1,(2tan

),(2tan

33332

22

2

333322

32

2

3

2

2

222

32

2

3

2

2

22

3

2

3

1

senaaaApppA

senaaaAsrA

aa

aapppD

aa

aasrD

D

DDA

pypxA

zyx

zyx

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 θi 1 0 d1 -90 θ1

2 a2 0 0 θ2

3 a3 0 0 θ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.

Parámetros de entrada del Robot

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;

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

Parámetros de entrada del Círculo R Radio del círculo xo Distancia del centro del circulo al origen en x

yo Distancia del centro del circulo al origen en y pz Posición deseada en z

Ciclo de Cálculo i=1;

for(x=0:1:360) Calculo del Círculo

beta=x*pi/180; Angulo para calcular el circulo

px=xo+R*cos(beta); Posición deseada en x py=yo+R*sin(beta); 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));

Matriz de los vectores (q) q(i,:)=[q1,q2,q3];

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);

Matriz de los vectores (Dx, Dy, Dz) Dx(i,:)=[dx]; Dy(i,:)=[dy];

Dz(i,:)=[dz];

Matriz de los vectores (px, py, pz)

X(i,:)=[px]; Y(i,:)=[py]; Z(i,:)=[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 Radio del círculo xo Distancia del centro del circulo al origen en x yo Distancia del centro del circulo al origen en y

zo Altura desde el origen hasta el centro del circulo en z

alfa=30*pi/180; Angulo de inclinación del plano alrededor de Y a=R; Radio mayor de la elipse b=R*cos(alfa); Radio menor de la elipse

Ciclo de cálculo i=1;

phi=0; for(n=0:1:360)

beta=n*pi/180;

Ecuación Paramétrica de una elipse x = a*cos(beta); y = b*sin(beta);

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));

Matriz de los vectores (q) q(i,:)=[q1,q2,q3];

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);

Matriz de los vectores (Dx, Dy, Dz) Dx(i,:)=[dx]; Dy(i,:)=[dy];

Dz(i,:)=[dz];

Matriz de los vectores (px, py, pz)

X(i,:)=[px]; Y(i,:)=[py]; Z(i,:)=[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; Numero de vueltas for(x=0:1:k*360)

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

pz=zo-2*sin(beta/(4*k)); 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));

Matriz de los vectores (q) q(i,:)=[q1,q2,q3];

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);

Matriz de los vectores (Dx, Dy, Dz) Dx(i,:)=[dx]; Dy(i,:)=[dy];

Dz(i,:)=[dz]; Matriz de los vectores (px, py, pz) X(i,:)=[px];

Y(i,:)=[py]; Z(i,:)=[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

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 i θi 1 0 d1 -90 1

2 a2 0 0 2

3 a3 0 90 3

4 0 0 -90 4

5 0 0 -90 5

6 0 d6 0 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.

d05 Y1

Y6

Y2

Y0

X6

X2

X1

X0

Z0

Z2 Z1

Z6

d06

Z4 Z5

Z3 a2

a3 d6

d06

Por tanto:

KRddd 6

6

0

5

0

KRddpc 6

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 6d rotada según el

origen al premultiplicarla por la matriz de rotación de Roll, Pitch, Yaw, KR ).

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:

)c(c

)sss+s(-c

)csc+s(s

6

6

6

336

236

136

dzdpz

dydpy

dxdpx

rdzdpz

rdydpy

rdxdpx

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 1, 2 y 3 con el método

trigonométrico, como se hizo en la tarea anterior.

))(),cos((2tan),(2tan

))(),cos((2tan),(2tan

2

2

cos

)1,(2tan

),(2tan

33332

22

2

333322

32

2

3

2

2

222

32

2

3

2

2

22

3

2

3

1

senaaaApppA

senaaaAsrA

aa

aapppD

aa

aasrD

D

DDA

pypxA

zyx

zyx

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

URRRT

:3

0

6

3

c1c2c3 - c1s2s3 -s 1 c1c2s3 + c1c 3s2

c2c3s1 - s1s2s3 c 1 c2s1s3 + c3s1s2

- c2s3 - c3s2 0 c2c3 - s2s3

c1 0 -s1 c2 -s2 0 c3 0 s3

s1 0 c1 s2 C2 0 s3 0 - c3

0 -1 0 0 0 1 0 1 0

R03 =

R03 =

c1c2c3 - c1s2s3 -s1 c1c2s3 + c1c 3s2

c2c3s1 - s1s2s3 c1 c2s1s3 + c3s1s2

- c2s3 - c3s2 0 c2c3 - s2s3

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:

ccθ -scψ+csθsψ ssψ+csθcψ

scθ ccψ+ssθsψ -csψ+ssθcψ

-sθ csψ cθcψ

U11 U12 U13

U21 U22 U23

U31 U32 U33

R36 =

T

URRRT

:3

0

6

3

),tan(

),tan(

)1,tan(

3231

2313

2

3333

uuA

uuA

uuA

Los ángulos de Euler corresponden a los ángulos de las articulaciones de la

siguiente manera: = 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:

r

px

py

2

2

2

2

1,tan(

1,tan(

r

px

r

pxA

r

py

r

pyA

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; Numero de vueltas

for(x=0:1:k*360)

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];

Matriz de los vectores (px, py, pz)

X(i,:)=[px]; Y(i,:)=[py]; Z(i,:)=[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.

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 i θi 1 0 d1 -90 1

2 a2 0 0 2

3 a3 0 90 3

4 0 0 -90 4

5 0 0 -90 5

6 a6 d6 0 6

d05 Y1

Y6

Y2

Y0

X6 X2

X1

X0

Z0

Z2 Z1

Z6

d06

Z4 Z5

Z3 a2

a3 d6

d06

a6

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:

KdRdd 6

0

5

0

El vector 5

0d indica la distancia del origen al centro de la muñeca, 6

0d 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 6d se trasforma en un vector cuyos

componentes son Tda 660 que premultiplica a la matriz de rotación de Roll, Pitch,

Yaw, KR ).

De esta forma al desacoplar los tres primero ángulos de los tres últimos, los tres primeros ángulos se calculan de acuerdo a:

6

6

6

6

6

0

5

0

0

0

d

a

R

zd

yd

xd

pz

py

px

d

a

Rdd

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

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.

ccθ -scψ+csθsψ ssψ+csθcψ

scθ ccψ+ssθsψ -csψ+ssθcψ

-sθ csψ cθcψ