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
U
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
c1
R 03 =
R R : 3 T
6
R3
c1c2c3 - c1s2s3
-s 1
c1c2s3 + c1c 3s2
c2c3s1 - s1s2s3
c 1
c2s1s3 + c3s1s2
-
c2s3 - c3s2
0
c2c3 - s2s3
c1c2c3 - c1s2s3
R 36 =
c2c3s1 - s1s2s3
-
-s1 c1
R R :
0
U
T ccθ
-scψ+csθsψ
ssψ+csθcψ
c2s1s3 + c3s1s2
scθ
ccψ+ssθsψ
-csψ+ssθcψ
c2c3 - s2s3
-sθ
csψ
cθcψ
0
c2s3 - c3s2
3 T
6
R3
c1c2s3 + c1c 3s2
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: ccθ
-scψ+csθsψ
ssψ+csθcψ
scθ
ccψ+ssθsψ
-csψ+ssθcψ
-sθ
csψ
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.
