UnityとGeminiでロボットを動かす！IK (Inverse Kinematics) 自作への挑戦 ~ その５

オブジェクト検出と座標変換の実装

前回までで、物理的に安定したロボットアームと、その制御インターフェース（IGripperなど）を構築しました。しかし、これまではスクリプトに記述された定型シーケンスを実行する「ティーチング」の段階に過ぎませんでした。

本記事では、GoogleのGemini Robotics-ERを用いてワークエリア内の物体を検知し、自律的にピック＆プレイス（運搬）を行う実験を紹介します。

NOTE: オブジェクト検出（物体検知）だけであれば、従来のCNNや通常のGemini（例：gemini-2.5-flash）でも十分に可能です。本シリーズの今後の記事では、Robotics-ER特有の推論能力を深掘りしていく予定です。

カメラ画像上の座標をテーブル上の座標へ変換

Gemini Robotics-ER APIから返される座標は、あくまで「画像上のピクセル座標」です。ロボットアームを動かすには、これを「テーブル上の3次元実座標」に変換しなければなりません。

Unityであれば Raycast を使うのが最も簡単ですが、本プロジェクトでは産業用ロボットシミュレータとしての汎用性を重視し、あえて物理的な設置パラメータに基づいた計算式で座標を算出します。

この手法は、私が以前、建設現場向けのAI・ARアプリを開発した際にも採用した実戦的なロジックです。製造現場や屋外計測など、センサーデバイスを斜め俯瞰で設置するケースで非常に重宝します。

ここでは、カメラを「逆さま（ローカルのForward軸で180度回転）」に設置し、ワークエリアを斜めに見下ろしている構成を想定します。

1. システムの全体像と物理配置

このシステムは、カメラが「逆さま」に設置されていることを数学的に吸収し、画像上のピクセルをワークエリア平面（$y=0$）の実座標へ復元します。

物理的な前提条件

• カメラの設置: ワークエリアの原点から $Z$ 方向に $L_{cam}$（約1m）離れた位置にあり、原点方向を見下ろしている。
• 180度回転の影響: 通常のカメラは画像の上側が「遠く」を映しますが、逆さまに設置されているため、　画像の上部（$v \approx 0$）が「カメラの足元（近く）」　を、　画像の下部（$v \approx Height$）が「遠方（原点付近）」　を映します。上の説明図を見れば、180度回転した結果、カメラのy軸がワークエリア前方を向いているのがお分かりになるかと思います。
• カメラの解像度：横方向に$W_{res}ピクセル$、縦方向に$H_{res}$ピクセルとします。
• カメラの画角：横方向に$FOV_{h}$、縦方向に$FOV_{v}$とします。
• OpenCV座標系: 左上が $(0,0)$、右方向が $u$ 正、下方向が $v$ 正です（実際には、OpenCVでは$(v,u)$の順番ですが、ここでは、$(u,v)$の順とします）。

2. 幾何学的な計算プロセス

ステップ1：画像中心基準への変換

OpenCVの $(u, v)$ を、中心を $(0,0)$ とし、上・右方向を正とする数学的座標 $(u', v')$ に変換します。

• $u' = u - \frac{W_{res}}{2}$
• $v' = \frac{H_{res}}{2} - v$
  これにより、画像の上部ほど $v'$ は大きなプラスになり、「足元（角度が深い）」として計算されます。

ステップ2：光学的相対角（α, β）の特定

ピンホールカメラモデルに基づき、各ピクセルがレンズの中心から何度傾いているかを算出します。

垂直相対角 $\alpha = \arctan\left( \frac{v'}{H_{res}/2} \cdot \tan\left(\frac{FOV_v}{2}\right) \right)$
水平相対角 $\beta = \arctan\left( \frac{u'}{W_{res}/2} \cdot \tan\left(\frac{FOV_h}{2}\right) \right)$

ステップ3：接地角（γ）の算出

カメラ自体の傾き（俯角 $\theta$）に $\alpha$ を加算します。

• $\gamma = \theta + \alpha$
  カメラが逆さまなので、画像の上側（$v'$がプラス）ほど接地角 $\gamma$ が大きくなり、より垂直（カメラに近い位置）を指すことになります。

ステップ4：ワークエリア上の実距離算出

三角関数を用いて、カメラ直下からの水平距離 $z_{dist}$ 、ワークエリアにおけるローカル座標 $(x, z)$ を求めます。

• $z_{dist} = \frac{H}{\tan(\gamma)}$
• $z = L_{cam} - z_{dist}$
• $x = \frac{H}{\sin(\gamma)} \cdot \tan(\beta)$

ロジック実装例

Unity上でこのロジックを実装したスクリプトが以下です。

using UnityEngine;

/// <summary>
/// 180度回転したカメラから取得したOpenCV座標をワークエリア座標に変換する
/// </summary>
public class WorkAreaCoordinateConverter : MonoBehaviour
{
    [Header("Configuration")]
    [SerializeField] private GameObject workArea;     // ワークエリア基準オブジェクト
    [SerializeField] private Camera captureCamera;   // 使用するカメラ

    private int imageWidth;
    private int imageHeight;
    private float fovHorizontal;
    private float fovVertical;

    private float H;      // カメラの高さ (WorkAreaから見たY)
    private float L_cam;  // カメラの奥行き (WorkAreaから見たZ)
    private float theta;  // 俯角 (水平から下向きに測った角度)

    void Start()
    {
        if (captureCamera == null || workArea == null) return;

        // 1. 画像サイズと画角の取得
        imageWidth = captureCamera.pixelWidth;
        imageHeight = captureCamera.pixelHeight;
        fovVertical = captureCamera.fieldOfView;
        fovHorizontal = 2f * Mathf.Atan(Mathf.Tan(fovVertical * Mathf.Deg2Rad / 2f) * captureCamera.aspect) * Mathf.Rad2Deg;

        // 2. ワークエリア空間におけるカメラの設置位置(H, L_cam)を取得
        Vector3 localPos = workArea.transform.InverseTransformPoint(captureCamera.transform.position);
        H = localPos.y;     // ワークエリア面からの高さ
        L_cam = localPos.z; // ワークエリア中心からの奥行き距離

        // 3. 俯角 theta の算出
        // カメラのForward(前)とWorkAreaのDown(下)の角度差から水平俯角を求める
        float angleToDown = Vector3.Angle(captureCamera.transform.forward, -workArea.transform.up);
        theta = 90f - angleToDown;

        Debug.Log($"[Init Success] H:{H:F2}, L_cam:{L_cam:F2}, Theta:{theta:F1}");
    }

    /// <summary>
    /// OpenCVピクセル座標(u, v)をワークエリアのローカル座標(X, 0, Z)へ投影
    /// </summary>
    public Vector3 ProjectToWorkAreaLocal(float u, float v)
    {
        // 1. OpenCV座標を中心基準(上・右がプラス)へ変換
        float u_prime = u - imageWidth * 0.5f;
        float v_prime = imageHeight * 0.5f - v; // OpenCVは上が0なので反転

        // 2. 相対角度(ラジアン)の算出
        float alpha = Mathf.Atan((v_prime / (imageHeight * 0.5f)) * Mathf.Tan(fovVertical * 0.5f * Mathf.Deg2Rad));
        float beta = Mathf.Atan((u_prime / (imageWidth * 0.5f)) * Mathf.Tan(fovHorizontal * 0.5f * Mathf.Deg2Rad));

        // 3. 接地角 gamma
        // 画像上部(v_prime > 0)ほど、alphaが足されて角度が深く(足元に)なる
        float gamma = (theta * Mathf.Deg2Rad) + alpha;

        // ガード: 水平より上(接地しない)の場合は無効値を返す
        if (gamma <= 0.001f) return new Vector3(0, -999, 0);

        // 4. ローカル z 座標の算出
        // カメラ設置位置 L_cam から原点方向(マイナス)へどれだけ戻るか
        float z_dist = H / Mathf.Tan(gamma);
        float localZ = L_cam - z_dist;

        // 5. ローカル x 座標の算出
        // 180度回転の影響で反転が必要な場合はここで調整可能(現状はそのまま)
        float horizontalDistToPoint = H / Mathf.Sin(gamma);
        float localX = horizontalDistToPoint * Mathf.Tan(beta);

        return new Vector3(localX, 0, localZ);
    }
}

実験結果：自律的なピック＆プレイス

構築した座標変換エンジンをロボットアームの制御システムに組み込み、Gemini Robotics-ERによる物体検知の結果（ラベル名が "cube" のもの）をターゲットとして運搬させる実験を行いました。

パラレルグリッパーの採用と精度の高い座標計算により、取りこぼしなく100%の成功率でタスクを完了できました。

次のステップ

今回はあらかじめ定義した「特定の物体」を運ぶだけでしたが、次はGemini Robotics-ERの言語理解能力を活かし、「赤い物体を右へ、青い物体を左へ」といった曖昧な指示をロボットに実行させる「VLA（Vision-Language-Action）」の領域へと踏み込んでいきます。

本記事で紹介した制作物

以下のGitHubプロジェクトで公開します。