% Simulation for rotation matrix
% Coded by funMV. 12/2013
clear; clc;
% x축에 대해 45도 회전
% 회전 행렬은 표준 축이 회전된 방향 벡터(unit vector)이다.
alpa = 45; % x축 기준
beta = 30; % y축
gama = 60; % z축
p2a = pi/180;
% (1) 직접 3x3크기의 R행렬을 만듬
rr1 = rot(alpa*p2a, beta*p2a, gama*p2a);
% (2) rx, ry, rz로 만드나 결과는 동일
ssx = sin(alpa*p2a);
csx = cos(alpa*p2a);
ssy = sin(beta*p2a);
csy = cos(beta*p2a);
ssz = sin(gama*p2a);
csz = cos(gama*p2a);
rx = [1 0 0; 0 csx -ssx; 0 ssx csx];
%{
rx =
1.0000 0 0
0 0.7071 -0.7071
0 0.7071 0.7071
%}
% x축을 중심으로 회전 시키므로 x축 방향은 불변
% 회전 각의 방향은 오른손 나사 방향으로 45도 회전
% 기존 y축은 [0 1 0]에서 [0 0.7071 0.7071]로 변경
% 기존 z축은 [0 0 1]에서 [0 -0.7071 0.7071]로 변경
% 회전 행렬 rx는 회전 후의 세 축의 방향 벡터(unit three vectors)
ry = [csy 0 ssy; 0 1 0; -ssy 0 csy];
rz = [csz -ssz 0; ssz csz 0; 0 0 1];
rr2 = rz*ry*rx;
% R(3x3)행렬에서 다시 3개의 각도 벡터를 추출
v = rpy(rr2);
v/p2a % 45, 40, 60이 나옴
% 3개의 각도 벡터에서 quaternion을 계산
q = angle2quat(v(1),v(2),v(3)); % angle to quaternion
% norm(q) = 1
% quaternion에서 다시 3개의 각도를 추출
rpy(q2r(q))/p2a % 세개의 각도 값 출력 순서: 60,30,45도로 나옴)
% matlab내부 함수를 이용해 각도 값 추출
[r1,r2,r3] = quat2angle(q); % 45,30,60도 나옴
r1/p2a
r2/p2a
r3/p2a
% 두 연속 회전에 의한 각도 값 비교
% quaternion의 연속 적용과 R회전 행렬의 연속 적용 후
% 결과 값이 같은지를 비교한다.
% 새로운 3개의 각도 벡터 정의
dom = [2, 5, 7]*p2a;
rr3 = rot(dom(1),dom(2),dom(3)); % 회전 행렬 생성
q2 = angle2quat(dom(1), dom(2), dom(3)); % quaternion 계산
qp1 = quatmultiply(q, q2); % 두 quaternion의 곱
%qp2 = qprod(q,q2); % 두 quaternion의 곱
[r4,r5,r6] =quat2angle(qp1); % quaternion에서 각도 추출
r4/p2a
r5/p2a
r6/p2a % 51.1953, 30.6278, 70.0420
rr4 = rr3*rr2; % 하나의 강체에 두 회전의 연속 적용
v3 = rpy(rr4); % 즉, R행렬의 연속 적용이나 quaternion의 곱에 의한
v3/p2a % 회전의 연속 적용이나 결과는 동일함
function v_rot=rpy(m_rot)
% function v_rot=rpy(m_rot)
%
% Description
% Computes the orientation vector (in rads) corresponding
% to a rotation matrix. (dim 3)
% [psi, theta, phi]
% R -> (alpa, beta, gama)로 변환 함수
% v_rot1 = alpa(x), v_rot2=beta(y), v_rot3=gama(z)
% 에 대응되는 각도임
% The input datum is:
% - m_rot.- a rotation matrix
%
% The return value is:
% the rpy vector
v_rot(3)=angle(m_rot(2,1)*i+m_rot(1,1));
v_rot(2)=angle(-m_rot(3,1)*i+m_rot(1,1)*cos(v_rot(3))+m_rot(2,1)*sin(v_rot(3)));
v_rot(1)=angle(m_rot(3,2)*i+m_rot(3,3));
return;
function r=rot(psi,theta,phi);
% function r=rot(psi,theta,phi)
%
% Description
% Computes the rotarion matrix corresponding to a oritentation vector
% The input data are:
% - psi .- the psi angle (radians)
% - theta .- the theta angle (radians)
% - phi .- the phi angle (radians)
%
% The return value is
% the rotation matrix (dim 3x3)
% Coded by
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Programmer : Jose Maria Martinez
%% Languaje : Matlab 4.2 c
%% Date : February 1996
%% Status : Prueba
%% History : 16-8-95 creation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
cosphi=cos(phi);
sinphi=sin(phi);
costheta=cos(theta);
sintheta=sin(theta);
cospsi=cos(psi);
sinpsi=sin(psi);
r=zeros(3,3);
r(1,1)=cosphi*costheta;
r(1,2)=cosphi*sintheta*sinpsi-sinphi*cospsi;
r(1,3)=cosphi*sintheta*cospsi+sinphi*sinpsi;
r(2,1)=sinphi*costheta;
r(2,2)=sinphi*sintheta*sinpsi+cosphi*cospsi;
r(2,3)=sinphi*sintheta*cospsi-cosphi*sinpsi;
r(3,1)=-sintheta;
r(3,2)=costheta*sinpsi;
r(3,3)=costheta*cospsi;
return;
%-----------------------------------------------------------------------
% Authors: Javier Civera -- jcivera@unizar.es
% J. M. M. Montiel -- josemari@unizar.es
% Robotics, Perception and Real Time Group
% Institute of Engineering Research (I3A)
% Universidad de Zaragoza, 50018, Zaragoza, Spain
% Date : May 2010
%-----------------------------------------------------------------------
function R=q2r(q)
x=q(2);
y=q(3);
z=q(4);
r=q(1);
R=[r*r+x*x-y*y-z*z 2*(x*y -r*z) 2*(z*x+r*y)
2*(x*y+r*z) r*r-x*x+y*y-z*z 2*(y*z-r*x)
2*(z*x-r*y) 2*(y*z+r*x) r*r-x*x-y*y+z*z];
% Product of two quaternion
function qp=qprod(q,p)
a=q(1);
v=reshape(q(2:4),3,1);
x=p(1);
u=reshape(p(2:4),3,1);
qp = [a*x-v'*u, (a*u+x*v)' + cross(v,u)'];
References
[1] 오일러각과 회전행렬(Euler Angles and Rotation Matrix)
[2] 각속도(Angular Velocity)와 오일러각, 회전행렬 간의 관계
[3] 쿼터니언(Quaternion)과 오일러각, 회전행렬 간의 관계
[4] 회전 보간: 쿼터니언(Quaternion)을 사용한 Slerp(구면 선형 보간)
댓글 없음:
댓글 쓰기