What is Rotation Matrix 1 : Rotation Sequence

 앞선 포스팅에"DCM 2"에서는 기준 좌표계에서 상대 좌표계로의 회전 관계를 3가지로 나누어 단순화 하고 이를 수식화 해보았다. 그리고 Rotation Matrix는 DCM과 같이 많이 사용되는 회전 행렬이며 DCM과 큰 차이가 없어 혼용되고 있는데 정확히는 DCM 행렬의 Transpose 관계이다. 그러므로 이전에 계산했던 DCM 행렬들의 Transpose가 곧 Rotation Matrix이다.

Rotation Matrix도 마찬가지로 Yaw, Pitch 그리고 Roll을 사용하는데 단순히 원하는 각도를 무작위로 넣어서 곱해주는 걸까? 물론 운이좋으면 원하는 바와 맞는 결과를 얻을 수 있겠지만 이를 규칙있게 잘 표현하기위해 Rotation Sequence라는 것이 존재한다. 다음에 정리된 표는 각 Rotation Sequence에 의한 결과를 정리한 표이다. 이는 Proper Euler Angles 그리고 Tait-Bryan Angles로 나뉜다(둘의 차이는 당장에 중요하지 않아서 나중에 다룰 예정임).

Table 1. Rotation Sequence

이렇게 표준화 해놓은 이유는 오일러 각의 순서가 일관되어야 하는 이유인데, 이런식으로 선택할 수 있는 시퀀스가 ​​많기 때문에 분야마다 표준이 다르다. 그러므로 세 개의 각도 회전 시퀀스가 ​​선택되면 항상 해당 순서로 변환해야 한다. 그렇지 않으면 올바른 최종 방향을 얻지 못할 수 있다.

그러면 많이 사용되는 3-2-1(Z-Y-X, Yaw-Pitch-Roll) 순서에 대한 Rotation Matrix을 확인해보자.

Fig 1. 3-2-1 Rotation Sequence Example

이에 대한 수식은 다음과 같다.

이 수식을 가지고 간단히 Matlab으로 plot해보는 시도를 해보자, 코드는 다음과 같다.

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42

clear all; close all; clc;   deg2rad = pi/180; rad2deg = 180/pi;   x = [1 0 0]'; y = [0 1 0]'; z = [0 0 1]';   Yaw   = 10 * deg2rad; Pitch = 20 * deg2rad; Roll  = 30 * deg2rad;   DCM_mat = angle2dcm(Yaw,Pitch,Roll,'ZYX'); Rot_mat = DCM_mat';% Rotation Matrix와 DCM은 Transpose 관계!   b_1 = Rot_mat*x; b_2 = Rot_mat*y; b_3 = Rot_mat*z;   figure; quiver3(0,0,0,x(1,1),x(2,1),x(3,1),'k','LineWidth',2); hold on; grid on; quiver3(0,0,0,y(1,1),y(2,1),y(3,1),'k','LineWidth',2); quiver3(0,0,0,z(1,1),z(2,1),z(3,1),'k','LineWidth',2); quiver3(0,0,0,b_1(1,1),b_1(2,1),b_1(3,1),'r','LineWidth',2); quiver3(0,0,0,b_2(1,1),b_2(2,1),b_2(3,1),'g','LineWidth',2); quiver3(0,0,0,b_3(1,1),b_3(2,1),b_3(3,1),'b','LineWidth',2); axis equal; axis([-1 1 -1 1 -1 1]); xlabel('X - base Frame'); ylabel('Y - base Frame'); zlabel('Z - base Frame'); str = sprintf('Yaw:%3.3f[deg],Pitch:%3.3f[deg],Roll:%3.3f[deg]',...     Yaw*rad2deg,Pitch*rad2deg,Roll*rad2deg); title(str); legend('X','Y','Z','b_1','b_2','b_3') %%%%%%%%%%%%%%  % Result: % b_1 = % 0.9254 % 0.1632 % -0.3420 % b_2 = % 0.0180 % 0.8826 % 0.4698 % b_3 = % 0.3785 % -0.4410 % 0.8138 %%%%%%%%%%%%%%%  Colored by Color Scripter

cs

Rotation Matrix를 직접 작성해도 문제없지만 Matlab이나 다른 언어로 작성된 DCM이나 Roation Matrix 함수를 제공하기 때문에 열심히 사용하면된다(물론 이해를 하고 사용하는게 가장 좋다). 이에 대한 결과는 다음과 같다.

Fig 2. Rotation Matrix Example Result

그러면 뭔가 동적으로 보여주는 예시를 들어보면, 코드는 다음과 같다.

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86

close all; clear all; clc;   deg2rad = pi/180; rad2deg = 180/pi;   z_final_angle = 30*deg2rad; y_final_angle = 20*deg2rad; x_final_angle = 10*deg2rad;   z_des_angle = linspace(0, z_final_angle,100)'; y_des_angle = linspace(0, y_final_angle,100)'; x_des_angle = linspace(0, x_final_angle,100)';   zero_z_angle = zeros(length(z_des_angle),1)+z_final_angle; zero_y_angle = zeros(length(y_des_angle),1)+y_final_angle; zero_x_angle = zeros(length(x_des_angle),1)+x_final_angle;   des_angle =  [ x_des_angle,  y_des_angle, z_des_angle;               zero_x_angle, zero_y_angle,zero_z_angle ];   z = [0 0 1]'; y = [0 1 0]'; x = [1 0 0]'; dcm_final = angle2dcm(des_angle(length(des_angle),3),des_angle(length(des_angle),2),des_angle(length(des_angle),1),'ZYX'); rot_final = dcm_final'; basis_z_final = rot_final*z; basis_y_final = rot_final*y; basis_x_final = rot_final*x; figure; quiver3(0,0,0,1,0,0,'k'); hold on;    grid on; xlabel('X-axis');   ylabel('Y-axis');   zlabel('Z-axis'); quiver3(0,0,0,0,1,0,'k'); quiver3(0,0,0,0,0,1,'k'); axis equal; axis([-1.5 1.5 -1.5 1.5 -1.5 1.5]); view(135,30)   % basisX = [1 0 0]';  basisY = [0 1 0]';    basisZ = [0 0 1]';   basisX_handler = quiver3(0,0,0,0,0,0); basisX_handler.Color = 'r'; basisX_handler.LineWidth = 3; basisX_handler.UData = 1;   basisX_handler.VData = 0;   basisX_handler.WData = 0;   basisY_handler = quiver3(0,0,0,0,0,0); basisY_handler.Color = 'g'; basisY_handler.LineWidth = 3; basisY_handler.UData = 0 ;  basisY_handler.VData = 1;   basisY_handler.WData = 0;   basisZ_handler = quiver3(0,0,0,0,0,0); basisZ_handler.Color = 'b'; basisZ_handler.LineWidth = 3; basisZ_handler.UData = 0;   basisZ_handler.VData = 0;   basisZ_handler.WData = 1; xlabel('X-axis');   ylabel('Y-axis');   zlabel('Z-axis');     quiver3(0,0,0,basis_z_final(1),basis_z_final(2),basis_z_final(3),'k'); quiver3(0,0,0,basis_y_final(1),basis_y_final(2),basis_y_final(3),'k'); quiver3(0,0,0,basis_x_final(1),basis_x_final(2),basis_x_final(3),'k'); pause(10)   for i=1:1:length(des_angle)     str = sprintf('Roll:%3.3f[deg], Pitch:%3.3f[deg], Yaw:%3.3f[deg]', des_angle(i,1)*rad2deg, des_angle(i,2)*rad2deg, des_angle(i,3)*rad2deg);     title(str);     dcm = angle2dcm( des_angle(i,3), des_angle(i,2), des_angle(i,1), 'ZYX' );     rot = dcm';     basis_Xr_vec = rot*x;     basis_Yr_vec = rot*y;     basis_Zr_vec = rot*z;       basisX_handler.UData = basis_Xr_vec(1,1);     basisX_handler.VData = basis_Xr_vec(2,1);     basisX_handler.WData = basis_Xr_vec(3,1);     basisY_handler.UData = basis_Yr_vec(1,1);     basisY_handler.VData = basis_Yr_vec(2,1);     basisY_handler.WData = basis_Yr_vec(3,1);     basisZ_handler.UData = basis_Zr_vec(1,1);     basisZ_handler.VData = basis_Zr_vec(2,1);     basisZ_handler.WData = basis_Zr_vec(3,1);       drawnow;     pause(0.01); end Colored by Color Scripter

cs

결과 영상은 다음과 같다.

코드와 같이 원하는 최종 회전 각도를 저장해놓으면 그에 대한 회전 행렬을 이용해 결과를 미리 보여주며 Red($b_1$), Green($b_2$) 그리고 Blue($b_3$)가 그 결과를 따라가는 것을 볼 수 있다. 그저 Handler를 이용하여 변화하는 vector를 보여주는 코드일 뿐이다.

다시 돌아와서, 다뤄봤던 Rotation Sequence는 3-2-1 인데 다른 건 안쓰일까? 라는 의문이 든다. 보통 항공기는 3-2-1을 많이 사용하는데 이는 "김벌락:Gimbal Lock"현상을 고려해서 사용한다. 그래서 어떻게 고려한 것이냐, 먼저 아래 표를 확인해보자

Table 2. Rotation Sequence get Angle

해당 표에서 3-2-1에 해당하는 부분을 살펴보면 다음과 같다.

$$\alpha(:yaw) = arctan\left(\frac{R_{21}}{R_{11}}\right)\;\;eq.1$$

$$\beta(:Pitch)=arctan\left(\frac{-R_{31}}{\sqrt{1-R_{31}^2}}\right)\;\;eq.2$$

$$\gamma(:Roll)=arctan\left(\frac{R_{32}}{R_{33}}\right)\;\;eq.3$$

위 식은 아래 식과 같이 봐야하는데

$eq.1$를 보면 $R_{11} = cos\theta sin\psi$이고  $R_{21} = -cos\theta sin\psi$ 이다. 같은 방법으로 $eq.2$, $eq.3$을 확인하면 역탄젠트 안에 있는 분수의 분모가 0이 되는 순간은 Rotation Matrix를 이용하여 각도를 계산 할 수 없는 상황이다. 다음 그림을 통해 확인해보자

그러므로 주어진 Rotation Matrix를 이용하여 각도를 계산할 수 없는 경우는 $\theta$가 $\pm\pi/2$일 경우와 $\pm\pi$에 $\psi$, $\phi$를 정의할 수 없게된다. 이러한 상황을 Singulrarity라고 하며 이 각도를 취하고 있을 때 Gimbal Lock에 빠져서 재대로된 각도를 계산해낼 수 없다. 감이 잘 안잡힐 수 있기 때문에 코드를 통해 확인해보자

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96

close all; clear all; clc;   deg2rad = pi/180; rad2deg = 180/pi;   z_final_angle = 45*deg2rad; y_final_angle = 180*deg2rad; x_final_angle = 50*deg2rad;   z_des_angle = linspace(0, z_final_angle,100)'; % y_des_angle = linspace(0, y_final_angle,100)'; y_des_angle = zeros(1, length(z_des_angle))'+y_final_angle; x_des_angle = linspace(0, x_final_angle,100)';   zero_z_angle = zeros(length(z_des_angle),1)+z_final_angle; zero_y_angle = zeros(length(y_des_angle),1)+y_final_angle; zero_x_angle = zeros(length(x_des_angle),1)+x_final_angle;   des_angle =  [ x_des_angle,  y_des_angle, z_des_angle;               zero_x_angle, zero_y_angle,zero_z_angle ];   z = [0 0 1]'; y = [0 1 0]'; x = [1 0 0]'; dcm_final = angle2dcm(des_angle(length(des_angle),3),des_angle(length(des_angle),2),des_angle(length(des_angle),1),'ZYX'); rot_final = dcm_final'; basis_z_final = rot_final*z; basis_y_final = rot_final*y; basis_x_final = rot_final*x; figure; quiver3(0,0,0,1,0,0,'k'); hold on;    grid on; xlabel('X-axis');   ylabel('Y-axis');   zlabel('Z-axis'); quiver3(0,0,0,0,1,0,'k'); quiver3(0,0,0,0,0,1,'k'); axis equal; axis([-1.5 1.5 -1.5 1.5 -1.5 1.5]); view(135,30)   % basisX = [1 0 0]';  basisY = [0 1 0]';    basisZ = [0 0 1]';   basisX_handler = quiver3(0,0,0,0,0,0); basisX_handler.Color = 'r'; basisX_handler.LineWidth = 3; basisX_handler.UData = 1;   basisX_handler.VData = 0;   basisX_handler.WData = 0;   basisY_handler = quiver3(0,0,0,0,0,0); basisY_handler.Color = 'g'; basisY_handler.LineWidth = 3; basisY_handler.UData = 0 ;  basisY_handler.VData = 1;   basisY_handler.WData = 0;   basisZ_handler = quiver3(0,0,0,0,0,0); basisZ_handler.Color = 'b'; basisZ_handler.LineWidth = 3; basisZ_handler.UData = 0;   basisZ_handler.VData = 0;   basisZ_handler.WData = 1; xlabel('X-axis');   ylabel('Y-axis');   zlabel('Z-axis');     quiver3(0,0,0,basis_z_final(1),basis_z_final(2),basis_z_final(3),'k'); quiver3(0,0,0,basis_y_final(1),basis_y_final(2),basis_y_final(3),'k'); quiver3(0,0,0,basis_x_final(1),basis_x_final(2),basis_x_final(3),'k'); % pause(10)   for i=1:1:length(des_angle) %     rot = rot_z(des_angle(i,3))*rot_y(des_angle(i,2))*rot_x(des_angle(i,1)); %     [y,p,r] = dcm2angle(rot','ZYX'); %     str = sprintf('Rotattion Matrix to Angle\nRoll:%3.3f[deg], Pitch:%3.3f[deg], Yaw:%3.3f[deg]\nTrue Angle\nRoll:%3.3f[deg], Pitch:%3.3f[deg], Yaw:%3.3f[deg]',... %         r*rad2deg, p*rad2deg, y*rad2deg, des_angle(i,1)*rad2deg, des_angle(i,2)*rad2deg, des_angle(i,3)*rad2deg); %     title(str);       dcm = angle2dcm( des_angle(i,3), des_angle(i,2), des_angle(i,1), 'ZYX' );     [yaw,pitch,roll] = dcm2angle(dcm,'ZYX');     rot = dcm';     basis_Xr_vec = rot*x;     basis_Yr_vec = rot*y;     basis_Zr_vec = rot*z;       str = sprintf('Rotation Matrix to Angle\nRoll:%3.3f[deg], Pitch:%3.3f[deg], Yaw:%3.3f[deg]\nTrue Angle\nRoll:%3.3f[deg], Pitch:%3.3f[deg], Yaw:%3.3f[deg]',...         roll*rad2deg, pitch*rad2deg, yaw*rad2deg, des_angle(i,1)*rad2deg, des_angle(i,2)*rad2deg, des_angle(i,3)*rad2deg);     title(str);       basisX_handler.UData = basis_Xr_vec(1,1);     basisX_handler.VData = basis_Xr_vec(2,1);     basisX_handler.WData = basis_Xr_vec(3,1);     basisY_handler.UData = basis_Yr_vec(1,1);     basisY_handler.VData = basis_Yr_vec(2,1);     basisY_handler.WData = basis_Yr_vec(3,1);     basisZ_handler.UData = basis_Zr_vec(1,1);     basisZ_handler.VData = basis_Zr_vec(2,1);     basisZ_handler.WData = basis_Zr_vec(3,1);       drawnow;     pause(0.1); end Colored by Color Scripter

cs

결과는 다음 영상과 같다.

영상에서 보이는 것과 같이 인위적으로 Pitch의 각을 180도 고정하였다. 이로인해 Rotation Matrix를 이용해서 역으로 각도를 계산하는데 실패하는 것을 확인할 수 있다.

이렇게 코드로 작성하여 확인할 수 있지만 뭔가 바로 바로 확인해보고 싶은데 지루하다. 자세의 변화를 다양한 방식으로 보여주는 tool이 존재한다. "QuatView"라는 프로그램을 추천한다. 아래의 영상처럼 Rotation Order를 3-2-1로 설정하고 Red(Pitch)를 90도로 설정하고 Green(Yaw)과 Blue(Roll)을 순차적으로 sliding bar로 움직여보자 그러면 Yaw를 움직여도 같은 방향으로 회전하고 Roll을 움직여도 같은 방향으로 회전함을 알 수 있다. 

지원되는 3D 그림은 Quaternion기반으로 보여주고 있기 때문에 문제없이 회전함을 보일 수 있지만 Euler로 보여준다면 분명 Roll과 Yaw의 자세를 위의 코드 결과와 같이 정의할 수 없을 것이다. 결과적으로 $\pm\pi/2$에서 김발락 현상이 생기기 때문에 해당 프로그램도 제한을 이렇게 둔 것이다. 그렇다면 이 Sequence는 항공기에서 많이 사용한다고 하는데 우리가 믿고타는 항공기 이래도 되는것인가? 사실 나도 잘 모르지만 보통 민항기의 경우 고받음각 : Pitch가 큰 기동은 하지않기 때문에 큰 문제가 없긴하다. 하지만 이러한 오일러각의 특이점 문제를 개선한 자세 표현법으로 사원수(Quaternion)가 많이 사용되고 있다. 이건 다음에 알아보자

Reference - 

[1] https://en.wikipedia.org/wiki/Euler_angles