ビジュアルサーボ（画像ベース制御）

画像の「ズレ」をそのまま制御量にするという発想

ロボットの手先をカメラで見ながら目標位置へ導くとき、素朴な方法は「画像から対象の3次元位置を推定し、その座標へ手先を動かす」ものです。これは推定→制御の二段構えで、推定した3次元姿勢の誤差がそのまま制御精度を決めてしまいます。ビジュアルサーボ（視覚サーボ）はこの発想を転換し、画像上で観測される特徴量そのものを制御のフィードバック信号として閉ループに組み込みます。目標画像で対象がどう見えるべきか（目標特徴量 s*）と、いま実際にどう見えているか（現在特徴量 s）の差を誤差 e = s - s* と定義し、この誤差をゼロへ駆動するようにカメラ（またはカメラを載せた手先）を動かします。

この枠組みは、フィードバック信号を画像平面上に取るか3次元空間に取るかで二つの系統に分かれます。画像特徴を直接使うのがIBVS（Image-Based Visual Servoing、画像ベース）、画像から対象の3次元姿勢を復元してその姿勢誤差を制御するのがPBVS（Position-Based Visual Servoing、位置ベース）です。両者は「何を誤差ベクトル e に選ぶか」だけの違いに見えますが、収束性・ロバスト性・軌道の性質がまったく異なります。

IBVSとPBVSの違い：誤差をどの空間で測るか

横にスクロール

要点は、IBVSは「画像内で正しく見えること」を保証しやすいが3次元的な動きが保証できず、PBVSは「3次元的に素直に動く」が画像内に対象が留まる保証がないという相補関係です。実務では両者の利点を組み合わせたハイブリッド手法（2-1/2-D ビジュアルサーボ。並進はIBVS的、回転はPBVS的に扱う）も使われます。以降はビジュアルサーボの数学的核心が凝縮されたIBVSを中心に、相互作用行列を軸に原理を解いていきます。

相互作用行列（画像ヤコビアン）：特徴速度とカメラ速度をつなぐ

IBVSの心臓部は、画像特徴の時間変化とカメラの空間速度を結ぶ線形写像です。カメラの空間速度を並進 v_c と角速度 ω_c をまとめた6次元ベクトル v = [v_c; ω_c] とすると、特徴量ベクトル s の変化率は次の関係で表せます。

ṡ = L(s, Z) v

  s   : 画像特徴ベクトル（例：点特徴の画素座標をならべたもの）
  v   : カメラの空間速度 [vx, vy, vz, ωx, ωy, ωz]
  L   : 相互作用行列（Interaction Matrix / 画像ヤコビアン, Image Jacobian）
  Z   : その特徴点のカメラ座標系での奥行き（深度）

この L が相互作用行列（画像ヤコビアン）です。マニピュレータで関節速度と手先速度を結ぶヤコビ行列と同じ役割を、いまは「カメラ速度」と「画像特徴速度」の間で果たします。ヤコビ行列の理解はそのまま応用が利くので、ヤコビ行列と操作性 を押さえておくと相互作用行列の階数落ち＝特異点の議論が一気に見通せます。

最も基本的な正規化画像座標の点特徴（焦点距離で正規化した座標 x = X/Z, y = Y/Z）1点に対する相互作用行列は、透視投影を微分して次の 2×6 行列として導かれます。

点特徴 (x, y)（正規化座標）1点あたりの相互作用行列:

      | -1/Z    0     x/Z    x·y       -(1+x²)     y  |
  L = |                                               |
      |  0    -1/Z    y/Z    1+y²       -x·y      -x  |

左3列（並進 vx,vy,vz の寄与）は 1/Z を含み奥行きに依存する。
右3列（回転 ωx,ωy,ωz の寄与）は座標だけで決まり Z に依存しない。

ここから二つの本質が読めます。第一に、並進運動の画像上への効き方は奥行き Z に反比例する（遠い点は動かしても画像上であまり動かない）ため、L の左半分は Z の推定値を必要とします。第二に、回転運動の効き方は Z に依存しない——回転だけなら深度を知らなくても画像変化を正しく予測できます。点特徴を複数（通常4点以上）使う場合は、各点の 2×6 行列を縦に積み上げて L を構成します。3次元の6自由度を制御するには特徴量の次元が6以上必要で、点なら3点で理論上6次元ですが、後述の特異点・多解性を避けるため実務では4点以上を配置するのが定石です。

カメラ空間からの制御則：誤差を指数的に潰す

相互作用行列が手に入れば、制御則は素直に導けます。誤差 e = s - s*（目標特徴 s* は定数とする）の時間微分は ė = ṡ = L v です。この誤差を指数的に減衰させたい、すなわち ė = -λ e（λ > 0 はゲイン）となるようにカメラ速度 v を決めます。

目標:  ė = -λ e   （誤差を指数関数的に 0 へ）
関係:  ė = L v

L が正方正則なら:   v = -λ L⁻¹ e
一般には擬似逆行列:  v = -λ L̂⁺ e     （L̂⁺ = (L̂ᵀL̂)⁻¹L̂ᵀ など）

これがIBVSの基本制御則です。毎制御周期でこの v を計算し、eye-in-hand なら手先速度指令として、必要なら手先ヤコビアンで関節速度に変換して送ります。L̂ は真の L の推定にすぎないため、実際に成り立つ誤差ダイナミクスは次のようになります。

実際の閉ループ:  ė = L v = -λ (L L̂⁺) e

  L̂ = L のとき:      ė = -λ e         （理想。単調・指数収束）
  L̂ ≠ L でも:        L L̂⁺ が正定値なら誤差は減少（局所安定は保たれる）

ここがIBVSの強みです。相互作用行列にモデル誤差（深度誤差や較正誤差）があっても、L L̂⁺ が正定値でありさえすれば誤差は減り続け、局所的な安定性は保たれます。深度 Z を1桁単位で外していても発散せず目標に収束することが多いのはこの構造ゆらぎへの寛容さゆえで、PBVSが姿勢推定誤差をそのまま位置誤差に転写してしまうのと対照的です。

収束性・特異点・較正依存：IBVSの3つの落とし穴

制御則 v = -λ L̂⁺ e は美しいものの、実機では次の3点が破綻要因になります。

1. 局所収束と大回転でのカメラ後退問題。 誤差ダイナミクス ė = -λ e はあくまで画像誤差 e の空間での話で、カメラの3次元軌道が最短経路になる保証はありません。特に、目標姿勢との差が光軸まわりの大きな回転（例：180度）を含むとき、画像上で特徴を「まっすぐ」目標へ動かそうとする力学が、カメラをいったん対象から大きく後退（退避）させてから戻すという不合理な3次元軌道を生みます。これが有名なカメラ後退問題（camera retreat）で、視野外れや作業空間の逸脱を招きます。原因は、画像誤差の直線的減衰が3次元運動の直線性と両立しない——L⁺ が張る運動が並進と回転を不自然に結合させることにあります。

2. 画像特異点：相互作用行列の階数落ち。 L̂⁺ は L̂ が行フルランクである限り定義できますが、特徴点の幾何配置によっては L が階数落ちし、あるカメラ運動が画像に一切変化を生まない局所特異が生じます。古典的な例が、3点の点特徴を使うときで、3点がある特定の円筒面（3点とカメラ光心を通す構成）上に載る配置ではヤコビアンが退化し、可制御性が失われます。これはマニピュレータの特異姿勢で det(J)=0 となり関節速度が発散するのと同型の現象です。

画像特異点の条件:  rank(L̂) < 制御したい自由度数
                  ⇔ L̂⁺ を通した速度指令が特定方向で発散/不能
対策:  ・冗長な特徴を使う（点を4点以上、面積・モーメントなど別種特徴を混ぜる）
       ・ダンピング最小二乗:  v = -λ L̂ᵀ(L̂L̂ᵀ + μ²I)⁻¹ e  で発散を抑制
       ・特徴選択そのものを設計し退化配置を避ける

3. 較正依存性。 相互作用行列 L̂ はカメラの内部パラメータ（焦点距離・主点）と、点なら深度 Z に依存します。内部パラメータの較正誤差は L̂ を系統的に歪め、深度誤差は並進項（1/Z を含む左3列）をスケールずれさせます。前述のとおりIBVSは局所安定という意味ではこれらに寛容ですが、寛容さは無限ではなく、L L̂⁺ が正定値を保つ範囲を超えると収束が遅化・振動・発散へ転じます。加えて eye-to-hand ではカメラと手先ベースの間の外部パラメータ（手先-カメラ変換、hand-eye calibration）の誤差も効いてきます。

収束のふるまいを疑似コードで押さえる

IBVSの1制御周期を擬似コードで示します。特徴抽出→相互作用行列の構成→速度指令→積分、というループが本質です。

# eye-in-hand IBVS の1制御周期（点特徴 n 点）
s_star = target_features            # 目標画像での特徴（定数）
lam    = 0.5                        # 収束ゲイン λ
mu2    = 1e-3                       # ダンピング係数 μ²（特異点近傍で有効）

while not converged:
    s   = detect_features(image)   # 現在の特徴（画素→正規化座標）
    Zh  = estimate_depth(s)        # 深度推定（ステレオ/RGB-D/目標値近似）
    L   = build_interaction_matrix(s, Zh)   # 2n×6 の推定相互作用行列 L̂
    e   = s - s_star               # 画像誤差

    # ダンピング付き擬似逆で速度指令（特異点で発散させない）
    v   = -lam * L.T @ inv(L @ L.T + mu2 * I) @ e   # カメラ空間速度 6D

    q_dot = arm_jacobian_pinv() @ v   # 必要なら手先→関節速度へ変換
    send_velocity(q_dot)
    converged = norm(e) < eps

build_interaction_matrix の中身が前掲の 2×6 行列を各点について積み上げる処理で、estimate_depth の精度と mu2 の設定が安定性を左右します。ゲイン λ を大きくすれば収束は速くなりますが、離散化した制御周期では大きすぎる λ が画像誤差のオーバーシュートや振動を招くため、実機では適応ゲイン（誤差が大きいほど λ を絞る）が併用されます。速度から姿勢を積分して逐次追従する構造は視覚オドメトリと同じ骨格で、視覚オドメトリとVisual SLAM の逐次推定と対比すると理解が深まります。

まとめ

ビジュアルサーボは、画像上で観測される特徴量の誤差 e = s - s* をそのまま閉ループのフィードバックに使う視覚制御であり、誤差を画像空間で測るIBVSと3次元姿勢空間で測るPBVSに大別されます。IBVSの核心は相互作用行列（画像ヤコビアン）L で、特徴速度とカメラ速度を ṡ = L v で結び、制御則 v = -λ L̂⁺ e によって画像誤差を指数的に潰します。並進項が奥行き Z に反比例し回転項が Z に依存しないという構造が、深度推定と較正への依存を生む一方、実際の誤差ダイナミクス ė = -λ L L̂⁺ e において L L̂⁺ が正定値でありさえすれば局所安定が保たれる——このモデル誤差への寛容さがIBVSの最大の武器です。同時に、大回転でのカメラ後退問題、特徴点配置による画像特異点、内部・外部パラメータの較正依存という3つの落とし穴があり、ダンピング最小二乗・冗長な特徴選択・平均相互作用行列といった対策で緩和します。IBVSとPBVSはどちらが優れているという関係ではなく、「画像内で正しく見えること」と「3次元的に素直に動くこと」のどちらを優先するかというトレードオフであり、その本質はすべて相互作用行列という一枚の線形写像に凝縮されています。