좌표계의 기초 표현과 이동 및 회전 행렬

선택과 후회에 가까운 아쉬움? 뭐 이딴것들에 대해 요즘 생각 중이랍니다. 특히 팀플레이와 팀 혹은 단체의 분위기를 making한다는 것이 얼마나 우수운 이야기인지를 생각하며... 결국은 consensus라는 것이 얼마나 말할때는 있어보이지만.. 결국 아무것도 없이 떠들때는 우수운 이야기인지를 또한 생각하지요. 특히 집단 지성이라는 브레인 스토밍이라는 것의 헛점도 생각한답니다. 아무튼 요딴 것들은 모두 일종의 "책임전가", 혹은 "책임회피"라는 건방진 생각까지 하게 되네요. 뭐...그렇다는 겁니다.ㅎㅎㅎ...

아무튼 오늘은 예전에 분리시킨 Robotics라는 카테고리에 글도 쓸겸 기초적인 부분을 오랜만에 정립시킬겸 해서 로보틱스의 완전 기초부분인 좌표계의 표현과 기초적인 이동 행렬 및 회전 행렬에 대한 이야기를 해볼까 합니다.^^

{A}라는 좌표계는 위 그림처럼 x-y-z축을 의미하는 방향벡로 구성되고 그 위의 점 P가 있네요...

그 점을 표현하는 방법으로 일반적으로 위와 같이 P_atA로 표현합니다.

좌표계가 {A}, {B} 두 개가 있고... 같은 점 P를 각 좌표계에서 표현하는 경우는 어떨까요?

먼저 {A} 좌표계에서 본 P와 {B} 좌표계에서 본 P를 각 각 P_atA, P_atB로 표현할 수 있습니다. 그리고, 두 좌표계는 위 그림에서는 회전은 일어나지 않았으니 두 좌표계의 원점만 보면 {B}좌표계의 원점을 {A}좌표계에서 볼 때를 정의하면 될 것입니다.

그래서 위 식과 같이 {A}에서 본 P를 {B} 좌표계에서 본 P와 {A}에서 {B}의 원점의 벡터 합으로 표현할 수 있을 것입니다.

이번에는 회전까지 들어갔다고 보죠...

그럴 경우는 {A}좌표계에서 {B} 좌표계까지 회전한 양을 정의한 회전행렬로 또한 표현할 수 있을 것입니다.

그러면 위와 같은 표현으로 {A} 좌표계에서 P 점을 정의할 수 있는 것이지요.

이걸... Homogeneous Transform으로 표현해서 하나의 연산자로 표현하면 위 식과 같습니다.

이제.. 점의 이동을 정의해 보겠습니다. 위 그림처럼 {A} 좌표계 안에서 P_1이 P_2로 이동하게 하는 것을 표현하는 것이지요.

일단 위와 같이 편하게 표현가능합니다.^^

이를 연산자로 표현하면 위 식처럼 표현됩니다. 위의 식은 homogeneous transform입니다.

Dq는 위 식과 같은 모양이구요...

이동 행렬인 Q행렬은 위 수식과 같습니다.

좌표계 사이의 회전을 위 행렬처럼 정의합니다.

그러나 한 좌표계 안에서 회전을 표현할 때는 그냥 R...이렇게만 표현하면 됩니다.

회전 중심 축과 회전각까지 포함해서 수식을 표현하면 위 식처럼 되지요...

각 축을 중심으로 회전한 회전행렬의 정의는 위 식과 같습니다.^^

이제 간단히 예제를 한 번 볼까요^^ 일단, {A} 좌표계의 (1, 2, 1)이라는 점을 z축 중심으로 30도만큼 회전하고, x축 방향으로 1만큼 이동하는 계산을 해보죠...^^

RotZ = @(a) [cos(a) -sin(a) 0; sin(a) cos(a) 0; 0 0 1];

R_AtoB = RotZ(30*pi/180)
D_q = [1 0 0]'

P_atA = [1 2 1]'

P_atB = R_AtoB * P_atA
P_atB = P_atB + D_q

위 코드로 예제를 표현해 볼 수 있네요. 이걸 실행하고 나면

일단 회전행렬과 이동행렬이 정의되고, 대상이 된ㄴ 점 P_atA가 나오고나서

그 결과가 나왔네요.^^ 이번에는 연산자로 처리해 보겠습니다.

T_AtoB = [R_AtoB D_q; 0 0 0 1]

X_hatA = [1 0 0]';
Y_hatA = [0 1 0]';
Z_hatA = [0 0 1]';

P_atA = [X_hatA Y_hatA Z_hatA P_atA; 0 0 0 1]

P_atB = T_AtoB * P_atA

위와 같이 T_AtoB라는 행렬이 회전과 이동을 모두 정의했습니다. 그리고 P_atA도 기준 좌표축까지 모두 함께 정의를 해두고 연산하는 것이지요

그랬을때 나온 결과입니다. P_atB의 제일 우측 값을 보면 앞선 방법과 같은 결과가 나왔다는 것을 알 수 있습니다.^^ 이제 이 방법을 [바로가기]에서 이야기한 quiver3라는 함수를 이용해서 그래픽하게 한 번 표현해 보죠^^

figure
hold on; grid on; view(-40, 30)
xlabel('x'); ylabel('y'); zlabel('z')
set(gcf, 'Color', [1,1,1])
axis([-1 2 -1 2 0 2])

quiver3(0, 0, 0, P_atA(1,1), P_atA(2,1), P_atA(3,1), '--', 'LineWidth', 1.5, 'Color', 'red')
quiver3(0, 0, 0, P_atA(1,2), P_atA(2,2), P_atA(3,2), '--', 'LineWidth', 1.5, 'Color', 'green')
quiver3(0, 0, 0, P_atA(1,3), P_atA(2,3), P_atA(3,3), '--', 'LineWidth', 1.5, 'Color', 'blue')
quiver3(0, 0, 0, P_atA(1,4), P_atA(2,4), P_atA(3,4), '--', 'LineWidth', 1.5, 'Color', 'black')

quiver3(0, 0, 0, P_atB(1,1), P_atB(2,1), P_atB(3,1), 'LineWidth', 2.5, 'Color', 'red')
quiver3(0, 0, 0, P_atB(1,2), P_atB(2,2), P_atB(3,2), 'LineWidth', 2.5, 'Color', 'green')
quiver3(0, 0, 0, P_atB(1,3), P_atB(2,3), P_atB(3,3), 'LineWidth', 2.5, 'Color', 'blue')
quiver3(0, 0, 0, P_atB(1,4), P_atB(2,4), P_atB(3,4), 'LineWidth', 1.5, 'Color', 'black')

hold off

이렇게 표현하고 나면...

요런 결과를 얻을 수 있답니다.^^. 정사영된 기준선 등등이 표현되면 좋겠지만.. 뭐.. 그건 다음에 하죠^^.