宇宙空間で作業するロボットアームが、漂うデブリを正確につかむ場面を想像してみてください。ロボットはカメラで対象物を捉えていますが、GPS もなければ、デブリの正確な 3 次元形状モデルも手元にありません。カメラ画像の中に映った特徴点だけが頼りです。
このとき、「画像の中の特徴点を望ましい位置に持っていく」ことだけを目標にカメラ（= ロボット）を動かす制御戦略があったら、3 次元復元の不確かさを回避しつつ、極めて直感的にロボットを導けるはずです。この発想を体系化したのが 画像ベースビジュアルサーボ（Image-Based Visual Servoing, IBVS） です。
前回の記事で扱った PBVS（Position-Based Visual Servoing）は、画像から 3 次元の姿勢を復元し、それを目標姿勢に一致させるアプローチでした。しかし 3 次元復元にはカメラキャリブレーションの精度やモデル誤差が大きく影響し、宇宙環境のような過酷な条件では破綻しやすいという問題がありました。
IBVS は「3 次元の世界に戻る」という中間ステップをバイパスし、画像空間の中で誤差を定義して直接最小化します。これにより、以下のような利点が生まれます。
- カメラキャリブレーション誤差にロバスト — 3D 復元を行わないため、カメラ内部パラメータの誤差の影響を受けにくい
- 画像内に特徴点が留まりやすい — 画像空間上で誤差を減少させるので、特徴点がフレームから逸脱しにくい
- 軌道上サービスや惑星探査ローバーの視覚誘導 — CAD モデルを持たない未知対象に対しても適用可能
本記事の内容
- IBVS の基本戦略 — 画像特徴量の誤差を直接最小化する発想
- 画像ヤコビアン（Interaction Matrix）$\bm{L}_s$ の構造と導出
- IBVS の制御則 $\bm{v}_c = -\lambda \hat{\bm{L}}_s^+ (\bm{s} – \bm{s}^*)$ の数学的意味
- 奥行き推定の問題と実用的な対策
- IBVS の利点と欠点の詳細な分析
- PBVS vs IBVS の包括的比較
- ハイブリッドアプローチ（2-1/2D Visual Servoing）
- Python による IBVS シミュレーション — 4 点特徴点追従とカメラ軌道の可視化
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
- カメラモデルと画像ヤコビアン — ピンホールカメラモデルと、3D 速度を画像上の速度に変換する画像ヤコビアンの導出
- 位置ベースビジュアルサーボ（PBVS） — 3D 姿勢を復元して制御する PBVS の原理と、IBVS との対比の基礎
IBVS の基本戦略
「画像の中で制御する」という発想
ロボットを視覚で制御するとき、最も素朴な方法は「画像から 3D 姿勢を復元し、その 3D 姿勢を目標に合わせる」（= PBVS）です。しかし、この方法は 3D 復元の精度に全てがかかっています。カメラの焦点距離が 1% ズレただけで、推定された 3D 位置は大きく外れてしまうかもしれません。
IBVS はこの問題を逆転の発想で解決します。「3D 空間に戻らず、画像の中だけで制御を完結させよう」というのが IBVS の核心です。
たとえば、対象物に 4 つの特徴点が見えているとしましょう。カメラの現在位置では、これら 4 点が画像上のある位置 $\bm{s} = (u_1, v_1, u_2, v_2, u_3, v_3, u_4, v_4)^\top$ に映っています。目標のカメラ位置では、同じ 4 点が $\bm{s}^* = (u_1^*, v_1^*, u_2^*, v_2^*, u_3^*, v_3^*, u_4^*, v_4^*)^\top$ に映るはずです。IBVS の目標は単純明快です。
画像上の特徴量 $\bm{s}$ を目標特徴量 $\bm{s}^*$ に一致させるようにカメラを動かす。
3D 空間での目標位置がどこかを明示的に計算する必要はありません。画像の中で特徴点が目標位置に来れば、カメラは自動的に正しい 3D 位置にいるのです。
特徴量ベクトル $\bm{s}$ の定義
IBVS で使う特徴量ベクトル $\bm{s}$ は、画像上で観測可能な量であれば何でもかまいません。最も一般的なのは点特徴の画像座標ですが、以下のような選択肢もあります。
| 特徴量の種類 | ベクトルの要素 | 次元（$n$ 点） |
|---|---|---|
| 画像座標 $(u, v)$ | 各点のピクセル位置 | $2n$ |
| 正規化画像座標 $(x, y)$ | $x = (u – c_u)/f_x$, $y = (v – c_v)/f_y$ | $2n$ |
| 画像モーメント | $m_{pq} = \sum x^p y^q$ | 選択次第 |
| 直線のパラメータ $(\rho, \theta)$ | 直線の原点距離と角度 | $2 \times$ 直線数 |
本記事では、最も直感的で広く使われる 正規化画像座標 $(x, y)$ を特徴量として用います。正規化座標はカメラの内部パラメータで補正済みの座標で、焦点距離 $f$ の効果が取り除かれているため、理論展開がシンプルになります。
$n$ 個の特徴点がある場合、特徴量ベクトルは次のようになります。
$$ \bm{s} = (x_1, y_1, x_2, y_2, \dots, x_n, y_n)^\top \in \mathbb{R}^{2n} $$
ここまでで、IBVS が「画像上の特徴量誤差を最小化する」という戦略であることがわかりました。しかし、画像上の特徴点の動きとカメラの動きの間には、一筋縄ではいかない非線形な関係があります。この関係を線形的に結びつけるのが、次に解説する画像ヤコビアン（Interaction Matrix）です。
画像ヤコビアン（Interaction Matrix）$\bm{L}_s$
画像上の速度とカメラの速度を結ぶ
カメラが動くと、画像上の特徴点も動きます。しかし、その対応は自明ではありません。カメラが前に進むと画像上の点は放射状に外へ動き、カメラが横に動くと画像上の点は逆方向にスライドし、カメラが回転すると画像上の点は複雑に移動します。
この「カメラの速度 → 画像上の特徴点の速度」という関係を線形近似的に記述するのが、画像ヤコビアン（あるいは Interaction Matrix）$\bm{L}_s$ です。
カメラの速度を 6 自由度のベクトル $\bm{v}_c$ で表します。
$$ \bm{v}_c = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^\top \in \mathbb{R}^6 $$
ここで $(v_x, v_y, v_z)$ はカメラフレームでの並進速度、$(\omega_x, \omega_y, \omega_z)$ は角速度です。画像上の特徴量の時間変化率 $\dot{\bm{s}}$ は、画像ヤコビアンを用いて次のように表されます。
$$ \dot{\bm{s}} = \bm{L}_s \, \bm{v}_c $$
$\bm{L}_s \in \mathbb{R}^{2n \times 6}$ は、$2n$ 次元の特徴量変化率と 6 次元のカメラ速度を結ぶ行列です。この関係は、前提記事「カメラモデルと画像ヤコビアン」で導出したものと同一ですが、ここでは制御の文脈で改めて意味を確認します。
1 点の画像ヤコビアンの導出
ピンホールカメラモデルにおいて、3D 点 $\bm{P} = (X, Y, Z)^\top$ の正規化画像座標は $x = X/Z$, $y = Y/Z$ です。この点がカメラの動きに応じてどう変化するかを求めましょう。
まず、カメラフレームにおける 3D 点の速度は次のように表されます。
$$ \dot{\bm{P}} = -\bm{v} – \bm{\omega} \times \bm{P} $$
ここで $\bm{v} = (v_x, v_y, v_z)^\top$ はカメラの並進速度、$\bm{\omega} = (\omega_x, \omega_y, \omega_z)^\top$ はカメラの角速度です。マイナス符号は、カメラが動くと対象点がカメラフレーム内で逆方向に動くことを反映しています。
成分で書くと、
$$ \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 $$
正規化座標 $x = X/Z$ の時間微分を商の微分法則で求めます。
$$ \dot{x} = \frac{\dot{X} Z – X \dot{Z}}{Z^2} = \frac{\dot{X}}{Z} – x \frac{\dot{Z}}{Z} $$
$\dot{X}$ と $\dot{Z}$ を代入して整理します。まず $\dot{X}/Z$ の項を展開すると、
$$ \frac{\dot{X}}{Z} = -\frac{v_x}{Z} – \omega_y + \omega_z \frac{Y}{Z} = -\frac{v_x}{Z} – \omega_y + \omega_z y $$
次に $x \cdot \dot{Z}/Z$ の項を展開すると、
$$ x \frac{\dot{Z}}{Z} = x \left( -\frac{v_z}{Z} – \omega_x \frac{Y}{Z} + \omega_y \frac{X}{Z} \right) = -\frac{x v_z}{Z} – x y \omega_x + x^2 \omega_y $$
これらを $\dot{x} = \dot{X}/Z – x \cdot \dot{Z}/Z$ に代入すると、
$$ \dot{x} = -\frac{v_x}{Z} + \frac{x v_z}{Z} – \omega_y + \omega_z y + x y \omega_x – x^2 \omega_y $$
項を並べ替えて整理すると、
$$ \dot{x} = -\frac{1}{Z} v_x + 0 \cdot v_y + \frac{x}{Z} v_z + xy \, \omega_x – (1 + x^2) \omega_y + y \, \omega_z $$
同様に $y = Y/Z$ について計算すると（対称性から $x \leftrightarrow y$, $v_x \leftrightarrow v_y$, $\omega_x \leftrightarrow \omega_y$ を入れ替えた構造が得られます）、
$$ \dot{y} = 0 \cdot v_x – \frac{1}{Z} v_y + \frac{y}{Z} v_z + (1 + y^2) \omega_x – xy \, \omega_y – x \, \omega_z $$
以上をまとめると、1 つの特徴点 $(x, y)$ に対する画像ヤコビアンは次の $2 \times 6$ 行列になります。
$$ \bm{L}_{s_i} = \begin{pmatrix} -\dfrac{1}{Z} & 0 & \dfrac{x}{Z} & xy & -(1+x^2) & y \\[6pt] 0 & -\dfrac{1}{Z} & \dfrac{y}{Z} & 1+y^2 & -xy & -x \end{pmatrix} $$
この行列の構造をよく観察すると、いくつかの重要な特徴が読み取れます。
- 並進成分（左 3 列）は奥行き $Z$ に依存する — $Z$ が未知だと正確な制御ができない。これが IBVS 最大の課題です
- 回転成分（右 3 列）は $Z$ に依存しない — カメラの回転に対する画像上の変化は奥行きに無関係
- 並進と回転が画像上で結合している — カメラの前進（$v_z$）と回転が画像上で似た効果を生み、これが後述のリトリート問題の原因
$n$ 点の全体画像ヤコビアン
$n$ 個の特徴点がある場合、全体の画像ヤコビアンは各点のヤコビアンを縦に積み重ねた行列になります。
$$ \bm{L}_s = \begin{pmatrix} \bm{L}_{s_1} \\ \bm{L}_{s_2} \\ \vdots \\ \bm{L}_{s_n} \end{pmatrix} \in \mathbb{R}^{2n \times 6} $$
カメラの速度は 6 自由度なので、$\bm{L}_s$ がランク 6 を持つためには少なくとも $n \geq 3$ 個の特徴点が必要です。実用上は $n = 4$ 点が一般的に使われ、その場合 $\bm{L}_s \in \mathbb{R}^{8 \times 6}$ は冗長な系（overdetermined system）となり、最小二乗的に解くことができます。
画像ヤコビアンの構造がわかったところで、いよいよ核心である IBVS の制御則を導出しましょう。
IBVS の制御則
誤差ダイナミクスの設計
IBVS の制御目的は、画像上の特徴量誤差を次のように定義したとき、
$$ \bm{e}(t) = \bm{s}(t) – \bm{s}^* $$
この誤差が時間とともに指数的に減衰するようにカメラの速度 $\bm{v}_c$ を決定することです。ここで $\bm{s}^*$ は目標特徴量（定数）です。
理想的な収束挙動として、誤差が 1 次指数減衰することを要求します。
$$ \dot{\bm{e}} = -\lambda \bm{e} $$
ここで $\lambda > 0$ はゲインパラメータで、値が大きいほど速く収束しますが、大きすぎると振動や発散のリスクがあります。この式は、各成分が $e_i(t) = e_i(0) \exp(-\lambda t)$ と減衰することを意味しており、制御工学で言う「指数安定性」を実現するものです。
制御則の導出
$\bm{s}^*$ が定数であることから、$\dot{\bm{e}} = \dot{\bm{s}}$ です。画像ヤコビアンの関係式 $\dot{\bm{s}} = \bm{L}_s \bm{v}_c$ を代入すると、
$$ \bm{L}_s \bm{v}_c = -\lambda (\bm{s} – \bm{s}^*) $$
この方程式から $\bm{v}_c$ を求めます。一般に $\bm{L}_s$ は正方行列ではない（$2n \times 6$、通常 $2n > 6$）ので、通常の逆行列は存在しません。そこでムーア・ペンローズ擬似逆行列 $\bm{L}_s^+$ を用います。
$$ \bm{L}_s^+ = (\bm{L}_s^\top \bm{L}_s)^{-1} \bm{L}_s^\top $$
両辺に左から $\bm{L}_s^+$ を掛けると、最終的な IBVS の制御則が得られます。
$$ \boxed{\bm{v}_c = -\lambda \hat{\bm{L}}_s^+ (\bm{s} – \bm{s}^*)} $$
ここで $\hat{\bm{L}}_s$ は画像ヤコビアンの推定値です。帽子（ハット）記号を付けているのは、実際の $\bm{L}_s$ を正確に計算するには各特徴点の奥行き $Z_i$ が必要だからです。実用上は奥行きの推定値 $\hat{Z}_i$ を用いた $\hat{\bm{L}}_s$ を使います。
制御則の直感的意味
この制御則は非常に直感的です。
- $(\bm{s} – \bm{s}^*)$ は「画像上での現在位置と目標位置のズレ」を表します
- $\hat{\bm{L}}_s^+$ は「画像上のズレをカメラの速度に変換する」写像です
- $-\lambda$ は「そのズレをどのくらい積極的に修正するか」の強さです
言い換えれば、「画像を見て、目標からのズレを確認し、そのズレを解消する方向にカメラを動かす」というフィードバック制御そのものです。人間が目で見ながら手を伸ばしてコップをつかむのと本質的に同じことを、数学的にエレガントに定式化したものです。
安定性の解析
IBVS 制御則の安定性を確認しましょう。$\hat{\bm{L}}_s = \bm{L}_s$（画像ヤコビアンが正確）の場合、閉ループダイナミクスは次のようになります。
$\dot{\bm{e}} = \bm{L}_s \bm{v}_c$ に制御則を代入すると、
$$ \dot{\bm{e}} = \bm{L}_s \left( -\lambda \bm{L}_s^+ \bm{e} \right) = -\lambda \bm{L}_s \bm{L}_s^+ \bm{e} $$
$\bm{L}_s$ がフルランク（ランク 6）であるとき、$\bm{L}_s \bm{L}_s^+$ は $\bm{L}_s$ の列空間への直交射影行列であり、$\bm{e}$ が $\bm{L}_s$ の列空間に含まれていれば $\bm{L}_s \bm{L}_s^+ \bm{e} = \bm{e}$ が成り立ちます。特に $2n = 6$（ちょうど 3 点）の場合は $\bm{L}_s$ が正方行列となり、$\bm{L}_s \bm{L}_s^+ = \bm{I}$ なので、
$$ \dot{\bm{e}} = -\lambda \bm{e} $$
が厳密に成り立ち、誤差は $\bm{e}(t) = \bm{e}(0) \exp(-\lambda t)$ と指数減衰します。
$2n > 6$ の場合（冗長な特徴点）でも、$\bm{L}_s$ がフルランクであれば局所漸近安定性が保証されます。ただし、画像ヤコビアンは特徴点の位置や奥行きに依存して時変であるため、大域的な安定性の証明にはリアプノフ関数を用いた議論が必要です。
リアプノフ関数候補として $V = \frac{1}{2} \bm{e}^\top \bm{e}$ を考えると、
$$ \dot{V} = \bm{e}^\top \dot{\bm{e}} = -\lambda \bm{e}^\top \bm{L}_s \bm{L}_s^+ \bm{e} $$
$\bm{L}_s$ がフルランクを保っている限り $\bm{e}^\top \bm{L}_s \bm{L}_s^+ \bm{e} \geq 0$ であり、$\bm{e} \neq \bm{0}$ であれば $\dot{V} < 0$ となって安定性が保証されます。
ここまでで IBVS の制御則とその理論的根拠を確立しました。しかし実際に制御則を適用しようとすると、画像ヤコビアンに含まれる奥行き $Z$ をどう扱うかが大きな問題として立ちはだかります。
奥行き推定の問題と対策
なぜ奥行きが問題になるのか
画像ヤコビアン $\bm{L}_{s_i}$ の並進成分には、各特徴点の奥行き $Z_i$ が含まれています。IBVS は「3D 復元を行わない」ことが売りですが、画像ヤコビアンを計算するために奥行き情報が必要というのは、一見矛盾しているように感じるかもしれません。
しかし、ここが IBVS の巧妙なところです。IBVS は奥行き $Z$ の正確な値を必要とするのではなく、大まかな推定値で十分に動作します。なぜなら、IBVS はフィードバック制御であり、毎ステップで画像誤差を観測して速度指令を更新するため、画像ヤコビアンの多少の誤差はフィードバックによって吸収されるからです。
奥行き推定の 4 つのアプローチ
実用上、奥行き $Z$ の推定にはいくつかのアプローチがあります。
アプローチ 1: 定数近似
最もシンプルな方法は、全特徴点の奥行きを一定値 $\hat{Z}$ に固定してしまうことです。
$$ \hat{Z}_i = Z_0 \quad (\forall i) $$
対象物までの距離がおおよそ分かっている場合（たとえば「ロボットアームのリーチは 2m 程度」）、$Z_0 = 2.0$ m のように設定します。この近似はかなり粗いですが、IBVS のフィードバック特性のおかげで、真値から 50% 程度ずれていても収束する場合が多いことが経験的に知られています。
アプローチ 2: 初期値 + 推定更新
初期フレームでステレオカメラや構造推定（SfM）を用いて奥行きを推定し、その後はカメラの動きに基づいて更新する方法です。
$$ \hat{Z}_i(t+1) = \hat{Z}_i(t) + \Delta Z_i $$
ここで $\Delta Z_i$ はカメラの並進速度 $v_z$ とタイムステップ $\Delta t$ から近似的に計算できます。
アプローチ 3: 奥行き非依存な特徴量の利用
画像モーメントなど、奥行きへの依存が小さい特徴量を選ぶことで問題を軽減する方法もあります。たとえば、対象物の画像上の面積 $a$ と重心 $(x_g, y_g)$ を特徴量として使う場合、面積の変化は奥行きの変化を間接的に反映するため、$Z$ を陽に推定する必要性が減ります。
アプローチ 4: 適応的な奥行き推定
オンラインで画像ヤコビアンのパラメータを推定する適応制御法です。これは最も洗練されたアプローチですが、実装の複雑さが増します。Piepmeier らが提案したブライデン型の更新則が代表的です。
$$ \hat{\bm{L}}_s(k+1) = \hat{\bm{L}}_s(k) + \alpha \frac{(\Delta \bm{s} – \hat{\bm{L}}_s(k) \Delta \bm{v}_c) \Delta \bm{v}_c^\top}{\|\Delta \bm{v}_c\|^2} $$
この更新則は、観測された特徴量変化 $\Delta \bm{s}$ と予測値 $\hat{\bm{L}}_s \Delta \bm{v}_c$ の差を使って画像ヤコビアンの推定値をオンラインで補正します。
奥行き誤差に対するロバスト性の定量的評価
IBVS が奥行き誤差にどの程度ロバストかを定量的に理解するために、スケーリングの観点から分析しましょう。画像ヤコビアンの並進成分は全て $1/Z$ に比例しています。奥行きの推定誤差を $\hat{Z} = \alpha Z$（$\alpha$ は誤差係数）とすると、推定された画像ヤコビアンの並進成分は真値の $1/\alpha$ 倍になります。
制御則に代入すると、並進速度は真の最適値の $1/\alpha$ 倍にスケーリングされます。$\alpha > 0$ である限り、速度の方向は正しく、大きさだけがスケーリングされます。フィードバック制御なので、大きさの誤差は次のステップで修正されます。
ただし、$\alpha$ が極端に大きい（奥行きの大幅な過大評価）場合は制御入力が小さくなりすぎて収束が遅くなり、$\alpha$ が極端に小さい（奥行きの大幅な過小評価）場合は制御入力が過大になって振動や発散を引き起こす可能性があります。
奥行きの問題への対策が見えてきたところで、IBVS の利点と欠点を整理しましょう。制御手法の選択においては、その手法の強みだけでなく限界も正確に理解することが極めて重要です。
IBVS の利点
IBVS には以下の 4 つの主要な利点があります。
利点 1: カメラキャリブレーション誤差へのロバスト性
PBVS では、画像から 3D 姿勢を復元する際にカメラの内部パラメータ（焦点距離、主点位置、歪み係数）の正確な値が必要です。これらのパラメータに誤差があると、復元された 3D 姿勢が歪み、制御が正しく機能しなくなります。
一方、IBVS では特徴量を正規化画像座標で扱う場合でも、キャリブレーション誤差の影響は「画像ヤコビアンのスケーリング」程度に留まります。ピクセル座標で直接特徴量を扱えば、キャリブレーション誤差の影響をさらに小さくできます。これは、IBVS が 3D 復元を経由しないことの直接的な帰結です。
利点 2: 特徴点の画像内保持
IBVS は画像空間上で誤差を減少させるため、特徴点が画像フレームの外に出にくいという性質があります。直感的に言えば、「画像上の点を目標位置に動かす」という制御目標は、点を画像の中に留めておく力学を内在しているのです。
PBVS では、3D 空間での最短経路に従ってカメラが動くため、途中で特徴点が視野から外れてしまうことがあります。特に大きな姿勢変化が必要な場合、この問題は顕著です。特徴点が視野外に出ると追跡が不可能になり、制御が破綻します。
利点 3: 対象物の 3D モデルが不要
PBVS では、画像から 3D 姿勢を復元するために対象物の 3D 形状モデルが必要です。工場のような管理された環境では CAD モデルが利用できますが、宇宙空間のデブリや未探査の小惑星表面では 3D モデルが入手できません。
IBVS は画像上の特徴点の位置だけで動作するため、対象物の 3D モデルを必要としません。目標画像（= 望ましいカメラ位置からの画像）さえあれば、制御を開始できます。
利点 4: 実装の簡便性
IBVS の制御則は擬似逆行列の計算と行列ベクトル積だけで構成されるため、計算コストが低く、実装がシンプルです。リアルタイム性が求められるロボットシステムにおいて、これは大きな利点です。PBVS のように PnP 問題を毎フレーム解く必要がありません。
利点を確認したところで、IBVS の欠点にも目を向けましょう。制御手法の選択は常にトレードオフであり、欠点を正確に理解することが適切な設計判断につながります。
IBVS の欠点
欠点 1: 作業空間での非直線経路
IBVS の最も大きな欠点は、画像空間での直線的な誤差減少が、3D 空間（作業空間）では非直線的な経路になることです。
画像上では特徴点が目標位置に向かって直線的に移動しますが、カメラの 3D 軌道は曲線を描きます。特に大きな回転が必要な場合、カメラは大きく迂回するような軌道を取ることがあります。これは宇宙空間のように推進剤が限られた環境では深刻な問題です。不必要に長い経路は推進剤の浪費を意味し、ミッションの継続性を脅かします。
欠点 2: リトリート問題
IBVS に特有の有名な問題として、リトリート問題（retreat problem） があります。これは、光軸周りの回転が必要な場合に、カメラが一度後退してから前進するという非直感的な挙動を示す現象です。
なぜこれが起きるかを直感的に理解しましょう。カメラが光軸周りに 90° 回転する必要がある場合を考えます。画像上では、特徴点群は回転した位置にあり、目標位置との間に大きなズレがあります。IBVS は画像上でこのズレを減少させようとしますが、画像ヤコビアンにおいて光軸周りの回転 $\omega_z$ と前後方向の並進 $v_z$ は独立に効くため、回転のみを指令するのが理想です。
しかし、回転の途中で特徴点の画像上の配置が変化すると、画像ヤコビアンの擬似逆行列を通じて $v_z$ 成分にも非零の指令が出てしまうことがあります。具体的には、画像上の特徴点配置のスケール（広がり）が目標と異なる場合に $v_z$ が発生し、カメラが後退してしまいます。
数学的には、回転中に特徴点の画像座標が変化すると、画像ヤコビアンのランクや条件数が変化し、擬似逆行列の解が並進成分を含むようになることが原因です。
欠点 3: 局所最小値の存在
画像ヤコビアンは特徴点の位置と奥行きに依存する時変行列であるため、IBVS の閉ループシステムは本質的に非線形です。このため、大域的な収束が保証されず、局所最小値に捕まる可能性があります。
特に、目標姿勢と現在姿勢の差が大きい場合（たとえば 180° に近い回転）、画像ヤコビアンが特異に近くなり、制御が破綻する恐れがあります。
欠点 4: カメラの後退時に奥行きが増大
カメラが対象物から離れる（リトリートする）と、特徴点の奥行き $Z_i$ が増加します。定数近似 $\hat{Z} = Z_0$ を使っている場合、推定誤差がどんどん大きくなり、制御性能が劣化する悪循環に陥る可能性があります。
IBVS の利点と欠点を整理したところで、PBVS と IBVS を包括的に比較し、両者の特性を対照的に理解しましょう。
PBVS vs IBVS の包括的比較
PBVS と IBVS は、ビジュアルサーボの 2 大アプローチとして長年にわたって研究されてきました。以下の表で主要な特性を比較します。
| 比較項目 | PBVS | IBVS |
|---|---|---|
| 制御空間 | 3D 作業空間（SE(3)） | 2D 画像空間 |
| 誤差の定義 | $\bm{e} = (\Delta \bm{t}, \Delta \bm{\theta})$ | $\bm{e} = \bm{s} – \bm{s}^*$ |
| 3D モデル | 必要 | 不要 |
| キャリブレーション感度 | 高い（3D 復元精度に直結） | 低い |
| 作業空間の経路 | 直線 + 測地線（最短） | 非直線（迂回の可能性） |
| 画像空間の経路 | 制御しない（視野逸脱リスク） | 直線的（視野内保持） |
| リトリート問題 | なし | あり（光軸回転時） |
| 局所最小値 | なし（大域安定） | あり（非線形性由来） |
| 奥行き依存 | 3D 復元に必要 | 画像ヤコビアンに必要 |
| 計算コスト | 高い（PnP 問題を毎フレーム解く） | 低い（行列演算のみ） |
| 収束速度 | 速い（直線経路） | 遅い場合あり（迂回経路） |
| 宇宙応用での適性 | モデル既知の衛星ドッキング | モデル未知のデブリ捕獲 |
いつ PBVS を使い、いつ IBVS を使うか
PBVS と IBVS の選択は、ミッション要件とシステム条件に依存します。
PBVS が適している場合: – 対象物の 3D モデルが正確に既知 – カメラキャリブレーションが高精度で行われている – 3D 空間での最短経路が重要（推進剤の節約など） – 大きな姿勢変化を伴うタスク
IBVS が適している場合: – 対象物の 3D モデルが不明または不正確 – カメラキャリブレーションの精度が保証できない – 特徴点を視野内に保持することが最優先 – 初期姿勢と目標姿勢の差が比較的小さい
実際のロボットシステムでは、PBVS と IBVS のどちらか一方だけでは不十分な場合があります。両者の長所を組み合わせたハイブリッドアプローチが研究されています。
ハイブリッドアプローチ: 2-1/2D Visual Servoing
発想
PBVS の「3D 空間での直線経路」と IBVS の「画像空間でのロバスト性」を両立させたいという動機から、2-1/2D Visual Servoing（Malis, Chaumette, Boudet, 1999）が提案されました。「2-1/2D」という名前は、完全な 3D（PBVS）と 2D（IBVS）の中間という意味です。
核心的なアイデアは、制御変数を 画像空間と 3D 空間から半分ずつ 選ぶことです。具体的には、以下のように特徴量を構成します。
$$ \bm{s} = (\bm{x}, \log Z, \bm{\theta u})^\top $$
ここで各成分の意味は以下の通りです。
- $\bm{x} = (x_g, y_g)^\top$ — 対象物の画像上の重心座標（画像空間の特徴量）
- $\log Z$ — 対象物の奥行きの対数（3D 空間の情報）
- $\bm{\theta u} = (\theta u_x, \theta u_y, \theta u_z)^\top$ — 現在姿勢から目標姿勢への回転の角度軸表現（3D 空間の情報）
なぜこの組み合わせが有効か
並進成分（$x_g, y_g, \log Z$）は画像ベースの制御に近く、カメラキャリブレーション誤差にロバストです。一方、回転成分（$\bm{\theta u}$）は 3D 空間で直接制御するため、リトリート問題を回避できます。
特に $\log Z$ を使う理由は巧妙です。カメラが前進すると $Z$ は減少し $\log Z$ も減少する一方、カメラが光軸周りに回転しても $Z$ はほとんど変化しません。つまり、$\log Z$ は並進の $v_z$ 成分を制御するための「純粋な」特徴量として機能し、回転との結合（= リトリート問題の原因）を避けることができるのです。
2-1/2D VS の画像ヤコビアン
この特徴量に対する画像ヤコビアンは次のようなブロック対角的な構造を持ちます。
$$ \bm{L}_s = \begin{pmatrix} \bm{L}_{xy} & \bm{L}_{xy\omega} \\ \bm{L}_{Z} & \bm{L}_{Z\omega} \\ \bm{0}_{3\times 3} & \bm{L}_{\theta u} \end{pmatrix} $$
下段のブロック構造が重要です。回転成分 $\bm{\theta u}$ は並進速度 $(v_x, v_y, v_z)$ に依存しません（左下が零行列）。これにより、並進と回転の制御が部分的に分離され、IBVS のようなカップリング問題が緩和されます。
利点と限界
2-1/2D VS は以下の利点を持ちます。
- IBVS のリトリート問題を回避
- PBVS に比べてキャリブレーション誤差にロバスト
- 並進と回転の制御が部分的に分離される
ただし、ホモグラフィ行列（現在画像と目標画像の間の射影変換）の推定が必要であり、特徴点の対応付けが正確にできないと性能が劣化します。また、$\bm{\theta u}$ の推定にはカメラの内部パラメータの知識が必要であるため、完全にキャリブレーションフリーではありません。
ここまでで理論的な枠組みを一通り解説しました。では、これらの理論を Python で実装し、IBVS の挙動を目で見て確認しましょう。
Python での IBVS シミュレーション
シミュレーションの設定
以下のシミュレーションでは、4 つの特徴点を持つ平面的な対象物に対して IBVS を適用し、カメラが目標位置に収束する過程を可視化します。シミュレーションの設定は以下の通りです。
- 対象物: 3D 空間の $Z = 0$ 平面上に配置された 4 つの点（正方形）
- 初期カメラ位置: 目標位置からずれた位置（並進 + 回転のオフセット）
- 制御則: $\bm{v}_c = -\lambda \hat{\bm{L}}_s^+ (\bm{s} – \bm{s}^*)$
- 画像ヤコビアン: 各ステップで真の奥行き $Z_i$ を使って正確に計算（理想条件）
まず、必要なライブラリのインポートと基本的な関数を定義します。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def rotation_matrix(axis, angle):
"""軸角度表現から回転行列を生成する（ロドリゲスの公式）"""
axis = np.array(axis, dtype=float)
axis = axis / np.linalg.norm(axis)
K = np.array([
[0, -axis[2], axis[1]],
[axis[2], 0, -axis[0]],
[-axis[1], axis[0], 0]
])
R = np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * (K @ K)
return R
def project_points(points_world, R_cw, t_cw):
"""ワールド座標の3D点をカメラの正規化画像座標に射影する"""
points_cam = (R_cw @ points_world.T).T + t_cw.reshape(1, 3)
x = points_cam[:, 0] / points_cam[:, 2]
y = points_cam[:, 1] / points_cam[:, 2]
Z = points_cam[:, 2]
return x, y, Z
def compute_interaction_matrix(x, y, Z):
"""n個の特徴点の画像ヤコビアン（Interaction Matrix）を計算する"""
n = len(x)
L = np.zeros((2 * n, 6))
for i in range(n):
xi, yi, Zi = x[i], y[i], Z[i]
L[2 * i] = [
-1.0 / Zi, 0, xi / Zi,
xi * yi, -(1 + xi**2), yi
]
L[2 * i + 1] = [
0, -1.0 / Zi, yi / Zi,
1 + yi**2, -xi * yi, -xi
]
return L
上のコードでは 3 つの基本関数を定義しました。rotation_matrix はロドリゲスの公式による回転行列の生成、project_points はワールド座標から正規化画像座標への射影、compute_interaction_matrix は前節で導出した画像ヤコビアンの構築をそれぞれ行います。特に compute_interaction_matrix は、各特徴点について $2 \times 6$ の小行列を縦に積み重ねる処理であり、先ほどの数式をそのまま実装したものになっています。
次に、カメラの速度をカメラの姿勢に反映するための更新関数を定義します。
def velocity_to_pose_update(v_c, dt):
"""カメラ速度ベクトルからポーズの微小更新量を計算する"""
v = v_c[:3] # 並進速度
omega = v_c[3:] # 角速度
dt_trans = v * dt # 並進の微小変化
angle = np.linalg.norm(omega) * dt
if angle < 1e-10:
dR = np.eye(3)
else:
axis = omega / np.linalg.norm(omega)
dR = rotation_matrix(axis, angle)
return dt_trans, dR
def update_camera_pose(R_cw, t_cw, v_c, dt):
"""カメラの速度指令に基づいてカメラポーズを更新する
v_c はカメラフレームでの速度なので、
カメラフレーム内で並進・回転を適用する
"""
dt_trans, dR = velocity_to_pose_update(v_c, dt)
# カメラフレーム内での並進を適用
t_cw_new = t_cw + R_cw.T @ dt_trans
# カメラフレーム内での回転を適用
R_cw_new = dR @ R_cw
return R_cw_new, t_cw_new
ここでのポイントは、速度指令 $\bm{v}_c$ がカメラフレームで定義されていることです。並進はカメラの向きに沿って行われるため、ワールド座標系への変換 $\bm{R}_{cw}^\top$ を使って座標変換しています。回転は現在の回転行列に左から微小回転行列 $\Delta \bm{R}$ を掛けることで更新します。
メインシミュレーション
いよいよ、IBVS 制御ループのメインシミュレーションを実装します。
# === 対象物の3D座標（ワールドフレーム、Z=0平面上の正方形） ===
half_size = 0.25 # 正方形の半辺長 [m]
points_world = np.array([
[-half_size, -half_size, 0.0],
[ half_size, -half_size, 0.0],
[ half_size, half_size, 0.0],
[-half_size, half_size, 0.0],
])
# === 目標カメラポーズ（対象物正面、距離1m） ===
R_desired = np.eye(3)
t_desired = np.array([0.0, 0.0, 1.0])
# 目標画像特徴量を計算
x_star, y_star, _ = project_points(points_world, R_desired, t_desired)
s_star = np.zeros(8)
for i in range(4):
s_star[2 * i] = x_star[i]
s_star[2 * i + 1] = y_star[i]
# === 初期カメラポーズ（目標からずれた位置） ===
R_init = rotation_matrix([0, 0, 1], np.radians(25)) # Z軸周り25°回転
R_init = R_init @ rotation_matrix([1, 0, 0], np.radians(10)) # X軸周り10°回転
t_init = np.array([0.15, -0.10, 1.5])
# === IBVSパラメータ ===
lam = 0.7 # 制御ゲイン
dt = 0.05 # タイムステップ [s]
max_iter = 300 # 最大反復数
tol = 1e-6 # 収束閾値
上のコードでは、対象物を原点付近の $0.5 \times 0.5$ m の正方形として定義し、目標カメラ位置を「正方形の正面 1m」に設定しています。初期カメラ位置は、目標から Z 軸周り 25 度、X 軸周り 10 度回転し、0.5m 遠い位置にずらしています。これは、IBVS が並進と回転の両方を同時に修正する能力をテストするための設定です。
# === IBVSシミュレーションループ ===
R_cw = R_init.copy()
t_cw = t_init.copy()
# 記録用
errors = []
velocities = []
cam_positions = []
image_trajectories = [[] for _ in range(4)]
for iteration in range(max_iter):
# 現在の画像特徴量を計算
x_cur, y_cur, Z_cur = project_points(points_world, R_cw, t_cw)
s_cur = np.zeros(8)
for i in range(4):
s_cur[2 * i] = x_cur[i]
s_cur[2 * i + 1] = y_cur[i]
# 画像特徴量の軌跡を記録
for i in range(4):
image_trajectories[i].append((x_cur[i], y_cur[i]))
# 誤差を計算
e = s_cur - s_star
error_norm = np.linalg.norm(e)
errors.append(error_norm)
# カメラ位置を記録（ワールド座標系）
cam_pos_world = -R_cw.T @ t_cw
cam_positions.append(cam_pos_world.copy())
# 収束判定
if error_norm < tol:
print(f"収束しました（反復 {iteration}回、誤差 {error_norm:.2e}）")
break
# 画像ヤコビアンを計算（真の奥行きを使用）
L = compute_interaction_matrix(x_cur, y_cur, Z_cur)
# 擬似逆行列を計算
L_pinv = np.linalg.pinv(L)
# IBVS制御則
v_c = -lam * L_pinv @ e
velocities.append(v_c.copy())
# カメラポーズを更新
R_cw, t_cw = update_camera_pose(R_cw, t_cw, v_c, dt)
errors = np.array(errors)
velocities = np.array(velocities)
cam_positions = np.array(cam_positions)
このコードが IBVS の核心部分です。各反復で、(1) 現在の画像特徴量を計算、(2) 誤差を求める、(3) 画像ヤコビアンを構築、(4) 擬似逆行列で制御速度を計算、(5) カメラ姿勢を更新、というサイクルを繰り返します。制御則 $\bm{v}_c = -\lambda \hat{\bm{L}}_s^+ (\bm{s} – \bm{s}^*)$ がわずか 1 行（v_c = -lam * L_pinv @ e）で実装されている点に注目してください。
結果の可視化
シミュレーション結果を 4 つの視点から可視化しましょう。
fig, axes = plt.subplots(2, 2, figsize=(14, 11))
# (1) 画像空間での特徴点の軌跡
ax1 = axes[0, 0]
colors = ['#00bcd4', '#ff9800', '#4caf50', '#e91e63']
labels = ['Point 1', 'Point 2', 'Point 3', 'Point 4']
for i in range(4):
traj = np.array(image_trajectories[i])
ax1.plot(traj[:, 0], traj[:, 1], '-', color=colors[i],
alpha=0.7, linewidth=1.5, label=labels[i])
ax1.plot(traj[0, 0], traj[0, 1], 'o', color=colors[i], markersize=10)
ax1.plot(traj[-1, 0], traj[-1, 1], 's', color=colors[i], markersize=10)
# 目標位置を×で表示
for i in range(4):
ax1.plot(s_star[2 * i], s_star[2 * i + 1], 'x', color=colors[i],
markersize=14, markeredgewidth=3)
ax1.set_xlabel('x (normalized)', fontsize=12)
ax1.set_ylabel('y (normalized)', fontsize=12)
ax1.set_title('Image-Space Feature Trajectories', fontsize=13)
ax1.legend(fontsize=9)
ax1.grid(True, alpha=0.3)
ax1.set_aspect('equal')
# (2) 誤差のノルムの推移
ax2 = axes[0, 1]
ax2.semilogy(errors, color='#00bcd4', linewidth=2)
ax2.set_xlabel('Iteration', fontsize=12)
ax2.set_ylabel('||e|| (log scale)', fontsize=12)
ax2.set_title('Feature Error Norm', fontsize=13)
ax2.grid(True, alpha=0.3)
# (3) カメラ速度の推移
ax3 = axes[1, 0]
vel_labels = ['$v_x$', '$v_y$', '$v_z$', '$\\omega_x$', '$\\omega_y$', '$\\omega_z$']
vel_colors = ['#e91e63', '#ff9800', '#4caf50', '#2196f3', '#9c27b0', '#795548']
for j in range(6):
ax3.plot(velocities[:, j], color=vel_colors[j], linewidth=1.5,
label=vel_labels[j], alpha=0.8)
ax3.set_xlabel('Iteration', fontsize=12)
ax3.set_ylabel('Camera velocity', fontsize=12)
ax3.set_title('Camera Velocity Commands', fontsize=13)
ax3.legend(fontsize=9, ncol=2)
ax3.grid(True, alpha=0.3)
# (4) 3Dカメラ軌道
ax4 = fig.add_subplot(2, 2, 4, projection='3d')
ax4.plot(cam_positions[:, 0], cam_positions[:, 1], cam_positions[:, 2],
'o-', color='#00bcd4', markersize=2, linewidth=1.5, label='Camera path')
ax4.plot(*cam_positions[0], 'o', color='#e91e63', markersize=10, label='Start')
ax4.plot(*cam_positions[-1], 's', color='#4caf50', markersize=10, label='Goal')
# 対象物の正方形を表示
sq = np.vstack([points_world, points_world[0]])
ax4.plot(sq[:, 0], sq[:, 1], sq[:, 2], 'k-', linewidth=2, label='Target object')
ax4.set_xlabel('X [m]', fontsize=10)
ax4.set_ylabel('Y [m]', fontsize=10)
ax4.set_zlabel('Z [m]', fontsize=10)
ax4.set_title('3D Camera Trajectory', fontsize=13)
ax4.legend(fontsize=9)
plt.tight_layout()
plt.savefig('ibvs_simulation.png', dpi=150, bbox_inches='tight')
plt.show()
このコードを実行すると 4 つのグラフが得られます。
左上: 画像空間での特徴点軌跡 — 4 つの特徴点が初期位置（丸）から目標位置（バツ）に向かって移動する様子が描かれます。IBVS の特徴として、画像空間では比較的直線的な軌跡を描くことが確認できます。各色が 4 つの特徴点に対応しており、全ての点が最終的に目標位置に収束していることが見て取れます。
右上: 誤差ノルムの推移 — 対数スケールで誤差が指数的に減衰していることが確認できます。ゲイン $\lambda = 0.7$ の設定で、約 100–150 回の反復で誤差が $10^{-6}$ 以下に到達しています。対数スケールで直線的に減少していることは、理論通り $\|\bm{e}(t)\| \propto \exp(-\lambda t)$ の指数減衰が実現していることを示しています。
左下: カメラ速度指令の推移 — 6 成分の速度指令が時間とともに 0 に収束する様子が見られます。初期段階では並進速度と角速度の両方が大きな値をとり、カメラが急速に動いていることがわかります。特に $\omega_z$（光軸周りの回転）成分が目立つのは、初期ポーズに Z 軸周り 25 度の回転を含めたためです。
右下: 3D カメラ軌道 — カメラの 3D 空間での移動経路が表示されます。開始位置（赤丸）からゴール（緑四角）に向かう軌道が曲線を描いていることが確認できるはずです。これが IBVS の欠点として述べた「作業空間での非直線経路」の視覚的な確認です。PBVS であればこの軌道は直線になりますが、IBVS では画像空間での最適化の結果として、3D 空間では迂回する経路が生じています。
奥行き推定誤差の影響実験
次に、奥行き推定が不正確な場合に IBVS がどの程度ロバストに動作するかを実験します。奥行きの真値に対して $\hat{Z} = \alpha Z$（$\alpha = 0.5, 1.0, 2.0, 5.0$）と定数倍のバイアスを与え、収束性を比較します。
import numpy as np
import matplotlib.pyplot as plt
def run_ibvs_with_depth_error(alpha, points_world, R_desired, t_desired,
R_init, t_init, lam=0.7, dt=0.05,
max_iter=500, tol=1e-6):
"""奥行き推定にバイアスalpha を掛けたIBVSシミュレーション"""
# 目標特徴量
x_star, y_star, _ = project_points(points_world, R_desired, t_desired)
s_star = np.zeros(8)
for i in range(4):
s_star[2 * i] = x_star[i]
s_star[2 * i + 1] = y_star[i]
R_cw = R_init.copy()
t_cw = t_init.copy()
errors = []
for iteration in range(max_iter):
x_cur, y_cur, Z_cur = project_points(points_world, R_cw, t_cw)
s_cur = np.zeros(8)
for i in range(4):
s_cur[2 * i] = x_cur[i]
s_cur[2 * i + 1] = y_cur[i]
e = s_cur - s_star
error_norm = np.linalg.norm(e)
errors.append(error_norm)
if error_norm < tol:
break
# 奥行きにバイアスを掛ける
Z_estimated = alpha * Z_cur
L = compute_interaction_matrix(x_cur, y_cur, Z_estimated)
L_pinv = np.linalg.pinv(L)
v_c = -lam * L_pinv @ e
R_cw, t_cw = update_camera_pose(R_cw, t_cw, v_c, dt)
return errors
# 複数の奥行きバイアスで実験
alphas = [0.5, 1.0, 2.0, 5.0]
alpha_labels = ['$\\hat{Z} = 0.5 Z$ (過小)', '$\\hat{Z} = Z$ (正確)',
'$\\hat{Z} = 2Z$ (2倍過大)', '$\\hat{Z} = 5Z$ (5倍過大)']
alpha_colors = ['#e91e63', '#4caf50', '#2196f3', '#ff9800']
fig, ax = plt.subplots(figsize=(10, 6))
for alpha, label, color in zip(alphas, alpha_labels, alpha_colors):
errs = run_ibvs_with_depth_error(
alpha, points_world, R_desired, t_desired,
R_init, t_init, lam=0.7, dt=0.05, max_iter=500
)
ax.semilogy(errs, color=color, linewidth=2, label=label)
ax.set_xlabel('Iteration', fontsize=12)
ax.set_ylabel('||e|| (log scale)', fontsize=12)
ax.set_title('IBVS Robustness to Depth Estimation Error', fontsize=13)
ax.legend(fontsize=11)
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('ibvs_depth_robustness.png', dpi=150, bbox_inches='tight')
plt.show()
このグラフからは、IBVS の奥行き推定誤差に対するロバスト性が明確に読み取れます。
- $\alpha = 1.0$（正確な奥行き） — 最も速い収束を示し、理論通りの指数減衰を実現しています
- $\alpha = 0.5$（半分に過小評価） — 制御ゲインが実質的に 2 倍になるため、やや振動的ですが収束します。過小評価はオーバーシュートのリスクがあります
- $\alpha = 2.0$（2 倍に過大評価） — 制御ゲインが実質的に半分になるため、収束は遅くなりますが安定です。過大評価は保守的な制御になります
- $\alpha = 5.0$（5 倍に過大評価） — 収束がかなり遅くなりますが、それでも最終的には収束しています。これは IBVS のフィードバック特性の強さを示しています
つまり、奥行きが 5 倍もずれていても制御が収束するという結果は、IBVS が「大まかな奥行きの推定さえあれば動作する」という冒頭の主張を裏付けています。ただし、過小評価の方が過大評価よりもリスクが高いことにも注意が必要です。
リトリート問題の再現実験
最後に、IBVS のリトリート問題を再現する実験を行います。光軸周りに大きな回転（90 度）が必要なケースを設定し、カメラの前後方向の移動を観察します。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# リトリート問題が起きやすい設定: 光軸周りに90°回転
R_retreat_init = rotation_matrix([0, 0, 1], np.radians(90))
t_retreat_init = np.array([0.0, 0.0, 1.0]) # 目標と同じ距離
# 目標ポーズ
R_retreat_desired = np.eye(3)
t_retreat_desired = np.array([0.0, 0.0, 1.0])
# 目標特徴量
x_star_r, y_star_r, _ = project_points(
points_world, R_retreat_desired, t_retreat_desired
)
s_star_r = np.zeros(8)
for i in range(4):
s_star_r[2 * i] = x_star_r[i]
s_star_r[2 * i + 1] = y_star_r[i]
# IBVSシミュレーション
R_cw = R_retreat_init.copy()
t_cw = t_retreat_init.copy()
cam_z_history = [] # カメラのZ座標（対象物からの距離）
errors_retreat = []
image_traj_retreat = [[] for _ in range(4)]
for iteration in range(600):
x_cur, y_cur, Z_cur = project_points(points_world, R_cw, t_cw)
s_cur = np.zeros(8)
for i in range(4):
s_cur[2 * i] = x_cur[i]
s_cur[2 * i + 1] = y_cur[i]
image_traj_retreat[i].append((x_cur[i], y_cur[i]))
e = s_cur - s_star_r
error_norm = np.linalg.norm(e)
errors_retreat.append(error_norm)
# カメラ位置のZ成分（対象物からの距離に相当）
cam_pos = -R_cw.T @ t_cw
cam_z_history.append(t_cw[2]) # カメラフレームでのZ = 対象物までの距離
if error_norm < 1e-6:
print(f"リトリート実験: 収束（反復 {iteration}回）")
break
L = compute_interaction_matrix(x_cur, y_cur, Z_cur)
L_pinv = np.linalg.pinv(L)
v_c = -0.5 * L_pinv @ e
R_cw, t_cw = update_camera_pose(R_cw, t_cw, v_c, dt)
cam_z_history = np.array(cam_z_history)
# 可視化
fig, axes = plt.subplots(1, 3, figsize=(18, 5))
# (1) 画像空間での特徴点軌跡
ax1 = axes[0]
for i in range(4):
traj = np.array(image_traj_retreat[i])
ax1.plot(traj[:, 0], traj[:, 1], '-', color=colors[i],
alpha=0.7, linewidth=1.5, label=labels[i])
ax1.plot(traj[0, 0], traj[0, 1], 'o', color=colors[i], markersize=10)
ax1.plot(traj[-1, 0], traj[-1, 1], 's', color=colors[i], markersize=10)
for i in range(4):
ax1.plot(s_star_r[2 * i], s_star_r[2 * i + 1], 'x',
color=colors[i], markersize=14, markeredgewidth=3)
ax1.set_xlabel('x (normalized)', fontsize=12)
ax1.set_ylabel('y (normalized)', fontsize=12)
ax1.set_title('Image-Space Trajectories (90° rotation)', fontsize=13)
ax1.legend(fontsize=9)
ax1.grid(True, alpha=0.3)
ax1.set_aspect('equal')
# (2) カメラの奥行き（対象物までの距離）の推移
ax2 = axes[1]
ax2.plot(cam_z_history, color='#e91e63', linewidth=2)
ax2.axhline(y=1.0, color='#4caf50', linestyle='--', linewidth=1.5,
label='Target distance (1.0 m)')
ax2.set_xlabel('Iteration', fontsize=12)
ax2.set_ylabel('Distance to target [m]', fontsize=12)
ax2.set_title('Camera Distance (Retreat Problem)', fontsize=13)
ax2.legend(fontsize=11)
ax2.grid(True, alpha=0.3)
# (3) 誤差の推移
ax3 = axes[2]
ax3.semilogy(errors_retreat, color='#2196f3', linewidth=2)
ax3.set_xlabel('Iteration', fontsize=12)
ax3.set_ylabel('||e|| (log scale)', fontsize=12)
ax3.set_title('Feature Error Norm (90° rotation)', fontsize=13)
ax3.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('ibvs_retreat_problem.png', dpi=150, bbox_inches='tight')
plt.show()
リトリート問題の実験結果からは、以下の重要な特徴が読み取れます。
左図（画像空間の軌跡） — 90 度の光軸回転が必要なため、画像上の特徴点は大きな弧を描いて移動しています。特徴点が画像空間内に留まっていることは IBVS の利点ですが、その軌道は複雑です。
中央図（カメラ距離の推移） — ここがリトリート問題の核心です。初期距離と目標距離は同じ 1.0m ですが、制御の途中でカメラが一度対象物から離れる（後退する = retreat）挙動が観察されます。距離が一時的に増加してから、最終的に 1.0m に戻ります。3D 空間で無駄な後退が発生しており、推進剤が限られた宇宙ミッションでは大きな問題です。
右図（誤差の推移） — 誤差の減少が純粋な指数減衰ではなく、途中で減衰が鈍化するプラトー領域が見られます。これはリトリート中に画像ヤコビアンの条件が変化し、制御効率が一時的に低下するためです。
まとめ
本記事では、画像ベースビジュアルサーボ（IBVS）について、理論から実装まで解説しました。
- IBVS の基本戦略: 3D 復元を行わず、画像上の特徴量誤差 $\bm{e} = \bm{s} – \bm{s}^*$ を直接最小化する制御手法です
- 画像ヤコビアン $\bm{L}_s$: カメラの 6 自由度速度と画像上の特徴量変化を結ぶ行列であり、並進成分は奥行き $Z$ に依存し、回転成分は $Z$ に依存しないという構造を持ちます
- 制御則: $\bm{v}_c = -\lambda \hat{\bm{L}}_s^+ (\bm{s} – \bm{s}^*)$ は擬似逆行列による最小二乗解であり、画像誤差を指数的に減衰させます
- 奥行き推定: IBVS は奥行きの正確な値を必要とせず、5 倍の推定誤差があっても収束するロバスト性を持ちます
- 利点と欠点: カメラキャリブレーション誤差へのロバスト性と視野内保持が主要な利点である一方、非直線経路とリトリート問題が欠点です
- PBVS との比較: 3D モデルの有無、キャリブレーション精度、経路効率などの観点から使い分けが必要です
- 2-1/2D VS: 画像空間と 3D 空間の特徴量を組み合わせることで、両方の利点を取り込むハイブリッドアプローチです
IBVS は、宇宙空間でのデブリ捕獲やランデブー・ドッキングにおいて、対象物の 3D モデルが不明な状況で特に力を発揮します。一方で、経路効率が重要な場面では PBVS や 2-1/2D VS との併用が有効です。
次のステップとして、以下の記事も参考にしてください。
- 宇宙ビジュアル誘導と姿勢推定 — 宇宙環境に特化した視覚誘導と、ランデブー・ドッキングにおけるポーズ推定技術