# 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、賽道）都有一致且可預期的手感與安全邊界。