占有格子地図入門 — LiDARとカメラの観測を「通れる確率」へ変える

LiDARの点群を画面に描くと、道路や壁の輪郭が美しく見える。しかし経路計画器が本当に知りたいのは「点がどこにあるか」だけではない。車体を置ける空間はどこか、見えていない場所はどこか、観測の確かさはどれくらいか、数秒前の歩行者をいつ消すかである。占有格子地図（Occupancy Grid Map）は、連続空間を小さなセルへ分け、各セルが占有されている確率を蓄積する。

格子は単純だが、単なる白黒画像ではない。センサーの光線が物体へ届くまでのセルは「自由」、反射点は「占有」、光線が通っていない場所は「未知」である。未知を自由と同じに塗ると、観測していない路地へ経路を引いてしまう。本稿では、測距一回を地図へ反映する式から、SLAM、動的物体、3D、経路計画、安全評価までをつなぐ。

占有格子の観測源となるLiDAR

画像: Concept of LiDAR(Cartographer3d, CC BY-SA 4.0)、Wikimedia Commons。LiDARのToF原理図であり、特定車両や本稿の格子実装を示す製品図ではない。

30秒で分かる結論

各セルに確率を持たせる。0.5付近は未知、1に近いほど占有、0に近いほど自由である。
ベイズ更新をlog-oddsへ変換すると、毎回の観測を足し算で蓄積できる。ただし上下限を設けないと、一度誤認したセルが永久に消えない。
測距の終端だけを障害物としてmarkし、センサーから終端までをray tracingでclearする。最大距離、欠測、透過、雨雪を同じ扱いにしない。
SLAMの地図は自己位置推定と長期構造、local costmapは直近の衝突回避を担う。静的地図を動的障害物で上書きし続けない。
車体は点ではない。障害物を車幅、位置誤差、停止誤差だけ膨張させ、経路計画・MPCへコストとして渡す。
セマンティック占有、時系列のoccupancy flow、NeRF/3DGSとの融合が研究されるが、最後に必要なのは衝突可能性と未観測領域の扱いである。

1. 点群から格子へ

図1 — 測距の終端をmarkし、手前をclearする逆センサモデル。灰色の未知領域は自由空間ではない。

世界座標 $(x,y)$ を原点 $(x_0,y_0)$ 、解像度 $r$ の格子へ変換する基本式は

i=\left\lfloor\frac{x-x_0}{r}\right\rfloor,\qquad j=\left\lfloor\frac{y-y_0}{r}\right\rfloor

である。 $r$ を半分にすれば一辺のセル数は2倍、2Dメモリは約4倍、3D voxelなら約8倍になる。細かければ常に良いわけではない。センサー分解能や自己位置誤差より細かいセルは、精密さではなくちらつきを増やすことがある。

地図座標、連続だがドリフトするodometry座標、車体座標、センサー座標の時刻付き変換が必要になる。点群だけ現在時刻で変換し、車体姿勢を古い時刻から取れば、走行中の壁が斜めに伸びる。地図バグに見えて時刻同期バグという例は多い。

2. ベイズ更新とlog-odds

セル $m_i$ が占有される事象を $O_i$ 、時刻 $t$ までの観測を $z_{1:t}$ 、姿勢を $x_{1:t}$ とする。求めたいのは

p_{t,i}=P(O_i\mid z_{1:t},x_{1:t})

である。通常のOccupancy Gridはセル間独立を仮定し、逆センサモデル $P(O_i\mid z_t,x_t)$ を使って更新する。この独立仮定は計算を軽くする代わりに「一本の壁が連続している」といった相関を捨てる。

確率をlog-odds

l_{t,i}=\log\frac{p_{t,i}}{1-p_{t,i}}

へ変換すると更新は

l_{t,i}=l_{t-1,i} +\log\frac{P(O_i\mid z_t,x_t)}{1-P(O_i\mid z_t,x_t)} -l_{0,i}

という足し算になる。事前確率 $p_0=0.5$ なら $l_0=0$ 。占有観測では正の値、自由観測では負の値を加える。最後は

p_{t,i}=\frac{1}{1+\exp(-l_{t,i})}

で確率へ戻せる。実装では $l_{min}\le l_{t,i}\le l_{max}$ へclampする。上限がなければ、長く壁だった場所から車が動いた後、自由観測を何百回も入れないと消えない。clampは過去をどれだけ忘れられるかという動的環境の設計値でもある。

3. 逆センサモデル

2D LiDARの一本のrayに対し、Bresenham法やDDAでセルを列挙し、反射終端を占有、手前を自由として更新する。ところが全ての戻り値を同じに扱えない。

有効な反射：終端を占有、手前を自由。
最大距離/反射なし：範囲内を自由にできる場合があるが、機器仕様と天候依存。
最小距離未満：筐体や近接盲点として未知に保つ。
雨雪・埃：孤立した短寿命点として空間・時間フィルタを使う。
ガラス・黒色物体：透過や低反射で「反射なし」が自由を意味しない場合がある。

カメラからはdepth、stereo、semantic segmentationを地面へ射影する。単眼のクラス結果だけから正確な距離は出ない。路面平面、深度ネットワーク、物体寸法など仮定を使うなら、その不確かさを格子へ広げる。Visual SLAM入門の特徴点地図と衝突判定用格子は目的が違う。

4. 自己位置誤差を地図へ伝える

センサー点 $z$ を姿勢 $x$ で世界へ変換する関数を $g(x,z)$ とし、姿勢共分散を $P_x$ 、測距共分散を $R_z$ とすれば、線形化した点の共分散は

P_p\approx J_xP_xJ_x^T+J_zR_zJ_z^T

である。自己位置が不確かなのに一点のセルだけを強く占有へすると、走行するたび壁が厚くなる。確率を近隣セルへ分配する、submap座標で更新して最適化後に再配置するなどの方法を使う。

SLAMのloop closureで過去姿勢が変わると、すでに世界座標へ焼き付けた点を戻すのは難しい。pose graphとsubmapを保持する設計は、最適化後の一貫性を回復しやすい。LiDAR SLAM入門はこの自己位置と地図の同時推定を扱う。

5. 静的地図、local grid、動的物体

実用システムでは一枚の地図へ全てを混ぜず、レイヤを分ける。

レイヤ	時間スケール	例	更新方針
静的	日〜年	壁、縁石、建物	SLAM/HD地図、慎重に更新
一時障害物	秒	駐車車両、落下物	mark/clear、時間減衰
動的物体	0.1〜数秒	車、歩行者、自転車	追跡と将来占有
規則・意味	地図版	車線、停止線、進入禁止	版管理された地図
安全マージン	制御周期	車体膨張、停止距離	速度と不確かさで更新

単純な時間減衰は

l_i(t+\Delta t)=e^{-\Delta t/\tau}l_i(t)

と書ける。 $\tau$ が短すぎると停止中の障害物が消え、長すぎると通過した車の影が残る。物体追跡で動的領域だけを別管理し、静的背景へ誤って蓄積しない方法が望ましい。

自動運転では将来時刻 $k$ ごとの占有確率 $P(O_{i,k})$ や、セル間の流れを予測するoccupancy flowも使われる。物体検出が「これは車」と箱を出すのに対し、occupancy predictionは分類できない物体や複数仮説も空間確率として表しやすい。

6. 2D、2.5D、3DとBEV

2D格子は高速だが、張り出した机、枝、トラック下の空間、坂を表しにくい。高さ統計をセルへ持つ2.5D elevation map、空間を立方体へ分けるvoxel、占有境界を木構造で圧縮するOctoMapなどを用途で選ぶ。

車載認識ではカメラ・LiDAR・レーダー特徴を鳥瞰図（BEV）へ変換し、地面上の占有、意味、速度を出す学習モデルが発展している。学習による補完は遮蔽の先を予測できる一方、確率が校正されているとは限らない。予測確率0.9のセルが実際にも約90%占有かをreliability diagramやExpected Calibration Errorで検証する。

3D表現を最終的に2D costmapへ落とす場合、どの高さ範囲を障害物とするかが重要だ。低い段差、通過可能な草、車体上部へ当たる梁を同じ投影規則にしない。

7. Costmapと経路計画へ接続する

占有確率をそのまま経路探索へ渡すのではなく、衝突領域と接近コストへ変換する。障害物からの距離 $d$ に対する膨張コストの例は

C(d)= \begin{cases} C_{lethal}, & d\le r_{inscribed}\\ C_0e^{-\alpha(d-r_{inscribed})}, & r_{inscribed}<d<r_{inflation}\\ 0, & d\ge r_{inflation} \end{cases}

である。車体形状、操舵時のswept volume、位置誤差、制御追従誤差、制動距離を考える。円形robot radiusだけでは長い車体の旋回内輪差を表せない。

経路計画入門のAやHybrid Aは格子コストを探索し、MPC入門は予測軌道上の障害物距離と車両制約を最適化する。地図更新5 Hz、制御50 Hzなら、制御側は地図時刻を確認し、古い地図を無期限に使わない。

ROS 2 Nav2のCostmap2Dはstatic、obstacle、voxel、inflationなどのレイヤをプラグインで組み合わせる。設定順序にも意味があり、障害物レイヤの後で膨張しなければ新しい障害物へ安全マージンが付かない。

8. V2Xと複数車両の地図

V2X入門の協調認識から占有を受け取ると、遮蔽先を早く知れる。しかし、相手の地図座標、測定時刻、自己位置共分散、観測源が必要である。同じ路側センサーの結果を複数車から受信して独立観測として加算すると、確信度を二重計上する。

地図タイルを統合するときは、通信遅延 $\Delta t$ の間に動的物体が $v\Delta t$ 移動する。静的構造と動的占有を分け、相手のデータをローカルセンサーより無条件に優先しない。悪意ある占有地図で道路全体を塞ぐ攻撃も想定し、署名だけでなく物理的整合性を検査する。

9. 失敗例と安全策

未知を自由にした

起動直後にセンサー未観測領域を0として初期化すると、経路が壁の向こうを通る。探索ロボットなら未知へ進む価値を別コストで扱い、公道車両なら停止可能距離内の未知を保守的に扱う。

clearが強すぎる

一回の「反射なし」で壁を消すと、ガラスや雨で障害物が消える。占有・自由で異なる更新量、複数回確認、センサー種別ごとの信頼度を使う。

地図が古い

タイムスタンプは新しくても、上流推定が停止して同じデータを再送している場合がある。生成時刻、シーケンス、更新領域、入力センサーのheartbeatを監視する。

インフレーションが車速に合わない

低速倉庫ロボットの固定半径を高速車へ流用すると停止できない。反応遅延 $T$ 、速度 $v$ 、最大減速度 $a$ なら縦方向余裕の概算は

d_{safe}=vT+\frac{v^2}{2a}+d_{margin}.

横と縦で異なる非対称膨張や、予測軌道全体の衝突判定が必要になる。

10. 評価指標と実験手順

セル単位ではIoU、precision、recall、Brier score、negative log-likelihood、校正誤差を使う。しかし自由セルが圧倒的に多いので全体accuracyは役に立ちにくい。距離帯、遮蔽、クラス、天候、更新後の時間で分ける。

システム評価では、衝突・near miss、経路不成立率、不要停止、最小障害物距離、地図更新遅延、メモリ・CPU、通信帯域を見る。特に「障害物が現れてから地図、計画、制御へ反映されるまで」をend-to-endで測る。

既知の壁と空間で、単一rayのmark/clearと座標変換を単体試験する。
記録データで同じ入力から同じ格子が出る決定性を確認する。
センサー遮蔽、時刻ずれ、姿勢誤差、雨雪ノイズ、通信断を注入する。
静止物、横切る歩行者、追越し車、停止後に再発進する物体をテストコースで測る。
計画器・制御器まで接続し、古い格子、unknown、更新途中の地図に対する縮退を確認する。
更新ごとに基準ログを再生し、地図だけでなく軌道と安全余裕の差分を取る。

11. 研究動向

Semantic occupancy：占有だけでなく、車道、歩道、車両、歩行者、植生などの分布を持つ。
4D occupancy/flow：3D空間と将来時間について占有・速度を予測し、箱検出に依存しない計画入力を作る。
不確実性校正：天候・地域・センサー故障で確率が過信にならないよう分布外検出と校正を行う。
Neural implicit map：連続座標をニューラル場で表し、解像度とメモリの制約を変える。衝突照会時間と安全保証が課題。
協調占有：車両・路側機で遮蔽領域を共有する。帯域、座標誤差、情報源相関、攻撃耐性が鍵になる。

新しい地図表現を評価するときは、描画の美しさより、unknownを区別できるか、確率が校正されるか、期限内に衝突照会できるか、故障時に保守的へ倒れるかを見る。

まとめ

占有格子地図は、空間を白黒へ塗る画像処理ではない。センサーが通過した自由空間、反射した占有空間、まだ見ていない未知空間を、時刻と誤差を伴う確率として蓄積するベイズ推定である。log-odds更新、ray tracing、clamp、時間減衰を理解すれば、消えない障害物と突然消える壁の原因を追える。

そして地図は単独で完成しない。SLAMが座標を支え、認識が動的物体と意味を与え、経路計画とMPCが車体形状・停止距離を含むコストとして使う。最終評価はセルIoUだけでなく、その地図によって車両が安全に止まり、不要停止を抑え、未知へ無謀に進まなかったかで決まる。

参考資料

#占有格子地図 #Occupancy Grid #ベイズ推定 #LiDAR #センサ融合 #Costmap #SLAM #自動運転