空間の数学

概要

ロボットの計算はどれも、ある物が別の物に対してどこにあるかを問います。カップに対するグリッパ、ベースに対するLiDAR、地図に対するロボット。空間の数学は、こうした問いに曖昧さなく答えるための小さな道具一式です。

数学そのものは古く、すでに確立しています。バグは慣例から生まれます。軸がどちらを向くか、四元数をどの順に格納するか、変換が点を座標系Aから座標系Bへ写すのか、その逆なのか。このハブは、右に曲がるはずのロボットが左に曲がるのを見たことがある人のためのページです。表現方法、ROSの座標系の慣例、推定や最適化の論文を読むのに足りるだけのリー群の用語を扱います。

重要な考え方

回転とその格納方法

回転行列は、行列式が+1の3×3の正規直交行列です。数値は9個ありますが、3 自由度（DoF）しかありません。合成は積で、逆は転置で求まるため、計算に使うには最も安全な形式です。

オイラー角は、順に適用する軸まわりの3つの角度を格納します。コンパクトで読みやすい一方、REP 103は使用を推奨していません。有効な回転順序が多数あり、分野によって既定の順序が異なるためです。また、どの回転順序にも、2つの軸が一直線に並んで1つの角度が定まらなくなる姿勢があります（ジンバルロック）。

単位四元数は、4つの数値で特異点なしに回転を格納します。REP 103では第一の選択肢です。四元数のバグの大半は2つの事実から生じます。1つ目に、qと-qは同じ回転を表すため、要素ごとに比較すると同一の回転でも一致しないことがあります。2つ目に、成分の並び順がライブラリによって異なります。

下のコードは、同じ90°のヨーを2通りの並び順で示し、2つの変換を合成します。

import numpy as np
from scipy.spatial.transform import Rotation
from spatialmath import SE3, UnitQuaternion

# A 90 degree yaw (rotation about z), written two ways.
r = Rotation.from_euler("z", np.pi / 2)
print("SciPy as_quat (x, y, z, w):", np.round(r.as_quat(), 4))
print("SciPy scalar_first (w, x, y, z):", np.round(r.as_quat(scalar_first=True), 4))
print("spatialmath UnitQuaternion (w, x, y, z):", np.round(UnitQuaternion.Rz(np.pi / 2).vec, 4))

# Compose transforms: T_map_base = T_map_odom @ T_odom_base
T_map_odom = SE3(2.0, 0.0, 0.0) * SE3.Rz(np.pi / 2)
T_odom_base = SE3(1.0, 0.0, 0.0)
T_map_base = T_map_odom * T_odom_base
print("base_link origin in map:", np.round(T_map_base.t, 3))
# The inverse gives the map origin seen from base_link
print("map origin in base_link:", np.round(T_map_base.inv().t, 3))

出力は次のとおりです。

SciPy as_quat (x, y, z, w): [0.     0.     0.7071 0.7071]
SciPy scalar_first (w, x, y, z): [0.7071 0.     0.     0.7071]
spatialmath UnitQuaternion (w, x, y, z): [0.7071 0.     0.     0.7071]
base_link origin in map: [2. 1. 0.]
map origin in base_link: [-1.  2.  0.]

同次変換

同次変換は、回転Rと並進pを1つの4×4行列にまとめます。これにより、座標系の連鎖が行列積の連鎖になります。各変換には、関係づける2つの座標系の名前を付けてください。このハブでは、T_a_bは座標系aで表した座標系bの姿勢であり、bで書かれた点をaへ写します。この命名なら内側の添字が打ち消し合い、T_map_odom @ T_odom_baseはT_map_baseになります。逆変換には閉じた形があります。回転はRの転置になり、並進はRの転置とpの積の符号を反転したものになります。一般の逆行列計算ではなく、こちらを使ってください。

座標系の慣例：REP 103とREP 105

ROSは、全文を読む価値のある2つの短い文書で慣例を定めています。REP 103はSI単位系と右手系の座標系を定めます。機体座標系はx前方、y左、z上です。ローカルな世界座標系はeast-north-up（ENU、東・北・上）を使います。サフィックス_opticalの付いたカメラ座標系はz前方、x右、y下で、サフィックス_nedはnorth-east-down（北・東・下）の座標系を示します。ヨー角は反時計回りに増え、地理的な姿勢では東向きがゼロです。これはコンパス方位とは異なります。

REP 105は移動プラットフォームの座標系に名前を付けています。base_linkはロボットに固定された座標系です。odomは世界に固定された座標系で、その中でロボットの姿勢は連続的に変化しますが、際限なくドリフトします。mapも世界に固定された座標系で、その中で姿勢はドリフトしないはずですが、自己位置推定が補正したときに不連続に跳ぶことがあります。各座標系は親を1つしか持てないため、ツリーはmap、odom、base_linkの順につながります。自己位置推定のコンポーネントはmapにおけるロボットの姿勢を計算しますが、配信するのはmapからodomへの変換で、odomからbase_linkへの変換はオドメトリ源に任せます。tf2ライブラリはこのツリーを保持し、指定した時刻における任意の2つの座標系間の問い合わせに答えます。

実務レベルのリー群

回転の集合SO(3)と剛体変換の集合SE(3)はリー群です。リー群とは、合成と逆はとれるものの、加算はできない滑らかな空間です。2つの回転行列を足しても回転行列にはなりません。各群は単位元に平坦な接空間、すなわちリー代数を持ち、そこでは通常のベクトル演算が使えます。SO(3)の接ベクトルは3次元ベクトルで、回転軸に角度を掛けたものです。SE(3)では、並進成分と回転成分を持つ6次元のツイストです。

両者は2つの写像でつながっています。指数写像は接ベクトルを群の元に変換し、対数写像はその逆を行います。実務のほとんどはこれで足ります。姿勢間の補間、回転の平均、小さな姿勢誤差の6次元ベクトルとしての表現とその共分散の設定、群の外に出ずに姿勢に勾配ステップを適用すること、などです。推定ライブラリも、/topics/kinematics/で扱う指数積（product of exponentials）による運動学も、これを土台にしています。

どこから始めるか

/learn/beginner/02-frames-and-rotations/を学んだ後、コードを書く間は/reference/rotation-representations/を開いておいてください。/learn/beginner/10-hello-ros2/ではtf2を実際に使います。プロジェクト/projects/ros2-talker-listener-tf2/では小さな変換ツリーを構築します。

ツールとプラットフォーム

よくある間違い

• w先頭の四元数をw末尾のAPIに渡してしまう。結果は回転としては有効ですが誤っているため、何もクラッシュしません。
• 変換を逆向きに読んでしまう。すべての変数名に座標系の名前を入れてください。
• 補間や平均にオイラー角を使ってしまう。四元数に変換するか、接空間で計算してください。
• 光学座標系を忘れてしまう。画像のz軸は上ではなく、レンズから外向きです。
• 四元数の正規化を最後にしか行わない。小さな誤差が積み重なるため、合成のたびに正規化し直してください。

さらに深く

• REP 103: Standard Units of Measure and Coordinate ConventionsとREP 105: Coordinate Frames for Mobile Platforms、ROS Enhancement Proposals。
• Joan Solà、Jeremie Deray、Dinesh Atchuthan、A micro Lie theory for state estimation in robotics、arXiv、2018年。実務向けの入門として最も読みやすい文献です。
• Kevin M. Lynch、Frank C. Park、「Modern Robotics」、Cambridge University Press、2017年。第3章で剛体運動、ツイスト、指数座標を扱います。
• Peter Corke、「Robotics, Vision and Control: Fundamental Algorithms in Python」、第3版、Springer、2023年。前半の章でコードによる姿勢の表現を扱います。