ハンドアイキャリブレーション入門 第2章：カメラとロボットの位置関係をシミュレーションで求める

シリーズの構成

章	内容	ロボット	カメラ
第1章	ハンドアイキャリブレーションの考え方	―	―
第2章（今回）	シミュレーションで AX = XB を解き、正しく解けることを確認	シミュレーション	シミュレーション
第3章	ノイズ・姿勢数・回転範囲で精度がどう変わるか、5つの解法を比較	シミュレーション	シミュレーション
第4章	RealSense の内部パラメータは信用できるか	―	実測
第5章	ボードの位置姿勢の計測はどれくらいばらつくか	―	実測
第6章	実測したばらつきを使って、実機での精度を予測する	シミュレーション	実測ノイズ
番外編（予定）	市販の安価なロボットアームで実機検証	実機	実測

シリーズ「ハンドアイキャリブレーション入門」の第2章です。第1章では、ハンドアイキャリブレーションがカメラとロボットの位置関係 X を求める作業であり、AX = XB という式を解けばよいことを整理しました。

この記事では、ロボットを使わずにシミュレーションで AX = XB を実際に解き、正しく解けることを確かめます。

おさらい：登場する3つの座標変換

今回も、カメラを手先に取り付けた構成（eye-in-hand）で考えます。変換の名前は OpenCV と同じ「a2b」（a の座標系の点を b の座標系に直す変換。b から見た a の位置と向き）で書きます。

• A = gripper2base（手先 → 台座）：ロボットのコントローラから読み出せる
• B = target2cam（ボード → カメラ）：カメラ画像からボードを検出して計算できる
• X = cam2gripper（カメラ → 手先）：求めたい位置関係

ボードは台に固定しておき、ロボットをいろいろな姿勢に動かして A と B の組を集めると、AX = XB を解いて X が求まります。

シミュレーションの考え方：答えの分かっている問題を作って解かせる

本来はロボットアームが必要ですが、正解の X を自分で決めたシミュレーションを使えば、ロボットなしで手法を検証できます。しかも、実機ではできない「正解との直接比較」ができます。

考え方は「答えの分かっている問題を自分で作り、それを解かせて答え合わせをする」です。実機のハンドアイキャリブレーションで得られるのは A と B の組だけなので、シミュレーションでも正解の X から A と B の組を作り、X を隠して calibrateHandEye に解かせます。

シミュレーションの作り方

ステップ1：正解を決める

まず、本来は分からない2つの値を、シミュレーションの中で決めてしまいます。

項目	設定した値	意味
X（cam2gripper）	位置 (50, 20, 100) mm、回転 約21°	カメラは手先の中心から少しずれた位置に、少し傾いて付いている
ボードの位置（target2base）	台座から x 方向に 500 mm、水平	ボードは台の上に置いて固定

X は以降の計算で A と B を作るのに使い、最後の答え合わせに使います。calibrateHandEye には渡しません。

ステップ2：ボードを見るカメラの姿勢を作る

次に、カメラの姿勢（cam2base）を15通り作ります。実機で「ロボットを15か所に動かして、そのたびにボードを撮影する」ことにあたります。

カメラの姿勢は、次の5つの値を乱数で決めて作ります。

値	範囲	意味
傾き θ	0〜35°	ボードの真上からどれだけ傾けるか
方位 φ	0〜360°	どの方角に傾けるか（真上から見たときの向き）
距離 d	350〜600 mm	ボードの中心からカメラまでの距離
注視点	中心から ±30 mm	カメラが狙う点。毎回ボードの真ん中だと不自然なので少しずらす
ロール ψ	±90°	光軸まわりにカメラを回す角度

傾き・方位・距離からカメラの位置が決まり、カメラの光軸（z 軸）を注視点に向け、最後に光軸まわりにロール分だけ回します。これで、どの姿勢でもボードがカメラの正面に写ります。

ハンドアイキャリブレーションでは、いろいろな向きに大きく回した姿勢を集めるほど精度が上がることが知られています。傾きとロールの範囲は、そのための設定です。範囲を変えると精度がどう変わるかは、第3章で調べます。

ステップ3：ロボットの姿勢 A とボードの姿勢 B を計算する

カメラの姿勢が決まると、ロボットの手先の姿勢 A と、カメラから見たボードの姿勢 B が計算で決まります。

A = cam2base · X⁻¹

カメラは手先にねじ止めされているので、カメラの位置が決まれば手先の位置も1つに決まります。「カメラをこの位置に置くには、手先がどこにあればよいか」を逆算したのがこの式で、実機ではロボットのコントローラから読み出す値にあたります。

B = cam2base⁻¹ · target2base

カメラとボードの位置がどちらも分かっているので、「カメラから見たボードの位置と向き」も計算できます。実機ではカメラ画像からボードを検出して計算する値にあたります。

実機とは計算の順番が逆（実機はロボットを動かすとカメラがついてくる）ですが、手先とカメラの関係は同じなので、得られる A と B は実機のデータと同じ性質を持ちます。先にカメラの姿勢を決めるのは、ロボットの姿勢をランダムに作ると、カメラがボードと関係ない方向を向いてしまうからです。

ステップ4：計測ノイズを加える

実際のカメラは、ボードの位置と向きを完璧には測れません。そこで B に小さな誤差を加えます。

B’ = B · N

N は小さな回転と移動を表す行列で、回転は各軸で標準偏差 0.1°、移動は各軸で標準偏差 0.5 mm の正規分布から乱数で作ります。この値は仮に置いたもので、第5章で RealSense の実測値に置き換えます。

ステップ5：解いて、正解と比べる

15組の A と B’ を calibrateHandEye に渡して X を推定し、ステップ1で決めた正解と比べます。誤差は次の2つで評価します。

• 回転誤差 [°]：推定した向きと正解の向きの差を、1つの回転にまとめたときの角度
• 並進誤差 [mm]：推定した位置と正解の位置の距離

図4は、こうして作った15通りの姿勢を3次元で表示したものです。

このシミュレーションの前提

今回は、問題を単純にするために次の前提を置いています。実機ではこれらも誤差の原因になるので、後の章で順に扱います。

• ロボットは完璧に正確で、A には誤差がない
• カメラの誤差は「ボードの姿勢 B のずれ」としてまとめて扱い、画像のピクセルやレンズのゆがみまでは再現していない
• 誤差は姿勢ごとに独立で、偏り（いつも同じ方向にずれる誤差）はない

コードのポイント：OpenCV に渡す行列の向きに注意

座標変換はすべて 4×4 の同次変換行列（回転と平行移動をまとめた行列）で扱い、@（行列の掛け算）でつなぎます。変数名は OpenCV の引数名と同じ「a2b」の形にそろえています。

gripper2base = cam2base @ inv_T(cam2gripper)   # A：手先の姿勢（ロボットが報告する値）
target2cam = inv_T(cam2base) @ target2base     # B：カメラから見たボードの姿勢

# 計測ノイズ（各軸 回転0.1°、並進0.5mm の正規分布）
noise = make_T(rng.normal(0, np.deg2rad(0.1), 3), rng.normal(0, 0.0005, 3))
target2cam_noisy = target2cam @ noise

inv_T は逆行列で、向きを反対にします（inv_T(cam2gripper) は gripper2cam）。つなぎ方が正しいかは、次のルールで確かめられます。

• a2b @ c2a = c2b：右側の行き先と左側の出発点が同じなら正しい
• 例：cam2base @ gripper2cam = gripper2base（右の行き先 cam と左の出発点 cam が同じ）

OpenCV には回転行列と並進ベクトルを別々のリストで渡します。

R_x, t_x = cv2.calibrateHandEye(
    R_gripper2base, t_gripper2base,   # A の回転・並進
    R_target2cam,   t_target2cam,     # B の回転・並進
    method=cv2.CALIB_HAND_EYE_TSAI,
)
# 戻り値は cam2gripper（X）

引数の向きを取り違えるのが一番多いミスです。コードの変数名を最初から OpenCV の名前にそろえておけば、そのまま渡すだけで済みます。

eye-to-hand の場合は、gripper2base の代わりにその逆行列の base2gripper を渡すと、戻り値が cam2base（X）になります。

比較する5つの解法：Tsai、Park、Horaud、Andreff、Daniilidis とは

結果を見る前に、比較する5つの解法について説明しておきます。OpenCV の calibrateHandEye には AX = XB を解く方法が5つ用意されていて、引数 method で選びます。名前は、それぞれの方法を提案した論文の著者名です。

解法	method に指定する値	提案（年）	解き方
Tsai	cv2.CALIB_HAND_EYE_TSAI	Tsai・Lenz（1989）	回転を先に解き、次に移動を解く。最も古典的な方法
Park	cv2.CALIB_HAND_EYE_PARK	Park・Martin（1994）	回転を先に解き、次に移動を解く。回転を最小二乗法で求める
Horaud	cv2.CALIB_HAND_EYE_HORAUD	Horaud・Dornaika（1995）	回転を先に解き、次に移動を解く。回転をクォータニオン（4つの数）で表す
Andreff	cv2.CALIB_HAND_EYE_ANDREFF	Andreff ほか（1999）	回転と移動を同時に解く。式全体を1つの連立一次方程式にする
Daniilidis	cv2.CALIB_HAND_EYE_DANIILIDIS	Daniilidis（1999）	回転と移動を同時に解く。両方をまとめて表すデュアルクォータニオンを使う

大きく分けると、回転を先に解いてから移動を解くタイプ（Tsai、Park、Horaud）と、回転と移動を同時に解くタイプ（Andreff、Daniilidis）があります。回転を先に解くタイプは手順が分かりやすい反面、回転の誤差が移動の計算に持ち越されます。

ノイズがなければ、どの解法でも同じ正解にたどり着きます。違いが出るのは計測ノイズがあるときです。

結果1：ノイズがなければ誤差はほぼゼロになる

まずノイズを0にして解きます。座標系の向きを間違えていなければ、誤差はほぼ0になるはずです。

解法	回転誤差 [°]	並進誤差 [mm]
Tsai	4.5e-15	9.1e-14
Park	6.5e-14	3.2e-12
Horaud	2.2e-14	3.2e-14
Andreff	1.1e-14	8.5e-13
Daniilidis	1.3e-14	9.6e-14

5つの解法すべてで、誤差が計算機の丸め誤差のレベル（10のマイナス12乗以下）になりました。座標系の扱いは正しいと言えます。

この確認は必ずやることをおすすめします。 A や B の向き（逆行列にするかどうか）を間違えても calibrateHandEye はエラーを出さず、それらしい値を返します。実機のデータで初めて動かすと、結果がずれていても原因が「カメラの精度」なのか「座標系の取り違え」なのか切り分けられません。

結果2：計測ノイズを入れると誤差は約1mm

ボード姿勢の計測に、各軸で回転0.1°・並進0.5mmのノイズを加えた場合です。

解法	回転誤差 [°]	並進誤差 [mm]
Tsai	0.054	1.02
Park	0.059	0.87
Horaud	0.058	0.87
Andreff	0.059	1.26
Daniilidis	0.060	0.85

回転誤差はどの解法も0.05〜0.06°とほぼ同じですが、並進誤差は0.85〜1.26mmとばらつきがあります。

ただし、これは乱数のシード1つ分の結果です。この表だけで「Daniilidis が一番良い」とは言えません。また、ノイズの大きさ（0.1°・0.5mm）は仮に置いた値です。実際の RealSense でどれくらいのノイズになるかは第5章で測り、第6章でその値を使って予測し直します。

ハマりどころ：OpenCV 5.0 には calibrateHandEye がない

pip install opencv-contrib-python で最新版（5.0.0）を入れたところ、cv2.calibrateHandEye が見つからないというエラーになりました。cv2.CALIB_HAND_EYE_TSAI などの定数は残っているのに、関数本体がありません。

4.x 系を指定して入れ直すと動きました（執筆時点では 4.14.0）。

pip install "opencv-contrib-python<5"

まとめと次章予告

• 正解の X を自分で決め、そこからロボット姿勢 A とボード姿勢 B の組を作れば、ロボットなしでハンドアイキャリブレーションを検証できる
• ノイズなしなら、5つの解法すべてで誤差が実質0になることを確認した
• 実機の前にこの確認をしておくと、座標系の取り違えを早く見つけられる
• 計測ノイズ（回転0.1°・並進0.5mm）を加えると、推定誤差は回転約0.06°、並進約1mm になった

次の第3章では、シードを変えて何百回も試行し、ノイズ・姿勢数・回転範囲によって5つの解法の精度がどう変わるかを比較します。

参考

• OpenCV ドキュメント：calibrateHandEye
• R. Y. Tsai and R. K. Lenz, “A new technique for fully autonomous and efficient 3D robotics hand/eye calibration,” IEEE Trans. Robotics and Automation, 1989.
• F. C. Park and B. J. Martin, “Robot sensor calibration: solving AX = XB on the Euclidean group,” IEEE Trans. Robotics and Automation, 1994.
• R. Horaud and F. Dornaika, “Hand-eye calibration,” The International Journal of Robotics Research, 1995.
• N. Andreff, R. Horaud, and B. Espiau, “On-line hand-eye calibration,” Proc. 2nd International Conference on 3-D Digital Imaging and Modeling, 1999.
• K. Daniilidis, “Hand-eye calibration using dual quaternions,” The International Journal of Robotics Research, 1999.