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 2
r P y
a2
1 2

 X0
P x 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 )

2
 3   A tan 2( D, 1  D )

 D  cos 3
r 2  s 2  a22 2
 a3
 D 
2a 2 a3
 p x2 2 2
  p y   p z   a 2  a3
2 2

 D 
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   p y

2
,  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   θ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.

d6
a3
a2 Z3

Z6
Z4 Z5
 X6
 Y 6
 X1
 X2
Z1 Z2
 Y 2
dd0066
 Y 1 d05

Z0

 Y 0
 X0
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 6

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

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 1, 2 y 3 con el método
trigonométrico, como se hizo en la tarea anterior.
  1   A tan 2( px,  py )

2
 3   A tan 2( D, 1  D )

 D  cos 3
r 2  s 2  a22 2
 a3
 D 
2a 2 a3
 p x2 2 2
  p y   p z   a 2  a3
2 2

 D 
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   p y

2
,  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:

 R3
6
  R   R :
3 T 
0
 U 

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

R 03 = 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1c2c3 - c1s2s3 -s 1 c1c2s3 + c1c 3s2

R 03 = 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2s3 + c1c 3s2 T
c1c2c3 - c1s2s3 -s1 ccθ -scψ+csθsψ ssψ+csθcψ

R 36 = c2c3s1 - s1s2s3

c1 c2s1s3 + c3s1s2 scθ ccψ+ssθsψ -csψ+ssθcψ

- c2s3 - c3s2 0 c2c3 - s2s3 -sθ csψ cθcψ

U11 U12 U13

 R3
6
  R   R : 3 T 
0
 U  
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  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:

 py  py 2
 py     A tan( , 1
r  r 2
r 
  px  px 2
      A tan( , 1
 r  r 2
 px
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.

d6
a3
a2 Z3

Z4 Z5
a6

 X1 Z6
 X6
 X2  Y 6
Z1 Z2
 Y 2
dd0066
 Y 1 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   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
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

5
0
6
0

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
6

componentes son a6 0d 6 T  que premultiplica a la matriz de rotación de Roll, Pitch,
 Yaw, RK  ).

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.