占有格子地図（Occupancy Grid Map）

「地図」を確率で表すという発想

移動ロボットが自律走行するには、周囲のどこが通れてどこが壁かを表す地図が要ります。しかしレーザ測距（LiDAR）や超音波センサの計測値は必ず誤差とノイズを含み、同じ場所を2回測っても微妙に違う値が返ります。地図を「壁がある／ない」の二値だけで確定的に表そうとすると、1回のノイズ混じりの観測でセルの状態が誤って書き換わり、地図が壊れやすくなります。

占有格子地図（Occupancy Grid Map）は、この問題を環境を格子セルに分割し、各セルが障害物に占有されている確率を持たせることで解決します。1つの観測で断定するのではなく、多数の観測を確率的に積み重ねてセルの占有確率を少しずつ更新していく設計です。ロボット工学における自己位置推定（SLAM）や経路計画の多くは、この確率地図を土台にしています。センサフュージョン自体は組み込み分野の慣性計測（IMUと加速度センサの融合）とも重なりますが、占有格子地図が扱うのは単一時点の姿勢推定ではなく「空間全体の占有状態という時間方向に蓄積される確率場」を推定する点が本質的に異なります。

格子表現とセルの意味

環境を一辺が数センチ〜数十センチ程度の正方形（2次元）または立方体（3次元、ボクセル）に分割し、各セル m_i に占有確率 p(m_i) を割り当てます。p(m_i) が1に近ければ障害物で埋まっている可能性が高く、0に近ければ自由空間（走行可能）である可能性が高いことを意味します。観測前や情報が全くない段階では p(m_i) = 0.5（未知）とし、「占有でも自由でもどちらでもあり得る」という無情報の状態を表現します。

セルの状態と確率の対応:

  p(m_i) → 0    : 自由空間（走行可能）
  p(m_i) = 0.5  : 未知（未観測、情報なし）
  p(m_i) → 1    : 占有（障害物）

地図全体は独立なセルの集合として扱われます。すなわち地図 m = {m_1, m_2, ..., m_n} の同時分布 p(m | z_1:t, x_1:t) を、セルごとの周辺分布の積 Π p(m_i | z_1:t, x_1:t) で近似します。ここで z_1:t は時刻1からtまでの観測列、x_1:t はロボットの姿勢（自己位置と向き）の履歴です。

逆センサモデルとベイズ更新

各セルの占有確率は、ロボットの姿勢 x_t とセンサ観測 z_t が得られるたびにベイズの定理で更新します。センサモデルには2通りの向きがあります。順センサモデル p(z_t | m_i, x_t)（地図が与えられたときに観測がどう出るか）は物理的に自然ですが、更新に使うのは向きを逆にした逆観測モデル（inverse sensor model） p(m_i | z_t, x_t)（観測とロボット姿勢が与えられたときにセルが占有である確率）です。

逆観測モデルは、測距ビームがセルの手前を通過するなら「自由」、ビームが到達した終端付近なら「占有」、ビームの届いていない領域なら「未知のまま」という3値の判定をセルごとに与える関数として構成します。1本のレーザビームに沿って、始点からヒット点手前までのセルは自由方向へ、ヒット点周辺のセルは占有方向へ更新されます。

セル m_i の占有確率をベイズの定理で逐次更新する式は次の通りです（マルコフ性、すなわち直前の推定が過去の観測すべてを要約しているという仮定を用います）。

再帰ベイズ更新（セル m_i、時刻 t の観測 z_t、姿勢 x_t）:

  p(m_i | z_1:t, x_1:t)
    = [ p(z_t | m_i, x_t) * p(m_i | z_1:t-1, x_1:t-1) ] / p(z_t | z_1:t-1, x_1:t)

占有 m_i と非占有 ¬m_i それぞれにこの式を立てて比を取ると、
正規化定数 p(z_t | z_1:t-1, x_1:t) が消え、オッズ（p / (1-p)）の形に整理できます:

  odds(m_i | z_1:t, x_1:t)
    = odds(m_i | z_t, x_t) * odds(m_i | z_1:t-1, x_1:t-1) / odds(m_i)

確率のままこの式を毎ステップ計算しようとすると、正規化のための除算が毎回必要になり数値的に扱いにくくなります。オッズの形に直すことで正規化定数が消え、あとは掛け算の連鎖だけが残ります。

対数オッズ表現による安定化

そこで実装では占有確率を直接持たず、**対数オッズ（log-odds）**という形に変換して持ちます。オッズは p / (1 - p) で定義され、対数オッズはその対数です。

対数オッズの定義:

  l(m_i) = log( p(m_i) / (1 - p(m_i)) )

逆変換（対数オッズから確率へ戻す）:

  p(m_i) = 1 - 1 / (1 + exp(l(m_i)))

対数オッズ表現の利点は、ベイズ更新の掛け算が単純な加算に変わることです。逆観測モデルから得られる対数オッズ l(m_i | z_t, x_t) を、事前の対数オッズに単純に足すだけで更新できます。

対数オッズでの逐次更新:

  l_t(m_i) = l_t-1(m_i) + l(m_i | z_t, x_t) - l_0(m_i)

  l_0(m_i) : 事前対数オッズ（p=0.5 なら l_0=0）
  l(m_i | z_t, x_t) : 逆観測モデルから決まる、
                       「自由」なら負、「占有」なら正、
                       「未知」なら0の増分

事前が p = 0.5 なら l_0 = 0 なので式はさらに単純化され、単に観測ごとの増分を足し込んでいくだけになります。この加算処理は除算や掛け算を含まないため計算コストが低く、p が0や1に張り付いても l は有限の範囲に収まりにくいという扱いにくさはあるものの、実装では l に上限・下限（クランプ）を設けることで数値的な発散を防ぐのが一般的です。占有確率が必要な箇所（可視化や閾値判定）でのみ、上記の逆変換で p に戻します。

経路計画との接続

占有格子地図が完成すれば、各セルの占有確率を閾値で二値化するか、確率そのものをコストとして扱うことで、そのままグラフ探索ベースの経路計画に接続できます。占有確率が閾値（例えば0.7）を超えるセルを「通行不可」、それ以外を「通行可能」なノードとみなし、格子をグラフとしてA*やDijkstra法で始点から終点までの最短経路を探索します。

確率をそのままコストへ変換する方式では、占有確率が中程度のセル（壁に近い、あるいはノイズで曖昧な領域）を完全な障害物として除外せず、通行コストを上げるだけに留めることで、遠回りしてでも安全な経路を優先する、あるいは他に道がなければ通過するといった柔軟な計画が可能になります。また、p = 0.5 に近い未知セルを積極的に選んで探索行動を計画する「フロンティア探索」は、地図構築そのものを目的とする探査ロボットで使われる典型的な応用です。

まとめ

占有格子地図は、環境を格子セルに分割し各セルの占有確率をベイズ推定で逐次更新することで、センサの不確実性を確率としてそのまま地図に織り込む表現です。要点は、(1) セル独立性を仮定して同時分布を周辺分布の積で近似し計算量を抑えること、(2) 順センサモデルではなく逆観測モデルを使って再帰ベイズ更新を行うこと、(3) 掛け算の連鎖による数値不安定と正規化計算を避けるため対数オッズ表現で加算処理に変換すること、(4) 完成した確率地図を閾値二値化またはコスト化することでA*やDijkstra法などの経路計画へそのまま接続できること。この確率的な地図表現は、SLAMで自己位置と地図を同時推定する際の地図側の基盤としても、探査ロボットが未知領域を効率よく調べるフロンティア探索の基盤としても機能する、移動ロボット工学の中核的なデータ構造です。