火星探査機のモデル化と制御

この例では、Simscape MultibodyおよびRobotics System Toolboxを使用して、サンプル取得タスクを実行する火星探査機をモデル化します。探査機は、硬い地形の表面で目的のパスを追従してターゲット位置で停止し、マニピュレーターを使用して表面からサンプルを採取して格納します。さまざまな応用の側面をモデル化するために、次の主要な機能を使用します。

• Grid Surface は硬い地形の表面をモデル化します。(Simscape™ Multibody™ のライセンスが必要)

• Point CloudおよびSpatial Contact Force は探査機の車輪と硬い地形の間の接触をモデル化します。(Simscape Multibody のライセンスが必要)

• Pure Pursuit (Robotics System Toolbox) は探査機のパス追従制御で使います。(Robotics System Toolboxのライセンスが必要)

• KinematicsSolver (Simscape Multibody のライセンスが必要) およびTrapezoidal Velocity Profile Trajectory (Robotics System Toolbox) (Robotics System Toolbox のライセンスが必要) は探査機のアームについてのジョイント空間の軌跡の計画と制御で使います。

• ジョイント モード構成 (Simscape Multibody のライセンスが必要) はエンドエフェクタとサンプルの間の相互作用をモデル化します。モード構成の詳細については、Revolute Jointブロックの [Mode Configuration] セクションを参照してください。

Mars Rover モデル

この例で説明しているサブシステムを確認するには、モデル sm_mars_rover.slx を参照してください。

火星探査機のアニメーション

Rover Plant Model and Control

このサブシステムは、システムの側面について、探査機、硬い地形の表面、およびパスの計画と制御をモデル化したものです。

Rover サブシステム

このサブシステムでは、シャシー、ロッカー ボギー サスペンション、車輪など、探査機のさまざまなコンポーネントをモデル化しています。ジオメトリの CAD パーツは、File Solidを使用して Simscape Multibody にインポートされます。

探査機のアクチュエータは、速度制御用に 6 つの車輪のそれぞれに取り付けられたトルク作動の 6 つの回転ジョイント、およびステアリングに使用される四隅の車輪の上部に取り付けられたトルク作動の 4 つの回転ジョイントに対応します。これに加え、サスペンション機構の主要コンポーネントである差動アーム、ロッカー、およびボギーの 3 つについてもモデル化しています。

車輪と硬い地形の間の接触は、Point CloudとGrid Surfaceの接触のペアをSpatial Contact Forceブロックと共に使用してモデル化しています。各車輪のグラウザの点は、Point Cloudブロックを使用して作成しています。

Rigid Terrain Surface

火星の表面をモデル化するために、Grid Surfaceブロックを使用して硬い地形を作成しています。STL ファイルからグリッド表面を作成するために必要なパラメーターの設定については、ファイル rover_rigid_terrain_params.m を参照してください。

Rover Path Planning and Control

このサブシステムでは、探査機のパス追従制御システムをモデル化しています。パスは、探査機が経由する目的の位置を示す X-Y 平面上の順序付きのウェイポイントで構成されます。これらのウェイポイントは高水準のパス プランナーから提供されるものとし、障害物のない探査機のパスを表すと仮定されます。これらのウェイポイントは roverDesiredPath.mat ファイルを使用して読み込むことができます。

このサブシステムの目的は、まず目的のパスと目的のシャシー線形速度に追従するために必要なステアリング角度と必要な車輪角速度を計算し、次にそれらのステアリング角度と車輪角速度を達成するために必要なアクチュエータ トルクを計算することです。

パス追従コントローラーの開発では、次の点が考慮されています。

• 火星探査機は一般に前進速度が低速 (cm/s 程度) であると想定されるため、運動のダイナミクスは無視し、運動学的方程式のみを使用して制御の問題にアプローチします。[1]

• 運動学の定式化を単純化するため、探査機は平面上を移動するものと見なします。[1]

• 探査機の四隅の車輪は独立したステアリングをもち、探査機でアッカーマン ステアリングを実行できます。この機能に基づいて、探査機はアッカーマン ステアリングを使用するものと見なします。

• アッカーマン ステアリング ジオメトリは、等価の旋回半径をもつ 2 次元の幾何学的二輪モデルを仮定して単純化されています。この単純化のために、それぞれの車輪のペアを中央にある単一の車輪と探査機の中心の旋回半径に対応する単一のステアリング角度で表しています。[1][2]

• 前輪と後輪は対称的にステアリングされるものと見なします。

• 車輪は滑らずに回転するものと仮定します。

上記の考慮事項に基づいて、6 輪の探査機を幾何学的二輪モデルで同等に表現できます。[1]

パス追従にはPure Pursuit (Robotics System Toolbox)が使用されます。これは、ロボットを現在位置からロボットの前方にある前方注視点に達するまで動かすのに必要なターゲットの方向角 ($\alpha$) を計算する幾何学的アルゴリズムです。[1]

ステアリング角度の定式化

探査機の四隅の各車輪のステアリング角度は 2 ステップで導出されます。最初に、幾何学的二輪モデルとターゲットの方向角 ($\alpha$) (単純追跡コントローラーから提供) を使用して、次のように二輪のステアリング角度 ($\delta$) と旋回半径 ($\mathit{R}$) を求めます。[2]

$\delta ={\tan }^{-1}\left(\frac{2\mathit{L}\sin \left(\alpha \right)}{{\mathit{L}}_{\mathit{d}}}\right)$

$\mathit{R}=\frac{\mathit{L}}{\tan \left(\delta \right)}$

ここで、

$$\begin{array}{l}\alpha :\mathrm{Target}\mathrm{Dir}\mathrm{Angle}\\ \delta :\mathrm{Bicycle}\mathrm{Steering}\mathrm{Angle}\\ \mathit{L}:\mathrm{Bicycle}\mathrm{Length}\\ {\mathit{L}}_{\mathit{d}}:\mathrm{Look}\mathrm{ahead}\mathrm{distance}\mathrm{for}\mathrm{Pure}\mathrm{Pursuit}\\ \mathit{R}:\mathrm{Turn}\mathrm{radius}\mathrm{of}\mathrm{rover}\mathrm{center}\end{array}$$

次に、計算された二輪のステアリング角度 ($\delta$) と探査機の旋回半径 ($\mathit{R}$) に基づいて、アッカーマン ステアリング ジオメトリを使用して個々のステアリング角度 (${\delta }_{\mathrm{LF}},{\delta }_{\mathrm{LB}},{\delta }_{\mathrm{RF}},{\delta }_{\mathrm{LB}}$) を求めます。[3]

$${\delta }_{\mathrm{LF}}={\tan }^{-1}\left(\frac{\mathit{L}}{\mathit{R}-\mathit{d}}\right)$$

$${\delta }_{\mathrm{RF}}={\tan }^{-1}\left(\frac{\mathit{L}}{\mathit{R}+\mathit{d}}\right)$$

${\delta }_{\mathrm{LB}}$ = -${\delta }_{\mathrm{LF}}$

${\delta }_{\mathrm{RB}}$ = -${\delta }_{\mathrm{RF}}$

ここで、

$$\begin{array}{l}{\delta }_{\mathrm{LF}},{\delta }_{\mathrm{LB}},{\delta }_{\mathrm{RF}},{\delta }_{\mathrm{LB}}:\mathrm{Steering}\mathrm{angles}\mathrm{of}\mathrm{respective}\mathrm{corner}\mathrm{wheels}\\ \mathit{L}:\mathrm{Bicycle}\mathrm{Length}\left(0.5*\mathrm{Chassis}\mathrm{Length}\right)\\ \mathit{d}:0.5*\mathrm{Chassis}\mathrm{Width}\\ \mathrm{ICR}:\mathrm{Instantenous}\mathrm{Center}\mathrm{of}\mathrm{Rotation}\end{array}$$

車輪角速度の定式化

さらに、アッカーマン ステアリング ジオメトリと旋回半径 ($\mathit{R}$) に基づいて、次のようにシャシー速度 ($\mathrm{Vc}$) と車輪角速度 ($\omega$) の関係を求めます。[3]

$${\omega }_{\mathrm{LF}}=\frac{{\mathit{V}}_{\mathit{C}}}{{\mathit{R}}_{\mathit{w}}}*\frac{\left(\sqrt{{\mathit{L}}^2+{\left(\mathit{R}-\mathit{d}\right)}^2}\right)}{\mathit{R}}$$

$${\omega }_{\mathrm{RF}}=\frac{{\mathit{V}}_{\mathit{C}}}{{\mathit{R}}_{\mathit{w}}}*\frac{\left(\sqrt{{\mathit{L}}^2+{\left(\mathit{R}+\mathit{d}\right)}^2}\right)}{\mathit{R}}$$

${\omega }_{\mathrm{LM}}=\frac{{\mathit{V}}_{\mathit{C}}}{{\mathit{R}}_{\mathit{w}}}*\frac{\left(\mathit{R}-\mathit{d}\right)}{\mathit{R}}$

$${\omega }_{\mathrm{RM}}=\frac{{\mathit{V}}_{\mathit{C}}}{{\mathit{R}}_{\mathit{w}}}*\frac{\left(\mathit{R}+\mathit{d}\right)}{\mathit{R}}$$

${\omega }_{\mathrm{LB}}={\omega }_{\mathrm{LF}}$

${\omega }_{\mathrm{RB}}={\omega }_{\mathrm{RF}}$

ここで、

$$\begin{array}{l}{{\omega }_{\mathrm{LF}},{\omega }_{\mathrm{LM}},{\omega }_{\mathrm{LB}},{\omega }_{\mathrm{RF}},{\omega }_{\mathrm{RM}},{\omega }_{\mathrm{RB}}}_{}:\mathrm{Angular}\mathrm{speeds}\mathrm{of}\mathrm{respective}\mathrm{wheels}\\ \mathrm{Vc}:\mathrm{Linear}\mathrm{speed}\mathrm{of}\mathrm{the}\mathrm{rover}\mathrm{chassis}\\ {\mathit{R}}_{\mathit{w}}:\mathrm{Radius}\mathrm{of}\mathrm{the}\mathrm{wheel}\end{array}$$

目的のパスと目的のシャシー線形速度に対するステアリング角度と車輪角速度の両方を定式化したら、PID コントローラーを使用してアクチュエータ (回転ジョイント) の実際のステアリング角度と角速度を目的の値まで駆動します。

探査機のシミュレーション結果:パス 1

探査機が 0.3 m/s 以下で不整地を移動するときのパス 1 の結果。これ以外の数量については、モデルの Rover Sensing サブシステムで確認できます。

探査機のシミュレーション結果:パス 2

探査機が 0.3 m/s 以下で不整地を移動するときのパス 2 の結果。これ以外の数量については、モデルの Rover Sensing サブシステムで確認できます。

Manipulator Plant Model and Control

このサブシステムは、表面からサンプルを採取して収集するための探査機のロボット アームとその軌跡の計画と制御をモデル化したものです。

Rover Arm

マニピュレーターは、シャシーのフロント エンドに取り付けられた 6-DOF アームとしてモデル化されています。そのアクチュエータはトルク作動の 6 つの回転ジョイントに対応します。符号化器のようなセンサーを再現するために、サブシステムは 6 つの回転ジョイントのそれぞれからジョイント角度を出力します。エンドエフェクタとサンプルの間の相互作用には、"ジョイント モード切り替え" を利用した簡易モデルを使用しています。エンドエフェクタがサンプルに十分に近づくと、それらを接続する最初は解放されていた 6-DOF ジョイントが係合されます。この 6-DOF ジョイントは位置範囲が狭く、サンプルはエンドエフェクタにほぼ拘束された状態になります。サンプルを含むエンドエフェクタがサンプルの格納場所に十分に近づくと、エンドエフェクタとサンプルの間の係合された 6-DOF ジョイントが解放され、サンプルと格納場所の間の最初は解放されていた 6-DOF ジョイントが係合されます。この 6-DOF ジョイントも位置範囲が狭く、サンプルはシャシーの格納場所にほぼ拘束された状態になります。

Planning and Control

マニピュレーターの Planning and Control サブシステムは、探査機がターゲット位置で停止すると有効になります。探査機が停止すると、アームのベースに対するサンプルの相対的な位置が Transform Sensor ブロック (搭載されたカメラを再現) を使用して計算されます。

エンドエフェクタの軌跡を計画するために、上記のようにタスク空間で 6 つのウェイポイントが定義されています。これらのウェイポイントの 4 つ (1、2、5、6) は、探査機のシャシーのジオメトリと格納ユニットの位置から事前に計算して導出したものです。これらは roverArmTaskSpaceConfig.mat から読み込むことができます。ウェイポイント 3 と 4 は、Transform Sensor ブロックから得られるサンプルの位置に基づいて計算されます。ジョイント空間の軌跡の計画法で、これらの 6 つのウェイポイントを使用してエンドエフェクタの軌跡が計画されます。プランナーは、まず、逆運動学モジュールを使用して、タスク空間の 6 つのすべてのウェイポイントをジョイント空間のウェイポイントに変換します (KinematicsSolverを使用して実装される sm_mars_rover_arm_ik.m を参照)。その後、それらのジョイント空間のウェイポイントを MATLAB Function ブロック Task Scheduler で使用して、上記の一連のモードの順にアームを進めます。スケジューラは、エンドエフェクタが特定の許容誤差値内でターゲット位置に達するとエンドエフェクタを次のモードに進めます。スケジューラの各タスクをTrapezoidal Velocity Profile Trajectory (Robotics System Toolbox)ブロックに入力として渡して、ジョイント空間の各ウェイポイント間の滑らかな軌跡が計算されます。

ジョイント空間の軌跡が生成されたら、PID コントローラーを使用してアクチュエータの実際の位置を目的の値まで駆動します。

マニピュレーターのシミュレーション結果

目的のウェイポイントを経由するエンドエフェクタの軌跡、およびアクチュエータの目的のジョイント角度と実際のジョイント角度の比較を示す結果。これ以外の数量については、モデルの Manipulator Sensing サブシステムで確認できます。

参考文献

[1] Filip, Jan, Martin Azkarate, and Gianfranco Visentin. "Trajectory control for autonomous planetary rovers."In 14th Symposium on Advanced Space Technologies in Robotics and Automation (ASTRA). 2017.

[2] Snider, Jarrod M. "Automatic steering methods for autonomous automobile path tracking."Robotics Institute, Pittsburgh, PA, Tech. Rep. CMU-RITR-09-08 (2009).

[3] X. Wu, L. Yang and M. Xu, "Speed following control for differential steering of 4WID electric vehicle," IECON 2014 - 40th Annual Conference of the IEEE Industrial Electronics Society, 2014, pp. 3054-3059, doi: 10.1109/IECON.2014.7048945.

参考

KinematicsSolver | Grid Surface | Point Cloud