はじめに

初めまして。大学院でロボティクスの研究をしているronaと申します。

本記事では「hand-eye calibration（ハンドアイ・キャリブレーション）」の仕組みについて、その基礎から理論までを体系的に解説します。

主に「OpenCVなどのライブラリを使って実装したことはあるけれど、内部でどのような計算が行われているのか分からずモヤモヤしている」という方を対象としています。ブラックボックスになりがちなキャリブレーションの内部構造を紐解き、数式の意味をスッキリと理解していただくことを目指します。

hand-eye calibrationとは

hand-eye calibration（ハンドアイ・キャリブレーション）とは、ロボットアームとカメラを組み合わせて視覚フィードバックを行うシステムにおいて、「ロボットの座標系」と「カメラの座標系」の正確な位置・姿勢関係を求めるための校正作業です。

カメラとロボットの配置関係によって、大きく以下の2種類に分類されます。

1. eye-in-hand

上図のように、「カメラがロボットの手先（エンドエフェクタ）に固定されている」構成です。（カメラが手先にくっついているように見えないのはすみません、、本当はくっついている図を載せるべきです💦）
ロボットが動くとカメラも一緒に動き、カメラの視点も変化します。

2. eye-to-hand

「カメラが外部の空間に固定されており、ロボットを外から俯瞰する」構成です。
ロボットが動いてもカメラの位置は変わりません。

hand-eye calibrationの必要性

ロボットアームが「カメラに映った物体を掴む（ピッキングする）」というタスクを想像してみてください。

画像処理や深度センサーを駆使して、カメラから見た物体の3次元位置（カメラ座標系での位置）が正確に計算できたとします。しかし、ロボットのコントローラは常に「ロボット自身の根元（ベース座標系）」を基準にして手先を動かしています。そのため、カメラ視点での座標をそのままロボットに伝えても、目的の場所へ正確に手を伸ばすことはできません。

ロボットに意図通りの動作をさせるためには、「カメラ座標系における物体の3次元位置」を「ロボットのベース座標系における3次元位置」へと変換する必要があります。この2つの座標系を繋ぐ架け橋となる変換行列（カメラとロボットの相対的な位置・姿勢関係）を事前に求める作業こそが、hand-eye calibrationの役割であり、ロボットを視覚ベースで正確に制御するための必須プロセスとなります。

定式化

ロボットとカメラのキャリブレーション問題を解くために、各座標系の関係を同次変換行列（$4 \times 4$ の行列）を用いて定式化します。

ここでは、ある座標系 $B$ から座標系 $A$ への同次変換行列を $T_{B}^{A}$ と表記します。（座標系 $A$ から見た座標系 $B$ の相対的な姿勢を表します）。

登場する主な座標系は以下の4つです。

$B$ : Base（ロボットのベース座標系）
$E$ : End-effector（ロボットの手先座標系）
$C$ : Camera（カメラ座標系）
$T$ : Target（キャリブレーションボードのターゲット座標系）

eye-in-handの場合

eye-in-handでは、カメラがロボットの手先に固定され、キャリブレーションターゲットが外部の空間に固定されています。

各変換行列の性質は以下の通りです。

$T_{E}^{B}$ : ロボットベースから見た手先の姿勢。ロボットのコントローラから順運動学によって取得できる変数（ロボットを動かすたびに変化する）。
$T_{T}^{C}$ : カメラから見たターゲットの姿勢。カメラの画像処理（チェスボードやARマーカーの認識など）から計算して取得できる変数（ロボットを動かしてカメラの視点が変わるたびに変化する）。
$T_{T}^{B}$ : ロボットベースから見たターゲットの姿勢。ターゲットは空間に置かれているため固定値だが、直接は測れない未知数。正確には測れるが，この値を正確に測る必要はない.
$T_{C}^{E}$ : 手先から見たカメラの姿勢。カメラは手先に固定されているため固定値。★これが今回求めたい未知の行列（目的の値）です。

eye-to-handの場合

eye-to-handでは、カメラが外部空間に固定され、キャリブレーションターゲットがロボットの手先に固定されています。

各変換行列の性質は以下の通りです。

$T_{E}^{B}$ : ロボットベースから見た手先の姿勢。ロボットのコントローラから取得できる変数。
$T_{T}^{C}$ : カメラから見たターゲットの姿勢。カメラの画像処理から取得できる変数。
$T_{T}^{E}$ : 手先から見たターゲットの姿勢。ターゲットは手先に固定されているため固定値だが、未知数。
$T_{C}^{B}$ : ロボットベースから見たカメラの姿勢。カメラは空間に固定されているため固定値。★これが今回求めたい未知の行列（目的の値）です。

解法

定式化した座標系の関係から、未知の固定値行列を求めるための有名な方程式 $AX = XB$ を導出します。複数の姿勢データからこの方程式を解くことで、目的のキャリブレーション行列を得ることができます。

eye-in-handの場合

ベース座標系 $B$ からターゲット座標系 $T$ への変換は、ロボットの手先を経由するルートを掛け合わせたもので表現できます。これを閉ループ方程式として表すと以下のようになります。

$$T_{E}^{B} \cdot T_{C}^{E} \cdot T_{T}^{C} = T_{T}^{B}$$

ここで、ロボットを2つの異なる姿勢（状態1と状態2）に動かしたとします。ターゲットは空間に固定されているため、ベースから見たターゲットの姿勢 $T_{T}^{B}$ は状態1でも状態2でも一定です。

状態1: $T_{E1}^{B} \cdot T_{C}^{E} \cdot T_{T1}^{C} = T_{T}^{B}$
状態2: $T_{E2}^{B} \cdot T_{C}^{E} \cdot T_{T2}^{C} = T_{T}^{B}$

右辺が等しいことから、以下の等式が成り立ちます。

$$T_{E1}^{B} \cdot T_{C}^{E} \cdot T_{T1}^{C} = T_{E2}^{B} \cdot T_{C}^{E} \cdot T_{T2}^{C}$$

未知数である $T_{C}^{E}$ を中心に整理するため、この式の両辺に対し、左から $(T_{E2}^{B})^{-1}$ を掛け、右から $(T_{T1}^{C})^{-1}$ を掛けます。

$$(T_{E2}^{B})^{-1} \cdot T_{E1}^{B} \cdot T_{C}^{E} = T_{C}^{E} \cdot T_{T2}^{C} \cdot (T_{T1}^{C})^{-1}$$

ここで、以下のように置き換えます。

$A = (T_{E2}^{B})^{-1} \cdot T_{E1}^{B}$ （姿勢2から見た姿勢1への手先の相対的な動き：既知）
$B = T_{T2}^{C} \cdot (T_{T1}^{C})^{-1}$ （状態1から状態2へのカメラから見たターゲットの相対的な動き：既知）
$X = T_{C}^{E}$ （求めたい未知の固定行列）

すると、キャリブレーションにおける代表的な方程式が得られます。

$$AX = XB$$

eye-to-handの場合

eye-to-handの場合、カメラ座標系 $C$ とロボットベース座標系 $B$ を経由してターゲットへ至るループは以下のようになります。

$$T_{C}^{B} \cdot T_{T}^{C} = T_{E}^{B} \cdot T_{T}^{E}$$

eye-in-handと同様に、ロボットを2つの異なる姿勢（状態1と状態2）に動かします。手先に固定されたターゲットの姿勢 $T_{T}^{E}$ は常に一定であることを利用し、$T_{T}^{E} =$ の形に変形します。

状態1から: $T_{T}^{E} = (T_{E1}^{B})^{-1} \cdot T_{C}^{B} \cdot T_{T1}^{C}$
状態2から: $T_{T}^{E} = (T_{E2}^{B})^{-1} \cdot T_{C}^{B} \cdot T_{T2}^{C}$

左辺が等しいことから、以下が成り立ちます。

$$(T_{E1}^{B})^{-1} \cdot T_{C}^{B} \cdot T_{T1}^{C} = (T_{E2}^{B})^{-1} \cdot T_{C}^{B} \cdot T_{T2}^{C}$$

両辺に対し、左から $T_{E2}^{B}$ を掛け、右から $(T_{T1}^{C})^{-1}$ を掛けます。

$$T_{E2}^{B} \cdot (T_{E1}^{B})^{-1} \cdot T_{C}^{B} = T_{C}^{B} \cdot T_{T2}^{C} \cdot (T_{T1}^{C})^{-1}$$

ここで、以下のように置き換えます。

$A = T_{E2}^{B} \cdot (T_{E1}^{B})^{-1}$ （ベースにおける手先の相対的な動き：既知）
$B = T_{T2}^{C} \cdot (T_{T1}^{C})^{-1}$ （カメラから見たターゲットの相対的な動き：既知）
$X = T_{C}^{B}$ （求めたい未知の固定行列）

これにより、eye-to-handの場合もまったく同じ形の方程式に帰着します。

$$AX = XB$$

方程式 $AX = XB$ の解法について

この方程式を解くための代表的な解析手法として、Tsai's method（Tsai-Lenz法）や、Dual Quaternion（双対四元数）を用いた Daniilidis の手法などが広く知られています。

現代では、OpenCVなどの画像処理ライブラリに cv2.calibrateHandEye といった専用の関数が用意されており、内部でこれらのアルゴリズムが実装されています。そのため、実務やプログラミングにおいては、ロボットを複数回動かして取得した $A$ と $B$ のデータセットを関数に渡すだけで、未知の行列 $X$ を容易に算出することが可能です。