Robotics と Simscape を使用したマニピュレーター アームのモデル化と制御

ABB YuMi ロボットを使用してピックアンドプレース ワークフローを実行し、Simulink® でロボットのアルゴリズムを設計してから Simscape™ を使用してテスト環境でアクションをシミュレートする方法を示します。この例では、関連するアルゴリズム設計に一層専念できるように、さまざまな忠実度レベルでシステムをモデル化する方法についても説明します。

モデル設計のさまざまな項目にさらに簡単に着目できるように、この例の設計要素は次の 3 つのセクションに分けられています。

1. 簡略化されたマニピュレーター システム ダイナミクスを使用したピックアンドプレース用のタスクおよび軌跡スケジューラの作成

2. コア マニピュレーター ダイナミクスの追加とコントローラーの設計

3. ロボットおよび環境の Simscape モデルでのワークフロー全体の検証

目標の概要

この例では、Simulink モデルが、完結した繰り返し可能なピックアンドプレース ワークフローに連続軌跡を形成するウェイポイントを変換します。モデルには 2 つの重要な要素があります。

タスク スケジューリングと軌跡生成の部分では、ロボットが状態間をどのように移行するかを定義します。これには、任意の時点におけるロボット コンフィギュレーションの状態、ゴール位置はどこか、グリッパーを開くか閉じるか、およびロボットに送信されている現在の軌跡が含まれます。

システム ダイナミクスの部分ではロボットの動作をモデル化します。これは、一連の基準軌跡と boolean のグリッパー コマンド (開または閉) が指定された場合のロボットの動きを定義します。システム ダイナミクスは、モデル全体の目的により、さまざまなレベルの忠実度でモデル化できます。

この例の目的は、タスク スケジューラの設計時に、ロボットのモーション制御が安定しているという前提の下でスケジューラが正確に動作すると確認することです。この部分では、迅速にシミュレートする簡単なモデルが望ましいため、システム ダイナミクスは "Joint Space Motion Model" ブロックを使用してモデル化されます。このブロックは、応答パラメーターが事前定義された安定したコントローラーの下でジョイント空間の基準軌跡を指定した場合に、マニピュレーターの動作をシミュレートします。タスク スケジューリングが完了すると、モデルの重要点はコントローラーの設計とシステムの検証に移行します。これには、より複雑なシステム ダイナミクス モデルが必要です。

ロボットと環境の定義

ABB YuMi ロボット モデルを読み込みます。このロボットは、2 本のアームをもつ産業用マニピュレーターです。この例では 1 本のアームのみを使用します。

robot = loadrobot('abbYumi','Gravity',[0 0 -9.81]);

シミュレートされた軌跡を再生する可視化を作成します。

iviz = interactiveRigidBodyTree(robot);
ax = gca;

補助関数の例を使用して一連の衝突オブジェクトを作成することにより、環境を追加します。

exampleHelperSetupWorkspace(ax);

共有シミュレーション パラメーターの初期化

この例では、一連の事前定義されたコンフィギュレーション configSequence を、abbSavedConfigs MAT ファイルからロボットの状態として使用します。

load abbSavedConfigs.mat configSequence

シミュレーション用に、ロボットの初期状態を、各ジョイントの位置、速度および加速度を含めて定義しなければなりません。

% Define initial state
q0 = configSequence(:,1); % Position
dq0 = zeros(size(q0)); % Velocity
ddq0 = zeros(size(q0)); % Acceleration

タスクおよび軌跡スケジューラの作成

最初のモデルを読み込みます。これは、モデルのタスク スケジューリングおよび軌跡生成のセクションに重点を置いています。

open_system('modelWithSimplifiedSystemDynamics.slx');

簡略化されたシステム ダイナミクス

モデルのスケジューリング部分に重点を置くために、システム ダイナミクスは Joint Space Motion Model ブロックを使用してモデル化されています。この運動モデルは、安定した正確な制御の下で、指定されたコンフィグレーションにロボットが到達できることを前提にしています。この例の後の部分で、システム ダイナミクスのより正確なモデル化について詳細に説明します。

グリッパーは、0 または 1 (開または閉) の単純な boolean コマンド入力、および命令された位置にグリッパーが到達したかどうかを示す出力としてモデル化されます。通常、ロボットは、他のコンフィギュレーション入力とは別にグリッパーを扱います。

タスク スケジューリング

ロボットが実行する一連のタスクは 8 つの状態です。

スケジューラは、MATLAB Function ブロック commandLogic を使用して実装されます。グリッパーの状態が達成され、マニピュレーターのすべてのジョイントがターゲット位置の事前定義されたしきい値内に到達した場合、スケジューラは状態を進めます。各タスクが Trapezoidal Velocity Profile Trajectory ブロックに入力され、各ウェイポイントの間に滑らかな軌跡が生成されます。

モデルのシミュレーション

提供されている Simulink モデルは、例に関連する変数をモデル ワークスペースに保存しています。必要に応じて、[Load Default Parameters] をクリックして変数を再初期化します。詳細については、モデル ワークスペース (Simulink)を参照してください。

sim を呼び出してモデルを実行します。

対話型の可視化を使用して動作を再生します。最初の動作後にサイクルが予測どおりにループすることを確認するために、モデルは余分に数秒シミュレートされます。このモデルは環境の相互作用をシミュレートしないため、このシミュレーションでロボットが実際に目標物をつかむことはありません。

simout = sim('modelWithSimplifiedSystemDynamics.slx');

% Visualize the motion using the interactiveRigidBodyTree object.
iviz.ShowMarker = false;
iviz.showFigure;
rateCtrlObj = rateControl(length(simout.tout)/(max(simout.tout)));
for i = 1:length(simout.tout)
    iviz.Configuration = simout.yout{1}.Values.Data(i,:);
    waitfor(rateCtrlObj);
end

コア マニピュレーター ダイナミクスの追加とコントローラーの設計

スケジューラの設計と検証が完了したので、次の 2 つの要素をもつロボットのコントローラーを追加します。

• ジョイント トルクとグリッパー コマンドを受け入れる、より複雑なマニピュレーター ダイナミクス モデル

• 目的のマニピュレーター状態と現在のマニピュレーター状態を与えられてジョイント トルクを返す、ジョイント空間コントローラー

追加したコントローラーを使用して、次に提供されているモデルを開きます。

open_system('modelWithControllerAndBasicRobotDynamics.slx');

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

コントローラーの設計が目的の場合、マニピュレーター ダイナミクスは、トルク入力を与えられたマニピュレーターのジョイント位置を表す必要があります。これは、Manipulator Dynamics サブシステム内の Forward Dynamics ブロックを使用して、現在の状態におけるジョイント トルクをジョイント加速度に変換してから、2 回積分して総合的なジョイント コンフィギュレーションを得ることによって実現されます。積分器は q0 および dq0、つまりジョイントの初期位置および初期速度に初期化されます。

さらに、グリッパー制御サブシステムは、グリッパー アクチュエータに対するジョイント制御トルクを、グリッパーを開閉するために加える力 10 N でオーバーライドします。

2 番目の積分器が飽和していることに注意してください。

適切に設計された位置コントローラーの下のマニピュレーターは、通常、ジョイント制限に達しませんが、グリッパー アクションから開ループ力を追加すると、現実的な応答を確保するためにジョイント制限が必要になることを意味します。モデルの精度を高めるには、ジョイント飽和を速度と結び付けて積分をリセットすることもできますが、このモデルではこのレベルの精度で十分です。

グリッパー センサー

このモデルではさらに、より詳細なグリッパー センサーを追加して、グリッパーが実際に開かれたか閉じられたかをチェックします。グリッパー センサーはジョイント コンフィギュレーションの最後の 2 つの値 (グリッパー位置に対応する値) を抽出し、MATLAB Function ブロック Gripper Logic で、closeGripper コマンドによって与えられる目的のグリッパー位置と比較します。"Gripper Status" は、グリッパー ジョイントの位置が closeGripper コマンドで与えられている目的の状態と一致する場合に 1 を返します。グリッパーがそれらの状態にまだ達していない場合、Gripper Status はゼロを返します。これは、前述の簡略化モデル内の Gripper Model の動作と一致します。

ジョイント空間コントローラー

このモデルは、モデルベースのアプローチをジョイント制御に実装する計算トルク コントローラーも追加します。詳細については、ロボティクス マニピュレーター ブロックを使用した安全な軌跡追従制御の実行の例の「ロボティクス マニピュレーター ブロックを使用して計算トルク コントローラーを構築」を参照してください。このモデルは同じコントローラーを使用しますが、Rethink Sawyer ではなく、ABB YuMi を rigidBodyTree の入力に使用します。

モデルのシミュレーション

新しいモデルを使用してシミュレーションを行い、結果を可視化します。

simout = sim('modelWithControllerAndBasicRobotDynamics.slx');

% Visualize the motion using the interactiveRigidBodyTree
iviz.ShowMarker = false;
iviz.showFigure;
rateCtrlObj = rateControl(length(simout.tout)/(max(simout.tout)));
for i = 1:length(simout.tout)
    iviz.Configuration = simout.yout{1}.Values.Data(i,:);
    waitfor(rateCtrlObj);
end

ロボットおよび環境の Simscape モデルでのワークフロー全体の検証

タスク スケジューラとコントローラーの設計が完了したので、より複雑なロボットと環境のモデルを追加します。物理システムの高忠実度モデルを作成できる Simscape Multibody™ を使用します。このアプリケーションで、Simscape は、ジョイント制限と接触のモデル化が組み込まれたダイナミクスを追加します。この最後の手順によってシミュレーションの精度が向上しますが、代償としてモデル化が複雑になり、シミュレーション速度が低下します。Simscape では、組み込みの可視化機能である "Mechanics Explorer" も提供されており、シミュレーション中およびシミュレーション後に表示できます。

最後の提供モデルを読み込みます。上面ビューは同じです。

open_system('modelWithSimscapeRobotAndEnvironmentDynamics.slx');

Simscape のロボットと環境プラント

前のモデルとの主な違いはプラント モデルです。前のモデルのコア マニピュレーター ダイナミクスが、ロボットおよび環境用の Simscape モデルに置き換えられています。

マニピュレーターと環境のダイナミクス

マニピュレーターと環境は、Simscape Multibody を使用して構築されます。ロボット モデルは、ロボットの URDF ファイル上で、提供されているメッシュと共に smimport を呼び出すことによって作成されました。次に、muxes タグと GoTo タグ経由でリンクすることにより、ジョイント トルクでジョイントが作動し、ジョイントの位置、速度および加速度を返すセンサーが装備されました。

このシミュレーションでは目標物 (ウィジェット) が実際にピックアップされるため、ウィジェット サイズを定義します。

widgetDimensions = [0.02 0.02 0.07];

接触モデル

このモデルの接触は次の 2 つのカテゴリに分けられます。

• グリッパーとウィジェットとの接触

• ウィジェットと環境との接触

いずれの場合も、表面と表面の直接接触の代わりに "接触プロキシ" が使用されます。接触プロキシを使用すると、モデル化が高速化され、パフォーマンスが向上します。グリッパーとウィジェットの接触の場合、グリッパーの接触は 2 つのブリック状の立体を使用してモデル化され、ウィジェットの界面は 8 つの球面接触を使用してモデル化されます。同様に、ウィジェットと環境の接触では、環境を表すブリック状の立体と接触する、ウィジェットの四隅の球面を使用します。

既定の状態に近付くように、接触モデルのパラメーターを定義します。

% Contact parameters
stiffness = 1e4;
damping = 30;
transition_region_width = 1e-4;
static_friction_coef = 1;
kinetic_friction_coef = 1;
critical_velocity = 1;

グリッパーの制御と検出

Gripper Control は同じですが、Gripper Sensor は変更されています。このグリッパーは実際に目標物をピックアップできるため、握力の高い場合にグリッパーは閉じた状態に達します。実際に閉じた位置に達することはありません。したがって、左右両方のグリッパーの反力がしきい値を超えた場合に true となる値 isGrippingObj を返すロジックが追加されました。MATLAB Function ブロック Gripper Logic は、この変数を入力として受け入れます。

モデルのシミュレーション

ロボットをシミュレートします。複雑度が高いため、これには数分かかる場合があります。

simout = sim('modelWithSimscapeRobotAndEnvironmentDynamics.slx');

Mechanics Explorer を使用して、シミュレーション中およびシミュレーション後のパフォーマンスを可視化します。

拡張性

この例では、ピックアンドプレース アプリケーション向けのスケジューリングおよび制御システムの設計に重点を置いてきました。さらなる調査項目として、コントローラーでのサンプリングの効果、Simscape Multibody を使用した予期せぬ接触の影響、ロボットが使用する電気モーターの動作を詳細に説明するようなマルチドメイン モデルへの拡張などがあります。