スリップ制御（TC・ABS・片輪浮き対策）¶

【シム／ホスト検証】

対象: drive.c の UpdateSlipLimiter / UpdateWheelLimit / ApplyAbs、drive.h、 control_params.h。確認した版: ファーム 2a2e9a8。 目標スリップ率とゲインは、実機の記録から測ったタイヤの曲線と遅れから式で決める（車両の校正 7）。現行ファームでの実機確認は無い （ファーム CLAUDE.md「机上値」、検証状況）。

3 つの機能が同じ仕組み（SlipLimiter）を共有しています。

機能	掛かる場面	単位	上限の共有
TC（トラクションコントロール）	駆動、後退中の車速 PI の減速、torque_mode	各輪独立	—
片輪浮き対策	同上。前輪に依らない見張り	各輪独立	TC と上限を共有（偏差は小さい方）
ABS	制動モード（`Drive_SetBrake`）と、前進中の車速 PI の減速	左右共通（select-low）	—

1. スリップの測り方¶

各後輪について、「その輪が滑っていなければ出るはずの速度」を基準速度とします。

\[ v_{\mathrm{ref},L} = v_f - \omega_z \frac{t}{2},\qquad v_{\mathrm{ref},R} = v_f + \omega_z \frac{t}{2} \]

\(v_f\): 前輪から推定した車体前後速度（駆動制御のスリップ判定用、\(\tau\approx 5\,\mathrm{ms}\)）
\(\omega_z\): IMU の実測ヨーレート（ジャイロ z）。\(t\): 後輪トレッド 0.155 m

旋回中は内輪と外輪の本来の速度が違います。ここを車体速度で共通化すると外輪が常に「空転」と判定されてTC が誤介入するので、ヨーレートから輪ごとの基準速度を作ります。ヨーレートは舵角からの自転車モデルで代用もできますが、 前輪が滑ると実際のヨーレートから乖離する（まさに TC が要る場面で基準が狂う）ため、IMU の実測値を優先します。

スリップ速度とスリップ率¶

制御が見るのは、トルクを掛けている向きのスリップ速度 \(s\)（[m/s]）です。

\[ s = \begin{cases} v_{\mathrm{wheel}} - v_{\mathrm{ref}} & (\text{駆動：}T>0) \\ v_{\mathrm{ref}} - v_{\mathrm{wheel}} & (\text{減速・制動：}T<0) \\ 0 & (T = 0) \end{cases} \]

駆動で空転していれば正、減速でロックしかけていても正になります（どちらも「タイヤがトルクの向きへ路面より先に進んでいる」）。 前進・後退・加速・減速を 1 つの式で扱えるのが利点です。

スリップ率は \(s / \max(|v_{\mathrm{ref}}|, v_{\mathrm{floor}})\)。分母に下限 \(v_{\mathrm{floor}}\)（slip_speed_floor_m_s、現在 0.5 m/s）を置くのは、車速が 0 に近いと率が発散するためです。低速で TC・ABS ごと無効にすると、発進時の空転を誰も見られなくなるので、下限より遅い間は「スリップ速度が \(v_{\mathrm{floor}}\times\)目標スリップ率を超えたか」を見る形にしています。

2. SlipLimiter — 連続の PI でトルクの上限を決める¶

目標スリップ \(s^*\) と実際の \(s\) の偏差を

\[ e = s^* - s,\qquad s^* = \lambda\,\max(|v_{\mathrm{ref}}|, v_{\mathrm{floor}}) \]

とします（\(\lambda\) は tc_slip_target または abs_slip_target。\(e>0\) が「余裕がある」）。出力はトルクの上限です。

非アクティブのとき:
    e >= 0 → 上限なし（最大値を返す）
    e <  0 → アクティブにする。積分の初期値 = 直前に実際に掛けていたトルクの大きさ
アクティブのとき:
    I     ← clamp(I + ki * e * dt,  min, max)
    上限  ← clamp(I + kp * e,       min, max)
    上限 >= 要求トルク かつ e > 0 → 非アクティブへ戻る（以後、要求どおり掛けても滑らない）

ポイントは 2 つです。

積分の初期値を「そのとき掛けていたトルク」にする。 上限を最大値から削り始めると、掛けているトルクに届くまで何もしないのと同じになるため。
要求以上に上限が戻ったら止める。 常時介入ではなく、滑ったときだけ働きます。

最終的な各輪のトルクは clamp(要求, ±上限) です。

なぜ「減→保持→増」のルールベースにしなかったか¶

実車の油圧 ABS はバルブの開閉しかできないので段を刻みますが、モータはトルクを連続に出せるので、連続の PI にしてあります。段を刻むと最初の一撃で深くロックし、再カットで必要以上に抜く（ロックなしの約 85 %）、とコードのコメントにあります（比較は Pi 側リポジトリ tools/fwsim/bench.py、【シム／ホスト検証】）。

3. TC（各輪独立）¶

UpdateWheelLimit が各輪のトルク上限を決めます。

駆動（車速 PI・torque_mode）と車速 PI の減速の両方に掛かります。後者も対象にするのは、目標車速のランプ（3.0 m/s²）が後輪だけの制動で出せる減速度を超えるため、旋回しながら減速すると後輪がロックしてスピンし得たからです。
駆動→減速の切り替わりで要求トルクと前周期のトルクの向きが違うときは、覚えているのは逆向きのグリップなので持ち越さずリセットします。
片輪を絞っても、その分を反対の輪へ載せません（総駆動力は減る）。総和を保つために反対の輪を押し増すと、要求していないヨーモーメントが出るためです。

4. 片輪浮き対策¶

TC の基準は前輪なので、前輪エンコーダが壊れると TC は働かない（または絞り続ける）。前輪に依らない見張りが別に要ります。

後輪左右の速度差から、ヨーレートで説明できる分を引いた「先走り」を見ます。

\[ \mathrm{lead}_i = (v_{\mathrm{wheel},i} - v_{\mathrm{ref},i}) - (v_{\mathrm{wheel},j} - v_{\mathrm{ref},j}) \quad(\text{トルクの向きに符号を合わせる}) \]

先走りが wheel_lift_diff_threshold_m_s（現在 0.35 m/s）を超えたら、その輪を同じ SlipLimiter で絞ります。偏差は TC の偏差と小さい方を使います。TC とは独立に ON/OFF できます。

さらに最終防壁として、後輪の周速が 7.5 m/s（最高速 5 m/s の 1.5 倍）を超えたら、判定に関係なくその輪のトルクを 0 にします。

5. ABS¶

制動モードの間と、前進中に車速 PI が減速トルクを出している間に働きます。

車速 PI の減速にも掛けるのは、2026-10-31 の会場の試走で、3.9 m/s から車速指令 0（加速度の上限 4.0 m/s²・制動フラグなし）で止めたとき、減速トルクに駆動用の TC だけが掛かり、上限が 20 ms ごとに 0 ↔ 0.15 N·m を往復して後輪が 40〜70 % 滑り、スピンしたためです。ホストのシム（tools/fwsim）では、手動の傾きで止める場面の滑っていた割合が 0.33 → 0.05 になりました。

各輪のロック傾向（\(v_{\mathrm{ref}} - v_{\mathrm{wheel}}\)、進行方向基準）から偏差を作り、左右で小さい方（ロックしかけている側）に合わせて、左右共通の上限にします（select-low）。
左右独立にしないのは、左右で路面の μ が違うときに後輪でヨーモーメントが出てスピン方向に振れるためです。実車の後輪 ABS と同じく安定性を優先しています。
出力は min(要求の制動トルク, ABS の上限) で、最大制動トルクは DRIVE_MAX_BRAKE_TORQUE_NM（= 1 輪の最大トルク 0.15 N·m）。

ABS のフォールバック¶

上限が要求の 20 % 以下に 0.2 秒張り付いたら、ブレーキを離すまで ABS を止めて要求どおりに制動します。

前輪エンコーダの故障や符号ミスで基準車速が高く出ると、スリップが「ロック側」に張り付き、緊急停止中でも制動を抜き続けてしまうためです。本来の ABS はグリップの限界付近（この車は最大制動の約 6 割）に留まるので、2 割以下に長く張り付くことは少ない、という前提に立っています。非常に滑る路面で本当に抜き続ける場面ではロック側に倒れる、と割り切っています。

6. 調整パラメータ¶

CONTROL_PARAM_TABLE が唯一の定義です。現在の値は Pi 側の vehicle.toml の [control] で、ファームの仮置き値は各範囲の下限です。

パラメータ	param_id	範囲	現在値	意味
`slip_speed_floor_m_s`	0x0015	0.1〜2.0	0.5	スリップ率の分母の下限
`tc_slip_target`	0x0011	0.02〜1.0	0.2	TC が保つスリップ率
`tc_kp_nm_per_m_s`	0x0012	0〜1.0	0.201	偏差→トルク上限（比例）
`tc_ki_nm_per_m`	0x0013	0〜50	5.34	同（積分）
`tc_min_torque_nm`	0x0014	0〜0.05	0	上限の下限
`wheel_lift_diff_threshold_m_s`	0x0051	0.05〜3.0	0.35	片輪浮きの先走りしきい値
`abs_slip_target`	0x0071	0.02〜1.0	0.25	ABS が保つスリップ率
`abs_kp_nm_per_m_s`	0x0072	0〜1.0	0.121	ABS 比例
`abs_ki_nm_per_m`	0x0073	0〜50	3.06	ABS 積分

目標スリップ率とゲインの決め方（2026-10-19 に数値最適化から式に置き換えた）

調整ツールが最適化するのはゲイン（kp・ki）だけです。tc_slip_target と abs_slip_target は「前後の力」と「横グリップ」のどちらを取るかという方針で、最適な値が無いからです。人が tradeoff() の表を見て決めます。

目標も一緒に探させると、実機のタイヤは「滑らせても前後力があまり落ちない形（tyre_c ≈ 1.0）」なので、「ABS のゲインを下げてロックさせた方が止まる」解が最適になったそうです（効率 0.91→0.97 と引き換えに、滑っていた割合 0.03→0.58）。後輪だけで制動する車で後輪をロックさせるとスピンしますが、モデルは横滑りを表せないので、その損をコストに入れられません。 — 2026-10-18 までの数値最適化（tools/ctrl_tune/optimize.py）のコメントより。

今は車両の校正 7（tools/calib/tyre.py）が、TC・ABS 自身に目標スリップ率を振って保たせた記録から「スリップ率 → 前後力」の曲線を測り、山の 95% の力が出るスリップ率を目標にします（横グリップを残す方針）。ゲインは、目標の点で線形化したスリップの動き \(\dot u = (r/J)T - a u\)（\(a = r^2 k/(J v)\)、\(k\) は曲線の傾き）と測った遅れから位相余裕 60° の式で決めます。旧最適化の TC のゲインは、この式で位相余裕 58° 相当でした。効きは閉ループの試験（校正 9）で、スリップ率が目標に保たれ振動しないかを実機で確かめます。

7. 設計上の教訓¶

基準速度のフィルタは判定専用に分ける。 PI 用のフィルタを流用すると、加速中に「常時空転」と誤判定する。
フォールバックを設ける。 センサ故障でフィードバックが張り付いたときに、制動を抜き続けない。
削ったトルクを反対輪に載せない。 総和を保つ「親切」は、要求していないヨーモーメントを生む。
ヨーレートは実測を使う。 滑ったとき自転車モデルは当てにならない。