走行モード（planner）¶
【シム／ホスト検証】
対象: raspi/auto/。確認した版: 上位 19291eb。周回時間の数値はシミュレータの値で、実機の値ではありません
（検証状況）。
planner の約束¶
自動運転の走らせ方は、すべて Planner の形にはめてあり、GUI から選べます（base.py）。守る約束は 4 つだけです。
plan()は状態を持ってよいが、reset()で完全に初期化できる。 モードを切り替えたときに前のモードの舵の平滑化が残っていると、engage した瞬間に前回の続きから舵が動き出すため。- 戻り値は
AutoState。指令（target_speed/target_steer/brake）と、その理由（reason）を必ず同時に返す。 「なぜ止まったか」が後から分からないのが、デバッグを最も消耗させる。 - 走ってよいか分からないときは
ready=False。迷ったら止まる。ready=Falseはplanning_nodeが制動指令に読み替える。 - パラメータは
paramsに宣言する。 GUI のスライダはこの宣言から自動生成されるので、planner を足しても GUI は 1 行も変えなくてよい。
新しい走らせ方を足す手順は 2 つ（raspi/auto/<名前>.py を書く、registry.py の PLANNERS に 1 行足す）だけです。
id は保存キーでもある（config/auto.json と localStorage がこの文字列でパラメータを覚える）ので、後から変えると設定が黙って既定値に戻ります。
単位は SI のまま（距離 [m]、角度 [rad]、車両座標・反時計回りが正）。度に直すのは表示用の *_deg だけです。
計画は入力が来たときだけ回します。LiDAR は 10 Hz、auto/cmd の発行は 50 Hz なので、同じ周で 5 回計算しても答えは変わらないため。
どの planner も「固定距離のハード停止」は持ちません。 近づきすぎたら止める役目は、STM32 の auto_stop
（速度に応じて伸びる動的停止距離、自動停止）に任せています。
正面が詰まったときに減速するのは、緊急停止ではなく走行の一部として残してあります。
一覧¶
| id | 名前 | 入力 | 地図 | 概要 |
|---|---|---|---|---|
ftg |
Follow the Gap | LiDAR | 不要 | 最も広く空いている方向へ |
de |
Disparity Extender | LiDAR | 不要 | 通れない隙間を先に塞いでから、一番遠い帯を狙う |
dp |
Disparity Pursuit | LiDAR | 不要 | DE の安全マージン ＋ FTG の Pure Pursuit 舵角 ＋ ヒステリシス |
ftg_cam |
Follow the Gap（カメラ） | カメラ（擬似 LiDAR） | 不要 | ftg を擬似 LiDAR（scan/cam）で流用 |
cam_centerline |
カメラ中心線 | カメラ | 不要 | 走行可能領域の回廊の中心線を追従 |
cam_e2e |
カメラ E2E | カメラ | 不要 | 画像から操舵を直接回帰（模倣学習） |
line_trace |
ライントレース | カメラ | 不要 | 白線を追う |
e2e_lidar |
LiDAR E2E | LiDAR | 不要 | 強化学習した方策（ONNX） |
slam2d_route |
経路選択走行 | LiDAR ＋ IMU ＋ 車速 | 作る | 地図→道路グラフ→経路→周回 |
follow_object |
対象追従 | カメラ ＋ LiDAR | 不要 | 映像上で選んだ対象との車間距離を保つ |
park_to_point |
駐車 | LiDAR | 局所のみ | 指定位置・向きへ自律的に接近して停止 |
sysid_* |
システム同定 | — | — | 同定専用の試験（検証の仕組み） |
開発記録（2026-10-14）によれば、GUI の自動運転モードからは slam2d_raceline（レーシングライン）が外され、slam2d_route に一本化されています
（コード上は PLANNERS に両方が登録されたままです）。後者は前者の派生で、分岐のあるコースにも対応します（地図と自己位置推定、レーシングライン）。
反射型（地図も自己位置も要らない）¶
Follow the Gap¶
① 視野を切り出す → ② 前処理（欠測・飽和・測距不能を安全側へ、最小値フィルタ）
→ ③ 安全バブル（最近傍の点の周りを侵入禁止に） → ④ gap_min 以上が続く最長の区間を選ぶ
→ ⑤ その区間の真ん中を狙う → ⑥ 速度は正面の余裕と、実際に切る舵の曲率で落とす
狙点の方位 \(\eta\) から舵角への変換は Pure Pursuit の \(\delta = \arctan(2L\sin\eta / L_d)\) です。
前方注視距離をギャップの奥行きにしたら壁に当たった
最初は \(L_d\) を「ギャップの奥行き（区間の平均距離）」にしていましたが、コーナーで曲がりきれず壁に当たる欠陥がありました。 交差点やコーナーでは曲がる方向のギャップが奥まで開けていることが多く、そのまま \(L_d\) にすると「奥まで見えるから急がなくていい」 という判断になり、その場の通路幅の中で曲がりきれません。\(L_d\) を速度比例 \(L_d = k\,v + L_{\min}\) にしたところ、 壁に近づいて減速するとき \(L_d\) も一緒に縮み、「減速している＝鋭く曲がりたい瞬間ほど鋭く曲がる」向きが揃いました。
速度は、正面の余裕による減速に加えて、実際に切る舵角から曲率 \(\kappa = \tan\delta / L\) を求めて横加速度の上限 \(v \le \sqrt{a_{\mathrm{lat,max}}/\kappa}\) をもう一つの上限にします（曲率ベースなので、緩い旋回は落としすぎず、きつい旋回は物理的な根拠がある分だけ確実に落ちる）。
Disparity Extender（DE）¶
F1TENTH で広く使われている手法です。FTG との違いは狙点の決め方だけです。
- 隣り合う測定値が
disparity_m以上飛んでいる所が「物の縁」。 - 縁の遠い側は、手前の物の陰なので、車体半幅ぶんは通れない。近い側の値 \(r\) で塗り潰す。塗る角度幅は \(\theta = \mathrm{atan2}(\text{safety\_half\_width}, r)\)（近いほど広い）。
塗るときは
min()で入れる（上書きだと処理順で結果が変わる。minなら順不同で同じ答えになり、常に安全側にしか動かない）。 - 残った所のうち一番遠い帯の真ん中を狙う。
ablation（シム、circuit コース、180 秒、いずれも衝突 0。行ごとに別々の比較）
| 変更 | 周回時間 |
|---|---|
塗った後に argmax で点を狙う（帯の端を選び、コーナー内側を舐める） |
48.7 s |
| 帯の真ん中を狙う | 16.9 s |
| 速度を「正面 ±20° の扇の最小値」で決める（道幅 1.0 m では中央にいても壁が 0.5 m 横にあるので、直線でも見通しが 1.5 m を超えない） | 62 s |
| 速度を「狙う方向の塗り後の距離」で決める | 17 s |
FTG → DE の比較（同条件）: circuit 37.2 s → 16.9 s、oval 36.5 s → 15.8 s、split は FTG が 1 周もできず DE で 40.5 s
（safety_half_width を 0.12 m に下げる）。 — disparity_extender.py のコメントより。シムの値です。
Disparity Pursuit¶
DE の「安全マージンの取り方・狙点の選び方」と、FTG の「Pure Pursuit による舵角」を合わせ、さらに狙点のヒステリシスと安全半幅の速度依存化を足したものです。 コードのコメントによれば、「安全マージンの取り方・狙点の選び方」は DE が、「狙点を舵角にする式」は FTG（Pure Pursuit 化済み）が優れているため、どちらか片方を持ってきても最良にならないので、両方の良い所を合わせた、という位置づけです。
カメラ系¶
cam_perception_nodeが前方カメラの画像から、ONNX のセグメンテーションで「走行可能／不可能」の 2 値マスクを作り、 魚眼のレンズモデル（Kannala-Brandt）で補正したうえで地面座標へ逆投影（IPM）し、占有格子から 擬似 LiDAR（scan/cam） と、 前方の距離ごとの走行可能な回廊の中心線（path/cam）の 2 つを出します。ftg_camは前者を、cam_centerlineは後者を使います。cam_e2eは、IPM の深度誤差が低いカメラ高さで拡大する問題（「実車で奥行きを掴むのが難しいと分かった」）を、 画像から操舵を直接回帰して迂回します。人間の運転ログによる教師あり学習で、速度は出力の舵角の曲率から決める（LiDAR は見ない）。line_traceは白線を検出して Pure Pursuit で追います。白線を見失ったら止まる（コースアウトの方が停止よりずっと危険なため）。- 矢印信号（
arrow_signal_node）は、CNN ではなく HSV のしきい値＋モーメントで読みます。シミュレータがカメラ画像を生成しないので学習データを合成できず、 実物の看板を実写するしかないのに対し、閉じた問題（3 矢印 × 薄い色 3 系統）なので古典的な処理で足りる、という判断です。
学習ベース（LiDAR）¶
e2e_lidar は、シミュレータ上で強化学習（Stable-Baselines3 の PPO）した方策を ONNX にしたものを読み込んで推論するだけの薄い planner です。
「コースに沿って進めたら＋報酬・衝突したら－報酬」で試行錯誤した方策で、模倣学習のように DE の実力を再現するのではなく、それを超えうる反面、
未知の点群パターンでの振る舞いは保証できないので、独立した安全策として STM32 の auto_stop で受けます。
モデルが読み込めなくても例外を投げず、今のモデルを保持したまま続行します（走行中のモデルが「壁」扱いに落ちるより安全側のため）。
学習は 2026-09 の v27 で止まっています（開発記録）。
駐車と対象追従¶
park_to_point¶
flowchart LR
A["クリックで目標指定"] --> B["ScanAnchor<br/>2 フレーム登録で姿勢"] --> C["LocalMap<br/>局所占有格子 + ESDF"] --> D["Hybrid A*<br/>障害物を避ける経路"] --> E["閉ループ追従"] --> F["掃引ベースの安全停止"]slam2d を使いません。 ポーズグラフ・ループ閉じ・g2opy が「現状不安定」で、駐車はそれを必要としないからです。
その代わり、クリックした瞬間のスキャンを原点のキーフレームにして、2 フレーム登録（点対線 ICP）で姿勢を保ちます。
なぜ地図ではなくスキャンアンカか
ユーザーが壁際をクリックしたときの意図は「あの壁に対してこの位置」です。推定がドリフトすると、駐車枠そのものが実世界の壁に対して静かに動き、 planner は誤った場所で「完了」と言います。誤差が誤差として現れず、成功として現れることが問題です。
シム実測（ジャイロバイアス 0.5°/s、オドメトリ +2 %）: 推測航法のみ 2/12 成功・最終位置誤差 16.3 cm → スキャンアンカ 7/12 成功・6.1 cm。
精度を支配するのは ScanAnchor.max_pair（対応点として認める距離）で、既定の 0.30 m は「1 周期ぶんの相対移動」想定で駐車には広すぎ、別の壁との誤対応を通す。
最新のベンチ（2026-10-14）は 42/48 成功・衝突 0/48。
follow_object¶
映像上でドラッグ選択した対象との車間距離を保って追従します。初版は前進オンリー・停止のみの安全側で、見失ったら舵を保持しつつ速度だけ 0 へ減衰し、
タイムアウトで停止。近づきすぎたら停止（後退はしない）。TargetTrack は事実（追跡中・見失い時間・方位・距離）だけを運び、
停止判断の責務は planner が持ちます。