2リンクロボットアームの可操作性楕円体を求めるMATLABプログラムの作成

可操作性楕円体により、ロボットの動かしやすさを表すことが出来ます。

ロボットの可操作性楕円体とはロボットの姿勢（関節状態）によって、動作しやすい姿勢（方向）や動作しづらい姿勢（方向）が存在します。これらのどれ...

これまでの記事では、2リンクモデルの可操作性楕円体を求める方法を紹介しました。

2リンクモデルの可操作性楕円体を求めてみよう可操作性楕円体を用いることで、ロボットの手先がどの方向にどれくらい動かしやすいかが分かります。 https://tajim...

今回の記事では、これまでに紹介した算出方法を用いて実際に可操作性楕円体を求めるプログラムをMATLABで作成します。

Contents

2リンクロボットアームの可操作性楕円体
可操作性楕円体を求めるプログラム
シミュレーション結果
まとめ

2リンクロボットアームの可操作性楕円体

今回の記事では、下図のような2つの回転関節による2リンクモデルについて、可操作性楕円体（Manipulability Ellipsoid）を求めます。

上の図のように、リンク1とリンク2の長さをそれぞれL₁とL₂、回転関節の角度をそれぞれθ₁とθ₂と定義します。

この2リンクモデルの可操作性楕円体を求めるMATLABプログラムを作成します。

可操作性楕円体を求めるプログラム

下記のプログラムは、今回の記事で取り扱う2リンクモデルの可操作性楕円体を求めるMATLABプログラムです。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Manipulability Ellipsoid of 2R Robot %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear;
close all;

% Simulation Conditions
L1 = 1; L2 = 1;
Th1 = deg2rad(30); Th2 = deg2rad(60);

% Jacobian Matrix
J = [
    -L1*sin(Th1)-L2*sin(Th1+Th2), -L2*sin(Th1+Th2);
    L1*cos(Th1)+L2*cos(Th1+Th2), L2*cos(Th1+Th2);
];

% Calculate Matrix A
A = J*J.';

% Calculate Eigen Value and Vector
[V,D] = eig(A)

% Calculate Axis and Volume of Ellipsoid
Lambda = [D(1,1)^0.5,D(2,2)^0.5];
Lambda_1 = Lambda(1);
Lambda_2 = Lambda(2);
V_1 = V(:,1);
V_2 = V(:,2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculate Manipulability Ellipsoid
a = Lambda_2;
b = Lambda_1;
N = 200;
x = zeros(1,4*N);
y = zeros(1,4*N);
for i = 1:2*N
    t = a-a/N*(i-1);
    x(i) = t;
    y(i) = b*(1-t^2/a^2)^0.5;
end
for i = 1:2*N
    t = -a+a/N*(i-1);
    x(2*N+i) = t;
    y(2*N+i) = -b*(1-t^2/a^2)^0.5;
end
x(end+1) = x(1);
y(end+1) = y(1);
Th_Z = atan2(V(2,2),V(1,2));
rotZ = [cos(Th_Z),-sin(Th_Z);sin(Th_Z),cos(Th_Z)];
ElipXY = rotZ*[x;y]; 

% Calculate Link Position
X1 = L1*cos(Th1);
Y1 = L1*sin(Th1);
X2 = L1*cos(Th1)+L2*cos(Th1+Th2);
Y2 = L1*sin(Th1)+L2*sin(Th1+Th2);
X = L1*cos(Th1)+L2*cos(Th1+Th2);
Y = L1*sin(Th1)+L2*sin(Th1+Th2);

% Draw Simulation Result
figure;
hold on;
plot([0,X1],[0,Y1],'k','Linewidth',10);
plot([X1,X2],[Y1,Y2],'k','Linewidth',10);

rectangle('Position',[-0.1,-0.1,0.2,0.2],'Curvature',[1,1],'FaceColor','g','EdgeColor','none');
rectangle('Position',[X1-0.1,Y1-0.1,0.2,0.2],'Curvature',[1,1],'FaceColor','g','EdgeColor','none');
rectangle('Position',[X-0.1,Y-0.1,0.2,0.2],'Curvature',[1,1],'FaceColor','b','EdgeColor','none');

plot(Lambda_1*[V_1(1), -V_1(1)]+X,Lambda_1*[V_1(2), -V_1(2)]+Y,'m','Linewidth',1.5);
plot(Lambda_2*[V_2(1), -V_2(1)]+X,Lambda_2*[V_2(2), -V_2(2)]+Y,'m','Linewidth',1.5);

plot(ElipXY(1,:)+X,ElipXY(2,:)+Y,'r','Linewidth',2);
hold off;
axis equal; grid on;
axis([-1.5 3 -0.5 3.0])

与えられたシミュレーション条件から可操作性楕円体の軸の方向や大きさを求めます。

ちなみに、後半（%%%・・・の後）はシミュレーション結果を（きれい？に）表示するためのプログラムで、無視して頂いても構いません。

以下では、プログラムの各項目について簡単に説明していきます。

シミュレーション条件の定義

まず、今回のシミュレーション条件を定義します。

% Simulation Conditions
L1 = 1;
L2 = 1;
Th1 = deg2rad(30);
Th2 = deg2rad(60);

今回はリンクの長さ（L₁とL₂）と各関節の角度（θ₁とθ₂）をシミュレーション条件として定義しました。

ヤコビ行列の算出

次に、ヤコビ行列Jを2リンクモデルのヤコビ行列の式にシミュレーション条件を代入して求めます。

% Jacobian Matrix
J = [
    -L1*sin(Th1)-L2*sin(Th1+Th2), -L2*sin(Th1+Th2);
    L1*cos(Th1)+L2*cos(Th1+Th2), L2*cos(Th1+Th2);
];

これで、ヤコビ行列Jを求めることが出来ました。

固有値と固有ベクトルの算出

そして、ヤコビ行列Jとヤコビ行列の転置行列J^Tから行列Aを求め、その行列Aの固有値Dと固有ベクトルVを算出します。

% Calculate Matrix A
A = J*J.';

% Calculate Eigen Value and Vector
[V,D] = eig(A)

ちなみに、これまでの記事では説明のために行列Aを用いていました。

しかし、下のように直接固有値と固有ベクトルを求めた方がプログラムはシンプルになります。

% Calculate Eigen Value and Vector
[V,D] = eig(J*J.')

もちろん、どちらを用いても算出結果のVとDは変わりません。

可操作性楕円体の軸と大きさの算出

最後に、求めた固有値Dと固有ベクトルVから可操作性楕円体の大きさλと軸の方向vを求めます。

% Calculate Axis and Volume of Ellipsoid
Lambda = [D(1,1)^0.5,D(2,2)^0.5];

Lambda_1 = Lambda(1);
Lambda_2 = Lambda(2);

V_1 = V(:,1);
V_2 = V(:,2);

固有値Dの1/2乗が可操作性楕円体の大きさλになり、固有ベクトルVが可操作性楕円体の軸方向vになります。

シミュレーション結果

求めた可操作性楕円体の情報（大きさと向き）をもとに、可操作性楕円体を描きます。

% Calculate Manipulability Ellipsoid
a = Lambda_2;
b = Lambda_1;
N = 200;
x = zeros(1,4*N);
y = zeros(1,4*N);
for i = 1:2*N
    t = a-a/N*(i-1);
    x(i) = t;
    y(i) = b*(1-t^2/a^2)^0.5;
end
for i = 1:2*N
    t = -a+a/N*(i-1);
    x(2*N+i) = t;
    y(2*N+i) = -b*(1-t^2/a^2)^0.5;
end
x(end+1) = x(1);
y(end+1) = y(1);
Th_Z = atan2(V(2,2),V(1,2));
rotZ = [cos(Th_Z),-sin(Th_Z);sin(Th_Z),cos(Th_Z)];
ElipXY = rotZ*[x;y]; 

% Calculate Link Position
X1 = L1*cos(Th1);
Y1 = L1*sin(Th1);
X2 = L1*cos(Th1)+L2*cos(Th1+Th2);
Y2 = L1*sin(Th1)+L2*sin(Th1+Th2);
X = L1*cos(Th1)+L2*cos(Th1+Th2);
Y = L1*sin(Th1)+L2*sin(Th1+Th2);

% Draw Simulation Result
figure;
hold on;
plot([0,X1],[0,Y1],'k','Linewidth',10);
plot([X1,X2],[Y1,Y2],'k','Linewidth',10);

rectangle('Position',[-0.1,-0.1,0.2,0.2],'Curvature',[1,1],'FaceColor','g','EdgeColor','none');
rectangle('Position',[X1-0.1,Y1-0.1,0.2,0.2],'Curvature',[1,1],'FaceColor','g','EdgeColor','none');
rectangle('Position',[X-0.1,Y-0.1,0.2,0.2],'Curvature',[1,1],'FaceColor','b','EdgeColor','none');

plot(Lambda_1*[V_1(1), -V_1(1)]+X,Lambda_1*[V_1(2), -V_1(2)]+Y,'m','Linewidth',1.5);
plot(Lambda_2*[V_2(1), -V_2(1)]+X,Lambda_2*[V_2(2), -V_2(2)]+Y,'m','Linewidth',1.5);

plot(ElipXY(1,:)+X,ElipXY(2,:)+Y,'r','Linewidth',2);
hold off;
axis equal; grid on;
axis([-1.5 3 -0.5 3.0])