逆運動学による軌跡制御モデリング

この例では、指定された軌跡に沿ってInverse Kinematicsブロックがマニピュレーターを駆動する方法を示します。目的の軌跡は、マニピュレーターのエンドエフェクタ用の、間隔が密な一連の姿勢として指定されます。軌跡の生成と中間点の定義は、ピック アンド プレース操作、空間加速度や速度のプロファイルからの軌跡の計算、さらにはカメラやコンピューター ビジョンを使用した主要な座標系の外部観察の模倣など多数のロボット工学アプリケーションを表します。軌跡は生成後、Inverse Kinematicsブロックを使用してジョイント空間の軌跡に変換されます。その後これを使用して、マニピュレーターおよびコントローラーのダイナミクスをシミュレートできます。

モデルの概要

モデルを読み込んで構成を確認します。

open_system('IKTrajectoryControlExample.slx');

モデルは次の 4 つの主要な処理で構成されます。

  • ターゲット姿勢の生成

  • 逆運動学

  • マニピュレーター ダイナミクス

  • 姿勢の測定

ターゲット姿勢の生成

この Stateflow チャートでは、マニピュレーターの現在のオブジェクティブである中間点を選択します。マニピュレーターが現在のオブジェクティブの許容誤差内に達すると、チャートは次の中間点にターゲットを調整します。チャートはさらに、関数eul2tformを使用して、中間点の成分を同次変換に変換し、組み立てます。選択できる中間点がなくなると、チャートはシミュレーションを終了します。

逆運動学

逆運動学によって一連のジョイント角度が計算され、エンドエフェクタの目的の姿勢が生成されました。Inverse KinematicsrigidBodyTreeモデルと共に使用して、エンドエフェクタのターゲット姿勢を同次変換として指定します。解の位置および向きに対して相対許容誤差の制約に一連の重みを指定して、ジョイント位置の初期推定を行います。ブロックは、ブロック パラメーターで指定されているrigidBodyTreeモデルから目的の姿勢を生成するジョイント位置のベクトルを出力します。解の滑らかな連続性を確保するために、前回のコンフィギュレーションの解が、ソルバーの開始位置として使用されます。最後のシミュレーション タイム ステップ以降にターゲット姿勢が更新されていない場合は、これによって計算の冗長性も軽減します。

マニピュレーター ダイナミクス

マニピュレーター ダイナミクスは 2 つのコンポーネントで構成されます。トルク信号を生成するコントローラーと、これらのトルク信号からマニピュレーターのダイナミクスをモデル化するダイナミクス モデルです。この例のコントローラーは、マニピュレーターの逆ダイナミクスによって計算されたフィードフォワード コンポーネントと、エラーを修正するフィードバック PD コントローラーを使用します。マニピュレーターのモデルは、rigidBodyTreeオブジェクトと連動するForward Dynamicsブロックを使用します。ダイナミクスと可視化の手法をより洗練させるために、Control Systems Toolbox™ ブロックセットおよび Simscape Multibody™ のツールを Forward Dynamics ブロックの代わりに使用することを検討してください。

姿勢の測定

姿勢の測定では、マニピュレーター モデルからジョイント角度測定値を取得して、[Waypoint Selection] セクションでフィードバックとして使用する同次変換行列に変換します。

マニピュレーターの定義

この例で使用するマニピュレーターは、Rethink Sawyer™ ロボット マニピュレーターです。このマニピュレーターを記述するrigidBodyTreeオブジェクトは、importrobotを使用して、URDF (Unified Robot Description Format) からインポートされます。

% Import the manipulator as a rigidBodyTree Object
sawyer = importrobot('sawyer.urdf');
sawyer.DataFormat = 'column';

% Define end-effector body name
eeName = 'right_hand';

% Define the number of joints in the manipulator
numJoints = 8;

% Visualize the manipulator
show(sawyer);
xlim([-1.00 1.00])
ylim([-1.00 1.00]);
zlim([-1.02 0.98]);
view([128.88 10.45]);

中間点の生成

この例でのマニピュレーターの目的は、イメージ coins.png で検出されるコインの境界を描画できるようになることです。まず、コインの境界を見つけるためにイメージが処理されます。

I = imread('coins.png');
bwBoundaries = imread('coinBoundaries.png');

figure
subplot(1,2,1)
imshow(I,'Border','tight')
title('Original Image')

subplot(1,2,2)
imshow(bwBoundaries,'Border','tight')
title('Processed Image with Boundary Detection')

イメージ処理後に、コインの端がピクセル位置として抽出されます。データは MAT ファイル boundaryData から読み込まれます。boundaries は cell 配列であり、各 cell には検出された単一の境界のピクセル座標を記述する配列が含まれています。このデータの生成方法をより包括的に確認するには、「イメージ内の境界トレース」 (Image Processing Toolbox が必要) の例を参照してください。

load boundaryData.mat boundaries
whos boundaries
  Name             Size            Bytes  Class    Attributes

  boundaries      10x1             25376  cell               

このデータをワールド座標系にマッピングするには、イメージの位置と、ピクセル座標と空間座標の間のスケーリングを定義する必要があります。

% Image origin coordinates
imageOrigin = [0.4,0.2,0.08];

% Scale factor to convert from pixels to physical distance
scale = 0.0015;

各点における目的のエンドエフェクタの向きのオイラー角も定義しなければなりません。

eeOrientation = [0, pi, 0];

この例では、エンドエフェクタがイメージの平面に対して常に垂直になるように、向きが選択されます。

この情報が定義されたら、目的の座標とオイラー角の各セットを中間点にまとめることができます。中間点はそれぞれ 6 要素ベクトルとして表現され、最初の 3 要素は、ワールド座標系におけるマニピュレーターの目的の xyz 位置に対応します。最後の 3 要素は、目的の向きの ZYX オイラー角に対応します。

Waypoint=[XYZϕzϕyϕx]

中間点が連結されて、n 行 6 列の配列を形成します。ここで n は軌跡内の姿勢の合計数です。配列内の各行は、軌跡内の中間点に対応します。

% Clear previous waypoints and begin building wayPoint array
clear wayPoints

% Start just above image origin
waypt0 = [imageOrigin + [0 0 .2],eeOrientation];

% Touch the origin of the image
waypt1 = [imageOrigin,eeOrientation];

% Interpolate each element for smooth motion to the origin of the image
for i = 1:6
    
    interp = linspace(waypt0(i),waypt1(i),100);
    wayPoints(:,i) = interp';
    
end

合計で、10 個のコインがあります。簡略化と高速化のために、コインの小さいサブセットは、中間点に渡される合計数を制限することによってトレースできます。以下では、イメージ内で numTraces = 3 個のコインがトレースされます。

% Define the number of coins to trace
numTraces = 3;

% Assemble the waypoints for boundary tracing
for i = 1:min(numTraces, size(boundaries,1))
    
    %Select a boundary and map to physical size
    segment = boundaries{i}*scale;
    
    % Pad data for approach waypoint and lift waypoint between boundaries
    segment = [segment(1,:); segment(:,:); segment(end,:)];
    
    % Z-offset for moving between boundaries
    segment(1,3) = .02;
    segment(end,3) = .02;
    
    % Translate to origin of image
    cartesianCoord = imageOrigin + segment;
    
    % Repeat desired orientation to match the number of waypoints being added
    eulerAngles = repmat(eeOrientation,size(segment,1),1);
    
    % Append data to end of previous wayPoints 
    wayPoints = [wayPoints;
                 cartesianCoord, eulerAngles]; 
end

この配列は、モデルに対する基本入力です。

モデルの設定

モデルを実行するには、いくつかのパラメーターを初期化しなければなりません。

% Initialize size of q0, the robot joint configuration at t=0. This will
% later be replaced by the first waypoint.
q0 = zeros(numJoints,1);

% Define a sampling rate for the simulation.
Ts = .01;

% Define a [1x6] vector of relative weights on the orientation and 
% position error for the inverse kinematics solver.
weights = ones(1,6);

% Transform the first waypoint to a Homogenous Transform Matrix for initialization
initTargetPose = eul2tform(wayPoints(1,4:6));
initTargetPose(1:3,end) = wayPoints(1,1:3)';

% Solve for q0 such that the manipulator begins at the first waypoint
ik = inverseKinematics('RigidBodyTree',sawyer);
[q0,solInfo] = ik(eeName,initTargetPose,weights,q0);

マニピュレーター動作のシミュレーション

モデルをシミュレートするには、sim コマンドを使用します。モデルは出力データセット jointData を生成し、次の 2 つのプロットで進行状況を表示します。

  • X Y Plot には、マニピュレーターのトレース動作が上から下の順に表示されます。円の間の線は、あるコインの輪郭から次のコインへ、マニピュレーターが遷移する際に発生します。

  • Waypoint Tracking プロットは、進行状況を 3D で可視化します。緑のドットはターゲット位置を示します。赤のドットは、フィードバック制御を使用するエンドエフェクタによって達成された実際のエンドエフェクタ位置を示します。

% Close currently open figures 
close all

% Open & simulate the model
open_system('IKTrajectoryControlExample.slx');
sim('IKTrajectoryControlExample.slx');

結果の可視化

このモデルは、シミュレーション後に可視化に使用できる 2 つのデータセットを出力します。ジョイント コンフィギュレーションは jointData として提供されます。ロボットのエンドエフェクタの姿勢は poseData として出力されます。

% Remove unnecessary meshes for faster visualization
clearMeshes(sawyer);

% Data for mapping image
[m,n] = size(I);

[X,Y] = meshgrid(0:m,0:n);
X = imageOrigin(1) + X*scale;
Y = imageOrigin(2) + Y*scale;

Z = zeros(size(X));
Z = Z + imageOrigin(3);

% Close all open figures
close all

% Initialize a new figure window
figure;
set(gcf,'Visible','on');

% Plot the initial robot position
show(sawyer, jointData(1,:)');
hold on

% Initialize end effector plot position
p = plot3(0,0,0,'.');
warp(X,Y,Z,I');

% Change view angle and axis
view(65,45)
axis([-.25 1 -.25 .75 0 0.75])

% Iterate through the outputs at 10-sample intervals to visualize the results
for j = 1:10:length(jointData)
    % Display manipulator model
    show(sawyer,jointData(j,:)', 'Frames', 'off', 'PreservePlot', false);
    
    % Get end effector position from homoegenous transform output
    pos = poseData(1:3,4,j);
    
    % Update end effector position for plot
    p.XData = [p.XData pos(1)];
    p.YData = [p.YData pos(2)];
    p.ZData = [p.ZData pos(3)];
    
    % Update figure
    drawnow
end