ejercicios resueltos cinematica inversa_matlab
TRANSCRIPT
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ψ