最新のリリースでは、このページがまだ翻訳されていません。 このページの最新版は英語でご覧になれます。

逆運動学の導出と 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))ここで  σ1=-L14+2L12L22+2L12XE2+2L12YE2-L24+2L22XE2+2L22YE2-XE4-2XE2YE2-YE4

θ2 の解のペアを表示します。

simplify(S.theta_2)
ans = 

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

解を後で使用できるように MATLAB 関数に変換します。関数 TH1_MLF および TH2_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: 逆運動学の計算と可視化

逆運動学を使用して、θ1θ2 を X 座標と Y 座標から計算します。

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))

系のヤコビアンを使用して、ジョイント速度をエンドエフェクタ速度に関連付けたりその逆に関連付けたりできます。

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

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

(dθ1dtdθ2dt)=J+.(dXdtdYdt)ここでJ+は J の Moore-Penrose 疑似逆行列

ヤコビアンのシンボリック式を MATLAB Function ブロックに変換することもできます。複数の中間点を Simulink モデルへの入力として定義して、ロボットのエンドエフェクタ位置の軌跡をシミュレートします。Simulink モデルでは、軌跡上の各中間点に到達するジョイント角度の値に基づいてモーション プロファイルを計算できます。詳細については、Inverse Kinematics of a 2-link Robot Arm および Teaching 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);
      caxis(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);
      caxis(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);
      caxis(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); 
      caxis(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