メカナムホイールロボットの可操作性楕円体についてmatlabで描画

Manipulability Ellipsoid of mecanum wheel robot

移動ロボットの可操作性楕円体を求めてみようという試みです。

メカナムロボットの運動学

f:id:sgrsn1711:20200428002334p:plain
sgrsn1711.hatenablog.com

上図のようなメカナムホイールロボットの運動学は以下の式のようになっています。
詳しく書いているので上のページも参考に。

$\begin{bmatrix} { v _ { 1 } } \\ { v _ { 2 } } \\ { v _ { 3 } } \\ { v _ { 4 } } \end{bmatrix} = \begin{bmatrix} { - \sin \varphi } & { \cos \varphi } & { L } \\ { - \sin \varphi } & { - \cos \varphi } & { L } \\ { \sin \varphi } & { - \cos \varphi } & { L } \\ { \sin \varphi } & { \cos \varphi } & { L } \end{bmatrix} \begin{bmatrix} { \cos \theta } & { \sin \theta } & { 0 } \\ { - \sin \theta } & { \cos \theta } & { 0 } \\ { 0 } & { 0 } & { 1 } \end{bmatrix} \begin{bmatrix}{ \dot { x } _ { l } } \\ { \dot { y } _ { I } } \\ { \dot { \theta } _ { I } } \end{bmatrix}$

以下の式のような形になっていることがわかります。

$\dot{ \theta } = \begin{bmatrix}{A}\end{bmatrix} \begin{bmatrix}{R(\theta)}\end{bmatrix} \dot{ x }$

ここで、式変形して以下のような形にしたとき、J(θ)をヤコビ行列と呼びます。
ロボットアームなどでは、ヤコビ行列から可操作性楕円体を求めて、そのロボットアームのある姿勢における操作性（動かしやすさ）を評価することがあります。

$\dot{ x } = \begin{bmatrix}{J(\theta)}\end{bmatrix} \dot{ \theta }$

今回はメカナムホイールロボットの可操作性楕円体を求めます。

ただし、回転に関する操作性は考慮しないため、使用する式を以下のように変えます。
$\dot { \theta } _ { I }$ に関する式を取り除きます。

$\begin{bmatrix} { v _ { 1 } } \\ { v _ { 2 } } \\ { v _ { 3 } } \\ { v _ { 4 } } \end{bmatrix} = \begin{bmatrix} { - \sin \varphi } & { \cos \varphi } \\ { - \sin \varphi } & { - \cos \varphi } \\ { \sin \varphi } & { - \cos \varphi } \\ { \sin \varphi } & { \cos \varphi } \end{bmatrix} \begin{bmatrix} { \cos \theta } & { \sin \theta } \\ { - \sin \theta } & { \cos \theta } \end{bmatrix} \begin{bmatrix}{ \dot { x } _ { l } } \\ { \dot { y } _ { I } } \end{bmatrix}$

コード

コードはmatlabで書きました。

fai = deg2rad(30);      % メカナムホイールのローラの傾き
theta = deg2rad(10);    % 絶対座標に対するロボット本体の傾き

A = [-sin(fai) cos(fai); -sin(fai) -cos(fai); sin(fai) -cos(fai);sin(fai) cos(fai)];
R = [cos(theta) sin(theta); -sin(theta) cos(theta)];
J = inv(R) * pinv(A);

% 可操作性楕円体の描画
figure(1);
X = 0;
Y = 0;
drawManipulabilityEllipsoid(X, Y, J);
hold on;

% ロボットの描画
b_wh= 0.2;  % 車輪の幅
d_wh = 0.3; % 車輪の直径
r_a = 0.5;  % 機体の幅
robot_shape = polyshape([-r_a/2 -r_a/2 r_a/2 r_a/2],[r_a/2 -r_a/2 -r_a/2 r_a/2]);
wheel1_x = [r_a/2 r_a/2 r_a/2+b_wh r_a/2+b_wh];
wheel1_y = [r_a/2+d_wh/2 r_a/2-d_wh/2 r_a/2-d_wh/2 r_a/2+d_wh/2];
wheel1_shape = polyshape(wheel1_x, wheel1_y);
wheel2_shape = polyshape(-wheel1_x, wheel1_y);
wheel3_shape = polyshape(-wheel1_x, -wheel1_y);
wheel4_shape = polyshape(wheel1_x, -wheel1_y);
robot_pose = rotate(robot_shape, rad2deg(theta));
wheel_pose = rotate([wheel1_shape wheel2_shape wheel3_shape wheel4_shape], rad2deg(theta));
plot(robot_pose, 'FaceColor', 'w');
plot(wheel_pose, 'FaceColor', 'b');
quiver(0,0,0.5*cos(theta),0.5*sin(theta), 'Color', 'k', 'LineWidth', 3, 'MaxHeadSize', 3);
quiver(0,0,0.5*cos(theta+pi/2),0.5*sin(theta+pi/2), 'Color', 'k', 'LineWidth', 3, 'MaxHeadSize', 3);
text(0.5*cos(theta),0.5*sin(theta),'X_R','FontSize',20);
text(0.5*cos(theta+pi/2),0.5*sin(theta+pi/2),'Y_R','FontSize',20);
axis equal; grid on;

楕円を描画する関数

function drawManipulabilityEllipsoid(X, Y, J)
    % 可操作性楕円体のための行列をヤコビ行列から作成
    A = J*J.';
    [V,D] = eig(A);
    % 固有値^(1/2)を取得
    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);
    N = 200;
    % 固有値から楕円の大きさを取得
    a = Lambda_2;
    b = Lambda_1;
    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]; 
    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);
end