3자유도 로봇 팔 기구학 (kinematics)

3자유도 로봇 팔을 제어하기 위해서는 정기구학(forward kinematics)과 역기구학(inverse kinematics) 식이 필요합니다. 이 장에서는 삼각함수를 이용하여 정기구학과 역기구학 식을 유도합니다.

 

3자유도 로봇 팔 (PUMA-560)

로봇의 관절은 보통 모터로 표현을 하기 때문에 모터의 회전축을 어떻게 설계하는가에 따라 유도하는 식이 달라질 수 있습니다. 모터의 회전축은 보통 3차원 축의 방향으로 설계를 합니다. 어떤 특정 로봇의 경우에는 축에 평행하자 않고 축에 각도가 있는 축을 가질 수도 있습니다. 이 프로젝트에서 사용한 로봇 팔의 구조는 PUMA-560과 같은 구조를 갖습니다.

3자유도 로봇 팔(PUMA-560)의 첫 번째 관절은 Z-축에 대해 회전하고, 두 번째 관절과 세 번째 관절은 Y-축에 대해 회전을 하여 3차원 공간에서 움직이는 로봇 팔입니다.

기구학에서 관절의 각도 표현은 3차원 좌표에 대해 절대좌표가 아닌 연결된 관절에 대한 상대 좌표로 표현을 합니다. 이에 대한 내용은 이 장에서는 다루지 않습니다.

 

 3자유도 로봇 팔 정기구학 (forward kinematics)

PUMA-560 3자유도 로봇 팔에 장착된 3개의 관절 각도를 통해 로봇 팔의 끝단(end-effect)의 위치를 계산하는 식을 유도합니다. 3자유도 로봇 팔의 정기구학 식을 유도하기 전에 2자유도 로봇 팔 SCARA 로봇 팔의 정기구학 식을 먼저 유도합니다.

 

2자유도 로봇 팔(SCARA) 정기구학

대표적인 2자유도 로봇 팔은 SCARA 로봇 팔이 있습니다. SCARA 로봇 팔은 평면(X-Y 좌표) 위에서 로봇 팔이 움직일 수 있습니다. 먼저 2자유도 로봇 팔을 다루는 것은 PUMA-560 로봇 팔의 두 번째와 세 번째 관절이 SCARA 로봇 팔과 같이 평면 상에서의 운동과 같기 때문에 SCARA 로봇 팔의 정기구학 식을 유도하면 PUMA-560 로봇 팔의 정기구학 식도 쉽게 유도를 할 수 있습니다.

SCARA 로봇 팔은 아래 그림 1과 같이 두 개의 관절로 이루어져 있으며 두 개의 관절은 모두 회전 축을 z 축을 갖기 때문에 로봇 팔은 x-y 평면 위에서만 이동을 할 수 있습니다.

위 그림의 오른쪽 식은 두 관절의 각도를 알면 로봇의 끝단의 x, y 좌표를 구할 수 있습니다.

 

3자유도 로봇 팔(PUMA-560) 정기구학

3자유도 로봇 팔의 두 번째와 세 번째 관절의 축은 y축으로써 위의 SCARA 로봇과 같이 두 관절의 축은 같고 축 방향만 z 축에서 y축으로 변경된 모양을 하고 있습니다. 그러므로 축 방향만 조정을 하여 정기구학 식을 유도할 수 있습니다.

3자유도 정기구학 식을 삼각함수를 통해 유도하는 방법은 여러 가지 방법이 있습니다. 이 절에서는 그림 2에서와 같이 가상의 벡터 p를 그려서 2자유도에서 유도한 정기구학 식을 이용하여 유도를 합니다.

두 번째 관절과 세 번째 관절과 벡터 p는 2자유도에서 유도한 정기구학식과 같으며, 두 번째 관절의 시작이 2자유도 로봇에서는 원점이란 것과 3자유도 로봇에서는 z 축에 대해 d 만큼의 차이가 있습니다.

그림 2에서는 두번째 관절과 세 번째 관절의 각도를 통해서 z축의 위치와 벡터 p의 위치를 구합니다.

그림 3은 벡터p와 x, y축에 대한 관계를 나타냅니다. 그림을 통해 간단하게 x, y의 값을 구할 수 있습니다.

위 그림에서 구한 x, y는 로봇의 관절 각도가 아닌 벡터 p에 대한 값으로써 그림 2에서 구한 벡터 p를 대입하면 아래 식 1과 같이 완전한 식으로 유도됩니다.

위 정기구학 식은 3자유도 로봇 팔의 3개 관절 각도를 통해 로봇 팔의 끝단(end-effect)의 좌표를 구할 수 있습니다.

 

 매트랩 (matlab)

프로젝트에서 정기구학(forward kinematics)은 C/C++ 코드로 작성을 합니다. 코드를 작성하기 전에 매트랩 소프트웨어를 이용하여 정기구학 식에 문제가 없는지 먼저 검토를 합니다. 정기구학에 사용된 몸체(body)의 길이는 3D 모델링 파일이나 실제 로봇을 실측하거나 하여 구해야 합니다.

 

3자유도 정기구학 (fk_3dof_arm.m)

% ---------------------------------------------------
% function : fk_3dof_arm(q1, q2, q3)
% ---------------------------------------------------
function [xyz] = fk_3dof_arm(q1, q2, q3)
% X = sprintf('parameter %d, %d, %d (angle : %d, %d, %d)', d, l1, l2, q1, q2, q3);
% disp(X);

% ================================================
d = 67.60;
l1 = 79.50;
l2 = 80.0;

% ================================================
q1 = deg2rad(q1);
q2 = deg2rad(q2);
q3 = deg2rad(q3);

x = cos(q1) * (l1 * cos(q2) + l2 * cos(q2 + q3));
y = sin(q1) * (l1 * cos(q2) + l2 * cos(q2 + q3));
z = l1 * sin(q2) + l2 * sin(q2 + q3) + d;

xyz = [x; y; z];
end

• l  d, l1, l2는 로봇 팔의 몸체 길이로써 상수입니다.