順運動学と逆運動学を使用したピックアンドプレースロボット

モデルを開く

この例では、ピックアンドプレースタスクを実行するデルタロボットをモデル化します。このロボットは、真空グリッパーを使用してパーツを拾い上げ、テーブル上の 4 つのマーカーそれぞれに向けパーツを動かし、最初のマーカーでパーツを落としてから、ホーム位置に戻ります。この例では以下を行う方法を説明します。

KinematicsSolverオブジェクトを作成し、それらを MATLAB Function ブロック経由で呼び出して、シミュレーション中の順運動学と逆運動学を計算する。
Spatial Contact Forceブロックを使用して接触をモデル化する。

モデル

Delta Robot サブシステム

Delta Robot サブシステムは、3-DOF デルタロボットをモデル化します。ロボットの運動学的構造により、エンドエフェクタの運動は純粋に並進です。ロボットのアクチュエータは、上部基板に取り付けられている、トルクによって作動する 3 つの回転ジョイントに対応します。エンコーダーのデータを再現するために、サブシステムはアクチュエータの位置 (角度) を出力します。基板の下にカメラフレームが取り付けられ、エンドエフェクタに向かって見下ろします。接触をモデル化しやすくするために、エンドエフェクタに関連するジオメトリが Simscape バス経由でエクスポートされます。詳細については、ブロックマスクを参照してください。

Delta Robot サブシステムを開く

Planning and Control サブシステム: 順運動学と逆運動学

エンドエフェクタの軌跡の計画は、ロボットのカメラフレームの xyz 座標を基準として行われるため、アクチュエータの位置と速度をエンドエフェクタの位置と速度に変換するには、順運動学マップが必要です。同様に、プランナーによって計算されたエンドエフェクタの目的の位置を、3 つのアクチュエータの対応する位置に変換するには、逆運動学マップが必要です。これらの順運動学および逆運動学の計算は、KinematicsSolverオブジェクトを使用して行われます。オブジェクトは、関数 sm_pick_and_place_robot_fk および関数 sm_pick_and_place_robot_ik での永続変数として定義されます。これらの関数は、以下で強調表示されている MATLAB Function ブロック Planning and Control/Forward Kinematics および Planning and Control/Inverse Kinematics によって呼び出されます。計算速度を上げて、KinematicsSolver オブジェクトが逆運動学の問題の目的の解を確実に求められるようにするために、以前の解を現在の問題の初期推定として使用します。Delta Robot サブシステムのパラメーターが変わるたびに、関数 sm_pick_and_place_robot_fk と関数 sm_pick_and_place_robot_ik はメモリからクリアされ、KinematicsSolver オブジェクトは次のシミュレーションの最初に再生成されます。これにより、KinematicsSolver オブジェクトとモデルの同期が確実に維持されます。

Planning and Control サブシステムを開く

Planning and Control サブシステム: パスプランナー

計画は、以下で強調表示されている MATLAB Function ブロック Planning and Control/Path Planner で行われます。プランナーは、3 つの異なるモード間でロボットを遷移させます。

パーツの真上の位置まで移動する
パーツをつかんでゴール位置まで移動する
ホームに戻る

1 つのモードが始まるたびに、エンドエフェクタを現在の位置からモードのゴール位置まで一定時間内に移動させる軌跡が計算されます。軌跡は 2 段階で生成されます。まず、現在の位置からゴール位置までのエンドエフェクタのパスに対応する 3 次多項式が xyz カメラ座標で計算されます。次に、初期と最終の速度および加速度がすべてゼロになるようなパスに沿った時間のスケーリングに使用される、5 次多項式が計算されます。エンドエフェクタの位置と速度が目標値に十分に近付くと、モードの遷移が発生します。現在の時間が与えられると、プランナーは、軌跡に沿ったエンドエフェクタの目的とする位置と速度、および目的とする真空の状態を返します。

Planning and Control サブシステムを開く

Planning and Control サブシステム: コントローラー

以下で強調表示されている Planning and Control/Controller サブシステムには、アクチュエータの実際の位置を目的の値へと駆動する、離散時間 PID コントローラーが含まれています。

Planning and Control サブシステムを開く

Vacuum サブシステム

パーツをつかむために、パーツとロボットのエンドエフェクタの間で単純な真空がモデル化されます。プランナーが吸引を命令するたびに、パーツの重心とエンドエフェクタの先端との間に、真空によって一定の力が加えられます。

Vacuum サブシステムを開く

接触サブシステム

End Effector-Part Contact Forces サブシステムおよび Part-Table Contact Forces サブシステム内の Spatial Contact Force ブロックは、接触をモデル化するために使用されます。シミュレーションを高速化するために、エンドエフェクタの先端の周囲に等間隔で配置された 3 つ接触点を、パーツと接触する際の完全な円柱ジオメトリのプロキシとして使用します。同様に、パーツの下端の周囲に等間隔で配置された 3 つの接触点を、パーツがテーブルと接触する際の接触プロキシとして使用します。真空力によってパーツがエンドエフェクタと接触状態に保たれ、摩擦によって移送中にパーツが滑らなくなります。

End Effector-Part Contact Forces サブシステムを開く

Part-Table Contact Forces サブシステムを開く