numpyで学ぶ座標系・回転・変換

ロボットのプログラムは位置と姿勢であふれています。そのどれもが、何かを基準に測られています。LiDARは点をLiDAR自身から見た位置で出力します。経路計画器はその点を地図基準で必要とします。アームのコントローラは、把持の位置姿勢をベース基準で必要とします。このレッスンでは、こうした座標系の間で数値を移すための道具を作ります。回転行列と4x4の同次変換です。すべての要素が見えるよう、numpyで書きます。本レッスンはレッスン01の続きです。レッスン03では、ここで扱う内容をすべて使ってアーム先端の位置を計算します。

座標系と右手の法則

座標系は、原点と、互いに直交する3本の単位軸x・y・zからなります。ロボットには多くの座標系があります。ワールドに固定されたもの、移動ベース上のもの、各リンク上のもの、各センサ上のもの、ツール上のものです。位置は座標系と組み合わせて初めて意味を持ちます。「カップは(0.5, 0.2, 0.1)にある」という文は、「ベース座標系で」と言うまで意味を持ちません。

ロボティクスの座標系は右手系です。右手の指をxの向きに伸ばし、yの方へ曲げてください。親指がzの向きを指します。言い換えると、zはxとyの外積です。同じ右手で回転の符号も決まります。親指を軸の向きに合わせると、指の曲がる向きが正の回転です。したがって、zまわりの正の回転はxをyの方へ回します。zがこちらを向いているとき、これは反時計回りに見えます。

名前の付け方を習慣にすると、座標系に関するバグの大半はなくなります。座標系aで表した座標系bの位置姿勢をT_a_b、座標系aで表した点をp_aと書きます。するとT_a_b @ p_bはp_aになり、連鎖もすっきり読めます。T_a_b @ T_b_cは、内側の文字が一致して打ち消し合うのでT_a_cになります。内側の文字が一致しなければ、そのコードは間違っています。

回転行列

回転行列R_a_bは3x3の行列です。その列は、座標系bのx・y・z軸を座標系aの座標で表したものです。この一文で、回転行列の他の性質はすべて説明できます。b座標で与えられたベクトルにR_a_bを掛けると、bの軸の重み付き和が得られます。これは同じベクトルをaで表したものです。

列が互いに直交する単位ベクトルなので、この行列は正規直交行列です。転置と自身の積は単位行列になり、逆行列は転置に等しくなります。行列式は+1です。行列式が-1なら鏡映を意味します。鏡映は右手系を左手系に変えるので、正しい回転にはなりません。ファイル、センサドライバ、手計算から得た回転行列には、この2つの確認を行う価値があります。

座標軸まわりの3つの基本回転があれば、任意の回転を作れます。

import numpy as np

def rot_z(theta: float) -> np.ndarray:
    """Rotation about the z axis by theta radians (right-hand rule)."""
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, 0.0],
                     [s,  c, 0.0],
                     [0.0, 0.0, 1.0]])

def rot_x(theta: float) -> np.ndarray:
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[1.0, 0.0, 0.0],
                     [0.0, c, -s],
                     [0.0, s,  c]])

def rot_y(theta: float) -> np.ndarray:
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[ c, 0.0, s],
                     [0.0, 1.0, 0.0],
                     [-s, 0.0, c]])

R = rot_z(np.deg2rad(30))
np.set_printoptions(precision=3, suppress=True)
print("R_z(30 deg) =\n", R)
print("columns are the rotated x, y, z axes; x axis now points at", R[:, 0])
print("R.T @ R is the identity:", np.allclose(R.T @ R, np.eye(3)))
print("det(R) =", round(float(np.linalg.det(R)), 6))

# A point 1 m along the rotated frame's x axis, expressed in the original frame
p_b = np.array([1.0, 0.0, 0.0])
print("p in frame a:", R @ p_b)

R_z(30 deg) =
 [[ 0.866 -0.5    0.   ]
 [ 0.5    0.866  0.   ]
 [ 0.     0.     1.   ]]
columns are the rotated x, y, z axes; x axis now points at [0.866 0.5   0.   ]
R.T @ R is the identity: True
det(R) = 1.0
p in frame a: [0.866 0.5   0.   ]

1列目の(0.866, 0.5, 0)が新しいx軸です。元のx方向にcos 30°、元のy方向にsin 30°の成分を持ちます。rot_yの符号の並びに注意してください。マイナス記号は、他の2つのように右上ではなく、左下にあります。これは右手の法則から導かれます（yまわりの正の回転はzをxの方へ回す）。記憶を頼りに行列を打ち込むと、典型的なバグの原因になる箇所です。

回転の合成

2回回転させるには、行列を掛け合わせます。注意すべき点は、行列の積が可換でないことです。3次元の回転も可換ではありません。本を1冊用意し、鉛直軸まわりに90°回してから、本自身の長辺まわりに90°回してください。最初に戻り、2つの回転を逆の順序で行います。本は別の向きになります。

import numpy as np

def rot_x(theta: float) -> np.ndarray:
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]])

def rot_z(theta: float) -> np.ndarray:
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])

def show(m: np.ndarray) -> np.ndarray:
    return np.round(m, 3) + 0.0   # adding 0.0 turns -0.0 into 0.0 for printing

q = np.pi / 2
zx = rot_z(q) @ rot_x(q)   # yaw 90 deg, then roll 90 deg about the new x axis
xz = rot_x(q) @ rot_z(q)   # roll 90 deg, then yaw 90 deg about the new z axis
print("Rz @ Rx =\n", show(zx))
print("Rx @ Rz =\n", show(xz))
print("same rotation?", np.allclose(zx, xz))
print("body x axis after Rz @ Rx:", show(zx[:, 0]))
print("body x axis after Rx @ Rz:", show(xz[:, 0]))

Rz @ Rx =
 [[0. 0. 1.]
 [1. 0. 0.]
 [0. 1. 0.]]
Rx @ Rz =
 [[ 0. -1.  0.]
 [ 0.  0. -1.]
 [ 1.  0.  0.]]
same rotation? False
body x axis after Rz @ Rx: [0. 1. 0.]
body x axis after Rx @ Rz: [0. 0. 1.]

1つ目の場合、本体のx軸は最終的にワールドのy方向を向きます。2つ目の場合はワールドのz方向を向きます。rot_z(a) @ rot_x(b)のような積には、等価な2通りの読み方があります。左から右に読むと、各回転は直前の回転でできた座標系の軸（動く軸、つまり本体の軸）まわりの回転です。右から左に読むと、各回転は最初の固定された軸まわりの回転です。どちらの読み方でも同じ行列になります。コードベースではどちらか一方の読み方に決めて守り、それをコメントに書いておいてください。

3つの角度の組が、軸の順序と固定軸・動く軸の区別を明示するまで曖昧なのも、これが理由です。こうした規約は24通り使われています。REP 103がオイラー角を推奨しない理由として挙げている点です（後述）。

同次変換

回転は、座標系bがどう回っているかを表します。完全な位置姿勢には、その原点がどこにあるかも必要です。同次変換は、この両方を1つの4x4行列にまとめます。

T_a_b = | R_a_b   t_a_b |
        | 0 0 0     1   |

ここでt_a_bは、aで表したbの原点の位置です。点を変換するには、末尾に1を付けて4次元ベクトルにし、掛け算をしてから1を取り除きます。最下行のおかげで並進も行列の積で表せるので、座標系の連鎖全体を1回の行列積にまとめられます。レッスン03の順運動学は、この連鎖で計算します。ROSのtf2ライブラリが「この座標系はあの座標系から見てどこにあるか」に答える仕組みも同じです。

逆変換は閉じた形で求まります。T_a_bの回転がR、並進がtなら、T_b_aの回転はR.T、並進は-R.T @ tです。一般の逆行列を使う必要はありません。一般の逆行列は計算が遅く、丸め誤差によって厳密な構造が崩れます。

次の例では、odom形式のワールド座標系の中に移動ベースが北を向いて立っています。ベースには前向きのセンサが取り付けられています。センサが物体を検出したので、その物体の位置をワールド座標系で求めます。続いて、逆向きの変換も行います。

import numpy as np

def rot_z(theta: float) -> np.ndarray:
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])

def make_transform(R: np.ndarray, t: np.ndarray) -> np.ndarray:
    """4x4 homogeneous transform from a 3x3 rotation and a 3-vector translation."""
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    return T

def invert_transform(T: np.ndarray) -> np.ndarray:
    """Inverse of a rigid transform without a general matrix inverse."""
    R, t = T[:3, :3], T[:3, 3]
    return make_transform(R.T, -R.T @ t)

def apply(T: np.ndarray, p: np.ndarray) -> np.ndarray:
    return (T @ np.append(p, 1.0))[:3]

np.set_printoptions(precision=3, suppress=True)

# Pose of the robot base in the world (odom) frame: 2 m east, 1 m north, facing north
T_world_base = make_transform(rot_z(np.pi / 2), np.array([2.0, 1.0, 0.0]))
# Pose of a forward-looking sensor on the base: 0.2 m forward, 0.5 m up, no rotation
T_base_sensor = make_transform(np.eye(3), np.array([0.2, 0.0, 0.5]))
# The sensor sees an object 1.5 m straight ahead and 0.3 m to its left
p_sensor = np.array([1.5, 0.3, 0.0])

T_world_sensor = T_world_base @ T_base_sensor
p_world = apply(T_world_sensor, p_sensor)
print("object in world frame:", p_world)

# Go back: where is that world point in the base frame?
T_base_world = invert_transform(T_world_base)
p_base = apply(T_base_world, p_world)
print("object in base frame: ", p_base)
print("closed-form inverse matches np.linalg.inv:",
      np.allclose(T_base_world, np.linalg.inv(T_world_base)))
print("T @ inv(T) is identity:", np.allclose(T_world_base @ T_base_world, np.eye(4)))

object in world frame: [1.7 2.7 0.5]
object in base frame:  [1.7 0.3 0.5]
closed-form inverse matches np.linalg.inv: True
T @ inv(T) is identity: True

答えを手計算で確かめます。ベースは(2, 1)にあり、北を向いています。したがって、ワールドでは「前」が+y、「左」が-xです。センサはベースの0.2 m前方、ワールドの(2, 1.2)にあります。物体はそこからさらに1.5 m前方、0.3 m左にあります。ワールドでは(2 - 0.3, 1.2 + 1.5) = (1.7, 2.7)で、高さはセンサと同じ0.5 mです。ベース座標系では、同じ物体は0.2 + 1.5 = 1.7 m前方、0.3 m左にあり、2行目と一致します。

REP 103：ROSが採用する規約

規約は、全員が同じものを使って初めて役に立ちます。ROSのエコシステムでは、Tully FooteとMike PurvisによるREP 103「Standard Units of Measure and Coordinate Conventions」に規約がまとめられています。当サイトもこれに従います。主な内容は次のとおりです。

• 単位はSI。長さはメートル、質量はキログラム、時間は秒、角度はラジアン。
• すべての座標系は右手系。
• ロボット本体はx前方、y左、z上。
• 近距離用のワールド座標系はENU（x東、y北、z上）。大きなfloat32値による精度の問題を避けるため、REPでは開始位置など近くに原点を置くことを推奨している。
• カメラ光学座標系（接尾辞_opticalを付けて命名）はz前方、x右、y下。これは画像処理の慣例で、カメラの本体座標系とは異なる。両者を取り違えると、すべての点群が90°回転する。
• 北・東・下（north-east-down）が必要な屋外システムでは、接尾辞_nedを付けた別の座標系を使う。
• 回転の表現は優先順に、四元数（クォータニオン）、回転行列、x・y・z軸まわりの固定軸ロール・ピッチ・ヨー。オイラー角は、競合する規約が多いため推奨されない。

関連文書のREP 105は、移動ベースの標準的な座標系としてmap、odom、base_linkを定めています。レッスン07でこれらを使います。その他の内容は空間数学のハブにまとめています。

四元数についての補足

回転行列は、3 自由度（DoF）を9個の数で表します。保存や送信には無駄が多く、掛け算を何度も繰り返すと、丸め誤差で行列が正規直交からずれていきます。ロール・ピッチ・ヨーは3個の数で済みます。しかし規約が曖昧で、2つの角度が同じ動きを表してしまう特異点（ジンバルロック）があります。

単位四元数は、1つの制約（長さが1）を持つ4個の数を使います。ジンバルロックがなく、再正規化の計算が軽く、姿勢の間をなめらかに補間できます。REP 103が四元数を第一に挙げ、ROSのメッセージが姿勢を四元数で運ぶのはこのためです。代償として、数値が直感的でなく、成分の順序がライブラリによって異なります。SciPyとROSのメッセージは(x, y, z, w)で、スカラー部が最後です。MuJoCoとspatialmath-pythonは(w, x, y, z)で、スカラー部が最初です。

import numpy as np
from scipy.spatial.transform import Rotation

r = Rotation.from_euler("z", 90, degrees=True)   # yaw of 90 degrees about z
np.set_printoptions(precision=4, suppress=True)
print("quaternion, SciPy default order (x, y, z, w):", r.as_quat())
print("quaternion, scalar first (w, x, y, z):       ", r.as_quat(scalar_first=True))
print("same rotation as a matrix:\n", np.round(r.as_matrix(), 4) + 0.0)

quaternion, SciPy default order (x, y, z, w): [0.     0.     0.7071 0.7071]
quaternion, scalar first (w, x, y, z):        [0.7071 0.     0.     0.7071]
same rotation as a matrix:
 [[ 0. -1.  0.]
 [ 1.  0.  0.]
 [ 0.  0.  1.]]

出力された2つの四元数は、同じ数を異なる順序で並べたものです。誤った規約で読むと、2つ目はzまわりではなくxまわりの回転を表すことになります。回転表現のチートシートに、変換方法と、どのライブラリがどの順序を使うかをまとめています。

演習

1. 上のコードを見ずに、右手の法則からrot_yを書いてください。次に確認します。yまわりの正の回転はzをxへ移すので、rot_y(np.pi / 2) @ [0, 0, 1]は(1, 0, 0)になるはずです。
2. ベースの0.3 m前方、0.4 m上にカメラが取り付けられ、20°下向きに傾いています（ベースのy軸まわりの正の回転）。カメラは、自身のx軸に沿って真正面2 mの点を見ています。この点はベース座標系でどこにあるでしょうか。また、視線は床面z = 0とベースの前方どれだけの距離で交わるでしょうか。変換はmake_transformとrot_yで作ってください。
3. 3回のrot_*呼び出しで作ったランダムな回転から始めます。float32で自身を10,000回掛け合わせ、R.T @ Rが単位行列からどれだけずれるかを測ってください。次に、SciPyのRotationオブジェクトで同じことを行ってください。違いを説明してください。
4. センサがカメラ光学座標系（z前方、x右、y下）になるように、センサの例を書き換えてください。センサの本体座標系と光学座標系の間には、どの回転が必要でしょうか。

まとめ

• 座標系は、原点と右手系の3本の軸からなる。変換をT_a_b（aで表したb）と名付けると、T_a_b @ T_b_cのような連鎖を目で確認できる。
• 回転行列の列は、回転後の座標系の軸である。正しい回転はR.T @ R = Iとdet(R) = +1を満たし、逆行列は転置に等しい。
• 回転は可換ではない。回転の列が固定軸まわりか動く軸まわりかを明示する。
• 4x4の同次変換は回転と並進をまとめて表す。その逆は、回転がR.T、並進が-R.T @ tである。
• REP 103は、SI単位、右手系、本体はx前方・y左・z上、局所ワールド座標系はENU、カメラ光学座標系はz前方と定めている。四元数の成分順序はライブラリによって異なる。

さらに読む

• Lynch and Park, 「Modern Robotics: Mechanics, Planning, and Control」, Cambridge University Press, 2017：第3章「Rigid-Body Motions」で、回転行列、同次変換、指数座標を、この分野で広く使われている記法で解説している。
• Corke, 「Robotics, Vision and Control: Fundamental Algorithms in Python」, 第3版, Springer, 2023：各考え方に、著者のオープンソースPythonツールボックスで実行できるコードを組み合わせている。位置姿勢と回転を扱うspatialmath-pythonも含まれる。
• REP 103, Standard Units of Measure and Coordinate Conventions：上でまとめたROSの規約。全文を通して読める短さである。
• SciPy Rotationのドキュメント：行列、四元数、回転ベクトル、オイラー角の相互変換と、scalar_firstオプションの説明。