Ejercicios Resueltos Cinematica Inversa - Matlab

EJERCICIO 2.
Programe la cinemtica inversa de un robot manipulador de 3 DOF, tal que realice

las siguientes tareas:

El robot seleccionado es del tipo RRR y la cinemtica inversa se obtuvo mediante
el mtodo geomtrico, como sigue:

La proyeccin de la herramienta sobre el plano x-y, y sobre el plano r-s permite
calcular los ngulos u
1,
u
2
y u
3,
utilizando diferentes propiedades trigonomtricas,
en las figuras se observan las proyecciones:

Realizando las diferentes operaciones trigonomtricas con el fin de encontrar los
ngulos por medio de la funcin arcotangente se obtuvo que:
S u
2

a
3

a
2

u
2

Z
0

r
u
1

r
Y
0

X
0

P
y

P
x

u
1

Z
2

P
y

Y
0

S
z

P
x
X
1
X
0

Z
0

r
Z
1

)) ( ), cos (( 2 tan ) , ( 2 tan
)) ( ), cos (( 2 tan ) , ( 2 tan
2
2
cos
) 1 , ( 2 tan
) , ( 2 tan
3 3 3 3 2
2 2
2
3 3 3 3 2 2
3 2
2
3
2
2
2 2 2
3 2
2
3
2
2
2 2
3
2
3
1
u u u
u u u
u
u
u
sen a a a A p p p A
sen a a a A s r A
a a
a a p p p
D
a a
a a s r
D
D
D D A
py px A
z y x
z y x
+ + =
+ =
+ +
=
+
=
=
=
=

Los parmetro de Denavit-Hartenberg utilizados para el robot seleccionado tipo
RRR, fueron los calculados para la Tarea 1, a continuacin se presentan:

n a
i
d
i
o
i

i

1 0 d1 -90 1
2 a2 0 0 2
3 a3 0 0 3

Para realizar los programas de cinemtica inversa se utilizo el software MatLab y
su Toolbox de Robtica. En el Toolbox se realizo la definicin del robot para poder
comprobar mediante simulacin la cinemtica inversa.

Para definir un robot con sus diferentes parmetros se tiene el siguiente programa:

L1=link([alfa1 a1 q10 d1]); Define el eslabn 1

ROBOTINA=robot({L1 L2 L3}); Asocia a una variable la configuracin del robot
a. Un crculo sobre un plano a una distancia Z
T.

Parmetros 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;
Definicin del robot
ROBOTINA=robot({L1 L2 L3}); Asocia a una variable la configuracin del robot

Parmetros de entrada del Crculo
R Radio del crculo
xo Distancia del centro del circulo al origen en x
yo Distancia del centro del circulo al origen en y
pz Posicin deseada en z

Ciclo de Clculo
i=1;
for(x=0:1:360)
Calculo del Crculo
beta=x*pi/180; Angulo para calcular el circulo
px=xo+R*cos(beta); Posicin deseada en x
py=yo+R*sin(beta); Posicin 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];

Actualizacin datos del vector d (obtenido por la
cinemtica 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 o grados sobre el plano x-y.

Para calcular el circulo en un plano inclinado se utiliza el mismo programa, solo se
modifica los parmetros de entrada y el ciclo de calculo, como sigue:

Parmetros de entrada del crculo rotado
R Radio del crculo
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 inclinacin del plano alrededor de Y
a=R; Radio mayor de la elipse
b=R*cos(alfa); Radio menor de la elipse

Ciclo de clculo
i=1;
phi=0;

for(n=0:1:360)

beta=n*pi/180;

Ecuacin Paramtrica de una elipse
x = a*cos(beta);
y = b*sin(beta);

Transformacin de Coordenadas
px = cos(phi)*x - sin(phi)*y;
py = sin(phi)*x + cos(phi)*y;
px = px + xo; Posicin deseada en x
py = py + yo; Posicin deseada en y
pz = (zo-0.5*R*sin(sigma))+(R*sin(sigma))*(1-sin(beta)); Posicin deseada en z

D=(px^2+py^2+pz^2-a2^2-a3^2)/(2*a2*a3);
q1=atan2(py,px);

q(i,:)=[q1,q2,q3];

cinemtica directa)

Dx(i,:)=[dx];
Dy(i,:)=[dy];
Dz(i,:)=[dz];

X(i,:)=[px];
Y(i,:)=[py];
Z(i,:)=[pz];

i=i+1;
end

figure(1)
grid
figure(2)

c. Un corte de manzana.

Para calcular el circulo en un plano inclinado se utiliza el mismo programa, solo se
modifican los parmetros de entrada y el ciclo de calculo, como sigue:

Parmetros 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 clculo

i=1;
k=10; Numero de vueltas
for(x=0:1:k*360)

Ecuacin de crculo con radio variable

R=ro*sin(beta/(2*k)); Radio variable de la espiral
pz=zo-2*sin(beta/(4*k)); Variacin de la altura

D=(px^2+py^2+pz^2-a2^2-a3^2)/(2*a2*a3);
q1=atan2(py,px);

q(i,:)=[q1,q2,q3];

cinemtica directa)

Dx(i,:)=[dx];
Dy(i,:)=[dy];
Dz(i,:)=[dz];
X(i,:)=[px];
Y(i,:)=[py];
Z(i,:)=[pz];
i=i+1;
end
figure(1)
grid
figure(2)

d. Un corte de manzana pero que la orientacin de los ltimos 3 DOF sigan planos
tangentes.

Primero se calcula la cinemtica inversa de un robot de seis grados de libertad. En
este caso se utilizo un robot de tipo RRR, cuyos parmetros de Denavit-Hartenberg
se observan a continuacin:

n a
i
d
i
o
i

i

1 0 d
1
-90 u
1

2 a
2
0 0 u
2

3 a
3
0 90 u
3

4 0 0 -90 u
4

5 0 0 -90 u
5

6 0 d
6
0 u
6

Para calcular la cinemtica inversa se utilizo el mtodo de desacople cinemtico ya
que los tres ltimos ejes Z se interceptan en un punto.

d
0
5

Y
1

Y
6

Y
2

Y
0

X
6

X
2

X
1

X
0

Z
0

Z
2

Z
1

Z
6

d
0
6

Z
4
Z
5

Z
3
a
2

a
3

d
6

d
0
6

Por tanto:
K
R d d d
6
6
0
5
0
=
K
R d d pc
6
=

Se calcula el vector al centro de la mueca ( 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
6
d rotada segn el
origen al premultiplicarla por la matriz de rotacin de Roll, Pitch, Yaw,
K
R ).

De esta forma se desacoplan los tres primeros ngulos de los tres ltimos. As los
tres primeros ngulos se calculan utilizando como vector de posicin, el que se
calcula de acuerdo a:

) c (c
) s s s + s (-c
) c s c + s (s
6
6
6
33 6
23 6
13 6
u
u | |
u | |
d zd pz
d yd py
d xd px
r d zd pz
r d yd py
r d xd px
=
=
=
=
=
=

En general como datos de entrada se tienen:

xd, Posicin deseada en x
yd, Posicin deseada en y
zd, Posicin deseada en z
|, ngulo de rotacin deseado sobre el eje z
u, ngulo de rotacin deseado sobre el eje y
ngulo de rotacin deseado sobre el eje z

Con estos datos se calculan entonces los ngulos u
1,
u
2
y

u
3
con el mtodo
trigonomtrico, como se hizo en la tarea anterior.
)) ( ), cos (( 2 tan ) , ( 2 tan
)) ( ), cos (( 2 tan ) , ( 2 tan
2
2
cos
) 1 , ( 2 tan
) , ( 2 tan
3 3 3 3 2
2 2
2
3 3 3 3 2 2
3 2
2
3
2
2
2 2 2
3 2
2
3
2
2
2 2
3
2
3
1
u u u
u u u
u
u
u
sen a a a A p p p A
sen a a a A s r A
a a
a a p p p
D
a a
a a s r
D
D
D D A
py px A
z y x
z y x
+ + =
+ =
+ +
=
+
=
=
=
=

La cinemtica inversa de los tres ltimos ngulos se calcula de acuerdo a:

( ) U R R R
T
= = :
3
0
6
3

c
u1
c
u2
c
u3
- c
u1
s
u2
s
u3
-s
u1
c
u1
c
u2
s
u3
+ c
u1
c
u3
s
u2

c
u2
c
u3
s
u1
- s
u1
s
u2
s
u3

c
u1
c
u2
s
u1
s
u3
+ c
u3
s
u1
s
u2

- c
u2
s
u3
- c
u3
s
u2
0 c
u2
c
u3
- s
u2
s
u3

c
u1
0 -s
u1
c
u2
-s
u2
0 c
u3
0 s
u3

s
u1
0 c
u1
s
u2
C
u2
0 s
u3
0 - c
u3

0 -1 0 0 0 1 0 1 0
R
0
3
=
R
0
3
=
c
u1
c
u2
c
u3
- c
u1
s
u2
s
u3
-s
u1

c
u1
c
u2
s
u3
+ c
u1
c
u3
s
u2

c
u2
c
u3
s
u1
- s
u1
s
u2
s
u3

c
u1
c
u2
s
u1
s
u3
+ c
u3
s
u1
s
u2

- c
u2
s
u3
- c
u3
s
u2
0 c
u2
c
u3
- s
u2
s
u3

U
11
= c
c
|
(c
1
c
2
c
3
-c
1
s
2
s
3
)+c
(c
2
c
3
s
1
-s
1
s
2
s
3
)-s
(-c
2
s
3
-c
3
s
2
)

U
12
= (c
+s
)(c
2
c
3
s
1
-s
1
s
2
s
3
)+c
(-c
2
s
3
-c
3
s
2
)+
(c
1
c
2
c
3
-c
1
s
2
s
3
)(-c
+c
)

U
13
= (s
+c
)(c
1
c
2
c
3
-c
1
s
2
s
3
)+c
(-c
2
s
3
-c
3
s
2
)+
(c
2
c
3
s
1
-s
1
s
2
s
3
)(-c
+c
)

U
21
= c
1
c
- s
1
c

U
22
= c
1
(c
+s
)-s
1
(-c
+c
)

U
23
= -s
1
(s
+c
)+c
1
(-c
+c
)

U
31
= c
(c
1
c
2
s
3
+c
1
c
3
s
2
)+c
(c
2
s
1
s
3
+c
3
s
1
s
2
)-s
(c
2
c
3
-s
2
s
3
)

U
32
= (c
2
s
1
s
3
+c
3
s
1
s
2
)(c
+s
)+c
(c
2
c
3
-s
2
s
3
)+
(c
1
c
2
s
3
+c
1
c
3
s
2
)(-c
+c
)

U
33
= (c
1
c
2
s
3
+c
1
c
3
s
2
)(s
+c
)+c
(c
2
c
3
-s
2
s
3
)+
(c
2
s
1
s
3
+c
3
s
1
s
2
)(-c
+c
)

Ahora para calcular los ngulos u
4
, u
5
y u
6
se calculan los ngulos de Euler |, u y
a partir de la matriz U como se muestra a continuacin:

c
|
c
-s
|
c
+c
|
s
s
|
s
+c
|
s

s
|
c
c
|
c
+s
|
s
-c
|
s
+s
|
s

-s
c
|
s

U
11
U
12
U
13

U
21
U
22
U
23

U
31
U
32
U
33

R
3
6
=
T
( ) = = = U R R R
T
:
3
0
6
3

) , tan(
) , tan(
) 1 , tan(
32 31
23 13
2
33 33
u u A
u u A
u u A
=
=
=
|
u

Los ngulos de Euler corresponden a los ngulos u de las articulaciones de la
siguiente manera:

| = u
4

u = u
5

= u
6

Ahora para calcular la orientacin que logre que los tres ltimos DOF de libertad
sigan planos tangentes, los datos que se darn como entrada a la cinemtica
inversa sern la posicin 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 crculo de radio variable, por tanto los ngulos de Roll, Pitch, Yaw
para lograr la orientacin tangente deseada se calculan como sigue:

u
r
px
u
py
2
2
2
2
1 , tan(
1 , tan(
r
px
r
px
A
r
py
r
py
A
H =
=
u

Parmetros 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 clculo

i=1;
k=10; Numero de vueltas
for(x=0:1:k*360)

Ecuacin de crculo con radio variable

R=ro*sin(beta/(2*k)); Radio variable de la espiral
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)); Variacin 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)

D=(pxf^2+pyf^2+pzf^2-a2^2-a3^2)/(2*a2*a3);
q1=atan2(pyf,pxf);
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);
q(i,:)=[q1,q2,q3];

X(i,:)=[px];
Y(i,:)=[py];
Z(i,:)=[pz];

i=i+1;
end

figure(1)
grid
figure(2)

EJERCICIO 3

Encontrar la cinemtica inversa de orientacin de 3 DOF cuando en el ltimo eje
coordenado a
6
es distinto de cero.

Utilizando el mismo robot de tipo RRR de la tarea anterior, cuyos parmetros de
Denavit-Hartenberg se cambian solo en la columna de las a
i,
al agregar el valor de
a
6
como se observa a continuacin:

n a
i
d
i
o
i

i

1 0 d
1
-90 u
1

2 a
2
0 0 u
2

3 a
3
0 90 u
3

4 0 0 -90 u
4

5 0 0 -90 u
5

6 a
6
d
6
0 u
6

d
0
5

Y
1

Y
6

Y
2

Y
0

X
6

X
2

X
1

X
0

Z
0

Z
2

Z
1

Z
6

d
0
6

Z
4
Z
5

Z
3
a
2

a
3

d
6

d
0
6

a
6

Para calcular la cinemtica inversa se utilizo el mtodo de desacople cinemtico ya
que los tres ltimos ejes Z se interceptan en un punto.

Por tanto:
K
dR d d =
6
0
5
0

El vector
5
0
d indica la distancia del origen al centro de la mueca,
6
0
d es el vector
que indica la posicin deseada de la herramienta, el calculo de la posicin real se
realiza teniendo en cuenta que la herramienta esta desplazada del centro de la
mueca y en esta caso la distancia
6
d se trasforma en un vector cuyos
componentes son | |
T
d a
6 6
0 que premultiplica a la matriz de rotacin de Roll, Pitch,
Yaw,
K
R ).

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
R d d

Donde la matriz R es matriz de rotacin de Roll, Pitch, Yaw:

Como datos de entrada se tiene: xd, posicin deseada en x
yd, posicin deseada en x
zd, posicin deseada en x
|, u, ngulos de rotacin deseados

A parte del cambio generado en el vector de distancia de la herramienta a la
mueca el proceso para calcular los dems ngulos es el mismo del ejercicio
anterior.
c
|
c
-s
|
c
+c
|
s
s
|
s
+c
|
s

s
|
c
c
|
c
+s
|
s
-c
|
s
+s
|
s

-s
c
|
s

Ejercicios Resueltos Cinematica Inversa - Matlab

Încărcat de

Informații document

Descriere originală:

Titlu original

Drepturi de autor

Formate disponibile

Partajați acest document

Partajați sau inserați document

Opțiuni de partajare

Vi se pare util acest document?

Este necorespunzător acest conținut?

Drepturi de autor:

Formate disponibile

Ejercicios Resueltos Cinematica Inversa - Matlab

Încărcat de

Drepturi de autor:

Formate disponibile

EJERCICIO 2.

Programe la cinemtica inversa de un robot manipulador de 3 DOF, tal que realice

S-ar putea să vă placă și