- はじめに — 角度を入れると手先がどこにあるか教えてくれる関数
- 座標系、回転行列、そして平行移動
- 同次変換行列がなぜ4×4なのか
- フレームを連鎖させるとなぜ掛け算になるのか
- 2リンク腕の順運動学を手で導く
- DHパラメータ — リンクひとつを4つの数字で書く
- 回転表現のトレードオフ — オイラー角、回転行列、クォータニオン
- 符号ひとつが間違うと実際に何が起きるか
- 締めくくり — 座標系を正確に書き留めることが半分です
はじめに — 角度を入れると手先がどこにあるか教えてくれる関数
ロボットアームの構造を決めてサーボを取り付けたとします。これで各関節の角度が読めるようになりました。肩30度、肘45度。
では手先はどこにありますか。
この問いの答えを求めるのが順運動学であり、ロボットアームで行うすべての計算の出発点です。逆運動学も、軌道計画も、衝突検査も、カメラが見た物体を腕の座標に移すことも、すべてこの計算の上に積み上がります。
幸い順運動学は簡単です。角度を入れると位置がひとつ出て、常に出て、掛け算数回で終わります。 関節が6個でも10個でも原理は同じです。
問題はリンクが増えると三角関数の式が手に負えないほど長くなることです。2リンクは手で書けて3リンクもどうにかなりますが、軸が3次元でそれぞれ違う方向を向く6リンクは手で書けません。だから人が使える表記法が必要で、それが同次変換行列とDHパラメータです。
この記事はその2つの道具がなぜあの形なのかを数字で説明します。
座標系、回転行列、そして平行移動
ロボットアームでは座標系がリンクごとにひとつ付きます。肩にひとつ、上腕にひとつ、前腕にひとつ、手先にひとつ。各座標系は自分のリンクに固定され一緒に動きます。
こうする理由は各リンクの中ではすべてが定数だからです。手先が手首の座標系でどこにあるかは、関節がどう動いても変わりません。変わるのは座標系同士の関係だけです。だから問題が「複雑な形状」から「座標系間の変換いくつか」に縮まります。
座標系同士の関係は2つでできています。どれだけ回転しているか、そしてどれだけ離れているかです。
回転は行列です。2次元で反時計回りにθだけ回る回転は、
R(θ) = [ cos θ -sin θ ]
[ sin θ cos θ ]
3次元でz軸まわりの回転は、
Rz(θ) = [ cos θ -sin θ 0 ]
[ sin θ cos θ 0 ]
[ 0 0 1 ]
この行列を列ごとに読むと意味がはっきりします。1列目は回転された座標系のx軸が元の座標系でどの方向を指すか、2列目はy軸、3列目はz軸です。回転行列は3つの軸の方向を並べて書いたものです。
平行移動はベクトルの足し算です。座標系がtだけ移動していれば、点の座標にtを足します。
ここに不便な点があります。回転は掛け算で平行移動は足し算です。両方を一緒に書くと、
p_world = R · p_local + t
この形は複数の段階を連鎖させると式が汚くなります。3段階つなげると、
p = R1·(R2·(R3·p + t3) + t2) + t1
= R1·R2·R3·p + R1·R2·t3 + R1·t2 + t1
関節が6個なら項が6個に増え、各項に手前の回転すべての積が付きます。手で扱うのが難しく、コードに移すのも面倒です。
同次変換行列がなぜ4×4なのか
解決策は意外と単純です。座標に1をひとつ付け足します。
3次元の点(x, y, z)を(x, y, z, 1)と書きます。そして4×4行列をこう作ります。
T = [ R11 R12 R13 tx ]
[ R21 R22 R23 ty ]
[ R31 R32 R33 tz ]
[ 0 0 0 1 ]
左上の3×3が回転行列、右上の3×1が平行移動、下の1行は常に0 0 0 1です。
この行列に(x, y, z, 1)を掛けると何が起きるか、1行目だけ計算してみます。
R11·x + R12·y + R13·z + tx·1
回転の1行目に平行移動の1成分目が足されました。まさにR·p + tの1成分目です。最後の行は0·x + 0·y + 0·z + 1·1 = 1なので、付け足した1がそのまま維持されます。
掛けられる対象に定数1が入っているため、その列の成分が足し算のように振る舞います。 これが4×4のすべてです。最後の行の0 0 0 1は、結果が依然として有効な点であり続けるようにする装置です。
数字で確認してみます。フレーム2がフレーム1に対してz軸まわりに45度回っていて、x方向に0.20メートル離れています。フレーム2で手先は自分のx軸上0.15メートルの地点にあります。
import numpy as np
np.set_printoptions(precision=6, suppress=True)
def transform_z(theta, tx):
"""z軸まわりにthetaだけ回してからx軸方向にtxだけ移動する4×4同次変換です。"""
c, s = np.cos(theta), np.sin(theta)
return np.array([[c, -s, 0, tx],
[s, c, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
T = transform_z(np.radians(45), 0.20)
print("T (フレーム2 -> フレーム1):")
print(T)
p_local = np.array([0.15, 0.0, 0.0, 1.0]) # フレーム2から見た手先
p_world = T @ p_local
print("フレーム2での手先:", p_local[:3])
print("フレーム1での手先:", np.round(p_world[:3], 6))
print("手で求めた値 :", np.round([0.20 + 0.15*np.cos(np.radians(45)),
0.15*np.sin(np.radians(45)), 0.0], 6))
実行結果です。
T (フレーム2 -> フレーム1):
[[ 0.707107 -0.707107 0. 0.2 ]
[ 0.707107 0.707107 0. 0. ]
[ 0. 0. 1. 0. ]
[ 0. 0. 0. 1. ]]
フレーム2での手先: [0.15 0. 0. ]
フレーム1での手先: [0.306066 0.106066 0. ]
手で求めた値 : [0.306066 0.106066 0. ]
手で計算すると、
x = 0.20 + 0.15 × cos(45度) = 0.20 + 0.106066 = 0.306066
y = 0 + 0.15 × sin(45度) = 0 + 0.106066 = 0.106066
行列が出した値と同じです。
同次変換行列には便利な性質がもうひとつあります。逆変換が安く済むということです。一般的な4×4行列の逆行列は計算が重いですが、同次変換行列は構造を利用してこう書けます。
T⁻¹ = [ Rᵀ -Rᵀ·t ]
[ 0 1 ]
回転行列の逆は転置なので(直交行列だから)、掛け算数回で済みます。手先座標系から見た物体の位置をベース座標系へ移すことと、その逆が両方とも安く済むという意味で、カメラが手首に付いた腕が毎フレーム行う計算はまさにこれです。
フレームを連鎖させるとなぜ掛け算になるのか
ここで、なぜこの表現がロボットアームにぴったり合うのかが分かります。
ベースからリンク1への変換をT01、リンク1からリンク2への変換をT12とします。リンク2座標系の点pをベース座標系へ移すには、
p_1 = T12 · p
p_0 = T01 · p_1 = T01 · T12 · p
変換をつなげることが行列を掛けることと正確に同じです。 先ほど見た汚い展開式が消えました。
関節が6個なら、
T06 = T01 · T12 · T23 · T34 · T45 · T56
これが順運動学のすべてです。各変換に関節角度がひとつ入っていて、掛けると手先の位置と姿勢が出ます。位置は結果の行列の右上3×1で、姿勢は左上3×3です。
掛け算の順序には注意が必要です。行列積は交換法則が成り立たないのでT01·T12とT12·T01は違います。順序を覚えるコツは添字を見ることです。内側の添字がかみ合う必要があります。T01·T12では1と1が接し、残るのが0と2なので結果はT02です。
2リンク腕の順運動学を手で導く
行列を使う前にもっとも単純な場合を三角関数で解いておくと、後で検算になります。
平面上にリンクが2つあります。上腕L1 = 0.20 m、前腕L2 = 0.15 m。肩角度θ1はx軸から測った絶対角、肘角度θ2は上腕に対する相対角です。
肘の位置から求めます。肩からθ1方向にL1だけ進んだので、
肘 = (L1·cos θ1, L1·sin θ1)
手先は肘からさらにL2だけ進みます。このとき前腕が向く絶対方向はθ1 + θ2です。相対角を絶対角に変えるこの一行が要点です。
x = L1·cos(θ1) + L2·cos(θ1 + θ2)
y = L1·sin(θ1) + L2·sin(θ1 + θ2)
θ1 = 30度、θ2 = 45度を入れると、
x = 0.20 × cos(30度) + 0.15 × cos(75度)
= 0.20 × 0.8660254 + 0.15 × 0.2588190
= 0.1732051 + 0.0388229
= 0.2120279
y = 0.20 × sin(30度) + 0.15 × sin(75度)
= 0.20 × 0.5000000 + 0.15 × 0.9659258
= 0.1000000 + 0.1448889
= 0.2448889
手先は(0.212028, 0.244889)です。
リンクをもうひとつ付けると同じ規則が繰り返されます。L3 = 0.10 m、θ3 = -60度なら3番目のリンクの絶対方向は30 + 45 - 60 = 15度で、
x = 0.2120279 + 0.10 × cos(15度) = 0.2120279 + 0.0965926 = 0.3086205
y = 0.2448889 + 0.10 × sin(15度) = 0.2448889 + 0.0258819 = 0.2707708
パターンが見えます。各リンクは自分より手前までの角度の和の方向へ、自分の長さだけ進みます。 平面腕ならこれで十分で、実際に関節の数がいくつでも同じ式を使います。
ところが関節軸がそれぞれ違う方向を向く3次元の腕では、この方式が崩れます。肩の最初の軸は垂直なのに2番目の軸は水平で、手首の3軸はまた互いに直交しています。角度を単純に足すことができません。ここから行列が必要になります。
DHパラメータ — リンクひとつを4つの数字で書く
リンクひとつをつなぐ変換には本来6つの自由度が必要です。回転3つに平行移動3つ。ところがロボットのリンクは適当に付いているわけではありません。関節軸があり、リンクはその軸同士をつなぎます。この構造を利用して座標系を規則に従って置くと、6つが4つに減ります。
デナビト・ハーテンバーグ規約がその規則で、4つの数字はこうです。
| 記号 | 名称 | 意味 |
|---|---|---|
a | リンク長 | 隣り合う2つの関節軸の間の共通垂線の長さ |
α | リンクねじれ | 共通垂線まわりに測った2つの関節軸の間の角度 |
d | リンクオフセット | 関節軸に沿って測った2つの共通垂線の間の距離 |
θ | 関節角 | 関節軸まわりに測った2つの共通垂線の間の角度 |
回転関節ではθが変数で残り3つが定数です。直動関節ではdが変数で残り3つが定数です。関節ひとつに変数ひとつという対応がきれいに保たれます。
この4つの数字が作る変換は、4つの基本変換をつなげたものです。
A_i = Rot_z(θ_i) · Trans_z(d_i) · Trans_x(a_i) · Rot_x(α_i)
展開するとこうなります。
A = [ cos θ -sin θ·cos α sin θ·sin α a·cos θ ]
[ sin θ cos θ·cos α -cos θ·sin α a·sin θ ]
[ 0 sin α cos α d ]
[ 0 0 0 1 ]
ここで必ず知っておくべき落とし穴があります。DH規約はひとつではありません。
上のものは標準(古典)DHで、クレイグの教科書が使う修正DHは積の順序が違います。
標準DH: A_i = Rot_z(θ_i) · Trans_z(d_i) · Trans_x(a_i) · Rot_x(α_i)
修正DH: A_i = Rot_x(α_{i-1}) · Trans_x(a_{i-1}) · Rot_z(θ_i) · Trans_z(d_i)
2つの規約は座標系を付ける位置が違い、だから同じロボットのパラメータ表が互いに異なります。修正DHではaとαにi-1の添字が付き、これが表を読むとき規約を見分けるもっとも速い手がかりです。論文から持ってきた表を標準DHのコードにそのまま入れると腕が変な形になり、原因を探すのに数日かかります。
参考までに、標準DHの積の順序をTrans_z(d)·Rot_z(θ)·Trans_x(a)·Rot_x(α)と書いた資料もよく見かけます。これは間違いではなく同じものです。Rot_zとTrans_zは同じ軸を扱うので交換可能で、Trans_xとRot_xも同様です。
3リンク平面腕のDH表を書いてみます。すべての関節軸が平面に垂直に並んでいるので、ねじれとオフセットは全部0です。
| i | a_i | α_i | d_i | θ_i |
|---|---|---|---|---|
| 1 | 0.20 | 0 | 0 | 変数θ1 |
| 2 | 0.15 | 0 | 0 | 変数θ2 |
| 3 | 0.10 | 0 | 0 | 変数θ3 |
コードで確認します。
import numpy as np
np.set_printoptions(precision=6, suppress=True)
def dh_transform(a, alpha, d, theta):
"""標準(古典)DH規約のリンク変換行列です。
Rot_z(theta) · Trans_z(d) · Trans_x(a) · Rot_x(alpha) を展開した形です。"""
ct, st = np.cos(theta), np.sin(theta)
ca, sa = np.cos(alpha), np.sin(alpha)
return np.array([[ct, -st * ca, st * sa, a * ct],
[st, ct * ca, -ct * sa, a * st],
[0., sa, ca, d],
[0., 0., 0., 1.]])
def forward_kinematics(dh_rows, q):
"""DH表の各行は(a, alpha, d, theta_offset)です。"""
T = np.eye(4)
for (a, alpha, d, offset), theta in zip(dh_rows, q):
T = T @ dh_transform(a, alpha, d, offset + theta)
return T
PLANAR_3R = [(0.20, 0.0, 0.0, 0.0),
(0.15, 0.0, 0.0, 0.0),
(0.10, 0.0, 0.0, 0.0)]
q = np.radians([30, 45, -60])
T = forward_kinematics(PLANAR_3R, q)
print("DH連鎖が与えた変換行列:")
print(T)
# 手で導いた閉じた形と比較します。
a = np.cumsum(q)
x = 0.20 * np.cos(a[0]) + 0.15 * np.cos(a[1]) + 0.10 * np.cos(a[2])
y = 0.20 * np.sin(a[0]) + 0.15 * np.sin(a[1]) + 0.10 * np.sin(a[2])
print(f"閉じた形: x={x:.6f} y={y:.6f}")
print(f"DH結果 : x={T[0,3]:.6f} y={T[1,3]:.6f}")
print("2つの結果は同じか:", np.allclose([x, y], T[:2, 3]))
print(f"手先の向き(3角度の合計) = {np.degrees(a[2]):.4f}度、"
f"行列から読んだ値 = {np.degrees(np.arctan2(T[1,0], T[0,0])):.4f}度")
実行結果です。
DH連鎖が与えた変換行列:
[[ 0.965926 -0.258819 0. 0.308621]
[ 0.258819 0.965926 0. 0.270771]
[ 0. 0. 1. 0. ]
[ 0. 0. 0. 1. ]]
閉じた形: x=0.308621 y=0.270771
DH結果 : x=0.308621 y=0.270771
2つの結果は同じか: True
手先の向き(3角度の合計) = 15.0000度、行列から読んだ値 = 15.0000度
手で導いた(0.308621, 0.270771)と行列連鎖が出した値が一致します。そして結果の行列の左上3×3から読んだ手先の向きが、3角度の合計である15度と同じです。位置だけでなく姿勢まで一度に出るのが行列方式の利点です。
現代のツールを使うとこの表をそのまま移せます。ピーター・コークのRobotics Toolbox for Python(PyPI roboticstoolbox-python、2026年7月時点で1.3.1、Python 3.10以上)では、標準DHリンクをRevoluteDH、修正DHリンクをRevoluteMDHで区別し、この2つをひとつのロボットに混ぜることはできません。規約がフラグではなくクラスで分かれていることが、かえって安全装置になっています。DHRobotでまとめて.fkine(q)を呼べば、上のコードが一行になります。
ROSのエコシステムはDHの代わりにURDFを使います。リンクとジョイントをXMLで書き、ジョイントごとに親リンクに対する位置と軸を明示します。
<joint name="elbow" type="revolute">
<parent link="upper_arm"/>
<child link="forearm"/>
<origin xyz="0.20 0 0" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="-2.6" upper="2.6" effort="1.5" velocity="3.0"/>
</joint>
URDFでジョイントタイプとして使える文字列は正確に6つです。revolute、continuous、prismatic、fixed、floating、planar。そしてlimit要素でeffortとvelocityは必須、lowerとupperは任意です。回転範囲が無限の関節ならcontinuousを使い上下限を省略します。
URDFがDHより優れている点は座標系を規則に合わせて置く必要がないことです。原点を都合のよい場所に置いて軸の方向だけ正確に書けばよいです。代わりにパラメータが関節あたり6つに増えます。人がCADから寸法を書き写すにはURDFのほうがずっと楽で、手で式を導くにはDHのほうが楽です。
回転表現のトレードオフ — オイラー角、回転行列、クォータニオン
位置は数字3つで終わりますが、姿勢はそうはいきません。3次元回転は自由度が3つですが、その3つをどう書くかにはいくつかの選択肢があり、それぞれ代償が異なります。
| 表現 | 保存する数 | 特異点 | 補間 | 合成コスト | 主に使う場所 |
|---|---|---|---|---|---|
| 回転行列 | 9(制約6個) | なし | 直接は不可 | 行列積27回 | 計算内部、運動学 |
| オイラー角(rpy) | 3 | ジンバルロック | 自然だが危険 | 行列に変換が必要 | 人が読み書きする場所、URDF |
| 軸角 | 4(または3) | 角度0で軸が未定義 | 中程度 | 中程度 | 回転ベクトル、微小回転 |
| クォータニオン | 4(制約1個) | なし | slerpで自然 | 掛け算16回 | 姿勢推定、補間、保存 |
各項目の代償をひとつずつ見ていきます。
回転行列は計算にもっとも便利です。ベクトルを回すのが掛け算1回で、合成も掛け算1回です。代わりに9つの数で3つの自由度を表現するので、6つの制約が付きます。列が互いに直交していて長さが1でなければなりません。浮動小数点誤差が積み重なるとこの制約が少しずつ崩れ、放っておくと物体が微妙に伸びたり歪んだりします。だから定期的に正規直交化が必要です。
オイラー角は人にもっとも親切です。ロール30度、ピッチ0度、ヨー90度と言えば誰でも絵が描けます。URDFのrpy属性がこれで、規約は固定軸基準でx、y、z順、行列ではRz(yaw)·Ry(pitch)·Rx(roll)です。
問題はジンバルロックです。2番目の角度が90度になると1番目の軸と3番目の軸が同じ方向を向くようになり、異なる角度の組み合わせが同じ回転を作ります。
import numpy as np
np.set_printoptions(precision=6, suppress=True)
def rotation_rpy(roll, pitch, yaw):
"""URDFのrpyと同じ規約です。固定軸x、y、z順に回すことが
行列ではRz(yaw) · Ry(pitch) · Rx(roll)と同じです。"""
cr, sr = np.cos(roll), np.sin(roll)
cp, sp = np.cos(pitch), np.sin(pitch)
cy, sy = np.cos(yaw), np.sin(yaw)
Rx = np.array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]])
Ry = np.array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]])
Rz = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]])
return Rz @ Ry @ Rx
def rpy_from_rotation(R):
"""回転行列からrpyを逆算します。分母がcos(pitch)である点が要点です。"""
pitch = np.arcsin(np.clip(-R[2, 0], -1.0, 1.0))
roll = np.arctan2(R[2, 1], R[2, 2])
yaw = np.arctan2(R[1, 0], R[0, 0])
return np.degrees([roll, pitch, yaw])
A = rotation_rpy(np.radians(0), np.radians(90), np.radians(30))
B = rotation_rpy(np.radians(-30), np.radians(90), np.radians(0))
print("roll=0, pitch=90, yaw=30 と roll=-30, pitch=90, yaw=0")
print(" 2つの行列は同じか:", np.allclose(A, B), " 最大差:", np.abs(A - B).max())
# ジンバルロックの直前で、ごく小さな姿勢変化がrpyをどれだけ揺らすか見ます。
base = rotation_rpy(np.radians(0), np.radians(89.9), np.radians(30))
print("\n(0, 89.9, 30)を逆算すると:", np.round(rpy_from_rotation(base), 4))
for eps in (0.001, 0.01, 0.1):
perturbed = rotation_rpy(np.radians(eps), 0.0, 0.0) @ base
print(f" x軸に{eps:>5}度揺らすとrpy = {np.round(rpy_from_rotation(perturbed), 4)}"
f" (行列最大差 {np.abs(perturbed - base).max():.6f})")
実行結果です。
roll=0, pitch=90, yaw=30 と roll=-30, pitch=90, yaw=0
2つの行列は同じか: True 最大差: 3.0616169978683824e-17
(0, 89.9, 30)を逆算すると: [ 0. 89.9 30. ]
x軸に0.001度揺らすとrpy = [ 0.4937 89.8995 30.4937] (行列最大差 0.000017)
x軸に 0.01度揺らすとrpy = [ 4.715 89.8946 34.715 ] (行列最大差 0.000175)
x軸に 0.1度揺らすとrpy = [30. 89.8268 60. ] (行列最大差 0.001745)
前の2行がジンバルロックそのものです。まったく違う2つの角度の組み合わせが同じ回転で、差が3×10⁻¹⁷なので浮動小数点精度のレベルで同一です。
後の3行が実務でより痛い部分です。ピッチが89.9度の姿勢で腕をx軸に0.01度だけ揺らしたのに、逆算したロールが0度から4.715度へ、ヨーが30度から34.715度へ動きました。入力0.01度に対して出力4.7度、470倍の増幅です。 0.1度揺らすとロールが30度、ヨーが60度になります。
原因は逆算する式の分母にあります。ロールとヨーを割るのに使う量がcos(pitch)で、ピッチが90度に近づくとこれが0に向かいます。0に近い数で割る計算は入力の小さな雑音を大きく膨らませます。
実際の症状はこうです。腕がこの姿勢付近を通るとき、エンコーダの雑音や浮動小数点誤差だけでロールとヨーの値が大きく跳ね、その値をそのまま補間すると手首が一回転します。物理的には腕がほとんど動いていないのに、制御器は60度移動しろという指令を受けたことになります。手首関節がぐるぐる巻き付く事故のかなりの部分がここから生まれます。
クォータニオンはジンバルロックがありません。4つの数(w, x, y, z)に制約がひとつ(長さ1)だけなので自由度3にぴったり合い、表現が連続的です。2つの姿勢の間の補間(slerp)が自然で、回転の合成が行列より安上がりです。
代償は2つあります。人が数字だけを見て姿勢を想像できないことがひとつで、qと-qが同じ回転を表す二重被覆がもうひとつです。2つのクォータニオンの間を補間するとき符号を合わせておかないと、近い道の代わりに遠い道を回ってしまいます。
実務の答えはだいたいこうなります。保存と補間はクォータニオンで、計算は回転行列で、人に見せるものだけオイラー角で。 3つとも使いつつ、それぞれが得意な場所にだけ置くということです。
符号ひとつが間違うと実際に何が起きるか
順運動学のコードでもっともよくあるバグは複雑なものではありません。符号ひとつ、角度の規約ひとつです。
関節角度の向きが逆になった場合を見てみます。サーボを組み立てるときギアを反対側から噛ませると、指令角度が大きくなるにつれ関節が逆方向に回ります。コードはそのままなのにθ2の符号だけが逆になったことになります。
import numpy as np
L1, L2 = 0.20, 0.15
def fk(theta1, theta2):
return np.array([L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2),
L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)])
good = fk(np.radians(30), np.radians(45))
bad = fk(np.radians(30), np.radians(-45)) # θ2の符号だけ反転させました
print(f"正しい手先 : ({good[0]:.6f}, {good[1]:.6f})")
print(f"符号反転 : ({bad[0]:.6f}, {bad[1]:.6f})")
print(f"ずれた距離 : {np.linalg.norm(good - bad) * 1000:.3f} mm")
print(f"理論値 2·L2·sin(45度) = {2 * L2 * np.sin(np.radians(45)) * 1000:.3f} mm")
print("\n原点付近ではよく見えません:")
for d2 in (1, 5, 15, 45, 90):
g, b = fk(np.radians(30), np.radians(d2)), fk(np.radians(30), np.radians(-d2))
print(f" θ2={d2:3}度 -> 誤差 {np.linalg.norm(g - b) * 1000:7.3f} mm")
実行結果です。
正しい手先 : (0.212028, 0.244889)
符号反転 : (0.318094, 0.061177)
ずれた距離 : 212.132 mm
理論値 2·L2·sin(45度) = 212.132 mm
原点付近ではよく見えません:
θ2= 1度 -> 誤差 5.236 mm
θ2= 5度 -> 誤差 26.147 mm
θ2= 15度 -> 誤差 77.646 mm
θ2= 45度 -> 誤差 212.132 mm
θ2= 90度 -> 誤差 300.000 mm
45度で212ミリメートルずれます。腕全体の長さが350ミリメートルなのにです。そして誤差の大きさが正確に2·L2·sin(θ2)であることも確認できます。手先が上腕の延長線に対して鏡のように反射されるからです。
最後の表がこのバグの性格を物語っています。ゼロ点付近ではほとんど見えません。 θ2 = 1度で誤差は5ミリメートルで、組み立て公差やバックラッシュと勘違いしやすい大きさです。だからホームポーズ付近だけでテストすると通過し、実際の作業角度に行くと完全に外れます。
同じ性格のバグがもう少しあります。
ラジアンと度の混同。 numpyの三角関数はすべてラジアンを受け取ります。30をそのまま入れると30ラジアン、つまり1719度と解釈され、腕が4回転以上回った姿勢が計算されます。幸いこのバグは結果があまりに突飛なのですぐに発覚します。
絶対角と相対角の混同。 上の式のθ2は上腕に対する相対角です。エンコーダが絶対角を返す腕でその値をそのまま入れるとθ1が2回足されます。θ1 = 0のときは2つの規約が同じ値を返すので、このバグもホームポーズでは見えません。
ゼロオフセット。 DH表のθは規約が定めた基準から測った角度で、サーボが報告する角度は機構的なゼロ点から測った値です。2つが違うと定数分だけずれ、その定数をDH表のオフセット項に入れる必要があります。上のコードでoffset + thetaと書いた部分がその場所です。
この4つを一度に押さえる検証方法がひとつあります。関節をひとつだけ動かしてみることです。 残りを0にしたまま関節をひとつずつ既知の角度に回し、計算が予測した手先位置と定規で測った実際の位置を比べます。6つを同時に動かして原因を探そうとすると、どの関節の問題か区別できません。
締めくくり — 座標系を正確に書き留めることが半分です
順運動学の数学は難しくありません。回転行列に1行と1列を付け足して平行移動まで掛け算にし、それらを順番に掛けます。それで全部です。
難しいのは数学ではなく規約です。角度をどこで測るか、どちらの向きが正か、絶対角か相対角か、どのDH規約か、ゼロ点がどこか。この5つを文書に書き留めておかないと、数週間後の自分が必ず間違えます。
そして順運動学が間違うと、その上のすべてが静かに間違います。 逆運動学は順運動学の逆なので同じ符号の誤りをそのまま受け継ぎ、ヤコビアンは順運動学の偏微分なのでやはり受け継ぎます。カメラ校正も、衝突検査も、軌道計画も同じです。上の層で妙な症状を何日も追いかけた末、結局DH表の符号ひとつだったという場合はよくあります。
だから順運動学を作ったら必ず定規で測ってください。関節ひとつずつ、既知の角度で、実際の距離と比べて。計算と定規が5ミリメートル以内で合えば、その上に何を積んでも大丈夫です。
次の段階はこの関数を逆さにひっくり返すことです。逆運動学編で、それがなぜずっと難しいのか、そしてここで作った行列たちがどうヤコビアンにつながっていくのかを扱います。この記事に出てきた線形代数と三角法をどの水準まで知っておくべきかは、ロボット工学に必要な数学編に整理してあります。
현재 단락 (1/267)
[ロボットアームの構造](/blog/electronics/2026-08-02-robot-arm-anatomy-and-joints)を決めてサーボを取り付けたとします。これで各関節の角度が読...