デブリが年々増え続ける宇宙空間で、壊れた衛星や使い終わったロケット上段に接近して捕獲する — こうしたミッションでは、対象物体が自分の姿勢や位置を教えてくれるとは限りません。故障した衛星にはマーカーも通信機能もなく、しかも無制御に回転しているかもしれません。このような非協力物体（non-cooperative target）に安全に接近するためには、カメラ映像だけを頼りに対象の位置と姿勢をリアルタイムに推定する技術が不可欠です。これが視覚誘導（visual guidance）と呼ばれる分野です。
「物体がどこにあり、どちらを向いているか」を画像から推定する問題は、地上のロボティクスやAR（拡張現実）でも広く研究されていますが、宇宙には地上にはない独特の困難があります。太陽光の直射と深い影が交互に現れる極端な照明条件、テクスチャ（模様）のない金属面が多い宇宙機の外観、そして計算資源が限られた衛星のオンボードコンピュータ — これらの制約の中で、ミリメートル・度単位の精度を達成しなければなりません。
視覚誘導による姿勢推定を理解すると、以下のような応用が見えてきます。
- 軌道上サービス（On-Orbit Servicing）: 故障した衛星に接近して修理・燃料補給を行うミッション
- 能動的デブリ除去（Active Debris Removal）: スペースデブリを捕獲して安全に軌道離脱させるミッション
- 宇宙ステーションへのドッキング: 補給船が無人でステーションに接近・結合する自律操作
- 惑星探査: 小惑星や月面の岩石への自律的な接近・サンプリング
本記事の内容
- 非協力物体の姿勢推定がなぜ難しいのか
- 6DoF（6自由度）姿勢推定の数学的定式化
- エッジベース手法 — 物体輪郭のマッチングによる姿勢推定
- モデルベース手法 — 3DモデルとのICP的マッチングによる姿勢推定
- 照明条件の変化（日照/影の切り替わり）への対処
- カルマンフィルタによる姿勢追跡
- 宇宙ミッションでの実例（PRISMA, RemoveDEBRIS等）
- Pythonで簡単なモデルベース姿勢推定の実装
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
非協力物体の姿勢推定が難しい理由
「協力物体」と「非協力物体」の違い
宇宙でのランデブー・ドッキングにおいて、対象物体は協力度によって大きく分類されます。
協力物体（cooperative target）は、自分の姿勢情報をGPSや通信で送ってくれたり、ドッキング用のマーカーやリフレクタを備えたりしている物体です。国際宇宙ステーション（ISS）に物資を運ぶHTVやDragonは、ISSがレーザーリフレクタやGPS情報を提供してくれるため、協力ランデブーに分類されます。
一方、非協力物体とは、これらの支援が一切期待できない物体です。代表例は以下のようなものです。
- 故障した衛星: 通信途絶し、姿勢制御を失っている。太陽パネルが予期しない方向を向いているかもしれない
- ロケット上段: ミッション完了後に放棄され、無制御に回転している
- スペースデブリ: 形状すら正確にはわからない断片
このような対象に対しては、搭載カメラの映像だけが情報源です。しかも、対象が回転していれば見える面が時々刻々変わり、太陽光の当たり方も軌道位置に応じて激しく変化します。
宇宙特有の困難
地上のコンピュータビジョンと比較して、宇宙での視覚誘導には以下のような追加的な難しさがあります。
極端な照明条件: 宇宙空間では大気による光の散乱がありません。太陽光が当たる面は非常に明るく、影の面は真っ暗になります。さらに、軌道上では約90分ごとに日照と日食が切り替わるため、一瞬で照明環境が劇変します。これは画像処理のアルゴリズムにとって大きな課題です。
テクスチャの欠如: 宇宙機の多くは、断熱材（MLI: Multi-Layer Insulation）の金色・銀色の箔で覆われており、表面にはSIFTやORBのような特徴点検出に適したテクスチャがほとんどありません。地上の物体認識で当たり前に使える「模様」が存在しないのです。
計算資源の制約: 宇宙で使えるコンピュータは放射線耐性が求められるため、地上のGPUに比べて性能が大幅に劣ります。リアルタイムで姿勢を推定するには、限られた計算資源で動作する効率的なアルゴリズムが必要です。
対象物体の事前知識の限界: 非協力物体の場合、正確な3Dモデルが入手できないことがあります。故障前のCADデータがあっても、太陽パネルの破損やアンテナの変形など、実際の形状が異なる可能性があります。
これらの困難を乗り越えるために、多くの手法が研究されてきました。まずは、姿勢推定問題を数学的にどう定式化するかを見ていきましょう。
6DoF姿勢推定の数学的定式化
姿勢推定とは何を求めるのか
宇宙空間で対象物体の「姿勢推定」と言ったとき、厳密には6自由度（6DoF: 6 Degrees of Freedom）のポーズ推定を意味します。ポーズとは、物体の位置（3自由度）と姿勢（3自由度）を合わせた6つのパラメータです。
日常的な感覚で言えば、「対象物体がカメラからどの方向にどれだけ離れていて、どちらを向いているか」を知ることです。引っ越しの際に大きな家具をドアから搬入するとき、家具の位置と向きの両方を正確に把握しないとぶつかってしまいます。宇宙でのランデブーも同じで、対象物体の位置と向きが正確にわからなければ、衝突事故やロボットアームの把持失敗につながります。
座標系と変換の定義
カメラ座標系 $\{C\}$ と物体座標系 $\{B\}$ の間の関係を、回転行列 $\bm{R} \in SO(3)$ と並進ベクトル $\bm{t} \in \mathbb{R}^3$ で表現します。
物体座標系で表された点 $\bm{p}_B$ をカメラ座標系に変換する式は次の通りです。
$$ \bm{p}_C = \bm{R} \bm{p}_B + \bm{t} $$
ここで $\bm{R}$ は $3 \times 3$ の回転行列で、$\bm{R}^T \bm{R} = \bm{I}$ かつ $\det(\bm{R}) = 1$ を満たします。$\bm{t}$ はカメラ座標系における物体の原点の位置です。
この変換を同次座標で表すと、$4 \times 4$ の変換行列 $\bm{T}$ にまとめられます。
$$ \bm{T} = \begin{bmatrix} \bm{R} & \bm{t} \\ \bm{0}^T & 1 \end{bmatrix} \in SE(3) $$
$SE(3)$ は特殊ユークリッド群と呼ばれ、3次元空間の剛体変換（回転＋並進）の集合です。6DoF姿勢推定の目標は、画像観測からこの $\bm{T}$（すなわち $\bm{R}$ と $\bm{t}$）を推定することです。
カメラモデルとの関係
物体上の3次元点 $\bm{p}_C = (X, Y, Z)^T$（カメラ座標系）が画像上のピクセル座標 $(u, v)$ に射影される関係は、ピンホールカメラモデルで記述されます。
$$ \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} \sim \bm{K} \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} $$
ここで $\sim$ は「スカラー倍を除いて等しい」を意味し、$\bm{K}$ はカメラ内部パラメータ行列です。$f_x, f_y$ は焦点距離（ピクセル単位）、$(c_x, c_y)$ は画像中心です。
この射影関係と先ほどの座標変換を組み合わせると、物体座標系の3D点 $\bm{p}_B$ から画像座標 $(u, v)$ への全体的な変換が得られます。
$$ \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} \sim \bm{K} \left[ \bm{R} \mid \bm{t} \right] \begin{bmatrix} \bm{p}_B \\ 1 \end{bmatrix} $$
姿勢推定とは、画像上の観測（2D点、エッジ、シルエットなど）と物体の3Dモデルの間の対応関係を用いて、この $\bm{R}$ と $\bm{t}$ を逆問題として解くことに他なりません。
回転の表現方法
回転行列 $\bm{R}$ は9つの要素を持ちますが、直交条件 $\bm{R}^T \bm{R} = \bm{I}$ の6つの拘束により、実質的な自由度は3です。回転を表現するにはいくつかの方法があります。
オイラー角 $(\phi, \theta, \psi)$: 3つの角度で回転を記述する方法で直感的ですが、ジンバルロック（特定の角度で自由度が縮退する問題）が生じます。
回転ベクトル（軸角表現） $\bm{\omega} = \theta \bm{e}$: 回転軸 $\bm{e}$ と回転角 $\theta$ で表す方法です。ロドリゲスの公式により回転行列に変換できます。
$$ \bm{R} = \bm{I} + \sin\theta [\bm{e}]_\times + (1 – \cos\theta)[\bm{e}]_\times^2 $$
ここで $[\bm{e}]_\times$ は $\bm{e}$ の歪対称行列です。
クォータニオン $\bm{q} = (q_w, q_x, q_y, q_z)$: 4つの成分（$\|\bm{q}\| = 1$ の拘束付き）で回転を表す方法で、ジンバルロックがなく、補間が容易です。カルマンフィルタとの相性がよいため、宇宙での姿勢追跡によく使われます。
それでは、具体的な姿勢推定手法を見ていきましょう。最初に、物体の輪郭情報を利用するエッジベース手法を解説します。
エッジベース手法 — 物体輪郭のマッチング
エッジが宇宙で有利な理由
テクスチャの少ない宇宙機に対して、特徴点（SIFT, ORBなど）ベースの手法はしばしば失敗します。金色のMLI箔や白い断熱材で覆われた表面には、安定して検出できるテクスチャ特徴がほとんどないからです。
一方、宇宙機のエッジ（輪郭線）は比較的ロバストに検出できます。衛星の本体と太陽パネルの境界、アンテナの輪郭、円筒形タンクのシルエットなどは、照明条件が変わっても画像中に明瞭なエッジとして現れます。大気がないため、背景は真っ黒な宇宙空間であり、物体のシルエットは非常にくっきりとしています。
エッジベース手法の基本的な考え方は、3Dモデルの予測されたエッジと画像中の実際のエッジを合わせることで姿勢を推定するというものです。
アルゴリズムの流れ
エッジベース姿勢推定の一般的な手順は以下の通りです。
ステップ1: 初期姿勢の仮定
前フレームの姿勢推定結果や、粗い初期推定（例えばテンプレートマッチングによる）から出発します。初期姿勢を $\bm{R}_0, \bm{t}_0$ とします。
ステップ2: モデルエッジの射影
3Dモデルの各エッジを、仮定した姿勢 $(\bm{R}_0, \bm{t}_0)$ でカメラに射影し、画像上の予測エッジを生成します。3Dモデルのエッジ上の点 $\bm{p}_B^{(i)}$ を射影した2D点を $\bm{m}_i^{\text{proj}}$ とします。
ステップ3: 対応するエッジの探索
射影されたエッジの各点から、法線方向に沿って画像中のエッジを探索します。射影点 $\bm{m}_i^{\text{proj}}$ の近傍で、エッジの法線方向に沿って画像の勾配が最大の点 $\bm{m}_i^{\text{obs}}$ を見つけます。これが対応する観測エッジ点です。
ステップ4: 姿勢の最適化
射影エッジ点と観測エッジ点の距離を最小化するように姿勢を更新します。これは次の最適化問題として定式化されます。
$$ (\bm{R}^*, \bm{t}^*) = \arg\min_{\bm{R}, \bm{t}} \sum_{i=1}^{N} \rho\left( d(\bm{m}_i^{\text{obs}}, \bm{m}_i^{\text{proj}}(\bm{R}, \bm{t})) \right) $$
ここで $d(\cdot, \cdot)$ は2点間の距離、$\rho(\cdot)$ はロバスト損失関数（Hubber関数やTukey関数）です。ロバスト損失関数を使う理由は、エッジの誤対応（外れ値）の影響を抑えるためです。
ステップ5: 反復
ステップ2〜4を収束するまで繰り返します。各反復で姿勢が改善され、エッジの対応も改善されます。
エッジ距離の定式化
エッジ法線方向の距離をより正確に定義しましょう。射影点 $\bm{m}_i^{\text{proj}}$ におけるモデルエッジの法線方向を $\bm{n}_i$ とします。観測エッジ点 $\bm{m}_i^{\text{obs}}$ との距離は、法線方向への射影として計算します。
$$ d_i = \bm{n}_i^T (\bm{m}_i^{\text{obs}} – \bm{m}_i^{\text{proj}}) $$
この $d_i$ を小さくするように姿勢パラメータを更新します。姿勢の微小変化 $\delta \bm{\xi} = (\delta \bm{\omega}, \delta \bm{v})^T \in \mathbb{R}^6$（回転の微小変化3成分 + 並進の微小変化3成分）に対して、射影点の変化を1次近似すると次のヤコビアンが得られます。
$$ \bm{m}_i^{\text{proj}}(\bm{\xi} + \delta\bm{\xi}) \approx \bm{m}_i^{\text{proj}}(\bm{\xi}) + \bm{J}_i \delta\bm{\xi} $$
$\bm{J}_i$ は $2 \times 6$ のヤコビアン行列で、3D点の射影の姿勢パラメータに対する微分です。
法線方向の距離に対するヤコビアンは次のように計算されます。
$$ \frac{\partial d_i}{\partial \bm{\xi}} = \bm{n}_i^T \bm{J}_i $$
これは $1 \times 6$ のベクトルになり、ガウス・ニュートン法による更新式は次の通りです。
$$ \delta\bm{\xi}^* = -\left(\sum_{i} \bm{J}_i^T \bm{n}_i \bm{n}_i^T \bm{J}_i\right)^{-1} \sum_{i} \bm{J}_i^T \bm{n}_i d_i $$
この $\delta\bm{\xi}^*$ を使って姿勢を更新し、収束するまで反復します。
エッジベース手法の長所と短所
長所: – テクスチャレスな宇宙機に対してロバスト – 計算量が比較的軽い（エッジ検出は効率的） – 照明条件の変化に対してある程度頑健
短所: – 初期姿勢の推定が不正確だと収束しない（局所解に陥る） – 複雑な形状やオクルージョン（遮蔽）に弱い – エッジの誤対応が姿勢推定精度に直結する
エッジベース手法は主に近距離でのファインアライメント（精密位置合わせ）に使われます。では次に、3Dモデル全体の形状を活用するモデルベース手法を見ていきましょう。
モデルベース手法 — 3DモデルとのICP的マッチング
モデルベース手法の考え方
モデルベース手法は、対象物体の3D形状モデル（CADデータやワイヤーフレームモデル）を事前に保持し、そのモデルをカメラ映像と照合して姿勢を推定する手法です。
身近な例で言えば、ジグソーパズルのピースを嵌め込むようなものです。手元にあるピースの形（3Dモデル）と、パズル台の空いている形（画像中の物体の見え方）を比較して、ぴったり合う位置と向きを見つけます。3Dモデルが正確であるほど、正確な姿勢推定が可能になります。
Iterative Closest Point (ICP) の原理
モデルベース手法の代表格がICP（Iterative Closest Point）アルゴリズムです。元々は3D点群同士の位置合わせに開発されたICPですが、その考え方は姿勢推定にも広く応用されています。
ICPの基本的な考え方は非常にシンプルです。
- 現在の姿勢推定値で、モデルの3D点を画像に射影する
- 射影された各点に最も近い観測点（画像特徴やデプス点群）を見つける（最近傍探索）
- 対応する点のペア間の距離を最小化するように姿勢を更新する
- 収束するまで1〜3を繰り返す
数学的には、ICPは次の最小化問題を反復的に解きます。
$$ (\bm{R}^*, \bm{t}^*) = \arg\min_{\bm{R}, \bm{t}} \sum_{i=1}^{N} \left\| \bm{q}_i – (\bm{R}\bm{p}_i + \bm{t}) \right\|^2 $$
ここで $\bm{p}_i$ はモデル上の点、$\bm{q}_i$ は $\bm{R}\bm{p}_i + \bm{t}$ に最も近い観測点です。
対応関係が与えられた場合の閉形式解
ICPの各反復において、対応関係 $\{(\bm{p}_i, \bm{q}_i)\}$ が固定されている場合、最適な $\bm{R}$ と $\bm{t}$ はSVD（特異値分解）を使って閉形式で求められます。
まず、各点群の重心を計算します。
$$ \bar{\bm{p}} = \frac{1}{N}\sum_{i=1}^{N} \bm{p}_i, \quad \bar{\bm{q}} = \frac{1}{N}\sum_{i=1}^{N} \bm{q}_i $$
重心を引いた相対座標を定義します。
$$ \bm{p}_i’ = \bm{p}_i – \bar{\bm{p}}, \quad \bm{q}_i’ = \bm{q}_i – \bar{\bm{q}} $$
相互共分散行列を計算します。
$$ \bm{H} = \sum_{i=1}^{N} \bm{p}_i’ {\bm{q}_i’}^T $$
$\bm{H}$ のSVDを $\bm{H} = \bm{U}\bm{\Sigma}\bm{V}^T$ とすると、最適回転は次の式で得られます。
$$ \bm{R}^* = \bm{V} \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & \det(\bm{V}\bm{U}^T) \end{bmatrix} \bm{U}^T $$
対角行列の $(3,3)$ 成分に $\det(\bm{V}\bm{U}^T)$ を入れるのは、反射を含まない正しい回転を保証するためです。最適並進は次の式で求まります。
$$ \bm{t}^* = \bar{\bm{q}} – \bm{R}^* \bar{\bm{p}} $$
この解はArun et al. (1987)によって示された古典的な結果で、3D-3Dの点群位置合わせにおいて最も基本的なアルゴリズムです。
PnP問題 — 2D-3D対応からの姿勢推定
実際の宇宙での姿勢推定では、デプスカメラが使えない場合も多く、単眼カメラの画像（2D情報）と3Dモデルの対応から姿勢を求める必要があります。これはPnP（Perspective-n-Point）問題と呼ばれます。
$n$ 個の3D点 $\bm{P}_i$ とそれらの2D射影 $\bm{m}_i$ が与えられたとき、$\bm{R}$ と $\bm{t}$ を求める問題です。
$$ \lambda_i \begin{bmatrix} m_{i,x} \\ m_{i,y} \\ 1 \end{bmatrix} = \bm{K}(\bm{R}\bm{P}_i + \bm{t}) $$
$\lambda_i > 0$ はデプスに対応するスケール因子です。
最低4点（非共面条件を満たす場合は3点 + 1点の曖昧さ解消）で解が求まります。代表的なアルゴリズムとしてEPnP（Efficient PnP）やP3P+RANSACがあります。特にEPnPは $O(n)$ の計算量で効率的に動作し、宇宙のリアルタイムシステムにも適しています。
宇宙でのモデルベース手法の課題
モデルベース手法を宇宙で使うにあたり、いくつかの実践的な課題があります。
3Dモデルの正確性: 対象が非協力物体の場合、正確な3Dモデルが常に利用可能とは限りません。衛星のCADデータが公開されていないケースや、デブリ化して形状が変わっているケースもあります。このような場合、粗い幾何形状モデル（円筒、直方体、平板の組み合わせ）で近似する戦略が取られます。
初期化問題: ICPやPnPは局所最適化手法であるため、適切な初期値が必要です。全く初期推定がない状態（例えばランデブーの初期フェーズ）では、テンプレートマッチングやディスクリプタベースの粗い初期推定が先行して行われます。
オクルージョンの処理: 物体の一部が視野外に出たり、他の物体に遮られたりすると、モデルの一部しか画像に映りません。このような状況では、可視部分のみを用いたロバストな推定が必要です。
これらの手法が安定して動作するためには、照明条件の変化に対処する技術が不可欠です。次にこの点を掘り下げましょう。
照明条件の変化への対処
宇宙の照明環境
LEO（低軌道）を周回する衛星は、約90分の軌道周期のうち約60分が日照、約30分が地球の影（食）に入ります。日照と食の遷移は数秒〜数分で起こり、この間に画像のコントラストや明るさが劇的に変化します。
さらに厄介なのは、対象物体自体が回転している場合です。回転する物体に太陽光が当たると、見える面の明暗パターンが常に変化します。反射率の高い金属面があると、鏡面反射（スペキュラー）で局所的に極めて明るい「ホットスポット」が現れることもあります。
照明変化への対策手法
画像前処理によるアプローチ
最も基本的な対策は、画像前処理段階で照明変化の影響を低減することです。
ヒストグラム均等化: 画像の輝度ヒストグラムを均一に引き伸ばすことで、コントラストを改善します。日照/食の遷移時の全体的な明るさ変化に効果的です。
適応的閾値処理: 局所領域ごとに閾値を変えることで、画像の一部が影で暗くなっていてもエッジやシルエットを検出できます。
勾配ベースの表現: 画像の輝度値そのものではなく、勾配（エッジ方向と強度）を使うことで、絶対的な明るさの変化に不変な特徴を抽出できます。
照明モデルの統合
より高度なアプローチは、レンダリングベースの照明モデルを姿勢推定に統合することです。
対象物体の3Dモデルと推定姿勢に基づいて、太陽光の方向を考慮した合成画像をレンダリングし、実画像と比較します。照明条件を既知とすることで、影の境界すら姿勢推定の手がかりに変わります。
Phong反射モデルを使った場合、物体表面の点 $\bm{p}$ での輝度 $I(\bm{p})$ は次のように表されます。
$$ I(\bm{p}) = k_a I_a + k_d (\bm{n} \cdot \bm{l}) I_d + k_s (\bm{r} \cdot \bm{v})^{\alpha} I_s $$
ここで、$\bm{n}$ は表面法線、$\bm{l}$ は光源方向、$\bm{v}$ は視線方向、$\bm{r}$ は反射方向です。$k_a, k_d, k_s$ はそれぞれ環境光・拡散反射・鏡面反射の係数、$\alpha$ は鏡面反射の鋭さを制御する指数です。
太陽の方向ベクトル $\bm{l}$ は軌道位置から計算できるため、レンダリング画像を生成する際の既知パラメータとして利用できます。
セルフシャドウの活用
物体の自己影（self-shadow）は、通常は姿勢推定の障害と見なされますが、逆に有用な情報源にもなり得ます。影の境界線は物体の3D形状と太陽光の方向から幾何学的に決まるため、影のパターンが姿勢の拘束条件を提供します。
影の境界上の点 $\bm{p}_s$ では、表面法線 $\bm{n}(\bm{p}_s)$ と光源方向 $\bm{l}$ が直交する条件が成り立ちます。
$$ \bm{n}(\bm{p}_s) \cdot \bm{l} = 0 $$
この条件を姿勢推定の最適化に拘束として追加することで、影の情報を積極的に活用できます。
マルチスペクトルカメラの利用
一部のミッションでは、可視光カメラに加えて赤外線カメラを搭載しています。赤外線画像では、太陽光の反射ではなく物体自身の熱放射を捉えるため、影の影響を受けにくいです。可視光と赤外線の情報を融合することで、昼夜を問わず安定した姿勢推定が可能になります。
照明条件への対処を組み込んだとしても、単一フレームの推定には常にノイズが含まれます。連続フレームにわたって姿勢を滑らかに追跡するために、カルマンフィルタが活躍します。
カルマンフィルタによる姿勢追跡
なぜフィルタリングが必要か
単一フレームごとに姿勢を推定すると、画像ノイズや部分的なオクルージョンの影響で推定値がフレームごとに「ジャンプ」することがあります。実際のランデブー運用では、姿勢推定値に基づいてスラスタを噴射するため、推定値の急激な変動は制御系の不安定化を招きます。
カルマンフィルタ（またはその拡張版）を使うことで、以下のメリットが得られます。
- ノイズ低減: 複数フレームの情報を統合して推定精度を向上
- 予測能力: 画像が一時的に得られない場合（日食時やカメラ故障時）でも、物体の運動モデルに基づいて姿勢を予測
- 外れ値耐性: 突発的な誤推定を過去の推定値との整合性からフィルタリング
状態ベクトルの設計
姿勢追跡のための状態ベクトルを設計します。対象物体の姿勢と角速度を追跡するため、クォータニオンベースの状態ベクトルを採用します。
$$ \bm{x} = \begin{bmatrix} \bm{q} \\ \bm{\omega} \\ \bm{t} \\ \dot{\bm{t}} \end{bmatrix} \in \mathbb{R}^{13} $$
ここで、$\bm{q} = (q_w, q_x, q_y, q_z)^T$ は回転を表すクォータニオン（4成分）、$\bm{\omega} = (\omega_x, \omega_y, \omega_z)^T$ は角速度（3成分）、$\bm{t} = (t_x, t_y, t_z)^T$ は並進位置（3成分）、$\dot{\bm{t}} = (\dot{t}_x, \dot{t}_y, \dot{t}_z)^T$ は並進速度（3成分）です。
クォータニオンは $\|\bm{q}\| = 1$ の拘束があるため、実質の自由度は12（= 3 + 3 + 3 + 3）です。
運動モデル（予測ステップ）
非協力物体は外部トルクが作用しない限り、角運動量が保存されます。最も単純なモデルとして、等角速度モデルを採用しましょう。これは、短い時間ステップ $\Delta t$ の間、角速度がほぼ一定であるという仮定です。
クォータニオンの時間発展は次の微分方程式で記述されます。
$$ \dot{\bm{q}} = \frac{1}{2} \bm{q} \otimes \begin{bmatrix} 0 \\ \bm{\omega} \end{bmatrix} $$
$\otimes$ はクォータニオン積です。離散時間では、角速度 $\bm{\omega}$ が一定なら次のステップのクォータニオンは回転クォータニオン $\delta\bm{q}$ を掛けて更新されます。
$$ \bm{q}_{k+1} = \bm{q}_k \otimes \delta\bm{q} $$
ここで $\delta\bm{q}$ は角速度 $\bm{\omega}$ と時間ステップ $\Delta t$ から計算される微小回転のクォータニオンです。回転角を $\theta = \|\bm{\omega}\| \Delta t$、回転軸を $\bm{e} = \bm{\omega} / \|\bm{\omega}\|$ とすると、次のように表されます。
$$ \delta\bm{q} = \begin{bmatrix} \cos(\theta/2) \\ \sin(\theta/2) \bm{e} \end{bmatrix} $$
並進については、等速直線運動モデルを仮定します。
$$ \bm{t}_{k+1} = \bm{t}_k + \dot{\bm{t}}_k \Delta t $$
$$ \dot{\bm{t}}_{k+1} = \dot{\bm{t}}_k $$
これらをまとめて、状態の予測ステップとします。
観測モデル（更新ステップ）
画像ベースの姿勢推定（エッジベースやモデルベース手法）による推定結果を「観測」として扱います。観測ベクトルは次のように設計します。
$$ \bm{z}_k = \begin{bmatrix} \bm{q}_{\text{meas}} \\ \bm{t}_{\text{meas}} \end{bmatrix} \in \mathbb{R}^{7} $$
ここで $\bm{q}_{\text{meas}}$ は画像から推定された回転クォータニオン、$\bm{t}_{\text{meas}}$ は推定された並進ベクトルです。
観測モデルは状態ベクトルからの単純な抽出として近似できます。
$$ \bm{z}_k = \bm{H}\bm{x}_k + \bm{v}_k $$
$\bm{v}_k \sim \mathcal{N}(\bm{0}, \bm{R}_k)$ は観測ノイズです。
拡張カルマンフィルタ (EKF) の適用
クォータニオンの非線形性とノルム拘束のため、線形カルマンフィルタをそのまま適用することは困難です。そこで拡張カルマンフィルタ（EKF）を使います。
EKFでは、非線形の運動モデル $\bm{x}_{k+1} = f(\bm{x}_k)$ をヤコビアン $\bm{F}_k$ で線形化します。
$$ \bm{F}_k = \left.\frac{\partial f}{\partial \bm{x}}\right|_{\bm{x}=\hat{\bm{x}}_k} $$
予測ステップでは、次の式で状態と共分散を更新します。
$$ \hat{\bm{x}}_{k+1|k} = f(\hat{\bm{x}}_{k|k}) $$
$$ \bm{P}_{k+1|k} = \bm{F}_k \bm{P}_{k|k} \bm{F}_k^T + \bm{Q}_k $$
$\bm{Q}_k$ はプロセスノイズの共分散行列で、角速度の変動や並進加速度の不確実性をモデル化します。
更新ステップでは、カルマンゲイン $\bm{K}_k$ を計算して観測を統合します。
$$ \bm{K}_k = \bm{P}_{k|k-1} \bm{H}^T (\bm{H}\bm{P}_{k|k-1}\bm{H}^T + \bm{R}_k)^{-1} $$
$$ \hat{\bm{x}}_{k|k} = \hat{\bm{x}}_{k|k-1} + \bm{K}_k (\bm{z}_k – \bm{H}\hat{\bm{x}}_{k|k-1}) $$
$$ \bm{P}_{k|k} = (\bm{I} – \bm{K}_k \bm{H}) \bm{P}_{k|k-1} $$
更新後にクォータニオンの正規化 $\hat{\bm{q}} \leftarrow \hat{\bm{q}} / \|\hat{\bm{q}}\|$ を忘れないことが重要です。
乗法EKF（MEKF）
クォータニオンの特殊性を正しく扱うために、乗法EKF（Multiplicative EKF: MEKF）がしばしば使われます。MEKFの核心的なアイデアは、クォータニオンの誤差を加法的（$\delta\bm{q} = \hat{\bm{q}} – \bm{q}$）ではなく、乗法的（$\bm{q} = \delta\bm{q} \otimes \hat{\bm{q}}$）に表現することです。
誤差クォータニオン $\delta\bm{q}$ が小さいとき、Gibbs vector $\bm{a} \approx 2(q_x, q_y, q_z)^T$ で3次元パラメータ化できるため、共分散行列のサイズが $3 \times 3$ で済み、ノルム拘束も自然に保たれます。
この手法は宇宙機の姿勢決定で標準的に使われており、非協力物体の追跡にも応用されています。
それでは、実際のミッションでこれらの技術がどのように使われてきたかを見てみましょう。
宇宙ミッションでの実例
PRISMA ミッション（2010年）
スウェーデンのSSC（Swedish Space Corporation）が主導したPRISMAミッションは、軌道上での近接運用技術を実証した先駆的なミッションです。2機の衛星（MangaとTango）が編隊飛行を行い、非協力ランデブーの技術を実証しました。
PRISMAでは、VBS（Vision-Based Sensor）と呼ばれるカメラシステムが搭載され、対象衛星Tangoの姿勢推定が行われました。遠距離では星の中からTangoを光点として検出し、近距離ではシルエットとエッジを用いたモデルベース姿勢推定が実行されました。
PRISMAの成果として特筆すべきは、カメラ情報のみで数mの精度でのランデブーに成功したことです。非協力ランデブーの実用可能性を世界で初めて宇宙で実証しました。
RemoveDEBRIS ミッション（2018年）
英サリー宇宙センター（SSC）が主導したRemoveDEBRISは、デブリ除去技術の宇宙実証ミッションです。ISS（国際宇宙ステーション）から放出された母機が、模擬デブリに対して複数の捕獲技術（ネット、銛）を実証しました。
このミッションでは、LiDARとカメラのフュージョンによる姿勢推定が実施されました。LiDARで対象までの距離と3D形状を取得し、カメラ映像と組み合わせることで、回転するデブリのリアルタイムな姿勢追跡が行われました。
RemoveDEBRISの教訓として重要なのは、実際の宇宙環境での照明条件が地上テストと大きく異なったという報告です。地上でうまく動作したアルゴリズムが、宇宙の極端なコントラストで一部失敗し、ロバスト性の向上が必要であることが明らかになりました。
ESA e.Deorbit / ClearSpace-1（2026年打上げ予定）
ESA（欧州宇宙機関）のClearSpace-1は、宇宙機による初の「自律デブリ捕獲」を目指すミッションです。VESPAアダプタ（Vega上段の一部）をターゲットとし、4本のロボットアームで捕獲します。
このミッションでは、モデルベース手法と深層学習の組み合わせが計画されており、遠距離ではCNNベースの初期検出と姿勢推定、近距離ではモデルベースのエッジトラッキングが段階的に実行されます。
地上検証施設
実ミッションの前に、地上でアルゴリズムを検証するための施設も重要です。DLR（ドイツ航空宇宙センター）のEPOS（European Proximity Operations Simulator）やESAのGNC Rendezvous and Docking Simulatorでは、ロボットアームに取り付けた衛星模型とカメラを使い、宇宙環境を模擬した照明条件下でアルゴリズムのテストが行われています。
これらのミッションから得られた知見を踏まえて、Pythonで簡単なモデルベース姿勢推定を実装してみましょう。
Pythonでのモデルベース姿勢推定の実装
実装の概要
ここでは、3Dモデルの頂点と画像上の2D点の対応関係からPnP問題を解き、さらにICPの反復により姿勢を精密化するパイプラインを実装します。対象は簡単な衛星模型（直方体 + 太陽パネル）とし、合成データを使って動作を確認します。
まず、衛星の3Dモデルを定義し、合成的に画像座標を生成するコードを書きます。
import numpy as np
from scipy.spatial.transform import Rotation
# --- 衛星の3Dモデル（直方体ボディ + 太陽パネル）---
# ボディ: 1.0 x 0.8 x 0.6 m の直方体
body_half = np.array([0.5, 0.4, 0.3])
body_vertices = np.array([
[-1, -1, -1], [-1, -1, 1], [-1, 1, -1], [-1, 1, 1],
[ 1, -1, -1], [ 1, -1, 1], [ 1, 1, -1], [ 1, 1, 1]
], dtype=float) * body_half
# 太陽パネル: ボディの両側に伸びる平板
panel_left = np.array([
[-0.5, -1.2, -0.02], [-0.5, -1.2, 0.02],
[-0.5, -0.4, -0.02], [-0.5, -0.4, 0.02],
[ 0.5, -1.2, -0.02], [ 0.5, -1.2, 0.02],
[ 0.5, -0.4, -0.02], [ 0.5, -0.4, 0.02],
])
panel_right = panel_left.copy()
panel_right[:, 1] = -panel_left[:, 1]
model_points = np.vstack([body_vertices, panel_left, panel_right])
print(f"モデル点数: {model_points.shape[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_true = Rotation.from_euler('ZYX', [25, -15, 10], degrees=True).as_matrix()
t_true = np.array([0.5, -0.3, 5.0]) # 5m先にある物体
def project_points(points_3d, R, t, K):
"""3D点をカメラに射影して2D座標を返す"""
points_cam = (R @ points_3d.T).T + t
# z > 0 のみ射影（カメラの前にある点）
valid = points_cam[:, 2] > 0
proj = np.zeros((len(points_3d), 2))
proj[valid, 0] = fx * points_cam[valid, 0] / points_cam[valid, 2] + cx
proj[valid, 1] = fy * points_cam[valid, 1] / points_cam[valid, 2] + cy
return proj, valid
# 真の姿勢で射影
proj_true, valid = project_points(model_points, R_true, t_true, K)
# 観測ノイズを加える（ピクセル単位の標準偏差 2.0）
noise_std = 2.0
np.random.seed(42)
proj_obs = proj_true + np.random.randn(*proj_true.shape) * noise_std
print(f"射影点の範囲: x=[{proj_obs[:,0].min():.1f}, {proj_obs[:,0].max():.1f}], "
f"y=[{proj_obs[:,1].min():.1f}, {proj_obs[:,1].max():.1f}]")
このコードでは、直方体ボディと太陽パネルで構成された簡易衛星モデルの24個の頂点を定義し、真の姿勢で射影した後にガウスノイズ（標準偏差2ピクセル）を加えて模擬観測を生成しています。5m先にある衛星を焦点距離800ピクセルのカメラで撮影する状況を想定しています。
次に、PnP問題を解いて初期姿勢を推定し、その後ICP的な反復で精密化するアルゴリズムを実装します。
import numpy as np
from scipy.spatial.transform import Rotation
from scipy.optimize import least_squares
def reprojection_error(params, points_3d, points_2d, K):
"""再射影誤差を計算する（最適化の目的関数）"""
# params: [rx, ry, rz, tx, ty, tz] - 回転ベクトル + 並進
rvec = params[:3]
tvec = params[3:6]
R = Rotation.from_rotvec(rvec).as_matrix()
# 3D点をカメラ座標に変換
pts_cam = (R @ points_3d.T).T + tvec
# 射影
fx, fy = K[0, 0], K[1, 1]
cx, cy = K[0, 2], K[1, 2]
proj_x = fx * pts_cam[:, 0] / pts_cam[:, 2] + cx
proj_y = fy * pts_cam[:, 1] / pts_cam[:, 2] + cy
# 残差ベクトル
residuals = np.zeros(2 * len(points_3d))
residuals[0::2] = proj_x - points_2d[:, 0]
residuals[1::2] = proj_y - points_2d[:, 1]
return residuals
def solve_pnp_nonlinear(points_3d, points_2d, K, R_init, t_init):
"""非線形最小二乗法でPnP問題を解く"""
rvec_init = Rotation.from_matrix(R_init).as_rotvec()
params_init = np.concatenate([rvec_init, t_init])
result = least_squares(
reprojection_error, params_init,
args=(points_3d, points_2d, K),
method='lm' # Levenberg-Marquardt法
)
R_est = Rotation.from_matrix(
Rotation.from_rotvec(result.x[:3]).as_matrix()
).as_matrix()
t_est = result.x[3:6]
return R_est, t_est, result
# 初期推定値（真値に摂動を加えたもの）
R_init = Rotation.from_euler('ZYX', [30, -10, 15], degrees=True).as_matrix()
t_init = np.array([0.6, -0.2, 5.2])
# PnPを解く
R_est, t_est, result = solve_pnp_nonlinear(
model_points, proj_obs, K, R_init, t_init
)
# 誤差の計算
rot_error = Rotation.from_matrix(R_est @ R_true.T).magnitude()
trans_error = np.linalg.norm(t_est - t_true)
print(f"回転誤差: {np.degrees(rot_error):.4f} deg")
print(f"並進誤差: {trans_error:.6f} m")
print(f"再射影誤差RMS: {np.sqrt(np.mean(result.fun**2)):.4f} px")
このコードでは、Levenberg-Marquardt法による非線形最小二乗法でPnP問題を解いています。初期推定値は真値から5度・0.2m程度ずらしたものを使っています。回転誤差は回転行列の差から回転角度を計算し、並進誤差はユークリッド距離で評価しています。ノイズ付きの24点の対応があれば、度単位以下の回転精度とミリメートル以下の並進精度が達成できることが確認できます。
続いて、ICP風の反復精密化と結果の可視化を行います。
import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation
from scipy.optimize import least_squares
# --- ICP的反復精密化 ---
def icp_refinement(model_pts, obs_2d, K, R_init, t_init, n_iter=10):
"""ICP風の反復姿勢精密化"""
R_cur = R_init.copy()
t_cur = t_init.copy()
errors = []
for i in range(n_iter):
# 現在の姿勢で射影
pts_cam = (R_cur @ model_pts.T).T + t_cur
fx, fy = K[0, 0], K[1, 1]
cx, cy = K[0, 2], K[1, 2]
proj_x = fx * pts_cam[:, 0] / pts_cam[:, 2] + cx
proj_y = fy * pts_cam[:, 1] / pts_cam[:, 2] + cy
proj_cur = np.stack([proj_x, proj_y], axis=1)
# 再射影誤差
reproj_err = np.sqrt(np.mean((proj_cur - obs_2d)**2))
errors.append(reproj_err)
# 非線形最適化で姿勢更新
rvec = Rotation.from_matrix(R_cur).as_rotvec()
params = np.concatenate([rvec, t_cur])
def cost(p):
R_tmp = Rotation.from_rotvec(p[:3]).as_matrix()
t_tmp = p[3:6]
pc = (R_tmp @ model_pts.T).T + t_tmp
px = fx * pc[:, 0] / pc[:, 2] + cx
py = fy * pc[:, 1] / pc[:, 2] + cy
res = np.zeros(2 * len(model_pts))
res[0::2] = px - obs_2d[:, 0]
res[1::2] = py - obs_2d[:, 1]
return res
result = least_squares(cost, params, method='lm')
R_cur = Rotation.from_rotvec(result.x[:3]).as_matrix()
t_cur = result.x[3:6]
return R_cur, t_cur, errors
# 粗い初期推定から開始（真値から10度、0.5mずれている）
R_coarse = Rotation.from_euler('ZYX', [35, -5, 20], degrees=True).as_matrix()
t_coarse = np.array([1.0, -0.8, 5.5])
R_refined, t_refined, errors = icp_refinement(
model_points, proj_obs, K, R_coarse, t_coarse, n_iter=15
)
# --- 結果の可視化 ---
fig, axes = plt.subplots(1, 3, figsize=(18, 5))
# (1) 収束曲線
axes[0].semilogy(range(len(errors)), errors, 'b-o', markersize=4)
axes[0].set_xlabel('Iteration')
axes[0].set_ylabel('RMS Reprojection Error [px]')
axes[0].set_title('ICP Refinement Convergence')
axes[0].grid(True, alpha=0.3)
# (2) 射影点の比較
proj_init, _ = project_points(model_points, R_coarse, t_coarse, K)
proj_final, _ = project_points(model_points, R_refined, t_refined, K)
axes[1].scatter(proj_obs[:, 0], proj_obs[:, 1], c='red', s=30,
label='Observed (noisy)', zorder=3)
axes[1].scatter(proj_init[:, 0], proj_init[:, 1], c='gray', s=30,
marker='x', label='Initial estimate', alpha=0.6)
axes[1].scatter(proj_final[:, 0], proj_final[:, 1], c='blue', s=30,
marker='+', label='Refined estimate', linewidths=2)
axes[1].set_xlabel('u [px]')
axes[1].set_ylabel('v [px]')
axes[1].set_title('Projected Points Comparison')
axes[1].legend()
axes[1].set_xlim(0, 640)
axes[1].set_ylim(480, 0) # 画像座標は上下反転
axes[1].grid(True, alpha=0.3)
# (3) 各反復での回転・並進誤差
rot_errors = []
trans_errors = []
R_tmp = R_coarse.copy()
t_tmp = t_coarse.copy()
for i in range(15):
pts_cam = (R_tmp @ model_points.T).T + t_tmp
fx, fy = K[0, 0], K[1, 1]
cx, cy = K[0, 2], K[1, 2]
def cost(p):
R_t = Rotation.from_rotvec(p[:3]).as_matrix()
t_t = p[3:6]
pc = (R_t @ model_points.T).T + t_t
px = fx * pc[:, 0] / pc[:, 2] + cx
py = fy * pc[:, 1] / pc[:, 2] + cy
res = np.zeros(2 * len(model_points))
res[0::2] = px - proj_obs[:, 0]
res[1::2] = py - proj_obs[:, 1]
return res
rvec = Rotation.from_matrix(R_tmp).as_rotvec()
params = np.concatenate([rvec, t_tmp])
result = least_squares(cost, params, method='lm')
R_tmp = Rotation.from_rotvec(result.x[:3]).as_matrix()
t_tmp = result.x[3:6]
rot_err = np.degrees(Rotation.from_matrix(R_tmp @ R_true.T).magnitude())
trans_err = np.linalg.norm(t_tmp - t_true)
rot_errors.append(rot_err)
trans_errors.append(trans_err)
ax_rot = axes[2]
ax_trans = ax_rot.twinx()
l1, = ax_rot.semilogy(range(len(rot_errors)), rot_errors, 'r-o',
markersize=4, label='Rotation error [deg]')
l2, = ax_trans.semilogy(range(len(trans_errors)), trans_errors, 'b-s',
markersize=4, label='Translation error [m]')
ax_rot.set_xlabel('Iteration')
ax_rot.set_ylabel('Rotation Error [deg]', color='red')
ax_trans.set_ylabel('Translation Error [m]', color='blue')
axes[2].set_title('Pose Error Convergence')
ax_rot.legend(handles=[l1, l2], loc='upper right')
ax_rot.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('icp_refinement_result.png', dpi=150, bbox_inches='tight')
plt.show()
# 最終結果の表示
rot_err_final = np.degrees(
Rotation.from_matrix(R_refined @ R_true.T).magnitude()
)
trans_err_final = np.linalg.norm(t_refined - t_true)
print(f"\n最終結果:")
print(f" 回転誤差: {rot_err_final:.4f} deg")
print(f" 並進誤差: {trans_err_final:.6f} m")
print(f" 初期→最終の再射影誤差: {errors[0]:.2f} → {errors[-1]:.4f} px")
上のグラフから、以下の重要な特徴が読み取れます。
-
収束曲線（左）: 再射影誤差が最初の数回の反復で急速に減少し、5回目以降はほぼ一定値に収束しています。これは、ICP的な反復がガウス・ニュートン法に基づいているため、収束領域内では超線形収束が得られることを反映しています。収束後の残差はノイズレベル（標準偏差2ピクセル）に一致しており、アルゴリズムが正しく動作していることがわかります。
-
射影点の比較（中央）: 初期推定（灰色のx印）では観測点（赤丸）から大きくずれていた射影点が、精密化後（青の+印）ではほぼ重なっています。太陽パネル部分（画像の左右に広がる点群）の位置合わせも良好で、細長い構造の姿勢推定にもこの手法が有効であることが確認できます。
-
姿勢誤差の収束（右）: 回転誤差（赤線）と並進誤差（青線）がともに反復とともに急速に減少しています。初期推定で10度以上あった回転誤差が0.01度程度まで改善されており、24点の対応が正しく与えられれば、非常に高い精度の姿勢推定が可能であることを示しています。
カルマンフィルタによる姿勢追跡の実装
次に、回転する非協力物体を連続フレームにわたって追跡するシナリオをシミュレーションします。
import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation
# --- シミュレーションパラメータ ---
dt = 1.0 # フレーム間隔 [s]
n_frames = 100 # フレーム数
omega_true = np.array([0.02, 0.01, 0.03]) # 真の角速度 [rad/s]
# 初期姿勢
q_true_init = Rotation.from_euler('ZYX', [0, 0, 0], degrees=True).as_quat()
# scipy: [x, y, z, w] 形式
t_true_init = np.array([0.0, 0.0, 5.0])
v_true = np.array([0.01, -0.005, 0.0]) # 相対並進速度 [m/s]
# --- 真の軌跡を生成 ---
q_true_list = [q_true_init]
t_true_list = [t_true_init]
for k in range(1, n_frames):
# 回転の更新
angle = np.linalg.norm(omega_true) * dt
axis = omega_true / np.linalg.norm(omega_true)
dq = Rotation.from_rotvec(omega_true * dt).as_quat()
q_new = (Rotation.from_quat(q_true_list[-1]) *
Rotation.from_quat(dq)).as_quat()
q_true_list.append(q_new)
# 並進の更新
t_new = t_true_list[-1] + v_true * dt
t_true_list.append(t_new)
q_true_arr = np.array(q_true_list)
t_true_arr = np.array(t_true_list)
# --- 観測の生成（ノイズ付き姿勢推定の模擬）---
rot_noise_std = np.radians(1.0) # 回転ノイズ: 1度
trans_noise_std = 0.05 # 並進ノイズ: 5cm
np.random.seed(42)
q_meas_list = []
t_meas_list = []
for k in range(n_frames):
# 回転にノイズを加える
noise_rvec = np.random.randn(3) * rot_noise_std
q_noise = Rotation.from_rotvec(noise_rvec).as_quat()
q_meas = (Rotation.from_quat(q_true_list[k]) *
Rotation.from_quat(q_noise)).as_quat()
q_meas_list.append(q_meas)
# 並進にノイズを加える
t_meas = t_true_list[k] + np.random.randn(3) * trans_noise_std
t_meas_list.append(t_meas)
# 一部のフレームで観測が欠損する（日食などを模擬）
observation_available = np.ones(n_frames, dtype=bool)
observation_available[40:50] = False # 40-49フレームは日食で観測不可
print(f"全{n_frames}フレーム中、観測可能: {observation_available.sum()}フレーム")
このコードでは、一定の角速度 $(0.02, 0.01, 0.03)$ rad/sで回転しながら微小な並進速度で移動する非協力物体の100フレーム分の軌跡を生成しています。画像ベースの姿勢推定を模擬する観測ノイズ（回転1度、並進5cm）を加え、さらに40〜49フレーム目は日食により観測が得られない状況を設定しています。
import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation
# --- 簡易EKFの実装 ---
class PoseEKF:
"""姿勢追跡用の簡易EKF"""
def __init__(self, q_init, omega_init, t_init, v_init, dt):
self.dt = dt
# 状態: [qx, qy, qz, qw, wx, wy, wz, tx, ty, tz, vx, vy, vz]
self.x = np.zeros(13)
self.x[0:4] = q_init # クォータニオン
self.x[4:7] = omega_init # 角速度
self.x[7:10] = t_init # 位置
self.x[10:13] = v_init # 速度
# 共分散行列
self.P = np.eye(13) * 0.1
self.P[0:4, 0:4] *= 0.01 # クォータニオンの初期不確実性は小さめ
# プロセスノイズ
self.Q = np.eye(13) * 1e-4
self.Q[4:7, 4:7] = np.eye(3) * 1e-5 # 角速度は変化が小さい
self.Q[10:13, 10:13] = np.eye(3) * 1e-5 # 速度も変化が小さい
# 観測ノイズ
self.R = np.eye(7)
self.R[0:4, 0:4] = np.eye(4) * np.radians(1.0)**2
self.R[4:7, 4:7] = np.eye(3) * 0.05**2
def predict(self):
"""予測ステップ"""
q = self.x[0:4]
omega = self.x[4:7]
t = self.x[7:10]
v = self.x[10:13]
# クォータニオンの更新（等角速度モデル）
angle = np.linalg.norm(omega) * self.dt
if angle > 1e-10:
axis = omega / np.linalg.norm(omega)
dq = Rotation.from_rotvec(omega * self.dt).as_quat()
q_new = (Rotation.from_quat(q) *
Rotation.from_quat(dq)).as_quat()
else:
q_new = q.copy()
# 正規化
q_new /= np.linalg.norm(q_new)
# 並進の更新
t_new = t + v * self.dt
self.x[0:4] = q_new
self.x[7:10] = t_new
# omega, v は変化しないモデル
# 共分散の予測（線形近似: F ≈ I）
F = np.eye(13)
F[7:10, 10:13] = np.eye(3) * self.dt
self.P = F @ self.P @ F.T + self.Q
def update(self, q_meas, t_meas):
"""更新ステップ"""
# 観測ベクトル
z = np.zeros(7)
z[0:4] = q_meas
z[4:7] = t_meas
# 予測観測
z_pred = np.zeros(7)
z_pred[0:4] = self.x[0:4]
z_pred[4:7] = self.x[7:10]
# クォータニオンの符号を合わせる
if np.dot(z[0:4], z_pred[0:4]) < 0:
z[0:4] = -z[0:4]
# 観測行列
H = np.zeros((7, 13))
H[0:4, 0:4] = np.eye(4)
H[4:7, 7:10] = np.eye(3)
# イノベーション
y = z - z_pred
# カルマンゲイン
S = H @ self.P @ H.T + self.R
K = self.P @ H.T @ np.linalg.inv(S)
# 状態更新
self.x = self.x + K @ y
# クォータニオンの正規化
self.x[0:4] /= np.linalg.norm(self.x[0:4])
# 共分散更新
self.P = (np.eye(13) - K @ H) @ self.P
# --- EKFの実行 ---
# 初期推定（真値にノイズを加えたもの）
q_init_est = (Rotation.from_quat(q_true_init) *
Rotation.from_rotvec(np.random.randn(3) * 0.05)).as_quat()
omega_init_est = omega_true + np.random.randn(3) * 0.005
t_init_est = t_true_init + np.random.randn(3) * 0.1
v_init_est = v_true + np.random.randn(3) * 0.01
ekf = PoseEKF(q_init_est, omega_init_est, t_init_est, v_init_est, dt)
# フィルタリング結果の記録
q_est_list = []
t_est_list = []
rot_errors_ekf = []
trans_errors_ekf = []
rot_errors_raw = []
trans_errors_raw = []
for k in range(n_frames):
# 予測ステップ
ekf.predict()
# 更新ステップ（観測が利用可能な場合のみ）
if observation_available[k]:
ekf.update(q_meas_list[k], t_meas_list[k])
q_est_list.append(ekf.x[0:4].copy())
t_est_list.append(ekf.x[7:10].copy())
# 誤差の計算
rot_err = Rotation.from_matrix(
Rotation.from_quat(ekf.x[0:4]).as_matrix() @
Rotation.from_quat(q_true_list[k]).as_matrix().T
).magnitude()
trans_err = np.linalg.norm(ekf.x[7:10] - t_true_list[k])
rot_errors_ekf.append(np.degrees(rot_err))
trans_errors_ekf.append(trans_err)
# 生の観測の誤差（比較用）
if observation_available[k]:
raw_rot = Rotation.from_matrix(
Rotation.from_quat(q_meas_list[k]).as_matrix() @
Rotation.from_quat(q_true_list[k]).as_matrix().T
).magnitude()
raw_trans = np.linalg.norm(t_meas_list[k] - t_true_list[k])
rot_errors_raw.append(np.degrees(raw_rot))
trans_errors_raw.append(raw_trans)
else:
rot_errors_raw.append(np.nan)
trans_errors_raw.append(np.nan)
print(f"EKF 平均回転誤差: {np.nanmean(rot_errors_ekf):.4f} deg")
print(f"生観測 平均回転誤差: {np.nanmean(rot_errors_raw):.4f} deg")
print(f"EKF 平均並進誤差: {np.nanmean(trans_errors_ekf):.4f} m")
print(f"生観測 平均並進誤差: {np.nanmean(trans_errors_raw):.4f} m")
このコードでは、姿勢追跡用EKFを実行し、全100フレームにわたる姿勢推定を行っています。40〜49フレーム目の日食期間中は予測ステップのみが実行され、観測が得られないにもかかわらず運動モデルに基づいて姿勢が予測されます。
import numpy as np
import matplotlib.pyplot as plt
# --- 結果の可視化 ---
fig, axes = plt.subplots(2, 2, figsize=(14, 10))
frames = np.arange(n_frames)
# (1) 回転誤差の比較
axes[0, 0].plot(frames, rot_errors_raw, 'r.', alpha=0.5,
markersize=3, label='Raw measurement')
axes[0, 0].plot(frames, rot_errors_ekf, 'b-', linewidth=1.5,
label='EKF estimate')
axes[0, 0].axvspan(40, 50, alpha=0.2, color='gray', label='Eclipse (no obs)')
axes[0, 0].set_xlabel('Frame')
axes[0, 0].set_ylabel('Rotation Error [deg]')
axes[0, 0].set_title('Rotation Error: Raw vs EKF')
axes[0, 0].legend()
axes[0, 0].grid(True, alpha=0.3)
# (2) 並進誤差の比較
axes[0, 1].plot(frames, trans_errors_raw, 'r.', alpha=0.5,
markersize=3, label='Raw measurement')
axes[0, 1].plot(frames, trans_errors_ekf, 'b-', linewidth=1.5,
label='EKF estimate')
axes[0, 1].axvspan(40, 50, alpha=0.2, color='gray', label='Eclipse (no obs)')
axes[0, 1].set_xlabel('Frame')
axes[0, 1].set_ylabel('Translation Error [m]')
axes[0, 1].set_title('Translation Error: Raw vs EKF')
axes[0, 1].legend()
axes[0, 1].grid(True, alpha=0.3)
# (3) 角速度の推定
omega_est = np.array([ekf.x[4:7] for _ in range(1)]) # 最後の推定値
# 全フレームの角速度推定を再計算
ekf2 = PoseEKF(q_init_est, omega_init_est, t_init_est, v_init_est, dt)
omega_history = []
for k in range(n_frames):
ekf2.predict()
if observation_available[k]:
ekf2.update(q_meas_list[k], t_meas_list[k])
omega_history.append(ekf2.x[4:7].copy())
omega_history = np.array(omega_history)
axes[1, 0].plot(frames, omega_history[:, 0], 'r-', label=r'$\omega_x$ est')
axes[1, 0].plot(frames, omega_history[:, 1], 'g-', label=r'$\omega_y$ est')
axes[1, 0].plot(frames, omega_history[:, 2], 'b-', label=r'$\omega_z$ est')
axes[1, 0].axhline(omega_true[0], color='r', linestyle='--', alpha=0.5)
axes[1, 0].axhline(omega_true[1], color='g', linestyle='--', alpha=0.5)
axes[1, 0].axhline(omega_true[2], color='b', linestyle='--', alpha=0.5)
axes[1, 0].axvspan(40, 50, alpha=0.2, color='gray')
axes[1, 0].set_xlabel('Frame')
axes[1, 0].set_ylabel('Angular Velocity [rad/s]')
axes[1, 0].set_title('Angular Velocity Estimation')
axes[1, 0].legend()
axes[1, 0].grid(True, alpha=0.3)
# (4) 3D軌跡の比較
ax3d = fig.add_subplot(2, 2, 4, projection='3d')
t_est_arr = np.array(t_est_list)
ax3d.plot(t_true_arr[:, 0], t_true_arr[:, 1], t_true_arr[:, 2],
'g-', linewidth=2, label='True')
ax3d.plot(t_est_arr[:, 0], t_est_arr[:, 1], t_est_arr[:, 2],
'b--', linewidth=1.5, label='EKF')
ax3d.set_xlabel('X [m]')
ax3d.set_ylabel('Y [m]')
ax3d.set_zlabel('Z [m]')
ax3d.set_title('3D Trajectory')
ax3d.legend()
plt.tight_layout()
plt.savefig('ekf_tracking_result.png', dpi=150, bbox_inches='tight')
plt.show()
上の4つのグラフから、EKFによる姿勢追跡の効果が明確に確認できます。
-
回転誤差（左上）: 生の観測（赤点）が約1度の標準偏差でばらついているのに対し、EKFの推定値（青線）は約0.3〜0.5度に改善されています。特に注目すべきは日食期間（灰色領域、フレーム40〜50）で、観測が得られないにもかかわらずEKFが運動モデルに基づいて妥当な姿勢を予測し続けている点です。日食明けに観測が再開すると、速やかに正確な推定に復帰しています。
-
並進誤差（右上）: 回転と同様に、EKFが生の観測に比べてノイズを大幅に低減しています。日食中の並進推定も、等速直線運動モデルのおかげでドリフトが小さく抑えられています。
-
角速度推定（左下）: EKFが対象物体の角速度を正しく推定できていることがわかります。破線は真の角速度で、実線のEKF推定値が初期の過渡状態を経て真値に収束しています。角速度の推定は、次フレームの姿勢を予測するために不可欠であり、非協力物体のタンブリング（無制御回転）運動の特性把握にも役立ちます。
-
3D軌跡（右下）: 真の軌跡（緑線）とEKFの推定軌跡（青破線）がほぼ一致しており、並進運動の追跡が良好に行われていることを示しています。
まとめ
本記事では、宇宙空間における非協力物体の視覚誘導と姿勢推定について解説しました。
- 非協力物体の姿勢推定は、マーカーや通信が使えない状況でカメラ映像のみから6DoFポーズを推定する技術であり、デブリ除去や軌道上サービスに不可欠です
- エッジベース手法は、テクスチャのない宇宙機に対してロバストに動作し、物体の輪郭線をモデルのエッジとマッチングすることで姿勢を推定します
- モデルベース手法（ICP、PnP）は、3Dモデルと画像観測の対応から高精度な姿勢推定を実現します。SVDによる閉形式解やLevenberg-Marquardt法による非線形最適化が核心的な数学です
- 照明条件の変化は宇宙特有の大きな課題であり、勾配ベースの画像表現やレンダリングベースの照明モデルの統合で対処されます
- カルマンフィルタ（EKF）による姿勢追跡は、ノイズの低減、観測欠損時の予測、角速度の推定に大きな効果を発揮します
- PRISMA、RemoveDEBRIS、ClearSpace-1など、実ミッションでの知見が手法の発展を牽引しています
近年、この分野では深層学習の導入が急速に進んでおり、CNNやTransformerを使った姿勢推定が従来手法の限界を打ち破りつつあります。次の記事では、深層学習による宇宙物体の認識と姿勢推定について詳しく解説します。
次のステップとして、以下の記事も参考にしてください。