ロボットアームを「ある位置に動かしたい」と思ったとき、まず必要なのは運動学 — 関節角度と手先位置の幾何学的な関係です。しかし、実際にモーターを回してアームを動かそうとすると、すぐに新しい疑問が浮かびます。「各関節にどれだけのトルクを加えれば、アームは望みどおりに動くのか？」
これが動力学（dynamics）の問いです。運動学が「どこにいるか」を記述するのに対し、動力学は「どうやって動くか」を記述します。宇宙ステーションのロボットアームが太陽電池パネルを把持して移動する場面を想像してください。各関節モーターの出力を正確に計算しなければ、パネルをぶつけてしまうかもしれません。地上のロボットでも、高速で正確な動作を実現するには動力学モデルが不可欠です。
マニピュレータの動力学を理解すると、以下のような応用に直結します。
- 制御系設計: 計算トルク法（computed torque method）やフィードフォワード制御では、動力学モデルを直接使ってトルクを計算します
- シミュレーション: 新しいロボットアームの設計段階で、関節モーターのスペック選定や軌道計画のために動力学シミュレーションが必須です
- 宇宙ロボティクス: 微小重力環境では、重力トルクの補償が不要になる代わりに、反力がベース衛星に伝わるため、より精密な動力学モデルが求められます
本記事の内容
- ラグランジュの運動方程式の復習と、マニピュレータへの適用方針
- 運動エネルギーとポテンシャルエネルギーの一般的な定式化
- 1リンクマニピュレータ（振り子）での具体的な導出
- 2リンクマニピュレータの完全な運動方程式導出（省略なし）
- 標準形 $\bm{M}(\bm{q})\ddot{\bm{q}} + \bm{C}(\bm{q}, \dot{\bm{q}})\dot{\bm{q}} + \bm{g}(\bm{q}) = \bm{\tau}$ の物理的意味
- Pythonで2リンクマニピュレータの自由運動をシミュレーション
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
- 剛体の姿勢表現と同次変換行列 — 座標変換の基礎。リンクの位置・姿勢を表す方法
- DHパラメータと順運動学 — 関節角度から手先位置を求める方法。運動学の基礎
解析力学（ラグランジアン、一般化座標、一般化力）の基本的な考え方を知っていると、本記事の理解がスムーズになります。ただし、必要な内容は本文中で復習しますので、初めて触れる方でも読み進められます。
運動学と動力学 — 何が違うのか
運動学: 幾何学の世界
運動学（kinematics）は、力やトルクを考えずに、関節変数と手先の位置・姿勢の関係だけを扱います。たとえば、2リンクのロボットアームで関節角度が $q_1 = 30°$, $q_2 = 45°$ のとき、手先の座標 $(x, y)$ はいくつかを計算するのが順運動学です。
これは地図上で「東に3km、北に4km歩いたら、出発点からどこにいるか」を計算するのと同じで、歩く速度や体力（=力学）は関係ありません。
動力学: 力とトルクの世界
動力学（dynamics）は、力・トルクと運動の因果関係を扱います。ロボットアームの各リンクには質量があり、重力が作用し、関節モーターがトルクを発生します。「このトルクを加えると、各関節がどのように加速するか」を記述するのが動力学の運動方程式です。
動力学には2つの問題があります。
| 問題の種類 | 入力 | 出力 | 用途 |
|---|---|---|---|
| 順動力学（forward dynamics） | 関節トルク $\bm{\tau}$ | 関節加速度 $\ddot{\bm{q}}$ | シミュレーション |
| 逆動力学（inverse dynamics） | 関節加速度 $\ddot{\bm{q}}$ | 関節トルク $\bm{\tau}$ | 制御、モーター選定 |
本記事ではラグランジュ法を使って運動方程式そのものを導出し、順動力学の観点からシミュレーションを行います。逆動力学の効率的な計算法（ニュートン・オイラー法）は次の記事で扱います。
運動学と動力学の違いが明確になったところで、動力学の運動方程式を導くための強力なツール — ラグランジュの運動方程式を復習しましょう。
ラグランジュの運動方程式 — エネルギーから運動を導く
なぜラグランジュ法なのか
ニュートンの運動方程式 $\bm{F} = m\bm{a}$ はシンプルですが、マニピュレータのように複数のリンクが連鎖した機構に直接適用しようとすると、関節の拘束力（リンク同士をつなぐ力）を一つ一つ考慮しなければなりません。2リンクなら何とかなりますが、6軸ロボットでは手に負えなくなります。
一方、ラグランジュ法はエネルギーベースのアプローチです。系のエネルギーさえ書ければ、拘束力は自動的に消えて、独立な関節変数についての運動方程式が得られます。これがマニピュレータ動力学でラグランジュ法が広く使われる理由です。
大雑把なイメージとしては、「力の釣り合いを逐一追う代わりに、系全体のエネルギー収支から運動を導く」のがラグランジュ法です。家計簿で言えば、全ての取引を追うのではなく、月初と月末の残高の差から全体の収支を把握するようなものです。
ラグランジアンの定義
一般化座標 $\bm{q} = (q_1, q_2, \ldots, q_n)^T$ で記述される $n$ 自由度の力学系に対して、ラグランジアン $L$ は運動エネルギー $K$ とポテンシャルエネルギー $U$ の差として定義されます。
$$ \begin{equation} L(\bm{q}, \dot{\bm{q}}) = K(\bm{q}, \dot{\bm{q}}) – U(\bm{q}) \end{equation} $$
ここで $\dot{\bm{q}} = (\dot{q}_1, \dot{q}_2, \ldots, \dot{q}_n)^T$ は一般化速度です。マニピュレータの場合、一般化座標は各関節角度（回転関節の場合）または関節変位（直動関節の場合）です。
ラグランジュの運動方程式
ラグランジアンから、各一般化座標 $q_i$ についての運動方程式が次のように得られます。
$$ \begin{equation} \frac{d}{dt}\frac{\partial L}{\partial \dot{q}_i} – \frac{\partial L}{\partial q_i} = \tau_i, \quad i = 1, 2, \ldots, n \end{equation} $$
右辺の $\tau_i$ は一般化座標 $q_i$ に対応する一般化力です。回転関節なら関節トルク [Nm]、直動関節なら関節力 [N] になります。
この方程式の左辺を言葉で読み下すと、次のようになります。
- $\frac{\partial L}{\partial \dot{q}_i}$: ラグランジアンを一般化速度 $\dot{q}_i$ で偏微分する（これは「一般化運動量」に対応）
- $\frac{d}{dt}$: それを時間微分する（運動量の時間変化率 = 力に対応）
- $\frac{\partial L}{\partial q_i}$: ラグランジアンを一般化座標 $q_i$ で偏微分する（位置に依存する力の効果）
- 差を取ると、一般化力 $\tau_i$ に等しい
この方程式が強力なのは、拘束力が自動的に消えることです。一般化座標を使うことで、リンク間の内力を考慮する必要がなくなり、外部から加えるトルク $\tau_i$ だけが右辺に残ります。
ラグランジュの運動方程式の形が分かったので、次にマニピュレータのエネルギーを具体的に計算していきましょう。まずは運動エネルギーからです。
マニピュレータの運動エネルギー
1つのリンクの運動エネルギー
マニピュレータの運動エネルギーを計算するには、各リンクの運動エネルギーを足し合わせます。1つの剛体リンクの運動エネルギーは、重心の並進運動エネルギーと重心まわりの回転運動エネルギーの和です。
$$ \begin{equation} K_i = \frac{1}{2} m_i \bm{v}_{c_i}^T \bm{v}_{c_i} + \frac{1}{2} \bm{\omega}_i^T \bm{I}_{c_i} \bm{\omega}_i \end{equation} $$
ここで各記号の意味は以下のとおりです。
| 記号 | 意味 |
|---|---|
| $m_i$ | リンク $i$ の質量 |
| $\bm{v}_{c_i}$ | リンク $i$ の重心の並進速度ベクトル |
| $\bm{\omega}_i$ | リンク $i$ の角速度ベクトル |
| $\bm{I}_{c_i}$ | リンク $i$ の重心まわりの慣性テンソル（3×3対称行列） |
この式は「質量 × 速度の2乗 / 2」を並進と回転の両方について書いたものです。質点なら並進の項だけで済みますが、マニピュレータのリンクは有限の大きさを持つ剛体なので、回転の項も必要です。
ヤコビアンを使った速度の表現
重心の並進速度 $\bm{v}_{c_i}$ と角速度 $\bm{\omega}_i$ は、関節速度 $\dot{\bm{q}}$ を使って表すことができます。これがヤコビアンです。
$$ \bm{v}_{c_i} = \bm{J}_{v_i}(\bm{q}) \dot{\bm{q}}, \quad \bm{\omega}_i = \bm{J}_{\omega_i}(\bm{q}) \dot{\bm{q}} $$
$\bm{J}_{v_i}$ はリンク $i$ の重心の並進ヤコビアン（$3 \times n$ 行列）、$\bm{J}_{\omega_i}$ は角速度ヤコビアン（$3 \times n$ 行列）です。これらは関節角度 $\bm{q}$ に依存します。
ヤコビアンを使うことで、リンク $i$ の運動エネルギーは次のように書き換えられます。
$\bm{v}_{c_i} = \bm{J}_{v_i} \dot{\bm{q}}$ を代入すると
$$ \frac{1}{2} m_i \bm{v}_{c_i}^T \bm{v}_{c_i} = \frac{1}{2} m_i \dot{\bm{q}}^T \bm{J}_{v_i}^T \bm{J}_{v_i} \dot{\bm{q}} $$
同様に $\bm{\omega}_i = \bm{J}_{\omega_i} \dot{\bm{q}}$ を代入すると
$$ \frac{1}{2} \bm{\omega}_i^T \bm{I}_{c_i} \bm{\omega}_i = \frac{1}{2} \dot{\bm{q}}^T \bm{J}_{\omega_i}^T \bm{I}_{c_i} \bm{J}_{\omega_i} \dot{\bm{q}} $$
これらを合わせると、リンク $i$ の運動エネルギーは
$$ K_i = \frac{1}{2} \dot{\bm{q}}^T \left( m_i \bm{J}_{v_i}^T \bm{J}_{v_i} + \bm{J}_{\omega_i}^T \bm{I}_{c_i} \bm{J}_{\omega_i} \right) \dot{\bm{q}} $$
全体の運動エネルギーと慣性行列
$n$ リンクのマニピュレータ全体の運動エネルギーは、各リンクの運動エネルギーの和です。
$$ K = \sum_{i=1}^{n} K_i = \frac{1}{2} \dot{\bm{q}}^T \left[ \sum_{i=1}^{n} \left( m_i \bm{J}_{v_i}^T \bm{J}_{v_i} + \bm{J}_{\omega_i}^T \bm{I}_{c_i} \bm{J}_{\omega_i} \right) \right] \dot{\bm{q}} $$
角括弧の中を 慣性行列（inertia matrix）$\bm{M}(\bm{q})$ と定義します。
$$ \begin{equation} \bm{M}(\bm{q}) = \sum_{i=1}^{n} \left( m_i \bm{J}_{v_i}^T(\bm{q}) \bm{J}_{v_i}(\bm{q}) + \bm{J}_{\omega_i}^T(\bm{q}) \bm{I}_{c_i} \bm{J}_{\omega_i}(\bm{q}) \right) \end{equation} $$
すると全体の運動エネルギーは非常にコンパクトに書けます。
$$ \begin{equation} K = \frac{1}{2} \dot{\bm{q}}^T \bm{M}(\bm{q}) \dot{\bm{q}} \end{equation} $$
慣性行列 $\bm{M}(\bm{q})$ は $n \times n$ の対称正定値行列です。対称性は定義から明らかで、正定値性は「$\dot{\bm{q}} \neq \bm{0}$ なら運動エネルギーが正」であることから従います。重要なのは、$\bm{M}$ が関節角度 $\bm{q}$ に依存すること — つまりアームの姿勢が変わると慣性の「見え方」が変わるということです。
運動エネルギーの一般形が得られたので、次にポテンシャルエネルギーを定式化しましょう。
マニピュレータのポテンシャルエネルギー
重力によるポテンシャルエネルギー
地上のマニピュレータでは、ポテンシャルエネルギーの主な原因は重力です。リンク $i$ の重心位置を $\bm{r}_{c_i}(\bm{q})$ とすると、重力ポテンシャルエネルギーは
$$ \begin{equation} U = -\sum_{i=1}^{n} m_i \bm{g}_0^T \bm{r}_{c_i}(\bm{q}) \end{equation} $$
ここで $\bm{g}_0$ は重力加速度ベクトルです。たとえばベース座標系で $z$ 軸が鉛直上向きなら $\bm{g}_0 = (0, 0, -g)^T$ です。このとき
$$ U = \sum_{i=1}^{n} m_i g \, z_{c_i}(\bm{q}) $$
となり、各リンクの重心高さ $z_{c_i}$ に質量と重力加速度を掛けて合計したものになります。これは高校物理で学ぶ「位置エネルギー = $mgh$」の自然な拡張です。
宇宙空間でのポテンシャルエネルギー
宇宙空間（微小重力環境）で動作するマニピュレータでは、$\bm{g}_0 \approx \bm{0}$ なので重力ポテンシャルエネルギーはほぼゼロになります。ただし、関節にバネが組み込まれている場合は弾性ポテンシャルエネルギーが存在します。
宇宙ロボティクスの文脈では、重力項がなくなることで運動方程式が簡単になる利点がある一方、ベース衛星の反力という新たな課題が生まれます。この話題は別の記事で扱います。
ラグランジアンの2つの構成要素 — 運動エネルギーとポテンシャルエネルギー — が揃いました。いよいよ具体的なマニピュレータで運動方程式を導出してみましょう。まずは最も単純な1リンクから始めます。
具体例1: 1リンクマニピュレータ（振り子）
モデルの設定
最も単純なマニピュレータとして、1つの回転関節に1本のリンクが取り付けられたモデルを考えます。これは実質的に単振り子です。
モデルのパラメータは次のとおりです。
| パラメータ | 記号 | 意味 |
|---|---|---|
| リンク長 | $l$ | リンクの長さ |
| リンク質量 | $m$ | リンクの質量 |
| 重心位置 | $l_c$ | 関節から重心までの距離 |
| 慣性モーメント | $I$ | 重心まわりの慣性モーメント |
| 関節角度 | $q$ | 鉛直下向きから測った角度 |
座標系は、関節位置を原点とし、鉛直下向きを $q = 0$ とします。
運動エネルギーの計算
重心の位置を関節角度 $q$ で表します。
$$ x_c = l_c \sin q, \quad y_c = -l_c \cos q $$
時間微分して重心の速度を求めます。
$$ \dot{x}_c = l_c \dot{q} \cos q, \quad \dot{y}_c = l_c \dot{q} \sin q $$
重心の速度の2乗は
$$ v_c^2 = \dot{x}_c^2 + \dot{y}_c^2 = l_c^2 \dot{q}^2 (\cos^2 q + \sin^2 q) = l_c^2 \dot{q}^2 $$
三角関数の恒等式 $\cos^2 q + \sin^2 q = 1$ を使いました。
回転の角速度は単純に $\omega = \dot{q}$ です。よって運動エネルギーは
$$ K = \frac{1}{2} m l_c^2 \dot{q}^2 + \frac{1}{2} I \dot{q}^2 = \frac{1}{2}(m l_c^2 + I) \dot{q}^2 $$
ポテンシャルエネルギーの計算
基準点を関節位置にとると、重心の高さは $y_c = -l_c \cos q$ です。
$$ U = m g y_c = -m g l_c \cos q $$
$q = 0$（鉛直下向き）のとき $U = -mgl_c$ で最小値をとり、$q = \pi$（鉛直上向き）のとき $U = mgl_c$ で最大値をとります。これは直感に合います。
ラグランジアンの構成
$$ L = K – U = \frac{1}{2}(m l_c^2 + I) \dot{q}^2 + m g l_c \cos q $$
ラグランジュの運動方程式の適用
ラグランジュの運動方程式 $\frac{d}{dt}\frac{\partial L}{\partial \dot{q}} – \frac{\partial L}{\partial q} = \tau$ を適用します。
まず $L$ を $\dot{q}$ で偏微分します。
$$ \frac{\partial L}{\partial \dot{q}} = (m l_c^2 + I) \dot{q} $$
これを時間微分します。
$$ \frac{d}{dt}\frac{\partial L}{\partial \dot{q}} = (m l_c^2 + I) \ddot{q} $$
次に $L$ を $q$ で偏微分します。
$$ \frac{\partial L}{\partial q} = -m g l_c \sin q $$
以上をラグランジュの運動方程式に代入すると
$$ (m l_c^2 + I) \ddot{q} – (-m g l_c \sin q) = \tau $$
整理して
$$ \begin{equation} (m l_c^2 + I) \ddot{q} + m g l_c \sin q = \tau \end{equation} $$
これが1リンクマニピュレータの運動方程式です。
結果の読み取り
得られた運動方程式を物理的に解釈しましょう。
- $(m l_c^2 + I)$: これは慣性行列（スカラー）です。$m l_c^2$ は平行軸の定理による関節まわりの慣性モーメントの寄与、$I$ は重心まわりの慣性モーメントです。両者の和が関節まわりの慣性モーメントになります
- $m g l_c \sin q$: 重力トルクです。$q = 0$（鉛直下向き）では $\sin 0 = 0$ で重力トルクはゼロ、$q = \pi/2$（水平）では最大になります。これは直感に合います
- $\tau = 0$ とすると、これは単振り子の運動方程式そのものです
1リンクの例で導出の流れが掴めました。この手順をもう1つの関節に拡張すると、どうなるでしょうか？次は2リンクマニピュレータに挑戦します。関節間の干渉（一方の関節の動きが他方に影響を与える）が現れ、動力学の真の面白さが見えてきます。
具体例2: 2リンクマニピュレータの完全導出
モデルの設定
2リンクの平面マニピュレータを考えます。2つの回転関節と2本のリンクからなり、全ての運動は鉛直平面内で起こります。
モデルのパラメータは以下のとおりです。
| パラメータ | リンク1 | リンク2 |
|---|---|---|
| リンク長 | $l_1$ | $l_2$ |
| 質量 | $m_1$ | $m_2$ |
| 重心までの距離 | $l_{c1}$ | $l_{c2}$ |
| 重心まわりの慣性モーメント | $I_1$ | $I_2$ |
| 関節角度 | $q_1$ | $q_2$ |
座標系は、関節1の位置を原点とし、$q_1$ は鉛直下向きから測ったリンク1の角度、$q_2$ はリンク1からのリンク2の相対角度とします。
各リンクの重心位置
リンク1の重心位置:
$$ x_{c1} = l_{c1} \sin q_1 $$
$$ y_{c1} = -l_{c1} \cos q_1 $$
リンク2の重心位置:
リンク2の根元は関節2の位置 $(l_1 \sin q_1, \, -l_1 \cos q_1)$ にあります。リンク2は基準からの絶対角度 $q_1 + q_2$ で傾いているので、
$$ x_{c2} = l_1 \sin q_1 + l_{c2} \sin(q_1 + q_2) $$
$$ y_{c2} = -l_1 \cos q_1 – l_{c2} \cos(q_1 + q_2) $$
各リンクの重心速度
リンク1の重心速度:
$x_{c1}, y_{c1}$ を時間微分します。
$$ \dot{x}_{c1} = l_{c1} \dot{q}_1 \cos q_1 $$
$$ \dot{y}_{c1} = l_{c1} \dot{q}_1 \sin q_1 $$
重心速度の2乗は
$$ v_{c1}^2 = l_{c1}^2 \dot{q}_1^2 $$
リンク2の重心速度:
$x_{c2}, y_{c2}$ を時間微分します。ここで $q_1 + q_2$ の微分は $\dot{q}_1 + \dot{q}_2$ になることに注意します。
$$ \dot{x}_{c2} = l_1 \dot{q}_1 \cos q_1 + l_{c2} (\dot{q}_1 + \dot{q}_2) \cos(q_1 + q_2) $$
$$ \dot{y}_{c2} = l_1 \dot{q}_1 \sin q_1 + l_{c2} (\dot{q}_1 + \dot{q}_2) \sin(q_1 + q_2) $$
重心速度の2乗を計算します。
$$ v_{c2}^2 = \dot{x}_{c2}^2 + \dot{y}_{c2}^2 $$
各項を展開すると
$$ \dot{x}_{c2}^2 = l_1^2 \dot{q}_1^2 \cos^2 q_1 + 2 l_1 l_{c2} \dot{q}_1 (\dot{q}_1 + \dot{q}_2) \cos q_1 \cos(q_1 + q_2) + l_{c2}^2 (\dot{q}_1 + \dot{q}_2)^2 \cos^2(q_1 + q_2) $$
$$ \dot{y}_{c2}^2 = l_1^2 \dot{q}_1^2 \sin^2 q_1 + 2 l_1 l_{c2} \dot{q}_1 (\dot{q}_1 + \dot{q}_2) \sin q_1 \sin(q_1 + q_2) + l_{c2}^2 (\dot{q}_1 + \dot{q}_2)^2 \sin^2(q_1 + q_2) $$
これらを足し合わせます。$\cos^2 \alpha + \sin^2 \alpha = 1$ を使うと、第1項と第3項がそれぞれまとまります。
$$ l_1^2 \dot{q}_1^2 (\cos^2 q_1 + \sin^2 q_1) = l_1^2 \dot{q}_1^2 $$
$$ l_{c2}^2 (\dot{q}_1 + \dot{q}_2)^2 (\cos^2(q_1 + q_2) + \sin^2(q_1 + q_2)) = l_{c2}^2 (\dot{q}_1 + \dot{q}_2)^2 $$
第2項（クロスタームの和）は、加法定理の逆 $\cos A \cos B + \sin A \sin B = \cos(A – B)$ を使います。
$$ 2 l_1 l_{c2} \dot{q}_1 (\dot{q}_1 + \dot{q}_2) [\cos q_1 \cos(q_1 + q_2) + \sin q_1 \sin(q_1 + q_2)] $$
$$ = 2 l_1 l_{c2} \dot{q}_1 (\dot{q}_1 + \dot{q}_2) \cos(q_1 + q_2 – q_1) $$
$$ = 2 l_1 l_{c2} \dot{q}_1 (\dot{q}_1 + \dot{q}_2) \cos q_2 $$
したがって、リンク2の重心速度の2乗は
$$ \begin{equation} v_{c2}^2 = l_1^2 \dot{q}_1^2 + l_{c2}^2 (\dot{q}_1 + \dot{q}_2)^2 + 2 l_1 l_{c2} \dot{q}_1 (\dot{q}_1 + \dot{q}_2) \cos q_2 \end{equation} $$
$\cos q_2$ が現れるのがポイントです。リンク2の相対角度 $q_2$ がリンク2の速度に影響するということで、これが後で慣性行列の非対角成分を生みます。
運動エネルギーの計算
リンク1の運動エネルギー:
角速度は $\omega_1 = \dot{q}_1$ なので
$$ K_1 = \frac{1}{2} m_1 v_{c1}^2 + \frac{1}{2} I_1 \omega_1^2 = \frac{1}{2} m_1 l_{c1}^2 \dot{q}_1^2 + \frac{1}{2} I_1 \dot{q}_1^2 = \frac{1}{2}(m_1 l_{c1}^2 + I_1) \dot{q}_1^2 $$
リンク2の運動エネルギー:
リンク2の絶対角速度は $\omega_2 = \dot{q}_1 + \dot{q}_2$ なので
$$ K_2 = \frac{1}{2} m_2 v_{c2}^2 + \frac{1}{2} I_2 (\dot{q}_1 + \dot{q}_2)^2 $$
$v_{c2}^2$ を代入して整理します。まず $v_{c2}^2$ の各項を展開し、回転の項と合わせます。
$$ K_2 = \frac{1}{2} m_2 \left[ l_1^2 \dot{q}_1^2 + l_{c2}^2 (\dot{q}_1 + \dot{q}_2)^2 + 2 l_1 l_{c2} \dot{q}_1 (\dot{q}_1 + \dot{q}_2) \cos q_2 \right] + \frac{1}{2} I_2 (\dot{q}_1 + \dot{q}_2)^2 $$
$l_{c2}^2$ の項と $I_2$ の項をまとめます。
$$ K_2 = \frac{1}{2} m_2 l_1^2 \dot{q}_1^2 + \frac{1}{2} (m_2 l_{c2}^2 + I_2)(\dot{q}_1 + \dot{q}_2)^2 + m_2 l_1 l_{c2} \dot{q}_1 (\dot{q}_1 + \dot{q}_2) \cos q_2 $$
全体の運動エネルギー:
$$ K = K_1 + K_2 $$
ここで $\dot{q}_1^2$, $\dot{q}_2^2$, $\dot{q}_1 \dot{q}_2$ の各係数を整理します。$(\dot{q}_1 + \dot{q}_2)^2 = \dot{q}_1^2 + 2\dot{q}_1 \dot{q}_2 + \dot{q}_2^2$ を展開すると
$$ K = \frac{1}{2} \left[ m_1 l_{c1}^2 + I_1 + m_2 l_1^2 + m_2 l_{c2}^2 + I_2 + 2 m_2 l_1 l_{c2} \cos q_2 \right] \dot{q}_1^2 $$
$$ + \frac{1}{2} \left[ m_2 l_{c2}^2 + I_2 \right] \dot{q}_2^2 $$
$$ + \left[ m_2 l_{c2}^2 + I_2 + m_2 l_1 l_{c2} \cos q_2 \right] \dot{q}_1 \dot{q}_2 $$
表記を簡潔にするため、以下の記号を導入します。
$$ \alpha = m_1 l_{c1}^2 + I_1 + m_2 l_1^2 + m_2 l_{c2}^2 + I_2 $$
$$ \beta = m_2 l_1 l_{c2} $$
$$ \delta = m_2 l_{c2}^2 + I_2 $$
すると運動エネルギーは
$$ \begin{equation} K = \frac{1}{2}(\alpha + 2\beta \cos q_2) \dot{q}_1^2 + \frac{1}{2} \delta \dot{q}_2^2 + (\delta + \beta \cos q_2) \dot{q}_1 \dot{q}_2 \end{equation} $$
この運動エネルギーを行列形式 $K = \frac{1}{2} \dot{\bm{q}}^T \bm{M}(\bm{q}) \dot{\bm{q}}$ で書くと、慣性行列は
$$ \begin{equation} \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} \end{equation} $$
慣性行列に $\cos q_2$ が現れていることに注目してください。これはリンク2の相対角度 $q_2$ によって慣性の見え方が変わることを意味します。$q_2 = 0$（リンク2がリンク1と一直線）のとき、アームは最大に伸びた状態で、慣性モーメントが最大になります。
ポテンシャルエネルギーの計算
各リンクの重心高さから、ポテンシャルエネルギーは
$$ U = m_1 g y_{c1} + m_2 g y_{c2} $$
各重心の $y$ 座標を代入すると
$$ U = -m_1 g l_{c1} \cos q_1 – m_2 g (l_1 \cos q_1 + l_{c2} \cos(q_1 + q_2)) $$
整理して
$$ \begin{equation} U = -(m_1 l_{c1} + m_2 l_1) g \cos q_1 – m_2 l_{c2} g \cos(q_1 + q_2) \end{equation} $$
ラグランジアンの構成
$$ L = K – U $$
$$ = \frac{1}{2}(\alpha + 2\beta \cos q_2) \dot{q}_1^2 + \frac{1}{2} \delta \dot{q}_2^2 + (\delta + \beta \cos q_2) \dot{q}_1 \dot{q}_2 $$
$$ + (m_1 l_{c1} + m_2 l_1) g \cos q_1 + m_2 l_{c2} g \cos(q_1 + q_2) $$
関節1についての運動方程式
ラグランジュの運動方程式 $\frac{d}{dt}\frac{\partial L}{\partial \dot{q}_1} – \frac{\partial L}{\partial q_1} = \tau_1$ を適用します。
ステップ1: $\frac{\partial L}{\partial \dot{q}_1}$ を計算
$L$ の中で $\dot{q}_1$ を含む項は、$\frac{1}{2}(\alpha + 2\beta \cos q_2) \dot{q}_1^2$ と $(\delta + \beta \cos q_2) \dot{q}_1 \dot{q}_2$ です。
$$ \frac{\partial L}{\partial \dot{q}_1} = (\alpha + 2\beta \cos q_2) \dot{q}_1 + (\delta + \beta \cos q_2) \dot{q}_2 $$
ステップ2: $\frac{d}{dt}\frac{\partial L}{\partial \dot{q}_1}$ を計算
時間微分します。$\alpha, \delta$ は定数ですが、$\cos q_2$ は時間の関数であることに注意します。$\frac{d}{dt} \cos q_2 = -\dot{q}_2 \sin q_2$ を使うと
$$ \frac{d}{dt}\frac{\partial L}{\partial \dot{q}_1} = (\alpha + 2\beta \cos q_2) \ddot{q}_1 – 2\beta \dot{q}_2 \sin q_2 \cdot \dot{q}_1 $$
$$ + (\delta + \beta \cos q_2) \ddot{q}_2 – \beta \dot{q}_2 \sin q_2 \cdot \dot{q}_2 $$
$\dot{q}_2 \sin q_2$ の項をまとめると
$$ \frac{d}{dt}\frac{\partial L}{\partial \dot{q}_1} = (\alpha + 2\beta \cos q_2) \ddot{q}_1 + (\delta + \beta \cos q_2) \ddot{q}_2 – \beta \sin q_2 (2 \dot{q}_1 \dot{q}_2 + \dot{q}_2^2) $$
ステップ3: $\frac{\partial L}{\partial q_1}$ を計算
$L$ の中で $q_1$（$\dot{q}_1$ ではなく）を含む項はポテンシャルエネルギーの部分です。
$$ \frac{\partial L}{\partial q_1} = -(m_1 l_{c1} + m_2 l_1) g \sin q_1 – m_2 l_{c2} g \sin(q_1 + q_2) $$
ステップ4: 運動方程式を組み立てる
$\frac{d}{dt}\frac{\partial L}{\partial \dot{q}_1} – \frac{\partial L}{\partial q_1} = \tau_1$ に代入すると
$$ (\alpha + 2\beta \cos q_2) \ddot{q}_1 + (\delta + \beta \cos q_2) \ddot{q}_2 – \beta \sin q_2 (2 \dot{q}_1 \dot{q}_2 + \dot{q}_2^2) $$
$$ + (m_1 l_{c1} + m_2 l_1) g \sin q_1 + m_2 l_{c2} g \sin(q_1 + q_2) = \tau_1 $$
関節2についての運動方程式
同様に $\frac{d}{dt}\frac{\partial L}{\partial \dot{q}_2} – \frac{\partial L}{\partial q_2} = \tau_2$ を計算します。
ステップ1: $\frac{\partial L}{\partial \dot{q}_2}$ を計算
$$ \frac{\partial L}{\partial \dot{q}_2} = \delta \dot{q}_2 + (\delta + \beta \cos q_2) \dot{q}_1 $$
ステップ2: $\frac{d}{dt}\frac{\partial L}{\partial \dot{q}_2}$ を計算
$\cos q_2$ の時間微分に注意して
$$ \frac{d}{dt}\frac{\partial L}{\partial \dot{q}_2} = \delta \ddot{q}_2 + (\delta + \beta \cos q_2) \ddot{q}_1 – \beta \dot{q}_2 \sin q_2 \cdot \dot{q}_1 $$
ステップ3: $\frac{\partial L}{\partial q_2}$ を計算
$q_2$ を含む項は運動エネルギーの $\cos q_2$ の部分とポテンシャルエネルギーです。
$$ \frac{\partial L}{\partial q_2} = -\beta \sin q_2 \cdot \dot{q}_1^2 – \beta \sin q_2 \cdot \dot{q}_1 \dot{q}_2 – m_2 l_{c2} g \sin(q_1 + q_2) $$
ステップ4: 運動方程式を組み立てる
$$ \delta \ddot{q}_2 + (\delta + \beta \cos q_2) \ddot{q}_1 – \beta \dot{q}_1 \dot{q}_2 \sin q_2 + \beta \sin q_2 \dot{q}_1^2 + \beta \sin q_2 \dot{q}_1 \dot{q}_2 + m_2 l_{c2} g \sin(q_1 + q_2) = \tau_2 $$
$\dot{q}_1 \dot{q}_2 \sin q_2$ の項が相殺するので、整理すると
$$ (\delta + \beta \cos q_2) \ddot{q}_1 + \delta \ddot{q}_2 + \beta \dot{q}_1^2 \sin q_2 + m_2 l_{c2} g \sin(q_1 + q_2) = \tau_2 $$
2リンクの運動方程式のまとめ
導出した2つの運動方程式をまとめます。
関節1:
$$ (\alpha + 2\beta \cos q_2) \ddot{q}_1 + (\delta + \beta \cos q_2) \ddot{q}_2 – \beta \sin q_2 (2 \dot{q}_1 \dot{q}_2 + \dot{q}_2^2) $$
$$ + (m_1 l_{c1} + m_2 l_1) g \sin q_1 + m_2 l_{c2} g \sin(q_1 + q_2) = \tau_1 $$
関節2:
$$ (\delta + \beta \cos q_2) \ddot{q}_1 + \delta \ddot{q}_2 + \beta \dot{q}_1^2 \sin q_2 + m_2 l_{c2} g \sin(q_1 + q_2) = \tau_2 $$
ここで $\alpha = m_1 l_{c1}^2 + I_1 + m_2 l_1^2 + m_2 l_{c2}^2 + I_2$, $\beta = m_2 l_1 l_{c2}$, $\delta = m_2 l_{c2}^2 + I_2$ です。
1リンクの運動方程式と比べると、劇的に複雑になっていることがわかります。特に $-\beta \sin q_2 (2\dot{q}_1 \dot{q}_2 + \dot{q}_2^2)$ や $\beta \dot{q}_1^2 \sin q_2$ のような速度に依存する非線形項が現れます。これらの項の物理的意味は、次のセクションで詳しく解説します。
標準形 $\bm{M}(\bm{q})\ddot{\bm{q}} + \bm{C}(\bm{q}, \dot{\bm{q}})\dot{\bm{q}} + \bm{g}(\bm{q}) = \bm{\tau}$ の物理的意味
行列形式への整理
前セクションで導出した2リンクの運動方程式を行列形式で書くと、次のようになります。
$$ \begin{equation} \bm{M}(\bm{q})\ddot{\bm{q}} + \bm{C}(\bm{q}, \dot{\bm{q}})\dot{\bm{q}} + \bm{g}(\bm{q}) = \bm{\tau} \end{equation} $$
これが $n$ 自由度マニピュレータの運動方程式の標準形です。2リンクの場合、各行列は以下のとおりです。
慣性行列 $\bm{M}(\bm{q})$:
$$ \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} $$
コリオリ・遠心力行列 $\bm{C}(\bm{q}, \dot{\bm{q}})$:
$$ \bm{C}(\bm{q}, \dot{\bm{q}}) = \begin{pmatrix} -\beta \dot{q}_2 \sin q_2 & -\beta (\dot{q}_1 + \dot{q}_2) \sin q_2 \\ \beta \dot{q}_1 \sin q_2 & 0 \end{pmatrix} $$
重力ベクトル $\bm{g}(\bm{q})$:
$$ \bm{g}(\bm{q}) = \begin{pmatrix} (m_1 l_{c1} + m_2 l_1) g \sin q_1 + m_2 l_{c2} g \sin(q_1 + q_2) \\ m_2 l_{c2} g \sin(q_1 + q_2) \end{pmatrix} $$
ここで $\bm{C}$ の定義には一意性がないことに注意します。上の定義はクリストッフェル記号を用いた標準的な定義の一つで、$\dot{\bm{M}} – 2\bm{C}$ が歪対称行列になるという重要な性質を持ちます。この性質はロボットの制御理論で頻繁に利用されます。
各項の物理的意味
この標準形の各項を、日常的な感覚と結びつけて理解しましょう。
慣性項 $\bm{M}(\bm{q})\ddot{\bm{q}}$:
ニュートンの第二法則 $F = ma$ のロボットアーム版です。$\bm{M}$ が質量に、$\ddot{\bm{q}}$ が加速度に対応します。ただし、$\bm{M}$ は行列なので、関節1を加速すると関節2にも影響が出ます（非対角成分の効果）。
慣性行列が姿勢 $\bm{q}$ に依存するのは、アームを伸ばした状態と折りたたんだ状態で「動かしにくさ」が変わることに対応します。腕を伸ばした状態で体を回転させるのと、腕を縮めた状態で回転するのでは、必要な力が全く違います — これと同じことがロボットアームでも起きます。
コリオリ・遠心力項 $\bm{C}(\bm{q}, \dot{\bm{q}})\dot{\bm{q}}$:
速度の2乗に比例する力で、2種類の効果を含みます。
- 遠心力効果: $\dot{q}_i^2$ に比例する項。1つの関節が回転すると、他のリンクに遠心力が作用します。バケツを振り回すと水が外に飛ぶのと同じ原理です
- コリオリ力効果: $\dot{q}_i \dot{q}_j$ ($i \neq j$) に比例する項。2つの関節が同時に動くときに現れる交差項です。回転するメリーゴーラウンドの上で歩くと変な力を感じる — あれがコリオリ力です
低速動作ではこれらの項は小さくなりますが、高速動作（たとえば産業用ロボットのピック＆プレース動作）では無視できない大きさになります。
重力項 $\bm{g}(\bm{q})$:
重力がロボットアームに及ぼすトルクです。アームが水平のとき最大で、鉛直のとき（リンクが重力方向に沿っているとき）ゼロになります。
ロボットが静止している場合（$\dot{\bm{q}} = \bm{0}$, $\ddot{\bm{q}} = \bm{0}$）でも、アームを支えるために $\bm{\tau} = \bm{g}(\bm{q})$ のトルクが必要です。重い荷物を腕を伸ばして持ち続けると疲れるのと同じで、ロボットのモーターも姿勢を保持するだけで電力を消費します。
標準形の一般化
2リンクで導出した標準形 $\bm{M}(\bm{q})\ddot{\bm{q}} + \bm{C}(\bm{q}, \dot{\bm{q}})\dot{\bm{q}} + \bm{g}(\bm{q}) = \bm{\tau}$ は、任意の $n$ 自由度のマニピュレータに一般化できます。
一般的に、コリオリ・遠心力行列はクリストッフェル記号を用いて次のように定義されます。
$$ C_{ij} = \sum_{k=1}^{n} c_{ijk} \dot{q}_k $$
ここで $c_{ijk}$ は第1種クリストッフェル記号と呼ばれ、慣性行列 $\bm{M}$ の要素から計算できます。
$$ c_{ijk} = \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) $$
この定義を使うと $\bm{C}$ が一意に定まり、歪対称性 $\dot{\bm{M}} – 2\bm{C}$ が保証されます。
重力ベクトルは、ポテンシャルエネルギーの勾配として
$$ g_i(\bm{q}) = \frac{\partial U}{\partial q_i} $$
と計算できます。
標準形の物理的意味が明確になったところで、この運動方程式を実際にPythonで解いて、2リンクマニピュレータがどのように動くかを視覚的に確認しましょう。
Pythonで2リンクマニピュレータのシミュレーション
シミュレーションの目的
ここでは、2リンクマニピュレータに外部トルクを加えずに（$\bm{\tau} = \bm{0}$）、初期条件だけを与えて自由運動をシミュレーションします。これは二重振り子（double pendulum）に相当し、カオス的な振る舞いで知られています。
運動方程式の標準形から、加速度を次のように求めます。
$$ \ddot{\bm{q}} = \bm{M}^{-1}(\bm{q}) \left[ \bm{\tau} – \bm{C}(\bm{q}, \dot{\bm{q}})\dot{\bm{q}} – \bm{g}(\bm{q}) \right] $$
$\bm{\tau} = \bm{0}$ の場合
$$ \ddot{\bm{q}} = \bm{M}^{-1}(\bm{q}) \left[ – \bm{C}(\bm{q}, \dot{\bm{q}})\dot{\bm{q}} – \bm{g}(\bm{q}) \right] $$
これを4次のルンゲ・クッタ法（scipy.integrate.solve_ivp）で数値積分します。
パラメータ設定と運動方程式の実装
まず、2リンクマニピュレータの動力学を計算する関数を実装します。
import numpy as np
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt
from matplotlib.patches import Circle
# --- パラメータ設定 ---
m1, m2 = 1.0, 1.0 # リンク質量 [kg]
l1, l2 = 1.0, 1.0 # リンク長 [m]
lc1, lc2 = 0.5, 0.5 # 重心までの距離 [m]
I1 = m1 * l1**2 / 12 # 細い棒の慣性モーメント [kg*m^2]
I2 = m2 * l2**2 / 12
g = 9.81 # 重力加速度 [m/s^2]
# 省略記号
alpha = m1 * lc1**2 + I1 + m2 * l1**2 + m2 * lc2**2 + I2
beta = m2 * l1 * lc2
delta = m2 * lc2**2 + I2
def dynamics(t, state, tau1=0.0, tau2=0.0):
"""2リンクマニピュレータの状態方程式（順動力学）"""
q1, q2, dq1, dq2 = state
# 慣性行列 M(q)
M = np.array([
[alpha + 2 * beta * np.cos(q2), delta + beta * np.cos(q2)],
[delta + beta * np.cos(q2), delta]
])
# コリオリ・遠心力項 C(q, dq) * dq
c = np.array([
-beta * np.sin(q2) * (2 * dq1 * dq2 + dq2**2),
beta * np.sin(q2) * dq1**2
])
# 重力項 g(q)
grav = np.array([
(m1 * lc1 + m2 * l1) * g * np.sin(q1) + m2 * lc2 * g * np.sin(q1 + q2),
m2 * lc2 * g * np.sin(q1 + q2)
])
# トルク
tau = np.array([tau1, tau2])
# 加速度: ddq = M^{-1} (tau - c - grav)
ddq = np.linalg.solve(M, tau - c - grav)
return [dq1, dq2, ddq[0], ddq[1]]
dynamics 関数は状態ベクトル $(q_1, q_2, \dot{q}_1, \dot{q}_2)$ を受け取り、その時間微分 $(\dot{q}_1, \dot{q}_2, \ddot{q}_1, \ddot{q}_2)$ を返します。慣性行列 $\bm{M}$ の逆行列を直接計算するのではなく、np.linalg.solve を使って連立方程式を解いています。これは数値的に安定で、計算効率も良い方法です。
数値積分の実行
初期条件を設定してシミュレーションを実行します。
# --- 初期条件 ---
q1_0 = np.pi / 3 # リンク1の初期角度 60° (鉛直下向きから)
q2_0 = np.pi / 4 # リンク2の初期角度 45° (リンク1から)
dq1_0 = 0.0 # 初期角速度ゼロ
dq2_0 = 0.0
state0 = [q1_0, q2_0, dq1_0, dq2_0]
# --- 数値積分 ---
t_span = (0, 10)
t_eval = np.linspace(0, 10, 2000)
sol = solve_ivp(dynamics, t_span, state0, method='RK45',
t_eval=t_eval, rtol=1e-10, atol=1e-12)
t = sol.t
q1 = sol.y[0]
q2 = sol.y[1]
dq1 = sol.y[2]
dq2 = sol.y[3]
許容誤差を rtol=1e-10, atol=1e-12 と非常に小さく設定しています。二重振り子はカオス系なので、数値誤差が時間とともに指数的に増大します。シミュレーション結果の信頼性を高めるためには、高精度の積分が重要です。
関節角度の時間変化を可視化
fig, axes = plt.subplots(2, 1, figsize=(10, 6), sharex=True)
axes[0].plot(t, np.degrees(q1), color='#00bcd4', linewidth=1.2, label=r'$q_1$')
axes[0].plot(t, np.degrees(q2), color='#ff9800', linewidth=1.2, label=r'$q_2$')
axes[0].set_ylabel('Joint Angle [deg]')
axes[0].legend(fontsize=12)
axes[0].set_title('2-Link Manipulator Free Motion (Double Pendulum)', fontsize=14)
axes[0].grid(True, alpha=0.3)
axes[1].plot(t, np.degrees(dq1), color='#00bcd4', linewidth=1.2, label=r'$\dot{q}_1$')
axes[1].plot(t, np.degrees(dq2), color='#ff9800', linewidth=1.2, label=r'$\dot{q}_2$')
axes[1].set_xlabel('Time [s]')
axes[1].set_ylabel('Joint Angular Velocity [deg/s]')
axes[1].legend(fontsize=12)
axes[1].grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('double_pendulum_angles.png', dpi=150, bbox_inches='tight')
plt.show()
上のグラフからは、2リンクマニピュレータ（二重振り子）の自由運動の特徴が読み取れます。
- 非周期的な運動: 単振り子（1リンク）のような規則的な振動ではなく、関節角度が複雑に変化します。これは二重振り子のカオス性を反映しています
- エネルギーの交換: $q_1$ と $q_2$ の振幅が交互に大きくなったり小さくなったりする傾向が見られます。リンク間でエネルギーが移動しているためです
- 角速度の急変: 特にリンク2の角速度 $\dot{q}_2$ が急激に変化する瞬間があります。これはリンク1の運動がコリオリ力・遠心力を通じてリンク2に強い影響を与えるためです
エネルギー保存の確認
外部トルクがゼロ（$\bm{\tau} = \bm{0}$）なので、系の全力学的エネルギーは保存されるはずです。これを数値的に確認しましょう。
# --- エネルギーの計算 ---
K_arr = np.zeros_like(t)
U_arr = np.zeros_like(t)
for i in range(len(t)):
q1_i, q2_i = q1[i], q2[i]
dq1_i, dq2_i = dq1[i], dq2[i]
# 運動エネルギー
M11 = alpha + 2 * beta * np.cos(q2_i)
M12 = delta + beta * np.cos(q2_i)
M22 = delta
dq = np.array([dq1_i, dq2_i])
M = np.array([[M11, M12], [M12, M22]])
K_arr[i] = 0.5 * dq @ M @ dq
# ポテンシャルエネルギー
U_arr[i] = (-(m1 * lc1 + m2 * l1) * g * np.cos(q1_i)
- m2 * lc2 * g * np.cos(q1_i + q2_i))
E_total = K_arr + U_arr
fig, axes = plt.subplots(2, 1, figsize=(10, 5), sharex=True)
axes[0].plot(t, K_arr, color='#e91e63', linewidth=1.0, label='Kinetic Energy $K$')
axes[0].plot(t, U_arr, color='#4caf50', linewidth=1.0, label='Potential Energy $U$')
axes[0].plot(t, E_total, color='white', linewidth=1.5, linestyle='--', label='Total Energy $E$')
axes[0].set_ylabel('Energy [J]')
axes[0].legend(fontsize=11)
axes[0].set_title('Energy Conservation Check', fontsize=14)
axes[0].grid(True, alpha=0.3)
# 全エネルギーの相対誤差
E_error = (E_total - E_total[0]) / np.abs(E_total[0])
axes[1].plot(t, E_error, color='#00bcd4', linewidth=1.0)
axes[1].set_xlabel('Time [s]')
axes[1].set_ylabel('Relative Energy Error')
axes[1].set_title('Relative Error in Total Energy', fontsize=14)
axes[1].grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('double_pendulum_energy.png', dpi=150, bbox_inches='tight')
plt.show()
print(f"全エネルギーの初期値: {E_total[0]:.6f} J")
print(f"全エネルギーの最大相対誤差: {np.max(np.abs(E_error)):.2e}")
エネルギーのグラフから、2つの重要な点が確認できます。
- 運動エネルギーとポテンシャルエネルギーが相補的に変化する: 一方が増加するとき他方が減少し、全エネルギー（破線）はほぼ一定に保たれています。これは自由運動（外力ゼロ）でエネルギー保存が成立していることを示しています
- 全エネルギーの相対誤差は $10^{-10}$ のオーダー: 積分の許容誤差を十分小さく設定したことで、数値的なエネルギー散逸がほとんどありません。もし誤差が大きかったら、シミュレーション結果の信頼性に疑問が生じます。エネルギー保存の確認は、動力学シミュレーションの妥当性検証の基本手法です
手先の軌跡を可視化
最後に、マニピュレータの手先（先端）がどのような軌跡を描くかを可視化します。
# --- 手先位置の計算 ---
x1 = l1 * np.sin(q1)
y1 = -l1 * np.cos(q1)
x2 = x1 + l2 * np.sin(q1 + q2)
y2 = y1 - l2 * np.cos(q1 + q2)
fig, ax = plt.subplots(figsize=(8, 8))
# 手先軌跡
scatter = ax.scatter(x2, y2, c=t, cmap='cool', s=0.5, alpha=0.7)
plt.colorbar(scatter, ax=ax, label='Time [s]')
# 初期姿勢
ax.plot([0, x1[0]], [0, y1[0]], 'o-', color='#ff9800', linewidth=3,
markersize=8, label='Initial pose')
ax.plot([x1[0], x2[0]], [y1[0], y2[0]], 'o-', color='#ff9800', linewidth=3,
markersize=8)
# 可動範囲
theta = np.linspace(0, 2 * np.pi, 200)
ax.plot((l1 + l2) * np.cos(theta), (l1 + l2) * np.sin(theta),
'--', color='gray', alpha=0.3, linewidth=0.8, label='Workspace boundary')
ax.plot(0, 0, 'ko', markersize=10, zorder=5)
ax.set_xlabel('$x$ [m]', fontsize=12)
ax.set_ylabel('$y$ [m]', fontsize=12)
ax.set_title('End-Effector Trajectory (Double Pendulum)', fontsize=14)
ax.set_aspect('equal')
ax.legend(fontsize=11, loc='upper right')
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('double_pendulum_trajectory.png', dpi=150, bbox_inches='tight')
plt.show()
手先の軌跡を見ると、二重振り子が描く複雑な曲線が確認できます。
- 軌跡は可動範囲（半径 $l_1 + l_2$ の円）の内側に収まっています。これは幾何学的な制約から当然ですが、シミュレーションの正しさを確認する一つの指標になります
- 軌跡には規則的なパターンが見えつつも、同じ経路を繰り返さない: これがカオス的挙動の特徴です。初期条件をわずかに変えるだけで、全く異なる軌跡になります
- 色の変化（時間の経過）から、手先が空間のある領域を偏りなく探索していることがわかります: これはエルゴード性に関連する現象です
アニメーション（オプション）
マニピュレータの動きをより直感的に理解するためのアニメーションコードも示します。
from matplotlib.animation import FuncAnimation
from IPython.display import HTML
fig, ax = plt.subplots(figsize=(8, 8))
ax.set_xlim(-2.5, 2.5)
ax.set_ylim(-2.5, 2.5)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.set_title('2-Link Manipulator Animation', fontsize=14)
line, = ax.plot([], [], 'o-', color='#00bcd4', linewidth=3, markersize=10)
trail, = ax.plot([], [], '-', color='#ff9800', alpha=0.3, linewidth=0.8)
ax.plot(0, 0, 'ko', markersize=12, zorder=5)
trail_x, trail_y = [], []
# 表示間隔（毎フレーム描画すると遅いので間引く）
skip = 5
def init():
line.set_data([], [])
trail.set_data([], [])
return line, trail
def animate(frame):
i = frame * skip
if i >= len(t):
i = len(t) - 1
xs = [0, x1[i], x2[i]]
ys = [0, y1[i], y2[i]]
line.set_data(xs, ys)
trail_x.append(x2[i])
trail_y.append(y2[i])
trail.set_data(trail_x, trail_y)
return line, trail
anim = FuncAnimation(fig, animate, init_func=init,
frames=len(t) // skip, interval=20, blit=True)
plt.show()
このアニメーションでは、2本のリンク（シアン色の棒）が関節を中心に振り回される様子と、手先の軌跡（オレンジ色）が時間とともに複雑なパターンを描く様子が確認できます。二重振り子の非直感的な動き — たとえばリンク2が突然反転したり、予想外の方向に跳ねたりする挙動 — はコリオリ力と遠心力の非線形な効果によるものです。
コリオリ・遠心力の効果を確認する
標準形の各項（慣性項、コリオリ・遠心力項、重力項）がそれぞれどの程度の大きさで寄与しているかを可視化し、物理的な理解を深めましょう。
# --- 各項の大きさを計算 ---
inertia_term = np.zeros((len(t), 2))
coriolis_term = np.zeros((len(t), 2))
gravity_term = np.zeros((len(t), 2))
for i in range(len(t)):
q1_i, q2_i = q1[i], q2[i]
dq1_i, dq2_i = dq1[i], dq2[i]
# 加速度（状態方程式から再計算）
state_i = [q1_i, q2_i, dq1_i, dq2_i]
deriv = dynamics(t[i], state_i)
ddq1_i, ddq2_i = deriv[2], deriv[3]
ddq = np.array([ddq1_i, ddq2_i])
# 慣性行列
M = np.array([
[alpha + 2 * beta * np.cos(q2_i), delta + beta * np.cos(q2_i)],
[delta + beta * np.cos(q2_i), delta]
])
# コリオリ・遠心力
c = np.array([
-beta * np.sin(q2_i) * (2 * dq1_i * dq2_i + dq2_i**2),
beta * np.sin(q2_i) * dq1_i**2
])
# 重力
grav = np.array([
(m1 * lc1 + m2 * l1) * g * np.sin(q1_i) + m2 * lc2 * g * np.sin(q1_i + q2_i),
m2 * lc2 * g * np.sin(q1_i + q2_i)
])
inertia_term[i] = M @ ddq
coriolis_term[i] = c
gravity_term[i] = grav
fig, axes = plt.subplots(2, 1, figsize=(10, 7), sharex=True)
for j, ax in enumerate(axes):
ax.plot(t, inertia_term[:, j], color='#00bcd4', linewidth=1.0,
label='Inertia $M\\ddot{q}$', alpha=0.8)
ax.plot(t, coriolis_term[:, j], color='#ff9800', linewidth=1.0,
label='Coriolis/Centrifugal $C\\dot{q}$', alpha=0.8)
ax.plot(t, gravity_term[:, j], color='#4caf50', linewidth=1.0,
label='Gravity $g$', alpha=0.8)
ax.set_ylabel(f'Torque on Joint {j+1} [Nm]')
ax.legend(fontsize=10, loc='upper right')
ax.grid(True, alpha=0.3)
axes[0].set_title('Contribution of Each Term in the Equation of Motion', fontsize=14)
axes[1].set_xlabel('Time [s]')
plt.tight_layout()
plt.savefig('double_pendulum_torque_terms.png', dpi=150, bbox_inches='tight')
plt.show()
このグラフからは、運動方程式の各項の寄与の大きさと特徴が明確に読み取れます。
- 重力項（緑）は滑らかに変化する: 関節角度の $\sin$ 関数で決まるため、急激な変化はありません。振幅は物理パラメータ（質量・リンク長・重力加速度）で決まり、速度に依存しません
- コリオリ・遠心力項（オレンジ）は速度の2乗に比例する: 高速運動時に大きくなり、静止時にはゼロです。関節2についてのコリオリ力 $\beta \dot{q}_1^2 \sin q_2$ は関節1の速度の2乗に依存するため、関節1が速く動くときにリンク2に強い遠心力が作用します
- 慣性項（シアン）は加速度の変化を反映して最も激しく振動する: これは $\bm{M}\ddot{\bm{q}} = \bm{\tau} – \bm{C}\dot{\bm{q}} – \bm{g}$ から計算されるため、コリオリ力と重力の合力の反力として現れます
実際のロボット制御では、これらの項を正確に計算して打ち消す（フィードフォワード補償する）ことで、高精度な動作が可能になります。制御の分野ではこれを計算トルク法（computed torque method）と呼びます。
まとめ
本記事では、ラグランジュ法を使ってマニピュレータの運動方程式を体系的に導出する方法を解説しました。
- ラグランジュ法はエネルギーベースのアプローチで、拘束力を考慮せずに運動方程式が得られる。マニピュレータ動力学の定式化に適している
- 運動エネルギーは慣性行列 $\bm{M}(\bm{q})$ を使って $K = \frac{1}{2}\dot{\bm{q}}^T \bm{M}(\bm{q}) \dot{\bm{q}}$ と表され、ポテンシャルエネルギーは各リンクの重心高さから計算される
- 1リンクマニピュレータは単振り子と等価で、$(ml_c^2 + I)\ddot{q} + mgl_c \sin q = \tau$ という比較的単純な方程式になる
- 2リンクマニピュレータでは、コリオリ力・遠心力の項が現れ、関節間の動的干渉が生じる。導出は長いが、ステップを丁寧に追えば体系的に行える
- 標準形 $\bm{M}(\bm{q})\ddot{\bm{q}} + \bm{C}(\bm{q}, \dot{\bm{q}})\dot{\bm{q}} + \bm{g}(\bm{q}) = \bm{\tau}$ はロボット制御の基盤であり、慣性項・コリオリ/遠心力項・重力項それぞれに明確な物理的意味がある
- シミュレーションにより、自由運動（二重振り子）のカオス的な振る舞い、エネルギー保存、各項の寄与の大きさを視覚的に確認した
ラグランジュ法は運動方程式を美しく導出できますが、$n$ が大きくなると計算量が $O(n^3)$ 以上に増大する問題があります。次の記事では、$O(n)$ の計算量で逆動力学を解けるニュートン・オイラー法を解説し、リアルタイム制御に使える効率的な動力学計算を学びます。
次のステップとして、以下の記事も参考にしてください。
- ニュートン・オイラー法による逆動力学 — 再帰的なアルゴリズムで効率的に逆動力学を解く
- 剛体の姿勢表現と同次変換行列 — 本記事で使った座標変換の復習