SLAM — 地図がないなら、作りながら走る

前回学んだ自己位置推定には「地図がある」という前提がありました。では初めて走る場所では？ 地図作りと自己位置推定を同時に解くのが SLAM（Simultaneous Localization and Mapping）です。鶏と卵のジレンマ、占有グリッド、そしてループ閉じ込みまでを動かして掴みます。

1. 鶏と卵のジレンマ

ロボットが LiDAR で壁までの距離を測ったとします。この観測から何が分かるでしょうか。実は「自分がどこにいるか」を知っていれば、測った点を世界座標に置くだけで地図が描けます。逆に「地図を持っていれば」、スキャンと地図を照合して自分の位置を割り出せます。

問題は、初めての場所ではどちらも持っていないこと。下のデモで3つのモードを切り替えて、この相互依存を体感してください。

鶏と卵 — 位置と地図、どちらかがあれば他方が解ける

スキャン照合で同時補正

青三角＝真の位置、赤三角＝オドメトリ（車輪の回転から積算した位置。誤差が溜まる）、緑三角＝推定位置。③でチェックを外すと、ずれた位置から観測を置いてしまい、地図が二重・湾曲していきます。チェックを入れると、スキャン同士の照合（スキャンマッチング）で位置と地図が同時に補正されます。

POINT — SLAM とは Simultaneous Localization and Mapping ＝自己位置推定と地図構築の同時実行。位置と地図は互いに依存するため別々には解けないが、「軌跡と地図の組」をまとめて1つの推定問題にすれば解ける。掃除ロボットから火星探査機、そして自動運転の地図作成まで、移動ロボットの基盤技術。

2. SLAM の定式化 — 同時確率として書く

SLAM を確率の言葉で書くと、次の同時事後分布の推定問題になります。

p(x_1:t, m | z_1:t, u_1:t) x_1:t＝ロボットの軌跡（各時刻の姿勢）、m＝地図、z_1:t＝センサ観測、u_1:t＝制御入力（オドメトリ）。x と m を「セットで」推定するのが肝

ポイントは、求めたいものが x と m の同時分布であること。「まず位置を確定してから地図」でも「まず地図を確定してから位置」でもなく、観測と矛盾しない（軌跡, 地図）の組合せを丸ごと探します。①②のモードが解けたのは片方が所与だったから。③では両方を変数にしたまま、整合性だけを手がかりに解くのです。

オンライン SLAM：現在の姿勢 x_t と地図だけを逐次推定（EKF-SLAM、パーティクルフィルタ系）。走行中のロボット向け。
フル SLAM：軌跡全体 x_1:t を後からまとめて最適化（グラフ SLAM）。精密な地図作成向け。現在の主流。

3. 地図のかたち — 占有グリッドマッピング

「地図 m」の代表的な表現が占有グリッド（occupancy grid）です。世界を小さなセルに切り、各セルが「物体で占有されている確率」を持ちます。LiDAR の1本のビームは2つの情報をくれます：ビームが通過したセルは空いている、ビームが止まったセルは占有されている。

スキャンを重ねるほど確信が強まる様子を、走らせながら見てみましょう（このデモでは位置は既知、つまりモード①の状況です）。

占有グリッドマッピング — グレーの霧が白と黒に晴れていく

走行速度

真の壁を重ねる

グレー＝未知（まだ観測していない）、白＝自由（ビームが通過）、黒＝占有（ビームが命中）。柱の裏側（オクルージョン）は最後まで灰色のまま残ることに注目。何周かすると輪郭が引き締まっていきます。

各セルの更新は、確率をそのまま掛け算する代わりにログオッズ（対数オッズ）の足し算で行うのが定石です。通過したら一定値を引き、命中したら一定値を足すだけなので、毎秒何十万セルの更新でも軽量に済みます。

l_t(セル) = l_t-1(セル) + log p(占有 | z_t) ⁄ (1 − p(占有 | z_t)) ログオッズ l = log(p/(1−p))。l=0 が「五分五分＝未知」、正が占有寄り、負が自由寄り。観測のたびに加算するだけでベイズ更新になる

注意 — 動くものを地図に書くと「幽霊」が残る占有グリッドは「世界は静止している」と仮定している。歩行者や他車をそのまま書き込むと、通過跡が黒い壁のように残ってしまう（次レッスンの「静的地図＋動的物体リスト」の2層表現がこの問題への答え）。

4. 累積誤差とループ閉じ込み

モード③のスキャンマッチングは、直前のスキャンとの照合なので小さな誤差がわずかに残り、距離とともに蓄積します。1周して出発点に戻ってきたとき、地図の上では「出発点とは別の場所」に居ることになってしまう — これが SLAM 最大の敵、累積誤差（ドリフト）です。

切り札がループ閉じ込み（loop closure）。「この景色、前に見たぞ！」と気づいた瞬間、始点と終点が同じ場所だという強力な拘束が生まれ、軌跡全体を遡って誤差をばら撒き直せます。

ループ閉じ込み — 二重になった地図がバネ仕掛けで一致する

真の軌跡を表示

白い点＝ポーズグラフのノード（過去の姿勢）、青い線＝オドメトリのエッジ。1周すると累積誤差で壁が二重にずれます。赤い点線が「ループ検出」の拘束エッジ。ボタンを押すと、補正が終端から始端へバネのように伝播し、地図が1つに重なります。

この補正の裏側がポーズグラフ最適化です。過去の姿勢をノード、姿勢間の相対観測（オドメトリやスキャン照合、ループ検出）をエッジとするグラフを作り、全エッジの矛盾が最小になる姿勢の組を求めます。

x^* = argmin_x Σ_(i,j) ‖ e_ij(x_i, x_j) ‖²_Σij e_ij＝「観測された相対姿勢」と「現在の推定から計算した相対姿勢」の食い違い。信頼度（共分散 Σ）で重み付けした二乗誤差の総和を最小化する

POINT — バネの網としての直感各エッジは「ノード i から見てノード j はこの位置にあるはず」と主張するバネだと思えばよい。ループ検出は、たるんだ網の端と端を強いバネで繋ぐ操作。手を離すと網全体が釣り合いの形（＝誤差最小の軌跡）にストンと落ち着く。実装では g2o や GTSAM などの非線形最小二乗ソルバが使われる。

5. Visual SLAM と LiDAR SLAM

SLAM は使うセンサによって性格が大きく変わります。二大流派を比べてみましょう。

	LiDAR SLAM	Visual SLAM
観測	点群（距離が直接手に入る）	画像（特徴点の方向のみ。距離は多視点から復元）
地図の中身	点群地図・占有グリッド	疎な特徴点地図（＋キーフレーム画像）
得意	幾何が正確、照明に無関係、夜でも同じ	センサが安価・軽量、テクスチャ豊富な場所、ループ検出（画像は場所の見分けが得意）
苦手	トンネル・長い廊下など特徴のない幾何、雨や雪の反射	暗所・逆光・のっぺりした壁、スケールの曖昧さ（単眼）、高速移動のブレ
代表例	LOAM、Cartographer、LIO-SAM	ORB-SLAM、VINS-Mono（カメラ＋IMU）

実務では対立ではなく補完の関係です。LiDAR＋IMU＋カメラを組み合わせ、ループ検出は画像で、幾何の精密化は点群で、と役割分担するシステムが主流になっています（前レッスンのセンサフュージョンがここでも効いてきます）。

6. まとめ

位置があれば地図が描け、地図があれば位置が分かる。両方ないときは、両方を同時に推定する — それが SLAM。
地図の定番表現は占有グリッド。ログオッズの加算だけでベイズ更新ができる。
逐次的な照合では誤差が蓄積する。ループ閉じ込み＋ポーズグラフ最適化で軌跡全体を一括補正する。
LiDAR SLAM と Visual SLAM は得意分野が異なり、実システムでは融合される。

一歩先へ — 自動運転と HD マップの関係実は量産自動運転車は、走行中に毎回ゼロから SLAM をしているわけではない。計測車両が事前に走って収集したデータを、巨大なポーズグラフとしてオフラインで最適化し、センチメートル精度の HD マップ（車線・信号・標識まで含む）を作っておく。市販車はその地図に対する自己位置推定（前レッスンの世界）を行うだけ — つまり SLAM の「M」を工場側で先に済ませる分業だ。一方、地図が古くなる問題に対しては、一般車両のセンサデータで差分を検出して地図を更新するクラウドソーシング型のパイプラインが使われており、ここでも SLAM 技術が心臓部になっている。