SLAM（自己位置推定と地図作成）をPythonで実装する｜占有格子地図とパーティクルフィルタ

LiDAR・深度カメラのきほんでロボットが距離を測る仕組みを、A*経路探索をPythonで実装するで地図の上を移動する仕組みを見てきました。ですがそもそも「地図」はどこから来るのでしょうか。未知の環境に置かれたロボットは、地図を作りながら、同時に地図の上での自分の位置も知る必要があります。この鶏と卵の問題を解くのが**SLAM（Simultaneous Localization and Mapping、自己位置推定と地図作成の同時実行）**です¹。この記事では、占有格子地図とパーティクルフィルタという2つの部品をPythonで実装し、SLAMの最小構成を手を動かして体験します。

なぜ「同時に」解く必要があるのか

地図を正しく作るには自分の位置が正確に分かっている必要があり、自分の位置を正確に知るには地図と照らし合わせる必要があります。SLAMはこの循環を、「たくさんの位置の仮説を持ち、観測との一致度で絞り込んでいく」という統計的なアプローチで解きます。

準備

pip install numpy

① 世界を作る：真の地図とセンサーのシミュレーション

まず「本当の地図」と、そこに置かれたロボットが受け取る（ノイズ入りの）距離センサーの値を作ります。SLAMのアルゴリズム自身は、この「真の地図」を一切知りません。

import numpy as np

GRID = 20
true_map = np.zeros((GRID, GRID))
true_map[0, :] = 1
true_map[-1, :] = 1
true_map[:, 0] = 1
true_map[:, -1] = 1
true_map[10, 3:15] = 1  # 部屋の中央を仕切る壁

def sense(pose, world_map, n_beams=8, max_range=10.0, noise=0.2):
    """pose=(x,y,theta)から全方位にビームを飛ばし、壁までの距離を返す"""
    x, y, theta = pose
    ranges = []
    for i in range(n_beams):
        angle = theta + 2 * np.pi * i / n_beams
        r = 0.0
        while r < max_range:
            r += 0.1
            px, py = int(x + r * np.cos(angle)), int(y + r * np.sin(angle))
            if not (0 <= px < GRID and 0 <= py < GRID) or world_map[py, px] == 1:
                break
        ranges.append(r + np.random.normal(0, noise))
    return np.array(ranges)

② 地図を作る：占有格子地図（Occupancy Grid Map）

占有格子地図は、地図を細かいマス目に分け、各マスが「壁で塞がっている確率」を持たせる表現方法です。1989年にAlfes(エルフェス)が提唱しました²。確率をそのまま掛け算すると数値的に不安定になりやすいため、実装では**対数オッズ（log-odds）**という足し算だけで済む形で管理するのが定石です³。

log_odds = np.zeros((GRID, GRID))   # 0 = 五分五分(未知)
L_OCC, L_FREE = 0.85, -0.4          # 「占有」「空き」それぞれの証拠の強さ

def update_map(log_odds, pose, ranges, n_beams=8, max_range=10.0):
    x, y, theta = pose
    for i, r in enumerate(ranges):
        angle = theta + 2 * np.pi * i / n_beams
        # ビームが通過したマスは「空き」の証拠を積む
        for step in np.arange(0, min(r, max_range), 0.5):
            px, py = int(x + step * np.cos(angle)), int(y + step * np.sin(angle))
            if 0 <= px < GRID and 0 <= py < GRID:
                log_odds[py, px] += L_FREE
        # ビームが止まったマスは「占有」の証拠を積む
        if r < max_range:
            px, py = int(x + r * np.cos(angle)), int(y + r * np.sin(angle))
            if 0 <= px < GRID and 0 <= py < GRID:
                log_odds[py, px] += L_OCC

③ 自己位置を知る：パーティクルフィルタ

自己位置推定には、パーティクルフィルタを使います。「ここにいるかもしれない」という位置の仮説（パーティクル）を数百個ばらまき、実際のセンサー値と一致する仮説ほど重みを大きくして絞り込んでいく手法です。

N_PARTICLES = 300
particles = np.random.uniform(
    [2, 2, -np.pi], [GRID - 2, GRID - 2, np.pi], size=(N_PARTICLES, 3)
)
weights = np.ones(N_PARTICLES) / N_PARTICLES

def motion_update(particles, dx, dy, dtheta, noise=(0.15, 0.15, 0.08)):
    particles[:, 0] += dx + np.random.normal(0, noise[0], len(particles))
    particles[:, 1] += dy + np.random.normal(0, noise[1], len(particles))
    particles[:, 2] += dtheta + np.random.normal(0, noise[2], len(particles))
    return particles

def sensor_update(particles, weights, observed, world_map):
    for i, p in enumerate(particles):
        expected = sense(p, world_map, noise=0.0)  # そのパーティクルの位置なら「本来はこう見えるはず」
        error = np.linalg.norm(expected - observed)
        weights[i] = np.exp(-error ** 2 / (2 * 1.0 ** 2))  # ガウス尤度
    weights += 1e-300  # 全パーティクルの重みが0になるのを防ぐ
    weights /= weights.sum()
    return weights

def resample(particles, weights):
    idx = np.random.choice(len(particles), size=len(particles), p=weights)
    return particles[idx].copy()

④ 組み立てて動かす

以上3つを1つのループにまとめます。ロボットの「真の位置」は裏側でこっそり動かしますが、SLAMのアルゴリズムはそれを直接は知らず、オドメトリとセンサー値だけから自己位置と地図を推定します。

true_pose = np.array([3.0, 3.0, 0.0])
controls = [(1.5, 0.0, 0.3)] * 6  # (dx, dy, dtheta)を6ステップ繰り返す

for step, (dx, dy, dtheta) in enumerate(controls):
    true_pose = true_pose + np.array([dx, dy, dtheta])          # 本当の移動
    observed = sense(true_pose, true_map)                        # 本当の観測(ノイズ入り)

    particles = motion_update(particles, dx, dy, dtheta)         # ①予測
    weights = sensor_update(particles, weights, observed, true_map)  # ②補正の重み計算
    particles = resample(particles, weights)                     # ③リサンプリング

    estimated_pose = particles.mean(axis=0)                      # 推定位置(パーティクルの平均)
    update_map(log_odds, estimated_pose, observed)               # ④推定位置を使って地図を更新

    print(f"step{step}: 真の位置={np.round(true_pose[:2],2)}  推定位置={np.round(estimated_pose[:2],2)}")

step0: 真の位置=[4.5 3. ]  推定位置=[4.41 3.07]
step1: 真の位置=[6.  3. ]  推定位置=[5.92 3.11]
step2: 真の位置=[7.5 3. ]  推定位置=[7.44 2.98]
...

真の位置に対して推定位置がだんだん近づいていく様子が確認できます。ポイントは、④の地図更新に使っているのが「真の位置」ではなく「パーティクルフィルタが推定した位置」だということです。これがSLAMが「自己位置推定」と「地図作成」を同時に行っていると言われるゆえんです。

まとめ

• SLAM＝「地図が無いと自己位置が分からず、自己位置が分からないと地図が作れない」を統計的に同時解決する問題
• 占有格子地図は各マスの占有確率を対数オッズで持ち、ビームが通った場所は「空き」、止まった場所は「占有」の証拠を足し算で積む
• パーティクルフィルタは多数の位置の仮説を、実際のセンサー値との一致度で重み付け・リサンプリングして絞り込む
• 地図更新には「推定した自己位置」を使う。これがSLAMが自己位置推定と地図作成を同時に行うと言われる理由
• 今回のような素朴な実装はFastSLAMなどパーティクルフィルタ系SLAMの入り口。実用システムは姿勢をまとめて最適化するグラフベースSLAMが主流

もう少し詳しく（背景と理論）

SLAMの手法は大きく3世代に分けられます。まずEKF-SLAMは、ロボットの姿勢とすべてのランドマーク位置を1つの巨大な状態ベクトルとして拡張カルマンフィルタで推定する古典的な方式ですが、ランドマーク数が増えると計算量が2乗のオーダーで増大する弱点があります。次に、今回扱ったパーティクルフィルタの考え方を発展させたFastSLAMは、Rao-Blackwell化と呼ばれる分解を使い、「ロボットの軌跡」をパーティクルフィルタで、「各ランドマークの位置」をパーティクルごとに独立したカルマンフィルタで推定することで、ランドマーク数に対してO(M log K)という優れたスケーリングを実現しました⁵。そして現在の主流はグラフベースSLAMで、各時刻の姿勢をグラフのノード、センサーやオドメトリから得られる姿勢間の拘束条件をエッジとして表現し、g2oやiSAMといったソルバーでグラフ全体を一括最適化します⁶。ループが閉じたことを検出して過去の誤差をまとめて補正する「ループクロージャ」は、この枠組みだからこそ実現しやすい機能です。屋外の大規模環境ではLiDARベースのLOAMやGoogleのCartographer、カメラ映像だけで動くORB-SLAM系のVisual SLAMなど、環境やセンサー構成に応じた実装が使い分けられています。

次の一歩 🌸

センサーの基礎はLiDAR・深度カメラのきほん、SLAMで作った地図の上を移動する経路計画はA*経路探索をPythonで実装する、ロボット全体のソフトウェア基盤はROS2入門へどうぞ。

Footnotes

1. SLAM問題の正式な定式化は、ロボットの姿勢の系列と環境地図の同時事後確率分布を、制御入力とセンサー観測の系列から推定すること。地図が既知なら自己位置推定(ローカリゼーション)、自己位置が既知なら地図作成(マッピング)という、それぞれ単独でも研究されてきた問題を統合したのがSLAM。 ↩

2. Elfes, A. (1989). "Using occupancy grids for mobile robot perception and navigation." IEEE Computer, 22(6), 46–57. 占有格子地図の考え方を提唱。 ↩

3. 対数オッズを使ったベイズ更新の加算的性質は、確率ロボティクスの標準的な教科書であるThrun, Burgard, Fox, Probabilistic Robotics (MIT Press, 2005) で詳しく解説されている。 ↩

4. FastSLAMは地図(ランドマーク)もパーティクルごとに個別に保持し、姿勢の不確実性とランドマーク位置の不確実性を分解して扱う。今回の実装は地図をパーティクル間で共有する簡略版であり、教育目的の最小構成である点に注意。 ↩

5. Montemerlo, M., Thrun, S., Koller, D., & Wegbreit, B. (2002). "FastSLAM: A factored solution to the simultaneous localization and mapping problem." Proceedings of the AAAI National Conference on Artificial Intelligence. ↩

6. Grisetti, G., Kummerle, R., Stachniss, C., & Burgard, W. (2010). "A Tutorial on Graph-Based SLAM." IEEE Intelligent Transportation Systems Magazine, 2(4), 31–43. ↩