順運動学をPythonで実装する｜ロボットアームの手先位置を計算する

ロボットアームを制御する第一歩は、「各関節を何度曲げたら、手先はどこに来るのか」を計算できること。これを**順運動学（Forward Kinematics）**と呼びます。この記事では2関節アームをPythonで実装し、関節角度から手先座標を求めます。三角関数がそのままロボットの動きになる、気持ちのいいテーマです。

考え方: 関節を順にたどる

根元から先端へ、関節を1つずつたどって位置を足していきます。1つ目の関節で腕が向いた方向へ、2つ目はその向きにさらに角度を足した方向へ——というふうに、角度を累積していくのがポイントです。

準備

pip install numpy

① 2関節アームの順運動学

長さ l1・l2 のリンクを持つアームです。手先位置は次の式になります。

import numpy as np

def forward_kinematics(theta1, theta2, l1=1.0, l2=0.7):
    # 1つ目の関節の先（肘）の位置
    x1 = l1 * np.cos(theta1)
    y1 = l1 * np.sin(theta1)
    # 2つ目の関節の先（手先）= 肘 + 2リンク分。角度は累積(theta1+theta2)
    x2 = x1 + l2 * np.cos(theta1 + theta2)
    y2 = y1 + l2 * np.sin(theta1 + theta2)
    return (x1, y1), (x2, y2)

elbow, hand = forward_kinematics(np.radians(30), np.radians(45))
print("肘:", np.round(elbow, 3))
print("手先:", np.round(hand, 3))

角度をラジアンに直して渡すと、肘と手先の座標が返ります。関節を30度・45度曲げたときの手先の位置が、三角関数だけで求まりました。

② 動かせる範囲（可到達領域）を描く

関節をぐるっと動かして、手先が届く範囲を点で埋めてみます。

reach = []
for t1 in np.linspace(0, np.pi, 30):
    for t2 in np.linspace(-np.pi, np.pi, 30):
        _, (hx, hy) = forward_kinematics(t1, t2)
        reach.append((hx, hy))

reach = np.array(reach)
print("手先が届く範囲: x", np.round(reach[:,0].min(),2), "〜", np.round(reach[:,0].max(),2))
print("最大リーチ:", round(np.max(np.hypot(reach[:,0], reach[:,1])), 3))  # ≈ l1+l2 = 1.7

最大リーチが l1 + l2 = 1.7 に一致します。腕をまっすぐ伸ばしたときが最遠、という直感どおりの結果です。

まとめ

• 順運動学＝関節角度から手先位置を求める計算
• 根元から先端へ、角度を累積しながらリンク長を足していく
• 2関節なら手先の向きは θ1 + θ2。三角関数だけで座標が出る
• 最大リーチはリンク長の和。逆運動学は別物でずっと難しい

もう少し詳しく（背景と理論）

多関節アームの運動学は、各関節の座標変換を掛け合わせて表すのが標準で、関節間の関係を4つのパラメータで系統的に記述する Denavit–Hartenberg（DH）記法（1955年）が広く使われます¹。各変換は回転と並進をまとめた4×4の同次変換行列で表現され、行列の積が手先の姿勢を与えます²。順運動学（関節角→手先位置）は一意で簡単ですが、逆運動学（手先位置→関節角）は非線形で、解が複数あったり存在しなかったりと格段に難しくなります³。さらに、ある姿勢では自由度が失われる特異点が生じ、そこでは関節速度が発散するため制御上の注意が要ります⁴。

次の一歩 🌸

アームの機構はメカ設計の第一歩、動かす制御はPID制御をPythonで実装、ロボットの記述形式はURDFでロボットを表すへどうぞ。

Footnotes

1. Denavit, J. & Hartenberg, R. S. (1955). "A kinematic notation for lower-pair mechanisms based on matrices." J. Applied Mechanics, 22, 215–221. ロボット運動学の標準記法。 ↩

2. 同次変換行列は回転3×3と並進3×1を4×4にまとめたもの。連鎖する関節の変換を行列積で合成でき、手先の位置・姿勢を一括で求められる。 ↩

3. 逆運動学は一般に閉形式解を持たず、数値解法（ヤコビ行列の擬似逆行列など）や幾何学的解法を使う。6自由度アームでは最大8通りの解を持つことがある。 ↩

4. 特異点ではヤコビ行列が退化し、手先を動かすのに必要な関節速度が無限大に近づく。可操作度（manipulability）で特異点への近さを評価する。 ↩