[로봇공학] 1. Forward Kinematics로 로봇팔 위치 찾기

서론

이전 글에 이어서 Planar 2 DOF Robot Arm 제어 시뮬레이션 만들기 프로젝트를 하기 위한 첫 단계를 나아가려고 한다. 오늘은 로봇공학의 아주 기본인 Forward Kinematics를 MATLAB을 통해 어떻게 시뮬레이션할 수 있는지 확인할 것이다.

 

따라서 오늘 확인해 볼 것은 Translation, Rotation and Transformation matrix에 대해서 알아보고, DH Parameter를 가지고 Forward Kinematics를 계산할 것이다. 그리고 마지막으로는 MATLAB에 이것들을 어떻게 Plot 해서 시뮬레이션하는지에 대해 알아볼 것이다.

---

Translation matrix

로봇공학을 공부하면서 가장 처음에 배우는 것이 Translation matrix일 것이다. 이 행렬은 2차원, 3차원 혹은 n차원 공간에서 한 점이 어디로 이동하는지를 계산해 준다. 즉, 기존의 위치를 가지는 matrix에다가 Translation matrix를 곱해주면 이동된 위치를 가지는 matrix로 계산할 수 있다.

Translation matrix

사실 엄밀히 따지면 Translation matrix를 곱해주는 것은 아니다. Translation을 할 때는 더해주는 것이 맞다. 하지만 우리는 더하고 곱하는 것보다 단순히 더하는 것을 더 선호한다. 그렇기 때문에 바로 다음에 나올 Rotation matrix와 잘 변형하여 Transformation matrix라는 아주 활용도 좋은 matrix를 만들 것이다.

---

Rotation matrix

이번에 볼 것은 Rotation matrix이다. 이 행렬은 2차원, 3차원 혹은 n차원 공간에 있는 한 점이 어디로 회전하는지를 계산해 준다. 즉, 기존에 가리키고 있는 벡터의 방향에 이 Rotation matrix를 곱하면 그 각도만큼 회전된 벡터가 되는 것이다.

Rotation matrix

이것은 실제로 2차원과 3차원에 적용해서 생각해 보면 쉽게 생각할 수 있다. 우리는 2차원 행렬의 회전과 3차원 행렬의 회전에 대해서 동역학에서 배운 적이 있을 것이다. 사실 배우지 않았다고 해도 별로 어렵지 않기 때문에, 공식처럼 외우면 된다.

2차원 회전 행렬

---

Transformation matrix

이제부터 이야기할 이 Transformation matrix가 정말 중요한 것이다. 사실 앞에서 언급한 Translation matrix와 Rotation matrix는 별로 사용하지 않는다. 하지만 그 둘을 잘 조합해서 만든 이 Transformation matrix는 정말 많이 사용하고, 정말 유용하다.

 

우리는 로봇팔을 제어 하거나 다룰 때 이동과 회전으로 모든 움직임을 설명할 수 있다. 각 관절과 프레임이 어떻게 회전하고 이동하는지로 우리가 원하는 모든 움직임을 만들 수 있는 것이다. 하지만, 어떨 때는 회전만 하고, 어떨 때는 이동만 한다고 해서 그때마다 다른 matrix를 쓰는 것은 비효율 적이다.

 

따라서 회전과 이동에 상관없이 항상 공통적으로 사용할 수 있는 Transformation matrix라는 form을 만든 것이다.

Transformation matrix

이 matrix를 보면 주의해야 하는 것이 하나 있다. 각 행렬의 마지막 항에 1이라는 의미 없는 값이 있다는 것이다. 이것은 연산 과정에서 Translation matrix를 구현하기 위해 넣은 것이기에, 최종적으로 결과 값을 사용할 때는 마지막 항인 1을 잘 제거하고 사용해야 한다.

---

Forward Kinematics

DH Parameter에 대해서 알아보기 전에 지금까지 다룬 Transformation matrix를 가지고 MATLAB에서 Forward Kinematics로 로봇팔의 끝점의 위치를 시뮬레이션해 보겠다.

전체 코드

clc; clear; close all;

Link_length = [1; 1];
target_q = [[deg2rad(30); deg2rad(30);] [deg2rad(10); deg2rad(40);] [deg2rad(100); deg2rad(-40);]];

function position = FK(Link_length, q)
    H0_1 = [cos(q(1)), -sin(q(1)), 0; sin(q(1)), cos(q(1)), 0; 0, 0, 1;];
    H1_2 = [cos(q(2)), -sin(q(2)), Link_length(1); sin(q(2)), cos(q(2)), 0; 0, 0, 1;];

    first_joint = H0_1*[Link_length(1);0;1;];
    second_joint = H0_1*H1_2*[Link_length(2);0;1;];

    position = [0, 0; first_joint(1), first_joint(2); second_joint(1), second_joint(2)];
end

figure;
grid on;
xlim([-2.5, 2.5]); % x 축 범위 설정
ylim([-2.5, 2.5]); % y 축 범위 설정
xlabel('X-axis');
ylabel('Y-axis');
title('2 DOF Robot Arm');
hold on;

for i = 1:1:3
    joint_pos = FK(Link_length, target_q(:, i));
    delay(500);
    line([0, joint_pos(2,1)], [0, joint_pos(2,2)], 'Color', 'b', 'LineWidth', 2);
    line([joint_pos(2,1), joint_pos(3,1)], [joint_pos(2,2), joint_pos(3,2)], 'Color', 'r', 'LineWidth', 2);
end

 

결과 화면

2 DOF Robot Arm 시뮬레이션

 

코드 분석

Initial Parameters

clc; clear; close all;

Link_length = [1; 1];
target_q = [[deg2rad(30); deg2rad(30);] [deg2rad(10); deg2rad(40);] [deg2rad(100); deg2rad(-40);]];

clc; clear; close all;은 MATLAB을 짤 대 항상 넣어주는 코드이다. 기존에 저장되어 있던 변수들과 배열 정보들을 초기화해 주는 것이다. 그리고 기존에 열려있던 창들도 모두 닫아주는 코드이다.

 

그다음으로는 여러 Initial Parameter를 정의해 준다. 보통 init_para라고도 많이 부르는데, 로봇팔의 길이나 목표로 하는 각도, 초기 각도, 질량 등을 정의해 주는 것이다. 여기서 한 번 변수를 정의해주고 나서 아래 코드들에서는 이 값들을 활용하거나 바꾸면서 사용한다.

 

코드를 보면 타겟 각도에 deg2rad()을 사용하였다. 즉, 각도를 degree가 아니라 radian 단위를 사용할 것이다. 특히, MATLAB에서는 대부분의 함수가 radian 단위를 사용하기 때문에 degree와 radian을 혼용하지 않도록 주의해야 한다.

 

Forward Kinematics

function position = FK(Link_length, q)
    H0_1 = [cos(q(1)), -sin(q(1)), 0; sin(q(1)), cos(q(1)), 0; 0, 0, 1;];
    H1_2 = [cos(q(2)), -sin(q(2)), Link_length(1); sin(q(2)), cos(q(2)), 0; 0, 0, 1;];

    first_joint = H0_1*[Link_length(1);0;1;];
    second_joint = H0_1*H1_2*[Link_length(2);0;1;];

    position = [0, 0; first_joint(1), first_joint(2); second_joint(1), second_joint(2)];
end

이 함수가 Forward Kinematics 함수이다. 함수에 로봇팔의 길이와 각도를 넣으면 해당 위치의 x, y 좌표를 반환해 준다. 이때, 각 관절들의 위치를 모두 반환함으로써, 로봇팔이 어떤 모양으로 있는지를 확인할 수 있게 하였다.

 

H0_1은 원점과 첫 번째 관절의 Transformation matrix이다. 그리고 H1_2는 첫 번째 관절과 두 번째 관절의 Transformation matrix이다. 첫 번째 관절의 위치는 원점에서 Transformation matrix에 첫 번째 frame의 길이를 곱하면 계산할 수 있다. 두 번째 관절의 위치는 두 Transformation을 곱하고(H0_2), 거기에 두 번째 frame의 길이를 곱하면 된다.

 

Plot Robot Arm

figure;
grid on;
xlim([-2.5, 2.5]);
ylim([-2.5, 2.5]);
xlabel('X-axis');
ylabel('Y-axis');
title('2 DOF Robot Arm');
hold on;

for i = 1:1:3
    joint_pos = FK(Link_length, target_q(:, i));
    pause(0.5);
    line([0, joint_pos(2,1)], [0, joint_pos(2,2)], 'Color', 'b', 'LineWidth', 2);
    line([joint_pos(2,1), joint_pos(3,1)], [joint_pos(2,2), joint_pos(3,2)], 'Color', 'r', 'LineWidth', 2);
end

마지막 코드는 계산한 로봇팔의 위치를 plot 하는 것이다. figure를 정의해주고, 거기에 for문을 사용하여 타겟 각도에 따른 로봇팔의 모양을 plot 했다. 이때, pause(0.5);를 넣어주어 0.5초마다 하나씩 볼 수 있도록 하였다.

---

마무리

오늘은 Forward Kinematics를 사용하여 각도에 다라 Planar 2 DOF Robot Arm가 어떻게 위치하는지를 확인할 수 있는 시뮬레이션을 만들었다. 즉, 관절의 각도를 사용하는 Joint space에서 위치 좌표를 사용하는 Task space로의 변환을 알아보았다. 단순히 이론으로만 공부하며 Transformation matrix를 계산하는 것보다 이렇게 직접 코드를 통해 눈으로 확인하는 것이 로봇공학을 공부함에 있어서 도움이 될 것이다.

 

다음 글에서는 Inverse Kinematics를 사용하여 Task space에서 Joint space를 제어하는 방법을 알아볼 것이다. 그렇게 될 경우 우리는 원하는 좌표에 로봇팔이 갈 수 있도록 각 관절의 각도를 계산할 수 있게 되는 것이다.