地図と自己位置推定(slam2d)¶
要点
- LiDAR の点群から地図を作り、地図の中の自分の位置を推定します(2D LiDAR SLAM)。
- 点ごとのゆがみの補正、推測航法を手がかりにした重ね合わせ(ICP)、ループ閉じなどを、車に依存しない独立したライブラリとして作っています。
- 精度の数値はほとんどがシミュレータの値です。Raspberry Pi 5 での処理時間は実測しています。
【シム/ホスト検証】+【実機確認】(処理時間と周回)
対象: slam2d/(車体非依存の汎用 2D LiDAR SLAM ライブラリ)と、それを使う走行モード
slam2d_raceline.py / slam2d_route.py。確認した版: 上位 7c7054e(2026-11-15)。
精度の数値はほとんどがシミュレータ(sim/slam_bench.py)の値です。公開データセットでの検証は未着手です(slam2d/README.md)。
設計の骨格¶
slam2d/ は raspi.* を一切 import しない独立ライブラリで、車体・センサ構成に依存しません。特定車両の動力学、LD06 のプロトコル、
Ackermann 固有のロジックは、MotionModel の抽象と薄いアダプタ(raspi/auto/_slam2d_nav.py)から注入します。
flowchart LR
S["LiDAR 1 周<br/>(10 Hz)"] --> D["点ごとの脱スキュー<br/>(ジャイロ・車速 100 Hz)"]
D --> P["推測航法で予測"]
P --> R["位置合わせ<br/>点対線 ICP<br/>(Gauss-Newton)"]
R --> K["キーフレーム<br/>→ 地図に焼く"]
K --> LC["ループ検出<br/>ポーズグラフ最適化<br/>(g2opy)"]
LC -. "補正" .-> K
1. 点ごとの脱スキュー¶
回転式 LiDAR は 1 周が瞬時ではない(LD06 は 100 ms)ので、車体が動くと点群が扇形に歪みます。歪んだまま地図に焼くと壁が二重になります。
「この周のあいだ twist は一定」と仮定する近似は、高速のヘアピンで破綻します(1 周 100 ms のあいだにヨーレートが 0 から 2.2 rad/s まで立ち上がると、3 m 先の点が 30 cm ずれ、
2 m/s で残差 RMS が 10〜15 cm になって位置合わせが誤った姿勢に貼り付いた)。そこで、ジャイロ・車速の時系列(100 Hz)から点ごとの時刻の姿勢を作って補正します。
このため、planning_node は 10 Hz の plan() とは別に、vehicle_state を受けるたびに SLAM に流し込みます。
点の扱いは 3 種類に分けます: 実測点(壁を打ち、手前を空きに彫る)、飽和点(測距上限。手前を空きに彫るだけ)、無効点(捨てる。「知らないことは書かない」)。
2. 位置合わせ — 点対線 ICP + 推測航法を事前分布に¶
- \(r_i\): 点と最寄りの壁の面要素との距離(平面なら法線方向、角や柱なら点対点)。\(\rho\) は Huber。
- 第 2 項が推測航法(ジャイロ + 車速)の事前分布で、\(\Omega_{\mathrm{pred}}\) は運動モデルの予測共分散の逆。予測の不確かさは「走った量」に比例させます (車輪径・ジャイロ倍率の誤差は距離・角度に比例、ゼロ点ずれは時間に比例)。
観測できる方向(壁に垂直)では点群の情報が桁違いに大きいので点群が勝ち、観測できない方向(通路の進行方向)では推測航法が残る—— 方向ごとの利得を手で決める必要がありません(Cartographer の Ceres スキャンマッチャや PL-ICP と同じ考え方、とコードにあります)。
相関スキャンマッチをやめた理由
候補姿勢の総当たり(相関スキャンマッチ)は初期値が悪いときに強い一方、答えが探索格子に量子化され、推測航法との融合が「得点 − 罰則」の近似になるため、 推測航法の系統誤差を正せません。 シムの実測では、車速の倍率誤差 2 % だけで ATE が 1.2 cm → 6 cm、ジャイロのゼロ点ずれ 0.5°/s だけで複雑なコースの ATE が 11 cm でした。 総当たりは、見失いからの復帰とループ検出の初期値づくりにだけ使います。
面要素の地図¶
距離変換そのものを残差にすると、複数スキャンを重ねた地図で壁が 2〜3 セルの厚みになり、壁の内側が距離 0 の平らな台地になって、そこに落ちた点は姿勢を何も拘束しません。 回頭方向の情報が推測航法と同程度まで落ち、ジャイロのゼロ点ずれが地図ごと回っていきました(2 周で向きが 17° ずれた)。 そこで壁のセルごとに「落ちた点の平均位置」と「近傍の点の主成分から作った法線」を持ち、残差を \(r = \mathbf{n}\cdot(\mathbf{p}-\mathbf{q})\) にします。 近傍の裏付けが少ないセルは対応に使いません(遠くの壁でまばらな点が「角」に見え、平行な壁しかない通路なのに進行方向が決まっているかのような情報が出るため)。
3. 地図作成中は「直近のキーフレームだけの局所地図」に合わせる¶
全体の地図に合わせると、周回して戻ったとき、ドリフトしてずれた「前の周の壁」と「今の周の壁」の両方に点が引っ張られ、壁が二重になった地図に中途半端な姿勢が出て、二重の壁が太り続けます。 周回のずれを正すのはループ閉じの役目で、フロントエンドは小さくなめらかにドリフトさせておく方が正しく直せます(Cartographer の local SLAM と pose graph の分担と同じ)。 凍結した地図の上では全体の地図に合わせます(追跡専用、ドリフトが溜まらない)。
4. ジャイロのゼロ点ずれは「止まっているとき」に測る¶
走行中の回頭は、地図作成中は局所地図を基準に測っています。ゼロ点ずれで姿勢がゆっくり回ると、その姿勢で焼いた局所地図も一緒に回るので、点群は「ずれていない」と答えます。 実測でも、真のゼロ点ずれ 0.5°/s に対して走行中の学習値は 0.01°/s にしかなりませんでした。 止まっている間はヨーレートの読みがそのままゼロ点ずれなので、静止中に速く学びます(ZUPT)。実車は ARM → engage の操作の間ずっと止まっているので、走り出す前に較正が終わります。 車速の倍率誤差も学びますが、進行方向が点群で観測できているときだけ更新します(通路の直線では点群が進行方向を決められず、比を取ると 1.0 が返って正しい値から引き戻されるため)。
5. 見失いの扱い¶
- 当たり率(照合できた点のうち壁から 5 cm 以内の割合)が下限を切るか、照合できた点が少ない・残差が大きいとき、その周期は推測航法の予測を採り、地図は焼かない。 地図はあるのに合わせる点が 1 つも無い周(全点が範囲外・壁の向こう・無効)も見失いとして数えます。数えないと、合わせていない推測航法の姿勢がキーフレームとして焼かれ、壁が二重になります。
- 凍結した地図で走るときは、地図の壁の向こうに見えた点(予測姿勢から地図の壁まで撃ったレイより 0.3 m 以上遠い点。低い壁越しに見えた奥など)を照合から外します。
- 凍結した地図で走るときは、地図に無い物(置かれた障害物・人・他車)の点を照合から外します。地図が空きと確定した所で、壁から 12 cm 以上離れた点です。 外さないと、壁の近くの障害物の点が壁へ対応付けられて姿勢を引っ張り、残差が大きくなって見失いになりました(2026-11-06。外すと同じ記録で見失い 13 周期 → 0)。
- 外した点(壁の向こう+手前)が全体の半分を超えたら、予測がずれて壁の点まで外れたのか、本当に地図に無い物で大きく遮られたのかを確かめます。 外した残りと全点の両方で合わせ、全点のうち壁に乗る点が明らかに多い方を採ります。以前は無条件に全点へ戻していて、停車中に何かが近くに来た周期に一致度が 1.00 → 0.23 と落ちて見失いました(2026-11-08 の記録)。 この変更(2026-11-09)は実機では確かめていません(開発記録)。
- 続いたら、予測の周りを総当たりで探し直す。
- 地図作成中にさらに続いたら、局所地図を今の点群から作り直して走り続ける(ずれを受け入れる)。 「見失い中は地図を更新しない」方式だと、一度見失うと永久に戻れませんでした。
- 周回中(RACE)は、見失っても 1.5 秒(
coast_s)までは推測航法の姿勢で経路を追い続け、その間は 0.8 m/s(coast_speed)に抑えます。それを超えたら止まります。 止まったまま 3 秒続いたら(LOST_RELOCATE_S)、今の推定姿勢を種に LOCATE(全域の探索)へ戻ります。凍結地図の上の追跡は予測の周り ±0.4 m・±15° しか探さないためです。
ループ閉じ¶
- 古いキーフレームだけで作った部分地図に照合します。「今作っている全体の地図」に照合すると、現在地付近は直前の数秒で焼いた壁(今の推定と整合している)が大半を占め、 照合は今の推定をほぼ追認し、「ドリフトした関係」がそのままループ拘束になります(ループ閉じ後に地図の精度が 0.72 → 0.26 に悪化したコースがあった)。
- 総当たりの 1 位が 2 位より十分高い、照合できた点・当たり率・RMS が基準を満たす、をすべて満たしたものだけを拘束にします(対称な形の取り違えを弾く)。
- 拘束は Huber で入れ、最適化してみて残差が桁違いに大きいものを外してやり直す。ポーズグラフの最適化は
g2opy。
ループ閉じの最適化は走行中には回さない
ループの検出(拘束を探す)は地図作成中に、1 周期に数段ずつ(loop_steps_per_update)進めます。一括だと Pi 5 で LiDAR の周期 100 ms を超えるためです。
ポーズグラフの最適化と地図の焼き直しはキーフレーム数に比例して重いので、EXPLORE → BUILD に遷移するときに 1 回だけ、1 周期に 8 キーフレームずつ(FREEZE_CHUNK)
進めます。その間は指令を出しません(ready=False)。
RACE 中は Frontend を直接使う追跡専用で、最適化は挟みません。
Pi 5 の実測では、360 キーフレームで 1.76 秒かかります(開発記録 2026-10-14)。キルスイッチ ENABLE_LOOP_CLOSURE で Frontend 単体運用に戻せますが、地図の大域的な歪みは直りません。
継ぎ目を照合してから確定する¶
周回の検出は出発点の約 0.7 m 手前で成立し、1 周目にはその時点で照合先が出発直後の約 2 m しかありません。出発点が直線の通路だと照合が前後に曖昧で捨てられ、
継ぎ目の拘束が 0 本のまま確定して出発点の壁が二重になりました(シム)。そこで最後の周回を検出しても、出発点を越えて走り足し、ループ拘束が 3 本増えるか
6 m 走ってから確定します(LAP_OVERRUN_LOOPS・LAP_OVERRUN_MAX_M)。
走行モードとしての状態機械¶
slam2d_raceline の状態機械は次のとおりです(slam2d_route はこれに、ミッションの終わりの停止・駐車を足す。地図を作って走る)。
stateDiagram-v2
direction LR
[*] --> EXPLORE: 新規の地図
[*] --> LOCATE: 保存済みの地図を読む
EXPLORE --> BUILD: ループ閉じを 1 回
BUILD --> DONE: 中心線とレーシングラインを作り保存
DONE --> LOCATE: 人が開始
LOCATE --> LOCATED: 自己位置を特定(engage 前)
LOCATED --> RACE: 走行を開始
LOCATE --> RACE: 自己位置を復元(engage 中)
RACE --> DETOUR: 障害物で進めない
DETOUR --> RACE
RACE --> LOCATE: 見失って止まった・解除した
| 状態 | 内容 |
|---|---|
EXPLORE |
地図作成。engage 中は Follow the Gap で走る(していなければ人のラジコン) |
BUILD |
中心線とレーシングラインを引く |
DONE |
保存して人を待つ |
LOCATE |
保存地図の上で自己位置を復元(グローバルローカリゼーション) |
LOCATED |
自己位置は特定できたが、まだ自動運転に入っていない。GUI の「走行を開始」を待つ(追跡を見失い続けたら LOCATE へ戻る) |
RACE |
周回 |
DETOUR |
障害物・壁に当たって進めない、または経路から外れすぎたときに、Hybrid A* で経路へ戻る |
自己位置の復元(LOCATE)¶
保存済みの地図を読んで RACE から走り始める場面では、直前の姿勢がありません。地図の「空きと確信できるセル」に候補位置をばら撒き、角度は粗い刻みで全周を回して、 各候補で今のスキャンを面要素地図に対して採点します。
- 候補の評価は 1 周期に 1 角度ずつ進めます(全探索を一度にやると、大きい地図では数百 ms〜秒のブロッキングになり、バス通信を止める)。車両は静止させたままです。
- 左右対称・繰り返し形状のコースでは誤認識しうるので、1 位と 2 位が僅差のときは自動では採用しません(
ambiguous)。 - ただし、粗い尺度だけで諦めてはいけません。オーバル状のコースでは反対側の直線が僅差で並びますが、上位候補を
Frontend.refine()で仕上げてから当たり率で比べると、 差が 0.03 しかなかったものが 0.98 対 0.60 に開きました。上位 12 候補(LOCATE_CANDIDATES)を 1 周期に 1 つずつ仕上げます。 - 仕上げた候補は、まず全点に対する当たり率と残差の厳密な尺度で比べます。地図に無い他車が走査線を遮ると、真の姿勢でもこの点数が潰れるので、
良い候補が無いときは遮り物に強い尺度(壁に乗った点の割合から、光路が壁を突き抜けた点の割合を引く。
slam2d/core/locscore.py)で比べ直します。 どちらの尺度でも 2 位が 1 位に迫っていれば断ります。 - 車が動いている間は探しません(途中までの候補は別の場所の点群で仕上げたものになる)。止まってから最初からやり直します。
- 走行中に解除したときは、地図を読み直して LOCATE からやり直しますが、まず止まった姿勢の周りだけで 1 回照合します(
_locate_at_pose)。 位置だけを種にした探索は Pi で 3〜12 秒かかり、解くたびに待っていたためです。外れたら種からの探索、さらに外れたら全域の探索に落ちます。 種の周りを探す半径は、解除してから止まるまでに転がる距離(\(v^2/2a\) に余裕を足したもの)で決めます。3.75 m/s で解けば約 3 m 転がるので、 固定の 0.4 m では真の位置が範囲に入らず、必ず全域の探索に落ちていました(2026-11-14 の実機の記録)。 - GUI の地図をタップして位置のヒントを与えたときは、その周り 0.4 m → 0.8 m → 1.2 m → 全域、と外れるたびに範囲を広げます。タップは真の位置からずれやすいためです。
- 範囲が絞れているとき(タップのヒント・解除した位置)は、採用する点数の下限を下げます。実機の地図では、一致度 0.99 で追えている真の姿勢でも厳密な点数が 0.27〜0.51 にとどまり、 下限 0.5 では特定できませんでした。間違った候補は 0.05 以下で、取り違えは範囲の中に限られ、2 位との比でも守られるので下げても安全、という判断です。
- タップの直後に人が車に触れた程度の微動では探索を捨てず、3 周期続けて動いたときだけ最初からやり直します。
重い計算は別プロセスで¶
道路グラフ(Pi で数百 ms)や経路ごとのレーシングライン(1 本数十〜数百 ms)を plan()(10 Hz)の中で回すと、指令が途切れて
cmd_deadman_ms(150 ms)で DISARM になります。そのため別プロセスのワーカーに投げ、plan() は毎周期、出来上がりを覗くだけにします。
ワーカーは地図の写しだけを触ります。
Pi 5 での処理時間(実測)¶
開発記録(2026-10-14)の実測で、Mac の約 5.5〜8 倍遅いとされています。下の表は内側を C にする前の値です。C にした後の値は C による高速化 にあります。
| 処理 | 平均 | p99 | 最大 |
|---|---|---|---|
| 地図作成中の 1 スキャン | 16.5 ms | 105 ms | 133 ms |
| 凍結地図での追従 | 8.2 ms | — | 11.1 ms |
ループ閉じの flush(360 キーフレーム) |
1.76 s | — | — |
地図作成中は LiDAR 周期(100 ms)を超えることがあります。 設計が「走りながら地図を作り込む」ではなく 「まず地図を作り(EXPLORE)、ループ閉じを 1 回払って、凍結地図で走る(RACE)」になっているのは、この制約と整合しています。