SLAM（自己位置推定と地図構築）

「地図が無いと自分の位置が分からない」という鶏卵問題

ロボットが未知の環境を動き回るとき、GPSが届かない屋内や惑星表面では絶対座標が得られません。地図さえあればセンサ観測と照合して自己位置を求められますし（既知地図でのローカリゼーション）、逆に自己位置さえ分かればセンサで見えた物体を地図に正しく配置できます（既知位置でのマッピング）。ところがSLAM（Simultaneous Localization and Mapping、自己位置推定と地図構築の同時実行）が対象とするのは、そのどちらも与えられていない状況です。

これが本質的に難しい理由は、位置の誤差と地図の誤差が互いに汚染し合う点にあります。オドメトリ（車輪の回転数などから移動量を積算する推定）には必ず誤差が乗り、その誤差は時間とともに積算されて発散します。誤った自己位置をもとにランドマークを地図に書き込めば地図がゆがみ、ゆがんだ地図で次の自己位置を推定すればさらに誤差が乗る——地図と位置の推定誤差が相関しながら循環的に増幅するため、単純に「まず地図を作ってから位置を求める」という分離ができません。SLAMはこの相関そのものを確率変数として同時に扱うことで、片方の情報がもう片方の誤差を訂正する構造を作ります。

EKF-SLAM：状態にランドマークごと詰め込む

最初期の実用的解法がEKF-SLAM（拡張カルマンフィルタSLAM）です。考え方は単純明快で、カルマンフィルタの状態ベクトルにロボットの姿勢だけでなく地図中の全ランドマーク座標も含めてしまいます。

状態ベクトル:
  x = [ ロボット姿勢(x, y, θ),  ランドマーク1(lx1, ly1),  ランドマーク2(lx2, ly2), ... ]

共分散行列 P（(3+2N)×(3+2N)）:
  ロボット-ロボット ブロック   : 自己位置の不確かさ
  ロボット-ランドマーク ブロック: 位置と各ランドマークの相関
  ランドマーク-ランドマーク ブロック: ランドマーク間の相関

予測ステップではオドメトリでロボット姿勢だけを進め（ランドマークは動かないので不変）、更新ステップでは観測されたランドマークとの対応づけ（データアソシエーション）を行い、通常のカルマンゲインで姿勢とランドマーク座標を同時に補正します。ここでの核心は非対角の相関ブロックです。あるランドマークの観測でロボットの姿勢が補正されると、その相関を通じて他のランドマーク座標もつられて補正されます。これにより、一度でも正確な観測ができたランドマークを介して、地図全体の相対的な整合性が保たれます。

EKF-SLAM の弱点:

  計算量: 状態次元が (3+2N) なので更新1回あたり O(N²)
          → ランドマーク数Nが増えると急速に破綻（N=数百が実用限界）
  線形化: 姿勢の回転はF・Hを非線形にするためヤコビ行列で線形近似
          → 線形化誤差が蓄積し、特に大規模ループで不整合を起こす
  単峰性: ガウス分布1つで不確かさを表すため、データアソシエーションを
          誤ると（別のランドマークを同一と誤認）致命的に発散する

パーティクルフィルタSLAM：仮説を複数持って条件付ける

EKF-SLAMの単峰性の弱点を解消するのがパーティクルフィルタSLAM（代表例がFastSLAM）です。発想の転換は、ロボットの経路そのものを多数の仮説（パーティクル、粒子）としてサンプリングで表現する点にあります。

FastSLAM の構造（Rao-Blackwell化パーティクルフィルタ）:

  各パーティクル k が持つもの:
    ・経路の仮説 x_{0:t}^(k)（1つの姿勢系列のサンプル）
    ・その経路を「既知」とした条件付き地図 m^(k)
      → 経路が確定すればランドマークは互いに独立になる
      → 地図はランドマークごとの小さなEKF（あるいはヒストグラム）N個の集合で済む

  1サイクル:
    1. 各パーティクルをオドメトリ＋雑音でサンプリング（経路仮説を1歩進める）
    2. 各パーティクルの地図で観測の尤度を計算 → 重みを更新
    3. 重みに応じてリサンプリング（尤もらしい経路仮説だけ生き残らせる）

ここで使われるのがRao-Blackwell化という数学的な分解です。経路 x_{0:t} が与えられれば、異なるランドマーク同士は観測を介した相関を持たず条件付き独立になるという性質を利用し、地図全体の同時分布を推定する代わりに、パーティクルごとにランドマークN個の独立な小さいフィルタに分解します。各パーティクルの地図を平衡木で管理すれば1回の観測更新はO(log N)で済み、これはEKF-SLAMの更新1回あたりO(N²)よりはるかに軽く、大規模な地図でも扱えるようになります（パーティクル数をMとすると1タイムステップ全体ではO(M log N)）。加えて、パーティクルの集合は多峰性（複数の対応づけ仮説）を自然に表現できるため、データアソシエーションの曖昧さにもEKF-SLAMより頑健です。

パーティクルフィルタSLAMにも弱点があります。リサンプリングを繰り返すと、過去の共通祖先へ多数のパーティクルが収束してしまう粒子涸渇が起き、特にループの閉じ込み（後述）のような長期の一貫性を要する場面で経路の多様性が失われがちです。

グラフベースSLAMとループクロージャ：後から地図を整合させる

現在の実用SLAM（LiDAR SLAM、Visual SLAMの多くを含む）で主流なのがグラフベースSLAMです。発想はEKF-SLAMやパーティクルフィルタSLAMのような逐次フィルタとは異なり、問題を1つの大きな最適化として後からまとめて解くことにあります。

グラフの構造:
  ノード : 各時刻のロボット姿勢（またはランドマーク位置）
  エッジ : ノード間の相対的な制約
    ・オドメトリ制約: 隣接する姿勢間の相対移動量（＋その不確かさ）
    ・観測制約     : 姿勢とランドマークの間の相対観測（＋不確かさ）
    ・ループクロージャ制約: 離れた時刻の姿勢同士が「同じ場所」だと分かったときの制約

最適化（グラフ最適化 / ポーズグラフ最適化）:
  全ノードの姿勢を変数として、
  「各エッジの制約からの逸脱」の二乗和を最小化する

  誤差関数: Σ_edges  || 予測される相対関係 − エッジの観測値 ||²_Σ⁻¹
  （Σ は各制約の不確かさ共分散、重み付き最小二乗）

  → 疎な非線形最小二乗問題としてガウス・ニュートン法や
    レーベンバーグ・マーカート法で反復的に解く

このグラフには、地図とロボットの全履歴が「制約の集合」として蓄えられます。累積誤差が生じるのは避けられませんが、決定的に重要なのが**ループクロージャ（閉ループ検出）**です。ロボットが移動して、過去に訪れた場所へ再び戻ってきたことをセンサ観測（画像の類似度、点群のマッチングなど）から検出できると、時間的には遠く離れた2つのノード間に新しい制約エッジを追加できます。

ループクロージャが誤差を解消する仕組み:

  1. ロボットが1周してスタート地点付近に戻る
  2. 現在の観測が「過去のノードNの観測と一致する」と検出
     （見た目の再認識・点群の位置合わせなど）
  3. ノードNと現在ノードの間に「実は同一地点」という制約エッジを追加
  4. グラフ最適化を再実行
     → 1周の間に積み上がった誤差が、経路全体の姿勢に再配分される
     → ループの起点から終点までの姿勢が滑らかに補正し直される

ループクロージャ以前は、経路上のオドメトリ誤差は消えることなく単調に積み重なるだけです。ループクロージャ制約が加わることで、それまで独立だった「行き」と「帰り」の推定が1つの制約でつながり、最適化が誤差を経路全体（特にループ内側のノード群）に分配し直します。これによって、局所的には粗くても大域的には閉じた地図が得られます。ただし、ループクロージャの検出自体を誤る（実際には別の場所なのに同一と誤認する）と、誤った制約が地図全体を破綻的にゆがめるため、検出の確からしさを別途検証する仕組み（幾何的整合性チェックなど）が欠かせません。

なぜ同時推定は本質的に難しいのか

3手法に共通する困難の根は同じです。第一に、誤差の相関です。ロボット工学特有の問題として、姿勢推定とランドマーク推定は独立変数ではなく、一方の誤差が他方に伝播し続ける結合系を成します。この相関を正しく保持しないと（例えば地図とロボットの不確かさを別々に扱うと）、実際より過小に見積もった自信過剰な推定になり、破綻に気づけません。第二に、データアソシエーションの組合せ爆発です。今見ているランドマークが地図上のどれに対応するかは事前には分からず、候補が多いほど誤対応のリスクも増えます。誤対応は上流の推定を汚染し、下流のすべての判断に波及します。第三に、線形化と多峰性のトレードオフです。EKF-SLAMは計算効率と引き換えに単峰・線形近似という仮定を置き、パーティクルフィルタSLAMは多峰性を扱える代わりに粒子数に比例した計算コストと粒子涸渇のリスクを負い、グラフベースSLAMはループクロージャで大域整合を回復できる代わりに、それまでの累積誤差にセンサ観測を頼った再検出（既訪認識）という別の困難な問題を新たに抱えます。SLAMは万能な一発解法ではなく、この3つの困難のどこにどれだけ計算資源を配分するかという設計判断の集合なのです。

まとめ

SLAMは、「地図が無いと位置が定まらず、位置が無いと地図が作れない」という相互依存を、自己位置と地図の同時推定として正面から扱う問題です。要点は、(1) 誤差が地図と位置の間で相関しながら循環するため単純な分離推定ができないこと、(2) EKF-SLAMは状態にランドマークを組み込み共分散の相関で地図全体を補正するが計算量O(N²)と単峰性が弱点であること、(3) パーティクルフィルタSLAM（FastSLAM）は経路を粒子でサンプリングしRao-Blackwell化で地図をランドマークごとの独立フィルタへ分解し多峰性と効率を両立させるが粒子涸渇のリスクを負うこと、(4) グラフベースSLAMは姿勢と制約をグラフとして持ち非線形最小二乗で一括最適化し、ループクロージャによって累積誤差を経路全体へ再配分して大域的な整合を回復すること。この「同時推定・データアソシエーション・線形化と多峰性のトレードオフ」という3つの困難の配分を理解することが、LiDAR SLAMやVisual SLAMなど個別実装を学ぶ土台になります。