コンテンツにスキップ

スリップ制御(TC・ABS・片輪浮き対策)

要点

  • 空転を抑える TC、ロックを防ぐ ABS、片輪が浮いたときの対策の 3 つが、同じ形の PI 制御でトルクの上限を決めます(TC と片輪浮き対策は TractionLimiter、ABS は SlipLimiter)。
  • 前輪で測った車速と後輪の周速の差から滑りを求め、連続の PI 制御でトルクの上限を決めます。
  • 目標のスリップ率は実機で測ったタイヤの曲線から、ゲインは実機で交差周波数を振って決めます。今のファーム(TC を作り直した版)は実機に書き込まれておらず、ゲインも録り直していません。

【シム/ホスト検証】

対象: drive.c の UpdateWheelLimit / UpdateSlipLimiter / ApplyAbs / ApplySpeedPiAbs、drive.h、 control_params.h。確認した版: ファーム 6b5bdb3、上位 2d0a44e。 目標スリップ率は車両の校正 7(タイヤ)、ゲインは校正 8(実機で交差周波数を振る)で決める。TC を TractionLimiter に作り直したファーム (2026-11-09)は書き込みの記録が無く、校正 8 も録り直していない (ファーム CLAUDE.md「現在の状態」、上位 PROGRESS.md、検証状況)。

3 つの機能は、スリップを目標に保つ同じ形の PI でトルクの上限を決めます。違うのは止め方だけです(2.)。

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

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\): 前輪の走行距離と加速度計のカルマンフィルタによる車速(駆動制御)。遅れがほぼ無いので、加速中もスリップ率に偏りが入らない
  • \(\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. 連続の 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\) が「余裕がある」)。出力はトルクの上限で、TC・ABS とも

滑り始めた周期(e が負になった周期): 積分 I を、実際に掛けていたトルクの大きさまで下げる
毎周期:
    I     ← clamp(I + ki * e * dt,  min, max)
    上限  ← clamp(I + kp * e,       min, max)

です。積分を「そのとき掛けていたトルク」から始めるのは、上限を最大値から削り始めると、掛けているトルクに届くまで何もしないのと同じになるためです。

止め方だけが TC と ABS で違います(drive.h の「制限のしかた」)。

止め方 理由
TC(TractionLimiter) 止めない。 滑らなければ積分が最大値まで戻り、そこが「制限なし」になる ABS と同じ止め方にすると、要求が満額(車速 PI が上限に張り付いている)のとき、滑りが収まった周期に制限なしへ跳ね戻って次の周期にまた滑り、周期的に入り切りする
ABS(SlipLimiter) 上限が要求トルク以上に戻り、かつ \(e>0\)(要求どおり掛けても滑らない)なら止める 上限が低いまま長く留まるとフォールバック(5.)が働く

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

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

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

3. TC(各輪独立)

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

  • 駆動(車速 PI・torque_mode)と、後退中の車速 PI の減速に掛かります。前進中の車速 PI の減速は TC ではなく ABS で絞ります (ApplySpeedPiAbs。その周期は TC の状態を捨てる)。減速に TC を使うと上限が 0 と満額を往復し、後輪が大きく滑ってスピンしたためです(5.)。
  • 積分は滑っていない間に最大値まで戻るので、「介入中かどうか」の状態は持ちません。要求トルクが上限で削られた周期が「TC が働いた」です(TELEMETRY の TC_ACTIVE)。
  • 駆動→減速の切り替わりで要求トルクと前周期のトルクの向きが違うときは、覚えているのは逆向きのグリップなので持ち越さずリセットします。
  • 片輪を絞っても、その分を反対の輪へ載せません(総駆動力は減る)。総和を保つために反対の輪を押し増すと、要求していないヨーモーメントが出るためです。

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)を超えたら、その輪を TC と同じ PI(TractionLimiter)で絞ります。 偏差は TC の偏差と小さい方を使います。TC とは独立に ON/OFF できます。

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

5. ABS

制動モードの間と、前進中(車速 > 0.05 m/s)に車速 PI が減速トルクを出している間に働きます。後退中の車速 PI の減速は対象外で、TC が絞ります。

車速 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)。 車速 PI の減速では、強く減速したい方の輪の要求で ABS を回し、その上限で両輪を絞ります。

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.2229 TC が保つスリップ率
tc_kp_nm_per_m_s 0x0012 0〜1.0 0.14324 偏差→トルク上限(比例)
tc_ki_nm_per_m 0x0013 0〜50 2.1486 同(積分)
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.1216 ABS が保つスリップ率
abs_kp_nm_per_m_s 0x0072 0〜1.0 0.07162 ABS 比例
abs_ki_nm_per_m 0x0073 0〜50 1.0743 ABS 積分

目標スリップ率とゲインの決め方

目標とゲインは別の校正で決めます(上位の docs/calibration.md §4.7・§4.8)。

目標スリップ率(校正 7、tools/calib/tyre.py): TC・ABS を切り、直線でトルクを 0 から最大までゆっくり上げて 「スリップ率 → 前後力」の曲線を測り、力が最大の 95% に届く最小のスリップ率を目標にします(上限 0.3。それより先は横力をほぼ失う)。 TC は各輪が同じ目標を使うので左右の平均、ABS は滑りやすい方に合わせます。

ゲイン(校正 8、tools/calib/tc_gain.py): 式では決めません。スリップ速度はトルクに対してほぼ積分器 \((r/J)/s\) なので、交差周波数 \(\omega_c\) を決めれば \(k_p = \omega_c J / r\)、\(k_i = k_p\,\omega_c/\mathrm{ratio}\) です。 \(\omega_c\) を 15〜240 rad/s の梯子で上げながら全開の発進(TC)と最大の制動(ABS)を実走し、最初に発振した \(\omega_c\) の半分以下 (ゲイン余裕 2 倍)で、スリップ率を目標に保てたもののうち最大を採ります。以前は位相余裕の式で決めていましたが、 実機では高すぎて振動しました(制動側の周速は TC・ABS を切ってもトルクを掛けると揺れ、式の前提の滑らかなプラントが成り立たない)。

効きは校正 9(calib_traction_check)で、スリップ率が目標に保たれ振動しないかを実機で確かめます。 なお、上位の PROGRESS.md は「TC のゲインは旧い数値最適化の値のまま」で、TC を作り直したファームを書き込んでから校正 8 を録り直す、としています。

7. 設計上の教訓

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