マットが動く、iPhoneカメラがブレる、mechArmも触る。ズレる前提でUnity座標系を作り直す

統合テーブル512,087点をPiに転送し、lookup距離はPICK側0.1mm、PLACE側1.2mmで揃った。数字としては完璧だった。しかし実機アームは赤ブロックから8cm、箱からも大きくズレて止まった。lookup結果が正しいことと、実機が正確に動くことは直結しない。そこから半日かけて、座標変換の前提そのものを疑い、ホモグラフィを捨て、2点実測ベースのアフィン変換に設計を書き換えた。

💡 Key Takeaway

lookup距離0.1mmが出ても、実機は8cmズレる。数値の正しさと動作の正しさが別物なのは、座標変換の前提（マットとロボットの相対位置が固定）が現実と合っていないからだった。「ズレる前提で設計する」というのは、この前提そのものを動的に解き直す設計に書き換えることだった。

Piが戻ってきた

LOG158で作った統合テーブル、512,087点を Pi にコピーするところから再開した。

scp ~/r2r2r-scripts/fk_scan_motion_based.csv ubuntu@10.42.0.1:/home/ubuntu/

行数 512,088 を確認して、r2r2r_loop.py を走らせる。ログは気持ちよく進んだ。

[Mac] ✓ 赤ブロック検出: pixel=(489, 819) → robot=(121.8, 124.9)
[Mac] ✓ 白箱検出:   pixel=(987, 761) → robot=(103.2, -29.3)
[Mac] ✓ PICK lookup成功:  dist=0.1mm
[Mac] ✓ PLACE lookup成功: dist=15.5mm
[Mac] コマンド送信中（11ステップ）
[HOME] [GRIPPER_OPEN] [TO_APPROACH_PICK] [TO_PICK] [GRIPPER_CLOSE]
[LIFTUP_TO_APPROACH] [TO_APPROACH_PLACE] [TO_PLACE]
[GRIPPER_OPEN] [LIFTUP_FROM_PLACE] [HOME]

11ステップ全部が通った。lookup 距離 0.1mm はサブミリ精度。PLACE も 15.5mm で閾値 100mm の内側。PLACE 側の動的追従もPhase Cに入って初めて通った。数字の上では申し分がなかった。

ただ、カメラ画像を見ると事情が違った。アームは赤ブロックの手前で止まっていて、グリッパは赤いブロックより 8cm ほどロボット寄りの空中を掴んでいた。箱のほうは、アームが完全に通り過ぎて、箱の反対側の空中に降りていた。

数字は通った。実機は外した。このギャップが、この日の作業の全体をドライブした。

カメラが動く、マットも動く

最初の仮説はシンプルだった。lookup テーブルで引いた EE 位置（Brep中点基準）と、実機のグリッパ先端の間に系統的なオフセットがあるんじゃないか——もしそうなら、2、3点実測して定数で引けば直る。

ただ、少し頭を冷やすと、別の問題があった。現状の座標変換は MAT_CORNERS_PX（マット4隅のpixel座標）と MAT_CORNERS_ROBOT（同じ4隅のロボット座標）をハードコードで持っている。ホモグラフィはこの2組の対応から計算される。

つまりマット位置とカメラ位置が固定であることが前提になっていた。でも実際は、ロボットの周りで何か作業するとマットは動くし、三脚は完全に固定できないし、iPhoneをつける位置も微妙に変わる。起動するたびに、ハードコードの値は少しずつ嘘になっていく。

カメラを付け替えたりするとズレが大きい（画像見るとわかるけど、ちょっとカメラ動かしただけですごいズレている）。カメラは3脚を使っているけど、それでもブレは常にあるので、カメラはブレる前提で全体の設計しないとうまくいかないはず。

この指摘で、問題の輪郭が変わった。「オフセット補正」の話ではなく、「座標系をどう作り直すか」の話だった。

マット4隅の動的検出（半歩の解決）

まず手を付けたのは「マット4隅を毎フレーム検出してホモグラフィを再計算する」方向だった。マット縁の黄色テープをHSVで検出し、4象限に分割して最大輪郭の重心を取る。この結果でホモグラフィを動的に作り直す設計にした。

実装して動かすと、4隅のうち LL（左下）だけが明らかに変な位置を指していた。他の3隅が期待値に対して +60〜+90px 程度ずれていたのに、LL は −79px、つまり反対方向にズレていた。

デバッグ用にマスク画像と輪郭可視化を分離して吐かせる debug スクリプトを作った。マスク画像を開くと、LL象限には縦マーカー（マット縁の黄色テープ）とは別に、もうひとつ横向きの黄色いテープが映っていた。マット縁にもう1枚、用途不明のテープが貼られていた。LL では横テープのほうが面積が大きく、「最大輪郭」ロジックが横テープを拾っていた。

横テープを物理的に剥がすと、LL は縦マーカーだけを検出するようになった。4隅のマスクがきれいに揃って、Detection画面の緑枠もマット縁に乗った。

ここで満足して進めば良さそうに見えた。が、エイイチの指摘が突き刺さった。

ロボットを触ったり、何か作業するとズレるから、常に完全固定なわけじゃないよ。だからズレる前提で設計しないとというのはそういう意味だよ。

マット4隅の pixel座標を動的検出したところで、対応する MAT_CORNERS_ROBOT はハードコードのまま。マットが動けば、この対応関係そのものが崩れる。4隅検出だけの動的化は、カメラブレには効くが、マットとロボットの相対位置が変わる問題には効かない。半歩の解決だった。

ロボットを基準にする発想

発想を切り替えた。マットとホモグラフィをまるごと捨てて、ロボット自身の上に貼った青シールを基準点にする。マットがどこに置かれていようと、ロボット基準で計測すれば自己完結する。

Joint 1 の円筒トップに青シール1枚、グリッパ本体に青シール3枚貼って、検出ロジックを組み直した。

def identify_markers(markers):
    """
    近接クラスタ（グリッパ群）と、そこから孤立している1点（基準点）に分ける
    """

動いた。Detection画面の緑枠は消えて（マット4隅不要になった）、Joint 1 基部の青シールの真上に BASE ラベルが乗った。ただ、ログに出た赤ブロックのロボット座標は非現実的な値だった。

[Mac] ✓ 赤ブロック検出: pixel=(719, 844)
[Mac]   基準点からの相対: pixel(-310, 542)
[Mac]   → robot座標: (351.2, 147.9) mm

robot_x=351mm は mechArm 270 のリーチを超えている。実装に計算誤りはない——rel_py × PIXEL_TO_MM_Y = 542 × 0.648 = 351.2。数式は通っていた。

原因はもっと手前にあった。「基準点pixel＝ロボット原点（0,0）」という暗黙の仮定を Claude 側が置いていた。でも Joint 1 基部の青シールは、ロボット原点ではない。カメラの俯瞰角度によっては、画像上ではロボット本体のかなり前方オフセットされた位置に映る。

基準点を1点置く方式は、その基準点が本当にロボット座標のどこに対応しているかを別途持たなければ意味がない。設計の土台がひとつ足りなかった。

実測値を教えるから、それで補正したら？

ここで提案が入った。

まず今のPICK位置にあるブロックとPLACEにある箱の中心点を、実測でX,Yを測って教える。まずはそれを補正したら？

この一手で景色が変わった。実測値を2点持てば、画像上の2点（赤ブロックpixel・白箱pixel）と、対応するロボット座標（実測）の組が揃う。基準点のロボット座標オフセットも、スケール係数も、全部この2組の対応から一気に決まる。基準点という概念そのものが不要になる。

マット4隅のホモグラフィと何が違うのか。マット4隅は「マットが絶対動かない」前提で値を固定していた。2点実測は「起動時の配置を真と定義して、そこから変換を作る」方式で、マットやカメラの位置はそのたびに新しくキャリブレーションされる。マットが動こうがカメラが動こうが、起動時に1回実測すれば整合する。

実装はシンプルだった。

def compute_affine_2points(red_px, white_px):
    dpx_x = white_px[0] - red_px[0]
    dpx_y = white_px[1] - red_px[1]
    du_x  = CALIB_WHITE_UNITY_X - CALIB_RED_UNITY_X
    du_z  = CALIB_WHITE_UNITY_Z - CALIB_RED_UNITY_Z
    scale_x = du_x / dpx_x
    scale_z = du_z / dpx_y
    return scale_x, scale_z, red_px, (CALIB_RED_UNITY_X, CALIB_RED_UNITY_Z)

2点間の差分から画像→Unity のスケール係数が出る。画像のX軸とUnityのX軸、画像のY軸とUnityのZ軸がそれぞれ対応する前提で、回転なし、直線近似。カメラが斜め俯瞰なので遠近歪みは完全には吸収できないが、マット中央付近の動作には十分実用になる見込み。完全なアフィンや射影変換が必要になったら、3点・4点に拡張する道が残っている。

軸名の混乱、Unityで統一する

この日、会話が何度も噛み合わなくなった瞬間があった。原因は「X」「Y」という文字が、文脈によって全く違うものを指していたせいだった。

「赤ブロックのXは＋？−？」という一問をしたときに、どの文脈の X を言っているのかで答えが3通りに分かれる。実際に会話で、Claude 側が mechArm のRobot座標系の慣習に引きずられて「画像右＝Robot Y 負」と答え、エイイチが実測の感覚で「左右はX」と答え、両者が微妙にすれ違った。

この一手で決着した。

XのプラスマイナスはUnityに合わせればいいんじゃないの？

そうだ。lookup テーブル (fk_scan_motion_based.csv) は Unity の FKスキャナーで生成されている。つまり、テーブル内の ee_x, ee_y, ee_z はすべて Unity 座標系。変換の出口を Unity 座標に合わせれば、そのまま lookup に入る。Unity を「正」と決めれば、全ての表記が一意になる。

確定したルールはシンプルだ。

• X軸 = 左右方向。画像左が X 正、画像右が X 負。

• Y軸 = 高さ（今回の2D変換では使わない）。

• Z軸 = 前方方向。画像奥が Z 正。

実測値をこの座標系に変換して、キャリブレーション定数として固めた。

CALIB_RED_UNITY_X   = -0.145   # 画像右、前方
CALIB_RED_UNITY_Z   =  0.203
CALIB_WHITE_UNITY_X =  0.195   # 画像左、前方
CALIB_WHITE_UNITY_Z =  0.155

実装は通った。検証は次へ

r2r2r_loop.py から MAT_CORNERS_PX / MAT_CORNERS_ROBOT / detect_mat_corners / compute_homography_dynamic / update_homography / pixel_to_robot / PIXEL_TO_MM_X/Y / calibrate_base_point を削除した。代わりに compute_affine_2points と pixel_to_unity を追加した。青シール検出関数（detect_blue_markers / identify_markers）は削除せず、v3（青シール実測EE位置をフィードバックする設計）のために保留した。

実機テストの直前で、Pi のある場所から移動する時間になった。検証はここからが本番で、キャリブレーション時の配置で起動した後、赤ブロックと白箱を任意の位置に置き直して動作が成立するかを見る。マットを動かしてみて、それでも動作が崩れないかを見る。カメラを少し動かしてみて、それでも動作が崩れないかを見る。ここが通れば、「ズレる前提で動く」という設計思想が実装として成立したことになる。

lookup距離がサブミリで出ていたのに実機が8cmズレたのは、座標変換の土台そのものが現実と合っていなかったからだった。その土台を、静的な宣言から、起動時の実測ベースに置き換えた。次のサイクルで、マットの位置を動かし、カメラをずらし、それでも lookup とアームが揃うかを見る。

※ プロジェクト名「R2R2R」は、UC Berkeley・Toyota Research Instituteの研究「Real2Render2Real」（Yu, Fu et al., 2025）の概念に触発されています。現実空間をデジタルツイン化し、仮想空間での試行から得た知見を実世界のモーター駆動へ還元する——その設計と実装過程を毎日記録しています。技術アプローチおよび目的は独自のものです。