地球上のロボットであれば、GPSや固定された外部カメラで自分や対象物の位置を知ることができます。しかし、宇宙空間ではGPSの電波は届かず、地上局からのコマンドには数秒から数十分の遅延があります。軌道上で故障衛星にアプローチするサービス衛星や、デブリを捕獲するロボットアームにとって、カメラ画像だけが対象物体の位置・姿勢をリアルタイムに知る唯一の手段なのです。
では、カメラが撮影した2次元画像から、3次元空間内の物体の運動をどのように追跡し、ロボットの制御に結びつけるのでしょうか。ここで鍵となるのが、3次元空間の点が画像上のどこに映るかを記述するカメラモデルと、3次元空間での速度変化が画像上の特徴点をどう動かすかを記述する画像ヤコビアン（Interaction Matrix）です。
カメラモデルと画像ヤコビアンを理解すると、以下のような応用が開けます。
- ビジュアルサーボ: カメラ画像のフィードバックでロボットアームをリアルタイム制御する技術の数学的基盤
- 宇宙ランデブー・ドッキング: 接近中の衛星間の相対位置・姿勢をカメラ画像から推定し、自動で軌道修正
- SfM（Structure from Motion）・SLAM: 複数画像から3次元構造を復元する際の射影幾何の基礎
- AR/VR: 現実空間と仮想空間の重ね合わせに必要なカメラキャリブレーション
本記事の内容
- ピンホールカメラモデルの直感的理解と数学的定式化
- 内部パラメータ行列 $\bm{K}$ と外部パラメータの導出
- レンズ歪みモデル（放射歪み・接線歪み）
- 画像ヤコビアン（Interaction Matrix）の完全な導出
- 画像ヤコビアンの構造解析と奥行き依存性
- Pythonによるカメラモデルと画像ヤコビアンの実装・可視化
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
- ヤコビ行列とヤコビアンの意味を理解する — ヤコビ行列の定義と幾何学的意味
- 同次座標と射影変換の基礎 — 射影幾何で用いる同次座標の考え方
ピンホールカメラモデル — カメラの最も基本的な数学モデル
暗い箱と小さな穴
カメラの原理を最もシンプルに理解するには、「暗箱」を想像してください。暗い箱の一方の壁に小さな穴（ピンホール）を開け、反対側の壁にスクリーンを置きます。外の風景からの光は、ピンホールを通過してスクリーン上に上下左右が反転した像を結びます。これがピンホールカメラ（pinhole camera）の原理であり、カメラの数学モデルの出発点です。
実際のカメラにはレンズがありますが、数学的にはまず「レンズのない理想的なピンホール」を考え、後からレンズの歪み効果を補正項として追加します。この戦略により、複雑な光学系の問題が「幾何学的な射影」という見通しのよい問題に帰着されます。
ピンホールカメラモデルの基本的な構成要素は次の3つです。
- 光学中心（カメラ中心）$\bm{O}$: ピンホールの位置。全ての光線はここを通過する
- 画像平面: 光学中心から距離 $f$（焦点距離）だけ離れた平面。光が投影されるスクリーン
- 光軸: 光学中心から画像平面に下ろした垂線。カメラが「まっすぐ前」を向く方向
ここで重要な工夫があります。物理的なピンホールカメラでは画像は光学中心の後ろ側に上下反転して映りますが、数学的には画像平面を光学中心の前方（距離 $f$ の位置）に置く仮想モデルを使います。こうすると像の反転がなくなり、式が簡潔になります。
では、このモデルを数式で定式化していきましょう。
座標系の設定
カメラモデルの定式化には、以下の座標系を使い分けます。
カメラ座標系 $\{C\}$: カメラの光学中心を原点とし、光軸方向を $Z_C$ 軸、画像の横方向を $X_C$ 軸、縦方向を $Y_C$ 軸に取ります。3次元空間内の点の位置を $\bm{P}_C = (X, Y, Z)^\top$ で表します。
画像座標系: 画像平面上の2次元座標です。2種類を区別します。
- 正規化画像座標 $(x, y)$: 焦点距離で正規化された無次元座標
- ピクセル座標 $(u, v)$: 実際の画像のピクセル位置
ワールド座標系 $\{W\}$: カメラの外部にある基準座標系。3次元空間内の点の位置を $\bm{P}_W$ で表します。
この座標系の階層構造を意識しておくと、「どの変換が何を担当しているか」が明確になります。3次元空間の点がピクセル座標に変換されるまでの流れは、ワールド座標 $\to$ カメラ座標 $\to$ 正規化画像座標 $\to$ ピクセル座標 という3段階です。
まずは最も核心的な変換、すなわちカメラ座標から正規化画像座標への変換（透視投影）から始めましょう。
透視投影の数学 — 3Dから2Dへの射影
相似三角形による導出
3次元空間内の点 $\bm{P}_C = (X, Y, Z)^\top$ がカメラ座標系で与えられたとき、この点がピンホールカメラの画像平面上のどこに映るかを求めます。
画像平面は光学中心から距離 $f$ の位置にあります。光学中心を通って点 $\bm{P}_C$ に向かう直線（光線）と画像平面の交点を求めればよいのです。これは相似三角形の関係から直ちに得られます。
$X$ 方向について考えましょう。光学中心から $Z$ 軸方向に距離 $f$ の画像平面上の $x$ 座標と、距離 $Z$ の位置にある点の $X$ 座標の関係は、相似三角形から
$$ \frac{x}{f} = \frac{X}{Z} $$
と書けます。同様に $Y$ 方向についても
$$ \frac{y}{f} = \frac{Y}{Z} $$
です。したがって、正規化画像座標 $(x, y)$ は
$$ x = f\frac{X}{Z}, \quad y = f\frac{Y}{Z} $$
となります。焦点距離 $f = 1$ と正規化する（あるいは $f$ を内部パラメータに吸収する）場合、さらに簡潔に
$$ x = \frac{X}{Z}, \quad y = \frac{Y}{Z} $$
と書けます。これが透視投影（perspective projection）の基本式です。
透視投影の本質 — 非線形な射影
透視投影の式 $x = X/Z$, $y = Y/Z$ を見ると、$Z$（奥行き）による除算が含まれています。これは本質的に非線形な変換です。遠くにある物体は小さく、近くの物体は大きく映る — 日常的な遠近感はこの $1/Z$ の因子から生じています。
この非線形性は、後で画像ヤコビアンを導出する際に重要な役割を果たします。画像上の特徴点の動きが物体の奥行き $Z$ に依存するということは、カメラ画像だけからは奥行き情報を完全に復元できないことを意味しています。
同次座標による表現
透視投影の $Z$ による除算は、同次座標（homogeneous coordinates）を導入すると、行列の積として統一的に書けます。3次元の点 $\bm{P}_C = (X, Y, Z)^\top$ の同次座標表現は $\tilde{\bm{P}}_C = (X, Y, Z, 1)^\top$ です。
このとき、透視投影を次のように表します。
$$ \begin{pmatrix} u’ \\ v’ \\ w’ \end{pmatrix} = \begin{pmatrix} f & 0 & 0 & 0 \\ 0 & f & 0 & 0 \\ 0 & 0 & 1 & 0 \end{pmatrix} \begin{pmatrix} X \\ Y \\ Z \\ 1 \end{pmatrix} $$
ここで、$(u’, v’, w’)^\top$ は同次座標であり、実際のピクセル座標は $w’$ で割って
$$ x = \frac{u’}{w’} = \frac{fX}{Z}, \quad y = \frac{v’}{w’} = \frac{fY}{Z} $$
で得られます。「除算」を同次座標の正規化（$w’$ で割る操作）に押し込めることで、射影変換全体を行列の積として扱えるようになります。これが同次座標の威力です。
透視投影の幾何学を理解したところで、次はカメラ固有の内部パラメータを導入し、正規化画像座標から実際のピクセル座標への変換を定式化しましょう。
内部パラメータ行列 $\bm{K}$
正規化座標からピクセル座標への変換
正規化画像座標 $(x, y)$ は、焦点距離を1とした無次元の座標です。しかし、実際のカメラでは画像はピクセルの格子で構成されており、いくつかのパラメータが追加されます。
まず、焦点距離とピクセルサイズの関係を考えます。焦点距離 $f$ がメートル単位で与えられ、ピクセルの物理的サイズが $X$ 方向に $s_x$、$Y$ 方向に $s_y$ メートルだとすると、焦点距離をピクセル単位に変換した値は
$$ f_x = \frac{f}{s_x}, \quad f_y = \frac{f}{s_y} $$
となります。一般に $s_x \neq s_y$（ピクセルが正方形でない場合）なので、$f_x$ と $f_y$ は独立したパラメータです。
次に、主点（principal point）$(c_x, c_y)$ を導入します。光軸が画像平面と交わる点をピクセル座標で表した値です。理想的には画像の中心ですが、製造誤差のため一般にはわずかにずれています。
以上をまとめると、カメラ座標 $(X, Y, Z)$ からピクセル座標 $(u, v)$ への変換は
$$ u = f_x \frac{X}{Z} + c_x, \quad v = f_y \frac{Y}{Z} + c_y $$
と書けます。
内部パラメータ行列の定義
上の変換を行列形式で書きましょう。同次座標を使うと、
$$ \begin{pmatrix} u \cdot Z \\ v \cdot Z \\ Z \end{pmatrix} = \begin{pmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} X \\ Y \\ Z \end{pmatrix} $$
ここで現れる $3 \times 3$ の行列が内部パラメータ行列（intrinsic parameter matrix）$\bm{K}$ です。
$$ \bm{K} = \begin{pmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{pmatrix} $$
$\bm{K}$ はカメラ固有のパラメータのみを含み、カメラがどこに置かれ、どの方向を向いているかには依存しません。カメラを買ったときに決まる「仕様書」のようなものです。
より一般的には、ピクセルが傾いている（skew がある）場合を考慮して
$$ \bm{K} = \begin{pmatrix} f_x & s & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{pmatrix} $$
と書くこともあります。ここで $s$ はスキューパラメータですが、現代のカメラではほぼ $s = 0$ であり、通常は無視します。
$\bm{K}$ の中身を理解したところで、次はカメラの「位置と姿勢」、すなわち外部パラメータを定式化します。
外部パラメータ — カメラの位置と姿勢
ワールド座標からカメラ座標への変換
ここまではカメラ座標系 $\{C\}$ で記述された3次元点 $(X, Y, Z)$ を画像座標に変換しました。しかし、実際の応用ではワールド座標系 $\{W\}$ で記述された点 $\bm{P}_W$ をカメラ画像に投影する必要があります。
ワールド座標系からカメラ座標系への変換は、回転 $\bm{R} \in SO(3)$（$3 \times 3$ の回転行列）と並進 $\bm{t} \in \mathbb{R}^3$ で記述されます。
$$ \bm{P}_C = \bm{R}\bm{P}_W + \bm{t} $$
ここで $\bm{R}$ はカメラの姿勢（向き）を、$\bm{t}$ はカメラの位置を表します。$\bm{R}$ と $\bm{t}$ をまとめて外部パラメータ（extrinsic parameters）と呼びます。
同次座標を使うと、この変換は $4 \times 4$ 行列で書けます。
$$ \begin{pmatrix} \bm{P}_C \\ 1 \end{pmatrix} = \begin{pmatrix} \bm{R} & \bm{t} \\ \bm{0}^\top & 1 \end{pmatrix} \begin{pmatrix} \bm{P}_W \\ 1 \end{pmatrix} $$
この $4 \times 4$ の行列はカメラの外部パラメータ行列で、SE(3)（Special Euclidean group）の要素です。
完全な射影モデル
内部パラメータ $\bm{K}$ と外部パラメータ $[\bm{R} \mid \bm{t}]$ を組み合わせると、ワールド座標からピクセル座標への完全な変換が得られます。
$$ \lambda \begin{pmatrix} u \\ v \\ 1 \end{pmatrix} = \bm{K} [\bm{R} \mid \bm{t}] \begin{pmatrix} \bm{P}_W \\ 1 \end{pmatrix} $$
ここで $\lambda$ はスケール因子（実際には $Z$ 座標に対応）です。$\bm{K}[\bm{R} \mid \bm{t}]$ は $3 \times 4$ の行列で、射影行列（projection matrix）$\bm{P}$ と呼ばれます。
$$ \bm{P} = \bm{K}[\bm{R} \mid \bm{t}] $$
この射影行列 $\bm{P}$ が、3次元世界の点を2次元画像に写す変換の全てを記述しています。内部パラメータ5個（$f_x, f_y, c_x, c_y, s$）と外部パラメータ6個（回転3自由度 + 並進3自由度）の計11個のパラメータで完全に決まります。
宇宙ロボティクスの文脈では、カメラはロボットアームの手先やサービス衛星の本体に取り付けられており、外部パラメータはロボットや衛星の運動に伴って時々刻々と変化します。一方、内部パラメータはカメラのハードウェアで決まるため、事前のキャリブレーションで求めておくのが一般的です。
ここまでは理想的なピンホールカメラモデルを扱ってきましたが、実際のカメラにはレンズがあり、その影響で画像に歪みが生じます。次に、この歪みのモデル化を見ていきましょう。
レンズ歪みモデル
なぜレンズ歪みを考える必要があるのか
ピンホールカメラモデルは、全ての光が1点（ピンホール）を通過すると仮定していますが、実際のカメラではレンズを通過する際に光路が曲がります。特に広角レンズではこの効果が顕著で、直線が画像上で曲線に映る「歪み」が発生します。
ビジュアルサーボでは画像上の特徴点位置を高精度に測定する必要があるため、レンズ歪みの補正は不可欠です。歪みを無視すると、制御に使う画像座標に系統的な誤差が含まれ、ロボットの位置決め精度が大幅に劣化します。
放射歪み（Radial Distortion）
最も支配的な歪みは放射歪みです。レンズの光軸からの距離（放射方向）に依存して像が伸び縮みします。
正規化画像座標 $(x, y)$ における像点の光軸からの距離を $r = \sqrt{x^2 + y^2}$ とすると、放射歪みによる補正は次のように書けます。
$$ x_{\text{radial}} = x(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) $$
$$ y_{\text{radial}} = y(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) $$
ここで $k_1, k_2, k_3$ は放射歪み係数です。
- $k_1 > 0$ のとき樽型歪み（barrel distortion）: 画像の端が外側に膨らむ
- $k_1 < 0$ のとき糸巻き型歪み（pincushion distortion）: 画像の端が内側に縮む
多くの実用場面では $k_1$ と $k_2$ の2項で十分な精度が得られます。$k_3$ は魚眼レンズなど極端な広角レンズの場合に必要になります。
接線歪み（Tangential Distortion）
接線歪みは、レンズと画像センサーが完全に平行でない場合に生じます。製造時のわずかな傾きが原因です。
$$ x_{\text{tangential}} = 2p_1 xy + p_2(r^2 + 2x^2) $$
$$ y_{\text{tangential}} = p_1(r^2 + 2y^2) + 2p_2 xy $$
ここで $p_1, p_2$ は接線歪み係数です。放射歪みに比べると影響は小さいことが多いですが、高精度な応用では考慮する必要があります。
歪みモデルの統合
放射歪みと接線歪みを合わせた完全な歪みモデルは以下の通りです。
$$ \begin{pmatrix} x_d \\ y_d \end{pmatrix} = (1 + k_1 r^2 + k_2 r^4 + k_3 r^6) \begin{pmatrix} x \\ y \end{pmatrix} + \begin{pmatrix} 2p_1 xy + p_2(r^2 + 2x^2) \\ p_1(r^2 + 2y^2) + 2p_2 xy \end{pmatrix} $$
歪み補正された座標 $(x_d, y_d)$ を使ってピクセル座標を計算します。
$$ u = f_x \cdot x_d + c_x, \quad v = f_y \cdot y_d + c_y $$
歪みパラメータ $(k_1, k_2, k_3, p_1, p_2)$ は、チェッカーボードパターンなどの既知の幾何形状を撮影するカメラキャリブレーションによって事前に求めます。OpenCV の calibrateCamera 関数はこの作業を自動化してくれます。
レンズ歪みまで含めたカメラモデルの全体像が見えました。ここからが本記事の核心です。カメラの前で物体が動いたとき、画像上の特徴点はどのように動くのか — この関係を記述する画像ヤコビアンの導出に進みましょう。
画像ヤコビアン（Interaction Matrix）の導出
画像ヤコビアンとは何か
ロボットアームの制御では、「関節の角速度とエンドエフェクタの速度の関係」をヤコビ行列で記述します。画像ヤコビアンはこれの「画像版」です。
想像してみてください。カメラの前に1つの点があり、カメラ（またはその点）が動いています。3次元空間でのこの点の速度（並進速度と角速度）が与えられたとき、画像上でこの点がどの方向にどれだけの速さで動くかを知りたいのです。
この関係を線形に近似する行列が画像ヤコビアン $\bm{L}$（Interaction Matrix とも呼ばれます）です。
$$ \dot{\bm{s}} = \bm{L} \bm{v}_c $$
ここで、
- $\dot{\bm{s}} = (\dot{u}, \dot{v})^\top$: 画像上の特徴点の速度（ピクセル/秒）
- $\bm{v}_c = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^\top$: カメラの速度（並進速度3成分 + 角速度3成分）
- $\bm{L}$: $2 \times 6$ の画像ヤコビアン
$\bm{L}$ を求めることが、ビジュアルサーボの制御則を設計するための第一歩です。
導出の出発点 — 正規化画像座標の時間微分
まずレンズ歪みを無視し、焦点距離 $f = 1$ と正規化した理想的なピンホールモデルで導出します。画像ヤコビアンの本質的な構造はこの簡略化されたモデルで十分に理解でき、実際のピクセル座標への変換は後から内部パラメータ行列 $\bm{K}$ を掛けるだけです。
正規化画像座標は
$$ x = \frac{X}{Z}, \quad y = \frac{Y}{Z} $$
でした。この式を時間 $t$ で微分して、$\dot{x}$ と $\dot{y}$ を $\dot{X}, \dot{Y}, \dot{Z}$ で表すことが目標です。
$x = X/Z$ の両辺を時間微分すると、商の微分公式により
$$ \dot{x} = \frac{\dot{X}Z – X\dot{Z}}{Z^2} = \frac{\dot{X}}{Z} – \frac{X\dot{Z}}{Z^2} $$
$x = X/Z$ であることを使って $X/Z^2 = x/Z$ と書き換えると、
$$ \dot{x} = \frac{\dot{X}}{Z} – x\frac{\dot{Z}}{Z} $$
同様に $y = Y/Z$ の時間微分は
$$ \dot{y} = \frac{\dot{Y}}{Z} – y\frac{\dot{Z}}{Z} $$
です。ここで $\dot{X}, \dot{Y}, \dot{Z}$ はカメラ座標系での3次元点の速度成分です。これらをカメラの運動（並進速度と角速度）で表す必要があります。
カメラの運動と3D点の速度の関係
カメラ座標系で見た3次元点 $\bm{P}_C = (X, Y, Z)^\top$ の運動は、カメラの並進速度 $\bm{v} = (v_x, v_y, v_z)^\top$ と角速度 $\bm{\omega} = (\omega_x, \omega_y, \omega_z)^\top$ によって
$$ \dot{\bm{P}}_C = -\bm{v} – \bm{\omega} \times \bm{P}_C $$
と書けます。マイナス符号が付くのは、カメラが右に動くと、カメラ座標系で見た点は左に動くためです（相対運動の符号）。
外積 $\bm{\omega} \times \bm{P}_C$ を成分で書き下すと、
$$ \bm{\omega} \times \bm{P}_C = \begin{pmatrix} \omega_y Z – \omega_z Y \\ \omega_z X – \omega_x Z \\ \omega_x Y – \omega_y X \end{pmatrix} $$
したがって、各成分は
$$ \dot{X} = -v_x – \omega_y Z + \omega_z Y $$
$$ \dot{Y} = -v_y – \omega_z X + \omega_x Z $$
$$ \dot{Z} = -v_z – \omega_x Y + \omega_y X $$
です。
$\dot{x}$ の展開
先に得た $\dot{x} = \dot{X}/Z – x\dot{Z}/Z$ に、上の $\dot{X}$ と $\dot{Z}$ の式を代入していきます。
$\dot{X}/Z$ の項に $\dot{X} = -v_x – \omega_y Z + \omega_z Y$ を代入すると、
$$ \frac{\dot{X}}{Z} = \frac{-v_x – \omega_y Z + \omega_z Y}{Z} = -\frac{v_x}{Z} – \omega_y + \omega_z \frac{Y}{Z} $$
ここで $Y/Z = y$ を使って
$$ \frac{\dot{X}}{Z} = -\frac{v_x}{Z} – \omega_y + \omega_z y $$
次に $x\dot{Z}/Z$ の項に $\dot{Z} = -v_z – \omega_x Y + \omega_y X$ を代入すると、
$$ x\frac{\dot{Z}}{Z} = x\frac{-v_z – \omega_x Y + \omega_y X}{Z} = -x\frac{v_z}{Z} – x\omega_x \frac{Y}{Z} + x\omega_y \frac{X}{Z} $$
$X/Z = x$, $Y/Z = y$ を使って
$$ x\frac{\dot{Z}}{Z} = -x\frac{v_z}{Z} – xy\omega_x + x^2\omega_y $$
$\dot{x} = \dot{X}/Z – x\dot{Z}/Z$ にこれらを代入すると、
$$ \dot{x} = \left(-\frac{v_x}{Z} – \omega_y + \omega_z y\right) – \left(-x\frac{v_z}{Z} – xy\omega_x + x^2\omega_y\right) $$
各項を整理すると、
$$ \dot{x} = -\frac{v_x}{Z} + x\frac{v_z}{Z} – \omega_y + \omega_z y + xy\omega_x – x^2\omega_y $$
$\omega_y$ の項を集めると $-\omega_y – x^2\omega_y = -(1 + x^2)\omega_y$ となるので、
$$ \dot{x} = -\frac{1}{Z}v_x + \frac{x}{Z}v_z + xy\,\omega_x – (1 + x^2)\omega_y + y\,\omega_z $$
$v_y$ の項がないことに注意してください。これは $x$ 座標が $Y$ 方向の並進には（直接的には）依存しないためです。ただし $v_y = 0$ は $\dot{x}$ の式に $v_y$ の係数が0であることを意味し、行列表現では0として明示的に書きます。
$\dot{y}$ の展開
同様の手順を $\dot{y} = \dot{Y}/Z – y\dot{Z}/Z$ に適用します。
$\dot{Y} = -v_y – \omega_z X + \omega_x Z$ を代入して $\dot{Y}/Z$ を計算すると、
$$ \frac{\dot{Y}}{Z} = -\frac{v_y}{Z} – \omega_z \frac{X}{Z} + \omega_x = -\frac{v_y}{Z} – \omega_z x + \omega_x $$
$y\dot{Z}/Z$ は先ほどの $\dot{Z}$ の式から
$$ y\frac{\dot{Z}}{Z} = -y\frac{v_z}{Z} – y^2\omega_x + xy\omega_y $$
したがって
$$ \dot{y} = \left(-\frac{v_y}{Z} – \omega_z x + \omega_x\right) – \left(-y\frac{v_z}{Z} – y^2\omega_x + xy\omega_y\right) $$
各項を整理すると
$$ \dot{y} = -\frac{v_y}{Z} + y\frac{v_z}{Z} + \omega_x – \omega_z x + y^2\omega_x – xy\omega_y $$
$\omega_x$ の項を集めると $\omega_x + y^2\omega_x = (1 + y^2)\omega_x$ となるので、
$$ \dot{y} = -\frac{1}{Z}v_y + \frac{y}{Z}v_z + (1 + y^2)\omega_x – xy\,\omega_y – x\,\omega_z $$
画像ヤコビアン行列の完成
$\dot{x}$ と $\dot{y}$ の式を行列形式でまとめましょう。
$$ \begin{pmatrix} \dot{x} \\ \dot{y} \end{pmatrix} = \bm{L}_s \begin{pmatrix} v_x \\ v_y \\ v_z \\ \omega_x \\ \omega_y \\ \omega_z \end{pmatrix} $$
ここで $\bm{L}_s$ が画像ヤコビアン（Interaction Matrix）であり、
$$ \bm{L}_s = \begin{pmatrix} -\dfrac{1}{Z} & 0 & \dfrac{x}{Z} & xy & -(1+x^2) & y \\[8pt] 0 & -\dfrac{1}{Z} & \dfrac{y}{Z} & 1+y^2 & -xy & -x \end{pmatrix} $$
です。この $2 \times 6$ の行列が、ビジュアルサーボの数学的な心臓部です。1つの特徴点について2行が得られるので、$n$ 個の特徴点を使うと $2n \times 6$ の画像ヤコビアンが構成されます。
カメラの6自由度の速度を一意に求めるには少なくとも3つの特徴点（$2 \times 3 = 6$ 行）が必要ですが、冗長性を持たせるために4点以上を使い、最小二乗法で解くのが一般的です。
画像ヤコビアンの式が得られたので、次にその構造を詳しく分析し、この行列が持つ物理的な意味を掘り下げます。
画像ヤコビアンの構造と物理的意味
並進部分と回転部分の分離
画像ヤコビアン $\bm{L}_s$ は、並進速度に対応する左半分と角速度に対応する右半分に分けて眺めると、その構造がよくわかります。
$$ \bm{L}_s = \begin{pmatrix} \bm{L}_v & \bm{L}_\omega \end{pmatrix} $$
並進部分 $\bm{L}_v$（左3列）:
$$ \bm{L}_v = \begin{pmatrix} -\dfrac{1}{Z} & 0 & \dfrac{x}{Z} \\[8pt] 0 & -\dfrac{1}{Z} & \dfrac{y}{Z} \end{pmatrix} $$
この部分は全ての要素に $1/Z$ が含まれています。つまり、並進運動による画像上の動きは奥行き $Z$ に反比例します。これは直感と合致します — 近くの物体が同じ速度で動けば画像上で大きく動き、遠くの物体は小さくしか動きません。
回転部分 $\bm{L}_\omega$（右3列）:
$$ \bm{L}_\omega = \begin{pmatrix} xy & -(1+x^2) & y \\ 1+y^2 & -xy & -x \end{pmatrix} $$
注目すべきは、回転部分に奥行き $Z$ が全く含まれていないことです。カメラが回転するとき、画像上の特徴点の動きは物体までの距離に依存しません。これは、純粋な回転ではカメラの光学中心の位置が変わらないためです。光学中心が動かなければ、各光線の方向は物体の距離によらず一意に決まるのです。
奥行き依存性の影響
画像ヤコビアンの並進部分が $Z$ に依存することは、ビジュアルサーボにとって根本的な課題を提起します。
画像から得られる情報は2次元のピクセル座標 $(u, v)$ のみです。しかし、画像ヤコビアンを計算するには3次元の奥行き $Z$ が必要です。これは、2D画像だけでは3D空間の情報を完全に復元できないという射影変換の本質的な情報損失を反映しています。
実際のビジュアルサーボでは、この奥行き $Z$ に対して以下のような対処が取られます。
- ステレオカメラ: 2台のカメラの視差から奥行きを推定
- 深度カメラ: LiDARやToFセンサーで直接奥行きを計測
- SfM: カメラの運動と複数フレームの画像から奥行きを復元
- 推定値の使用: 奥行きの概算値を使い、制御の安定性で吸収する（ロバストな制御設計が必要）
宇宙ロボティクスの文脈では、ランデブー段階での相対距離はレーダーやLiDARで粗く推定できるため、その推定値を画像ヤコビアンに代入する手法がよく使われます。
特異点の分析
画像ヤコビアン $\bm{L}_s$ は $2 \times 6$ の行列なので、$n$ 個の特徴点から構成される $2n \times 6$ の行列のランクが6未満になる状況（特異配置）が存在し得ます。
特に注意すべき特異な状況は以下の通りです。
- 全ての特徴点が画像中心にある ($x = y = 0$): 並進と回転の効果が区別できなくなる
- 全ての特徴点が一直線上にある: $2n \times 6$ 行列のランクが不足する
- $Z \to \infty$: 並進部分 $\bm{L}_v \to \bm{0}$ となり、並進速度の推定が不可能になる
このような特異配置を避けるために、特徴点の選び方やカメラの配置を工夫する必要があります。
ピクセル座標での画像ヤコビアン
ここまでは正規化画像座標 $(x, y)$ で画像ヤコビアンを導出しました。実際のピクセル座標 $(u, v)$ での画像ヤコビアンは、内部パラメータ行列 $\bm{K}$ を使って変換できます。
ピクセル座標と正規化座標の関係 $u = f_x x + c_x$, $v = f_y y + c_y$ を時間微分すると
$$ \dot{u} = f_x \dot{x}, \quad \dot{v} = f_y \dot{y} $$
したがって、ピクセル座標での画像ヤコビアンは
$$ \bm{L}_{\text{pixel}} = \begin{pmatrix} f_x & 0 \\ 0 & f_y \end{pmatrix} \bm{L}_s $$
と書けます。具体的に展開すると、
$$ \bm{L}_{\text{pixel}} = \begin{pmatrix} -\dfrac{f_x}{Z} & 0 & \dfrac{u – c_x}{Z} & \dfrac{(u – c_x)(v – c_y)}{f_y} & -f_x – \dfrac{(u – c_x)^2}{f_x} & \dfrac{f_x(v – c_y)}{f_y} \\[8pt] 0 & -\dfrac{f_y}{Z} & \dfrac{v – c_y}{Z} & f_y + \dfrac{(v – c_y)^2}{f_y} & -\dfrac{(u – c_x)(v – c_y)}{f_x} & -\dfrac{f_y(u – c_x)}{f_x} \end{pmatrix} $$
ここで $x = (u – c_x)/f_x$, $y = (v – c_y)/f_y$ を代入しています。
画像ヤコビアンの数学的構造を理解したところで、いよいよPythonによる実装と可視化に移りましょう。理論で導いた式をコードに落とし込み、カメラモデルの挙動と画像ヤコビアンの特性を目で見て確認します。
Pythonでカメラモデルの実装と可視化
透視投影の可視化
まずは、3次元空間内の点がピンホールカメラの画像平面上にどのように投影されるかを可視化します。単純な立方体の頂点を3D空間に配置し、透視投影で2D画像に射影する過程を見てみましょう。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# カメラ内部パラメータ
fx, fy = 800, 800 # 焦点距離（ピクセル単位）
cx, cy = 320, 240 # 主点（画像中心）
K = np.array([[fx, 0, cx],
[0, fy, cy],
[0, 0, 1]])
# 立方体の頂点（ワールド座標系、単位: メートル）
cube = np.array([
[-0.5, -0.5, -0.5], [ 0.5, -0.5, -0.5],
[ 0.5, 0.5, -0.5], [-0.5, 0.5, -0.5],
[-0.5, -0.5, 0.5], [ 0.5, -0.5, 0.5],
[ 0.5, 0.5, 0.5], [-0.5, 0.5, 0.5]
]).T # (3, 8)
# 立方体の辺（頂点の接続）
edges = [(0,1),(1,2),(2,3),(3,0),
(4,5),(5,6),(6,7),(7,4),
(0,4),(1,5),(2,6),(3,7)]
# 外部パラメータ: カメラの位置と姿勢
R = np.eye(3) # カメラは正面を向いている
t = np.array([[0], [0], [3]]) # カメラの前方3mに立方体
# ワールド座標→カメラ座標
P_cam = R @ cube + t # (3, 8)
# 透視投影: カメラ座標→ピクセル座標
u = fx * P_cam[0, :] / P_cam[2, :] + cx
v = fy * P_cam[1, :] / P_cam[2, :] + cy
# 可視化
fig, axes = plt.subplots(1, 2, figsize=(14, 5))
# 左: 3Dビュー
ax3d = fig.add_subplot(121, projection='3d')
for (i, j) in edges:
ax3d.plot3D(*zip(cube[:, i], cube[:, j]), 'b-', linewidth=1.5)
ax3d.scatter(*(-t.flatten()), color='red', s=100, label='Camera')
ax3d.set_xlabel('X [m]')
ax3d.set_ylabel('Y [m]')
ax3d.set_zlabel('Z [m]')
ax3d.set_title('3D Scene')
ax3d.legend()
# 右: 2D射影画像
axes[1].set_xlim(0, 640)
axes[1].set_ylim(480, 0) # 画像座標系（y軸反転）
for (i, j) in edges:
axes[1].plot([u[i], u[j]], [v[i], v[j]], 'b-', linewidth=1.5)
axes[1].plot(u, v, 'ro', markersize=6)
axes[1].set_xlabel('u [pixel]')
axes[1].set_ylabel('v [pixel]')
axes[1].set_title('Projected Image (Pinhole Camera)')
axes[1].set_aspect('equal')
axes[1].grid(True, alpha=0.3)
plt.tight_layout()
plt.show()
左の3Dビューでは、カメラ（赤点）の前方3mに立方体が配置されています。右の2D画像では、透視投影によって立方体の奥の面が手前の面より小さく映り、遠近感が表現されていることがわかります。全ての辺が画像上で正しく接続されており、ピンホールカメラモデルの透視投影が幾何学的に一貫していることが確認できます。
奥行きによる射影の変化
次に、物体の奥行き（カメラからの距離）が変わると画像上のサイズがどう変化するかを可視化します。
import numpy as np
import matplotlib.pyplot as plt
# カメラ内部パラメータ
fx, fy = 800, 800
cx, cy = 320, 240
# 正方形の頂点（1m x 1m、Z軸方向に配置）
square = np.array([
[-0.5, -0.5, 0],
[ 0.5, -0.5, 0],
[ 0.5, 0.5, 0],
[-0.5, 0.5, 0],
[-0.5, -0.5, 0] # 閉じるための頂点
]).T # (3, 5)
distances = [2, 3, 5, 8, 12]
colors = plt.cm.viridis(np.linspace(0, 1, len(distances)))
fig, ax = plt.subplots(figsize=(8, 6))
ax.set_xlim(0, 640)
ax.set_ylim(480, 0)
for Z, color in zip(distances, colors):
# 透視投影
u = fx * square[0, :] / Z + cx
v = fy * square[1, :] / Z + cy
ax.plot(u, v, '-o', color=color, linewidth=2, markersize=4,
label=f'Z = {Z} m')
ax.set_xlabel('u [pixel]')
ax.set_ylabel('v [pixel]')
ax.set_title('Perspective Projection at Different Depths')
ax.legend()
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.show()
このグラフから、透視投影の2つの重要な性質が読み取れます。
- 距離に反比例するサイズ: 画像上の正方形のサイズは奥行き $Z$ に反比例しています。$Z = 2$ m の正方形は $Z = 12$ m のものに比べて約6倍大きく映ります。これは透視投影の式 $u = f_x X/Z + c_x$ に含まれる $1/Z$ の因子の直接的な帰結です。
- 画像中心への収束: 奥行きが大きくなるにつれ、全ての正方形が画像の主点 $(c_x, c_y)$ に向かって収縮しています。$Z \to \infty$ の極限では、全ての点が主点に一致します。これは無限遠の物体は光軸上の1点に見えるという直感に合います。
レンズ歪みの可視化
放射歪みが画像にどのような影響を与えるかを視覚的に確認します。
import numpy as np
import matplotlib.pyplot as plt
def apply_distortion(x, y, k1, k2, p1, p2):
"""レンズ歪みモデルの適用"""
r2 = x**2 + y**2
r4 = r2**2
# 放射歪み
radial = 1 + k1 * r2 + k2 * r4
x_d = x * radial + 2 * p1 * x * y + p2 * (r2 + 2 * x**2)
y_d = y * radial + p1 * (r2 + 2 * y**2) + 2 * p2 * x * y
return x_d, y_d
# 格子点を生成（正規化画像座標）
n_grid = 15
x_lin = np.linspace(-1, 1, n_grid)
y_lin = np.linspace(-0.75, 0.75, n_grid)
xx, yy = np.meshgrid(x_lin, y_lin)
# 歪みパラメータ
distortion_cases = {
'No Distortion': (0, 0, 0, 0),
'Barrel (k1=-0.3)': (-0.3, 0, 0, 0),
'Pincushion (k1=0.3)': (0.3, 0, 0, 0),
'Barrel + Tangential': (-0.3, 0.05, 0.01, -0.01),
}
fig, axes = plt.subplots(2, 2, figsize=(12, 9))
axes = axes.flatten()
for ax, (title, (k1, k2, p1, p2)) in zip(axes, distortion_cases.items()):
xd, yd = apply_distortion(xx, yy, k1, k2, p1, p2)
# 横線
for i in range(n_grid):
ax.plot(xd[i, :], yd[i, :], 'b-', linewidth=0.8)
# 縦線
for j in range(n_grid):
ax.plot(xd[:, j], yd[:, j], 'b-', linewidth=0.8)
ax.set_title(title, fontsize=12)
ax.set_xlim(-1.5, 1.5)
ax.set_ylim(-1.1, 1.1)
ax.set_aspect('equal')
ax.grid(True, alpha=0.2)
plt.suptitle('Lens Distortion Models', fontsize=14, y=1.01)
plt.tight_layout()
plt.show()
4つのパネルから、レンズ歪みの性質が明確に読み取れます。
- 歪みなし（左上）: 格子は完全な直線を保っています。これがピンホールカメラモデルの理想的な投影です。
- 樽型歪み（右上）: $k_1 < 0$ のとき、画像の端に向かうほど格子が外側に膨らみます。広角レンズでよく見られ、直線が弓なりに曲がります。
- 糸巻き型歪み（左下）: $k_1 > 0$ のとき、画像の端が内側に引き込まれます。望遠レンズで起きやすい歪みです。
- 樽型 + 接線歪み（右下）: 放射歪みに加えて接線歪みが加わると、格子の変形が非対称になります。特に、対角方向にわずかなシフトが見られ、これがレンズの傾きによる接線歪みの効果です。
これらの歪みは、カメラキャリブレーションで歪みパラメータを求めることにより、画像処理の前段階で補正されます。
ここからが本記事のハイライトです。画像ヤコビアンを実装し、カメラの各運動成分が画像上の特徴点をどのように動かすかを可視化します。
Pythonで画像ヤコビアンの実装と可視化
画像ヤコビアンの実装
まず、正規化画像座標での画像ヤコビアン $\bm{L}_s$ を計算する関数を実装します。
import numpy as np
def image_jacobian(x, y, Z):
"""
正規化画像座標での画像ヤコビアン（Interaction Matrix）を計算する。
Parameters
----------
x, y : float
正規化画像座標
Z : float
特徴点の奥行き（カメラ座標系でのZ座標）
Returns
-------
L : ndarray, shape (2, 6)
画像ヤコビアン
"""
L = np.array([
[-1/Z, 0, x/Z, x*y, -(1 + x**2), y],
[ 0, -1/Z, y/Z, 1 + y**2, -x*y, -x]
])
return L
def image_jacobian_pixel(u, v, Z, fx, fy, cx, cy):
"""
ピクセル座標での画像ヤコビアンを計算する。
Parameters
----------
u, v : float
ピクセル座標
Z : float
特徴点の奥行き
fx, fy : float
焦点距離（ピクセル単位）
cx, cy : float
主点
Returns
-------
L : ndarray, shape (2, 6)
ピクセル座標での画像ヤコビアン
"""
# 正規化画像座標に変換
x = (u - cx) / fx
y = (v - cy) / fy
# 正規化画像座標の画像ヤコビアン
Ls = image_jacobian(x, y, Z)
# ピクセル座標への変換行列
diag_f = np.array([[fx, 0],
[0, fy]])
return diag_f @ Ls
この実装は導出した数式をそのまま反映しています。image_jacobian が正規化画像座標版、image_jacobian_pixel がピクセル座標版です。ピクセル座標版は、正規化版に焦点距離の対角行列を左から掛けるだけで得られます。
6自由度の速度成分ごとの効果の可視化
画像ヤコビアンの6列は、カメラの6自由度（$v_x, v_y, v_z, \omega_x, \omega_y, \omega_z$）それぞれに対応しています。各速度成分が画像上の特徴点をどの方向に動かすかをベクトル場（フローフィールド）として可視化しましょう。
import numpy as np
import matplotlib.pyplot as plt
def image_jacobian(x, y, Z):
"""正規化画像座標での画像ヤコビアン"""
L = np.array([
[-1/Z, 0, x/Z, x*y, -(1 + x**2), y],
[ 0, -1/Z, y/Z, 1 + y**2, -x*y, -x]
])
return L
# 画像上の格子点
n = 12
x_range = np.linspace(-0.8, 0.8, n)
y_range = np.linspace(-0.6, 0.6, n)
xx, yy = np.meshgrid(x_range, y_range)
Z = 2.0 # 奥行き
# 6つの速度成分
velocity_labels = [
r'$v_x$ (Translation X)',
r'$v_y$ (Translation Y)',
r'$v_z$ (Translation Z)',
r'$\omega_x$ (Rotation X)',
r'$\omega_y$ (Rotation Y)',
r'$\omega_z$ (Rotation Z)',
]
fig, axes = plt.subplots(2, 3, figsize=(16, 10))
axes = axes.flatten()
for k in range(6):
ax = axes[k]
# 単位速度ベクトル（k番目の成分が1、他は0）
vel = np.zeros(6)
vel[k] = 1.0
dx_arr = np.zeros_like(xx)
dy_arr = np.zeros_like(yy)
for i in range(n):
for j in range(n):
L = image_jacobian(xx[i, j], yy[i, j], Z)
ds = L @ vel
dx_arr[i, j] = ds[0]
dy_arr[i, j] = ds[1]
# ベクトルの大きさで色付け
magnitude = np.sqrt(dx_arr**2 + dy_arr**2)
ax.quiver(xx, yy, dx_arr, dy_arr, magnitude,
cmap='coolwarm', scale=15, width=0.004)
ax.set_title(velocity_labels[k], fontsize=12)
ax.set_xlim(-1, 1)
ax.set_ylim(-0.8, 0.8)
ax.set_aspect('equal')
ax.axhline(0, color='gray', linewidth=0.5)
ax.axvline(0, color='gray', linewidth=0.5)
ax.grid(True, alpha=0.2)
plt.suptitle(f'Image Jacobian Flow Fields (Z = {Z} m)', fontsize=14)
plt.tight_layout()
plt.show()
6つのフローフィールドから、画像ヤコビアンの各列が表す物理的意味が鮮明に読み取れます。
並進成分: 1. $v_x$（左上）: 全ての矢印が左方向を指しています。カメラが右に移動すると、画像上の全ての点が左に移動します。矢印の長さは一様で、画像上の位置にほぼ依存しません（画像中心からの距離の影響は弱い）。 2. $v_y$（上中央）: 全ての矢印が上方向を指しています。$v_x$ と同様に、カメラの $Y$ 方向の並進は画像上で一様な動きを引き起こします。 3. $v_z$（右上）: 矢印が画像中心から放射状に外向きに広がっています。これはFOE（Focus of Expansion）パターンで、カメラが前進すると画像上の全ての点が中心から外側に向かって動くことを意味します。中心付近では矢印が短く、周辺では長くなっています。
回転成分: 4. $\omega_x$（左下）: 矢印が下向きに配列されていますが、$y$ 座標が大きいほど矢印も長くなっています。カメラが $X$ 軸回りに回転すると、画像上の点は $y$ 座標に依存した速度で動きます。 5. $\omega_y$（下中央）: $\omega_x$ と類似した対称パターンが $x$ 方向に現れています。 6. $\omega_z$（右下）: 矢印が画像中心の周りを回転するパターンを示しています。これはカメラの光軸回りの回転に対応し、画像上の点は中心からの距離に比例した速度で円運動します。
奥行き依存性の可視化
最後に、画像ヤコビアンの最も重要な特性である奥行き依存性を可視化します。同じ画像座標の点でも、奥行きが異なると画像ヤコビアンがどう変化するかを確認しましょう。
import numpy as np
import matplotlib.pyplot as plt
def image_jacobian(x, y, Z):
"""正規化画像座標での画像ヤコビアン"""
L = np.array([
[-1/Z, 0, x/Z, x*y, -(1 + x**2), y],
[ 0, -1/Z, y/Z, 1 + y**2, -x*y, -x]
])
return L
# 固定の画像座標
x0, y0 = 0.3, 0.2
# 奥行きの範囲
Z_range = np.linspace(0.5, 10, 200)
# 各速度成分に対するゲイン（L行列の各要素）を記録
# vxに対するdx/dt（L[0,0]）とvzに対するdx/dt（L[0,2]）
gain_vx = [] # -1/Z
gain_vz = [] # x/Z
gain_wx = [] # xy（定数）
gain_wy = [] # -(1+x^2)（定数）
for Z in Z_range:
L = image_jacobian(x0, y0, Z)
gain_vx.append(abs(L[0, 0]))
gain_vz.append(abs(L[0, 2]))
gain_wx.append(abs(L[0, 3]))
gain_wy.append(abs(L[0, 4]))
fig, axes = plt.subplots(1, 2, figsize=(14, 5))
# 左: 並進ゲイン
axes[0].plot(Z_range, gain_vx, 'b-', linewidth=2, label=r'$|L_{11}|$ ($v_x$ gain)')
axes[0].plot(Z_range, gain_vz, 'r-', linewidth=2, label=r'$|L_{13}|$ ($v_z$ gain)')
axes[0].set_xlabel('Depth Z [m]', fontsize=12)
axes[0].set_ylabel('Gain (absolute value)', fontsize=12)
axes[0].set_title('Translation Gain vs Depth', fontsize=13)
axes[0].legend(fontsize=11)
axes[0].grid(True, alpha=0.3)
axes[0].set_ylim(0, 3)
# 右: 回転ゲイン
axes[1].plot(Z_range, gain_wx, 'g-', linewidth=2,
label=r'$|L_{14}|$ ($\omega_x$ gain)')
axes[1].plot(Z_range, gain_wy, 'm-', linewidth=2,
label=r'$|L_{15}|$ ($\omega_y$ gain)')
axes[1].set_xlabel('Depth Z [m]', fontsize=12)
axes[1].set_ylabel('Gain (absolute value)', fontsize=12)
axes[1].set_title('Rotation Gain vs Depth', fontsize=13)
axes[1].legend(fontsize=11)
axes[1].grid(True, alpha=0.3)
axes[1].set_ylim(0, 2)
plt.suptitle(f'Depth Dependency of Image Jacobian (x={x0}, y={y0})',
fontsize=14)
plt.tight_layout()
plt.show()
2つのグラフから、画像ヤコビアンの奥行き依存性について決定的な違いが読み取れます。
左（並進ゲイン）: $v_x$ と $v_z$ に対するゲインはともに $Z$ が小さいとき非常に大きく、$Z$ が大きくなるにつれて急速に減衰しています。これは $1/Z$ の依存性を反映しています。物体が近い（$Z$ が小さい）とき、わずかなカメラの並進でも画像上で大きな変位が生じます。逆に、物体が遠い（$Z$ が大きい）とき、カメラを大きく動かしても画像上の動きは微小です。このことは、遠方の物体に対する並進ベースのビジュアルサーボが困難であることを意味しています。
右（回転ゲイン）: $\omega_x$ と $\omega_y$ に対するゲインは奥行き $Z$ に全く依存せず、一定の水平線を描いています。回転ゲインが $(x, y)$ のみに依存し $Z$ に依存しないことが数値的にも確認できました。この性質は、回転自由度のみを使うビジュアルサーボが奥行き推定の誤差に対してロバストであることを示唆しています。
画像ヤコビアンによる速度場のアニメーション的可視化
理論と実装の統合として、カメラの複合運動（並進 + 回転を同時に行う場合）が画像フローにどう影響するかを可視化します。
import numpy as np
import matplotlib.pyplot as plt
def image_jacobian(x, y, Z):
"""正規化画像座標での画像ヤコビアン"""
L = np.array([
[-1/Z, 0, x/Z, x*y, -(1 + x**2), y],
[ 0, -1/Z, y/Z, 1 + y**2, -x*y, -x]
])
return L
# 画像上の格子点
n = 15
x_range = np.linspace(-0.8, 0.8, n)
y_range = np.linspace(-0.6, 0.6, n)
xx, yy = np.meshgrid(x_range, y_range)
# 3つのシナリオ
scenarios = {
'Forward + Yaw': np.array([0, 0, 0.5, 0, 0, 0.3]),
'Lateral + Pitch': np.array([0.3, 0, 0, 0, 0.2, 0]),
'Approach + Roll': np.array([0, 0, 1.0, 0, 0, 0.5]),
}
Z_values = [1.5, 3.0, 6.0]
fig, axes = plt.subplots(len(scenarios), len(Z_values),
figsize=(16, 14))
for row, (scenario_name, vel) in enumerate(scenarios.items()):
for col, Z in enumerate(Z_values):
ax = axes[row, col]
dx_arr = np.zeros_like(xx)
dy_arr = np.zeros_like(yy)
for i in range(n):
for j in range(n):
L = image_jacobian(xx[i, j], yy[i, j], Z)
ds = L @ vel
dx_arr[i, j] = ds[0]
dy_arr[i, j] = ds[1]
magnitude = np.sqrt(dx_arr**2 + dy_arr**2)
ax.quiver(xx, yy, dx_arr, dy_arr, magnitude,
cmap='coolwarm', scale=20, width=0.004)
ax.set_xlim(-1, 1)
ax.set_ylim(-0.8, 0.8)
ax.set_aspect('equal')
ax.grid(True, alpha=0.2)
if row == 0:
ax.set_title(f'Z = {Z} m', fontsize=12)
if col == 0:
ax.set_ylabel(scenario_name, fontsize=11)
plt.suptitle('Combined Motion: Image Flow Fields at Different Depths',
fontsize=14)
plt.tight_layout()
plt.show()
$3 \times 3$ のパネルから、複合運動と奥行きの相互作用について重要な知見が得られます。
- Forward + Yaw（前進 + ヨー回転）: $Z = 1.5$ m（近距離）では前進による放射状フローが支配的で、ヨー回転の効果は相対的に小さくなっています。しかし $Z = 6.0$ m（遠距離）では前進の $v_z$ ゲインが $1/Z$ で減衰するため、ヨー回転の効果が卓越し、フローパターンは回転的に変化しています。
- Lateral + Pitch（横移動 + ピッチ回転）: 同様に、近距離では並進が、遠距離では回転が支配的です。
- Approach + Roll（前進 + ロール回転）: 近距離では強い放射状の拡大フローが見られますが、遠距離ではロール回転による回転フローが混ざり、渦巻き状のパターンになっています。
この奥行きによるフローパターンの変化は、ビジュアルサーボの制御設計で極めて重要です。同じカメラ速度でも、対象物体までの距離が変わるとフィードバック信号の性質が質的に変化するため、制御器のゲインを適応的に調整する必要があることを示しています。
宇宙ロボティクスにおける実用上の考慮事項
宇宙環境特有の課題
地上のビジュアルサーボと宇宙でのそれには、いくつかの本質的な違いがあります。
照明条件の過酷さ: 宇宙空間では太陽光が直射し、影の部分は完全な暗闇です。対象衛星が太陽に対してどの角度にあるかによって、画像のコントラストが劇的に変化します。地上のように均一な環境光は期待できないため、特徴点の検出・追跡アルゴリズムには高いロバスト性が求められます。
放射線によるノイズ: 宇宙空間の高エネルギー粒子がカメラセンサーに当たると、画像上に一時的な白い点（ホットピクセル）が発生します。これはSEU（Single Event Upset）と呼ばれ、特徴点の誤検出の原因になります。
計算資源の制約: 宇宙用コンピュータは放射線耐性を確保するため、地上の最新CPUに比べて桁違いに低性能です。画像ヤコビアンの計算やカメラキャリブレーションのアルゴリズムは、限られた計算資源で実時間動作する必要があります。
カメラキャリブレーションの課題
宇宙空間ではカメラキャリブレーションにも特有の困難があります。打上げ時の振動や宇宙空間の温度変化（太陽面 $+120°$C、影面 $-150°$C）によって、カメラの内部パラメータが地上でのキャリブレーション時から変化する可能性があります。
このため、オンラインキャリブレーション（運用中に内部パラメータを推定・更新する手法）や、自己キャリブレーション（既知の幾何形状を必要としないキャリブレーション）が研究されています。
画像ヤコビアンの奥行き推定
宇宙ランデブーでは、接近段階の距離範囲が数km（遠距離フェーズ）から数cm（ドッキングフェーズ）まで大きく変化します。画像ヤコビアンの並進部分は $1/Z$ に依存するため、奥行き推定の精度がビジュアルサーボの性能に直結します。
遠距離フェーズではレーダーやLiDARの計測値を使い、近距離フェーズでは画像ベースの奥行き推定（ステレオビジョンや既知の対象形状からの推定）に切り替えるなど、距離に応じたセンサーフュージョン戦略が採用されます。
以上の実用的な課題を踏まえた上で、次の記事ではこれらの数学的基礎を用いてビジュアルサーボの制御則を構築していきます。
まとめ
本記事では、ビジュアルサーボの数学的基礎となるカメラモデルと画像ヤコビアンについて解説しました。
- ピンホールカメラモデル: 3D空間の点が $x = X/Z$, $y = Y/Z$ という透視投影で2D画像に射影される。$1/Z$ の非線形性が遠近感の本質
- 内部パラメータ行列 $\bm{K}$: 焦点距離 $(f_x, f_y)$ と主点 $(c_x, c_y)$ を含む $3 \times 3$ の行列。カメラ固有のハードウェア仕様を表す
- 外部パラメータ $[\bm{R} \mid \bm{t}]$: カメラの位置と姿勢を記述する回転行列と並進ベクトル。ワールド座標からカメラ座標への変換を与える
- レンズ歪みモデル: 放射歪み（$k_1, k_2, k_3$）と接線歪み（$p_1, p_2$）で実際のレンズの光学的影響を補正
- 画像ヤコビアン $\bm{L}_s$: カメラの6自由度の速度と画像上の特徴点速度を結ぶ $2 \times 6$ の行列。並進部分は奥行き $Z$ に依存し、回転部分は $Z$ に依存しない
- 奥行き依存性: 画像ヤコビアンの並進ゲインは $1/Z$ で減衰し、遠方の物体に対する並進ベースのサーボを困難にする。一方、回転ゲインは距離によらず一定
これらの数学的道具を手にしたことで、カメラ画像とロボットの動きの関係を定量的に記述できるようになりました。次の記事では、この画像ヤコビアンを制御ループに組み込み、カメラ画像のフィードバックでロボットアームを目標位置に誘導する位置ベースビジュアルサーボ（PBVS）の制御則を導出します。
次のステップとして、以下の記事も参考にしてください。
- 位置ベースビジュアルサーボの理論と実装 — 画像ヤコビアンを使ったPBVS制御則の導出と実装
- ヤコビ行列とヤコビアンの意味を理解する — 画像ヤコビアンの数学的基盤であるヤコビ行列の一般論