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） 
 邏輯順序： 
 
 讀取感測器與指令：四輪輪速、IMU、方向盤、T_driver。 
 更新錯誤偵測（Fault 檢查）。 
 若允許 TC：
 
 估測車速 v。 
 計算後輪 slip ratio（左右與平均）。 
 濾波方向盤轉角。 
 根據模式 + 方向盤 + 車速，計算目標 slip λ*。 
 依據誤差 e = λ* − λ̄，執行 PI 控制，得到 ΔT。 
 
 
 外層安全邏輯覆蓋：低速、嚴重打滑、大轉角、Fault → 改寫 T_cmd。 
 將最終 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})： 
 
 小轉角：(d < 4,\deg)
 
 幾乎直線，(k_{steer} = 1.0)。 
 
 
 輕〜中彎：(4 \le d < 20,\deg)
 
 以線性方式遞減：
[
k_{steer} = 1.0 - s_1 (d - 4)
] 
 斜率 (s_1) 由 steer_sens 決定（模式不同斜率不同）。 
 
 
 中〜重彎：(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
] 
 實作時： 
 
 先依當前
Kp_eff 和 Ki_eff 計算 p_term 與 i_term。 
 積分部分有限制： 
 
 [
\int e,dt \in [-I_{max}, I_{max}], \quad I_{max} = 1.0
] 
 4.4 積分重置邏輯（Anti-windup） 
 避免在以下情況發生「積分殘留」： 
 
 低速起步（v 很小）。 
 駕駛收油（T_driver 幾乎為 0）。 
 
 設計： 
 
 若 (v < 2,\text{km/h})：
 
 直接令 integral = 0。 
 
 
 若 (|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 模式切換流程 
 
 車手按 MODE 按鈕，或經由 CAN 設定 target_mode 。 
 若當下車速 (< 1,\text{km/h})：
 
 立即套用新模式參數到 current_mode 。 
 將 integral = 0、steer_deg_f = 0。 
 
 
 若當下車速 (\ge 1,\text{km/h})：
 
 只更新 target_mode ，並設 change_pending = true ，但不改變實際控制。 
 等車輛之後停下來時，在控制迴圈中偵測 v 變小，才套用新模式。 
 
 
 
 6.3 對車手的提示（建議） 
 
 儀表顯示目前 current_mode ，以及若有 change_pending 則以閃爍或圖示表示「等待靜止後切換」。 
 這樣車手可以預先選好下一個模式，到起點或 PIT 停下來時會自動接上新參數。 
 
 
 7. 總結：分層思維 
 設計這套 TC 時，可以用「由內而外」的分層方式來理解： 
 
 最內層：PI 控制 
 
 只管：誤差 e → ΔT。 
 內含自適應 Kp（看 μ）與積分 Anti-windup。 
 
 
 中間層：目標 slip 生成 
 
 只管：模式 + 方向盤 + 車速 → λ*。 
 反映駕駛意圖（直線 / 繞 8 / 賽道）與當下彎道狀態。 
 
 
 外層：安全機制 
 
 只管：是否在危險狀態（低速、嚴重打滑、大轉角、Fault）。 
 一旦觸發，會直接覆蓋內層計算結果，強制進入保守或關閉狀態。 
 
 
 
 依照這個分層結構，你可以在不破壞外層 Safety 的前提下，調整與實驗內層的 Kp/Ki、λ* 表格與 μ 估測演算法，使車輛在不同場景（直線、繞 8、賽道）都有一致且可預期的手感與安全邊界。