安全層とフェイルセーフ¶

【設計の説明】

対象: vehicle.c / vehicle.h、heartbeat.c、power.h、app.c。確認した版: ファーム 2a2e9a8。自動停止・ブレーキホールドの数値は 【未検証】（検証状況）。

上位（Pi）が落ちても、STM32 単独で車を止められるのが設計の前提です。ここでは「止め方」を中心に説明します。

誰の指令に従うか¶

Vehicle_Update() が毎周期、次の優先順位で決めます。

flowchart TD
    S[Vehicle_Update] --> E{緊急停止<br/>ラッチ中?}
    E -- はい --> F[フェイルセーフ]
    E -- いいえ --> C{COMMAND が<br/>100 ms 以内に届いた?}
    C -- はい --> R[上位の指令を適用<br/>ApplyRasCommand]
    C -- いいえ --> F
    F --> FB["最大制動 → 止まったら後輪を位置保持<br/>舵は最後の角度のまま保持<br/>ホーン・パッシング・ウィンカーを解除"]

Pi が一度も繋がっていない起動直後も「フェイルセーフ」側です。つまり、Pi が ARM を出さない限り車は押さえ込まれています。

4 つの安全層¶

層	発動条件	動作	解除
① `DRIVE_POWER` のハード遮断	駆動系の過電流 12 A（50 ms 継続）。シグナル系の過電流 6 A（仮値）では LiDAR と駆動の両方を遮断	MD への給電を切る	ラッチ。マイコンのリセットまで復帰しない
② IWDG（独立ウォッチドッグ）	メインループが 500 ms 止まる	MCU をリセット	—
③ ハートビート断	Pi の GPIO の 100 Hz 矩形波が 50 ms 途切れる	緊急停止をラッチ（フェイルセーフと同じ制動）	ラッチ。ハートビートが戻った状態でボタン 2 を押すまで解除しない
④ COMMAND 途絶	UART の COMMAND が 100 ms 来ない	自動でブレーキ（フェイルセーフ）	COMMAND が戻れば復帰

③のハートビートは UART とは別経路です。UART の COMMAND 途絶（100 ms）より速く（50 ms）、 通信そのものが生きていても Pi のソフトが固まった場合を拾えます。

ハートビートは、少なくとも 10 エッジを一度でも観測するまでは監視を始めません。配線していない単体のベンチ確認で緊急停止させないためです（HEARTBEAT_MIN_EDGES）。
IMU の I2C が張り付いてメインループが長く止まったあとは、ハートビートの基準時刻をリセットします。止まっていた間は判定できていなかっただけなので、直後に偶然エッジを取りこぼして誤って緊急停止をラッチしないための処置です（app.c）。
電圧低下（8 セル × 1.0 V = 8.0 V、復帰 8.8 V、1 秒継続）は警告のみで何も遮断しません。 Ni-MH は負荷時のサグが大きいので電圧は長めに平均して判定します。

緊急停止でも駆動電源は切らない¶

電源を切ると止まれない

緊急停止がラッチされても、駆動電源は入れたままにします。電源を切ると MD が制動をかけられず惰行に入り、かえって止まるまでの距離が伸びるからです。電源を落とすのは過電流のように「流し続けること自体が危険」なときの処置で、緊急停止でやるべきなのは最短で止めることです。

ARM（走行許可）の条件¶

上位が出す ARM 要求は、次のすべてを満たすときだけ「走行してよい」状態（armed）になります。

armed = ARM 要求  かつ  ステア中心が較正済み  かつ  3 台の MD が起動済み
        かつ  モードが MANUAL または AUTO

stateDiagram-v2
    direction TB
    [*] --> 待機: 起動（駆動電源 OFF）
    待機 --> MD起動待ち: ARM 要求 + 中心較正済み<br/>→ 駆動電源を入れる
    MD起動待ち --> 走行可: 3 台の MD が使える
    MD起動待ち --> MD異常: 5 秒経っても揃わない
    走行可 --> 停止待ち: ARM が外れた
    走行可 --> MD異常: 走行中にどれかが使えなくなった
    停止待ち --> 待機: 車速 0.05 m/s 未満 or 3 秒経過
    MD異常 --> 待機: ARM を外し駆動電源が落ちる

MD の起動待ち: MD は給電から約 1 秒、指令を受け付けません。その間に車速 PI を回すと、積分が振り切れた状態で MD が動き出して最大トルクが一気に出るため、3 台が使える状態になるまで armed にしません（上限 5 秒、VEHICLE_MD_BOOT_TIMEOUT_S）。
ARM が外れたら、すぐには駆動電源を切らない。 走行中に外れたら、停車（< 0.05 m/s）するまで最大制動を続けてから切ります（上限は最高速から校正した制動の減速度で止まり切れる時間の 1.5 倍、約 3 秒。減速度は上位が送る）。上位は通信途絶やデッドマンで「安全側」として DISARM を送ってくるため、 ARM ビットで即座に電源を切ると惰行になってしまうからです。
MD の健全性は毎周期監視します。走行中にどれかの MD が使えなくなったら、両後輪を最大制動して md_fault をラッチします（片側の後輪 MD が止まって反対側だけが駆動を続けると、ヨーモーメントが出るため）。強制制動中は、上位のサイドブレーキ要求を聞きません（位置保持は制動より優先されるので、走行中に入ると最大制動が効かなくなる）。
ステアの中心が未較正の間は、駆動電源を入れません。 絶対角が信用できないためです。

舵の扱い¶

上位の舵角指令は舵角速度の上限でレート制限してから渡します（既定 0.5 rad/s。COMMAND が値を載せればそれを使う。 Pi は 7.0 rad/s を毎回載せています）。急な舵の変化で据え切りの負荷をかけないためです。
armed でない間は舵角を進めず、最後に送った角度を MD に保持させ続けます。 指令フレームを送らないと、ステア MD がトルク上限を適用しないので「MD が揃うまで armed にしない」条件が永久に解けなくなります。
フェイルセーフでも舵は最後の角度のまま保持します（直進に戻すと車体が予期しない方向へ動くため）。

ブレーキホールド¶

制動して |車速| が 0.2 m/s 未満 の状態が 0.3 秒続いたら、後輪を位置制御に切り替えて保持します（サイドブレーキと同じ仕組み）。上位の brake・自動停止・フェイルセーフのどの制動でも同じです。

しきい値を「ほぼ 0」にしない理由

MD の制動は低速で抜けます（0.6 m/s 以下で弱まり、2 rad/s 未満で 0）。坂ではトルクの釣り合う速度でずるずる動き続けるので、しきい値が低いと永久に保持へ移れません。0.2 m/s なら勾配約 12° まで拾えます（0.1 m/s だと約 6°）。保持中にこの車速を超えたら（押された・保持し切れない）制動トルクからやり直します。【未検証】

自動停止¶

【未検証】COMMAND の auto_stop フラグを立てている間、走行方向にある障害物を STM32 自身が検出して制動します。上位の計画がミスしても、速度に応じて伸びる距離で止まる最後の砦です。

\[ d_{\mathrm{stop}} = v \cdot t_d + \frac{v^2}{2a} + m \]

項目	値	出どころ
遅延 \(t_d\)	LiDAR の 1 回転 0.1 s ＋判定の間引き 5 ms ＋ MD のトルクの遅れ	後ろは校正 6 の値を上位が送る
減速度 \(a\)	校正 9 の ABS 込みの制動の減速度	上位が `vehicle.toml` の値を送る（ファームは値を持たない）
余裕 \(m\)	既定 15 cm（0〜100 cm、Pi が `auto_stop_margin_cm` で送る）	`ras_link.h`

LiDAR: 今の実舵角のまま止まるまでに車体が掃く範囲（予想進路）に、直近 300 ms の点が 3 点以上あれば障害物あり。単発のノイズは 1 点しか占めないので、時間方向のデバウンスではなく空間方向の点密度で誤検知を弾きます。 車体の外形の内側の点は数えません（車体に立てた支柱や部品の反射。外形の内側に障害物はありません）。
超音波: LiDAR が新鮮なデータを持たないときのフォールバック（\(d_{\mathrm{stop}}\) 以内）。それとは独立に、5 cm 未満なら常に停止 （LiDAR の最短測距距離や取付高さの死角を補う保険）。超音波はセンサが車体の先端より内側にあるので、しきい値に取付オフセット（前 4 cm・後 3 cm）を足し、「車体の先端から障害物まで」で判定します。

予想進路¶

舵角から自転車モデルの曲率 \(\kappa = \tan\delta / L\)（\(L\) = 0.23 m）を出し、後車軸の中心が半径 \(R = 1/|\kappa|\) の円弧を進むとします。車体（左右 ±15 cm = 半車幅 9 cm ＋余裕 6 cm）は回転の中心 \(C\) のまわりを回る剛体なので、掃く範囲は \(C\) を中心とする円環

\[ R - w \;\le\; |P - C| \;\le\; \left|\left(x_{\mathrm{end}},\; R + w\right)\right| \]

のうち、\(C\) から見た角度が「その半径で最も先にある車体の点」から、先端の中心が \(d_{\mathrm{stop}}\) 進むまでに回る角度 \(\Phi = d_{\mathrm{stop}} / |(x_{\mathrm{front}}, R)|\) の範囲です。

舵を切っていない（\(|\kappa| < 0.02\) /m）ときは矩形（LiDAR から \(d_{\mathrm{stop}}\) ＋ 23 cm、後ろは 14 cm、±15 cm）。真っ直ぐの矩形のままだと、舵を切っているときに曲がった先の障害物を見落とし、曲がって通らない外側の壁で止まります。
指令ではなく実舵角を使います。舵が指令に追いついていない（据え切りの途中・ラックに当たっている）ときに、実際に進む向きを見るためです。
後退でも舵が同じなら回転の中心は同じ側です。後退で前端が外へ振り出す分も円環の外径に入れています。
2 kHz で 179 点を見るので、点ごとに atan2 は呼ばず、cos・sin の加法定理で比べます。判定は 5 ms ごとに間引きます。
舵角どおりに曲がる（横滑りしない）と仮定するので、グリップの限界付近で外へ膨らむ分は余裕 6 cm の内側でしか拾えません。
車体を円弧に沿って細かく動かした総当たりと比べ、当たる点を見落とさないこと、余分に拾うのは車体の真横か縁から 2 cm 以内だけであることを、ホストのテスト（tools/fwsim/tests/test_auto_stop_path.py）で確かめています。

計算例: 2 m/s で直進中

\(d_{\mathrm{stop}} = (2\times0.105 + 2^2/(2\times3.0))\times100 + 15 \approx 103\,\mathrm{cm}\)。 LiDAR の判定距離は \(103+23 \approx 126\,\mathrm{cm}\)。

進行方向は指令ではなく実車速の符号で決めます。「前進中に後退を指令」した瞬間でも実車はまだ前進しており、その一瞬に後方センサへ切り替わって前方の障害物を見失う窓ができるためです。ただしほぼ静止（< 0.05 m/s）のときは指令した方向を使います。静止中は実車速の符号が定まらず常に前方判定に固定され、「前方に障害物・後方は空いているのに後退できない」デッドロックになるからです。

緊急停止（ハートビート断）のフェイルセーフはLiDAR を使いません。

再 ARM の扱い¶

ここはファームと Pi の記述が噛み合っていないので、注意して読んでください。

Pi 側（io_node）には、「E-Stop・駆動電源ラッチ・STM32 再起動・COMMAND 途絶のあとは、ARM を一度外すまで走らせない」再 ARM ラッチの実装があります（_update_rearm_latch）。理由はコードにあります: STM32 は E-Stop を解除した次の周期から指令をそのまま実行するので、ARM 付きの指令を流し続けていると、車のそばでボタン 2 を押した瞬間に走り出すからです。
開発記録（PROGRESS.md の 2026-10-14）には「ファームも同じ規則を持つ（vehicle.c の rearm_required）」とありますが、 確認したファームのコミット（2a2e9a8）の vehicle.c にはその名前がありません。

したがって現時点では「緊急停止の解除で ARM 付きの指令が流れ続けていると走り出す」ことを防ぐのは Pi 側だけだと考えてください。

この設計から学べること¶

上位が落ちたときの動作を、上位の側ではなく下位に持たせる。 通信・ソフト・電源の 3 経路それぞれに独立した検出がある。
止める＝電源を切る、ではない。 制動できる状態を保ったまま止める。
ラッチは「人が確かめて解除する」。 原因が消えたら自動復帰、にしない。
初期化の完了を条件に走行を許可する。 MD の起動待ち、ステア中心の較正。
フェイルセーフ側の動作も「何を保持するか」を決める。 舵は直進に戻さない。