informerobotica
DESCRIPTION
roboticaTRANSCRIPT
[Escriba texto]
PRACTICA Nº1 DE ROBOTICATRANSFORMACIONES ESPACIALES Y MATRIZ DE ROTACION
I. OBJETIVOS Visualizar en 3D sistemas de coordenadas con diferentes posiciones y orientaciones Comprobación matemática de la nueva posición de un punto al someterlo a una
transformación. Verificar las propiedades de la matriz de rotación Comprobar las transformaciones de la matriz de rotación a par de rotación, formula de
Rodríguez, ángulos de Euler y cuaternios.
II. PROCEDIMIENTO
1. Introducción
La base matemática de la robótica constituye la matemática del movimiento del cuerpo rígido. Aquí se estudia las diversas formas de representar la orientación y posición de un sistema en el espacio tridimensional. Básicamente la orientación y posición de un cuerpo rígido está determinado por la matriz de rotación y el vector de traslación, estos se pueden representar conjuntamente por medio de una matriz homogénea. Una matriz de rotación es una matriz ortogonal con propiedades particulares que la hacen especial, está a la vez posee diversas representaciones descritas en la literatura que van desde el par de rotación, la formula de Rodríguez, los ángulos de Euler y la representación por cuaternios. En la presente práctica se ejercitara con todos ellos, utilizando el soporte de MATLAB y su toolbox de robótica escrito por Peter Coke.
2. Representación de la Rotación y Traslación.
a) Usando la función “frame” (se adjunta), represente una rotación y traslación nula
(R=[1 0 0; 0 1 0; 0 0 1] y t=[0 0 0]T).>> TA=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];>> frame(TA,'c',1);>> axis([-0.5 1 -0.5 1 -0.5 1])>> grid on>> view(-33,16)
b) Representar una rotación de 90˚ en el eje Y.>> T=roty(pi/2)
1
-0.5
0
0.5
1 -0.5
00.5
1
-0.5
0
0.5
1
Y
Eje Y
X
Z
Eje X
Eje
Z
[Escriba texto]
>> frame (TA,'c',1);>> frame (T,'r',1);>> grid on>> view (-33,18)
c) Representar una rotación de -90˚ grados en el eje Z.>> T=rotz(-pi/2)
>> frame(TA,'c',1);>> frame(T,'r',1);>> grid on>> view(-25,52)
d) Representar una traslación de 0.5 en el eje X.>> T=transl(0.5,0,0)
2
-0.5
0
0.5
1-1 -0.5 0 0.5 1
0
0.2
0.4
0.6
0.8
1
Y
YX
ZZ
Eje YEje X
X
Eje
Z
-0.5
0
0.5
1 -0.50
0.51
-1
-0.5
0
0.5
1
YY
Eje Y
ZX
Z
X
Eje X
Eje
Z
-0.50
0.51
1.5
-0.5
0
0.5
10
0.2
0.4
0.6
0.8
1
XX
Eje X
Z
Y
Z
Y
Eje Y
Eje
Z
[Escriba texto]
>> frame(TA,'c',1);>> frame(T,'r',1);>> grid on>> view(-30,30)
e) Representar una traslación de 0.5 en el eje X, luego una rotación de 90 grados en el eje Y, y posteriormente una rotación de -90˚ en el eje Z (todos en el sistema resultante).
>> T=transl(0.5,0,0)*roty(pi/2)*rotz(-pi/2)
>> frame(TA,'c',1);>> frame(T,'r',1);>> grid on>> view(-53,22)
f) Representar una matriz homogénea que describa:1. Traslación de (2, 4, 0) y rotación en el eje X en 30 grados.1. Traslación de (-1,-3,2) y rotación en el eje Z en 60 grados.
>> close all>> TB=transl(2,4,0)*rotx(pi/6)>> TC=transl(-1,-3,2)*rotz(pi/3);>> frame(TA,'c',1);>> frame(TB,'b',1);>> frame(TC,'m',1);>> axis([-2 3 -2 3 -2 3])>> grid on>> view(-54,22)
3. Operaciones con transformaciones
a. ¿Cuáles son las nuevas coordenadas de P1(2,1,3) si este pertenece a un cuerpo rígido que ha sufrido una rotación de 90 grados en el eje Y?
La matriz de rotación en el eje Y es:R =[ cos(th), 0, sin(th)][ 0, 1, 0][ -sin(th), 0, cos(th)]
th=pi/2R = 0.0000 0 1.0000 0 1.0000 0 -1.0000 0 0.0000
P1=[2 1 3]'P1 = 2 1 3
Las nuevas coordenadas están representadas por P2P2=R*P1P2 = 3.0000
3
-0.50
0.51
1.5
-0.5
0
0.5
10
0.2
0.4
0.6
0.8
1
XX
Eje X
Z
Y
Z
Y
Eje Y
Eje
Z
-0.5 0 0.5 1 1.5
-1
-0.5
0
0.5
1-1
-0.5
0
0.5
1
ZX
X
Eje X
Y
Z
Y
Eje Y
Eje
Z
-2-1
01
23 -2
-1
0
1
2
3
-2
-1
0
1
2
3X
Y
Eje Y
Z
X
Y
Eje X
ZX
ZY
Eje
Z
[Escriba texto]
1.0000 -2.0000
b. ¿Cuáles son las nuevas coordenadas de P1 (2,1,3) si este ha sufrido una traslación de 2 en el eje X?La matriz de rotación es la matriz identidad.
H =[ 1, 0, 0, 2][ 0, cos(th), -sin(th), 0][ 0, sin(th), cos(th), 0][ 0, 0, 0, 1]
th=0H = 1 0 0 2 0 1 0 0 0 0 1 0 0 0 0 1
P1=[2 1 3 1]'P1 = 2 1 3 1
Las nuevas coordenadas están representadas por P2P2=H*P1 P2 = 4
131
c. ¿Cuáles son las nuevas coordenadas de P1(2,1,3) si este ha sufrido una traslación de 2 en eje X y luego una rotación de 90 grados en el eje Y?
H =[ cos(th), 0, sin(th), 2][ 0, 1, 0, 0][ -sin(th), 0, cos(th), 0][ 0, 0, 0, 1]
th=pi/2 H =0.0000 0 1.0000 2.0000 0 1.0000 0 0-1.0000 0 0.0000 0 0 0 0 1.0000
P1=[2 1 3 1]' P1 = 2 1 3 1
Las nuevas coordenadas están representadas por P2P2=H*P1P2 = 5.0000 1.0000 -2.0000 1.0000
d. ¿Cuáles son las nuevas coordenadas de P1(10,7,6) si este ha sufrido una traslación de (10,5,0), luego una rotación de -90 grados en el eje Y luego una rotación de 90 grados en el eje X? Usando el Toolbox de Robótica:
T1=transl(10,5,0)
T1 = 1 0 0 10 0 1 0 5 0 0 1 0 0 0 0 1
T2=roty (-pi/2)
T2 = 0. 0 -1 0 0 1. 0 0 1 0 0 0 0 0 0 1
T3=rotx (pi/2)
T3 = 1 0 0 0 0 0 1 0 0 1 0 0 0 0 0 1P1 = 10
4
[Escriba texto]
P1=[10 7 6 1]' 7 6 1
P2=H*P1
P2 = 3 -1 10 1
Propiedades de la matriz de Rotación.
a) Consecuencia del movimiento de un robot de 3gdl se consiguió la siguiente matriz de rotación:R =0.8091 -0.1010 0.5789-0.0010 0.9849 0.1732-0.5876 -0.1407 0.7968
1. Halle su determinante det(R)
ans = 1.0000
2. Verifique R−1=RTTranspuestaR'
ans = 0.8091 -0.0010 -0.5876 -0.1010 0.9849 -0.1407 0.5789 0.1732 0.7968
inv(R) ans = 0.8091 -0.0010 -0.5876 -0.1010 0.9849 -0.1407 0.5789 0.1732 0.7968
3. Si se considera sus filas como r1, r2 y r3, verificar:
r1 r1T=r2 r2
T=r3r3T=1
r1=R(1,:)r1 = 0.8091 -0.1010 0.5789r1'ans = 0.8091 -0.1010 0.5789r1*r1'ans = 1.0000
r1 r2T=r1r 3
T=r 2r3T=0 r2=R(2,:)
r2 = -0.0010 0.9849 0.1732r1*r2'ans = -4.0577e-007
5
[Escriba texto]
[r1 ] xr2=r 3
r3 = -0.5876 -0.1407 0.7968
r1x=[0 -r1(3) r1(2);r1(3) 0 -r1(1);-r1(2) r1(1) 0]r1x = 0 -0.5789 -0.1010 0.5789 0 -0.8091 0.1010 0.8091 0r3_=r1x*r2'r3_ = -0.5876 -0.1407 0.7968
4. Si se considera sus columnas como r1,r2,r3 verifique la expresión anterior.
r1=R(:,1) r1 = 0.8091 -0.0010 -0.5876 r1'*r1 ans = 1.0000 r2'*r3 ans = -8.1052e-007
Representación de la matriz de Rotación.
a) Usando el método simbólico de MATLAB verifique la fórmula para:1. Representación por ángulos de EULER ZYZ.2. Representación por ángulos de EULER RPY.syms phi th xhiRz=[cos(phi) -sin(phi) 0;sin(phi) cos(phi) 0;0 0 1]
Rz =[ cos(phi), -sin(phi), 0][ sin(phi), cos(phi), 0][ 0, 0, 1]
Ry=[cos(th) 0 sin(th);0 1 0;-sin(th) 0 cos(th)]
Ry = [ cos(th), 0, sin(th)] [ 0, 1, 0 ] [ -sin(th), 0, cos(th)]
Rx=[1 0 0;0 cos(xhi) -sin(xhi);0 sin(xhi) cos(xhi)]
Rx=[ 1, 0, 0][ 0, cos(xhi), -sin(xhi)][ 0, sin(xhi), cos(xhi)]
Rzz=[cos(xhi) -sin(xhi) 0;sin(xhi) cos(xhi) 0;0 0 1]
Rzz =[ cos(xhi), -sin(xhi), 0][ sin(xhi), cos(xhi), 0][ 0, 0, 1]
1. R1=Rz*Ry*Rzz (ZYZ) R1 = [cos(phi)*cos(th)*cos(xhi)-sin(phi)*sin(xhi), -cos(phi)*cos(th)*sin(xhi)-sin(phi)*cos(xhi), cos(phi)*sin(th)][sin(phi)*cos(th)*cos(xhi)+cos(phi)*sin(xhi),
6
[Escriba texto]
-sin(phi)*cos(th)*sin(xhi)+cos(phi)*cos(xhi), sin(phi)*sin(th)][-sin(th)*cos(xhi), sin(th)*sin(xhi),cos(th)]
2. R2=Rz*Ry*Rx (RPY) R2 = [cos(phi)*cos(th), -sin(phi)*cos(xhi)+cos(phi)*sin(th)*sin(xhi), sin(phi)*sin(xhi)+cos(phi)*sin(th)*cos(xhi)][sin(phi)*cos(th), cos(phi)*cos(xhi)+sin(phi)*sin(th)*sin(xhi), -cos(phi)*sin(xhi)+sin(phi)*sin(th)*cos(xhi)][-sin(th),cos(th)*sin(xhi),cos(th)*cos(xhi)]
b) Para la matriz de Rotación del apartado 4, halle los ángulos de Euler ZYZ y RPY
tr2eul(R)*180/pi (Euler) ans = 16.6573 37.1731 -13.4649
tr2rpy(R)*180/pi (RPY) ans = -0.0675 35.9880 -10.0135
c) Para la matriz de rotación del apartado 4 halle el ángulo y eje según las expresiones de la teoría y compruebe el resultado con la instrucción rotvec(v,th).
th=acos((trace(R)-1)/2)*180/pi (Grados) th = 37.3051
th=acos((trace(R)-1)/2) (Radianes)
K=(1/(2*sin(th)))*[R(3,2)-R(2,3) R(1,3)-R(3,1) R(2,1)-R(1,2)]'
th = 0.6511K = -0.2590 0.9624 0.0826
rotvec(K,th)
r = 0.7954 -0.0500 0.5832 0.0500 0.7954 0.1569 -0.5832 -0.1569 0.7954ans = 0.8091 -0.1010 0.5789 0 -0.0010 0.9849 0.1732 0 -0.5876 -0.1407 0.7968 0 0 0 0 1.0000
d) Suponer que R es generado por una rotación de 90 grados en Z, luego una rotación de 30° en
Y, luego una rotación de 60° en X (todos en el sistema fijo), hallar su ángulo y eje.
R1=rotx(pi/3)*roty(pi/6)*rotz(pi/2)
R1 = 0.0000 -0.8660 0.5000 0 0.5000 -0.4330 -0.7500 0 0.8660 0.2500 0.4330 0 0 0 0 1.0000
R1_=R1(1:3,1:3)R1_ = 0.0000 -0.8660 0.5000 0.5000 -0.4330 -0.7500 0.8660 0.2500 0.4330
th1=acos((trace(R1_)-1)/2) th1 =
7
[Escriba texto]
2.0944
K1=(1/(2*sin(th1)))*[R1_(3,2)-R1_(2,3) R1_(1,3)-R1_(3,1) R1_(2,1)-R1_(1,2)]'
K1 = 0.5774 -0.2113 0.7887
e) Para el ángulo y eje obtenido en c), halle R utilizando la fórmula de Rodríguez.
R=[0.809139 -0.101016 0.57887;-0.0009527 0.984886 0.173199;-0.587616 -0.140694 0.796814]
R = 0.8091 -0.1010 0.5789 -0.0010 0.9849 0.1732 -0.5876 -0.1407 0.7968
th=acos((trace(R)-1)/2) th = 0.6511
K=(1/(2*sin(th)))*[R(3,2)-R(2,3) R(1,3)-R(3,1) R(2,1)-R(1,2)]'
K = -0.2590 0.9624 0.0826
Kx=[0 -K(3) K(2);K(3) 0 -K(1);-K(2) K(1) 0]Kx = 0 -0.0826 0.9624 0.0826 0 0.2590 -0.9624 -0.2590 0
I=eye(3) I = 1 0 0 0 1 0 0 0 1
RO=I+sin(th)*Kx+(1-cos(th))*(Kx)^2RO = 0.8091 -0.1010 0.5789 -0.0010 0.9849 0.1732 -0.5876 -0.1407 0.7968
f) Para la R del apartado c) y d) obtenga la representación de R según la representación de Rodríguez.
R=[0.809139 -0.101016 0.57887;-0.0009527 0.984886 0.173199;-0.587616 -0.140694 0.796814]
R = 0.8091 -0.1010 0.5789 -0.0010 0.9849 0.1732 -0.5876 -0.1407 0.7968
th=acos((trace(R)-1)/2) th = 0.6511
K=(1/(2*sin(th)))*[R(3,2)-R(2,3) R(1,3)-R(3,1) R(2,1)-R(1,2)]'
K = -0.2590 0.9624 0.0826
r11=th*Kr11 = -0.1686 0.6266 0.0537
Representación por cuaternios
A. Para la matriz de rotación del apartado 5c) halle su representación en cuaternios usando las siguientes instrucciones: quaternion([s v1 v2 v3]), quaternion(v,th), quaternion(R).
v =
8
[Escriba texto]
v=sin(th/2)*K -0.0828 0.3078 0.0264
quaternion([cos(th/2) v(1) v(2) v(3)])ans =0.94748 <-0.082824, 0.30779, 0.026403>
quaternion(v,th)ans = 0.94748 <-0.082824, 0.30779, 0.026403>
quaternion(R)ans = 0.94748 <-0.082824, 0.30779, 0.026403>
B. El punto p1(1,1,0) es rotado alrededor del eje Y en 90°, hallar su nueva posición en el sistema de referencia según :
1.-Metodo convencional.2.-Usando Cuaternios.1.-Metodo convencional. P1=[1 1 0]'
P1 = 1 1 0
R2=roty(pi/2)R2 = 0.0000 0 1.0000 0 0 1.0000 0 0 -1.0000 0 0.0000 0 0 0 0 1.0000
R_=R2(1:3,1:3)R= 0.0000 0 1.0000 0 1.0000 0 -1.0000 0 0.0000
P2=R_*P1P2 = 0.0000 1.0000 -1.0000
2.-Usando Cuaternios. Q1=quaternion(R)
Q1 = 0.70711 <0, 0.70711, 0>
P1=quaternion([0 1 1 0]) P1 = 0 <1, 1, 0>
Q1_=inv(Q1) Q1_ = 0.70711 <0, -0.70711, 0>
P2_=Q1*P1*Q1_ P2_ = 0 <2.2204e-016, 1, -1>
9
[Escriba texto]
INFORMEa) Realizar una función (archivo m) que convierta de la matriz de Rotación R a representación ángulo y
eje.Código Fuente.% Debemos introducir una matriz de orden 3x3 a la cual llamaremos% matriz R (es la matriz de rotación) si la matriz es de orden 3x3% entonces debe:% 1. Dar el ángulo teta con la formula th=acos((trace(R)-1)/2)*180/pi….% el ángulo será provisto en grados sexagesimales.% % 2. Dar el eje mediante la siguiente formula th=acos((trace(R)-1)/2)% dado en(Radianes)% K=(1/(2*sin(th)))*[R(3,2)-R(2,3) R(1,3)-R(3,1) R(2,1)-R(1,2)]'% % Para verificar si el ángulo th y el valor de K son correctos % se debe ejecutar el comando….. rotvec(K,th), al ejecutar este% comando me debe votar una matriz idéntica a la matriz R.% % Nota : Si la matriz introducida no es de orden 3x3 me debe votar un% mensaje que diga q la matriz es incorrecta. function[th, k] = rotvec_() matrixStr = input('Ingresar una matriz 3x3: ', 's'); R = evalin('caller', matrixStr); if(all(size(R) == [3 3]) ~= 1) disp('Se requiere una matriz de 3x3'); return; end th = acos((trace(R)-1)./2)*180/pi; disp('Angulo en grados sexagesimales: th'); disp('Eje:K'); k = (1/(2*sin(th./180.*pi))).*([(R(3,2)-R(2,3)) (R(1,3)-R(3,1)) (R(2,1)-R(1,2))]'); % prueba r = rotvec(k, th./180.*pi); r = r(1:3, 1:3); if(sum(sum(R - r)) < 1e-4)%hay error de redondeo disp('Comprobamos con el comando rotvec:') disp('Los valores de K y th son validos ya que se obtiene la matriz R'); end
Resultados:
10
[Escriba texto]
b) Realizar una función (archivo .m ) que convierta de la representación de R por la formula de Rodríguez.
%primero definimos k y th para encontrar ro=matriz de rotacion origen%LA REPRESENTACION DE r ESTA DADO PO EL EJE k Y EL ANGULO thfunction [r]=rodriguez(th,k)th1=th*pi/180;%convertimos en radianesx = [0 -k(3) k(2);k(3) 0 -k(1);-k(2) k(1) 0]I=eye(3)disp('matriz de rotacion origen')ro = I+sin(th1)*x+(1-cos(th1))*(x)^2
APLICACION
11
[Escriba texto]
12