入門 · レッスン 4 / 10

逆運動学：解析解と数値解

余弦定理と減衰最小二乗ソルバの2通りで、2リンクアームの先端を目標に置く関節角を求めます。到達不能な目標の扱い方と、特異点が数値に与える影響も確かめます

入門45 分更新 2026年11月14日無料期間 2027年1月31日

このレッスンで学ぶこと

平面2リンクアームの閉形式の逆運動学を導出し、肘の2つの解をどちらも返す
関節角を計算する前に到達不能な目標を検出し、境界のケースを説明する
同じ問題を減衰最小二乗の反復とscipy.optimize.least_squaresで数値的に解き、どちらも閉形式解と一致することを示す
アームのヤコビ行列の行列式を計算し、減衰がないと特異点の近くでステップが発散する理由を説明する

レッスン03では、関節角から先端位置を求めました。実際のタスクはその逆向きです。カメラが部品を見つけ、経路計画器がたどるべき点列を生成し、コントローラはそれぞれの点にツールを置く関節角を必要とします。これが逆運動学（IK）です。答えが一意とは限らず、存在しないこともあるため、順運動学より難しい問題です。このレッスンでは、同じ2リンクアーム（リンク長0.4 mと0.3 m）について、厳密な式と、汎用のロボットソフトウェアが使う種類の数値ソルバの2通りで解きます。その後、特異点の近くで何がうまくいかなくなるかを見ます。

解は2つ、1つ、またはなし

レッスン03で求めた円環の内側にある目標に対しては、アームの置き方が2通りあります。ベースから目標への直線の一方の側に肘を置くか、もう一方の側に置くかです。円環の内縁と外縁では2つの解が一致します（アームが完全に折りたたまれた状態、または完全に伸びた状態）。円環の外側には解がまったくありません。

リンク長0.4 mと0.3 mのアームで、目標(0.5, 0.2) mに対する逆運動学の2つの解。実線のアームは肘上げ（elbow-up）解で、肘はベースから目標への直線より上にあります。破線のアームは肘下げ（elbow-down）解です。どちらもエンドエフェクタを同じ点に置きます。

実機のアームを動かすソフトウェアは、どちらかの分岐を選び、それを維持する必要があります。動作の途中で分岐を切り替えると、肘はアームがまっすぐ伸びた姿勢を通過して振れます。先端のわずかな変化に対して、大きく、場合によっては危険な動きになります。

閉形式解

まず、ベースから目標までの距離r² = x² + y²を求めます。2本のリンクとベースから目標への直線は、辺の長さがl1、l2、rの三角形をつくります。レッスン03ではr² = l1² + l2² + 2 l1 l2 cos(theta2)であることを示しました。これを肘の角度について解きます。

cos(theta2) = (x^2 + y^2 - l1^2 - l2^2) / (2 l1 l2)
theta2      = +acos(...)  or  -acos(...)

2つの符号が2つの分岐に対応します。1行目の右辺が1より大きいか-1より小さい場合、実数の角度は存在しません。これはまさに、目標が円環の外側にあるかどうかの判定です。

theta2がわかれば、肩の角度は、目標の方向から、曲がったアームが加える角度を引いたものになります。

theta1 = atan2(y, x) - atan2(l2 sin(theta2), l1 + l2 cos(theta2))

atanではなくatan2を使ってください。2引数の形は両方の入力の符号を見て、全周にわたって正しい象限の角度を返します。atan(y / x)は(1, 1)と(-1, -1)を区別できず、xが0のときに失敗します。

import numpy as np

L1, L2 = 0.4, 0.3   # link lengths, m (same arm as lesson 3)

def fk(q: np.ndarray) -> np.ndarray:
    t1, t2 = q
    return np.array([L1 * np.cos(t1) + L2 * np.cos(t1 + t2),
                     L1 * np.sin(t1) + L2 * np.sin(t1 + t2)])

def ik_analytic(x: float, y: float) -> list[np.ndarray]:
    """All joint solutions (theta1, theta2) that put the tip at (x, y). Empty if unreachable."""
    c2 = (x**2 + y**2 - L1**2 - L2**2) / (2 * L1 * L2)
    if abs(c2) > 1.0 + 1e-12:          # target outside the annulus
        return []
    c2 = float(np.clip(c2, -1.0, 1.0))  # absorb rounding on the boundary
    solutions = []
    for sign in (+1.0, -1.0):          # the two square roots give the two elbow branches
        t2 = sign * np.arccos(c2)
        t1 = np.arctan2(y, x) - np.arctan2(L2 * np.sin(t2), L1 + L2 * np.cos(t2))
        solutions.append(np.array([t1, t2]))
    return solutions

def elbow_side(q: np.ndarray, target: np.ndarray) -> str:
    """Is the elbow to the left (above) or right (below) of the line from base to target?"""
    elbow = np.array([L1 * np.cos(q[0]), L1 * np.sin(q[0])])
    cross = target[0] * elbow[1] - target[1] * elbow[0]
    return "elbow-up" if cross > 0 else "elbow-down"

target = np.array([0.5, 0.2])
for q in ik_analytic(*target):
    err = np.linalg.norm(fk(q) - target)
    print(f"{elbow_side(q, target):10s} theta1 {np.rad2deg(q[0]):7.2f} deg  "
          f"theta2 {np.rad2deg(q[1]):7.2f} deg  FK error {err:.1e} m")

for far in ([0.8, 0.0], [0.05, 0.0]):
    print(f"target {far}: {len(ik_analytic(*far))} solutions")
print(f"target [0.7, 0.0]: {len(ik_analytic(0.7, 0.0))} solutions (arm straight, both branches coincide)")

elbow-down theta1  -11.52 deg  theta2   80.41 deg  FK error 0.0e+00 m
elbow-up   theta1   55.12 deg  theta2  -80.41 deg  FK error 2.8e-17 m
target [0.8, 0.0]: 0 solutions
target [0.05, 0.0]: 0 solutions
target [0.7, 0.0]: 2 solutions (arm straight, both branches coincide)

各解を順運動学に通すと、先端は目標から1e-16 m以内に収まります。これはどのIKコードでも実行すべき確認です。0.8 mの目標は到達範囲0.7 mの外にあり、0.05 mの目標は0.1 mの穴の内側にあるため、どちらも解を返しません。外縁ちょうどにある目標は、同一の2つの解を返します。小さな許容誤差とclipがないと、丸め誤差でコサインが1.0000000000000002になり、アームが到達できる目標なのにarccosがnanを返すことがあります。

どちらの分岐を「上」と呼ぶかを決めるために、コードはベースと目標を結ぶ直線のどちら側に肘があるかを調べています。この目標では、正のtheta2（肘を反時計回りに曲げた状態）で肘が直線の下側に来ます。theta2の符号だけで分岐の名前を決めないでください。目標がベースの反対側にあると、この関係が逆転するからです。

数値ソルバ

閉形式解が存在するのは、このアームが単純だからです。7関節の協働ロボットや、きれいな三角形を崩すオフセットをもつアームのような一般のアームでは公式がないことが多く、ロボットソフトウェアはIKを数値的に解きます。考え方は、初期推定から始め、先端が目標からどれだけ離れているかを調べ、ヤコビ行列を使って関節をどう変えるかを決める、というものです。

ヤコビ行列J(q)は、先端位置を関節角で偏微分した値を並べた行列です。関節の小さな変化dqに対して、先端は約J dqだけ動きます。誤差eを取り除くにはJ dq = eとすればよいことになります。これを直接解くのがニュートン法です。解の近くでは速く収束しますが、Jの逆行列が求まらない特異点の近くでは振る舞いが悪くなります。減衰最小二乗法は、ステップを制限する小さな項を加えます。

dq = J^T (J J^T + lambda^2 I)^-1 e

lambdaが0のとき、これは通常の最小二乗（擬似逆行列）ステップです。小さな正のlambdaは、1ステップあたりの精度をわずかに犠牲にする代わりに、Jが特異に近いときにもステップを有界に保ちます。この手法はLevenberg-Marquardt法とも呼ばれ、SciPyのleast_squaresもメソッドの一つとして提供しています。

import numpy as np
from scipy.optimize import least_squares

L1, L2 = 0.4, 0.3   # link lengths, m

def fk(q: np.ndarray) -> np.ndarray:
    t1, t2 = q
    return np.array([L1 * np.cos(t1) + L2 * np.cos(t1 + t2),
                     L1 * np.sin(t1) + L2 * np.sin(t1 + t2)])

def jacobian(q: np.ndarray) -> np.ndarray:
    """d(tip position) / d(joint angles), a 2x2 matrix."""
    t1, t2 = q
    s1, c1 = np.sin(t1), np.cos(t1)
    s12, c12 = np.sin(t1 + t2), np.cos(t1 + t2)
    return np.array([[-L1 * s1 - L2 * s12, -L2 * s12],
                     [ L1 * c1 + L2 * c12,  L2 * c12]])

def ik_dls(target: np.ndarray, q0: np.ndarray, damping: float = 0.01,
           tol: float = 1e-10, max_iter: int = 200) -> tuple[np.ndarray, int]:
    """Damped least-squares (Levenberg-Marquardt style) iteration on the joint angles."""
    q = q0.astype(float).copy()
    for i in range(max_iter):
        e = target - fk(q)
        if e @ e < tol**2:
            return q, i
        Jq = jacobian(q)
        dq = Jq.T @ np.linalg.solve(Jq @ Jq.T + damping**2 * np.eye(2), e)
        q += dq
    return q, max_iter

def ik_analytic(x: float, y: float) -> list[np.ndarray]:
    """Closed form from the previous block."""
    c2 = (x**2 + y**2 - L1**2 - L2**2) / (2 * L1 * L2)
    if abs(c2) > 1.0 + 1e-12:
        return []
    c2 = float(np.clip(c2, -1.0, 1.0))
    out = []
    for sign in (+1.0, -1.0):
        t2 = sign * np.arccos(c2)
        out.append(np.array([np.arctan2(y, x) - np.arctan2(L2 * np.sin(t2), L1 + L2 * np.cos(t2)), t2]))
    return out

def wrap(a: np.ndarray) -> np.ndarray:
    return (a + np.pi) % (2 * np.pi) - np.pi

target = np.array([0.5, 0.2])
closed_form = ik_analytic(*target)

for q0 in (np.array([0.0, 0.5]), np.array([1.0, -0.5])):
    q, iters = ik_dls(target, q0)
    gap = min(np.abs(wrap(q - qa)).max() for qa in closed_form)
    print(f"DLS from {q0}: theta = {np.round(np.rad2deg(wrap(q)), 2)} deg, {iters} iterations, "
          f"tip error {np.linalg.norm(fk(q) - target):.1e} m, gap to closed form {gap:.1e} rad")

res = least_squares(lambda q: fk(q) - target, x0=[0.0, 0.5], jac=lambda q: jacobian(q))
print(f"least_squares:   theta = {np.round(np.rad2deg(wrap(res.x)), 2)} deg, "
      f"evaluations {res.nfev}, error {np.linalg.norm(res.fun):.1e} m")
gap = min(np.abs(wrap(res.x - qa)).max() for qa in closed_form)
print(f"least_squares gap to closed form: {gap:.1e} rad")

DLS from [0.  0.5]: theta = [-11.52  80.41] deg, 6 iterations, tip error 1.7e-11 m, gap to closed form 7.6e-11 rad
DLS from [ 1.  -0.5]: theta = [ 55.12 -80.41] deg, 7 iterations, tip error 1.4e-12 m, gap to closed form 6.2e-12 rad
least_squares:   theta = [-11.52  80.41] deg, evaluations 6, error 6.1e-14 m
least_squares gap to closed form: 2.6e-13 rad

どちらの数値ソルバも、6回または7回の反復で、閉形式解と1e-10 rad未満の差で一致します。それぞれがどちらの解を見つけたかに注意してください。肘を反時計回りに曲げた状態から始めると、反復は肘下げの分岐に落ち着きます。逆向きから始めると肘上げの解を見つけます。数値ソルバは1つの答え、大まかに言えば初期推定に最も近い答えを返し、もう1つの解が存在することを知りません。経路を追従するときには、これが望ましい性質です。各点の初期値に前の点の解を使えば、アームは1つの分岐にとどまります。単発の目標では、初期値を意図して選ぶ必要があるということです。

wrap関数も重要です。2π異なる角度は、連続回転する関節では同じ関節位置ですが、数値としては異なります。そのため、角度は-πからπの範囲に折り返してから比較してください。機械的な可動範囲をもつ関節では、解いた後にその範囲を確認するか、範囲を境界条件としてソルバに渡す必要があります。

特異点

このアームのヤコビ行列の行列式はl1 l2 sin(theta2)になります。これはアームがまっすぐ伸びたときと完全に折りたたまれたときに0になります。これらの姿勢では、2つの関節は先端をアームに直交する方向にしか動かせず、どんな関節速度でもアームに沿った方向には動かせません。これが特異点です。特異点の近くで、失われた方向に先端を少し動かそうとすると、非常に大きな関節運動が必要になります。次のコードでは、まず行列式が小さくなる様子を示します。次に、ほぼまっすぐなアーム（theta2が0.5°）から、ベースに1 cm近い点に向けて線形化したステップを1回、減衰ありとなしで計算します。最後に、到達範囲外の目標を与えます。

import numpy as np

L1, L2 = 0.4, 0.3   # link lengths, m

def fk(q: np.ndarray) -> np.ndarray:
    t1, t2 = q
    return np.array([L1 * np.cos(t1) + L2 * np.cos(t1 + t2),
                     L1 * np.sin(t1) + L2 * np.sin(t1 + t2)])

def jacobian(q: np.ndarray) -> np.ndarray:
    t1, t2 = q
    s1, c1, s12, c12 = np.sin(t1), np.cos(t1), np.sin(t1 + t2), np.cos(t1 + t2)
    return np.array([[-L1 * s1 - L2 * s12, -L2 * s12],
                     [ L1 * c1 + L2 * c12,  L2 * c12]])

print("det J = L1 L2 sin(theta2):")
for deg in (90, 30, 5, 0.5, 0):
    q = np.array([0.0, np.deg2rad(deg)])
    print(f"  theta2 {deg:5.1f} deg  det J {np.linalg.det(jacobian(q)):+.5f} m^2")

# One linearised step from an almost straight arm (theta2 = 0.5 deg): pull the tip 1 cm towards the base
q = np.array([0.0, np.deg2rad(0.5)])
e = np.array([-0.01, 0.0])
Jq = jacobian(q)
for lam in (0.0, 0.01, 0.05):
    dq = Jq.T @ np.linalg.solve(Jq @ Jq.T + lam**2 * np.eye(2), e)
    moved = fk(q + dq) - fk(q)
    print(f"damping {lam:4.2f}: largest joint step {np.rad2deg(np.abs(dq)).max():7.2f} deg, "
          f"tip moved ({moved[0]:+.5f}, {moved[1]:+.5f}) m")

# A target outside the reach: iterate damped least squares with two damping values
target = np.array([0.9, 0.3])
print(f"unreachable target {target}, best possible miss {np.linalg.norm(target) - (L1 + L2):.4f} m")
for lam in (0.05, 0.2):
    q = np.array([0.3, 0.3])
    misses = []
    for _ in range(500):
        Jq = jacobian(q)
        q = q + Jq.T @ np.linalg.solve(Jq @ Jq.T + lam**2 * np.eye(2), target - fk(q))
        misses.append(np.linalg.norm(target - fk(q)))
    print(f"  damping {lam:4.2f}: theta = {np.round(np.rad2deg(q), 2) + 0.0} deg, "
          f"last three misses {np.round(misses[-3:], 4)} m")

det J = L1 L2 sin(theta2):
  theta2  90.0 deg  det J +0.12000 m^2
  theta2  30.0 deg  det J +0.06000 m^2
  theta2   5.0 deg  det J +0.01046 m^2
  theta2   0.5 deg  det J +0.00105 m^2
  theta2   0.0 deg  det J +0.00000 m^2
damping 0.00: largest joint step  382.99 deg, tip moved (-1.31672, -0.30220) m
damping 0.01: largest joint step    7.11 deg, tip moved (-0.00150, +0.00003) m
damping 0.05: largest joint step    0.29 deg, tip moved (-0.00001, +0.00004) m
unreachable target [0.9 0.3], best possible miss 0.2487 m
  damping 0.05: theta = [ 40.47 -57.03] deg, last three misses [0.3258 0.3161 0.3329] m
  damping 0.20: theta = [18.43  0.  ] deg, last three misses [0.2487 0.2487 0.2487] m

結果を順に読みます。まっすぐな状態から0.5°の位置では、行列式は90°のときの約1%です。減衰なしのステップは、先端を1 cm動かすために383°の関節回転を要求します。線形モデルは小さなステップでしか成り立たないため、先端は1.3 m離れた位置に行ってしまいます。減衰0.01ではステップが7°に、0.05では0.3°に制限されます。どちらも1回のステップでは1 cm全体を動かせません。アームに沿った方向では、先端は肘の曲がりの2乗に比例する量しか動かないため、ソルバは反復する必要があります。

到達不能な目標は、トレードオフのもう一方の側を示しています。アームにできる最善は、目標をまっすぐ指して0.2487 m外すことです。減衰0.2では反復はまさにそこに落ち着き、theta1は目標を向き、肘はまっすぐ伸びます。減衰0.05では落ち着きません。特異なまっすぐ伸びた姿勢の周りで振動し、外れ量は0.32 mと0.33 mの間で跳ねます。小さな減衰は、目標が到達可能なときに速く正確な収束をもたらします。大きな減衰は、特異点の近くや作業空間の外で穏やかな振る舞いをもたらします。「さらに読む」に挙げたSamuel Bussのチュートリアルも、同じトレードオフを明示しています。そこでは減衰をその場で選ぶ手法を紹介し、誤差ベクトルの大きさをクランプして、小さな減衰でも到達範囲外の目標に対して適切に振る舞うようにすることを提案しています。

特異点はソルバだけの問題ではありません。特異姿勢を通る直線を追従するよう指令された実機のアームは、そこで非常に大きな関節速度を要求します。コントローラは減速するか、フォルトで停止するか、経路から外れるかのいずれかになります。3次元のアームには何種類かの特異点があり、経路計画器はそれらを意図的に避けます。理論は運動学ハブにまとめています。

演習

ik_analyticを使って、アームの先端に中心(0.45, 0.1)、半径0.1 mの円を100ステップで描かせてください。全体を通して肘上げの分岐を保ち、連続する点の間でいずれかの関節角が最も大きく変化した量を出力してください。
ik_dlsで演習1を繰り返してください。各点の初期値には前の点の解を使います。すべての点で結果を閉形式解と比較してください。
円の中心を(0.55, 0.1)に移し、作業空間の外縁からはみ出すようにしてください。到達不能な部分で、それぞれの手法はどう振る舞うでしょうか。実機のロボットではどちらの振る舞いが望ましいか、理由とともに考えてください。
least_squaresの呼び出しにtheta2の範囲±150°を加え（bounds引数）、範囲付きのソルバが範囲なしのソルバと異なる分岐を選ばざるを得なくなる目標を見つけてください。

2リンクアームのプロジェクトでは、この演習を発展させて、図形を描くアームを作ります。

まとめ

逆運動学は、目標姿勢に対する関節角を求めます。2リンクアームでは、作業空間の内部で解が2つ、縁で1つ、外側ではなしです。
閉形式解では、theta2に余弦定理、theta1にatan2を使います。arccosを呼ぶ前に、小さな許容誤差をつけてコサインを±1と比較してください。
数値IKはdq = J^T (J J^T + lambda^2 I)^-1 eを反復するか、最小二乗ソルバを呼び出します。初期値に最も近い解を見つけ、丸め誤差の範囲で閉形式解と一致します。
特異点ではヤコビ行列のランクが落ちます。減衰なしのステップはそこで発散します。減衰はわずかな精度と引き換えに、有界で安定したステップと、到達不能な目標に対する妥当な振る舞いをもたらします。

さらに読む

Lynch and Park, 「Modern Robotics: Mechanics, Planning, and Control」, Cambridge University Press, 2017：第6章「Inverse Kinematics」。6.2節で数値的な逆運動学を、5.3節で特異点を扱っています。
Buss, “Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares methods”, University of California, San Diego, 2009：上で使った反復法を特異値分解で解析した、短く明快な比較。
Siciliano, Sciavicco, Villani and Oriolo, 「Robotics: Modelling, Planning and Control」, Springer, 2009：第3章「Differential Kinematics and Statics」で、3次元アームのヤコビ行列と特異点を扱っています。
SciPyのleast_squaresドキュメント：信頼領域法とLevenberg-Marquardt法、境界条件、結果オブジェクトのフィールド。