Main Content

2링크 로봇 팔의 역기구학 유도 및 적용

이 예제에서는 MATLAB®과 Symbolic Math Toolbox™를 사용하여 2링크 로봇 팔의 역기구학을 유도하고 적용합니다.

이 예제에서는 조인트 파라미터와 엔드 이펙터 위치를 기호적으로 정의하고, 정기구학과 역기구학의 해를 계산하여 시각화하고, 로봇 팔의 운동을 시뮬레이션하는 데 유용한 시스템 야코비 행렬을 구합니다.

1단계: 기하 파라미터 정의하기

로봇의 링크 길이, 조인트 각도, 엔드 이펙터 위치를 기호 변수로 정의합니다.

syms L_1 L_2 theta_1 theta_2 XE YE

로봇의 링크 길이에 대한 값을 지정합니다.

L1 = 1;
L2 = 0.5;

2단계: 엔드 이펙터의 X 및 Y 좌표 정의하기

엔드 이펙터의 X 및 Y 좌표를 조인트 각도 (θ1 ,θ2)의 함수로 정의합니다.

XE_RHS = L_1*cos(theta_1) + L_2*cos(theta_1+theta_2)
XE_RHS = L2cos(θ1+θ2)+L1cos(θ1)
YE_RHS = L_1*sin(theta_1) + L_2*sin(theta_1+theta_2)
YE_RHS = L2sin(θ1+θ2)+L1sin(θ1)

기호 표현식을 MATLAB 함수로 변환합니다.

XE_MLF = matlabFunction(XE_RHS,'Vars',[L_1 L_2 theta_1 theta_2]);
YE_MLF = matlabFunction(YE_RHS,'Vars',[L_1 L_2 theta_1 theta_2]);

3단계: 정기구학 계산 및 시각화하기

정기구학은 조인트 각도를 엔드 이펙터 위치로 변환합니다((θ1,θ2)f(θ1,θ2)(XE,YE)). 주어진 특정 조인트 각도 값에 대해 정기구학을 사용하여 엔드 이펙터 위치를 계산합니다.

조인트 각도의 입력값을 0<θ1<90-180<θ2<180로 지정합니다.

t1_degs_row = linspace(0,90,100);
t2_degs_row = linspace(-180,180,100);
[tt1_degs,tt2_degs] = meshgrid(t1_degs_row,t2_degs_row);

각도 단위를 도에서 라디안으로 변환합니다.

tt1_rads = deg2rad(tt1_degs);
tt2_rads = deg2rad(tt2_degs);

MATLAB 함수 XE_MLFYE_MLF를 각각 사용하여 X 및 Y 좌표를 계산합니다.

X_mat = XE_MLF(L1,L2,tt1_rads,tt2_rads);
Y_mat = YE_MLF(L1,L2,tt1_rads,tt2_rads);

헬퍼 함수 plot_XY_given_theta_2dof를 사용하여 X 및 Y 좌표를 시각화합니다.

plot_XY_given_theta_2dof(tt1_degs,tt2_degs,X_mat,Y_mat,(L1+L2))

4단계: 역기구학 구하기

역기구학은 엔드 이펙터 위치를 조인트 각도로 변환합니다((XE,YE)g(XE,YE)(θ1,θ2)). 정기구학 방정식에서 역기구학을 구합니다.

정기구학 방정식을 정의합니다.

XE_EQ = XE == XE_RHS;
YE_EQ = YE == YE_RHS;

θ1θ2에 대해 방정식을 풉니다.

S = solve([XE_EQ YE_EQ], [theta_1 theta_2])
S = struct with fields:
    theta_1: [2x1 sym]
    theta_2: [2x1 sym]

구조체 Sθ1θ2에 대한 복수의 해를 표현합니다. θ1에 대한 한 쌍의 해를 표시합니다.

simplify(S.theta_1)
ans = 

(2atan(2L1YE+σ1L12+2L1XE-L22+XE2+YE2)2atan(2L1YE-σ1L12+2L1XE-L22+XE2+YE2))where  σ1=-L14+2L12L22+2L12XE2+2L12YE2-L24+2L22XE2+2L22YE2-XE4-2XE2YE2-YE4

θ2에 대한 한 쌍의 해를 표시합니다.

simplify(S.theta_2)
ans = 

(-σ1σ1)where  σ1=2atan(-L12+2L1L2-L22+XE2+YE2L12+2L1L2+L22-XE2-YE2-L12+2L1L2-L22+XE2+YE2)

나중에 사용할 수 있도록 해를 MATLAB 함수로 변환합니다. 함수 TH1_MLFTH2_MLF는 역기구학을 표현합니다.

TH1_MLF{1} = matlabFunction(S.theta_1(1),'Vars',[L_1 L_2 XE YE]);
TH1_MLF{2} = matlabFunction(S.theta_1(2),'Vars',[L_1 L_2 XE YE]);
TH2_MLF{1} = matlabFunction(S.theta_2(1),'Vars',[L_1 L_2 XE YE]);
TH2_MLF{2} = matlabFunction(S.theta_2(2),'Vars',[L_1 L_2 XE YE]);

5단계: 역기구학 계산 및 시각화하기

역기구학을 사용하여 X 및 Y 좌표에서 θ1θ2를 계산합니다.

X 및 Y 좌표의 그리드 점을 정의합니다.

[xmat,ymat] = meshgrid(0:0.01:1.5,0:0.01:1.5);

MATLAB 함수 TH1_MLF{1}TH2_MLF{1}을 각각 사용하여 각도 θ1θ2를 계산합니다.

tmp_th1_mat = TH1_MLF{1}(L1,L2,xmat,ymat);
tmp_th2_mat = TH2_MLF{1}(L1,L2,xmat,ymat);

각도 단위를 라디안에서 도로 변환합니다.

tmp_th1_mat = rad2deg(tmp_th1_mat);
tmp_th2_mat = rad2deg(tmp_th2_mat);

(X,Y) = (1.5,1.5)와 같은 일부 입력 좌표는 엔드 이펙터의 도달 가능 작업 공간을 벗어납니다. 역기구학 해는 보정이 필요한 허수 세타 값을 생성할 수 있습니다. 허수 세타 값을 보정하십시오.

th1_mat = NaN(size(tmp_th1_mat));
th2_mat = NaN(size(tmp_th2_mat));

tf_mat = imag(tmp_th1_mat) == 0;
th1_mat(tf_mat) = real(tmp_th1_mat(tf_mat));

tf_mat = imag(tmp_th2_mat) == 0;
th2_mat(tf_mat) = real(tmp_th2_mat(tf_mat));

헬퍼 함수 plot_theta_given_XY_2dof를 사용하여 각도 θ1θ2를 시각화합니다.

plot_theta_given_XY_2dof(xmat,ymat,th1_mat,th2_mat)

6단계: 시스템 야코비 행렬 계산하기

시스템 야코비 행렬은 다음과 같이 정의합니다.

J=d(X,Y)d(θ1,θ2)=(dXdθ1dXdθ2dYdθ1dYdθ2)

the_J = jacobian([XE_RHS YE_RHS],[theta_1 theta_2])
the_J = 

(-L2sin(θ1+θ2)-L1sin(θ1)-L2sin(θ1+θ2)L2cos(θ1+θ2)+L1cos(θ1)L2cos(θ1+θ2))

시스템 야코비 행렬 J와 해당 무어-펜로즈 의사 역행렬 J+를 사용하여 조인트 속도를 엔드 이펙터 속도에 연결하고 그 반대로 연결할 수도 있습니다.

(dXdtdYdt)=(dXdθ1dXdθ2dYdθ1dYdθ2).(dθ1dtdθ2dt)

(dXdtdYdt)=J.(dθ1dtdθ2dt)

(dθ1dtdθ2dt)=J+.(dXdtdYdt)

야코비 행렬의 기호 표현식을 MATLAB 함수 블록으로 변환할 수도 있습니다. 복수의 중간점을 Simulink® 모델의 입력으로 정의하여 궤적에서 로봇의 엔드 이펙터 위치를 시뮬레이션하십시오. Simulink 모델은 조인트 각도 값을 기반으로 궤적의 각 중간점에 도달하기 위한 모션 프로파일을 계산할 수 있습니다. 자세한 내용은 Inverse Kinematics of a 2-link Robot ArmTeaching Rigid Body Dynamics 항목을 참조하십시오.

헬퍼 함수

function plot_theta_given_XY_2dof(X_mat,Y_mat,theta_1_mat_degs,...
                                  theta_2_mat_degs)

xlab_str = 'X (m)';
ylab_str = 'Y (m)';

figure;
hax(1) = subplot(1,2,1);
   contourf(X_mat, Y_mat, theta_1_mat_degs);
      clim(hax(1), [-180 180]);
      colormap(gca,'jet'); colorbar
      xlabel(xlab_str, 'Interpreter', 'tex');
      ylabel(ylab_str, 'Interpreter', 'tex');
      title(hax(1), '\theta_1', 'Interpreter', 'tex')
      axis('equal')
hax(2) = subplot(1,2,2);
   contourf(X_mat, Y_mat, theta_2_mat_degs);
      clim(hax(2), [-180 180]);
      colormap(gca,'jet'); colorbar
      xlabel(xlab_str, 'Interpreter', 'tex');
      ylabel(ylab_str, 'Interpreter', 'tex');
      title(hax(2), '\theta_2', 'Interpreter', 'tex')
      axis('equal')

end


function plot_XY_given_theta_2dof(theta_1_mat_degs,theta_2_mat_degs,...
                                  X_mat,Y_mat,a_cmax)
                              
xlab_str = '\theta_1 (degs)';
ylab_str = '\theta_2 (degs)';

figure;
hax(1) = subplot(1,2,1);
   contourf(theta_1_mat_degs, theta_2_mat_degs, X_mat);
      clim(hax(1), [0 a_cmax]);
      colormap(gca,'jet'); colorbar
      xlabel(xlab_str, 'Interpreter', 'tex');
      ylabel(ylab_str, 'Interpreter', 'tex');
      title(hax(1), 'X_E', 'Interpreter', 'tex')
hax(2) = subplot(1,2,2);
   contourf(theta_1_mat_degs, theta_2_mat_degs, Y_mat); 
      clim(hax(1), [0 a_cmax]);
      colormap(gca,'jet'); colorbar
      xlabel(xlab_str, 'Interpreter', 'tex');
      ylabel(ylab_str, 'Interpreter', 'tex');
      title(hax(2), 'Y_E', 'Interpreter', 'tex')

end

참고 항목

함수