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

PUMA-560 3자유도 로봇 팔의 끝단(end-effect)의 좌표를 로봇의 관절 각도를 계산하기 위한 식을 유도합니다. 3자유도 역기구학 식 유도는 정기구학 식을 유도하는 것보다 많이 복잡합니다. 3자유도 로봇 팔의 역기구학 식을 유도하기 전에 SCARA(2자유도) 로봇 팔의 역기구학 식을 먼저 유도합니다..

 

 2자유도의 역기구학

2자유도 로봇 팔(SCARA)의 역기구학 식의 유도는 정기구학 식에서 시작을 합니다. 그림 1은 2자유도 로봇 팔의 정기구학 식입니다.

정기구학 식에서 두 번째 관절 각도만 있는 식으로 유도를 하기 위해 x, y에 각각을 제곱을 한 후에 x, y 값을 더한 식을 만듭니다.

위에서 정리한 식을 다음과 같은 절차로 식을 정리합니다.

(1) 식 1의 오른쪽 제곱의 항을 풉니다.

(2) 제곱을 해서 더한 식의 오른쪽 항의 제곱을 푼 후에 sin, cos 제곱법칙을 이용하여 식을 정리합니다.

(3) 정리된 식에는 첫 번째 관절 각도와 두 번째 관절 각도의 합으로 된 항을 sin, cos 덧셈 공식을 적용하여 최종 식에는 각 관절의 각도 한 개로 이루어진 식으로 정리합니다.

다음은 식을 정리한 후에 sin, cos 제곱공식으로 정리를 하면 두 번째 관절 각도만 있는 식으로 유도가 됩니다.

코사인(cos)로 된 식이기 때문에 관절의 각도를 시컨트(sec)로 구할 수 있으나 로봇 팔에서는 360도 범위에서 각도를 구해야 하기 때문에 코탄젠트(cot)로 각도를 구해야 합니다. 코사인(cos) 값은 아래와 같이 정리되고 사인(sin) 값은 sin, cos 제곱 공식을 이용하여 아래와 같이 구할 수 있습니다.

사인(sin), 코사인(cos) 값은 탄젠트(tan)로 유도할 수 있으며, 탄젠트는 코탄젠트(cot)를 이용하여 각도를 구할 수 있습니다. 코탄젠트의 수학함수(math.h)에서 atan() 함수와 atan2() 함수를 지원하는데 atan() 함수는 180도 범위에서 각도를 구할 수 있고 atan2() 함수가 우리가 원하는 360도 범위에서 각도를 구할 수 있는 함수이기 때문에 다음과 같은 식을 유도할 수 있습니다.

위 식에서 두 번째 관절의 각도는 양수와 음수와 같이 두 개의 해가 존재합니다. 두번째 관절의 양수와 음수의 의미는 다음과 같습니다. (elbow up, elbow down)

2자유도 로봇 팔의 끝단(end-effect)의 좌표 (x, y)가 주어졌을 때 로봇 팔은 위 그림과 같이 두 가지 경로가 존재합니다. 3자유도 PUMA-560 로봇 팔의 경우에는 바닥에 있는 물건을 잡는 용도로써 항상 음수를 사용합니다. (elbow down)

 

첫번째 관절 각을 구하기 위해서는 관절의 시작점과 마지막 관절의 끝을 이은 선의 각도가 감마일 때 감마에서 베타각을 빼면 원하는 첫 번째 관절의 각도가 됩니다. 감마각과 베타각은 아래 그림 에서와 같이 구할 수 있습니다.

최종 첫 번째 관절을 구하는 식은 다음과 같습니다.

식 5에 구한 두 번째 관절의 각과 위에서 구한 첫 번째 관절을 구하는 식을 정리하면 다음과 같습니다.

다 관절 로봇 팔에서 역기구학의 각을 구할 때는 맨 마지막 관절에서 역순으로 각을 구하고, 여기서 구하는 각은 절대각도가 아닌 바로 전 관절의 각도에 대한 상대 각도입니다.

 

 3자유도의 역기구학

3자유도 역기구학 식은 2자유도 역기구학 식을 유도하는 방법과 같은 원리입니다. 2자유도에서 유도하는 것과 같게 하기 위해 가상으로 벡터 p를 그리면 벡터 p와 두번째 관절과 세 번째 관절은 2자유도와 같게 됩니다.

2자유도에서 두 번째 관절의 각도를 위해 x, y에 대해 식을 만든 것과 같이 3자유도의 세 번째 관절의 각도를 위해 p, z에 대해 다음과 같이 식을 만듭니다. (2자유도의 정기구학 식을 참조합니다.)

p, z에 각각 제곱을 하면 다음과 같은 식을 얻을 수 있습니다.

제곱 된 p와 z를 더한 후에 sin, cos 제곱공식을 사용하여 정리를 한 후에 다음 식을 정리하기 위해 순서를 변경합니다.

위 식에서 나중에 세 번째 관절 각도만 남기기 위해서 그림 3.에 있는 (z - d)의 값으로 대입을 하여 식을 다시 정리합니다.

세 번째 관절 각도만 남기기 위해 sin, cos 덧셈공식과 제곱공식을 이용하여 두 번째 관절 각도를 제거하여 순수한 세번째 관절 각도만 있는 식으로 유도를 합니다.

세번째 관절 각도로 유도한 식에 벡터 p는 그림 3에 있는 x, y 값으로 대입을 하여 완전한 세번째 관절 각도와 변수 x, y, z로 구성된 식이 유도됩니다. (d, l1, l2는 로봇 몸체의 길이로써 상수입니다.)

시컨트(sec, 코사인 역함수) 함수를 통해서는 로봇 관절의 정확한 각도를 구할 수 없습니다. 로봇 관절은 코탄젠트로 구하기 위해서 사인각도를 다음과 같이 구합니다.

최종 세번째 관절을 구하는 식은 다음과 같습니다.

두번째 관절의 각도를 구하기 위해서는 아래 그림과 같이 두번째 관절과 세번째 관절에 연결된 선을 그은 후에 직각 삼각형을 만듭니다.

위 그림을 통해서 두번째 관절의 각도를 구하는 식을 유도할 수 있으며 다음은 두번째 관절을 구하는 최종식입니다.

첫 번째 관절의 각도는 그림 4에서 p벡터의 각도로써 x, y 값을 통해 다음과 같이 구할 수 있습니다.

최종 3자유도 로봇 팔의 역기구학 식은 다음과 같습니다. 관절의 각도를 구하는 순서는 세 번째 관절의 각도를 구한 후에 두 번째 관절의 각도를 구해야 되며, 이 프로젝트에서 두번째 관절 각도는 음수 값을 사용합니다.

 

 매트랩(matlab) 코드

3자유도 역기구학 식을 매트랩으로 작성한 매트랩 코드입니다.

% ---------------------------------------------------
% function : lk_3dof_arm(x, y, z)
% ---------------------------------------------------
function [q] = lk_3dof_arm(x,y, z)
% X = sprintf('parameter (%d, %d, %d)', x, y, z);
% disp(X);

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

% -------------------------------------------------------------
% q1
q1 = atan2(y, x);

% -------------------------------------------------------------
% D
D = (x^2 + y^2 + (z-d)^2 -(l1^2 + l2^2)) / (2 * l1 * l2);

% -------------------------------------------------------------
% q3
q3_p = atan2(sqrt(1 - D^2), D);
q3_n = atan2((-1 * sqrt(1 - D^2)), D);

% -------------------------------------------------------------
% q2
% q2_p = atan2(z-d, sqrt(x^2 + y^2)) - atan2(l1 + l2 * cos(q3_p), l2 * sin(q3_p));
% q2_n = atan2(z-d, sqrt(x^2 + y^2)) - atan2(l1 + l2 * cos(q3_n), l2 * sin(q3_n));

q2_p = atan2(z - d, sqrt(x^2 + y^2)) - atan2(l2 * sin(q3_p), l1 + l2 * cos(q3_p));
q2_n = atan2(z - d, sqrt(x^2 + y^2)) - atan2(l2 * sin(q3_n), l1 + l2 * cos(q3_n));


% -------------------------------------------------------------
%
q1 = rad2deg(q1);
q2_p = rad2deg(q2_p);
q2_n = rad2deg(q2_n);
q3_p = rad2deg(q3_p);
q3_n = rad2deg(q3_n);
q = [q1; q2_n; q3_n];
end