UNIVERSIDAD CATÓLICA DE SANTA MARÍA FACULTAD DE CIENCIAS E INGENIERÍAS FÍSICAS Y FORMALES ESCUELA PROFESIONAL DE INGENIERÍA MECÁNICA, MECÁNICAELÉCTRICA Y MECATRÓNICA ROBÓTICA I
PRÁCTICA #3: HERRAMIENTAS DE ORIENTACIÓN ESPACIAL
“
”
Arequipa-Perú Abril-2017
Herramientas de orientación espacial 1. Objetivos -Emplear las herramientas matemáticas de orientación espacial en la solución de problemas de localización de robots. -Construir programas que permitan automatizar el uso de las herramientas de orientación espacial. -Emplear los programas construidos en la solución de problemas de orientación y localización espacial de robots. 2. Material y Equipo -PC y MATLAB 3. Procedimiento -Deducir la matriz de rotación resultante de realizar las siguientes operaciones respecto de dos sistemas de referencia (A) fijo y (B) móvil y representar en un diagrama dichas operaciones.
Z A Z B
X B
X A
Y B
Y A
a) Rotar un ángulo de 90° respecto del eje Z A
Z A
Z B
X A
Y B
X B
Y A
cos90 =[090 0 =[10
− 90 0 ] cos90 0 0 1 −10 00] 0 1
b) Rotar un ángulo de 45° respecto del eje X B
Z A
1 =[00 1 =[00
Y B
Z B
X B
X A
0 cos45 45 0 0.0.77071 071
0 ] − 45 cos45 0 −0.0.77071071]
Y A
c) Rotar un ángulo de 30° respecto del eje Z B
Z A Y B Z B X B
cos30 − 30 0 =[030 cos30 ] 0 0 1 0. 8 66 −0. 5 0 =[ 0.05 0.8066 10]
X A Y A
∗ ∗ −0. 3 536 −0. 6 124 0. 7 071 =[ 0.0.3853666 0.−0.61245 0.70071]
-Escriba programas (funciones) en MATLAB para calcular las matrices de rotación básicas, dados como parámetros el eje de giro y el ángulo de rotación. function [R]=MR(eje,alfa) %ingresar el primer parametro entre apostofes como: 'x',60 if eje=='x' R=[1 0 0; 0 cosd(alfa) -sind(alfa); 0 sind(alfa) cosd(alfa)]; end if eje =='y' R=[cosd(alfa) 0 sind(alfa); 0 1 0; -sind(alfa) 0 cosd(alfa)]; end if eje =='z' R=[cosd(alfa) -sind(alfa) 0; sind(alfa) cosd(alfa) 0; 0 0 1]; end end
-Escriba programas (funciones) en MATLAB para calcular las matrices de rotación básicas, dados como parámetros el eje de giro y el ángulo de rotación, en forma simbólica. function [R]=simbolica(eje,alfa) %ingresar el primer parametro entre apostofes como: 'x',60 alfa=sym(alfa);%Convierte a variable simbolica if eje=='x' R=[1 0 0;0 cos(alfa) -sin(alfa); 0 sin(alfa) cos(alfa)]; end if eje =='y' R=[cos(alfa) 0 sin(alfa);0 1 0; -sin(alfa) 0 cos(alfa)]; end if eje =='z' R=[cos(alfa) -sin(alfa) 0; sin(alfa) cos(alfa) 0;0 0 1]; end end
-Soluciones el problema 1 empleando las funciones implementadas en los dos ítems anteriores. Contrastar los resultados obtenidos con el resultado teórico. clear all, clc, close all; R1S=simbolica('z',90) R1=MR('z',90); R2S=simbolica('x',45) R2=MR('x',45); R3S=simbolica('z',30) R3=MR('z',30); RR=R1*R2*R3
Como podemos ver la matriz de rotación obtenida es idéntica al del problema 1, al igual que todas las matrices de cada rotación. La razón por la cual se multiplica R1S primero es porque al ser una rotación respecto al eje fijo esta debe premultiplicar, las siguientes rotaciones se posmultiplican ya que son rotaciones respecto al eje móvil por eso R2S es después y luego R3S.
-Desarrollar un programa en MATLAB que permita realizar composición de matrices de rotación y se adapte a la solución de problemas de orientación espacial empleando esta herramienta. El programa proporcionará respuestas numéricas y respuestas simbólicas según sea el problema. clear all, clc, close all; N= input ('Ingrese el numero de rotaciones a realizar: '); i=1; RR=1; RR_S=1; while i<=N a= input('EJE DE ROTACION (MINUSCULAS): ', 's');%INGRESA CHAR a_1 =input('ANGULO DE ROTACION: ');%INGRESA ANGULO DE LA ROTACION 1 A=MR(a,a_1);%SE CALCULA LA MATRIZ DE LA ROTACION 1 R1=input('Ingrese F si es sobre el eje fijo o M caso contrario (MAYUSCUAS): ','s'); switch R1 case 'F' RR=A*RR; RR_S=simbolica(a,a_1)*RR_S; case 'M' RR=RR*A; RR_S=RR_S*simbolica(a,a_1); end i=i+1; end disp('Resultado Numerico') RR disp('Resultado Simbolico') RR_S
-Deducir la matriz que representa la rotación de un ángulo de 45º alrededor de un eje dado por el vector k=[1,1,2]T . Obtener los ángulos roll, pitch y yaw alrededor de los ejes principales del S.R. que orientan el S.R. de la misma forma que esta rotación alrededor del eje k. Revisar la posibilidad de emplear MATLAB en la solución del problema.
Para poder resolver el problema planteado primero se escribe al vector k como un vector unitario para esto es necesario obtener la raíz cuadrada de la suma de cada uno de los elementos del vector y elevarlos al cuadrado.
1 +1 +2 =√ 6 →=(√ 16 + √ 16 + √ 26) =0.4082,0.4082,0.8165 Rotar respecto a este vector es equivalente a rotar primero el eje z un ángulo alfa, luego rotar v (móvil) un ángulo phi y posteriormente rotar w un ángulo teta, luego deshacer el movimiento de phi y de alfa. Z
Primero debemos obtener los valores de alfa (Plano XY)
Y 0.4082
X
45°
0.4082
Valor de phi (Ángulo entre el vector y el eje z)
Z
35.257 0 . 8 1 6 5
Y
X
,=∝∗∗∗−∗−∝ ∝=45° =35.2572° =45° Empleando MATLAB se tiene: clc, clear all t=[1 1 2]; a=sqrt(t(1)^2+t(2)^2+t(3)^2); k=[t(1)/a t(2)/a t(3)/a]; %si k(1)^2+k(2)^2+k(3)^2=1 (VECTOR UNITARIO) alfa=atand(k(2)/k(1)); phi=atand((sqrt(k(1)^2+k(2)^2))/(k(3))); R=MR('z',alfa)*MR('y',phi)*MR('z',45)*MR('y',-phi)*MR('z',-alfa); %v=1-cosd(alfa); %R=[k(1)*k(1)*v+cosd(alfa) k(1)*k(2)*v-k(3)*sind(alfa) k(1)*k(3)*v+k(2)*sind(alfa); % k(1)*k(2)*v+k(3)*sind(alfa) k(2)*k(2)*v+cosd(alfa) k(2)*k(3)*vk(1)*sind(alfa); %k(1)*k(3)*v-k(2)*sind(alfa) k(2)*k(3)*v+k(1)*sind(alfa) k(3)*k(3)*v+cosd(alfa)]; %0.7559 -0.5285 % 0.6262 0.7559 % -0.1910 0.3863
0.3863 -0.1910 0.9024
disp('MATRIZ DE ROTACION SOBRE UN EJE DEFINIDO POR EL VECTOR K'); disp(' '); disp(k); disp(' '); disp(R);
De forma general: v=1-cosd(alfa); R=[k(1)*k(1)*v+cosd(alfa) k(1)*k(2)*v-k(3)*sind(alfa) k(1)*k(3)*v+k(2)*sind(alfa); %k(1)*k(2)*v+k(3)*sind(alfa) k(2)*k(2)*v+cosd(alfa) k(2)*k(3)*v(1)*sind(alfa); k(1)*k(3)*v-k(2)*sind(alfa) k(2)*k(3)*v+k(1)*sind(alfa) (3)*k(3)*v+cosd(alfa)];
Teniendo la matriz de rotación se puede emplear un algoritmo creado para calcular los ángulos de Euler XYZ a partir de una matriz.
El algoritmo empleado será explicado en los siguientes ejercicios. -Escriba un programa en MATLAB para calcular la matriz de rotación R cuando el usuario introduzca los ángulos de Euler XYZ. clear all, clc, close all; X=input('Introduzca el angulo con respecto a X (YAW): '); Y=input('Introduzca el angulo con respecto a Y (PITCH): '); Z=input('Introduzca el angulo con respecto a Z (ROLL): '); RX=MR('x',X); RX_S=simbolica('x',X); RY=MR('y',Y); RY_S=simbolica('y',Y); RZ=MR('z',Z); RZ_S=simbolica('z',Z); RR=RZ*RY*RX RR_s=RZ_S*RY_S*RX_S
-Escriba un programa MATLAB que calcule los ángulos de Euler X YZ cuando el usuario introduzca la matriz de rotación R. clear all clc for i=1:3 for j=1:3 R(i,j)=input('DATOS: '); end end y=asind(-R(3,1)); x=asind((R(3,2))/(cosd(y))); z=acosd((R(1,1))/(cosd(y))); disp(' '); fprintf('X (YAW): %f \n',x); fprintf('Y (PITCH): %f \n',y); fprintf('Z (ROLL): %f \n',z);
-Escriba programas (funciones) en MATLAB para solucionar problemas de orientación espacial con cuaternios y su álgebra, tanto en forma numérica como simbólica.
Función que permite sumar, multiplicar e incluso multiplicar por un escalar: function [Q] = cuat(q1,q2,modo) switch(modo) case 'suma' for i=1:4 Q(i)=q1(i)+q2(i); end case'producto' Q=[q1(1)*q2(1)-q1(2)*q2(2)-q1(3)*q2(3)-q1(4)*q2(4) q1(1)*q2(2)+q1(2)*q2(1)+q1(3)*q2(4)-q1(4)*q2(3) q1(1)*q2(3)+q1(3)*q2(1)+q1(4)*q2(2)-q1(2)*q2(4) q1(1)*q2(4)+q1(4)*q2(1)+q1(2)*q2(3)-q1(3)*q2(2)]; case'escalar' a=input('ESCALAR: '); Q=a*q1; Q1=a*q2; disp(Q1);
% end %disp(Q); end
Programa para solucionar problemas de orientación espacial con cuaternios ang= input('Ingrese el angulo que se rotara: '); a=sqrt(t(1)^2+t(2)^2+t(3)^2); k=[t(1)/a t(2)/a t(3)/a]; %si k(1)^2+k(2)^2+k(3)^2=1 (VECTOR UNITARIO)
Q = [cosd(ang/2) k*sind(ang/2)]; Q_C=[cosd(ang/2) -k*sind(ang/2)]; k Q R=cuat(Q,[0 v],'producto'); R=cuat(R,Q_C,'producto') %realizas la formula Q*(0,r)*q*; ang=sym(ang); Q = [cos(ang/2) k*sin(ang/2)]; Q_C=[cos(ang/2) -k*sin(ang/2)] Rs=cuat(Q,[0 v],'producto'); Rs=cuat(Rs,Q_C,'producto') %realizas la formula Q*(0,r)*q*;
-Obtener el cuaternio que representa una rotación de 90° sobre el eje k(3,-2,1). Rot(k,90°). Aplique sus funciones MATLAB para las operaciones de la solución. Luego crear un script solución del problema. Contraste sus resultados con la respuesta analítica.
Empleando los programas anteriores se obtiene:
clear all, clc, close all; t=input('Ingrese el vector sobre el cual se realizara la rotacion(EJM: [x y z] ): '); ang= input('Ingrese el angulo que se rotara: '); a=sqrt(t(1)^2+t(2)^2+t(3)^2); k=[t(1)/a t(2)/a t(3)/a]; %si k(1)^2+k(2)^2+k(3)^2=1 (VECTOR UNITARIO) Q = [cosd(ang/2) k*sind(ang/2)] Q_C=[cosd(ang/2) -k*sind(ang/2)];
4. Conclusiones y Observaciones
Los cuaternios además de resolver problemas de orientación también pueden ser utilizados para resolver problemas que contengan rotación y traslación, ambos juntos. Al momento de hacer la composición de matrices de rotación es importante darse cuenta si dicha rotación es respecto del eje fijo o del móvil. Si es respecto al fijo se premultiplica de lo contrario solo se posmultiplica. Los ángulos de Euler sólo representan orientación y a pesar de solo usar tres números estos no permiten la composición de rotaciones. La idea fundamental para encontrar la matriz de rotación resultante de un par de rotación es hacer coincidir el eje con el vector respecto al cual se quiere rotar y posteriormente deshacer dichas rotaciones, todo mediante la composición de matrices de rotación, al ser estos respecto de los ejes móviles solo se posmultiplican. El producto de cuaternios parece una operación compleja pero es fácilmente deducible si se emplea la ley de composición interna de los cuaternios. La práctica nos ayuda a juntarnos más como compañeros y futuros ingenieros, por esa razón creemos que es bueno seguir trabajando en grupo.
