宇宙空間でロボットアームが故障した衛星を掴もうとしている場面を想像してください。カメラに映った対象物を見ながら「あと何センチ右へ」「もう少し手首を回転させて」と手先を誘導したい — このとき、カメラ画像から対象物の3次元的な位置と姿勢をまず復元し、作業空間上の誤差をゼロにするように制御するのが位置ベースビジュアルサーボ（Position-Based Visual Servoing, PBVS）です。
PBVSは以下のような場面で威力を発揮します。
- 軌道上サービス（On-Orbit Servicing）: 故障衛星や宇宙デブリに対して、ロボットアームで捕獲・修理を行う際に、カメラ映像から対象の3Dポーズをリアルタイムに推定して接近制御を行います。
- 惑星探査ローバーのマニピュレータ制御: 岩石サンプルの採取や科学機器の設置で、対象物の位置と姿勢を画像から推定し、正確にアームを誘導します。
- 産業用ロボットのピック＆プレース: 工場のラインを流れる部品をカメラで認識し、3D位置を推定してロボットハンドで正確に把持します。
本記事の内容
- PBVSの基本戦略と処理パイプライン
- PnP問題（Perspective-n-Point）の数理 — カメラポーズ推定の核心
- P3PとEPnPアルゴリズムの概要
- 作業空間での制御則の導出
- 姿勢誤差の表現（回転行列からaxis-angleへ）
- PBVSの利点・欠点とIBVSとの比較
- Pythonによる3D PBVSシミュレーション
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
- カメラモデルと画像ヤコビアン — ピンホールカメラモデル、カメラ内部パラメータ、画像座標変換の基礎
PBVSの基本戦略 — 「見て、測って、動かす」
2つのビジュアルサーボ
ビジュアルサーボは大きく2つの方式に分かれます。どちらもカメラ画像をフィードバックに使う点では同じですが、何を制御変数にするかが根本的に異なります。
- 位置ベースビジュアルサーボ（PBVS）: 画像から3Dポーズを推定し、作業空間（3D空間）で誤差を定義して制御する
- 画像ベースビジュアルサーボ（IBVS）: 画像上の特徴点座標を直接使い、画像空間（2D空間）で誤差を定義して制御する
イメージとしては、PBVSは「カメラ画像を立体地図に変換してからナビゲーションする」方式、IBVSは「カメラ画像の見え方そのものを頼りにナビゲーションする」方式です。前者は地図の正確さに依存しますが直感的な経路を生成でき、後者は地図が不要ですが経路が予測しにくくなることがあります。
PBVSの処理パイプライン
PBVSの処理は以下の3ステップで構成されます。
ステップ1: 画像特徴の抽出
カメラが撮影した画像から、対象物上の特徴点（マーカー、コーナー、エッジなど）を検出します。特徴点の画像座標 $(u_i, v_i)$ を得ます。
ステップ2: 3Dポーズ推定（PnP問題を解く）
検出した特徴点の画像座標と、対象物上での既知の3D座標との対応関係を使って、カメラから見た対象物の位置 $\bm{t}$ と姿勢 $\bm{R}$ を推定します。これがPnP問題（Perspective-n-Point）と呼ばれる、PBVSの心臓部です。
ステップ3: 作業空間での制御
推定した現在のポーズ $(\bm{R}, \bm{t})$ と目標ポーズ $(\bm{R}^*, \bm{t}^*)$ の差（誤差）を計算し、その誤差をゼロにする速度指令をロボットに送ります。
この3ステップを毎フレーム繰り返すことで、ロボットは対象物に向かって収束していきます。数式で書くと、制御ループは以下の流れです。
$$ \text{画像} \xrightarrow{\text{特徴検出}} (u_i, v_i) \xrightarrow{\text{PnP}} (\bm{R}, \bm{t}) \xrightarrow{\text{制御則}} \bm{v}_c \xrightarrow{\text{ロボット}} \text{移動} $$
ここで $\bm{v}_c$ はカメラ（エンドエフェクタ）の速度指令です。
処理パイプラインの全体像が見えたところで、PBVSの最も技術的に重要なパーツである「PnP問題」の数理に踏み込みましょう。
PnP問題 — 画像からカメラの位置・姿勢を復元する
PnP問題とは何か
PnP問題を日常的な場面で説明してみます。あなたが写真を1枚見せられて、「この写真はどこに立って、どの方向を向いて撮ったか？」と聞かれたとします。写真に写っている建物や目印の実際の位置（3D座標）がわかっていれば、写真上でのそれらの位置（2D座標）と照らし合わせて、カメラの位置と向きを逆算できます。これがまさにPnP問題です。
数学的に定式化します。対象物上の $n$ 個の点の3D座標が既知であるとします。
$$ \bm{P}_i = \begin{pmatrix} X_i \\ Y_i \\ Z_i \end{pmatrix}, \quad i = 1, 2, \dots, n $$
これらの点がカメラに撮影され、画像上で以下の2D座標として観測されます。
$$ \bm{p}_i = \begin{pmatrix} u_i \\ v_i \end{pmatrix}, \quad i = 1, 2, \dots, n $$
ピンホールカメラモデルにより、3D点とその画像投影の関係は次式で表されます。
$$ s_i \begin{pmatrix} u_i \\ v_i \\ 1 \end{pmatrix} = \bm{K} \begin{pmatrix} \bm{R} & \bm{t} \end{pmatrix} \begin{pmatrix} X_i \\ Y_i \\ Z_i \\ 1 \end{pmatrix} $$
ここで $\bm{K}$ はカメラ内部パラメータ行列（焦点距離や主点を含む）、$\bm{R} \in SO(3)$ はカメラの回転行列、$\bm{t} \in \mathbb{R}^3$ はカメラの並進ベクトル、$s_i$ はスケール因子です。
PnP問題の目標: $n$ 組の対応 $(\bm{P}_i, \bm{p}_i)$ とカメラ内部パラメータ $\bm{K}$ が既知のとき、$\bm{R}$ と $\bm{t}$ を求めよ。
この問題を解くには最低何点の対応が必要でしょうか？ 回転行列は3自由度（回転軸2つ + 回転角1つ）、並進ベクトルは3自由度で、合計6自由度を求める必要があります。1点の対応から2つの方程式（$u$ と $v$）が得られるので、最低3点（= 6方程式）あれば理論的には解けます。ただし、3点の場合は最大4つの解が得られる（多義性がある）ため、実用上は4点以上を使うのが一般的です。
P3P — 3点から最大4解を求める
P3P（Perspective-3-Point）は、3組の対応点からカメラポーズを求める最小構成の手法です。3点しか使わないため計算が高速ですが、最大4つの解が出るため、追加の情報（4点目や幾何学的制約）で一意に絞り込む必要があります。
P3Pの幾何学的な直感を説明します。カメラの光学中心を $O$、3つの3D点を $\bm{P}_1, \bm{P}_2, \bm{P}_3$ とします。画像上の投影 $\bm{p}_1, \bm{p}_2, \bm{p}_3$ から、カメラから各点への方向ベクトル（視線方向）がわかります。
$$ \bm{d}_i = \bm{K}^{-1} \begin{pmatrix} u_i \\ v_i \\ 1 \end{pmatrix}, \quad \hat{\bm{d}}_i = \frac{\bm{d}_i}{\|\bm{d}_i\|} $$
方向ベクトル間の角度からコサインを計算します。
$$ \cos \alpha_{jk} = \hat{\bm{d}}_j \cdot \hat{\bm{d}}_k $$
ここで $\alpha_{jk}$ はカメラ中心 $O$ から見た $\bm{P}_j$ と $\bm{P}_k$ の挟角です。
一方、3D空間での点間距離 $d_{jk} = \|\bm{P}_j – \bm{P}_k\|$ は既知です。カメラ中心から各点までの距離を $r_1, r_2, r_3$ とおくと、余弦定理により次の3本の方程式が立ちます。
$$ d_{12}^2 = r_1^2 + r_2^2 – 2 r_1 r_2 \cos \alpha_{12} $$
$$ d_{13}^2 = r_1^2 + r_3^2 – 2 r_1 r_3 \cos \alpha_{13} $$
$$ d_{23}^2 = r_2^2 + r_3^2 – 2 r_2 r_3 \cos \alpha_{23} $$
これは $r_1, r_2, r_3$ に関する3元連立方程式です。変数置換 $x = r_1 / r_2$, $y = r_1 / r_3$ を行うと、$r_1$ を消去でき、最終的に4次方程式に帰着します。4次方程式は最大4つの実数解を持つため、P3Pは最大4つのカメラポーズ候補を返します。
4つの候補から正しい解を選ぶには、4点目の対応点を使って再投影誤差が最小となる候補を選ぶのが一般的です。この方法をP3P + 1点検証と呼びます。RANSAC（ランダムサンプル合意法）と組み合わせることで、ノイズや外れ値が含まれるデータにも頑健に対応できます。
EPnP — 効率的な多点ポーズ推定
P3Pは最小構成で高速ですが、多数の対応点を使って精度を上げたい場合はどうすればよいでしょうか。素朴に全点を使う手法は計算コストが $O(n)$ を超えてしまいますが、EPnP（Efficient PnP）は $O(n)$ の計算量で $n$ 点すべてを活用する優れたアルゴリズムです。
EPnPの核心的なアイデアは、$n$ 個の3D点を4つの仮想制御点（virtual control points）の重み付き線形結合で表現することです。
$$ \bm{P}_i = \sum_{j=1}^{4} \alpha_{ij} \bm{C}_j, \quad \sum_{j=1}^{4} \alpha_{ij} = 1 $$
ここで $\bm{C}_j$ は4つの制御点、$\alpha_{ij}$ は重心座標（barycentric coordinates）です。$n$ 個の3D点の代わりに、4つの制御点のカメラ座標 $\bm{C}_j^c$（合計12個の未知数）だけを求めれば十分になります。
ピンホールカメラモデルに代入して整理すると、以下の線形方程式系が得られます。
$$ \bm{M} \bm{x} = \bm{0} $$
ここで $\bm{x} = (\bm{C}_1^{c\top}, \bm{C}_2^{c\top}, \bm{C}_3^{c\top}, \bm{C}_4^{c\top})^\top \in \mathbb{R}^{12}$ は制御点のカメラ座標を並べたベクトル、$\bm{M}$ は $2n \times 12$ の係数行列です。
$\bm{M}^\top \bm{M}$ の固有値分解（または $\bm{M}$ のSVD）により、$\bm{M}$ の零空間の基底ベクトル $\bm{v}_1, \bm{v}_2, \dots$ を求めます。解は次の形で表されます。
$$ \bm{x} = \sum_{k=1}^{N} \beta_k \bm{v}_k $$
$N = 1, 2, 3, 4$ のケースそれぞれで、制御点間の距離制約（剛体制約）
$$ \|\bm{C}_i^c – \bm{C}_j^c\|^2 = \|\bm{C}_i – \bm{C}_j\|^2 $$
を使って $\beta_k$ を決定します。最も再投影誤差が小さいケースが最終解となります。
EPnPの大きな利点は、$\bm{M}$ の構成が $O(n)$ で済むこと、そして固有値分解は $12 \times 12$ の小さな行列に対して行うため $n$ に依存しないことです。これにより、対応点が数百個あっても実時間で解けます。
PnP問題を解いてカメラの3Dポーズが得られたら、次は「現在のポーズ」と「目標のポーズ」の差をどう制御するかが問題になります。ここからPBVSの制御則を導出しましょう。
PBVSの制御則 — 作業空間でのフィードバック制御
位置誤差の定義
PBVSの制御則を導くために、まず誤差を定義します。現在のカメラポーズと目標ポーズが、それぞれあるワールド座標系に対して以下のように表されるとします。
- 現在のポーズ: カメラ座標系 $\{C\}$ からワールド座標系 $\{W\}$ への変換 $(\bm{R}_c, \bm{t}_c)$
- 目標ポーズ: 目標カメラ座標系 $\{C^*\}$ からワールド座標系 $\{W\}$ への変換 $(\bm{R}^*, \bm{t}^*)$
PBVSでは、目標座標系 $\{C^*\}$ から見た現在のカメラ座標系 $\{C\}$ の相対ポーズとして誤差を定義するのが自然です。
並進誤差は、目標座標系で表した位置のずれです。
$$ \bm{e}_t = \bm{t}_c – \bm{t}^* $$
ここでの $\bm{e}_t$ は、カメラが目標位置からどれだけずれているかを表す3次元ベクトルです。直感的には「目的地までの残り距離と方向」を意味します。
回転誤差の定義は少し工夫が必要です。目標姿勢に対する現在の姿勢のずれを、相対回転行列として表します。
$$ \bm{R}_e = \bm{R}_c (\bm{R}^*)^\top $$
$\bm{R}_e = \bm{I}$（単位行列）であれば姿勢の誤差がゼロ、つまり目標姿勢に一致していることを意味します。しかし回転行列は $3 \times 3$ の行列であり、そのままでは制御則に組み込みにくいため、より扱いやすい表現に変換する必要があります。
姿勢誤差の表現 — 回転行列からaxis-angle表現へ
回転行列を制御に使いやすい形に変換する方法はいくつかありますが、PBVSで最も広く使われるのがaxis-angle表現（回転軸・回転角表現）です。
任意の回転行列 $\bm{R}_e \in SO(3)$ は、ある軸 $\hat{\bm{u}}$（単位ベクトル）周りの角度 $\theta$ の回転として表現できます。これはオイラーの回転定理として知られています。
回転角 $\theta$ は回転行列のトレースから求められます。回転行列のトレースと回転角の関係式を導出してみましょう。回転行列のトレースは以下で与えられます。
$$ \text{tr}(\bm{R}_e) = 1 + 2\cos\theta $$
この式は、回転行列を軸 $\hat{\bm{u}}$ 周りの回転として表すロドリゲスの回転公式から導かれます。ロドリゲスの回転公式は次の通りです。
$$ \bm{R}_e = \bm{I} + \sin\theta \, [\hat{\bm{u}}]_\times + (1 – \cos\theta) \, [\hat{\bm{u}}]_\times^2 $$
ここで $[\hat{\bm{u}}]_\times$ は $\hat{\bm{u}} = (u_1, u_2, u_3)^\top$ の歪対称行列（スキューシンメトリック行列）です。
$$ [\hat{\bm{u}}]_\times = \begin{pmatrix} 0 & -u_3 & u_2 \\ u_3 & 0 & -u_1 \\ -u_2 & u_1 & 0 \end{pmatrix} $$
ロドリゲスの公式のトレースを計算します。まず $\text{tr}(\bm{I}) = 3$ です。次に $[\hat{\bm{u}}]_\times$ の対角成分はすべて0なので $\text{tr}([\hat{\bm{u}}]_\times) = 0$ です。$[\hat{\bm{u}}]_\times^2$ のトレースを計算すると、
$$ [\hat{\bm{u}}]_\times^2 = \hat{\bm{u}} \hat{\bm{u}}^\top – \bm{I} $$
であることから（これは $\|\hat{\bm{u}}\| = 1$ のとき成り立つ公式です）、
$$ \text{tr}([\hat{\bm{u}}]_\times^2) = \text{tr}(\hat{\bm{u}} \hat{\bm{u}}^\top) – \text{tr}(\bm{I}) = \|\hat{\bm{u}}\|^2 – 3 = 1 – 3 = -2 $$
これらを代入すると、
$$ \text{tr}(\bm{R}_e) = 3 + 0 + (1 – \cos\theta)(-2) = 3 – 2 + 2\cos\theta = 1 + 2\cos\theta $$
したがって、回転角は次式で求められます。
$$ \theta = \arccos\left(\frac{\text{tr}(\bm{R}_e) – 1}{2}\right) $$
回転軸 $\hat{\bm{u}}$ は、$\bm{R}_e$ の反対称成分から抽出できます。ロドリゲスの公式から、
$$ \bm{R}_e – \bm{R}_e^\top = 2\sin\theta \, [\hat{\bm{u}}]_\times $$
$\sin\theta \neq 0$ の場合、右辺の歪対称行列から回転軸の各成分を読み取れます。
$$ \hat{\bm{u}} = \frac{1}{2\sin\theta} \begin{pmatrix} R_{32} – R_{23} \\ R_{13} – R_{31} \\ R_{21} – R_{12} \end{pmatrix} $$
ここで $R_{ij}$ は $\bm{R}_e$ の $(i,j)$ 成分です。
axis-angle表現による姿勢誤差ベクトルは次のように定義されます。
$$ \bm{e}_\theta = \theta \hat{\bm{u}} $$
このベクトルの方向が回転軸、大きさが回転角を表します。$\bm{e}_\theta = \bm{0}$ であれば姿勢誤差ゼロです。この表現の優れた点は、姿勢の誤差が3次元ベクトルとして扱えるため、並進誤差 $\bm{e}_t$ と同じ枠組みで統一的にフィードバック制御を設計できることです。
姿勢誤差を3次元ベクトルに変換できたので、いよいよ並進と回転を統合した制御則を設計しましょう。
制御則の設計 — 比例フィードバック
PBVSの制御則はシンプルです。並進誤差 $\bm{e}_t$ と姿勢誤差 $\bm{e}_\theta$ を積み上げた6次元誤差ベクトル
$$ \bm{e} = \begin{pmatrix} \bm{e}_t \\ \bm{e}_\theta \end{pmatrix} \in \mathbb{R}^6 $$
に対して、比例制御（Pコントローラ）を適用します。
$$ \bm{v}_c = -\lambda \bm{e} $$
ここで $\bm{v}_c = (\bm{v}^\top, \bm{\omega}^\top)^\top \in \mathbb{R}^6$ はカメラの速度指令（並進速度 $\bm{v}$ + 角速度 $\bm{\omega}$）、$\lambda > 0$ はゲインです。
この制御則は直感的に理解できます。誤差が大きいほど速く動き、誤差がゼロに近づくほどゆっくりになります。$\lambda$ が大きいほど収束が速くなりますが、あまり大きくするとオーバーシュートや振動が生じます。
より一般には、並進と回転のゲインを独立に設定できます。
$$ \bm{v}_c = -\begin{pmatrix} \lambda_t \bm{I}_3 & \bm{0} \\ \bm{0} & \lambda_\theta \bm{I}_3 \end{pmatrix} \bm{e} $$
ここで $\lambda_t$ は並進ゲイン、$\lambda_\theta$ は回転ゲインです。並進と回転の収束速度を独立にチューニングしたい場合に便利です。
安定性の直感的理解
制御則 $\bm{v}_c = -\lambda \bm{e}$ の安定性を直感的に考えてみます。リアプノフ関数として誤差のノルムの二乗
$$ V = \frac{1}{2} \|\bm{e}\|^2 = \frac{1}{2} \bm{e}^\top \bm{e} $$
を考えます。もしカメラの速度が指令通りに実現される（すなわち $\dot{\bm{e}} \approx -\bm{v}_c$）なら、
$$ \dot{V} = \bm{e}^\top \dot{\bm{e}} = \bm{e}^\top (-\bm{v}_c) = -\lambda \bm{e}^\top \bm{e} = -\lambda \|\bm{e}\|^2 \leq 0 $$
$\dot{V}$ が常に非正なので、$V$ は単調に減少します。$\dot{V} = 0$ となるのは $\bm{e} = \bm{0}$ のときのみなので、誤差は指数的にゼロに収束します。具体的には、
$$ \bm{e}(t) = \bm{e}(0) e^{-\lambda t} $$
となり、時定数 $\tau = 1/\lambda$ で収束します。
ただし、これはカメラの速度が指令通りに完全に実現され、かつ3Dポーズ推定が完璧であるという理想的な仮定のもとでの結果です。実際にはロボットの動的特性やポーズ推定の誤差が存在するため、より精密な安定性解析が必要になります。
ここまでPBVSの制御則を設計しました。次に、この方式が持つ利点と欠点を整理し、もう一つのアプローチであるIBVSとの違いを明確にしましょう。
PBVSの利点と欠点
PBVSの利点
1. 作業空間で直線的な経路を生成する
PBVSは作業空間（3D空間）で誤差を定義するため、カメラ（エンドエフェクタ）は目標に向かって3D空間上で直線的な経路を描きます。これは多くのロボットタスク（特に障害物を避けながら接近するタスク）で望ましい性質です。宇宙空間での衛星接近のように、予測可能で滑らかな軌道が要求される場面では大きなメリットとなります。
2. 制御則が直感的で設計しやすい
誤差が「位置のずれ○○メートル」「角度のずれ○○ラジアン」と物理的に解釈できるため、ゲイン調整や安全制約の設定が容易です。たとえば「接近速度は0.1 m/s以下」「残り1mで減速開始」といった制約を自然に組み込めます。
3. カメラと制御対象の幾何学的関係に依存しない
制御は作業空間で完結するため、カメラの配置（エンドエフェクタ上か、外部固定か）やレンズ特性が変わっても、制御則を再設計する必要がありません。PnP部分がカメラの幾何を吸収してくれます。
PBVSの欠点
1. 3Dポーズ推定の精度に強く依存する
PBVSの性能は、PnP問題を解いて得られるポーズ推定の精度に直結します。画像ノイズ、特徴点検出の誤差、カメラキャリブレーション（内部パラメータ $\bm{K}$ ）の不正確さ、そして対象物の3Dモデルの誤差 — これらすべてがポーズ推定に影響し、制御性能を劣化させます。
特に宇宙環境では、強い日光とその影が共存するハイコントラストな照明条件や、対象物（デブリなど）の正確な3Dモデルが入手できない場合があり、ポーズ推定が困難になることがあります。
2. 特徴点が画像外に出る（FOV脱落）リスク
PBVSは作業空間で経路を計画するため、その経路に沿って動くときに対象物がカメラの視野（Field of View, FOV）から外れてしまう可能性があります。特に大きな回転が必要な場合、3D空間で最短経路をとると画像上では特徴点が視野外に出てしまい、追跡が途切れる危険があります。
これはIBVSにはない弱点です。IBVSは画像空間で直接制御するため、画像上の特徴点を常に視野内に保つ制約を自然に満たす傾向があります。
3. キャリブレーション要件が厳しい
PBVSはカメラの内部パラメータ $\bm{K}$（焦点距離、主点、歪み係数など）と対象物の3Dモデルの両方を事前に正確に把握している必要があります。これに対してIBVSは、理想的には $\bm{K}$ の正確な知識がなくても（大まかな値でも）機能するとされています。
PBVSとIBVSの比較
ここで両方式を簡潔に比較しておきます。
| 項目 | PBVS | IBVS |
|---|---|---|
| 制御空間 | 作業空間（3D） | 画像空間（2D） |
| 必要な情報 | 3Dモデル + カメラ内部パラメータ | 画像ヤコビアン（奥行きの推定が必要） |
| 経路の特性 | 3Dで直線的 | 画像上で直線的 |
| FOV脱落リスク | あり（大回転時） | 低い |
| キャリブレーション感度 | 高い | 比較的低い |
| 局所極小 | なし | あり（180度回転問題など） |
PBVSとIBVSはそれぞれ一長一短があり、実際のアプリケーションでは両者を組み合わせたハイブリッド方式も広く研究されています。IBVSの詳細な数理については、次の記事で解説します。
ここまでの理論的な内容を踏まえて、Pythonで実際にPBVSをシミュレーションし、制御の振る舞いを確認してみましょう。
PBVSのPythonシミュレーション
シミュレーションの概要
ここでは、3D空間内のカメラが対象物（4つのマーカー点を持つ平面ターゲット）に対してPBVS制御で接近・姿勢合わせを行うシミュレーションを実装します。シミュレーションは以下の流れで進みます。
- 対象物の3D点を定義する
- カメラの初期ポーズと目標ポーズを設定する
- 毎ステップで3D点をカメラに投影してPnP問題を解く（ここではOpenCVの
solvePnPを使用） - 推定ポーズから誤差を計算し、比例制御で速度指令を生成する
- カメラポーズを更新する
まず、必要なユーティリティ関数を定義します。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def rotation_matrix_x(angle):
"""X軸周りの回転行列"""
c, s = np.cos(angle), np.sin(angle)
return np.array([[1, 0, 0],
[0, c, -s],
[0, s, c]])
def rotation_matrix_y(angle):
"""Y軸周りの回転行列"""
c, s = np.cos(angle), np.sin(angle)
return np.array([[c, 0, s],
[0, 1, 0],
[-s, 0, c]])
def rotation_matrix_z(angle):
"""Z軸周りの回転行列"""
c, s = np.cos(angle), np.sin(angle)
return np.array([[c, -s, 0],
[s, c, 0],
[0, 0, 1]])
def skew_symmetric(v):
"""3次元ベクトルから歪対称行列を生成"""
return np.array([[0, -v[2], v[1]],
[v[2], 0, -v[0]],
[-v[1], v[0], 0]])
回転行列の基本操作に加えて、歪対称行列の生成関数を定義しました。歪対称行列は、回転軸の抽出や角速度の計算で必要になります。
次に、axis-angle表現に基づく姿勢誤差計算の関数を定義します。
import numpy as np
def rotation_to_axis_angle(R):
"""回転行列からaxis-angle表現を計算"""
# 回転角: tr(R) = 1 + 2*cos(theta)
trace_val = np.trace(R)
# 数値誤差で範囲外になるのを防ぐ
cos_theta = np.clip((trace_val - 1.0) / 2.0, -1.0, 1.0)
theta = np.arccos(cos_theta)
if abs(theta) < 1e-10:
# 回転角がほぼゼロ → 誤差なし
return np.zeros(3), 0.0
if abs(theta - np.pi) < 1e-6:
# 回転角が180度に近い場合の特殊処理
# 対称成分から軸を抽出
S = (R + R.T) / 2.0 - np.eye(3) * cos_theta
idx = np.argmax(np.diag(S))
u = S[:, idx] / np.linalg.norm(S[:, idx])
return u, theta
# 一般的な場合: 反対称成分から軸を抽出
u = np.array([R[2, 1] - R[1, 2],
R[0, 2] - R[2, 0],
R[1, 0] - R[0, 1]]) / (2.0 * np.sin(theta))
return u, theta
def compute_pose_error(R_current, t_current, R_desired, t_desired):
"""現在のポーズと目標ポーズから6次元誤差ベクトルを計算"""
# 並進誤差
e_t = t_current - t_desired
# 回転誤差: R_e = R_current @ R_desired^T
R_e = R_current @ R_desired.T
axis, angle = rotation_to_axis_angle(R_e)
e_theta = angle * axis
return np.concatenate([e_t, e_theta])
回転角が0や180度に近い場合には数値的に不安定になるため、特殊処理を入れています。特に180度回転の場合は $\sin\theta \approx 0$ で通常の軸抽出ができないため、対称成分から軸を求めています。
続いて、PnP問題を解くための関数と、カメラへの3D点投影関数を実装します。ここでは教育目的で、OpenCVを使わずに直接的PnP（DLT法に基づく簡易実装）を書きます。
import numpy as np
def project_points(points_3d, K, R, t):
"""3D点をカメラに投影して2D画像座標を得る"""
# カメラ座標系に変換
points_cam = (R @ points_3d.T).T + t.reshape(1, 3)
# 射影 (Z で割る)
points_proj = points_cam[:, :2] / points_cam[:, 2:3]
# 画像座標に変換
fx, fy = K[0, 0], K[1, 1]
cx, cy = K[0, 2], K[1, 2]
u = fx * points_proj[:, 0] + cx
v = fy * points_proj[:, 1] + cy
return np.column_stack([u, v])
def solve_pnp_dlt(points_3d, points_2d, K):
"""DLT法による簡易PnPソルバー"""
n = len(points_3d)
# 正規化画像座標に変換
K_inv = np.linalg.inv(K)
points_norm = []
for i in range(n):
p_h = np.array([points_2d[i, 0], points_2d[i, 1], 1.0])
p_n = K_inv @ p_h
points_norm.append(p_n[:2] / p_n[2])
points_norm = np.array(points_norm)
# DLT方程式系を構築
A = np.zeros((2 * n, 12))
for i in range(n):
X, Y, Z = points_3d[i]
x, y = points_norm[i]
A[2 * i] = [X, Y, Z, 1, 0, 0, 0, 0, -x*X, -x*Y, -x*Z, -x]
A[2 * i + 1] = [0, 0, 0, 0, X, Y, Z, 1, -y*X, -y*Y, -y*Z, -y]
# SVDで解く
_, S, Vt = np.linalg.svd(A)
P = Vt[-1].reshape(3, 4)
# RとtをQR分解で抽出
M = P[:, :3]
U_m, S_m, Vt_m = np.linalg.svd(M)
R_est = U_m @ Vt_m
# det(R) = 1 を保証
if np.linalg.det(R_est) < 0:
R_est = -R_est
# スケール復元
scale = np.mean(S_m)
t_est = P[:, 3] / scale
if np.linalg.det(U_m @ Vt_m) < 0:
t_est = -t_est
return R_est, t_est
project_points は標準的なピンホールカメラモデルによる投影を行い、solve_pnp_dlt はDLT（Direct Linear Transform）法に基づくPnPの簡易的な実装です。DLT法は教育的に分かりやすい反面、回転行列の直交性を厳密に保証しないため、SVDによる最近接直交行列への射影を行っています。
ここまでで基本的な部品が揃いました。いよいよPBVSの制御ループ全体をシミュレーションします。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def rotation_matrix_x(a):
c, s = np.cos(a), np.sin(a)
return np.array([[1,0,0],[0,c,-s],[0,s,c]])
def rotation_matrix_y(a):
c, s = np.cos(a), np.sin(a)
return np.array([[c,0,s],[0,1,0],[-s,0,c]])
def rotation_matrix_z(a):
c, s = np.cos(a), np.sin(a)
return np.array([[c,-s,0],[s,c,0],[0,0,1]])
def rotation_to_axis_angle(R):
trace_val = np.trace(R)
cos_theta = np.clip((trace_val - 1.0) / 2.0, -1.0, 1.0)
theta = np.arccos(cos_theta)
if abs(theta) < 1e-10:
return np.zeros(3), 0.0
if abs(theta - np.pi) < 1e-6:
S = (R + R.T) / 2.0 - np.eye(3) * cos_theta
idx = np.argmax(np.diag(S))
u = S[:, idx] / np.linalg.norm(S[:, idx])
return u, theta
u = np.array([R[2,1]-R[1,2], R[0,2]-R[2,0], R[1,0]-R[0,1]]) / (2*np.sin(theta))
return u, theta
def project_points(pts_3d, K, R, t):
pts_cam = (R @ pts_3d.T).T + t.reshape(1,3)
pts_proj = pts_cam[:,:2] / pts_cam[:,2:3]
u = K[0,0]*pts_proj[:,0] + K[0,2]
v = K[1,1]*pts_proj[:,1] + K[1,2]
return np.column_stack([u, v])
# --- 設定 ---
# 対象物の特徴点（ワールド座標系）: 正方形マーカー
marker_size = 0.5 # [m]
target_points = np.array([
[-marker_size/2, -marker_size/2, 0],
[ marker_size/2, -marker_size/2, 0],
[ marker_size/2, marker_size/2, 0],
[-marker_size/2, marker_size/2, 0]
])
# カメラ内部パラメータ
fx, fy = 800.0, 800.0
cx, cy = 320.0, 240.0
K = np.array([[fx, 0, cx],
[0, fy, cy],
[0, 0, 1]])
# 目標ポーズ: 対象物正面 1.0m 手前
R_desired = np.eye(3)
t_desired = np.array([0.0, 0.0, 1.0])
# 初期ポーズ: 目標から位置・姿勢ともにずれた状態
R_init = rotation_matrix_z(np.radians(25)) @ rotation_matrix_x(np.radians(15))
t_init = np.array([0.6, -0.4, 2.5])
# 制御パラメータ
lambda_gain = 0.8 # 比例ゲイン
dt = 0.05 # タイムステップ [s]
n_steps = 200 # 最大ステップ数
tol = 1e-4 # 収束判定閾値
# --- シミュレーションループ ---
R_current = R_init.copy()
t_current = t_init.copy()
history_t = [t_current.copy()]
history_error = []
history_pos_error = []
history_rot_error = []
for step in range(n_steps):
# 誤差計算
e_t = t_current - t_desired
R_e = R_current @ R_desired.T
axis, angle = rotation_to_axis_angle(R_e)
e_theta = angle * axis
error = np.concatenate([e_t, e_theta])
error_norm = np.linalg.norm(error)
history_error.append(error_norm)
history_pos_error.append(np.linalg.norm(e_t))
history_rot_error.append(angle)
if error_norm < tol:
print(f"収束: ステップ {step}, 誤差ノルム = {error_norm:.6f}")
break
# 比例制御による速度指令
v_cmd = -lambda_gain * error
v_trans = v_cmd[:3] # 並進速度
omega = v_cmd[3:] # 角速度
# カメラポーズ更新
t_current = t_current + v_trans * dt
# 回転の更新（ロドリゲスの回転公式で微小回転を適用）
omega_norm = np.linalg.norm(omega)
if omega_norm > 1e-10:
omega_hat = omega / omega_norm
delta_theta = omega_norm * dt
K_skew = np.array([[0, -omega_hat[2], omega_hat[1]],
[omega_hat[2], 0, -omega_hat[0]],
[-omega_hat[1], omega_hat[0], 0]])
delta_R = (np.eye(3) + np.sin(delta_theta) * K_skew
+ (1 - np.cos(delta_theta)) * K_skew @ K_skew)
R_current = delta_R @ R_current
history_t.append(t_current.copy())
history_t = np.array(history_t)
print(f"最終位置誤差: {history_pos_error[-1]:.6f} m")
print(f"最終姿勢誤差: {np.degrees(history_rot_error[-1]):.4f} deg")
このコードの構造を整理します。初期状態では、カメラは目標位置から右に0.6m、下に0.4mずれ、さらに1.5m遠く（Z方向で2.5m vs 目標1.0m）に位置しています。姿勢もZ軸周りに25度、X軸周りに15度傾いた状態です。制御ループでは毎ステップ、誤差を計算して比例制御で速度指令を生成し、カメラポーズを更新します。回転の更新にはロドリゲスの回転公式を使い、回転行列の直交性を保っています。
続いて、シミュレーション結果を可視化します。
import numpy as np
import matplotlib.pyplot as plt
# （前のコードで得られた history_t, history_error,
# history_pos_error, history_rot_error を使用）
fig, axes = plt.subplots(1, 3, figsize=(18, 5))
# (a) 3D軌道
ax = fig.add_subplot(131, projection='3d')
ax.plot(history_t[:, 0], history_t[:, 1], history_t[:, 2],
'b-', linewidth=1.5, label='Camera path')
ax.scatter(*t_init, color='red', s=100, marker='o', label='Start')
ax.scatter(*t_desired, color='green', s=100, marker='*', label='Goal')
# 対象物の描画
tp = np.vstack([target_points, target_points[0]])
ax.plot(tp[:, 0], tp[:, 1], tp[:, 2], 'k-', linewidth=2, label='Target')
ax.set_xlabel('X [m]')
ax.set_ylabel('Y [m]')
ax.set_zlabel('Z [m]')
ax.set_title('Camera Trajectory (3D)')
ax.legend(fontsize=8)
# (b) 誤差のノルム推移
time_axis = np.arange(len(history_error)) * dt
axes[1].semilogy(time_axis, history_error, 'b-', linewidth=1.5)
axes[1].set_xlabel('Time [s]')
axes[1].set_ylabel('Error norm')
axes[1].set_title('Total Error Convergence')
axes[1].grid(True, alpha=0.3)
# (c) 位置・姿勢誤差の個別推移
axes[2].semilogy(time_axis, history_pos_error, 'b-',
linewidth=1.5, label='Position error [m]')
axes[2].semilogy(time_axis, np.degrees(history_rot_error), 'r--',
linewidth=1.5, label='Rotation error [deg]')
axes[2].set_xlabel('Time [s]')
axes[2].set_ylabel('Error')
axes[2].set_title('Position & Rotation Error')
axes[2].legend()
axes[2].grid(True, alpha=0.3)
plt.tight_layout()
plt.show()
このグラフから、PBVSの重要な特性が読み取れます。
- 3D軌道（左図）: カメラは初期位置（赤丸）から目標位置（緑星）に向かって、3D空間上でほぼ直線的な経路を描いて接近しています。これがPBVSの最大の特長 — 作業空間での予測可能な経路 — を如実に示しています。
- 誤差の指数的減衰（中央図）: 対数スケールで見ると、誤差ノルムがほぼ直線的に減少しています。これは指数的減衰 $\|\bm{e}(t)\| \propto e^{-\lambda t}$ を意味し、先ほどのリアプノフ解析の理論予測と完全に一致しています。
- 位置と姿勢の収束（右図）: 位置誤差と姿勢誤差がほぼ同じペースで収束しています。これはゲイン $\lambda$ を並進・回転で共通にしたためです。実用上は、並進と回転のスケールが異なる（メートル vs ラジアン）ため、独立ゲインで調整することが推奨されます。
画像面上での特徴点軌跡の確認
PBVSの欠点として指摘した「画像からの脱落リスク」を確認するため、シミュレーション中の画像面上での特徴点の軌跡もプロットしてみましょう。
import numpy as np
import matplotlib.pyplot as plt
def rotation_matrix_x(a):
c, s = np.cos(a), np.sin(a)
return np.array([[1,0,0],[0,c,-s],[0,s,c]])
def rotation_matrix_y(a):
c, s = np.cos(a), np.sin(a)
return np.array([[c,0,s],[0,1,0],[-s,0,c]])
def rotation_matrix_z(a):
c, s = np.cos(a), np.sin(a)
return np.array([[c,-s,0],[s,c,0],[0,0,1]])
def rotation_to_axis_angle(R):
trace_val = np.trace(R)
cos_theta = np.clip((trace_val - 1.0) / 2.0, -1.0, 1.0)
theta = np.arccos(cos_theta)
if abs(theta) < 1e-10:
return np.zeros(3), 0.0
if abs(theta - np.pi) < 1e-6:
S = (R + R.T) / 2.0 - np.eye(3) * cos_theta
idx = np.argmax(np.diag(S))
u = S[:, idx] / np.linalg.norm(S[:, idx])
return u, theta
u = np.array([R[2,1]-R[1,2], R[0,2]-R[2,0], R[1,0]-R[0,1]]) / (2*np.sin(theta))
return u, theta
def project_points(pts_3d, K, R, t):
pts_cam = (R @ pts_3d.T).T + t.reshape(1,3)
pts_proj = pts_cam[:,:2] / pts_cam[:,2:3]
u = K[0,0]*pts_proj[:,0] + K[0,2]
v = K[1,1]*pts_proj[:,1] + K[1,2]
return np.column_stack([u, v])
# --- 設定（前と同じ） ---
marker_size = 0.5
target_points = np.array([
[-marker_size/2, -marker_size/2, 0],
[ marker_size/2, -marker_size/2, 0],
[ marker_size/2, marker_size/2, 0],
[-marker_size/2, marker_size/2, 0]
])
fx, fy = 800.0, 800.0
cx, cy = 320.0, 240.0
K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
R_desired = np.eye(3)
t_desired = np.array([0.0, 0.0, 1.0])
R_init = rotation_matrix_z(np.radians(25)) @ rotation_matrix_x(np.radians(15))
t_init = np.array([0.6, -0.4, 2.5])
lambda_gain = 0.8
dt = 0.05
n_steps = 200
tol = 1e-4
# --- シミュレーション（画像座標も記録） ---
R_current = R_init.copy()
t_current = t_init.copy()
image_traj = [] # 各ステップの画像座標
for step in range(n_steps):
pts_2d = project_points(target_points, K, R_current, t_current)
image_traj.append(pts_2d.copy())
e_t = t_current - t_desired
R_e = R_current @ R_desired.T
axis, angle = rotation_to_axis_angle(R_e)
e_theta = angle * axis
error = np.concatenate([e_t, e_theta])
if np.linalg.norm(error) < tol:
break
v_cmd = -lambda_gain * error
t_current = t_current + v_cmd[:3] * dt
omega = v_cmd[3:]
omega_norm = np.linalg.norm(omega)
if omega_norm > 1e-10:
omega_hat = omega / omega_norm
dth = omega_norm * dt
Ks = np.array([[0,-omega_hat[2],omega_hat[1]],
[omega_hat[2],0,-omega_hat[0]],
[-omega_hat[1],omega_hat[0],0]])
dR = np.eye(3) + np.sin(dth)*Ks + (1-np.cos(dth))*Ks@Ks
R_current = dR @ R_current
image_traj = np.array(image_traj)
# --- 可視化: 画像面上の特徴点軌跡 ---
fig, ax = plt.subplots(figsize=(8, 6))
colors = ['#e74c3c', '#3498db', '#2ecc71', '#f39c12']
labels = ['Point 1', 'Point 2', 'Point 3', 'Point 4']
for i in range(4):
ax.plot(image_traj[:, i, 0], image_traj[:, i, 1],
'-', color=colors[i], linewidth=1.2, label=labels[i])
ax.scatter(image_traj[0, i, 0], image_traj[0, i, 1],
color=colors[i], marker='o', s=80, zorder=5)
ax.scatter(image_traj[-1, i, 0], image_traj[-1, i, 1],
color=colors[i], marker='*', s=120, zorder=5)
# 画像境界（FOV）
img_w, img_h = 640, 480
ax.plot([0, img_w, img_w, 0, 0], [0, 0, img_h, img_h, 0],
'k--', linewidth=2, label='Image boundary')
ax.set_xlabel('u [px]')
ax.set_ylabel('v [px]')
ax.set_title('Feature Point Trajectories in Image Plane (PBVS)')
ax.set_xlim(-50, img_w + 50)
ax.set_ylim(img_h + 50, -50) # 画像座標系はy軸下向き
ax.legend(loc='lower right', fontsize=8)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.show()
この画像面プロットから、PBVSの重要な性質が見えてきます。
- 画像面上の軌跡は直線的ではない: PBVSは3D空間で直線経路を生成しますが、画像面上ではそれが非線形な曲線として現れます。これはカメラの射影変換（3D→2D）が非線形であるためです。対照的にIBVSは画像面上で直線的な軌跡を生成します。
- FOV内に留まっているか: 今回の初期条件では特徴点は画像境界内に収まっていますが、初期ずれがさらに大きい場合（特に大きな回転を伴う場合）には、途中で特徴点が画像外に出てしまう可能性があります。これがPBVSの実用上の課題です。
- 丸印（初期）から星印（最終）へ: 最終的に4点とも画像中心付近に集まっていることが確認できます。これはカメラが正面から適切な距離で対象物を捉える目標ポーズに収束した証拠です。
ゲインの影響を確認する
制御ゲイン $\lambda$ を変えるとどうなるかを確認してみましょう。
import numpy as np
import matplotlib.pyplot as plt
def rotation_matrix_x(a):
c, s = np.cos(a), np.sin(a)
return np.array([[1,0,0],[0,c,-s],[0,s,c]])
def rotation_matrix_z(a):
c, s = np.cos(a), np.sin(a)
return np.array([[c,-s,0],[s,c,0],[0,0,1]])
def rotation_to_axis_angle(R):
trace_val = np.trace(R)
cos_theta = np.clip((trace_val - 1.0) / 2.0, -1.0, 1.0)
theta = np.arccos(cos_theta)
if abs(theta) < 1e-10:
return np.zeros(3), 0.0
if abs(theta - np.pi) < 1e-6:
S = (R + R.T) / 2.0 - np.eye(3) * cos_theta
idx = np.argmax(np.diag(S))
u = S[:, idx] / np.linalg.norm(S[:, idx])
return u, theta
u = np.array([R[2,1]-R[1,2], R[0,2]-R[2,0], R[1,0]-R[0,1]]) / (2*np.sin(theta))
return u, theta
# 設定
R_desired = np.eye(3)
t_desired = np.array([0.0, 0.0, 1.0])
R_init = rotation_matrix_z(np.radians(25)) @ rotation_matrix_x(np.radians(15))
t_init = np.array([0.6, -0.4, 2.5])
dt = 0.05
n_steps = 300
gains = [0.3, 0.8, 2.0, 5.0]
fig, ax = plt.subplots(figsize=(10, 6))
for lam in gains:
R_c = R_init.copy()
t_c = t_init.copy()
errors = []
for step in range(n_steps):
e_t = t_c - t_desired
R_e = R_c @ R_desired.T
axis, angle = rotation_to_axis_angle(R_e)
e_theta = angle * axis
err = np.concatenate([e_t, e_theta])
errors.append(np.linalg.norm(err))
if np.linalg.norm(err) < 1e-6:
break
v_cmd = -lam * err
t_c = t_c + v_cmd[:3] * dt
omega = v_cmd[3:]
omega_norm = np.linalg.norm(omega)
if omega_norm > 1e-10:
oh = omega / omega_norm
dth = omega_norm * dt
Ks = np.array([[0,-oh[2],oh[1]],[oh[2],0,-oh[0]],[-oh[1],oh[0],0]])
dR = np.eye(3) + np.sin(dth)*Ks + (1-np.cos(dth))*Ks@Ks
R_c = dR @ R_c
time_ax = np.arange(len(errors)) * dt
ax.semilogy(time_ax, errors, linewidth=1.8, label=f'$\\lambda = {lam}$')
ax.set_xlabel('Time [s]', fontsize=12)
ax.set_ylabel('Error norm', fontsize=12)
ax.set_title('Effect of Gain on PBVS Convergence', fontsize=13)
ax.legend(fontsize=11)
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.show()
このグラフから、ゲインの影響が明確にわかります。
- $\lambda = 0.3$（低ゲイン）: 収束は安定していますが遅く、定常状態に達するまでに長い時間がかかります。対数プロットでの傾きが緩やかなことから、時定数 $\tau = 1/\lambda \approx 3.3$ 秒に対応する遅い指数減衰であることが読み取れます。
- $\lambda = 0.8$（中ゲイン）: バランスの取れた設定です。適度な速度で収束し、安定性も問題ありません。時定数は $\tau \approx 1.25$ 秒です。
- $\lambda = 2.0$ および $\lambda = 5.0$（高ゲイン）: 収束が非常に速くなりますが、離散時間制御（ $dt = 0.05$ s）との兼ね合いで、$\lambda \cdot dt$ が1に近づくと不安定になるリスクがあります。$\lambda = 5.0, dt = 0.05$ では $\lambda \cdot dt = 0.25$ であり、まだ安定領域にありますが、実際のロボットでは遅延やモデル誤差があるため、あまり大きなゲインは避けるべきです。
宇宙ロボティクスへのPBVS適用における課題
PBVSの理論とシミュレーションを学んだところで、宇宙環境特有の課題にも触れておきましょう。
照明条件の過酷さ
宇宙空間では大気による散乱光がないため、太陽光が直接当たる部分と影の部分のコントラストが極端に大きくなります。対象物が半分明るく半分真っ暗というような状況では、特徴点検出が困難になり、PnPの入力の質が劣化します。
対象物の3Dモデルの不確かさ
協力的な衛星（事前に3Dモデルが入手可能）であれば問題は少ないですが、故障した衛星やデブリは正確な3Dモデルがない場合があります。さらに、太陽電池パドルが展開角度不明のまま広がっていたり、アンテナが破損していたりすると、事前の3Dモデルとの差異がポーズ推定誤差に直結します。
微小重力環境での動特性
地上のロボットアームとは異なり、宇宙空間ではアームの運動がベース衛星にも反力を及ぼすため、ベース衛星とアームの連成運動を考慮する必要があります。PBVSの制御則は「カメラの速度指令がそのまま実現される」ことを前提としていますが、この仮定が成り立たない場合、制御性能が劣化します。
通信遅延とオンボード処理
地上との通信遅延があるため、PnP問題の解法やフィードバック制御はすべてオンボード（衛星搭載コンピュータ上）で実行する必要があります。計算資源が限られる環境では、EPnPのような効率的なアルゴリズムの選択が重要になります。
これらの課題に対処するために、PBVSとIBVSを組み合わせたハイブリッド方式や、深層学習によるロバストな特徴点検出、モデルフリーのポーズ推定手法など、活発な研究が進められています。
まとめ
本記事では、位置ベースビジュアルサーボ（PBVS）について、基本原理から数理的な詳細、Pythonでのシミュレーションまでを解説しました。
- PBVSの基本戦略: 画像から3Dポーズを推定し、作業空間で誤差を定義して制御する「見て、測って、動かす」方式です。
- PnP問題: $n$ 個の2D-3D対応点からカメラポーズを求める問題であり、PBVSの心臓部です。P3P（最小構成）やEPnP（効率的な多点解法）といったアルゴリズムがあります。
- 制御則: 並進誤差とaxis-angle表現の姿勢誤差を積み上げた6次元誤差ベクトルに対して、比例制御 $\bm{v}_c = -\lambda \bm{e}$ を適用します。理論的には指数的収束が保証されます。
- 利点と欠点: 作業空間での直線経路と直感的な制御設計が利点ですが、3D推定精度への依存やFOV脱落リスクが欠点です。
- シミュレーション: Pythonで3D PBVSを実装し、カメラが目標ポーズに向かって直線的に収束する様子を確認しました。画像面上では非線形な軌跡になることも確認しました。
PBVSは「まず3D情報を復元してから制御する」という自然な発想に基づく手法ですが、その3D復元の精度がボトルネックになりうるという本質的なトレードオフを持っています。これに対して、3D復元を迂回して画像情報を直接使うIBVS（画像ベースビジュアルサーボ）は、異なるトレードオフを持つ相補的なアプローチです。
次の記事では、そのIBVSの数理と実装を解説します。
- 画像ベースビジュアルサーボ（IBVS） — 画像空間で直接制御するもう一つのアプローチ