UNIVERSIDAD CATLICA DE SANTA MARA

FACULTAD DE CIENCIAS E INGENIERAS FSICAS Y FORMALES

ESCUELA PROFESIONAL DE INGENIERA MECNICA, MECNICA-
ELCTRICA Y MECATRNICA
ROBTICA I

PRCTICA #3:
HERRAMIENTAS DE ORIENTACIN ESPACIAL

Arequipa-Per
Abril-2017
 Herramientas de orientacin espacial

1. Objetivos
-Emplear las herramientas matemticas de orientacin espacial en la solucin de
problemas de localizacin de robots.
-Construir programas que permitan automatizar el uso de las herramientas de
orientacin espacial.
-Emplear los programas construidos en la solucin de problemas de orientacin y
localizacin espacial de robots.

2. Material y Equipo

-PC y MATLAB

3. Procedimiento

-Deducir la matriz de rotacin resultante de realizar las siguientes operaciones respecto de

dos sistemas de referencia (A) fijo y (B) mvil y representar en un diagrama dichas
operaciones.

ZA

ZB

XB
XA
YB YA

a) Rotar un ngulo de 90 respecto del eje ZA

ZA
cos 90 90 0
= [ 90 cos 90 0]
ZB 0 0 1
YB 0 1 0
= [1 0 0]
0 0 1

XB YA
XA
b) Rotar un ngulo de 45 respecto del eje XB

ZA
1 0 0
= [0 cos 45 45]
YB 0 45 cos 45
ZB
1 0 0
= [0 0.7071 0.7071]
0 0.7071 0.7071
XB
XA
YA

c) Rotar un ngulo de 30 respecto del eje ZB

ZA
cos 30 30 0
YB = [ 30 cos 30 0]
0 0 1
ZB
0.866 0.5 0
XB = [ 0.5 0.866 0]
0 0 1

XA

YA

0.3536 0.6124 0.7071

= [ 0.866 0.5 0 ]
0.3536 0.6124 0.7071
-Escriba programas (funciones) en MATLAB para calcular las matrices de rotacin bsicas, dados
como parmetros el eje de giro y el ngulo de rotacin.
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 rotacin bsicas, dados
como parmetros el eje de giro y el ngulo de rotacin, en forma simblica.
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 terico.
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 rotacin obtenida es idntica al del problema 1, al igual que
todas las matrices de cada rotacin. La razn por la cual se multiplica R1S primero es porque al
ser una rotacin respecto al eje fijo esta debe premultiplicar, las siguientes rotaciones se
posmultiplican ya que son rotaciones respecto al eje mvil por eso R2S es despus y luego R3S.

-Desarrollar un programa en MATLAB que permita realizar composicin de matrices de rotacin

y se adapte a la solucin de problemas de orientacin espacial empleando esta herramienta. El
programa proporcionar respuestas numricas y respuestas simblicas segn 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 rotacin 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 rotacin alrededor del eje k. Revisar la
posibilidad de emplear MATLAB en la solucin del problema.

Para poder resolver el problema planteado primero se escribe al vector k como un vector
unitario para esto es necesario obtener la raz cuadrada de la suma de cada uno de los elementos
del vector y elevarlos al cuadrado.

1 1 2
12 + 12 + 22 = 6 = ( + + )
6 6 6

= (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
(mvil) un ngulo phi y posteriormente rotar w un ngulo teta, luego deshacer el movimiento
de phi y de alfa.
Z

Primero debemos obtener los Y

valores de alfa (Plano XY)

0.4082 45

X 0.4082
Valor de phi (ngulo entre el vector y el eje z)

35.257
2 Y

0.8165
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)*v-
k(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.3863

% 0.6262 0.7559 -0.1910
% -0.1910 0.3863 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 rotacin 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 rotacin 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 XYZ cuando el usuario introduzca
la matriz de rotacin 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 orientacin espacial

con cuaternios y su lgebra, tanto en forma numrica como simblica.

Funcin 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 orientacin 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 rotacin de 90 sobre el eje k(3,-2,1). Rot(k,90).
Aplique sus funciones MATLAB para las operaciones de la solucin. Luego crear un script solucin
del problema. Contraste sus resultados con la respuesta analtica.

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 adems de resolver problemas de orientacin tambin pueden ser

utilizados para resolver problemas que contengan rotacin y traslacin, ambos
juntos.
Al momento de hacer la composicin de matrices de rotacin es importante darse
cuenta si dicha rotacin es respecto del eje fijo o del mvil. Si es respecto al fijo se
premultiplica de lo contrario solo se posmultiplica.
Los ngulos de Euler slo representan orientacin y a pesar de solo usar tres
nmeros estos no permiten la composicin de rotaciones.
La idea fundamental para encontrar la matriz de rotacin resultante de un par de
rotacin es hacer coincidir el eje con el vector respecto al cual se quiere rotar y
posteriormente deshacer dichas rotaciones, todo mediante la composicin de
matrices de rotacin, al ser estos respecto de los ejes mviles solo se posmultiplican.
El producto de cuaternios parece una operacin compleja pero es fcilmente
deducible si se emplea la ley de composicin interna de los cuaternios.
La prctica nos ayuda a juntarnos ms como compaeros y futuros ingenieros, por
esa razn creemos que es bueno seguir trabajando en grupo.