宇宙ステーションの外壁に沿ってロボットアームを滑らせながら、壁面を一定の力で磨く作業を想像してみてください。壁に平行な方向では「どこまで移動するか」を正確に制御したい一方、壁に垂直な方向では「どのくらいの力で押し付けるか」を制御したい — つまり、方向ごとに位置制御と力制御を使い分ける必要があります。
もし全方向を位置制御だけで制御すると、壁との微小な位置ずれが巨大な接触力を生み出し、機器やロボットを破損しかねません。かといって全方向を力制御にすると、壁に沿った移動の精度が保てず、磨き残しや作業漏れが生じます。この「位置と力の両立」という根本的な問題を解決するのが、ハイブリッド力/位置制御（Hybrid Force/Position Control）です。
ハイブリッド力/位置制御は、以下のような場面で威力を発揮します。
- 軌道上修理・メンテナンス: 宇宙ステーション外壁の研磨、ボルト締め付けなど、拘束面に沿った精密作業
- パネル挿入・組立: 太陽電池パネルを壁面に沿ってスライドさせ、スロットに差し込む作業
- 地上産業用途: 研磨、バリ取り、組立ライン上の嵌合作業
本記事の内容
- 拘束フレームと自然拘束・人工拘束の概念
- 選択行列 $\bm{S}$ による作業空間の分割
- Raibert-Craig のハイブリッド制御則の導出
- 安定性の解析
- インピーダンス制御との比較と使い分け
- 宇宙環境での具体的応用例
- Python シミュレーションによる拘束面に沿った作業の再現
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
なぜハイブリッド制御が必要か
ロボットが環境に接触しない自由空間での作業であれば、純粋な位置制御で十分です。目標軌道を設定し、関節トルクでその軌道を追従させれば、タスクは完了します。しかし、ロボットが壁や対象物に接触した瞬間、状況は一変します。
接触がある場面では、ロボットのエンドエフェクタの運動は環境からの拘束を受けます。たとえば、テーブルの上に手を押し付けているとき、手はテーブル面に垂直な方向には自由に動けません。テーブルが手の動きを拘束しているからです。この状態で「テーブル面から 1mm 下に移動せよ」という位置指令を出すと、ロボットはテーブルを突き破ろうとして巨大な力が発生し、機器の破損につながります。
一方、テーブルに平行な方向では手は自由に動けます。この方向では位置指令が意味を持ちます。つまり、拘束面に垂直な方向では力を制御し、拘束面に平行な方向では位置を制御するのが自然な発想です。
この直感を数学的に定式化したのが、1981 年に Raibert と Craig が提案したハイブリッド力/位置制御です。彼らの核心的なアイデアは、作業空間を「位置を制御すべき部分空間」と「力を制御すべき部分空間」に直交分解し、それぞれに独立な制御器を配置するというものです。
では、この「空間の分割」をどのように数学的に表現すればよいのでしょうか。その鍵となるのが拘束フレームの概念です。
拘束フレームの概念
自然拘束と人工拘束
ロボットが環境に接触しているとき、タスク空間（通常は 6 次元: 3 次元の並進 + 3 次元の回転）における各方向の変数は、「位置/速度」と「力/トルク」のいずれか一方のみが独立に指定可能です。この原則を理解するために、自然拘束（Natural Constraints）と人工拘束（Artificial Constraints）の概念を導入します。
まず、自然拘束とは環境の物理的な幾何形状から自動的に決まる拘束です。テーブルに手を押し付けている例で考えましょう。テーブル面を $xy$ 平面とし、法線方向を $z$ 軸とします。
- $z$ 方向の速度: テーブルが剛体であれば、手は $z$ 方向に動けません。つまり $v_z = 0$ が自然拘束として課されます
- $x$, $y$ 方向の力: 摩擦がなければ、テーブルは $x$, $y$ 方向の力をエンドエフェクタに及ぼしません。つまり $f_x = 0$, $f_y = 0$ が自然拘束です
このように、環境がある方向の運動を拘束するとき、その方向には速度の自然拘束（$v = 0$）が生じ、直交する方向には力の自然拘束（$f = 0$）が生じます。
次に、人工拘束とは制御設計者が意図的に指定する目標値です。自然拘束が課されていない各方向に対して、設計者は以下のように目標を設定します。
- $z$ 方向の力: 速度が拘束されている方向に対して、接触力の目標値 $f_z^d$ を設定します（例: 10 N で壁を押す）
- $x$, $y$ 方向の位置/速度: 力が拘束されている方向に対して、位置の目標軌道 $x^d(t)$, $y^d(t)$ を設定します（例: 直線経路に沿って移動する）
重要なポイントは、ある方向で位置と力の両方を同時に独立に指定することはできないということです。これは物理的に自然な制約で、剛体壁に押し付けた手の $z$ 方向の位置はすでに壁の位置で決まっており、自由に指定できるのは力だけです。逆に、壁に平行な方向の力は接触が摩擦なしなら既にゼロであり、自由に指定できるのは位置だけです。
拘束フレームの定義
自然拘束と人工拘束を体系的に記述するために、拘束フレーム（Constraint Frame）を導入します。
拘束フレームとは、タスクの幾何学的性質に合わせて定義される座標系です。典型的には、接触面の法線方向に 1 軸を合わせ、接触面に平行な方向に残りの軸を配置します。この座標系で見ると、各軸方向がきれいに「位置制御すべき方向」と「力制御すべき方向」に分離されます。
たとえば、傾斜面に沿って研磨する作業を考えます。ワールド座標系 $\{W\}$ で見ると位置制御と力制御の方向が混在しますが、傾斜面に合わせた拘束フレーム $\{C\}$ を定義すれば、法線方向が力制御、接線方向が位置制御とすっきり分離できます。
拘束フレーム $\{C\}$ とワールドフレーム $\{W\}$ の間の関係は回転行列 $\bm{R}_{WC}$ で表されます。拘束フレーム内での力ベクトル $\bm{f}_C$ やスパフレーム内での速度ベクトル $\bm{v}_C$ は、この回転行列を通じてワールドフレームの量に変換できます。
$$ \bm{f}_W = \bm{R}_{WC} \bm{f}_C, \quad \bm{v}_W = \bm{R}_{WC} \bm{v}_C $$
ここまでで、拘束面に合わせた座標系を導入することで、各方向の制御モード（位置 or 力）を明確に分離できることがわかりました。次に、この分離を数学的にエレガントに表現する選択行列を導入します。
選択行列 $\bm{S}$ による空間分割
選択行列の定義
拘束フレームで見たとき、各方向が「位置制御」か「力制御」かを指定する道具が、選択行列（Selection Matrix）$\bm{S}$ です。
直感的には、$\bm{S}$ は「この方向は力制御」というスイッチのようなものです。6 自由度の場合、$\bm{S}$ は $6 \times 6$ の対角行列で、各対角要素は 0 か 1 のいずれかを取ります。
$$ \bm{S} = \mathrm{diag}(s_1, s_2, s_3, s_4, s_5, s_6), \quad s_i \in \{0, 1\} $$
各要素の意味は以下の通りです。
- $s_i = 1$: 第 $i$ 方向は力制御を行う
- $s_i = 0$: 第 $i$ 方向は位置制御を行う
この定義により、$\bm{S}$ は力制御方向への射影演算子として機能し、$\bm{I} – \bm{S}$ は位置制御方向への射影演算子として機能します。ここで $\bm{I}$ は $6 \times 6$ の単位行列です。
直交補空間としての性質
選択行列の最も重要な性質は、タスク空間を 2 つの直交補空間に分割することです。
$$ \bm{S} + (\bm{I} – \bm{S}) = \bm{I} $$
これは当然の恒等式ですが、物理的には深い意味があります。タスク空間の任意のベクトル（位置ベクトルでも力ベクトルでも）を、力制御方向の成分と位置制御方向の成分に完全に分解できるということです。
力のベクトル $\bm{f}$ に $\bm{S}$ を作用させると、力制御方向の力成分だけが残ります。
$$ \bm{f}_{\text{force}} = \bm{S} \bm{f} $$
位置のベクトル $\bm{x}$ に $(\bm{I} – \bm{S})$ を作用させると、位置制御方向の位置成分だけが残ります。
$$ \bm{x}_{\text{position}} = (\bm{I} – \bm{S}) \bm{x} $$
さらに、射影演算子の基本的な性質として以下が成り立ちます。
$\bm{S}$ のべき等性を確認します。$\bm{S}$ は対角成分が 0 か 1 の対角行列なので、
$$ \bm{S}^2 = \bm{S} $$
が成り立ちます。同様に、
$$ (\bm{I} – \bm{S})^2 = \bm{I} – 2\bm{S} + \bm{S}^2 = \bm{I} – 2\bm{S} + \bm{S} = \bm{I} – \bm{S} $$
であり、$(\bm{I} – \bm{S})$ もべき等です。さらに、2 つの射影は直交しています。
$$ \bm{S}(\bm{I} – \bm{S}) = \bm{S} – \bm{S}^2 = \bm{S} – \bm{S} = \bm{O} $$
この直交性は、力制御と位置制御が互いに干渉しないことの数学的保証です。力制御ループの出力は位置制御方向には一切影響を与えず、位置制御ループの出力も力制御方向には一切影響を与えません。
具体例: 壁面研磨タスク
テーブル面（$xy$ 平面）に沿ってエンドエフェクタを移動させながら、一定の力で押し付ける研磨タスクを考えます。ワールドフレームと拘束フレームが一致していると仮定すると、タスク空間は 3 次元（$x$, $y$, $z$ の並進のみ）として簡略化でき、選択行列は次のようになります。
$$ \bm{S} = \begin{pmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 1 \end{pmatrix} $$
これは「$x$, $y$ 方向は位置制御（$s_1 = s_2 = 0$）、$z$ 方向は力制御（$s_3 = 1$）」を意味します。したがって、
$$ (\bm{I} – \bm{S}) = \begin{pmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 0 \end{pmatrix} $$
が位置制御方向の射影になります。位置目標 $\bm{x}^d = (x^d(t), y^d(t), \ast)^T$ の $x$, $y$ 成分が研磨軌道を定義し、力目標 $\bm{f}^d = (\ast, \ast, f_z^d)^T$ の $z$ 成分が押し付け力を定義します（$\ast$ は該当の制御器が使わないため任意）。
選択行列により作業空間を力制御と位置制御に分割する数学的枠組みが整いました。次に、この枠組みの上で実際にどのような制御則を設計するのか、Raibert-Craig のハイブリッド制御則を導出します。
ハイブリッド制御の制御則
作業空間のダイナミクス
制御則を導出する出発点として、エンドエフェクタの作業空間における運動方程式を記述します。ロボットの関節空間における一般的な運動方程式は次の通りです。
$$ \bm{M}(\bm{q})\ddot{\bm{q}} + \bm{C}(\bm{q}, \dot{\bm{q}})\dot{\bm{q}} + \bm{g}(\bm{q}) = \bm{\tau} + \bm{J}^T(\bm{q})\bm{f}_{\text{ext}} $$
ここで、$\bm{M}(\bm{q})$ は慣性行列、$\bm{C}(\bm{q}, \dot{\bm{q}})$ はコリオリ・遠心力項、$\bm{g}(\bm{q})$ は重力項、$\bm{\tau}$ は関節トルク、$\bm{J}(\bm{q})$ はヤコビ行列、$\bm{f}_{\text{ext}}$ は外力（環境からの接触力）です。
作業空間の座標 $\bm{x}$ と関節座標 $\bm{q}$ の関係は、ヤコビ行列を通じて次のように表されます。
$$ \dot{\bm{x}} = \bm{J}(\bm{q})\dot{\bm{q}} $$
この時間微分を取ると、
$$ \ddot{\bm{x}} = \bm{J}(\bm{q})\ddot{\bm{q}} + \dot{\bm{J}}(\bm{q})\dot{\bm{q}} $$
を得ます。関節空間の運動方程式をヤコビ行列の逆を使って作業空間に変換すると、作業空間での運動方程式が得られます。$\bm{J}$ が正則であると仮定し、$\ddot{\bm{q}} = \bm{J}^{-1}(\ddot{\bm{x}} – \dot{\bm{J}}\dot{\bm{q}})$ を代入すると、
$$ \bm{M}_x(\bm{x})\ddot{\bm{x}} + \bm{C}_x(\bm{x}, \dot{\bm{x}})\dot{\bm{x}} + \bm{g}_x(\bm{x}) = \bm{f}_{\text{cmd}} + \bm{f}_{\text{ext}} $$
ここで、$\bm{M}_x = \bm{J}^{-T}\bm{M}\bm{J}^{-1}$ は作業空間の慣性行列、$\bm{f}_{\text{cmd}} = \bm{J}^{-T}\bm{\tau}$ はエンドエフェクタに等価な指令力です。$\bm{C}_x$, $\bm{g}_x$ はそれぞれコリオリ項と重力項の作業空間表現です。
Raibert-Craig の制御則
Raibert と Craig（1981）は、上記の作業空間ダイナミクスに対して、選択行列を用いて位置制御器と力制御器を独立に設計する制御則を提案しました。
制御指令力 $\bm{f}_{\text{cmd}}$ を次のように構成します。
$$ \bm{f}_{\text{cmd}} = (\bm{I} – \bm{S})\bm{f}_{\text{pos}} + \bm{S}\bm{f}_{\text{force}} $$
第 1 項 $(\bm{I} – \bm{S})\bm{f}_{\text{pos}}$ は位置制御方向の指令力であり、位置制御器の出力を力制御方向に漏れないように射影しています。第 2 項 $\bm{S}\bm{f}_{\text{force}}$ は力制御方向の指令力であり、力制御器の出力を位置制御方向に漏れないように射影しています。
それぞれの制御器は以下のように設計します。
位置制御器（PD制御 + 重力補償 + 動力学補償）:
$$ \bm{f}_{\text{pos}} = \bm{M}_x \left[ \ddot{\bm{x}}^d + \bm{K}_P^{\text{pos}}(\bm{x}^d – \bm{x}) + \bm{K}_D^{\text{pos}}(\dot{\bm{x}}^d – \dot{\bm{x}}) \right] + \bm{C}_x \dot{\bm{x}} + \bm{g}_x $$
ここで $\bm{K}_P^{\text{pos}}$ は位置制御の比例ゲイン行列、$\bm{K}_D^{\text{pos}}$ は微分ゲイン行列です。$\ddot{\bm{x}}^d$ はフィードフォワード加速度です。動力学項 $\bm{M}_x$, $\bm{C}_x$, $\bm{g}_x$ を含めることで、理想的には非線形ダイナミクスを完全に補償し、位置誤差に関する線形の閉ループ応答を実現します。
力制御器（PI制御）:
$$ \bm{f}_{\text{force}} = \bm{f}^d + \bm{K}_P^{\text{force}}(\bm{f}^d – \bm{f}) + \bm{K}_I^{\text{force}} \int_0^t (\bm{f}^d – \bm{f}) \, d\tau $$
ここで $\bm{f}^d$ は目標力、$\bm{f}$ は力センサで測定した実際の接触力、$\bm{K}_P^{\text{force}}$ は力制御の比例ゲイン行列、$\bm{K}_I^{\text{force}}$ は積分ゲイン行列です。力制御器にはフィードフォワード項として $\bm{f}^d$ を含めています。積分項は定常偏差を除去するために必要です。環境の剛性が高い場合でも、目標力への収束を保証します。
制御則の全体構造
上記の位置制御器と力制御器を統合し、関節トルク指令に変換した全体像を整理します。
まず、拘束フレームでの制御指令を計算します。
$$ \bm{f}_{\text{cmd}} = (\bm{I} – \bm{S})\bm{f}_{\text{pos}} + \bm{S}\bm{f}_{\text{force}} $$
次に、この指令力を関節トルクに変換します。
$$ \bm{\tau} = \bm{J}^T(\bm{q}) \bm{f}_{\text{cmd}} $$
制御系のブロック図を言葉で表現すると、次のようになります。
- 拘束フレームで目標位置 $\bm{x}^d$ と目標力 $\bm{f}^d$ を設定する
- 位置制御器が位置誤差 $\bm{e}_x = \bm{x}^d – \bm{x}$ に基づいて $\bm{f}_{\text{pos}}$ を計算する
- 力制御器が力誤差 $\bm{e}_f = \bm{f}^d – \bm{f}$ に基づいて $\bm{f}_{\text{force}}$ を計算する
- 選択行列 $\bm{S}$ と $(\bm{I} – \bm{S})$ で適切な方向に射影し、合算する
- ヤコビ行列の転置で関節トルクに変換する
この構造の美しさは、位置制御器と力制御器が完全に独立に設計できる点です。$\bm{S}$ と $(\bm{I} – \bm{S})$ の直交性により、一方のゲイン調整が他方に影響を与えません。
ここまでで制御則の定式化が完了しました。しかし、この制御系は本当に安定なのでしょうか。設計したゲインのもとで位置誤差と力誤差が時間とともに収束することを保証する必要があります。次節で安定性を解析します。
安定性解析
位置制御方向の安定性
位置制御方向の閉ループ特性を調べます。理想的な動力学補償が実現されている場合、位置制御方向の誤差ダイナミクスは次のように線形化されます。
位置誤差を $\bm{e}_x = \bm{x}^d – \bm{x}$ と定義し、動力学補償が完全であると仮定すると、位置制御方向では次の関係が成立します。
$$ \ddot{\bm{e}}_x + \bm{K}_D^{\text{pos}} \dot{\bm{e}}_x + \bm{K}_P^{\text{pos}} \bm{e}_x = \bm{0} $$
これは 2 次の線形常微分方程式であり、$\bm{K}_P^{\text{pos}} > \bm{0}$（正定値）かつ $\bm{K}_D^{\text{pos}} > \bm{0}$（正定値）であれば、系は漸近安定です。
この安定条件を確認するために、ゲイン行列が対角的な場合を考えます。$\bm{K}_P^{\text{pos}} = \mathrm{diag}(k_{p,1}, \dots, k_{p,n})$、$\bm{K}_D^{\text{pos}} = \mathrm{diag}(k_{d,1}, \dots, k_{d,n})$ とすると、各方向 $i$ の特性方程式は次の通りです。
$$ s^2 + k_{d,i} s + k_{p,i} = 0 $$
この 2 次方程式の根が全て負の実部を持つ条件は、$k_{p,i} > 0$ かつ $k_{d,i} > 0$ です。これはフルビッツの安定判別法から直ちにわかります。
各方向について自然周波数 $\omega_{n,i}$ と減衰比 $\zeta_i$ を次のように設定できます。
$$ \omega_{n,i} = \sqrt{k_{p,i}}, \quad \zeta_i = \frac{k_{d,i}}{2\omega_{n,i}} = \frac{k_{d,i}}{2\sqrt{k_{p,i}}} $$
臨界減衰（$\zeta_i = 1$）を実現するには $k_{d,i} = 2\sqrt{k_{p,i}}$ と設定します。この場合、振動なく最速で目標値に収束します。
力制御方向の安定性
力制御方向の安定性は、環境のモデルに依存します。環境を剛性 $k_e$ のバネとしてモデル化するのが最も基本的なアプローチです。
環境との接触点における力と変位の関係を線形バネモデルで表します。
$$ f = k_e (x – x_e) $$
ここで $x_e$ は環境表面の位置です。力制御器として PI 制御を用いた場合、力の誤差 $e_f = f^d – f$ のダイナミクスを導出します。
簡単化のため 1 自由度で考えます。エンドエフェクタの質量を $m$、位置を $x$、環境剛性を $k_e$ とすると、力制御方向の運動方程式は次のようになります。
$$ m \ddot{x} = f_{\text{cmd}} – f $$
ここで $f$ は環境からの反力 $f = k_e(x – x_e)$ です。力制御器の出力を代入すると、
$$ m \ddot{x} = f^d + K_P^f (f^d – f) + K_I^f \int_0^t (f^d – f) \, d\tau – f $$
$f = k_e(x – x_e)$ を代入し、$f^d$ が一定であることを利用して整理します。$e_f = f^d – f = f^d – k_e(x – x_e)$ とすると、$\dot{e}_f = -k_e \dot{x}$ および $\ddot{e}_f = -k_e \ddot{x}$ です。
$\ddot{x} = -\ddot{e}_f / k_e$ を運動方程式に代入すると、
$$ -\frac{m}{k_e} \ddot{e}_f = (1 + K_P^f) e_f + K_I^f \int_0^t e_f \, d\tau $$
これを微分して 3 次の ODE に変換すると、
$$ \frac{m}{k_e} \dddot{e}_f + (1 + K_P^f) \dot{e}_f + K_I^f e_f = 0 $$
ラプラス変換して特性方程式を得ます。
$$ \frac{m}{k_e} s^3 + (1 + K_P^f) s + K_I^f = 0 $$
この 3 次方程式にラウスの安定判別法を適用すると、$s^2$ の係数が 0 であるため、安定性のためには追加の条件が必要となります。実用的には、ダンピング項（力誤差の微分ゲイン $K_D^f$）を加えた PID 力制御器を用いるか、環境モデルに粘性減衰 $b_e$ を含めることで安定化を図ります。
PID 力制御器を用いた場合の特性方程式は次のようになります。
$$ \frac{m}{k_e} s^3 + K_D^f s^2 + (1 + K_P^f) s + K_I^f = 0 $$
ラウスの安定判別法により、安定条件は次の通りです。
$$ K_D^f > 0, \quad 1 + K_P^f > 0, \quad K_I^f > 0, \quad K_D^f (1 + K_P^f) > \frac{m}{k_e} K_I^f $$
最後の条件は、積分ゲインが大きすぎると不安定になることを意味しています。直感的には、積分器の蓄積が過大な力変動を引き起こし、環境のバネ効果と相まって振動が発散するためです。環境剛性 $k_e$ が大きいほど（硬い壁ほど）$m/k_e$ が小さくなり、安定余裕が広がるという興味深い性質もあります。
動力学補償の不完全さへの対処
実際のシステムでは、動力学モデル $\bm{M}_x$, $\bm{C}_x$, $\bm{g}_x$ が完全には既知でないため、動力学補償に誤差が残ります。この場合、位置制御方向の誤差方程式にはモデル化誤差 $\bm{\delta}$ が残留項として現れます。
$$ \ddot{\bm{e}}_x + \bm{K}_D^{\text{pos}} \dot{\bm{e}}_x + \bm{K}_P^{\text{pos}} \bm{e}_x = \bm{\delta}(\bm{q}, \dot{\bm{q}}, \ddot{\bm{q}}) $$
$\bm{\delta}$ が有界であれば、位置誤差は有界な残留誤差の範囲内に収まります。ゲインを高くすることで残留誤差を小さくできますが、センサノイズの増幅やアクチュエータの飽和とのトレードオフが存在します。
安定性の条件が明らかになったところで、ハイブリッド力/位置制御の最大のライバルであるインピーダンス制御との違いを明確にしましょう。両者は接触タスクに対する異なるアプローチであり、使い分けの指針を理解することが実用上非常に重要です。
インピーダンス制御との比較
制御哲学の違い
インピーダンス制御とハイブリッド力/位置制御は、どちらもロボットと環境の接触を扱う制御手法ですが、根本的な哲学が異なります。
インピーダンス制御は、エンドエフェクタの動的な振る舞い（質量-バネ-ダンパ系）を目標インピーダンスに合わせる手法です。力は直接制御するのではなく、位置偏差と目標インピーダンスモデルの関係から間接的に決まります。つまり「柔らかさ」を設計する制御です。
$$ \bm{M}_d (\ddot{\bm{x}} – \ddot{\bm{x}}^d) + \bm{B}_d (\dot{\bm{x}} – \dot{\bm{x}}^d) + \bm{K}_d (\bm{x} – \bm{x}^d) = \bm{f}_{\text{ext}} $$
ハイブリッド力/位置制御は、作業空間を位置制御と力制御に明確に分割し、各部分空間で独立に制御する手法です。力は目標値に直接追従するよう制御されます。つまり「どの方向で何を制御するか」を明示的に設計する制御です。
$$ \bm{f}_{\text{cmd}} = (\bm{I} – \bm{S})\bm{f}_{\text{pos}} + \bm{S}\bm{f}_{\text{force}} $$
力の制御精度
インピーダンス制御では、定常状態における接触力は環境剛性 $k_e$ と目標インピーダンスのバネ定数 $K_d$ の関係で決まります。
$$ f_{\text{steady}} = \frac{k_e K_d}{k_e + K_d} \Delta x $$
ここで $\Delta x$ は環境表面の位置と目標位置の差です。環境剛性が既知でなければ、正確な力を実現できません。
一方、ハイブリッド制御では力を直接フィードバックするため、環境剛性が未知でも、力センサの精度の範囲内で目標力に追従できます。PI制御器の積分項が定常偏差を除去するためです。
環境の既知性への依存
ハイブリッド制御は拘束フレームの方向と選択行列を事前に定義する必要があるため、環境の幾何形状が既知であることを前提とします。壁の向きが変わったり、未知の障害物に遭遇すると、選択行列の設定が不適切になり性能が劣化します。
インピーダンス制御は目標インピーダンスを設定するだけなので、環境の幾何形状を詳細に知らなくても機能します。接触方向が事前に分からない場合でも、適切な「柔らかさ」を設定しておけば安全に接触できます。
比較表
| 項目 | ハイブリッド力/位置制御 | インピーダンス制御 |
|---|---|---|
| 力の追従精度 | 高い（直接フィードバック） | 低い（間接的） |
| 環境の既知性 | 必要（拘束方向が既知） | 不要（未知環境に対応可能） |
| 力センサ | 必須 | 不要（力センサなしも可） |
| 接触遷移 | 非接触→接触で不連続 | スムーズ |
| 設計の直感性 | 方向ごとに明確 | インピーダンスパラメータの調整 |
| 典型的用途 | 研磨、バリ取り、精密組立 | 未知環境での接触、柔軟な操作 |
使い分けの指針
実用上の使い分け指針をまとめると以下のようになります。
- 環境の幾何形状が既知で、接触面が定義できる → ハイブリッド制御
- 力の精度が重要で、目標力に正確に追従する必要がある → ハイブリッド制御
- 環境が未知または変動し、接触方向が予測できない → インピーダンス制御
- 非接触と接触を切り替えるタスクがある → インピーダンス制御
- 両方の利点が必要な場合 → 2 つを組み合わせたハイブリッドインピーダンス制御
近年の研究では、力制御方向にインピーダンス特性を持たせたり、選択行列を連続的に切り替えたりする発展的な手法も提案されています。
ハイブリッド制御とインピーダンス制御の使い分けが理解できたところで、次にハイブリッド制御が特に威力を発揮する宇宙環境での応用について見ていきましょう。
宇宙環境での応用
微小重力環境の特殊性
宇宙でのロボット操作は、地上とは大きく異なる環境制約があります。ハイブリッド力/位置制御を宇宙に適用する際に考慮すべき特殊性を整理します。
微小重力: 地上のロボットでは重力補償が制御の大きな部分を占めますが、微小重力環境では $\bm{g}(\bm{q}) \approx \bm{0}$ となるため、重力補償が不要になるという利点があります。一方で、重力による「安定化効果」が失われるため、微小な力の不均衡が予期しない運動を引き起こす可能性があります。
ベースの非固定: 宇宙ステーションのロボットアーム（カナダアーム2など）は、ベースが宇宙ステーション構体に固定されていますが、自由飛行するサービス衛星のマニピュレータではベースが固定されていません。このため、マニピュレータの反動でベース衛星が回転・並進する反動ダイナミクスを考慮する必要があります。
通信遅延: 地上からの遠隔操作では通信遅延が存在します。LEO（低軌道）で数百ミリ秒、月面で約 1.3 秒、火星では約 4〜24 分の遅延があります。ハイブリッド制御の力フィードバックループにこの遅延が入ると不安定化するため、自律的なオンボード制御が必要です。
真空・熱環境: 真空中では対流冷却がなく、直射日光下と影で温度差が 200°C 以上になり得ます。力センサやアクチュエータの特性が温度依存であるため、キャリブレーションの変動を制御器が吸収する必要があります。
軌道上修理・メンテナンス
ハイブリッド力/位置制御の最も直接的な宇宙応用は、軌道上サービス（On-Orbit Servicing: OOS）における修理・メンテナンス作業です。
たとえば、宇宙ステーション外壁のマイクロメテオロイド衝突痕の研磨作業を考えます。この作業では、外壁面に垂直な方向は一定の力で押し付ける力制御、外壁面に平行な方向は研磨パスに沿った位置制御が求められます。
拘束フレームを外壁面に合わせて設定し、選択行列を適切に設計することで、ハイブリッド制御がそのまま適用できます。微小重力環境では重力補償が不要なため、地上よりもシンプルな制御構造で高精度な力制御が実現できるという利点があります。
パネル挿入・組立
大型構造物の軌道上組立において、太陽電池パネルやモジュールの挿入・嵌合作業は重要なタスクです。たとえば、パネルをガイドレールに沿ってスライドさせてスロットに挿入する作業では、以下のように制御を分割します。
- ガイドレール方向（挿入方向）: 位置制御 — パネルを指定位置まで正確に移動
- ガイドレールに垂直な方向: 力制御 — ガイドレールとの接触力を一定値以下に維持（ジャミング防止）
- 回転方向: パネルの姿勢をガイドレールに合わせる位置制御と、こじれを検知する力制御の組み合わせ
この制御構成により、パネルがガイドレールに引っかかった場合でも、力制御方向で過大な力を検知して押し戻す方向にトルクを生成し、ジャミングを回避できます。
自由飛行サービス衛星
ベース非固定の自由飛行サービス衛星でハイブリッド制御を適用する場合、前述の反動ダイナミクスが重要になります。マニピュレータが対象物に力を加えると、その反力でベース衛星が回転します。
この問題に対処するには、一般化ヤコビ行列（Generalized Jacobian Matrix: GJM）を用いて、ベース衛星の運動とマニピュレータの運動を統合的に扱います。GJM を用いた運動方程式に基づいてハイブリッド制御を設計することで、ベースの反動を考慮した正確な力/位置制御が可能になります。
宇宙応用の概要を把握したところで、いよいよ Python シミュレーションに入ります。拘束面に沿ったロボットの研磨作業をシミュレートし、ハイブリッド力/位置制御が実際にどのように動作するかを目で確認しましょう。
Python シミュレーション: 壁面に沿った研磨作業
シミュレーション設定
ここでは 2 次元平面内でのシンプルなシミュレーションを行います。壁面（水平面）に沿ってエンドエフェクタが移動しながら、壁面に一定の力で押し付ける研磨作業をモデル化します。
座標系は $x$ 軸を壁面に平行な方向（位置制御方向）、$y$ 軸を壁面に垂直な方向（力制御方向）とします。エンドエフェクタは質量 $m$ の質点としてモデル化します。
まず、基本的なパラメータを定義しシミュレーション環境を構築します。
import numpy as np
import matplotlib.pyplot as plt
# シミュレーションパラメータ
dt = 0.001 # タイムステップ [s]
T = 5.0 # シミュレーション時間 [s]
N = int(T / dt) # ステップ数
# ロボットエンドエフェクタのパラメータ
m = 2.0 # 質量 [kg]
# 環境パラメータ（壁面: y = 0 の水平面）
y_wall = 0.0 # 壁面の y 座標
k_e = 5000.0 # 環境剛性 [N/m]
b_e = 50.0 # 環境粘性減衰 [Ns/m]
# 位置制御ゲイン（x 方向: PD制御）
Kp_pos = 400.0 # 比例ゲイン
Kd_pos = 40.0 # 微分ゲイン（臨界減衰: 2*sqrt(Kp_pos) = 40）
# 力制御ゲイン（y 方向: PI制御）
Kp_force = 0.5 # 比例ゲイン
Ki_force = 10.0 # 積分ゲイン
# 目標値
f_desired = 10.0 # 目標押し付け力 [N]（y 方向、壁面への押し付け）
# x 方向の目標軌道（正弦波状の研磨パス）
def x_desired(t):
return 0.1 * np.sin(2 * np.pi * 0.5 * t) # 振幅 0.1m, 周波数 0.5Hz
def dx_desired(t):
return 0.1 * 2 * np.pi * 0.5 * np.cos(2 * np.pi * 0.5 * t)
def ddx_desired(t):
return -0.1 * (2 * np.pi * 0.5)**2 * np.sin(2 * np.pi * 0.5 * t)
上のコードでは、研磨作業のシミュレーション環境を定義しています。壁面は $y = 0$ に配置した剛性 $k_e = 5000$ N/m のバネモデルです。位置制御のゲインは臨界減衰条件 $K_D = 2\sqrt{K_P}$ を満たすように設計しています。目標軌道は壁面に沿った正弦波状の研磨パスです。
ハイブリッド制御シミュレーションの実行
次に、メインのシミュレーションループを実装します。
import numpy as np
import matplotlib.pyplot as plt
# シミュレーションパラメータ
dt = 0.001
T = 5.0
N = int(T / dt)
m = 2.0
y_wall = 0.0
k_e = 5000.0
b_e = 50.0
Kp_pos = 400.0
Kd_pos = 40.0
Kp_force = 0.5
Ki_force = 10.0
f_desired = 10.0
def x_desired(t):
return 0.1 * np.sin(2 * np.pi * 0.5 * t)
def dx_desired(t):
return 0.1 * 2 * np.pi * 0.5 * np.cos(2 * np.pi * 0.5 * t)
def ddx_desired(t):
return -0.1 * (2 * np.pi * 0.5)**2 * np.sin(2 * np.pi * 0.5 * t)
# 初期状態
x = 0.0 # x 位置
y = 0.002 # y 位置（壁面に少しめり込んだ状態から開始）
vx = 0.0 # x 速度
vy = 0.0 # y 速度
# 力誤差の積分値
force_error_integral = 0.0
# データ記録用
time_hist = np.zeros(N)
x_hist = np.zeros(N)
y_hist = np.zeros(N)
vx_hist = np.zeros(N)
vy_hist = np.zeros(N)
fx_cmd_hist = np.zeros(N)
fy_cmd_hist = np.zeros(N)
f_contact_hist = np.zeros(N)
x_des_hist = np.zeros(N)
# メインシミュレーションループ
for i in range(N):
t = i * dt
time_hist[i] = t
# --- 環境接触力の計算 ---
if y < y_wall:
# 壁面にめり込んでいる → 接触力発生
penetration = y_wall - y
f_contact = k_e * penetration + b_e * (-vy)
f_contact = max(f_contact, 0.0) # 引張力は発生しない
else:
f_contact = 0.0
# --- 選択行列による空間分割 ---
# x 方向: 位置制御 (S_x = 0)
# y 方向: 力制御 (S_y = 1)
# --- 位置制御器（x 方向）---
x_d = x_desired(t)
dx_d = dx_desired(t)
ddx_d = ddx_desired(t)
ex = x_d - x
dex = dx_d - vx
# 計算トルク法（動力学補償 + PD制御）
fx_cmd = m * (ddx_d + Kp_pos * ex + Kd_pos * dex)
# --- 力制御器（y 方向）---
force_error = f_desired - f_contact
force_error_integral += force_error * dt
# 積分値のアンチワインドアップ
force_error_integral = np.clip(force_error_integral, -50.0, 50.0)
# PI力制御: 目標力 + 比例項 + 積分項
fy_cmd = f_desired + Kp_force * force_error + Ki_force * force_error_integral
# 力指令は壁面方向（-y方向）のみ許可
fy_cmd = max(fy_cmd, 0.0)
# --- 運動方程式の積分 ---
# x 方向: fx_cmd がそのまま作用
ax = fx_cmd / m
# y 方向: fy_cmd（壁面への押し付け力）と接触反力
# fy_cmd は -y 方向（壁面への押し付け）、f_contact は +y 方向（壁面からの反力）
ay = (-fy_cmd + f_contact) / m
# オイラー法による積分
vx += ax * dt
vy += ay * dt
x += vx * dt
y += vy * dt
# データ記録
x_hist[i] = x
y_hist[i] = y
vx_hist[i] = vx
vy_hist[i] = vy
fx_cmd_hist[i] = fx_cmd
fy_cmd_hist[i] = fy_cmd
f_contact_hist[i] = f_contact
x_des_hist[i] = x_d
このシミュレーションループでは、各タイムステップで以下の処理を行っています。まず環境との接触力を計算し、次に $x$ 方向の位置制御器と $y$ 方向の力制御器をそれぞれ独立に動作させます。最後に運動方程式をオイラー法で時間積分しています。アンチワインドアップ処理により、接触前や過渡状態での積分値の過大な蓄積を防いでいます。
結果の可視化
シミュレーション結果をグラフで確認します。
import numpy as np
import matplotlib.pyplot as plt
# （上記シミュレーションの実行後に続けて実行）
fig, axes = plt.subplots(2, 2, figsize=(14, 10))
fig.suptitle('Hybrid Force/Position Control Simulation', fontsize=14, fontweight='bold')
# (1) x 方向の位置追従
ax1 = axes[0, 0]
ax1.plot(time_hist, x_des_hist * 1000, 'b--', label='Desired $x$', linewidth=1.5)
ax1.plot(time_hist, x_hist * 1000, 'r-', label='Actual $x$', linewidth=1.2)
ax1.set_xlabel('Time [s]')
ax1.set_ylabel('x position [mm]')
ax1.set_title('Position Control (x direction)')
ax1.legend()
ax1.grid(True, alpha=0.3)
# (2) y 方向の位置（壁面との関係）
ax2 = axes[0, 1]
ax2.plot(time_hist, y_hist * 1000, 'r-', label='End-effector $y$', linewidth=1.2)
ax2.axhline(y=y_wall * 1000, color='k', linestyle='-', linewidth=2, label='Wall surface')
ax2.set_xlabel('Time [s]')
ax2.set_ylabel('y position [mm]')
ax2.set_title('End-effector Penetration (y direction)')
ax2.legend()
ax2.grid(True, alpha=0.3)
# (3) 接触力の追従
ax3 = axes[1, 0]
ax3.plot(time_hist, f_contact_hist, 'r-', label='Contact force', linewidth=1.2)
ax3.axhline(y=f_desired, color='b', linestyle='--', linewidth=1.5, label=f'Desired force ({f_desired} N)')
ax3.set_xlabel('Time [s]')
ax3.set_ylabel('Force [N]')
ax3.set_title('Force Control (y direction)')
ax3.legend()
ax3.grid(True, alpha=0.3)
# (4) 制御指令力
ax4 = axes[1, 1]
ax4.plot(time_hist, fx_cmd_hist, 'b-', label='$f_x$ command', linewidth=1.0, alpha=0.8)
ax4.plot(time_hist, fy_cmd_hist, 'r-', label='$f_y$ command', linewidth=1.0, alpha=0.8)
ax4.set_xlabel('Time [s]')
ax4.set_ylabel('Command force [N]')
ax4.set_title('Control Commands')
ax4.legend()
ax4.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('hybrid_control_simulation.png', dpi=150, bbox_inches='tight')
plt.show()
このグラフから、ハイブリッド制御の特徴が明確に読み取れます。
- 位置制御方向（左上）: $x$ 方向の実際の位置（赤線）が目標軌道（青破線）にほぼ完全に追従しています。臨界減衰に設計したPDゲインが機能しており、オーバーシュートなく滑らかに追従しています。
- 接触深さ（右上）: $y$ 方向のエンドエフェクタ位置は壁面の直下で安定しています。目標力 10 N に対応する環境剛性 5000 N/m での平衡めり込み量は $10/5000 = 0.002$ m = 2 mm であり、シミュレーション結果がこの理論値に収束していることが確認できます。
- 力制御方向（左下）: 接触力は初期の過渡状態を経て、目標値 10 N に正確に収束しています。PI制御器の積分項が定常偏差を除去し、力の追従精度を保証しています。
- 制御指令力（右下）: $x$ 方向の指令力は正弦波状の加減速パターンを示し、$y$ 方向の指令力は接触力を維持するために比較的一定の値に収束しています。
拘束面の変化への対応
次に、拘束面が途中で傾く場合のシミュレーションを行います。実際の宇宙作業では、作業対象の表面が平坦でない場合があり、拘束フレームの適応的な変更が必要です。
import numpy as np
import matplotlib.pyplot as plt
# シミュレーションパラメータ
dt = 0.001
T = 8.0
N = int(T / dt)
m = 2.0
k_e = 5000.0
b_e = 50.0
Kp_pos = 400.0
Kd_pos = 40.0
Kp_force = 0.5
Ki_force = 10.0
f_desired = 10.0
def wall_surface(x_pos):
"""壁面の y 座標: x < 0.05 で水平、x >= 0.05 で 15度の傾斜"""
if x_pos < 0.05:
return 0.0
else:
return -np.tan(np.radians(15)) * (x_pos - 0.05)
def wall_normal(x_pos):
"""壁面の法線方向（単位ベクトル）"""
if x_pos < 0.05:
return np.array([0.0, 1.0])
else:
theta = np.radians(15)
return np.array([np.sin(theta), np.cos(theta)])
# 初期状態
x, y = 0.0, 0.002
vx, vy = 0.0, 0.0
force_error_integral = 0.0
# 目標: x 方向に等速移動（壁面に沿って）
x_speed = 0.03 # 30 mm/s
# データ記録用
time_hist = np.zeros(N)
x_hist = np.zeros(N)
y_hist = np.zeros(N)
f_contact_hist = np.zeros(N)
fn_hist = np.zeros(N) # 法線方向接触力
wall_y_hist = np.zeros(N)
for i in range(N):
t = i * dt
time_hist[i] = t
# 壁面の位置と法線
y_w = wall_surface(x)
n = wall_normal(x) # 法線方向（壁面から離れる方向）
tangent = np.array([-n[1], n[0]]) # 接線方向
# 環境接触力の計算
# 壁面からの距離（法線方向の深さ）
disp = np.array([x, y]) - np.array([x, y_w])
penetration_depth = -np.dot(disp, n) # 法線方向のめり込み量
if penetration_depth > 0:
vel = np.array([vx, vy])
vel_normal = np.dot(vel, n)
f_contact_mag = k_e * penetration_depth + b_e * (-vel_normal)
f_contact_mag = max(f_contact_mag, 0.0)
f_contact = f_contact_mag * n # 法線方向の接触力ベクトル
else:
f_contact = np.array([0.0, 0.0])
f_contact_mag = 0.0
# --- 適応的拘束フレームでのハイブリッド制御 ---
# 接線方向（位置制御）: 壁面に沿って等速移動
x_d_tangent = x_speed * t # 接線方向の目標位置
pos_tangent = np.dot(np.array([x, y]) - np.array([0, wall_surface(0)]), tangent)
vel_tangent = np.dot(np.array([vx, vy]), tangent)
e_pos = x_d_tangent - pos_tangent
de_pos = x_speed - vel_tangent
ddx_d_tangent = 0.0 # 等速なのでフィードフォワード加速度はゼロ
f_pos_cmd = m * (ddx_d_tangent + Kp_pos * e_pos + Kd_pos * de_pos)
f_pos_vec = f_pos_cmd * tangent # 接線方向の力ベクトル
# 法線方向（力制御）: 一定の押し付け力
force_error = f_desired - f_contact_mag
force_error_integral += force_error * dt
force_error_integral = np.clip(force_error_integral, -50.0, 50.0)
f_force_cmd = f_desired + Kp_force * force_error + Ki_force * force_error_integral
f_force_cmd = max(f_force_cmd, 0.0)
f_force_vec = -f_force_cmd * n # 壁面に押し付ける方向（-n方向）
# 合成指令力
f_cmd = f_pos_vec + f_force_vec
# 運動方程式の積分
ax_total = (f_cmd[0] + f_contact[0]) / m
ay_total = (f_cmd[1] + f_contact[1]) / m
vx += ax_total * dt
vy += ay_total * dt
x += vx * dt
y += vy * dt
# データ記録
x_hist[i] = x
y_hist[i] = y
f_contact_hist[i] = f_contact_mag
fn_hist[i] = f_contact_mag
wall_y_hist[i] = y_w
# 可視化
fig, axes = plt.subplots(1, 3, figsize=(18, 5))
fig.suptitle('Hybrid Control with Inclined Surface Transition', fontsize=14, fontweight='bold')
# (1) エンドエフェクタの軌跡と壁面
ax1 = axes[0]
# 壁面の描画
x_wall = np.linspace(-0.01, 0.3, 500)
y_wall_line = np.array([wall_surface(xi) for xi in x_wall])
ax1.plot(x_wall * 1000, y_wall_line * 1000, 'k-', linewidth=3, label='Wall surface')
ax1.fill_between(x_wall * 1000, y_wall_line * 1000, y_wall_line * 1000 - 5,
color='gray', alpha=0.3)
ax1.plot(x_hist * 1000, y_hist * 1000, 'r-', linewidth=1.2, label='End-effector path')
ax1.set_xlabel('x [mm]')
ax1.set_ylabel('y [mm]')
ax1.set_title('End-effector Trajectory')
ax1.legend()
ax1.grid(True, alpha=0.3)
ax1.set_aspect('equal')
# (2) 法線方向の接触力
ax2 = axes[1]
ax2.plot(time_hist, fn_hist, 'r-', linewidth=1.0)
ax2.axhline(y=f_desired, color='b', linestyle='--', linewidth=1.5, label=f'Desired ({f_desired} N)')
ax2.axvline(x=0.05 / x_speed, color='gray', linestyle=':', linewidth=1.0, label='Surface transition')
ax2.set_xlabel('Time [s]')
ax2.set_ylabel('Normal contact force [N]')
ax2.set_title('Force Tracking')
ax2.legend()
ax2.grid(True, alpha=0.3)
# (3) 壁面からの距離
penetration_hist = np.array([wall_surface(x_hist[i]) - y_hist[i] for i in range(N)])
ax3 = axes[2]
ax3.plot(time_hist, penetration_hist * 1000, 'g-', linewidth=1.0)
ax3.axvline(x=0.05 / x_speed, color='gray', linestyle=':', linewidth=1.0, label='Surface transition')
ax3.set_xlabel('Time [s]')
ax3.set_ylabel('Penetration depth [mm]')
ax3.set_title('Wall Penetration')
ax3.legend()
ax3.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('hybrid_control_inclined.png', dpi=150, bbox_inches='tight')
plt.show()
このシミュレーションでは、壁面が途中で 15 度傾斜する環境を設定しています。結果から以下の重要なポイントが読み取れます。
- エンドエフェクタの軌跡（左）: エンドエフェクタは壁面に沿って移動し、水平面から傾斜面への遷移をスムーズに通過しています。拘束フレームを壁面の法線方向に適応的に更新することで、接触を維持しながら作業を継続できています。
- 力の追従（中央）: 傾斜面への遷移点（灰色の点線）で一時的な力の変動が見られますが、PI制御器の作用により速やかに目標力 10 N に再収束しています。この過渡的な変動は、拘束フレームが急激に変化したことに起因しており、実際のシステムでは表面形状のなめらかな遷移やゲインスケジューリングで軽減できます。
- 壁面へのめり込み深さ（右）: めり込み量は目標力と環境剛性から予測される平衡値 $f^d / k_e = 2$ mm 付近で安定しています。傾斜面への遷移に伴う一時的な変動は、力制御器が補正しています。
ハイブリッド制御 vs 純粋位置制御の比較
最後に、ハイブリッド制御を使う場合と使わない場合（純粋位置制御のみ）の差を比較するシミュレーションを行います。
import numpy as np
import matplotlib.pyplot as plt
dt = 0.001
T = 3.0
N = int(T / dt)
m = 2.0
k_e = 5000.0
b_e = 50.0
Kp_pos = 400.0
Kd_pos = 40.0
Kp_force = 0.5
Ki_force = 10.0
f_desired = 10.0
# 壁面の y 座標にわずかな不確かさがある状況
y_wall_true = 0.0 # 真の壁面位置
y_wall_assumed = 0.001 # 仮定した壁面位置（1mm の誤差）
def x_desired_comp(t):
return 0.05 * t # 等速直線運動
def dx_desired_comp(t):
return 0.05
results = {}
for method in ['hybrid', 'position_only']:
x, y = 0.0, y_wall_assumed + 0.002
vx, vy = 0.0, 0.0
force_error_integral = 0.0
time_h = np.zeros(N)
x_h = np.zeros(N)
y_h = np.zeros(N)
fc_h = np.zeros(N)
for i in range(N):
t = i * dt
time_h[i] = t
# 接触力計算
if y < y_wall_true:
pen = y_wall_true - y
fc = k_e * pen + b_e * (-vy)
fc = max(fc, 0.0)
else:
fc = 0.0
# x 方向: 位置制御（共通）
xd = x_desired_comp(t)
dxd = dx_desired_comp(t)
ex = xd - x
dex = dxd - vx
fx_cmd = m * (Kp_pos * ex + Kd_pos * dex)
if method == 'hybrid':
# y 方向: 力制御
fe = f_desired - fc
force_error_integral += fe * dt
force_error_integral = np.clip(force_error_integral, -50.0, 50.0)
fy_cmd = f_desired + Kp_force * fe + Ki_force * force_error_integral
fy_cmd = max(fy_cmd, 0.0)
else:
# y 方向: 位置制御（仮定した壁面位置に押し込む）
y_target = y_wall_assumed - f_desired / k_e # 目標力に対応する位置
ey = y_target - y
dey = 0.0 - vy
fy_cmd = m * (Kp_pos * ey + Kd_pos * dey)
fy_cmd = max(fy_cmd, 0.0)
ax_val = fx_cmd / m
ay_val = (-fy_cmd + fc) / m
vx += ax_val * dt
vy += ay_val * dt
x += vx * dt
y += vy * dt
x_h[i] = x
y_h[i] = y
fc_h[i] = fc
results[method] = {'time': time_h, 'x': x_h, 'y': y_h, 'fc': fc_h}
# 比較プロット
fig, axes = plt.subplots(1, 2, figsize=(14, 5))
fig.suptitle('Hybrid Control vs Position-Only Control', fontsize=14, fontweight='bold')
# 接触力の比較
ax1 = axes[0]
ax1.plot(results['hybrid']['time'], results['hybrid']['fc'],
'b-', linewidth=1.2, label='Hybrid control')
ax1.plot(results['position_only']['time'], results['position_only']['fc'],
'r-', linewidth=1.2, label='Position-only control')
ax1.axhline(y=f_desired, color='k', linestyle='--', linewidth=1.0, label=f'Desired ({f_desired} N)')
ax1.set_xlabel('Time [s]')
ax1.set_ylabel('Contact force [N]')
ax1.set_title('Contact Force Comparison')
ax1.legend()
ax1.grid(True, alpha=0.3)
# 力誤差の比較
ax2 = axes[1]
hybrid_err = np.abs(results['hybrid']['fc'] - f_desired)
pos_err = np.abs(results['position_only']['fc'] - f_desired)
# 移動平均でスムージング
window = 100
hybrid_err_smooth = np.convolve(hybrid_err, np.ones(window)/window, mode='same')
pos_err_smooth = np.convolve(pos_err, np.ones(window)/window, mode='same')
ax2.plot(results['hybrid']['time'], hybrid_err_smooth,
'b-', linewidth=1.2, label='Hybrid control')
ax2.plot(results['position_only']['time'], pos_err_smooth,
'r-', linewidth=1.2, label='Position-only control')
ax2.set_xlabel('Time [s]')
ax2.set_ylabel('|Force error| [N] (smoothed)')
ax2.set_title('Force Error Comparison')
ax2.legend()
ax2.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('hybrid_vs_position_comparison.png', dpi=150, bbox_inches='tight')
plt.show()
# 定常状態の力誤差を数値で比較
print("=== 定常状態の力誤差（t > 2.0s の平均）===")
mask = results['hybrid']['time'] > 2.0
print(f"ハイブリッド制御: {np.mean(np.abs(results['hybrid']['fc'][mask] - f_desired)):.4f} N")
print(f"位置制御のみ: {np.mean(np.abs(results['position_only']['fc'][mask] - f_desired)):.4f} N")
この比較シミュレーションから、ハイブリッド制御の優位性が定量的に確認できます。
- 接触力の追従（左）: ハイブリッド制御（青線）は目標力 10 N に正確に収束しているのに対し、純粋位置制御（赤線）は壁面位置の 1 mm の不確かさにより、目標力からの定常偏差が残っています。位置制御では $k_e \times 1 \text{ mm} = 5$ N の力誤差が生じる計算です。
- 力誤差（右）: ハイブリッド制御の定常的な力誤差はほぼゼロに収束する一方、位置制御のみでは環境モデルの不確かさに起因する力誤差が持続しています。このことは、環境パラメータの正確な知識が不要なハイブリッド制御の実用上の大きな利点を示しています。
特に宇宙環境では、ロボットアームのキャリブレーション誤差や対象物の位置不確かさが避けられないため、力を直接フィードバックするハイブリッド制御の頑健性は極めて重要です。
実装上の注意点
ここまでの理論とシミュレーションを実際のシステムに実装する際に注意すべき点を補足します。
力センサの配置とノイズ対策
ハイブリッド制御は力センサのフィードバックに依存するため、センサの品質が制御性能に直結します。6 軸力/トルクセンサをエンドエフェクタの手首に配置するのが一般的ですが、センサ出力にはノイズが含まれるため、ローパスフィルタを適用する必要があります。ただし、フィルタの遮断周波数が低すぎると力制御ループの応答が遅くなり、安定余裕が減少するというトレードオフがあります。
拘束フレームの更新
曲面に沿った作業や、作業対象の位置・姿勢が変化する場合、拘束フレームをリアルタイムで更新する必要があります。力センサの測定値から接触面の法線方向を推定する手法や、視覚センサ（カメラ）からの情報を統合する手法が研究されています。
接触遷移の処理
非接触状態から接触状態への遷移（アプローチフェーズ）は、ハイブリッド制御の弱点の一つです。接触が確立されていない段階で力制御器を動作させると、目標力を達成しようとして急激な運動を指令し、衝突時の衝撃が大きくなる危険があります。実用的には、以下の対策が取られます。
- アプローチフェーズでは低速の位置制御を使用し、接触検知後にハイブリッド制御に切り替える
- 切り替え時のトランジェントを抑えるため、力の目標値をゼロから徐々に立ち上げる（ランプ入力）
- インピーダンス制御をアプローチフェーズに使い、接触確立後にハイブリッド制御に切り替えるハイブリッド戦略を採用する
選択行列の連続的な切り替え
従来のハイブリッド制御では選択行列 $\bm{S}$ の各成分は 0 か 1 の離散値ですが、作業フェーズの切り替え時に離散的なジャンプが生じます。これを滑らかにするため、$s_i$ を $[0, 1]$ の連続値として時間とともに変化させる拡張が提案されています。$s_i = 0.5$ は、その方向に位置制御と力制御を半々のウェイトで適用することを意味し、遷移時の不連続性を軽減できます。
まとめ
本記事では、Raibert-Craig のハイブリッド力/位置制御について、基礎概念から制御則の導出、安定性解析、そして Python シミュレーションまでを解説しました。
- 拘束フレーム: タスクの幾何学に合わせた座標系を導入し、自然拘束と人工拘束の概念で各方向の制御モードを分類する
- 選択行列 $\bm{S}$: 対角行列による射影演算子で、タスク空間を位置制御方向と力制御方向に直交分解する
- 制御則: 位置制御器（PD + 動力学補償）と力制御器（PI）を独立に設計し、選択行列で合成する
- 安定性: 位置制御方向は PD ゲインの正定値性、力制御方向は環境剛性と PI ゲインのバランスで安定性が決まる
- インピーダンス制御との使い分け: 環境が既知で力の精度が重要ならハイブリッド制御、環境が未知ならインピーダンス制御が適する
- 宇宙応用: 微小重力・ベース非固定・通信遅延などの特殊性を考慮した上で、軌道上修理やパネル挿入に適用できる
ハイブリッド力/位置制御は、ロボットが環境と接触しながら器用に作業を行うための基盤的な制御手法です。宇宙ロボティクスの分野では、軌道上サービスや月面・火星での建設作業など、ますます重要性を増していくでしょう。
次のステップとして、以下の記事も参考にしてください。