チートシート
DHパラメータと指数積（PoE）のチートシート
標準DH、修正DH、指数積（PoE）公式を同じ3関節アームに適用して並べて比較します。NumPyのコードで、3つとも同じエンドエフェクタの位置姿勢になることを示します。
このシートは、データシートのDHパラメータ表と教科書のスクリュー軸モデルを手にしていて、両者が同じアームを表していることを確かめたい人のためのものです。順運動学でよく使われる3つの表記法を並べ、それぞれを1つの3関節アームに適用し、結果が一致することを数値で確認します。背景は順運動学を参照してください。用語集: DHパラメータ、指数積、同次変換。
3つの表記法
| 標準DH | 修正DH | 指数積（空間座標系） | |
|---|---|---|---|
| 関節ごとのパラメータ | theta_i, d_i, a_i, alpha_i | alpha_(i-1), a_(i-1), theta_i, d_i | スクリュー軸S_i = (omega_i, v_i)、6個の数値 |
| リンク変換 | Rz(theta) Tz(d) Tx(a) Rx(alpha) | Rx(alpha) Tx(a) Rz(theta) Tz(d) | exp([S_i] theta_i) |
| 座標系iの位置 | リンクiの遠位端、関節軸i+1上 | リンクiの近位端、関節軸i上 | リンク座標系はなし。ベースとツールのみ |
| 関節軸 | 座標系i-1のz | 座標系iのz | omega_i（ホーム姿勢でベース座標系により表す） |
| ツールの位置姿勢 | 表の最終行、または追加の固定変換 | 最後の関節の後に追加の固定変換が必要 | M（全関節がゼロのときのツールの位置姿勢） |
| チェーン全体 | T = A_1 A_2 … A_n | T = T_1 T_2 … T_n T_tool | T = exp([S_1] q_1) … exp([S_n] q_n) M |
| 平行またはほぼ平行な軸 | 共通法線が定まらず、軸のわずかな誤差でパラメータが大きく跳ぶ | 同じ問題がある | 特別扱いは不要 |
| 直動関節 | d_iが変数 | d_iが変数 | omega = 0、v = 単位方向ベクトル |
渡されたDH表は、使う前に必ず表のヘッダを読んでください。修正DHの表を標準DHのコードに通すと、もっともらしいが誤ったアームになります。
例題のアーム
回転関節3つで構成します。関節1はベースの鉛直軸z0まわりに回ります。関節2と関節3は互いに平行で、ホーム姿勢では水平軸-y0まわりに回ります。そのため正の角度でアームが持ち上がります。肩の高さD1 = 0.30 m、上腕A2 = 0.40 m、前腕A3 = 0.35 mです。ホーム姿勢（全関節がゼロ）では、アームはx0方向にまっすぐ伸びます。
| 関節 | 標準DH: theta, d, a, alpha | 修正DH: alpha_(i-1), a_(i-1), theta, d | 空間スクリュー軸: omega、軸上の点q、v = -omega x q |
|---|---|---|---|
| 1 | q1, 0.30, 0, +pi/2 | 0, 0, q1, 0.30 | (0, 0, 1), (0, 0, 0), (0, 0, 0) |
| 2 | q2, 0, 0.40, 0 | +pi/2, 0, q2, 0 | (0, -1, 0), (0, 0, 0.30), (0.30, 0, 0) |
| 3 | q3, 0, 0.35, 0 | 0, 0.40, q3, 0 | (0, -1, 0), (0.40, 0, 0.30), (0.30, 0, -0.40) |
| ツール | 3行目に含まれる | 関節3の後に固定のTx(0.35) | M: 回転Rx(+pi/2)、位置(0.75, 0, 0.30) |
標準DHの表はツールで終わります。a_3が前腕の長さを担うためです。修正DHの表は関節3で終わるので、前腕の長さは別のツール変換に入れる必要があります。同じアームを表す2つの表が最終行で食い違う最も多い原因は、この非対称性です。
NumPyによる標準DHと修正DH
import numpy as np
D1, A2, A3 = 0.30, 0.40, 0.35 # base height and link lengths (m)
def rot_x(a: float) -> np.ndarray:
c, s = np.cos(a), np.sin(a)
return np.array([[1, 0, 0, 0], [0, c, -s, 0], [0, s, c, 0], [0, 0, 0, 1.0]])
def rot_z(a: float) -> np.ndarray:
c, s = np.cos(a), np.sin(a)
return np.array([[c, -s, 0, 0], [s, c, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1.0]])
def trans(x: float = 0.0, y: float = 0.0, z: float = 0.0) -> np.ndarray:
t = np.eye(4)
t[:3, 3] = [x, y, z]
return t
def dh_standard(theta: float, d: float, a: float, alpha: float) -> np.ndarray:
return rot_z(theta) @ trans(z=d) @ trans(x=a) @ rot_x(alpha)
def dh_modified(alpha_prev: float, a_prev: float, theta: float, d: float) -> np.ndarray:
return rot_x(alpha_prev) @ trans(x=a_prev) @ rot_z(theta) @ trans(z=d)
def fk_standard(q: np.ndarray) -> np.ndarray:
table = [(q[0], D1, 0.0, np.pi / 2), (q[1], 0.0, A2, 0.0), (q[2], 0.0, A3, 0.0)]
t = np.eye(4)
for row in table:
t = t @ dh_standard(*row)
return t
def fk_modified(q: np.ndarray) -> np.ndarray:
table = [(0.0, 0.0, q[0], D1), (np.pi / 2, 0.0, q[1], 0.0), (0.0, A2, q[2], 0.0)]
t = np.eye(4)
for row in table:
t = t @ dh_modified(*row)
return t @ trans(x=A3) # fixed tool offset after the last joint
q = np.array([0.4, 0.7, -1.1])
print(np.round(fk_standard(q), 4))
print("standard == modified:", np.allclose(fk_standard(q), fk_modified(q)))NumPyによる指数積
単位回転スクリューの指数写像の閉形式は、回転部分をロドリゲスの公式から求め、並進部分にそれに対応する項を加えて得られます。次のコードでは、これをscipy.linalg.expmと照合します。
exp([S] t) = [ R(t) G(t) v ] R(t) = I + sin(t) W + (1 - cos(t)) W^2
[ 0 1 ] G(t) = I t + (1 - cos(t)) W + (t - sin(t)) W^2
W = skew(omega), |omega| = 1import numpy as np
from scipy.linalg import expm
D1, A2, A3 = 0.30, 0.40, 0.35 # same arm as the DH block (m)
def skew(w: np.ndarray) -> np.ndarray:
return np.array([[0, -w[2], w[1]], [w[2], 0, -w[0]], [-w[1], w[0], 0.0]])
def twist_matrix(s: np.ndarray) -> np.ndarray:
m = np.zeros((4, 4))
m[:3, :3] = skew(s[:3])
m[:3, 3] = s[3:]
return m
def exp_twist(s: np.ndarray, theta: float) -> np.ndarray:
"""Closed-form exponential of a unit revolute screw (omega, v)."""
w, v = s[:3], s[3:]
k = skew(w)
rot = np.eye(3) + np.sin(theta) * k + (1 - np.cos(theta)) * k @ k
g = np.eye(3) * theta + (1 - np.cos(theta)) * k + (theta - np.sin(theta)) * k @ k
t = np.eye(4)
t[:3, :3], t[:3, 3] = rot, g @ v
return t
def revolute_screw(w: list[float], q_point: list[float]) -> np.ndarray:
w, q_point = np.array(w, float), np.array(q_point, float)
return np.concatenate([w, -np.cross(w, q_point)])
# Space-frame screw axes at the home pose (all joint angles zero)
S = [revolute_screw([0, 0, 1], [0, 0, 0]),
revolute_screw([0, -1, 0], [0, 0, D1]),
revolute_screw([0, -1, 0], [A2, 0, D1])]
M = np.array([[1, 0, 0, A2 + A3], [0, 0, -1, 0], [0, 1, 0, D1], [0, 0, 0, 1.0]]) # tool pose at home
def fk_poe_space(q: np.ndarray) -> np.ndarray:
t = np.eye(4)
for s_i, q_i in zip(S, q):
t = t @ exp_twist(s_i, q_i)
return t @ M
q = np.array([0.4, 0.7, -1.1])
t = fk_poe_space(q)
print(np.round(t, 4))
print("closed form == expm:", np.allclose(exp_twist(S[1], 0.7), expm(twist_matrix(S[1]) * 0.7)))どちらのブロックも、q = (0.4, 0.7, -1.1) radに対して同じ行列を出力します。
500姿勢による独立した検証
このブロックでは、2つのDH表記法にRobotics Toolbox for PythonのRevoluteDHクラスとRevoluteMDHクラスを使います。両者を、expmで組んだPoEチェーンと比較します。
import numpy as np
import roboticstoolbox as rtb
from scipy.linalg import expm
D1, A2, A3 = 0.30, 0.40, 0.35
std = rtb.DHRobot([rtb.RevoluteDH(d=D1, alpha=np.pi / 2), rtb.RevoluteDH(a=A2), rtb.RevoluteDH(a=A3)], name="std")
mod = rtb.DHRobot([rtb.RevoluteMDH(d=D1), rtb.RevoluteMDH(alpha=np.pi / 2), rtb.RevoluteMDH(a=A2)], name="mod")
mod.tool = np.array([[1, 0, 0, A3], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1.0]])
def hat(s: np.ndarray) -> np.ndarray:
w, v = s[:3], s[3:]
return np.array([[0, -w[2], w[1], v[0]], [w[2], 0, -w[0], v[1]], [-w[1], w[0], 0, v[2]], [0, 0, 0, 0.0]])
S = np.array([[0, 0, 1, 0, 0, 0], [0, -1, 0, D1, 0, 0], [0, -1, 0, D1, 0, -A2]], float) # (omega, v)
M = np.array([[1, 0, 0, A2 + A3], [0, 0, -1, 0], [0, 1, 0, D1], [0, 0, 0, 1.0]])
def poe(q: np.ndarray) -> np.ndarray:
t = np.eye(4)
for s, qi in zip(S, q):
t = t @ expm(hat(s) * qi)
return t @ M
rng = np.random.default_rng(0)
worst = 0.0
for q in rng.uniform(-np.pi, np.pi, size=(500, 3)):
t_poe = poe(q)
worst = max(worst, np.abs(std.fkine(q).A - t_poe).max(), np.abs(mod.fkine(q).A - t_poe).max())
print(f"max difference over 500 random poses: {worst:.2e}")ボディ座標系のPoE
同じアームは、ホーム姿勢のツール座標系で表したスクリュー軸B_iを使っても記述できます。その場合はT = M exp([B_1] q_1) … exp([B_n] q_n)となります。2組の軸はB_i = Ad(M^-1) S_iの関係で結ばれます。ワールド座標系での軸の位置が分かっている場合は空間形式を使います。ヤコビ行列に基づく逆運動学のループの多くのように、ツール座標系を基準とする場合はボディ形式を使います。
DH表をスクリュー軸に変換する
- すべての関節をゼロにし、表を使って各DH座標系を計算します。
- 関節iについて、その関節の回転軸となる座標系（標準DHでは座標系i-1、修正DHでは座標系i）のz軸をベース座標系で表し、omega_iとします。
- その座標系の原点をq_iとし、v_i = -omega_i x q_iとします。
- Mは、ゼロ姿勢におけるDHチェーン全体です。ツール変換があればそれも含めます。
逆方向の変換は一意ではありません。PoEモデルはリンク座標系の位置を決めないため、1組のスクリュー軸に多数のDH表が対応します。
よくある落とし穴
- データシートの関節ゼロ位置が、DHのゼロと異なることがあります。その場合、表にはthetaのオフセットがあり、チェーンを評価する前にそれを加える必要があります。
- 単位: データシートのDH表は、長さをmm、角度を度で示すことが多くあります。使う前に両方を変換してください。
- 2つの平行な軸（ここでは関節2と関節3）があると、DHの共通法線は軸方向の位置が任意になります。2つの表がどちらも正しいまま、dの値が異なることがあります。
- 回転関節のスクリュー軸では、omegaは単位ベクトルです。単位ベクトルでないomegaを使うと、関節角が警告なしにスケーリングされます。
- 手で組んだモデルは、最後のブロックのように、複数のランダムな姿勢で別の情報源（ツールボックス、MuJoCo、ベンダーの順運動学）と照合してください。1つの姿勢で一致しても、ほとんど何も証明できません。
さらに読む
- K. M. Lynch、F. C. Park「Modern Robotics: Mechanics, Planning, and Control」Cambridge University Press、2017年、doi:10.1017/9781316661239。第4章でPoEによる順運動学を、付録CでDHパラメータを扱っています。
- 各変換の回転部分については回転の表現。
- 逆運動学と運動学のトピックページ。