정방향 기구학(forward kinematics)의 기초와 Two Link Planar 예제

 사실 PinkWink는 기구학을 제대로 사용한 적이 없습니다. 항상 누군가 만들어 놓은 기구학적 모델을 이용한다던지 혹은 그냥 [바로가기]처럼 기하학적으로 푼다든지 하는 일들이 많았습니다. 그렇게 기하학적으로 풀면 머리가 좀 아프면서도 또 뭔가 정신이 맑아지는 듯한 느낌도 있거든요^^. 

 요즘은 정말 원없이 재미있게 연구(공부)하고 그걸 바로바로 적용시키는 재미에 푹 빠져있었답니다. 모터 제어기의 (회로부분빼고^^) 제어기의 이론적 코어를 완성하고, 그걸 (회사의 여건상) 직접 임베디드로 구현하는 작업을 거의 끝내가고 있거든요. 물론 실제 완전히 적용할려면 당연히 더 많은 노가다성 일들이 남아 있죠. 물론 이런 종류의 임베디드 환경이 처음이니 옆자리 동료를 무쟈게 괴롭히면서^^ 말이죠^^) 

 그런데 이제 이렇게 노가다성 일이 남게 되었얼때, 또 다른일에 눈길은 최소한 돌려야되더라구요. 왜냐면 그래야 또 정말 재미있는 일을 맡아서 수행할 수 있기 때문이지요. 그렇게 또 다른일에 도움이 되는 일을 하고 싶은데 그 때를 대비해서 살짝 기구학을 좀 기초스럽게 개념을 확립할려는 의도가 있지요.^^.

 사실 오늘은 그런 기초에 관련된 부분을 다룰건데요. 뭔가 좀 이상하고 뭔가 좀 허술해보이더라도 이런저런 이유가 있으니 참아주세용~~^^ 아 그리고 이 글은 당연히 그냥 교과서에 나오는 기초입니다.^^. 그래서 일일이 출처를 밝히는건 포기했어요. 워낙 기초스러운 이야기라서요. 좀 더 깊은 이야기나 증명등이 필요하신 분들은 기구학 책을 보시기 바랍니다.^^

(인터넷 어디선가 받은 좌표계 그림... 그러나 출처를 잃어버렸다. 아시는 분은 연락바랍니다.ㅠㅠ)

일단 위 그림처럼 절대 좌표계(x-y-z)와 달리 움직이는 좌표계(n-o-a)를 생각하는 것이 기본이 되는거죠. ㅎㅎ 그 벡터의 기본 방향벡터는

이구요. 이전 좌표계에서 바라본 다음 좌표계의 위치벡터는

이라고 하죠. 이 두 정보를 모두 가진 좌표계 행렬을

로 두게 됩니다. 그리고 많이 쓰게 될 몇몇 행렬 중에서 각 축 별로 이동하는 이동행렬은

이구요. 흔히 roll이라고 하는 n축 중심의 회전행렬은

이구요. pitch라고 하는 o축 중심의 회전행렬은

이고, yaw라고 하는 a축 중심의 회전행렬은

입니다. 나중을 위해 역행렬에 대한 이야기를 좀 해야겠네요^^. 먼저 위에서 이야기한 좌표계 행렬 F의 역행렬은

입니다. 그것은 벡터 n, o, a가 서로 직교하고 크기가 1이라는 특징때문에 나타나는 결과입니다. 당연히 비슷하게 생긴 이동 행렬인 T도 역행렬은

비슷한 모양으로 나타나는 것이지요. 그리고 결정적으로 각 회전행렬들의 역행렬은 전치행렬(Transpose)입니다. 그건 회전행렬이 orthogonal matrix이기 때문인데요. 역시 전 증명 생략~~~. 

인터넷 어디선가 받은 Two Link Planar의 그림. 그러나 역시 또 출처를 잃어버렸다.ㅠㅠ.

이제 예제하나 하죠. 3차원 공간에서 적합한 예제를 다루면 좋겠지만... 그러면 기본 개념을 확인하는데 너무 복잡하고 변수가 많으니 아주아주 간단한 Two Link Planar 로봇을 보겠습니다.^^ 사실 저 예제는 그냥 한 평면에 다 있다고 보면 되요. 좌표계는 원점(0번)과 1번 링크, 2번링크에 있는 것까지 총 3개의 좌표계가 보이네요^^.

여기서 유명한 DH 변환 방식을 적용했습니다. 흠... 뭘까? 하시겠지만.... [바로가기]에 보시면 기가막히게 잘 설명되어 있습니다.~~~~만.... 단점이 영어라는 거죠.. ㅋㅋㅋ. 아무튼 이걸 살짝 해석해보면

DH Parameters (Denavit-Hartenberg)

Step1) z축 중심으로 회전

Step2) z축 방향으로 d만큼 회전

Step3) x축 방향으로 a만큼 이동

Step4) 새로 만들 x축을 중심으로 회전 (새로 만들 z축과 이전 z축 사이의 각도만큼)

입니다.~~~^^ 무쟈게 간단하죠. ㅎㅎ 요걸 잘 만들어준 Jacques Denavit과 Richard Hartenberg라는 분들게 감사하고 싶습니다. 이 분들이 이 개념을 만든게 1955년이라는군요. 헉....^^ 아무튼 위의 Two Link Planar에 적용된 걸 다시 보면

링크가 2개니까 2개의 행이구요. 사실 한 평면에 있기 때문에 마지막 열의 alpha는 다 0입니다. 당연히 z축 중심의 회전인 theta는 그림에서 정의한 대로 theta1, 2이구요. z축 방향의 이동이 없으므로 d도 다 0이지요. 무쟈게 간단하네요^^

먼저 0번 베이스 좌표계에서 1번 링크로 변환하는 행렬은 위의 DH표에서 보면 z축 중심의 회전(theta1)과 x축방향으로의 a1만큼의 이동이지요.

1번 링크에서 2번 링크로 이동하는 것 또한 그렇습니다.

그럼 최종적으로는 위와 같이 전체 변환은 계산할 수 있지요^^.  이제 이걸 확인해볼께요. 당연히 MATLAB으로~~~^^

RotZ = @(theta) [cos(theta) -sin(theta) 0 0; sin(theta) cos(theta) 0 0; 0 0 1 0 ; 0 0 0 1];
Trans = @(x, y, z) [1 0 0 x; 0 1 0 y; 0 0 1 z; 0 0 0 1];

T_0_1 = RotZ(theta1)*Trans(a1, 0, 0);
T_1_2 = RotZ(theta2)*Trans(a2, 0, 0);
T_total = T_0_1*T_1_2;

y0 = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
y0_1 = T_0_1 * y0;
y0_2 = T_total * y0;

일단 회전행렬과 이동행렬은 미리 정의를 해 두었습니다. 그리고 위 DH 변환표대로 변환행렬을 적용했지요. 뭐 사실 수식을 전개한걸 다 사용하지 않았습니다. MATLAB은 행렬연산이 무쟈게 잘되니까요^^. 그래도 혹시 이런 연산을 직접해야할 수도 있기 때문에 수식으로는 다 전개했구요^^. 그리고 좌표평면상 원점을 정의하고 이를 변환하도록 하구요.

theta1 = 45*pi/180;
theta2 = -30*pi/180;

a1 = 15;
a2 = 15;

% DH Table
%      |  theta   d    a    alpha
% ---------------------------------
%    1 |  theta1  0    a1     0
%    2 |  theta2  0    a2     0

RotZ = @(theta) [cos(theta) -sin(theta) 0 0; sin(theta) cos(theta) 0 0; 0 0 1 0 ; 0 0 0 1];
Trans = @(x, y, z) [1 0 0 x; 0 1 0 y; 0 0 1 z; 0 0 0 1];

T_0_1 = RotZ(theta1)*Trans(a1, 0, 0);
T_1_2 = RotZ(theta2)*Trans(a2, 0, 0);
T_total = T_0_1*T_1_2;

y0 = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
y0_1 = T_0_1 * y0;
y0_2 = T_total * y0;

originX = [y0(1,4), y0_1(1,4), y0_2(1,4)];
originY = [y0(2,4), y0_1(2,4), y0_2(2,4)];

plot(originX, originY, '*--', 'linewidth', 2);
axis([min([originX, originY])-4, max([originX, originY])+4, min([originX, originY])-4, max([originX, originY])+4])
grid on
axis square

axisY0X = y0(1, 1:2);
axisY0_1X = y0_1(1, 1:2);
axisY0_2X = y0_2(1, 1:2);

axisY0Y = y0(2, 1:2);
axisY0_1Y = y0_1(2, 1:2);
axisY0_2Y = y0_2(2, 1:2);

line([originX(1), axisY0X(1)], [originY(1), axisY0Y(1)], 'color', 'r', 'linewidth', 3);
line([originX(1), axisY0X(2)], [originY(1), axisY0Y(2)], 'color', 'g', 'linewidth', 3);

line([originX(2), originX(2)+axisY0_1X(1)], [originY(2), originY(2)+axisY0_1Y(1)], 'color', 'r', 'linewidth', 3);
line([originX(2), originX(2)+axisY0_1X(2)], [originY(2), originY(2)+axisY0_1Y(2)], 'color', 'g', 'linewidth', 3);

line([originX(3), originX(3)+axisY0_2X(1)], [originY(3), originY(3)+axisY0_2Y(1)], 'color', 'r', 'linewidth', 3);
line([originX(3), originX(3)+axisY0_2X(2)], [originY(3), originY(3)+axisY0_2Y(2)], 'color', 'g', 'linewidth', 3);

xPol = [1, 2, 1, 1];
yPol = [1, 1, 2, 1];
pol0 = [xPol; yPol; [0 0 0 0]; [1 1 1 1]];
pol0_1 = T_0_1*pol0;
pol0_2 = T_total*pol0;

line(xPol, yPol, 'color', 'k', 'linewidth', 2);
line(pol0_1(1,:), pol0_1(2,:), 'color', 'k', 'linewidth', 2);
line(pol0_2(1,:), pol0_2(2,:), 'color', 'k', 'linewidth', 2);

그리고 그냥 그리면 좀 재미 없어서 pol0에 정의된 삼각형을 하나 추가했습니다. 위 코드의 나머지는 그림 그리는 거니까 그리 어렵지 않을거구요. 그리고 지금은 그냥 개념을 잘 잡았는지 스스로 확인하는 거기 때문에 뭔가 다른 기능은 없지요^^. 코드 처음에 theta1, theta2, a1, a2를 변경시켜보면 뭐가 바뀌는지 알 수 있어요~~^^ 위 코드를 실행한 결과는

처럼 나타납니다. 살짝 코드를 보시면 알겠지만, 비록 평면상이지만 좌표계도 표현했구요. 그리고 밋밋한것 같아서 삼각형도 하나 넣어둔거에요^^. 그리고 위의 코드는 GitHub에 공개[바로가기]되어 있으니 다운로드(가 필요할 것 같진 않지만^^)하실 수 있습니다.