このページの内容は最新ではありません。最新版の英語を参照するには、ここをクリックします。
逆運動学による軌跡制御モデリング
この Simulink の例では、指定された軌跡に沿ってInverse Kinematicsブロックがマニピュレーターを駆動する方法を示します。目的の軌跡は、マニピュレーターのエンドエフェクタ用の、間隔が密な一連の姿勢として指定されます。軌跡の生成とウェイポイントの定義は、ピック アンド プレース操作、空間加速度や速度のプロファイルからの軌跡の計算、さらにはカメラやコンピューター ビジョンを使用した主要な座標系の外部観察の模倣など多数のロボティクス アプリケーションを表します。軌跡は生成後、Inverse Kinematicsブロックを使用してジョイント空間の軌跡に変換されます。その後これを使用して、マニピュレーターおよびコントローラーのダイナミクスをシミュレートできます。
モデルの概要
モデルを読み込んで構成を確認します。
open_system('IKTrajectoryControlExample.slx');
モデルは次の 4 つの主要な処理で構成されます。
ターゲット姿勢の生成
逆運動学
マニピュレーター ダイナミクス
姿勢の測定
ターゲット姿勢の生成
この Stateflow チャートでは、マニピュレーターの現在のオブジェクティブであるウェイポイントを選択します。マニピュレーターが現在のオブジェクティブの許容誤差内に達すると、チャートは次のウェイポイントにターゲットを調整します。チャートはさらに、関数eul2tform
を使用して、ウェイポイントの成分を同次変換に変換し、組み立てます。選択できるウェイポイントがなくなると、チャートはシミュレーションを終了します。
逆運動学
逆運動学によって一連のジョイント角度が計算され、エンドエフェクタの目的の姿勢が生成されました。Inverse KinematicsをrigidBodyTree
モデルと共に使用して、エンドエフェクタのターゲット姿勢を同次変換として指定します。解の位置および向きに対して相対許容誤差の拘束に一連の重みを指定して、ジョイント位置の初期推定を行います。ブロックは、ブロック パラメーターで指定されている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 オイラー角に対応します。
ウェイポイントが連結されて、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 個のコインがあります。簡略化と高速化のために、コインの小さいサブセットは、ウェイポイントに渡される合計数を制限することによってトレースできます。このコードが画像内で追跡するコインの数は 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 プロットは、進行状況を 3 次元で可視化します。緑のドットはターゲット位置を示します。赤のドットは、フィードバック制御を使用するエンドエフェクタによって達成された実際のエンドエフェクタ位置を示します。
% 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