コンテンツにスキップ

地図を作って走る(slam2d_route)

要点

  • 最初の 1 周で LiDAR の地図を作り、その地図の上で自分の位置を推定しながら、計算した走行ラインで周回する、SURGE Mark.2 の主力のモードです。
  • 分岐のあるコースでは、地図から道路のつながり(道路グラフ)を作り、経由点や矢印信号で経路を選びます。経由点が無ければ、見積もったラップタイムが一番短い周回ルートを自動で選びます。
  • 障害物や他の車は、走行ラインからの横ずれを時刻つきで計画して追い抜きます。路面のグリップは走りながら学習します。

【実機確認】(周回)+【シム/ホスト検証】

対象: slam2d_route.py(slam2d_raceline.py の派生)、raspi/nav/、slam2d/。確認した版: 上位 7c7054e(2026-11-15)。 2026-11-09 の変更(解除後に経路に沿って止まる、など)は実機では未確認です(開発記録)。

しくみ

走行の段階

stateDiagram-v2
  [*] --> EXPLORE: 地図なしで開始
  [*] --> LOCATE: 保存した地図を読む
  EXPLORE --> BUILD: 1 周して戻った
  BUILD --> DONE: 経路ができた(地図を保存)
  DONE --> LOCATE: 走行を開始
  LOCATE --> RACE: 地図の上で自分の位置が分かった
  RACE --> DETOUR: 障害物で進めない
  DETOUR --> RACE: 抜けた
  RACE --> [*]: ミッション終了(停止点で止まる/駐車)
段階 すること
EXPLORE(地図作成) 走行を許可していれば Follow the Gap で、していなければ人のラジコンで 1 周し、地図を作る
BUILD(経路づくり) ループを閉じて地図を整え、道路グラフ・中心線・道幅・走行ラインを計算する
LOCATE(自己位置の復元) 保存した地図の上で、今どこにいるかを探す
RACE(周回) 走行ラインを追う(点群の合間も50 Hz で追従)。障害物や他の車は追い抜く
DETOUR(迂回) 追い抜けない静止物を Hybrid A* で抜ける

地図の作り方と自己位置の推定は 地図と自己位置推定、走行ラインと速度の決め方は レーシングライン、追い方は 経路追従と速度指令 で詳しく扱っています。 このページでは、経路の選び方・終わり方・追い抜きを中心に説明します。

自動運転タブの地図パネル

地図パネル。作った地図の上に、速度で色分けした走行ラインと自分の位置が重なる(シミュレータで撮影)。

前方映像に重ねた走行の計画

前方映像に重ねた計画(実機)。紫が今走っている経路、青の破線が指令の舵角のまま進んだときの進路、黄が実際の舵角での進路。指令と実際のずれを映像の上で読める。

設計の詳細

経路を選ぶ — 道路グラフと経由点

地図作成で走った 1 周の軌跡だけを経路にすると、走らなかった分岐は選べません。 slam2d_route は地図全体から道路グラフ(nav/roadgraph.py)を作り、その上で経路を選びます。

slam2d_raceline(土台) slam2d_route(主力)
経路の元 地図作成の最後の 1 周の軌跡 地図全体の道路グラフ
分岐 走った枝しか無い 経由点で選ぶ
道幅の測り方 地図全体で測る 選んだ経路の回廊だけで測る(分岐の口で測りすぎない)
走行中の切り替え なし グループ A〜D(GUI・矢印信号)
終わり方 走り続ける ミッション(n 周、または t 秒)を終えたら停止点で止まる、または駐車する
  • 経由点を置いていなければ自動で選びます。 スタートから出て戻る周回ルートを道路グラフ上で全部挙げ、それぞれに走行ラインを引いて、見積もったラップタイムが一番短いものを選びます。 スタートは道路グラフ(通路の中心線)へ寄せて使います。寄せる半径はまず 0.6 m で試し、近くに道が無いときだけ 1.5 m で試し直します。通路の角から出発すると、中心線から 0.9 m 離れることがあったためです(広い半径では別の道へ誤って寄せやすいので、最初からは使いません)。
  • ルールで通れない道は「避ける範囲」として地図に壁を塗ってから道路グラフを作ります。
  • 走行中に経路を切り替えられます。 経由点のグループ A〜D と自動経路を、GUI のボタンか、前方カメラで読んだ矢印信号で選びます。

経由点の編集

経由点の編集。グループ A(青・5 点)と B(橙・3 点)を地図に置いたところ。上の行には「周回ルート 4 通りを比べて最速」と自動経路の選び方も出る(シミュレータで撮影)。

重い計算は別のプロセスで

道路グラフ(Raspberry Pi で約 0.2 秒)や、経路ごとの走行ライン(1 本 0.1〜0.5 秒、仕上げ込みで数秒)を、10 Hz の計画ループの中で計算すると指令が途切れ、 指令の途絶を検知した安全機構が走行許可を外してしまいます。そこで重い計算は別のプロセスのワーカーに投げ、計画ループは毎周期出来上がりを確かめるだけにしています。

止まり方 — ミッションの終わり

ミッションが終わると、今の位置から停止点までの経路を道路グラフで引き、終点でちょうど 0 m/s になる速度で走ります。 最後は低速で這い、残り距離が「今の速度で止まれる距離」を切ったら制動します。 停止点が「駐車」なら、道の上で止まってから駐車のモードに引き継ぎます。静止してから渡すのは、駐車のモードが低速・局所の地図を前提にしているからです。

追い抜き — 時刻つきの Frenet 格子

障害物や他の車があると、走行ラインからの横ずれ \(d(s)\)(\(s\) は経路に沿った距離)の候補をまとめて作り、 障害物のその時刻の位置と重ならないものの中から、抜ける時間が一番短いものを選びます(overtake.py)。

地図に無い点の塊でも、壁の近くにある長くて細い線(長さ約 40 cm 超)は、障害物ではなく「壁が動いた」ものとみなして避けません(raspi/nav/obstacles.py の _is_wall_shift)。 コースの壁は他の車がぶつかるとへこんだりずれたりするので、地図を作った後に数 cm〜30 cm ずれることがあります。 2026-11-14 の実機の記録では、毎周同じ 2 か所に長さ 0.7 m・厚さ 1 cm の線が障害物として出ていました。本物の障害物は、もっと短く、壁から 0.3 m 以上離れていました。 Werling ら(2010)の Frenet 格子、TUM の graph-based local planner、ForzaETH の Predictive Spliner と同じ考え方です。

\[ d(s) = \begin{cases} \text{5 次多項式(今の横ずれから目標の横ずれへ)} & \text{入る} \\ d_T & \text{並ぶ} \\ d_T\,\bigl(1 - \mathrm{smoothstep}\bigr) & \text{戻る} \end{cases} \]
  • 今の横ずれから出発するので、避けている途中・戻る途中でも毎周期引き直せます。
  • 時刻で判定するので、同じ向きに走る遅い車は「抜く間に相手が進む」ぶんだけ並ぶ区間が伸び、抜く前に追いつかない候補は塞がれません。
  • 形を決め打ちにしない。 入る長さ・並ぶ長さ・戻る長さを格子状に振り、曲がれる限界まで使う短い入り方も候補に入れます。
  • 避ける間も速度を抑えない。 速度は曲率と摩擦だけで決め、安全は横と前後の余裕で確保します。

LiDAR に見えるのは物の手前の面だけなので、見えない奥行きを足して扱います(動く物は車 1 台分の長さ)。動く物は、時間とともに位置の不確かさが増えるとして範囲を広げます。 追い抜けない静止物は、DETOUR の段階で Hybrid A* を使って抜けます。

走りながら路面を学ぶ

床の材質で変わるグリップは、事前に測るのではなく走行中に学習して速度に反映します(レーシングライン)。

  • 横の上限(どこまで横 G を使えるか)は、ラップタイムを手がかりに学習します。
  • 前後の加速・減速の上限は、横の使い具合ごとの包絡として学習します。
  • アンダーステアの強さも推定します。

学習した値は地図ごとに保存し、2 回目からは 1 周目から学習済みの値で走ります。