RWD TC 控制邏輯、運算方法與安全機制說明

tags: Traction Control Control Logic Safety STM32F446

---

1. 整體控制概念

1.1 控制目標

• 依照路況與駕駛意圖（油門、方向盤）動態調整後輪 slip ratio，維持在一個「有抓地、但允許適度打滑」的區間。
• 直線追求最大加速（較高 slip）。
• 繞 8 / 賽道過彎時，優先穩定性（較低 slip，降低過度 oversteer 風險）。
• 在任何感測器異常或條件不明時，優先安全：關閉或大幅限制 TC 介入。

1.2 單次控制迴圈流程（10 ms, 100 Hz）

邏輯順序：

1. 讀取感測器與指令：四輪輪速、IMU、方向盤、T_driver。
2. 更新錯誤偵測（Fault 檢查）。
3. 若允許 TC：
   • 估測車速 v。
   • 計算後輪 slip ratio（左右與平均）。
   • 濾波方向盤轉角。
   • 根據模式 + 方向盤 + 車速，計算目標 slip λ*。
   • 依據誤差 e = λ* − λ̄，執行 PI 控制，得到 ΔT。
4. 外層安全邏輯覆蓋：低速、嚴重打滑、大轉角、Fault → 改寫 T_cmd。
5. 將最終 T_cmd 與狀態資訊送出給馬達與儀表。

---

2. 運算方法：訊號到 Slip 控制

2.1 車速估測

• 輸入：前輪輪速 (\omega_{FL}, \omega_{FR})。
• 輪胎半徑：(R_{front})。

運算：

[ v = \max\left( \frac{\omega_{FL} + \omega_{FR}}{2} R_{front},\ 0.1 \right) ]

實作重點：

• 只用非驅動輪（前輪）當作車速來源，避免驅動輪打滑時速度估測錯誤。
• 下限 0.1 m/s 避免後面除以 0。

2.2 後輪 Slip Ratio

• 輸入：後輪輪速 (\omega_{RL}, \omega_{RR})、車速 (v)、後輪半徑 (R_{rear})。

[ \lambda_L = \frac{\omega_{RL} R_{rear} - v}{\max(v, 0.1)}, \quad \lambda_R = \frac{\omega_{RR} R_{rear} - v}{\max(v, 0.1)} ]

平均 slip：

[ \bar{\lambda} = \frac{\lambda_L + \lambda_R}{2} ]

物理意義：

• (\lambda = 0)：完全滾動，無打滑。
• (0 < \lambda < 0.3)：可接受的加速打滑區間。
• (\lambda > 0.5)：嚴重打滑，需要強制限扭或收油。

2.3 方向盤轉角濾波

方向盤訊號可能有抖動，使用一階低通：

[ \delta_{f}[k] = \alpha \delta_{f}[k-1] + (1-\alpha) \delta_{raw}[k] ]

• (\alpha = 0.92) 左右，對應約 3 Hz 截止頻率，能濾掉高頻雜訊又不延遲太多。

2.4 簡易路面 μ 估測

使用縱向加速度最大值作為路面摩擦指標：

• 記錄一個指數平滑峰值：

[ a_{x,peak}[k] = \max(0.95, a_{x,peak}[k-1] + 0.05, |a_x[k]|, \ |a_x[k]|) ]

• 依 (a_{x,peak}) 粗略分級：
  • (> 8.5,\text{m/s}^2)：高 μ（乾柏油）。
  • (5.5 \sim 8.5,\text{m/s}^2)：中 μ（略濕或一般柏油）。
  • (3.5 \sim 5.5,\text{m/s}^2)：低 μ（濕滑地面）。
  • (< 3.5,\text{m/s}^2)：很滑（碎石、草、冰）。

對應一個增益倍數 μ_scale：0.5–1.0，用來縮放 Kp。

---

3. 目標 Slip λ* 運算方法

3.1 模式相關基準值

每個模式有一組基準參數：

• 直線：
  • (K_p) 大、(\lambda_{base} \approx 0.20)、方向盤敏感度低。
• 繞 8：
  • (K_p) 中、(\lambda_{base} \approx 0.16)、方向盤敏感度中等。
• 賽道：
  • (K_p) 較小、(\lambda_{base} \approx 0.14)、方向盤敏感度高（轉角一大就明顯降 slip）。

這些參數封裝在 TC_ModeState_t 中：

• Kp, Ki
• lambda_base
• steer_sens
• min_tq_ratio

3.2 方向盤對 λ* 的影響

令 (d = |\delta_f|)（濾波後方向盤轉角，單位 deg）。

使用分段線性 + 模式敏感度來估計剩餘縱向抓地比例 (k_{steer})：

1. 小轉角：(d < 4,\deg)
   • 幾乎直線，(k_{steer} = 1.0)。
2. 輕〜中彎：(4 \le d < 20,\deg)
   • 以線性方式遞減： [ k_{steer} = 1.0 - s_1 (d - 4) ]
   • 斜率 (s_1) 由 steer_sens 決定（模式不同斜率不同）。
3. 中〜重彎：(d \ge 20,\deg)
   • 再以較小斜率往下掉到下限 (k_{steer,min} \approx 0.4)。

3.3 速度補償

高速時可接受稍高目標 slip，低速時保守：

• (v_{kmh} = v \times 3.6)。
• 定義

[ \text{speed_factor} = 0.97 + 0.03 \cdot \min\left(\frac{v_{kmh}}{40}, 1\right) ]

約略在 0.97–1.0 之間微調。

3.4 λ* 綜合計算

綜合模式基準、方向盤與速度：

[ \lambda^* = \max\left( \lambda_{base} \cdot k_{steer} \cdot \text{speed_factor},\ 0.08 \right) ]

• 下限 0.08 避免過度保守導致動力太小。
• 模式越偏「穩定」的（賽道），(\lambda_{base}) 越小，且 steer_sens 越大（大轉角時 λ* 明顯下降）。

---

4. PI 控制與積分管理

4.1 誤差定義

[ e = \lambda^* - \bar{\lambda} ]

• (e > 0)：實際 slip 比目標低 → 可以多給一點扭矩。
• (e < 0)：實際 slip 過大 → 需要限縮扭矩。

4.2 自適應 Kp

根據 μ 估測結果縮放 Kp：

[ K_{p,eff} = K_p \cdot \text{mu_scale} ]

• 高 μ：mu_scale ≈ 1.0 → 使用原始 Kp，反應積極。
• 低 μ：mu_scale ≈ 0.5–0.65 → Kp 變小，避免在低抓地路面上震盪。

4.3 PI 控制律

[ \Delta T = K_{p,eff} e + K_i \int e, dt ]

實作時：

1. 先依當前 Kp_eff 和 Ki_eff 計算 p_term 與 i_term。
2. 積分部分有限制：

[ \int e,dt \in [-I_{max}, I_{max}], \quad I_{max} = 1.0 ]

4.4 積分重置邏輯（Anti-windup）

避免在以下情況發生「積分殘留」：

• 低速起步（v 很小）。
• 駕駛收油（T_driver 幾乎為 0）。

設計：

1. 若 (v < 2,\text{km/h})：
   • 直接令 integral = 0。
2. 若 (|T_{driver}| < 2,\text{Nm})：
   • 積分快速衰減，例如 integral *= 0.1。

這樣可避免：

• 上一輪在打滑時累積的積分影響下一次起步。
• 收油後，TC 仍因舊積分而錯誤限扭。

---

5. 扭矩命令與三層安全機制

5.1 原始扭矩命令

根據 PI 結果先算一個「候選」扭矩：

[ T_{raw} = T_{driver} (1 + \Delta T) ]

再以模式決定的最小扭矩比例做內部飽和：

• (T_{min} = T_{driver} \cdot \text{min_tq_ratio})
• (T_{max} = T_{driver})

[ T_{pi} = \min(\max(T_{raw}, T_{min}), T_{max}) ]

5.2 第一層安全：低速關閉 TC

如果速度很低（例如 v < 3 km/h）：

• 直接讓馬達扭矩 = 駕駛要求：

[ T_{cmd} = T_{driver},\quad \text{TC 狀態 = OFF} ]

理由：

• 起步時 slip 訊號還不穩定，容易誤判。
• 低速起步可先依靠駕駛腳感。

5.3 第二層安全：嚴重打滑保護

若任一後輪 slip 超過安全上限（例如 |λ_L| or |λ_R| > 0.4）：

• 強制大幅限扭，保護輪胎與穩定：

[ T_{cmd} = 0.5 T_{driver}\quad \text{或更低} ]

• TC 狀態標記為 SAFETY。

5.4 第三層安全：大轉角保守模式

若濾波後方向盤轉角非常大（例如 |δ_f| > 30°）：

• 視為重彎或近似 U-turn，必須高度保守：

[ T_{cmd} = T_{driver} \cdot \text{min_tq_ratio} ]

• 對應模式：
  • 直線模式：min_tq_ratio ≈ 0.55。
  • 繞 8 模式：≈ 0.40。
  • 賽道模式：≈ 0.30。

5.5 Fault 狀態下的行為

若 TC_Fault_IsSafeToRun() 回傳 false：

• 代表感測器或扭矩命令有重大異常。
• 建議作法：
  • 將 tc_status 設為 FAULT。
  • 視車輛需求：
    • 直接設 T_cmd = 0（完全不輸出，交回原車 ECU 控制）。
    • 或設成一個安全的固定比例（例如 0.3 * T_driver，當作 limp-home）。
• 同時透過 CAN 發出錯誤碼與故障旗標，讓儀表或 log 紀錄。

---

6. 模式切換邏輯（直線 / 繞 8 / 賽道）

6.1 設計原則

• 只在靜止時切換模式：避免行駛中改變控制器行為，造成車手感覺突變。
• 模式改變時，重新套用一組參數：(K_p, K_i, \lambda_{base}, \text{steer_sens}, \text{min_tq_ratio})。
• 模式切換當下會重置部分內部狀態（例如積分、方向盤濾波器）。

6.2 模式切換流程

1. 車手按 MODE 按鈕，或經由 CAN 設定 target_mode。
2. 若當下車速 (< 1,\text{km/h})：
   • 立即套用新模式參數到 current_mode。
   • 將 integral = 0、steer_deg_f = 0。
3. 若當下車速 (\ge 1,\text{km/h})：
   • 只更新 target_mode，並設 change_pending = true，但不改變實際控制。
   • 等車輛之後停下來時，在控制迴圈中偵測 v 變小，才套用新模式。

6.3 對車手的提示（建議）

• 儀表顯示目前 current_mode，以及若有 change_pending 則以閃爍或圖示表示「等待靜止後切換」。
• 這樣車手可以預先選好下一個模式，到起點或 PIT 停下來時會自動接上新參數。

---

7. 總結：分層思維

設計這套 TC 時，可以用「由內而外」的分層方式來理解：

1. 最內層：PI 控制
   • 只管：誤差 e → ΔT。
   • 內含自適應 Kp（看 μ）與積分 Anti-windup。
2. 中間層：目標 slip 生成
   • 只管：模式 + 方向盤 + 車速 → λ*。
   • 反映駕駛意圖（直線 / 繞 8 / 賽道）與當下彎道狀態。
3. 外層：安全機制
   • 只管：是否在危險狀態（低速、嚴重打滑、大轉角、Fault）。
   • 一旦觸發，會直接覆蓋內層計算結果，強制進入保守或關閉狀態。

依照這個分層結構，你可以在不破壞外層 Safety 的前提下，調整與實驗內層的 Kp/Ki、λ* 表格與 μ 估測演算法，使車輛在不同場景（直線、繞 8、賽道）都有一致且可預期的手感與安全邊界。