2リンクアームの順運動学

順運動学が答える問いは1つです。関節角が与えられたとき、ツールはどこにあるか。これはほぼすべてのロボットプログラムで最初に行う計算です。シミュレータはアームを描画するために、衝突判定器はリンクの位置を知るために順運動学を必要とします。逆運動学のソルバはどれも、内側のループで順運動学を呼び出します。本レッスンでは、実用になる最も単純なアーム、つまり2つの回転関節を持つ平面アームについて順運動学を導きます。まず三角法で、次にレッスン02の同次変換で求めます。その後、アームの作業空間を描きます。レッスン04では、この問題を逆向きに解きます。

アーム

アームはベース座標系のx-y平面内で動きます。関節1はベース原点にあり、長さl1の第1リンクを回転させます。関節2はリンク1の先端（肘）にあり、長さl2の第2リンクを回転させます。リンク2の先端がエンドエフェクタで、ここで注目する点です。両関節の軸はどちらもz方向、つまり紙面の手前向きです。

レッスン02とREP 103の慣例に従います。コードでは角度の単位はラジアンです。正の角度は、+zから見て反時計回りの回転です（右手の法則）。theta1はベースのx軸から測ります。theta2はベースのx軸ではなくリンク1の方向から測ります。そのためtheta2がゼロのとき、アームはまっすぐ伸びています。この相対的な定義は、肘の関節エンコーダが実際に測る量と一致します。

このアームの自由度（DoF）は2で、先端の座標も2つです。そのため先端の位置は制御できますが、最終リンクの向きを独立に制御することはできません。その向きはtheta1 + theta2で、位置を決めればおのずと決まります。

幾何学による順運動学

ベースからアームをたどります。肘は、角度theta1の直線上をl1だけ進んだ位置、すなわち(l1 cos theta1, l1 sin theta1)にあります。肘の角度は肩の角度に加算されるので、リンク2はベースのx軸から測って角度theta1 + theta2の方向を向きます。リンク2の先端は、その方向にさらにl2だけ進んだ位置にあります。2つのベクトルを足すと次のようになります。

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

これがこのアームの順運動学のすべてです。リンク長を0.4 mと0.3 mとしてnumpyで書くと、次のとおりです。

import numpy as np

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

def fk_geometric(theta1: float, theta2: float) -> tuple[np.ndarray, np.ndarray]:
    """Elbow and end-effector positions in the base frame, by trigonometry."""
    elbow = np.array([L1 * np.cos(theta1), L1 * np.sin(theta1)])
    tip = elbow + np.array([L2 * np.cos(theta1 + theta2), L2 * np.sin(theta1 + theta2)])
    return elbow, tip

for deg1, deg2 in [(0, 0), (90, 0), (0, 90), (30, 60), (45, -90)]:
    elbow, tip = fk_geometric(np.deg2rad(deg1), np.deg2rad(deg2))
    print(f"theta1 {deg1:4d} deg, theta2 {deg2:4d} deg -> "
          f"elbow ({elbow[0]:6.3f}, {elbow[1]:6.3f}) m, tip ({tip[0]:6.3f}, {tip[1]:6.3f}) m")

theta1    0 deg, theta2    0 deg -> elbow ( 0.400,  0.000) m, tip ( 0.700,  0.000) m
theta1   90 deg, theta2    0 deg -> elbow ( 0.000,  0.400) m, tip ( 0.000,  0.700) m
theta1    0 deg, theta2   90 deg -> elbow ( 0.400,  0.000) m, tip ( 0.400,  0.300) m
theta1   30 deg, theta2   60 deg -> elbow ( 0.346,  0.200) m, tip ( 0.346,  0.500) m
theta1   45 deg, theta2  -90 deg -> elbow ( 0.283,  0.283) m, tip ( 0.495,  0.071) m

コードを信用する前に、各行をスケッチと照らし合わせてください。両関節がゼロのとき、アームはx軸に沿って伸び、先端は0.7 mの位置にあります。肘が90°のとき、第2リンクは(0.4, 0)から真上を向きます。(30°, 60°)ではリンク2がベースのx軸から90°の方向を向くので、先端は肘の真上、0.3 m高い位置にあります。最後のケースでは肘を時計回りに曲げるので、先端はx軸の方へ下がります。

変換の連鎖による順運動学

三角法は2リンクなら使えますが、3Dで6リンクになると扱いきれなくなります。規模が大きくなっても使えるのは、レッスン02の方法です。すべてのリンクに固有の座標系を与え、チェーンに沿って変換を掛け合わせます。

アームに沿って進み、各ステップを4x4の同次変換として書き出します。

1. z軸まわりにtheta1だけ回転する（関節1）。
2. 新しいx軸に沿ってl1だけ平行移動する（リンク1）。ここで肘に到達し、向きはまだリンク1に沿っている。
3. z軸まわりにtheta2だけ回転する（関節2）。
4. 新しいx軸に沿ってl2だけ平行移動する（リンク2）。ここで先端に到達し、x軸はリンク2に沿っている。

4つの行列の積がT_base_toolで、先端にあるツール座標系の姿勢を表します。最後の列が先端の位置、左上の2x2ブロックがツールの向きです。

import numpy as np

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

def rot_z_h(theta: float) -> np.ndarray:
    """4x4 homogeneous rotation about z."""
    c, s = np.cos(theta), np.sin(theta)
    T = np.eye(4)
    T[:2, :2] = [[c, -s], [s, c]]
    return T

def trans_x_h(d: float) -> np.ndarray:
    """4x4 homogeneous translation along x."""
    T = np.eye(4)
    T[0, 3] = d
    return T

def fk_transforms(theta1: float, theta2: float) -> np.ndarray:
    """Pose of the end-effector frame in the base frame: rotate, slide along the link, repeat."""
    return rot_z_h(theta1) @ trans_x_h(L1) @ rot_z_h(theta2) @ trans_x_h(L2)

def fk_geometric(theta1: float, theta2: float) -> np.ndarray:
    return np.array([L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2),
                     L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)])

T = fk_transforms(np.deg2rad(30), np.deg2rad(60))
np.set_printoptions(precision=3, suppress=True)
print("T_base_tool at (30 deg, 60 deg):\n", np.round(T, 3) + 0.0)
print("tool x axis points at", np.round(np.rad2deg(np.arctan2(T[1, 0], T[0, 0])), 1), "deg")

rng = np.random.default_rng(0)
q = rng.uniform(-np.pi, np.pi, size=(1000, 2))
err = [np.linalg.norm(fk_transforms(a, b)[:2, 3] - fk_geometric(a, b)) for a, b in q]
print(f"max difference over 1000 random configurations: {max(err):.2e} m")

T_base_tool at (30 deg, 60 deg):
 [[ 0.    -1.     0.     0.346]
 [ 1.     0.     0.     0.5  ]
 [ 0.     0.     1.     0.   ]
 [ 0.     0.     0.     1.   ]]
tool x axis points at 90.0 deg
max difference over 1000 random configurations: 2.24e-16 m

並進の列(0.346, 0.5)は、(30°, 60°)での幾何学的な結果と一致します。回転ブロックは、ツールのx軸がベースのy方向を向いていることを示しています。これはリンク2の向きである90°方向です。1000通りのランダムな関節配置で、2つの方法の差は最大2.2e-16 mでした。これは64ビット浮動小数点数の丸め誤差です。両者は同じ関数を2通りに書いたものです。

変換を使う方法は、同じ手間でより多くのものが得られます。位置だけでなく向きも返します。各関節がどの軸まわりに回転するかを変えるだけで、3Dに拡張できます。さらに途中のすべての座標系の姿勢も得られ、これは衝突判定器が必要とする情報です。産業界ではこの手順を2通りに定式化しています。各リンクを4つの数で表すDenavit-Hartenbergパラメータと、各関節をスクリュー軸で表す指数積（product of exponentials）です。両者の比較はDHとPoEのチートシートを参照してください。

モデル誤差が先端に及ぼす影響

順運動学の精度は、そこに入れる数値の精度で決まります。実際のリンク1がモデルより1 mm長ければ、計算した先端位置は最大1 mmずれます。角度の誤差は距離によって増幅されます。肩のエンコーダのゼロ点が0.1°ずれていると、先端は「リーチ×0.1°をラジアンに換算した値」だけ横にずれます。アームをまっすぐ伸ばした状態では0.7 × 0.00175で、約1.2 mmです。ベース側の誤差は最も長いてこを介して効くため、先端付近の誤差よりも影響が大きくなります。アームで関節のゼロ点とリンク長をキャリブレーションするのはこのためです。図面から写したモデルがミリ単位で実機と一致することがまれなのも、同じ理由です。

到達可能な作業空間

先端はどの点に到達できるでしょうか。ベースから先端までの距離は、肘の角度だけで決まります。2つのリンクがつくる三角形に余弦定理を適用すると、次の式が得られます。

r^2 = l1^2 + l2^2 + 2 l1 l2 cos(theta2)

次に肩の角度theta1が、その距離を保ったまま先端を一周させます。肘をまっすぐ伸ばすと（theta2 = 0）、リーチはl1 + l2 = 0.7 mです。完全に折り返すと（theta2 = 180°）、|l1 - l2| = 0.1 mです。その間のすべての半径に到達できるので、作業空間は2つの円に挟まれた輪（円環）になります。以下のコードは両関節をグリッド上で掃引し、各関節配置のリーチを測って、両端の値を式と比べます。次に、肘を±150°に制限して同じ掃引を繰り返します。これは典型的な機械的ストッパの例です。

import numpy as np

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

def fk_geometric(theta1: np.ndarray, theta2: np.ndarray) -> np.ndarray:
    x = L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2)
    y = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)
    return np.stack([x, y], axis=-1)

def sweep(theta2_limit_deg: float, n: int = 361) -> np.ndarray:
    """Distance from the base to the tip over a grid of joint angles."""
    t1 = np.linspace(-np.pi, np.pi, n)
    lim = np.deg2rad(theta2_limit_deg)
    t2 = np.linspace(-lim, lim, n)
    g1, g2 = np.meshgrid(t1, t2)
    return np.linalg.norm(fk_geometric(g1, g2), axis=-1)

r = sweep(180.0)
print(f"full joint range:     r_min {r.min():.4f} m, r_max {r.max():.4f} m")
print(f"  expected:           r_min {abs(L1 - L2):.4f} m, r_max {L1 + L2:.4f} m")
print(f"  workspace area:     {np.pi * (r.max()**2 - r.min()**2):.4f} m^2")

r = sweep(150.0)
inner = np.sqrt(L1**2 + L2**2 + 2 * L1 * L2 * np.cos(np.deg2rad(150)))
print(f"theta2 within +-150:  r_min {r.min():.4f} m (expected {inner:.4f} m), r_max {r.max():.4f} m")

# Reach as a function of the elbow angle alone, for the chart
for d in range(0, 181, 10):
    rad = np.sqrt(L1**2 + L2**2 + 2 * L1 * L2 * np.cos(np.deg2rad(d)))
    print(f"[{d}, {rad:.3f}]", end=", " if d < 180 else "\n")

full joint range:     r_min 0.1000 m, r_max 0.7000 m
  expected:           r_min 0.1000 m, r_max 0.7000 m
  workspace area:     1.5080 m^2
theta2 within +-150:  r_min 0.2053 m (expected 0.2053 m), r_max 0.7000 m
[0, 0.700], [10, 0.697], [20, 0.690], [30, 0.677], [40, 0.659], [50, 0.636], [60, 0.608], [70, 0.576], [80, 0.540], [90, 0.500], [100, 0.456], [110, 0.410], [120, 0.361], [130, 0.309], [140, 0.257], [150, 0.205], [160, 0.156], [170, 0.117], [180, 0.100]

関節を全範囲で動かすと、掃引結果は円環そのものになります。内側0.1 m、外側0.7 mで、面積は約1.51 m²です。肘を±150°に制限しても外縁は変わりませんが、内縁は0.205 mまで外に広がります。アームを十分に折りたためなくなり、先端をベースの近くまで持ってこられないためです。グラフは最後に出力した行、つまり肘の角度に対するリーチをプロットしたものです。

曲線はゼロ付近で平らです。まっすぐ伸びた肘を10°曲げても、リーチは3 mmしか短くなりません。180°付近では、曲線は逆向きに再び急になります。アームを伸ばしきった側でのこの平坦さは、特異点の最初の兆候です。theta2 = 0では、関節を少し動かしても先端はアームの方向にほとんど動きません。このことは、レッスン04の逆運動学で実際に問題を引き起こします。

作業空間については、実用上の注意点が2つあります。第一に、円環の内側の点は、位置に関してのみ到達可能です。実際の作業ではツールの向きも指定することが多く、その向きで到達できる点の集合はより小さくなります。ツールがあらゆる向きで到達できる点の集合は器用作業空間（dexterous workspace）と呼ばれ、さらに小さくなります。第二に、円環全体に到達できるのは、障害物が何もない場合だけです。ベース、テーブル、アーム自身がその一部を使えなくします。そこで必要になるのが衝突判定と動作計画のハブです。

演習

1. fk_transformsに3つ目の回転関節と0.1 mのリンクを追加してください。3つの関節すべてを掃引し、新しい内側半径と外側半径を報告してください。3リンクアームはベース原点に到達できるでしょうか。
2. リンク長を0.35 mと0.35 mに変えてください。内側半径はどうなるでしょうか。また、設計者が等しいリンク長を選ぶ理由は何でしょうか。
3. theta1を0〜180°に制限し（テーブルの縁より下に振れてはならない、テーブル設置型のアーム）、theta2は制限なしとしてください。先端が到達できる点をプロットまたは出力し、その形を説明してください。
4. 第1関節がz軸まわり、第2関節がy軸まわりに回転する3Dアームについて、fk_transformsを書いてください。1つの関節配置について、手計算の結果と照合してください。

まとめ

• 順運動学は、関節角をエンドエフェクタの姿勢に写像します。結果は一意で常に定義され、シミュレータ・衝突判定器・IKソルバの内部で使われます。
• 平面2リンクアームではx = l1 cos θ1 + l2 cos(θ1 + θ2)、y = l1 sin θ1 + l2 sin(θ1 + θ2)です。ここでθ2はリンク1から測ります。
• rot_z(θ1) @ trans_x(l1) @ rot_z(θ2) @ trans_x(l2)の連鎖からは、同じ位置に加えて向きも得られます。3Dアームに拡張できるのはこの方法です。
• 関節を全範囲で動かせる2リンクアームの作業空間は、|l1 - l2|からl1 + l2までの円環です。肘の関節制限は内側半径を外に広げます。

さらに読む

• Lynch and Park, Modern Robotics: Mechanics, Planning, and Control, Cambridge University Press, 2017：第4章「Forward Kinematics」では、空間座標系とエンドエフェクタ座標系のそれぞれで指数積公式を導出しています。
• Siciliano, Sciavicco, Villani and Oriolo, Robotics: Modelling, Planning and Control, Springer, 2009：第2章「Kinematics」では、シリアルマニピュレータの順運動学をさらに詳しく扱っています。
• Corke, Robotics, Vision and Control: Fundamental Algorithms in Python, 3rd edition, Springer, 2023：著者のオープンソースのRobotics Toolbox for Pythonを使い、アームの運動学をコードで計算しています。

訂正

• 2026年11月14日：器用作業空間を「指定した向きで到達できる点の集合」と説明していました。正しくは「あらゆる向きで到達できる点の集合」です。