ロボットアームを動かすとき、「同じ速さで腕を振っても、腕を伸ばした状態と畳んだ状態では必要なトルクがまるで違う」という経験はないでしょうか。フィギュアスケーターがスピンの最中に腕を引き寄せると回転が速くなるのも同じ原理です。ロボットマニピュレータの世界では、この「姿勢によって変わる動かしやすさ」が運動方程式の中に行列として明示的に現れます。
マニピュレータの運動方程式は、次のたった一行に集約されます。
$$ \bm{M}(\bm{q})\ddot{\bm{q}} + \bm{C}(\bm{q},\dot{\bm{q}})\dot{\bm{q}} + \bm{g}(\bm{q}) = \bm{\tau} $$
この式の $\bm{M}$ が慣性行列、$\bm{C}$ がコリオリ・遠心力行列、$\bm{g}$ が重力項です。制御工学の教科書ではこれらを天下り的に導入しがちですが、それぞれの項には明確な物理的意味があり、その意味を理解することが高精度な制御（計算トルク法、適応制御など）への第一歩になります。
さらに、宇宙空間ではこの運動方程式が地上とは異なる特殊な形をとります。重力が消えるだけでなく、自由浮遊する本体との反力まで考えなければなりません。宇宙ロボティクスの動力学を理解するためにも、各項の物理を正確に押さえることは不可欠です。
本記事の内容
- 運動方程式 $\bm{M}(\bm{q})\ddot{\bm{q}} + \bm{C}(\bm{q},\dot{\bm{q}})\dot{\bm{q}} + \bm{g}(\bm{q}) = \bm{\tau}$ の全体像
- 慣性行列 $\bm{M}(\bm{q})$ の物理的意味と数学的性質（正定値対称、姿勢依存性）
- コリオリ・遠心力項 $\bm{C}(\bm{q},\dot{\bm{q}})\dot{\bm{q}}$ の物理的意味とクリストッフェル記号
- 反対称性 $\dot{\bm{M}} – 2\bm{C}$ の深い意味
- 重力項 $\bm{g}(\bm{q})$ と宇宙環境の特殊性
- 動力学パラメータの同定問題
- Pythonによる2リンクアームの各項の可視化
前提知識
この記事を読む前に、以下の知識があると理解が深まります。
- ラグランジュ法によるマニピュレータの運動方程式導出 — 本記事はこの運動方程式の各項を深掘りする続編です
- ラグランジュ力学の基本（一般化座標、一般化力の概念）
- 線形代数の基本（対称行列、正定値行列、固有値）
運動方程式の標準形 — 全体像を把握する
まず、マニピュレータ動力学の運動方程式がどこから来て、何を表しているのかを確認しましょう。
$n$ 自由度のマニピュレータの一般化座標を $\bm{q} = (q_1, q_2, \dots, q_n)^T$（各関節角度）とします。ラグランジュ法から導かれる運動方程式は、次の 標準形 にまとめることができます。
$$ \bm{M}(\bm{q})\ddot{\bm{q}} + \bm{C}(\bm{q},\dot{\bm{q}})\dot{\bm{q}} + \bm{g}(\bm{q}) = \bm{\tau} $$
各項の役割を一言でまとめると次の通りです。
| 項 | 記号 | 直感的な意味 |
|---|---|---|
| 慣性項 | $\bm{M}(\bm{q})\ddot{\bm{q}}$ | 「加速するために必要な力」— ニュートンの $F = ma$ の一般化 |
| コリオリ・遠心力項 | $\bm{C}(\bm{q},\dot{\bm{q}})\dot{\bm{q}}$ | 「動いているせいで生じる見かけの力」— 回転座標系で現れる力 |
| 重力項 | $\bm{g}(\bm{q})$ | 「姿勢を維持するために重力に抗するトルク」 |
| 関節トルク | $\bm{\tau}$ | 各関節のモータが出すトルク |
この式を「ニュートンの第二法則の一般化」と捉えると理解しやすくなります。$F = ma$ を思い出してください。質量 $m$ に相当するのが慣性行列 $\bm{M}$、加速度 $a$ に相当するのが $\ddot{\bm{q}}$ です。ただし、ロボットの場合は「質量」が姿勢によって変わり、さらに速度に依存する力（コリオリ・遠心力）と姿勢に依存する力（重力）が加わります。
式を変形して、関節加速度について解くと次のようになります。
$$ \ddot{\bm{q}} = \bm{M}(\bm{q})^{-1}\left[\bm{\tau} – \bm{C}(\bm{q},\dot{\bm{q}})\dot{\bm{q}} – \bm{g}(\bm{q})\right] $$
この式は「与えたトルク $\bm{\tau}$ から、コリオリ力と重力を差し引いた残りが、慣性行列を通じて加速度に変換される」ことを示しています。制御の観点からは、望みの加速度を実現するために、コリオリ力と重力を正確にキャンセルするトルクを計算する必要があります。これが 計算トルク法 の核心であり、各項の正確な理解が制御精度に直結する理由です。
では、各項を一つずつ丁寧に見ていきましょう。まずは最も基本的な慣性行列からです。
慣性行列 $\bm{M}(\bm{q})$ の物理的意味と数学的性質
直感的理解 — 姿勢によって変わる「重さ」
日常的な例で考えてみましょう。ダンベルを持って腕を伸ばした状態で水平に回転させるのと、ダンベルを体の近くに引き寄せた状態で回転させるのとでは、同じ角加速度を出すために必要な力がまるで違います。腕を伸ばした状態では「重く」感じ、畳んだ状態では「軽く」感じます。
これは質量が変わったのではなく、慣性モーメント が変わったからです。回転運動における「動かしにくさ」は、質量だけでなく質量の分布（回転軸からの距離）にも依存します。ロボットマニピュレータでは各リンクが回転するため、関節角度 $\bm{q}$ が変わるとリンクの質量分布が変わり、結果として慣性が変化します。
慣性行列 $\bm{M}(\bm{q})$ は、まさにこの「姿勢によって変わる慣性」を $n \times n$ の行列として表現したものです。
数学的定義
$n$ 自由度マニピュレータの慣性行列 $\bm{M}(\bm{q})$ は、系全体の運動エネルギー $T$ を用いて定義されます。
$$ T = \frac{1}{2}\dot{\bm{q}}^T \bm{M}(\bm{q}) \dot{\bm{q}} $$
この式は、質点の運動エネルギー $T = \frac{1}{2}mv^2$ の自然な拡張です。スカラーの質量 $m$ が行列 $\bm{M}$ に、速度 $v$ が一般化速度ベクトル $\dot{\bm{q}}$ に置き換わっています。
具体的には、各リンク $i$ の運動エネルギー（並進 + 回転）の総和から導出されます。
$$ T = \sum_{i=1}^{n} \left( \frac{1}{2} m_i \dot{\bm{p}}_i^T \dot{\bm{p}}_i + \frac{1}{2} \bm{\omega}_i^T \bm{I}_i \bm{\omega}_i \right) $$
ここで $m_i$ はリンク $i$ の質量、$\dot{\bm{p}}_i$ は重心の並進速度、$\bm{\omega}_i$ は角速度、$\bm{I}_i$ は重心まわりの慣性テンソルです。
リンク $i$ の重心速度と角速度は、ヤコビ行列 $\bm{J}_{v,i}(\bm{q})$（並進）と $\bm{J}_{\omega,i}(\bm{q})$（回転）を用いて次のように表されます。
$$ \dot{\bm{p}}_i = \bm{J}_{v,i}(\bm{q})\dot{\bm{q}}, \quad \bm{\omega}_i = \bm{J}_{\omega,i}(\bm{q})\dot{\bm{q}} $$
これを運動エネルギーの式に代入すると、$\dot{\bm{q}}$ でくくり出せます。
$$ T = \frac{1}{2}\dot{\bm{q}}^T \underbrace{\sum_{i=1}^{n} \left( m_i \bm{J}_{v,i}^T \bm{J}_{v,i} + \bm{J}_{\omega,i}^T \bm{I}_i \bm{J}_{\omega,i} \right)}_{\bm{M}(\bm{q})} \dot{\bm{q}} $$
したがって、慣性行列の明示的な表現は次の通りです。
$$ \bm{M}(\bm{q}) = \sum_{i=1}^{n} \left( m_i \bm{J}_{v,i}(\bm{q})^T \bm{J}_{v,i}(\bm{q}) + \bm{J}_{\omega,i}(\bm{q})^T \bm{I}_i \bm{J}_{\omega,i}(\bm{q}) \right) $$
この表現から、$\bm{M}(\bm{q})$ がヤコビ行列を通じて $\bm{q}$ に依存すること、すなわち 姿勢によって変わる ことが明確にわかります。
慣性行列の3つの重要な性質
慣性行列には、制御設計上きわめて重要な性質が3つあります。
性質1: 対称性 — $\bm{M}(\bm{q}) = \bm{M}(\bm{q})^T$
上の定義式を見ると、$\bm{M}(\bm{q})$ は $\bm{A}^T\bm{A}$ の形の行列の和になっています。$\bm{A}^T\bm{A}$ が常に対称であることから（$(\bm{A}^T\bm{A})^T = \bm{A}^T\bm{A}$）、$\bm{M}(\bm{q})$ も対称行列です。
物理的には、これは「関節 $i$ を動かしたときに関節 $j$ に及ぼす慣性効果と、関節 $j$ を動かしたときに関節 $i$ に及ぼす慣性効果が等しい」ことを意味します。慣性の相互作用には方向性がないのです。
性質2: 正定値性 — $\bm{x}^T\bm{M}(\bm{q})\bm{x} > 0 \quad (\forall \bm{x} \neq \bm{0})$
運動エネルギーは「動いていれば必ず正」でなければなりません。$T = \frac{1}{2}\dot{\bm{q}}^T \bm{M}(\bm{q}) \dot{\bm{q}} > 0$（$\dot{\bm{q}} \neq \bm{0}$ のとき）が常に成り立つため、$\bm{M}(\bm{q})$ は正定値です。
正定値であるということは、$\bm{M}(\bm{q})$ の全ての固有値が正であり、逆行列 $\bm{M}(\bm{q})^{-1}$ が常に存在することを保証します。これは先ほどの $\ddot{\bm{q}} = \bm{M}^{-1}[\cdots]$ の計算が常にできることを意味し、制御設計上きわめて重要です。
性質3: 姿勢依存性 — 対角要素と非対角要素の意味
$\bm{M}(\bm{q})$ の $(i,j)$ 要素 $M_{ij}(\bm{q})$ は、関節 $j$ の加速 $\ddot{q}_j$ が関節 $i$ に及ぼす慣性効果を表します。
- 対角要素 $M_{ii}(\bm{q})$: 関節 $i$ 自身の実効慣性（その関節だけを加速するのに必要なトルク）
- 非対角要素 $M_{ij}(\bm{q})$（$i \neq j$）: 関節間の慣性結合（関節 $j$ を加速すると関節 $i$ にも力が伝わる効果）
2リンクアームを例にとると、慣性行列は次のような形になります。
$$ \bm{M}(\bm{q}) = \begin{pmatrix} M_{11}(q_2) & M_{12}(q_2) \\ M_{12}(q_2) & M_{22} \end{pmatrix} $$
ここで注目すべきは、$M_{22}$（第2関節自身の慣性）は $q_2$ に依存しないことです。第2リンクから見れば、自分自身の回転中心からの質量分布は関節角度によらず一定だからです。一方、$M_{11}$（第1関節の実効慣性）は $q_2$ に依存します。第2リンクが伸びた姿勢のとき、第1関節から見た慣性モーメントが大きくなるためです。
慣性楕円体 — 慣性行列の幾何学的解釈
慣性行列の性質を視覚的に捉えるために、慣性楕円体 という概念が便利です。
運動エネルギーが一定値 $T_0$ となる関節速度の集合は、次のように表されます。
$$ \dot{\bm{q}}^T \bm{M}(\bm{q}) \dot{\bm{q}} = 2T_0 $$
これは関節速度空間における楕円体を定義します。$\bm{M}(\bm{q})$ の固有値が大きい方向（固有ベクトルの方向）では楕円体が短くなり、固有値が小さい方向では長くなります。
物理的な解釈は明快です。楕円体が短い方向は「慣性が大きく、動かしにくい方向」であり、長い方向は「慣性が小さく、動かしやすい方向」です。ロボットの姿勢が変わると固有値が変わるため、楕円体の形も変化します。これが「姿勢によって動かしやすさの方向が変わる」ことの幾何学的表現です。
慣性行列の性質がわかったところで、次はより捉えにくい項 — 動いていること自体から生まれる力 — について考えましょう。
コリオリ・遠心力項 $\bm{C}(\bm{q},\dot{\bm{q}})\dot{\bm{q}}$ の物理的意味
直感的理解 — 回転が生む「見かけの力」
車でカーブを曲がるとき、体が外側に引っ張られる感覚があります。これが遠心力です。また、回転するメリーゴーラウンドの上でボールを投げると、ボールは曲がった軌道を描きます。投げた人から見ればボールに横向きの力が加わっているように見える — これがコリオリ力です。
マニピュレータでも全く同じことが起こります。ある関節が回転しているとき、その回転によって他のリンクには遠心力やコリオリ力が生じます。これらは「実際にはない力」（見かけの力）ですが、関節座標系で運動方程式を書くときには明示的な項として現れます。
重要な点は、これらの力は速度に依存する ことです。静止しているロボット（$\dot{\bm{q}} = \bm{0}$）にはコリオリ力も遠心力も働きません。動いている場合にのみ現れる力であり、高速動作では無視できない大きさになります。
数学的定義 — クリストッフェル記号
コリオリ・遠心力項は、慣性行列 $\bm{M}(\bm{q})$ の要素の偏微分から導かれます。ラグランジュ方程式を展開すると、次の項が現れます。
$$ \sum_{j=1}^{n}\sum_{k=1}^{n} c_{ijk}(\bm{q}) \dot{q}_j \dot{q}_k $$
ここで $c_{ijk}$ は 第1種クリストッフェル記号（Christoffel symbols of the first kind）と呼ばれ、次のように定義されます。
$$ c_{ijk}(\bm{q}) = \frac{1}{2}\left( \frac{\partial M_{ij}}{\partial q_k} + \frac{\partial M_{ik}}{\partial q_j} – \frac{\partial M_{jk}}{\partial q_i} \right) $$
この記号は微分幾何学に由来するもので、「関節空間の曲率」を表現しています。関節空間が「曲がっている」ために、まっすぐ動こうとしても横方向の力が生じるのです。
コリオリ力と遠心力の区別
$c_{ijk} \dot{q}_j \dot{q}_k$ の形から、2種類の力を区別できます。
遠心力（$j = k$ の項）: $c_{ijj} \dot{q}_j^2$
一つの関節 $j$ が回転していることで、関節 $i$ に生じる力です。速度の2乗に比例するため、高速回転では急激に大きくなります。フィギュアスケーターのスピンで体感する力がまさにこれです。
コリオリ力（$j \neq k$ の項）: $2 c_{ijk} \dot{q}_j \dot{q}_k$
2つの異なる関節 $j, k$ が同時に回転しているときに生じる力です。2つの速度の積に比例するため、一方の関節だけが動いているときには現れません。
コリオリ行列 $\bm{C}(\bm{q},\dot{\bm{q}})$ の構成
クリストッフェル記号を用いて、コリオリ・遠心力項を行列形式にまとめることができます。$\bm{C}(\bm{q},\dot{\bm{q}})$ の $(i,j)$ 要素は次のように定義されます。
$$ C_{ij}(\bm{q},\dot{\bm{q}}) = \sum_{k=1}^{n} c_{ijk}(\bm{q}) \dot{q}_k = \sum_{k=1}^{n} \frac{1}{2}\left( \frac{\partial M_{ij}}{\partial q_k} + \frac{\partial M_{ik}}{\partial q_j} – \frac{\partial M_{jk}}{\partial q_i} \right) \dot{q}_k $$
これにより、コリオリ・遠心力は $\bm{C}(\bm{q},\dot{\bm{q}})\dot{\bm{q}}$ というベクトルとして簡潔に表現されます。
ここで、$\bm{C}$ の定義は一意ではないことに注意してください。$\bm{C}\dot{\bm{q}}$ という積が正しければ、$\bm{C}$ 自体の分け方にはある程度の自由度があります。上述のクリストッフェル記号による定義は、最も広く使われるものであり、後述する反対称性という重要な性質を保証します。
2リンクアームでの具体例
具体的なイメージをつかむために、2リンク平面アームのコリオリ・遠心力項を見てみましょう。2リンクアームの慣性行列が次の形をしているとします。
$$ \bm{M}(\bm{q}) = \begin{pmatrix} \alpha + 2\beta\cos q_2 & \delta + \beta\cos q_2 \\ \delta + \beta\cos q_2 & \delta \end{pmatrix} $$
ここで $\alpha$, $\beta$, $\delta$ はリンクのパラメータ（質量、長さ、慣性モーメント）から決まる定数です。
$\bm{M}$ を $q_2$ で偏微分すると（$q_1$ には依存しないので $\partial \bm{M}/\partial q_1 = \bm{0}$）、次のようになります。
$$ \frac{\partial \bm{M}}{\partial q_2} = \begin{pmatrix} -2\beta\sin q_2 & -\beta\sin q_2 \\ -\beta\sin q_2 & 0 \end{pmatrix} $$
クリストッフェル記号を計算すると、$h = -\beta \sin q_2$ として、コリオリ・遠心力項は次のように簡潔に書けます。
$$ \bm{C}(\bm{q},\dot{\bm{q}})\dot{\bm{q}} = \begin{pmatrix} -2h\dot{q}_1\dot{q}_2 – h\dot{q}_2^2 \\ h\dot{q}_1^2 \end{pmatrix} $$
第1関節に作用する力 $-2h\dot{q}_1\dot{q}_2 – h\dot{q}_2^2$ は、コリオリ力（$\dot{q}_1\dot{q}_2$ の項）と遠心力（$\dot{q}_2^2$ の項）の和です。第2関節に作用する力 $h\dot{q}_1^2$ は、第1関節の回転による遠心力のみです。$q_2 = 0$（腕がまっすぐ伸びた状態）のとき $h = 0$ となり、コリオリ・遠心力が消えるのは、対称な配置では見かけの力が現れないことと整合しています。
コリオリ・遠心力項の構造が見えてきたところで、次はこの項が満たす美しい性質 — 反対称性 — について掘り下げましょう。
反対称性 $\dot{\bm{M}} – 2\bm{C}$ の意味
エネルギー保存が生む制約
慣性行列 $\bm{M}$ とコリオリ行列 $\bm{C}$ の間には、次の驚くべき関係が成り立ちます。
$$ \bm{N}(\bm{q},\dot{\bm{q}}) = \dot{\bm{M}}(\bm{q}) – 2\bm{C}(\bm{q},\dot{\bm{q}}) \quad \text{は反対称行列} $$
つまり $\bm{N}^T = -\bm{N}$ であり、任意のベクトル $\bm{x}$ に対して $\bm{x}^T \bm{N} \bm{x} = 0$ が成り立ちます。
この性質は天から降ってきたわけではありません。その根源はエネルギー保存にあります。
導出 — なぜ反対称なのか
外力がない場合（$\bm{\tau} = \bm{0}$, $\bm{g} = \bm{0}$）、系のエネルギーが保存されることを考えましょう。運動エネルギーの時間微分を計算します。
$$ \frac{dT}{dt} = \frac{d}{dt}\left(\frac{1}{2}\dot{\bm{q}}^T \bm{M} \dot{\bm{q}}\right) $$
積の微分法則を適用すると、次のようになります。
$$ \frac{dT}{dt} = \frac{1}{2}\ddot{\bm{q}}^T \bm{M} \dot{\bm{q}} + \frac{1}{2}\dot{\bm{q}}^T \dot{\bm{M}} \dot{\bm{q}} + \frac{1}{2}\dot{\bm{q}}^T \bm{M} \ddot{\bm{q}} $$
$\bm{M}$ の対称性から、第1項と第3項は等しく、まとめて次のように書けます。
$$ \frac{dT}{dt} = \dot{\bm{q}}^T \bm{M} \ddot{\bm{q}} + \frac{1}{2}\dot{\bm{q}}^T \dot{\bm{M}} \dot{\bm{q}} $$
ここで運動方程式 $\bm{M}\ddot{\bm{q}} = -\bm{C}\dot{\bm{q}}$（外力・重力なし）を代入すると、次のようになります。
$$ \frac{dT}{dt} = \dot{\bm{q}}^T(-\bm{C}\dot{\bm{q}}) + \frac{1}{2}\dot{\bm{q}}^T \dot{\bm{M}} \dot{\bm{q}} = \frac{1}{2}\dot{\bm{q}}^T (\dot{\bm{M}} – 2\bm{C}) \dot{\bm{q}} $$
エネルギーが保存されるためには $\frac{dT}{dt} = 0$ でなければならず、これが任意の $\dot{\bm{q}}$ について成り立つためには、$\dot{\bm{M}} – 2\bm{C}$ が反対称であることが必要十分条件です。
制御工学への意味 — リアプノフ安定性
この反対称性は、制御設計において強力な道具になります。
例えば、計算トルク法の安定性解析では、リアプノフ関数として $V = \frac{1}{2}\bm{s}^T\bm{M}\bm{s}$ のような二次形式を用いることがよくあります（$\bm{s}$ は追従誤差に関するスライディング変数）。このリアプノフ関数の時間微分を計算すると、$\dot{\bm{M}} – 2\bm{C}$ の反対称性のおかげで、$\bm{s}^T(\dot{\bm{M}} – 2\bm{C})\bm{s} = 0$ となり、余分な項が自動的にキャンセルされます。
この性質がなければ、ロボット制御の安定性証明はずっと複雑になるでしょう。ある意味、物理学（エネルギー保存）が制御工学を助けてくれているのです。
クリストッフェル記号による定義の必然性
先ほど「$\bm{C}$ の定義は一意ではない」と述べましたが、クリストッフェル記号による定義を採用すると、反対称性が自動的に保証されます。他の定義方法（例えば、$c_{ijk}$ の代わりに偏微分を単純に振り分ける方法）では、この性質が成り立たない場合があります。
クリストッフェル記号は微分幾何学における「接続」に対応しており、測地線（エネルギー最小の経路）を記述する自然な量です。物理と幾何学の深い結びつきがここに現れています。
反対称性という構造的性質を理解したところで、次は運動方程式の最後の項 — 重力が姿勢に与える影響 — を見ていきましょう。
重力項 $\bm{g}(\bm{q})$ と宇宙環境の特殊性
直感的理解 — 姿勢を保つためのトルク
テーブルの上に置かれたロボットアームが水平に腕を伸ばしている場面を想像してください。モータの電源を切ったら何が起こるでしょうか。腕は重力で下に落ちてしまいます。つまり、腕を水平に保つだけでもモータはトルクを出し続けなければなりません。
重力項 $\bm{g}(\bm{q})$ は、この「姿勢を維持するために重力に抗して出さなければならないトルク」を表します。$\ddot{\bm{q}} = \bm{0}$（加速なし）かつ $\dot{\bm{q}} = \bm{0}$（速度ゼロ）の静止状態では、運動方程式は次のようになります。
$$ \bm{g}(\bm{q}) = \bm{\tau} $$
つまり、ロボットが特定の姿勢で静止するためには、重力項と等しいトルクを出し続ける必要があります。
数学的定義 — ポテンシャルエネルギーの勾配
重力項は、系全体の重力ポテンシャルエネルギー $U(\bm{q})$ の一般化座標による偏微分として定義されます。
$$ g_i(\bm{q}) = \frac{\partial U}{\partial q_i} $$
ここで $U(\bm{q})$ は各リンクの重心位置における重力ポテンシャルの総和です。
$$ U(\bm{q}) = \sum_{i=1}^{n} m_i g_0 h_i(\bm{q}) $$
$g_0$ は重力加速度、$h_i(\bm{q})$ はリンク $i$ の重心の高さです。
ベクトルとしてまとめると次のようになります。
$$ \bm{g}(\bm{q}) = \frac{\partial U}{\partial \bm{q}} = \nabla_{\bm{q}} U(\bm{q}) $$
重力項が速度 $\dot{\bm{q}}$ に依存しない（姿勢 $\bm{q}$ のみの関数）ことは、重力が保存力であることの反映です。
2リンクアームの重力項
2リンク平面アーム（鉛直面内）の場合、各リンクの重心高さは次のように表されます。
$$ h_1 = l_{c1}\sin q_1 $$
$$ h_2 = l_1 \sin q_1 + l_{c2}\sin(q_1 + q_2) $$
ここで $l_1$ は第1リンクの長さ、$l_{c1}$, $l_{c2}$ はそれぞれのリンクの重心までの距離です。ポテンシャルエネルギーは次のようになります。
$$ U = (m_1 l_{c1} + m_2 l_1)g_0 \sin q_1 + m_2 l_{c2} g_0 \sin(q_1 + q_2) $$
$q_1$ と $q_2$ で偏微分すると、重力項が得られます。
$$ g_1 = (m_1 l_{c1} + m_2 l_1)g_0 \cos q_1 + m_2 l_{c2} g_0 \cos(q_1 + q_2) $$
$$ g_2 = m_2 l_{c2} g_0 \cos(q_1 + q_2) $$
$q_1 = 0$（水平）のとき重力トルクが最大になり、$q_1 = \pm\pi/2$（鉛直）のとき最小（ゼロ）になることが読み取れます。腕を水平に保つのが最も「つらい」姿勢であるという直感と一致しています。
宇宙環境の特殊性 — $\bm{g} = \bm{0}$ の世界
宇宙空間（軌道上の微小重力環境）では、重力加速度 $g_0 \approx 0$ であるため、次のようになります。
$$ \bm{g}(\bm{q}) = \bm{0} $$
運動方程式は次のように簡略化されます。
$$ \bm{M}(\bm{q})\ddot{\bm{q}} + \bm{C}(\bm{q},\dot{\bm{q}})\dot{\bm{q}} = \bm{\tau} $$
一見すると「重力がない分、制御が簡単になる」と思えるかもしれません。しかし、宇宙ロボティクスには地上にはない特有の課題があります。
1. 重力補償が不要になるメリット
地上のロボットでは、モータは常に重力に抗するトルクを出し続けなければなりません。これがモータの定格トルクの大部分を占めることもあります。宇宙では重力補償が不要なため、その分のトルクを加減速に使うことができ、同じモータでもより高速な動作が可能になります。
2. コリオリ・遠心力は残る
重力がゼロでも、コリオリ力と遠心力は消えません。これらは「回転」から生じる力であり、重力とは無関係です。むしろ、宇宙ロボットは微小重力のおかげで高速動作しやすくなるため、速度の2乗に比例するコリオリ・遠心力がかえって大きくなる場合があります。
3. 自由浮遊ベースの問題
宇宙ロボットが衛星やスペースステーションに固定されている場合、マニピュレータを動かすと反力が衛星本体に伝わり、衛星の姿勢が変化します。これは作用・反作用の法則の帰結であり、運動方程式は「本体 + マニピュレータ」の結合系として記述する必要があります。
$$ \begin{pmatrix} \bm{M}_{bb} & \bm{M}_{bm} \\ \bm{M}_{mb} & \bm{M}_{mm} \end{pmatrix} \begin{pmatrix} \ddot{\bm{q}}_b \\ \ddot{\bm{q}}_m \end{pmatrix} + \begin{pmatrix} \bm{C}_b \\ \bm{C}_m \end{pmatrix} = \begin{pmatrix} \bm{\tau}_b \\ \bm{\tau}_m \end{pmatrix} $$
ここで添字 $b$ は衛星本体、$m$ はマニピュレータを表します。衛星にスラスタがない場合 $\bm{\tau}_b = \bm{0}$ となり、運動量保存則から追加の拘束条件が生まれます。この結合効果を無視すると、マニピュレータの先端が狙った場所に到達できなくなります。
4. ダンピングの欠如
地上では重力が一種のダンピング効果（エネルギーの散逸）を果たしますが、宇宙ではそれがありません。一度動き出したマニピュレータは能動的にブレーキをかけない限り止まりません。制御の応答性と安定性のバランスがより重要になります。
宇宙環境の特殊性を踏まえると、運動方程式の各項を正確に把握することの重要性が一層増します。次に、これらの項を計算するために必要な動力学パラメータをどのように特定するかを見ていきましょう。
動力学パラメータの同定
なぜパラメータ同定が必要か
計算トルク法のような制御手法では、$\bm{M}(\bm{q})$, $\bm{C}(\bm{q},\dot{\bm{q}})$, $\bm{g}(\bm{q})$ を正確に計算する必要があります。しかし、これらを計算するためには各リンクの動力学パラメータ — 質量、重心位置、慣性テンソル — を知らなければなりません。
CAD図面から計算する方法もありますが、実機では次のような理由でCAD値と実際の値にずれが生じます。
- ケーブルや配管の質量が無視されている
- 加工誤差や組み立て誤差
- ペイロード（把持物体）の特性が事前にわからない
- 宇宙ロボットの場合、宇宙環境での経年変化（デガッシングによる質量減少など）
そこで、ロボットの運動データ（関節角度、速度、加速度、トルク）から動力学パラメータを推定する 動力学パラメータ同定（dynamic parameter identification）が必要になります。
動力学方程式の線形パラメータ化
パラメータ同定のカギとなるのは、マニピュレータの運動方程式が動力学パラメータについて 線形 であるという性質です。
各リンク $i$ の動力学パラメータを次のようにまとめます。
- 質量: $m_i$
- 重心位置（リンク座標系）: $l_{cx,i}$, $l_{cy,i}$, $l_{cz,i}$
- 質量 $\times$ 重心位置の積: $ml_{x,i} = m_i l_{cx,i}$ 等（第1モーメント）
- 慣性テンソルの6成分: $I_{xx,i}$, $I_{xy,i}$, $I_{xz,i}$, $I_{yy,i}$, $I_{yz,i}$, $I_{zz,i}$
1リンクあたり10個のパラメータがあり、$n$ 自由度ロボットでは最大 $10n$ 個のパラメータになります。これらをまとめたパラメータベクトルを $\bm{\theta}$ とすると、運動方程式は次のように書き直せます。
$$ \bm{M}(\bm{q})\ddot{\bm{q}} + \bm{C}(\bm{q},\dot{\bm{q}})\dot{\bm{q}} + \bm{g}(\bm{q}) = \bm{Y}(\bm{q}, \dot{\bm{q}}, \ddot{\bm{q}}) \bm{\theta} $$
ここで $\bm{Y}(\bm{q}, \dot{\bm{q}}, \ddot{\bm{q}})$ は リグレッサ行列（regressor matrix）と呼ばれ、$n \times 10n$ の行列です。この行列はパラメータ $\bm{\theta}$ を含まず、関節の運動（位置、速度、加速度）のみから計算できます。
最小二乗法による同定
リグレッサ行列の性質を利用すると、パラメータ同定は線形最小二乗問題に帰着します。
複数の姿勢・速度・加速度のデータ $(\bm{q}_k, \dot{\bm{q}}_k, \ddot{\bm{q}}_k, \bm{\tau}_k)$ を $k = 1, 2, \dots, N$ 個収集して、次の連立方程式を立てます。
$$ \begin{pmatrix} \bm{Y}_1 \\ \bm{Y}_2 \\ \vdots \\ \bm{Y}_N \end{pmatrix} \bm{\theta} = \begin{pmatrix} \bm{\tau}_1 \\ \bm{\tau}_2 \\ \vdots \\ \bm{\tau}_N \end{pmatrix} $$
これを短く $\bm{W}\bm{\theta} = \bm{d}$ と書くと、最小二乗解は次のように求まります。
$$ \hat{\bm{\theta}} = (\bm{W}^T\bm{W})^{-1}\bm{W}^T\bm{d} $$
基底パラメータ
ここで注意すべき点があります。$10n$ 個のパラメータの全てが独立に同定可能とは限りません。運動方程式に影響を与えないパラメータの組み合わせ（冗長パラメータ）が存在する場合があります。
例えば、回転関節の回転軸方向の質量分布は、その関節のトルクに影響を与えません。このような冗長性を取り除いた最小限のパラメータ集合を 基底パラメータ（base parameters）と呼びます。基底パラメータの数は $10n$ より少なくなり、$\bm{W}^T\bm{W}$ の条件数（行列の計算安定性の指標）が改善されます。
宇宙ロボットにおける同定の課題
宇宙ロボットのパラメータ同定には、地上のロボットにはない特有の困難があります。
加速度の計測が困難: 微小重力環境では、加速度センサのバイアスやドリフトが問題になります。数値微分によるノイズ増幅も深刻です。
トルク計測の不確実性: 宇宙用の関節にはトルクセンサが搭載されていない場合があります。モータ電流からトルクを推定しますが、摩擦モデルの不確実性が残ります。
実験機会の制限: 地上では何度でも実験を繰り返せますが、宇宙ではロボットの動作時間や電力が限られています。少数のデータから効率的にパラメータを同定する手法が求められます。
適応制御との融合: パラメータが不確実な場合、パラメータ推定と制御を同時に行う 適応制御 が有効です。ここでもリグレッサ行列の線形性と、$\dot{\bm{M}} – 2\bm{C}$ の反対称性が安定性証明の要になります。
理論的な枠組みを一通り整理しました。ここからは、2リンクアームを題材にして、慣性行列、コリオリ項、重力項の振る舞いをPythonで可視化し、直感的な理解を深めましょう。
Pythonで2リンクアームの各項を可視化する
2リンクアームのパラメータ設定
まず、2リンク平面アームのパラメータと動力学行列を計算する関数を定義します。
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Ellipse
from matplotlib.gridspec import GridSpec
# --- 2リンクアームのパラメータ ---
m1, m2 = 3.0, 2.0 # リンク質量 [kg]
l1, l2 = 1.0, 0.8 # リンク長 [m]
lc1, lc2 = 0.5, 0.4 # 重心までの距離 [m]
I1 = m1 * l1**2 / 12 # 慣性モーメント（一様な棒） [kg m^2]
I2 = m2 * l2**2 / 12
g0 = 9.81 # 重力加速度 [m/s^2]
# 動力学パラメータ（簡略化した定数）
alpha = I1 + I2 + m1 * lc1**2 + m2 * (l1**2 + lc2**2)
beta = m2 * l1 * lc2
delta = I2 + m2 * lc2**2
def inertia_matrix(q2):
"""慣性行列 M(q) を計算"""
c2 = np.cos(q2)
M = np.array([
[alpha + 2 * beta * c2, delta + beta * c2],
[delta + beta * c2, delta]
])
return M
def coriolis_matrix(q2, dq1, dq2):
"""コリオリ行列 C(q, dq) を計算（クリストッフェル記号ベース）"""
h = -beta * np.sin(q2)
C = np.array([
[h * dq2, h * (dq1 + dq2)],
[-h * dq1, 0.0]
])
return C
def gravity_vector(q1, q2):
"""重力項 g(q) を計算"""
g1 = (m1 * lc1 + m2 * l1) * g0 * np.cos(q1) + m2 * lc2 * g0 * np.cos(q1 + q2)
g2 = m2 * lc2 * g0 * np.cos(q1 + q2)
return np.array([g1, g2])
このコードでは、先ほど導出した2リンクアームの慣性行列、コリオリ行列、重力項をそのまま関数として実装しています。パラメータは質量 $m_1 = 3\,\text{kg}$, $m_2 = 2\,\text{kg}$、リンク長 $l_1 = 1\,\text{m}$, $l_2 = 0.8\,\text{m}$ の現実的な値を設定しました。
慣性行列の姿勢依存性と慣性楕円体の可視化
慣性行列が姿勢によってどう変化するかを見てみましょう。第2関節角度 $q_2$ を変えたときの慣性行列の各要素と、対応する慣性楕円体を可視化します。
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Ellipse
# パラメータ（前のコードブロックと同じ）
m1, m2 = 3.0, 2.0
l1, l2 = 1.0, 0.8
lc1, lc2 = 0.5, 0.4
I1 = m1 * l1**2 / 12
I2 = m2 * l2**2 / 12
g0 = 9.81
alpha = I1 + I2 + m1 * lc1**2 + m2 * (l1**2 + lc2**2)
beta = m2 * l1 * lc2
delta = I2 + m2 * lc2**2
def inertia_matrix(q2):
c2 = np.cos(q2)
return np.array([
[alpha + 2 * beta * c2, delta + beta * c2],
[delta + beta * c2, delta]
])
# q2 を変化させて慣性行列の要素をプロット
q2_range = np.linspace(-np.pi, np.pi, 200)
M11_vals, M12_vals, M22_vals = [], [], []
eigenval_max, eigenval_min = [], []
for q2 in q2_range:
M = inertia_matrix(q2)
M11_vals.append(M[0, 0])
M12_vals.append(M[0, 1])
M22_vals.append(M[1, 1])
eigvals = np.linalg.eigvalsh(M)
eigenval_min.append(eigvals[0])
eigenval_max.append(eigvals[1])
fig, axes = plt.subplots(1, 2, figsize=(14, 5))
# 左: 慣性行列の要素
ax = axes[0]
ax.plot(np.degrees(q2_range), M11_vals, label=r'$M_{11}(q_2)$', linewidth=2)
ax.plot(np.degrees(q2_range), M12_vals, label=r'$M_{12}(q_2)$', linewidth=2)
ax.plot(np.degrees(q2_range), [delta] * len(q2_range), label=r'$M_{22}$ (const)',
linewidth=2, linestyle='--')
ax.set_xlabel(r'$q_2$ [deg]', fontsize=12)
ax.set_ylabel('Inertia [kg m²]', fontsize=12)
ax.set_title('Inertia Matrix Elements vs. $q_2$', fontsize=13)
ax.legend(fontsize=11)
ax.grid(True, alpha=0.3)
# 右: 固有値
ax = axes[1]
ax.plot(np.degrees(q2_range), eigenval_max, label=r'$\lambda_{\max}$', linewidth=2)
ax.plot(np.degrees(q2_range), eigenval_min, label=r'$\lambda_{\min}$', linewidth=2)
ax.fill_between(np.degrees(q2_range), eigenval_min, eigenval_max, alpha=0.15)
ax.set_xlabel(r'$q_2$ [deg]', fontsize=12)
ax.set_ylabel('Eigenvalue [kg m²]', fontsize=12)
ax.set_title('Eigenvalues of $M(q)$ vs. $q_2$', fontsize=13)
ax.legend(fontsize=11)
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('inertia_elements_eigenvalues.png', dpi=150, bbox_inches='tight')
plt.show()
左のグラフから、$M_{11}$（第1関節の実効慣性）は $q_2$ に対して $\cos q_2$ のカーブで変化し、$q_2 = 0$（腕が伸びた状態）で最大、$q_2 = \pm 180°$（腕が折り畳まれた状態）で最小になることがわかります。これは直感通りです。腕を伸ばした方が第1関節のモーメントアームが長くなるため、「重く」感じるのです。一方、$M_{22}$（第2関節の実効慣性）は定数であり、$q_2$ によらず一定です。$M_{12}$（慣性結合項）も $\cos q_2$ に従って変化し、関節間の慣性カップリングの強さが姿勢に依存することが見て取れます。
右のグラフでは固有値の変化を示しています。最大固有値と最小固有値の比（条件数）は姿勢によって変わります。$q_2 = 0$ 付近で条件数が大きくなり、加速しやすい方向と加速しにくい方向の差が大きくなります。
次に、慣性楕円体を複数の姿勢で描画してみましょう。
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Ellipse
# パラメータ
m1, m2 = 3.0, 2.0
l1, l2 = 1.0, 0.8
lc1, lc2 = 0.5, 0.4
I1 = m1 * l1**2 / 12
I2 = m2 * l2**2 / 12
alpha = I1 + I2 + m1 * lc1**2 + m2 * (l1**2 + lc2**2)
beta = m2 * l1 * lc2
delta = I2 + m2 * lc2**2
def inertia_matrix(q2):
c2 = np.cos(q2)
return np.array([
[alpha + 2 * beta * c2, delta + beta * c2],
[delta + beta * c2, delta]
])
# 4つの姿勢での慣性楕円体
q2_samples = [0, np.pi/4, np.pi/2, np.pi]
q2_labels = ['$q_2 = 0°$\n(fully extended)', '$q_2 = 45°$',
'$q_2 = 90°$', '$q_2 = 180°$\n(folded)']
fig, axes = plt.subplots(1, 4, figsize=(16, 4))
for idx, (q2, label) in enumerate(zip(q2_samples, q2_labels)):
ax = axes[idx]
M = inertia_matrix(q2)
eigvals, eigvecs = np.linalg.eigh(M)
# 楕円体: 慣性が大きい方向は楕円が短い（動かしにくい）
# スケール: 1/sqrt(eigenvalue) に比例
scale = 0.3
width = scale / np.sqrt(eigvals[0])
height = scale / np.sqrt(eigvals[1])
angle = np.degrees(np.arctan2(eigvecs[1, 0], eigvecs[0, 0]))
ellipse = Ellipse(xy=(0, 0), width=2*width, height=2*height,
angle=angle, fill=True, alpha=0.3, color='steelblue',
edgecolor='navy', linewidth=2)
ax.add_patch(ellipse)
# 固有ベクトルの方向を矢印で表示
for i in range(2):
vec = eigvecs[:, i] * scale / np.sqrt(eigvals[i])
ax.annotate('', xy=(vec[0], vec[1]), xytext=(0, 0),
arrowprops=dict(arrowstyle='->', color='red', lw=1.5))
ax.set_xlim(-0.5, 0.5)
ax.set_ylim(-0.5, 0.5)
ax.set_aspect('equal')
ax.set_title(label, fontsize=11)
ax.set_xlabel(r'$\dot{q}_1$', fontsize=11)
if idx == 0:
ax.set_ylabel(r'$\dot{q}_2$', fontsize=11)
ax.grid(True, alpha=0.3)
ax.text(0.05, 0.95, f'$\\lambda$: {eigvals[0]:.2f}, {eigvals[1]:.2f}',
transform=ax.transAxes, fontsize=9, va='top',
bbox=dict(boxstyle='round', facecolor='wheat', alpha=0.7))
plt.suptitle('Inertia Ellipsoids at Different Configurations', fontsize=14, y=1.02)
plt.tight_layout()
plt.savefig('inertia_ellipsoids.png', dpi=150, bbox_inches='tight')
plt.show()
この図から、楕円体の形が姿勢によって大きく変化することが視覚的に確認できます。$q_2 = 0$（完全に伸ばした状態）では楕円が最も扁平で、特定の方向に非常に動かしにくいことを示しています。$q_2 = 180°$（折り畳まれた状態）では楕円がより円に近づき、どの方向にも比較的均等に動かしやすくなります。赤い矢印は固有ベクトルの方向であり、楕円の主軸に一致しています。固有値が大きい方向（慣性が大きい方向）では矢印が短く、固有値が小さい方向（慣性が小さい方向）では矢印が長くなっています。
コリオリ・遠心力の可視化
次に、関節が動いているときに生じるコリオリ・遠心力を可視化します。第1関節が一定速度 $\dot{q}_1$ で回転しているとき、第2関節角度によってどのような力が生じるかを見てみましょう。
import numpy as np
import matplotlib.pyplot as plt
# パラメータ
m1, m2 = 3.0, 2.0
l1, l2 = 1.0, 0.8
lc1, lc2 = 0.5, 0.4
I1 = m1 * l1**2 / 12
I2 = m2 * l2**2 / 12
beta = m2 * l1 * lc2
delta = I2 + m2 * lc2**2
def coriolis_vector(q2, dq1, dq2):
"""コリオリ・遠心力ベクトル C(q,dq)*dq を計算"""
h = -beta * np.sin(q2)
c1 = -2 * h * dq1 * dq2 - h * dq2**2 # 第1関節への力
c2 = h * dq1**2 # 第2関節への力
return np.array([c1, c2])
# ケース1: 第1関節のみ回転 (dq1 = 2, dq2 = 0)
# ケース2: 両関節が回転 (dq1 = 2, dq2 = 1)
# ケース3: 第2関節のみ回転 (dq1 = 0, dq2 = 2)
q2_range = np.linspace(-np.pi, np.pi, 200)
cases = [
(2.0, 0.0, '$\\dot{q}_1 = 2$, $\\dot{q}_2 = 0$'),
(2.0, 1.0, '$\\dot{q}_1 = 2$, $\\dot{q}_2 = 1$'),
(0.0, 2.0, '$\\dot{q}_1 = 0$, $\\dot{q}_2 = 2$'),
]
fig, axes = plt.subplots(1, 3, figsize=(16, 5))
for idx, (dq1, dq2, title) in enumerate(cases):
ax = axes[idx]
c1_vals = []
c2_vals = []
for q2 in q2_range:
c = coriolis_vector(q2, dq1, dq2)
c1_vals.append(c[0])
c2_vals.append(c[1])
ax.plot(np.degrees(q2_range), c1_vals, label='Joint 1 (Coriolis+Centrifugal)',
linewidth=2)
ax.plot(np.degrees(q2_range), c2_vals, label='Joint 2 (Centrifugal)',
linewidth=2, linestyle='--')
ax.axhline(y=0, color='gray', linewidth=0.5)
ax.set_xlabel(r'$q_2$ [deg]', fontsize=12)
ax.set_ylabel('Torque [N·m]', fontsize=12)
ax.set_title(title, fontsize=12)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3)
plt.suptitle('Coriolis and Centrifugal Torques vs. Configuration', fontsize=14)
plt.tight_layout()
plt.savefig('coriolis_forces.png', dpi=150, bbox_inches='tight')
plt.show()
3つのケースを比較すると、興味深い違いが見えてきます。
ケース1（$\dot{q}_1 = 2$, $\dot{q}_2 = 0$）では、第1関節のみが回転しているため、第1関節へのコリオリ・遠心力はゼロであり、第2関節のみに遠心力が作用しています。この力は $\sin q_2$ に比例し、$q_2 = \pm 90°$ で最大になります。
ケース2（$\dot{q}_1 = 2$, $\dot{q}_2 = 1$）では、両関節が回転しており、コリオリ力（速度の積に比例する項）が現れます。第1関節に作用する力が大きくなっているのは、コリオリ力と遠心力が重なっているためです。
ケース3（$\dot{q}_1 = 0$, $\dot{q}_2 = 2$）では、第2関節のみが回転しています。第2関節への遠心力はゼロ（$h\dot{q}_1^2 = 0$）であり、第1関節にのみ遠心力（$-h\dot{q}_2^2$ の項）が作用しています。第2関節の回転が第1関節にトルクを及ぼすという、関節間の動的カップリングが明確に見て取れます。
重力項の姿勢依存性の可視化
重力項が姿勢によってどう変化するかを可視化します。
import numpy as np
import matplotlib.pyplot as plt
# パラメータ
m1, m2 = 3.0, 2.0
l1, l2 = 1.0, 0.8
lc1, lc2 = 0.5, 0.4
g0 = 9.81
def gravity_vector(q1, q2):
g1 = (m1 * lc1 + m2 * l1) * g0 * np.cos(q1) + m2 * lc2 * g0 * np.cos(q1 + q2)
g2 = m2 * lc2 * g0 * np.cos(q1 + q2)
return np.array([g1, g2])
# q1, q2 を変化させて重力トルクのマップを作成
q1_range = np.linspace(-np.pi, np.pi, 100)
q2_range = np.linspace(-np.pi, np.pi, 100)
Q1, Q2 = np.meshgrid(q1_range, q2_range)
G1 = np.zeros_like(Q1)
G2 = np.zeros_like(Q2)
for i in range(len(q1_range)):
for j in range(len(q2_range)):
gv = gravity_vector(Q1[j, i], Q2[j, i])
G1[j, i] = gv[0]
G2[j, i] = gv[1]
fig, axes = plt.subplots(1, 2, figsize=(14, 5))
# 第1関節の重力トルク
im1 = axes[0].contourf(np.degrees(Q1), np.degrees(Q2), G1, levels=30, cmap='RdBu_r')
axes[0].set_xlabel(r'$q_1$ [deg]', fontsize=12)
axes[0].set_ylabel(r'$q_2$ [deg]', fontsize=12)
axes[0].set_title(r'Gravity Torque $g_1(q_1, q_2)$ [N·m]', fontsize=13)
plt.colorbar(im1, ax=axes[0])
# 第2関節の重力トルク
im2 = axes[1].contourf(np.degrees(Q1), np.degrees(Q2), G2, levels=30, cmap='RdBu_r')
axes[1].set_xlabel(r'$q_1$ [deg]', fontsize=12)
axes[1].set_ylabel(r'$q_2$ [deg]', fontsize=12)
axes[1].set_title(r'Gravity Torque $g_2(q_1, q_2)$ [N·m]', fontsize=13)
plt.colorbar(im2, ax=axes[1])
plt.suptitle('Gravity Torque Map in Joint Space', fontsize=14)
plt.tight_layout()
plt.savefig('gravity_torque_map.png', dpi=150, bbox_inches='tight')
plt.show()
左のヒートマップ（$g_1$）を見ると、第1関節の重力トルクは $q_1 = 0$（水平）付近で最大、$q_1 = \pm 90°$（鉛直）付近で最小になることがわかります。また、$q_2$ にも依存しており、腕を伸ばした姿勢（$q_2 \approx 0$）で第2リンクの重力寄与が大きくなります。
右のヒートマップ（$g_2$）は $q_1 + q_2$ のみに依存する（等高線が $q_1 + q_2 = \text{const}$ の斜め線になっている）ことが確認できます。これは第2関節の重力トルクが $\cos(q_1 + q_2)$ に比例するという理論と完全に一致しています。
反対称性 $\dot{\bm{M}} – 2\bm{C}$ の数値検証
最後に、$\dot{\bm{M}} – 2\bm{C}$ が本当に反対称であることを数値的に検証しましょう。
import numpy as np
# パラメータ
m1, m2 = 3.0, 2.0
l1, l2 = 1.0, 0.8
lc1, lc2 = 0.5, 0.4
I1 = m1 * l1**2 / 12
I2 = m2 * l2**2 / 12
beta = m2 * l1 * lc2
delta = I2 + m2 * lc2**2
alpha = I1 + I2 + m1 * lc1**2 + m2 * (l1**2 + lc2**2)
def inertia_matrix(q2):
c2 = np.cos(q2)
return np.array([
[alpha + 2 * beta * c2, delta + beta * c2],
[delta + beta * c2, delta]
])
def coriolis_matrix(q2, dq1, dq2):
h = -beta * np.sin(q2)
return np.array([
[h * dq2, h * (dq1 + dq2)],
[-h * dq1, 0.0]
])
def M_dot(q2, dq2):
"""慣性行列の時間微分 dM/dt = (dM/dq2) * dq2"""
s2 = np.sin(q2)
dMdq2 = np.array([
[-2 * beta * s2, -beta * s2],
[-beta * s2, 0.0]
])
return dMdq2 * dq2
# ランダムな姿勢・速度で反対称性をテスト
np.random.seed(42)
print("=== Skew-symmetry test: N = dM/dt - 2C ===")
print(f"{'q2 [deg]':>10} {'dq1':>8} {'dq2':>8} {'N + N^T (should be ~0)':>30}")
print("-" * 60)
for _ in range(6):
q2 = np.random.uniform(-np.pi, np.pi)
dq1 = np.random.uniform(-3, 3)
dq2 = np.random.uniform(-3, 3)
Mdot = M_dot(q2, dq2)
C = coriolis_matrix(q2, dq1, dq2)
N = Mdot - 2 * C
# 反対称ならば N + N^T = 0
skew_check = N + N.T
max_err = np.max(np.abs(skew_check))
print(f"{np.degrees(q2):10.1f} {dq1:8.2f} {dq2:8.2f} {max_err:30.2e}")
print()
print("全てのケースで N + N^T ≈ 0 となり、反対称性が確認されました。")
出力を見ると、全てのランダムな姿勢・速度の組み合わせにおいて、$\bm{N} + \bm{N}^T$ のノルムが数値誤差（$10^{-16}$ 程度）のレベルでゼロになっています。これにより、クリストッフェル記号ベースのコリオリ行列定義が反対称性を確かに満たすことが数値的にも裏付けられました。
各項のトルク寄与の比較
最後に、2リンクアームが特定の運動をしたときに、慣性項・コリオリ項・重力項がそれぞれどの程度のトルクに寄与するかを時間変化で比較します。
import numpy as np
import matplotlib.pyplot as plt
# パラメータ
m1, m2 = 3.0, 2.0
l1, l2 = 1.0, 0.8
lc1, lc2 = 0.5, 0.4
I1 = m1 * l1**2 / 12
I2 = m2 * l2**2 / 12
g0 = 9.81
alpha = I1 + I2 + m1 * lc1**2 + m2 * (l1**2 + lc2**2)
beta = m2 * l1 * lc2
delta = I2 + m2 * lc2**2
def inertia_matrix(q2):
c2 = np.cos(q2)
return np.array([
[alpha + 2 * beta * c2, delta + beta * c2],
[delta + beta * c2, delta]
])
def coriolis_vector(q2, dq1, dq2):
h = -beta * np.sin(q2)
return np.array([
-2 * h * dq1 * dq2 - h * dq2**2,
h * dq1**2
])
def gravity_vector(q1, q2):
g1 = (m1 * lc1 + m2 * l1) * g0 * np.cos(q1) + m2 * lc2 * g0 * np.cos(q1 + q2)
g2 = m2 * lc2 * g0 * np.cos(q1 + q2)
return np.array([g1, g2])
# 運動の定義: 正弦波的な関節運動
t = np.linspace(0, 3, 300)
q1 = np.pi/4 * np.sin(2 * np.pi * 0.5 * t)
q2 = np.pi/3 * np.sin(2 * np.pi * 0.8 * t + np.pi/4)
dq1 = np.pi/4 * 2 * np.pi * 0.5 * np.cos(2 * np.pi * 0.5 * t)
dq2 = np.pi/3 * 2 * np.pi * 0.8 * np.cos(2 * np.pi * 0.8 * t + np.pi/4)
ddq1 = -np.pi/4 * (2 * np.pi * 0.5)**2 * np.sin(2 * np.pi * 0.5 * t)
ddq2 = -np.pi/3 * (2 * np.pi * 0.8)**2 * np.sin(2 * np.pi * 0.8 * t + np.pi/4)
# 各項のトルク寄与を計算
tau_inertia = np.zeros((len(t), 2))
tau_coriolis = np.zeros((len(t), 2))
tau_gravity = np.zeros((len(t), 2))
tau_total = np.zeros((len(t), 2))
for i in range(len(t)):
M = inertia_matrix(q2[i])
ddq = np.array([ddq1[i], ddq2[i]])
dq = np.array([dq1[i], dq2[i]])
tau_i = M @ ddq
tau_c = coriolis_vector(q2[i], dq1[i], dq2[i])
tau_g = gravity_vector(q1[i], q2[i])
tau_inertia[i] = tau_i
tau_coriolis[i] = tau_c
tau_gravity[i] = tau_g
tau_total[i] = tau_i + tau_c + tau_g
fig, axes = plt.subplots(2, 1, figsize=(12, 8), sharex=True)
for joint_idx, ax in enumerate(axes):
ax.plot(t, tau_inertia[:, joint_idx], label='Inertia $M\\ddot{q}$',
linewidth=2, alpha=0.8)
ax.plot(t, tau_coriolis[:, joint_idx], label='Coriolis $C\\dot{q}$',
linewidth=2, alpha=0.8)
ax.plot(t, tau_gravity[:, joint_idx], label='Gravity $g$',
linewidth=2, alpha=0.8)
ax.plot(t, tau_total[:, joint_idx], label='Total $\\tau$',
linewidth=2.5, color='black', linestyle='--')
ax.set_ylabel(f'Joint {joint_idx + 1} Torque [N·m]', fontsize=12)
ax.set_title(f'Torque Contributions — Joint {joint_idx + 1}', fontsize=13)
ax.legend(fontsize=10, loc='upper right')
ax.grid(True, alpha=0.3)
axes[1].set_xlabel('Time [s]', fontsize=12)
plt.tight_layout()
plt.savefig('torque_contributions.png', dpi=150, bbox_inches='tight')
plt.show()
この時間変化のグラフから、いくつかの重要な観察ができます。
-
重力項が支配的: 地上のロボットアームでは、重力項のトルクが全体の中で最も大きな割合を占めています。特に第1関節では、重力トルクが慣性トルクやコリオリトルクを大きく上回る時間帯があります。これが「地上ロボットでは重力補償が重要」と言われる所以です。
-
コリオリ項は速度が大きいときに影響: コリオリ・遠心力のトルクは、速度が最大になるタイミング（位置の変化が激しいとき）で大きくなっています。低速動作では無視できるレベルですが、高速動作では無視できなくなります。
-
慣性項は加速度に比例: 慣性トルクは、関節加速度（$\ddot{q}$）が大きいとき — つまり動きの折り返し点 — でピークを迎えます。正弦波運動では、位置が最大・最小のときに加速度が最大になるため、その近辺で慣性トルクが大きくなっています。
-
各項の位相がずれている: 慣性項（加速度に比例）、コリオリ項（速度に比例）、重力項（位置に依存）は互いに異なる位相で変動するため、これらが打ち消し合ったり強め合ったりします。正確な制御にはこれら全てを同時に考慮する必要があります。
まとめ
本記事では、マニピュレータの運動方程式 $\bm{M}(\bm{q})\ddot{\bm{q}} + \bm{C}(\bm{q},\dot{\bm{q}})\dot{\bm{q}} + \bm{g}(\bm{q}) = \bm{\tau}$ の各項の物理的意味を深掘りしました。
-
慣性行列 $\bm{M}(\bm{q})$ は姿勢によって変化する「動かしにくさ」を表す正定値対称行列であり、対角要素は各関節の実効慣性、非対角要素は関節間の慣性結合を表します。慣性楕円体を通じて、動かしやすい方向と動かしにくい方向を幾何学的に把握できます。
-
コリオリ・遠心力項 $\bm{C}(\bm{q},\dot{\bm{q}})\dot{\bm{q}}$ は関節の回転から生まれる見かけの力であり、クリストッフェル記号によって体系的に記述されます。速度の2乗に比例するため、高速動作で特に重要になります。
-
反対称性 $\dot{\bm{M}} – 2\bm{C}$ はエネルギー保存から導かれる構造的性質であり、リアプノフ安定性解析において制御設計を大幅に簡略化します。
-
重力項 $\bm{g}(\bm{q})$ はポテンシャルエネルギーの勾配であり、宇宙空間ではゼロになります。ただし、宇宙ロボットでは自由浮遊ベースとの結合やダンピングの欠如など、別の課題が生じます。
-
動力学パラメータの同定 では、運動方程式の線形パラメータ化を利用して、実測データから質量、重心位置、慣性テンソルを推定できます。
これらの各項の正確な理解は、次のステップである 計算トルク法（computed torque control）の基盤になります。計算トルク法では、$\bm{M}$, $\bm{C}$, $\bm{g}$ を用いてモデルベースの非線形フィードバック制御を構成し、マニピュレータに線形的な応答を実現させます。
次の記事では、この動力学モデルを制御に活用する方法を解説します。
- 計算トルク法によるマニピュレータ制御 — 本記事で理解した各項を使って、モデルベース制御を実装する