逆運動学をPythonで実装する｜「手先をここに置きたい」から関節角度を求める

順運動学では「関節角度→手先位置」を三角関数だけで計算しました。実際のロボット制御で欲しいのはたいてい逆方向、「この位置に手先を置きたい、関節を何度にすればいい？」です。これが**逆運動学（Inverse Kinematics, IK）**で、順運動学と違って一意に解けなかったり、そもそも解が無かったりする、一段ややこしい問題です¹。この記事では2関節アームを題材に、幾何学的に解く方法と、数値的に解く方法の両方をPythonで実装します。

なぜ逆運動学は難しいのか

順運動学は「角度を入れると座標が1つ出る」関数でした。逆運動学はその逆関数を求めることですが、三角関数は周期的なので同じ手先位置に至る関節角度の組み合わせが複数あることがあります。さらに、腕の長さより遠い場所は物理的にそもそも届きません。

準備

pip install numpy

① 幾何学的に解く：余弦定理から関節角度を逆算する

長さ l1・l2 の2関節アームで、手先を (x, y) に置きたいとします。原点から目標までの距離 r と、肘の角度（2つ目の関節の角度）の間には、三角形の余弦定理の関係があります。

import numpy as np

def inverse_kinematics(x, y, l1=1.0, l2=0.7, elbow="up"):
    r2 = x**2 + y**2
    r = np.sqrt(r2)
    if r > l1 + l2 or r < abs(l1 - l2):
        return None  # 届かない（遠すぎる／近すぎる）

    # 余弦定理: r^2 = l1^2 + l2^2 - 2*l1*l2*cos(π-theta2)
    cos_t2 = (r2 - l1**2 - l2**2) / (2 * l1 * l2)
    cos_t2 = np.clip(cos_t2, -1.0, 1.0)  # 浮動小数点誤差でわずかに範囲外に出るのを防ぐ

    sign = 1 if elbow == "up" else -1
    theta2 = sign * np.arccos(cos_t2)
    theta1 = np.arctan2(y, x) - np.arctan2(l2 * np.sin(theta2), l1 + l2 * np.cos(theta2))
    return theta1, theta2

arccos は常に0〜πの範囲しか返さないので、符号（sign）を反転させることで肘を上げた解と下げた解の両方を作れます。これが「解が複数ある」の正体です。

② 順運動学で検算する

前回実装した順運動学に代入して、本当に目標へ届いているか確かめます。

def forward_kinematics(theta1, theta2, l1=1.0, l2=0.7):
    x1 = l1 * np.cos(theta1)
    y1 = l1 * np.sin(theta1)
    x2 = x1 + l2 * np.cos(theta1 + theta2)
    y2 = y1 + l2 * np.sin(theta1 + theta2)
    return (x1, y1), (x2, y2)

target = (1.2, 0.5)
for elbow in ["up", "down"]:
    theta1, theta2 = inverse_kinematics(*target, elbow=elbow)
    _, hand = forward_kinematics(theta1, theta2)
    print(f"{elbow:4s}: theta=({np.degrees(theta1):.1f}, {np.degrees(theta2):.1f})度  手先={np.round(hand, 4)}")

print("届かない目標:", inverse_kinematics(5, 5))

up  : theta=(-9.6, 81.8)度  手先=[1.2 0.5]
down: theta=(54.8, -81.8)度  手先=[1.2 0.5]
届かない目標: None

肘上げ・肘下げどちらの関節角度でも、順運動学に通すとちゃんと目標 (1.2, 0.5) に一致します。腕の長さの合計 l1+l2=1.7 より遠い (5, 5) は None——解なしが正しく検出できています。

③ 数値的に解く：ヤコビ行列で少しずつ近づける

2関節なら幾何学的に解けますが、関節が6個・7個になると閉じた式を作るのが大変になります。そこで実務でよく使われるのが、ヤコビ行列を使って目標に少しずつ近づける数値解法です。

def jacobian(theta1, theta2, l1=1.0, l2=0.7):
    # 手先座標(x,y)を関節角度(theta1,theta2)で偏微分した2x2行列
    return np.array([
        [-l1*np.sin(theta1) - l2*np.sin(theta1+theta2), -l2*np.sin(theta1+theta2)],
        [ l1*np.cos(theta1) + l2*np.cos(theta1+theta2),  l2*np.cos(theta1+theta2)],
    ])

def ik_numerical(x_target, y_target, theta_init=(0.3, 0.3), iters=200, lr=1.0):
    theta = np.array(theta_init, dtype=float)
    for _ in range(iters):
        _, (hx, hy) = forward_kinematics(*theta)
        err = np.array([x_target - hx, y_target - hy])
        if np.linalg.norm(err) < 1e-6:
            break
        j = jacobian(*theta)
        dtheta = lr * np.linalg.pinv(j) @ err   # 誤差をヤコビ行列の擬似逆行列で角度変化量に変換
        theta += dtheta
    return theta

theta = ik_numerical(1.2, 0.5)
_, hand = forward_kinematics(*theta)
print("数値解:", np.round(np.degrees(theta), 2), "度  手先=", np.round(hand, 4))

数値解: [-9.58 81.79] 度  手先= [1.2 0.5]

幾何学的解法の「肘上げ」の解とほぼ一致しました。ヤコビ行列は「関節角度をわずかに動かしたとき、手先がどちらにどれだけ動くか」を表す行列で、その擬似逆行列を使うことで「手先をこの誤差分だけ動かすには、関節角度をどう変えればいいか」を逆算できます²。

まとめ

• 逆運動学＝手先位置から関節角度を逆算する計算。順運動学と違い、解が複数・解なしになり得る
• 2関節アームは余弦定理で幾何学的に解け、arccosの符号で肘上げ・肘下げの2解を作れる
• 目標までの距離が腕の長さの範囲外なら解なし（None）
• 関節が増えて式が作りにくいときは、ヤコビ行列の擬似逆行列で少しずつ近づける数値解法が使える

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

逆運動学の解の数は、一般に関節数と機構の対称性に依存します。6自由度の一般的なアームでは、幾何学的な工夫（最後の3軸を1点に集中させる「スフェリカル手首」など）を使うと閉じた式（解析解）が得られ、最大8通りの解を持つことが知られています³。関節数がさらに多い冗長自由度アーム（人間の腕に近い7軸など）では解が無数に存在し、閉じた式では解けないため、本記事で扱ったヤコビ行列法や、それを拡張した減衰最小二乗法（Damped Least Squares, DLS）⁴が実務で広く使われます。DLSは、順運動学の記事で触れた特異点（ヤコビ行列が退化して擬似逆行列が不安定になる姿勢）の近くでも安定して収束するよう、誤差項に小さな減衰係数を加えて数値的な暴れを抑える手法です。

次の一歩 🌸

2関節アームの座標計算は順運動学をPythonで実装する、アームを動かす制御はPID制御をPythonで実装、ロボットの記述形式はURDFでロボットを表すへどうぞ。

Footnotes

1. 順運動学は関節角度→手先位置への一意な写像だが、逆運動学はその逆写像を求める問題であり、一般に非線形方程式を解く必要がある。解の個数・存在性は機構（自由度数やリンク配置）に依存する。 ↩

2. ヤコビ行列 J は手先座標を関節角度で偏微分した行列で、関節速度と手先速度の関係を dx = J・dθ として表す。逆に dθ = J⁺・dx（J⁺は擬似逆行列）とすることで、手先を目標へ近づける角度変化量を求められる。 ↩

3. 手首がスフェリカル（3軸が1点で交わる）構造を持つ6自由度アームでは、位置決めと姿勢決めを分離でき、幾何学的に閉じた式で解ける。分岐（肘上げ/下げ、手首の向きなど）の組み合わせにより、最大8通りの解を持ちうる。 ↩

4. 減衰最小二乗法は誤差 dθ = Jᵀ(JJᵀ + λ²I)⁻¹・dx のように小さな正則化項 λ² を加えることで、特異点付近でヤコビ行列がほぼ特異になっても関節角度が発散しないようにする手法。ロボット制御分野で広く使われる。 ↩