TC 開發流程 tags: Traction Control RWD Race Car IMU CAN Logger 📋 目錄 1. 系統概述 2. 感測訊號與衍生量 3. 控制邏輯詳細推導 4. 自適應 Target Slip 算法 5. 實驗流程與工況規劃 6. 數據檢查表 7. 代碼框架 1. 系統概述 1.1 控制對象與目標 控制對象 ：馬達扭矩（anti-spin，無煞車） 傳感器 ：四輪輪速 + IMU（6 軸）+ 馬達轉速/扭矩 控制目標 ： 讓後驅輪 slip ratio 收斂在 0.10～0.20 （直線）/ 0.08～0.12 （彎道） 減少低 μ 路面打滑，提升 0–60 km/h 加速性能 增加出彎穩定性（防過度 yaw） 1.2 核心優勢 賽車應用 ：降低加速時間 20～30%（低 μ） IMU 自適應 ：彎道自動降低 slip 目標，平衡速度與穩定 即時反饋 ：10 Hz 控制頻率，<100 ms 響應延遲 2. 感測訊號與衍生量 2.1 原始訊號（CAN 直接讀取） 訊號 符號 來源 單位 採樣率 前左輪速 (\omega_{FL}) CAN/WSM rad/s 100 Hz 前右輪速 (\omega_{FR}) CAN/WSM rad/s 100 Hz 後左輪速 (\omega_{RL}) CAN/WSM rad/s 100 Hz 後右輪速 (\omega_{RR}) CAN/WSM rad/s 100 Hz 縱向加速度 (a_x) IMU m/s² 100 Hz 橫向加速度 (a_y) IMU m/s² 100 Hz 偏航角速度 (\dot{\psi}) IMU rad/s 100 Hz 馬達轉速 (\omega_m) CAN/inverter rpm 100 Hz 馬達命令扭矩 (T_{driver}) CAN/VCU N·m 333Hz 馬達實際扭矩 (T_{actual}) CAN/inverter N·m 100 Hz 2.2 衍生量計算（核心演算法） 2.2.1 車速估測 (v) （Low-pass + IMU Fusion） 前輪平均速度（原始）: v_wheel = (ω_FL + ω_FR) / 2 × R_front IMU 縱向積分（漂移補正）: v_imu = ∫ a_x dt (with high-pass filter) 融合（Complementary Filter）: v = α × v_wheel + (1 - α) × v_imu 其中 α = 0.8（高速時多信任前輪，低速多信任 IMU） 防止低速除零: v_safe = max(v, 0.1 m/s) Python 實作 ： def estimate_vehicle_speed(omega_fl, omega_fr, a_x, R_front=0.32, alpha=0.8): """ 輸入: omega_fl, omega_fr (rad/s), a_x (m/s²) 輸出: v_safe (m/s) """ v_wheel = (omega_fl + omega_fr) / 2 * R_front # IMU 積分部分（假設已有高通濾波） v_imu = alpha_imu * v_imu_prev + (1 - alpha_imu) * a_x * dt v = alpha * v_wheel + (1 - alpha) * v_imu return max(v, 0.1) 2.2.2 後輪 Slip Ratio (\lambda_L, \lambda_R) 定義（ISO 6954 標準）: λ_L = (ω_RL × R_rear - v) / max(v, 0.1) λ_R = (ω_RR × R_rear - v) / max(v, 0.1) 平均 Slip: λ̄ = (λ_L + λ_R) / 2 物理意義: λ = 0 : 完全滾動，無打滑 0 < λ < 0.3 : 加速打滑（TC 目標區間） λ > 0.5 : 嚴重打滑（失控） λ_L ≠ λ_R : 單輪打滑（可診斷直線穩定性） Python 實作 ： def compute_slip_ratio(omega_rl, omega_rr, v_safe, R_rear=0.32): """計算後輪 slip ratio""" slip_L = (omega_rl * R_rear - v_safe) / max(v_safe, 0.1) slip_R = (omega_rr * R_rear - v_safe) / max(v_safe, 0.1) slip_avg = (slip_L + slip_R) / 2 return slip_L, slip_R, slip_avg 2.2.3 彎道檢測（IMU Yaw Rate） 直線 / 彎道判定閾值（可調，初始值推薦）: θ_straight = 5°/s (直線) θ_light = 15°/s (輕彎) θ_heavy = 25°/s (重彎) 實現: if |ψ̇| < θ_straight: state = "STRAIGHT" elif θ_straight ≤ |ψ̇| < θ_light: state = "LIGHT_TURN" elif θ_light ≤ |ψ̇| < θ_heavy: state = "HEAVY_TURN" else: state = "SPIN_RISK" Python 實作 ： def detect_turn_state(psi_dot, thresholds=(5, 15, 25)): """ 輸入: psi_dot (rad/s, IMU yaw rate) 輸出: state (str) """ psi_dot_deg = abs(psi_dot) * 180 / 3.14159 if psi_dot_deg < thresholds[0]: return "STRAIGHT" elif psi_dot_deg < thresholds[1]: return "LIGHT_TURN" elif psi_dot_deg < thresholds[2]: return "HEAVY_TURN" else: return "SPIN_RISK" 3. 控制邏輯詳細推導 3.1 TC 控制架構圖 [油門輸入 T_driver] ↓ [後輪 Slip 計算] → λ̄ = (λ_L + λ_R) / 2 ↓ [IMU Yaw Rate] → |ψ̇| → 選定 Target λ* ↓ [Slip Error] → e = λ* - λ̄ ↓ [PI Controller] → ΔT = K_p × e + K_i × ∫e dt ↓ [扭矩限縮] → T_cmd = T_driver × (1 + ΔT) ↓ [馬達執行 & 保護邏輯] ↓ [實際扭矩送出] 3.2 PI 控制器推導 3.2.1 Slip Error 定義 e(t) = λ*(t) - λ̄(t) e > 0 : slip 低於目標 → 要放寬扭矩（加油） e < 0 : slip 高於目標 → 要限縮扭矩（減油） 3.2.2 比例項 (Proportional) ΔT_p = K_p × e 物理意義: - K_p 越大，反應越快（但容易超調） - K_p 越小，反應越慢（但更穩定） 推薦初值（賽車用）: 直線: K_p = 3.0 ~ 5.0 (aggressive，想要快速加速) 彎道: K_p = 2.0 ~ 3.0 (保守，防 spin) 低μ: K_p = 2.0 ~ 2.5 (中等) 物理直覺 ：如果打滑很嚴重（e = -0.3），用 K_p=5，會一次砍 150% 的扭矩（實際限縮到 0.5 倍），很快就能回到目標。 3.2.3 積分項 (Integral) ΔT_i = K_i × ∫e dt 物理意義: - 消除穩態誤差（e 小但持久時逐漸累積） - 低速起步用（此時 P 反應慢） 推薦初值: K_i = 0.3 ~ 1.0 積分限制（Anti-windup）: ∫e dt 最大值 = 1.0 → 防止 I 項無限累積 3.2.4 完整 PI 控制律 ΔT(t) = K_p × e(t) + K_i × ∫₀ᵗ e(τ) dτ T_cmd = T_driver × (1 + ΔT) ; 原始扭矩限縮 T_cmd = max(0.3 × T_driver, min(T_driver, T_cmd)) ; 限縮範圍 [30%~100%] Python 實作 ： class PIController: def __init__(self, Kp=3.0, Ki=0.5, dt=0.01, anti_windup_max=1.0): self.Kp = Kp self.Ki = Ki self.dt = dt self.integral = 0.0 self.anti_windup_max = anti_windup_max def update(self, error): """計算 PI 輸出""" # P 項 p_term = self.Kp * error # I 項（含 anti-windup） self.integral += error * self.dt self.integral = max(-self.anti_windup_max, min(self.anti_windup_max, self.integral)) i_term = self.Ki * self.integral # 總輸出 delta_T = p_term + i_term return delta_T def reset(self): """低速時重置積分""" self.integral = 0.0 3.3 扭矩命令生成與保護邏輯 Step 1: 計算 Slip Error e = λ* - λ̄ Step 2: PI 控制器 ΔT = Kp × e + Ki × ∫e dt Step 3: 扭矩限縮計算 T_cmd_raw = T_driver × (1 + ΔT) Step 4: 飽和限縮（Saturation） T_cmd_saturated = clamp(T_cmd_raw, 0.3 × T_driver, 1.0 × T_driver) Step 5: 安全邏輯（Safety Logic） if v < 3 km/h: // 低速起步 T_cmd = T_driver // TC OFF，用前輪參考加速 elif λ_L > 0.4 or λ_R > 0.4: // 單輪過度打滑 T_cmd = 0.5 × T_driver // 強制砍 50% elif state == "SPIN_RISK": // IMU 偵測過度 yaw T_cmd = 0.3 × T_driver // 進入失控防護 else: T_cmd = T_cmd_saturated // 正常 PI 控制 Python 實作 ： def compute_torque_command(T_driver, slip_avg, slip_L, slip_R, v, psi_dot, pi_controller, turn_state, thresholds=(5, 15, 25)): """完整扭矩命令生成""" # Safety: 低速關閉 TC if v < 3 / 3.6: # 3 km/h return T_driver, "LOW_SPEED" # Safety: 單輪過度打滑 if abs(slip_L) > 0.4 or abs(slip_R) > 0.4: return 0.5 * T_driver, "SINGLE_WHEEL_SLIP" # Safety: 過度 yaw（失控風險） psi_dot_deg = abs(psi_dot) * 180 / 3.14159 if psi_dot_deg > thresholds[2]: return 0.3 * T_driver, "SPIN_RISK" # 正常 TC 控制 delta_T = pi_controller.update(error=slip_avg) T_cmd_raw = T_driver * (1 + delta_T) T_cmd = max(0.3 * T_driver, min(T_driver, T_cmd_raw)) return T_cmd, turn_state 4. 自適應 Target Slip 算法 4.1 Target Slip 自適應表 根據 IMU yaw rate 和檔位即時調整 slip 目標： ┌─────────────────────────────────────────────────────────┐ │ IMU Yaw Rate 與 Target Slip 自適應表 │ ├──────────────────┬──────────────┬──────────┬─────────────┤ │ 駕駛狀態 │ yaw rate範圍 │ λ* │ 控制策略 │ ├──────────────────┼──────────────┼──────────┼─────────────┤ │ 直線全油門加速 │ |ψ̇| < 5°/s │ 0.18~0.20│ 最大縱向力 │ │ │ │ (Gear 1) │ 0-60 最快 │ ├──────────────────┼──────────────┼──────────┼─────────────┤ │ 輕微轉向加速 │ 5°/s ≤|ψ̇| │ 0.14~0.16│ 平衡加速 │ │ │ < 15°/s │ (Gear 2) │ 與側向穩定 │ ├──────────────────┼──────────────┼──────────┼─────────────┤ │ 中等轉向 (彎道) │15°/s ≤|ψ̇| │ 0.10~0.12│ 優先穩定， │ │ │ < 25°/s │ (Gear 3) │ 防 oversteer │ ├──────────────────┼──────────────┼──────────┼─────────────┤ │ 急彎 / 失控預防 │ |ψ̇| ≥ 25°/s │ 0.08~0.10│ 全力穩定， │ │ │ │ (Gear 4) │ 強制限縮 │ └──────────────────┴──────────────┴──────────┴─────────────┘ 4.2 自適應演算法實作 def compute_target_slip(psi_dot, v): """ 根據 yaw rate 和速度自適應計算 target slip 輸入: psi_dot: IMU 偏航角速度 (rad/s) v: 車速 (m/s) 輸出: lambda_target: 目標 slip ratio gear: 檔位標籤 (用於調試) """ psi_dot_deg = abs(psi_dot) * 180 / 3.14159 # 檔位切換 & Target Slip if psi_dot_deg < 5: lambda_target = 0.18 + 0.02 * (v / 20) # 速度越高，target 稍高 gear = "GEAR_1_MAX_ACCEL" elif psi_dot_deg < 15: # 線性插值: 5°/s 時 0.15，15°/s 時 0.12 lambda_target = 0.15 - 0.03 * (psi_dot_deg - 5) / 10 gear = "GEAR_2_LIGHT_TURN" elif psi_dot_deg < 25: lambda_target = 0.10 + 0.02 * (25 - psi_dot_deg) / 10 gear = "GEAR_3_HEAVY_TURN" else: # Spin risk lambda_target = 0.08 gear = "GEAR_4_SPIN_RISK" return lambda_target, gear 4.3 低 μ 路面自適應補正 可選擴展：如果有簡易的摩擦力估測（例如從加速度飽和判斷），進一步調整： def estimate_mu_level(a_x_max_recent, slip_avg_recent): """ 簡化的摩擦力等級估測 邏輯： - 高 μ (乾柏油): a_x_max > 8 m/s², slip < 0.3 - 中 μ (濕地): a_x_max ≈ 4~6 m/s², slip 0.3~0.4 - 低 μ (冰/雪): a_x_max < 3 m/s², slip > 0.4 """ if a_x_max_recent > 8: return "HIGH_MU" elif a_x_max_recent > 5: return "MEDIUM_MU" else: return "LOW_MU" def adjust_target_slip_for_mu(lambda_target, mu_level): """在摩擦力低時，進一步降低 target slip（保守）""" if mu_level == "LOW_MU": return lambda_target * 0.85 # 降低 15% elif mu_level == "MEDIUM_MU": return lambda_target * 0.92 # 降低 8% else: return lambda_target 5. 實驗流程與工況規劃 5.1 實驗場地與地面條件 地面類型 路面特徵 μ 等級 場地要求 高 μ 乾燥柏油、新鋪 μ ≈ 0.85~1.0 平坦，無雨 中 μ 濕柏油、一般水泥 μ ≈ 0.6~0.75 濕潤但不積水 低 μ 碎石、濕草地、冰 μ ≈ 0.3~0.5 或人為灑水 5.2 單次測試 SOP（Standard Operating Procedure） 5.2.1 測試前準備（15 分鐘） □ 輪胎檢查 □ 胎壓一致（左右前後差異 < 0.1 bar） □ 胎溫 20~30°C（冷胎，確保可重複性） □ 胎面無異物 □ CAN Logger 配置 □ 記錄頻率設定：100 Hz (wheel speed, IMU) □ CAN ID 表確認無誤 □ 存儲空間檢查（預期每次 5~10 MB） □ 馬達 / 油門校準 □ 油門全開 -> T_driver max 確認 □ 油門全關 -> T_driver = 0 確認 □ 天氣記錄 □ 溫度、溼度、風速 □ 路面溫度（紅外溫度計） 5.2.2 Baseline 測試 - TC OFF（3 趟） 場景: 直線加速，全油門 0~60 km/h 每趟流程: 1. 車停在起點，冷靜等待 (IMU 零點漂移穩定) 2. 油門緩慢踩到 50% (預熱) 3. 在 v = 0 瞬間，全油門 (t=0 記點) 4. 保持全油門直到 60 km/h，然後收油 5. 停車 5 分鐘，冷卻胎溫 數據記錄: ├─ 起點位置 (GPS) ├─ 環境溫度、胎溫 ├─ CAN 原始數據 (導出為 CSV) └─ 駕駛備註 (路面狀況、車感描述) 觀察重點: ├─ 最大 slip ratio: λ_max ? ├─ 0~60 km/h 時間: t_60 ? ├─ 打滑持續時間 (λ > 0.3): t_slip ? └─ 前後輪 slip 差異: |λ_L - λ_R| ? 5.2.3 TC ON - 直線測試（3 趟） 場景: 同上，但啟動 TC（Target λ* = 0.18） 參數設定: ├─ Kp = 3.5 (aggressive) ├─ Ki = 0.5 └─ 彎道檢測閾值: θ_straight = 5°/s 觀察重點 (對比 Baseline): ├─ 最大 slip ratio 是否 < 0.25? ├─ 0~60 km/h 時間是否縮短 (預期 -15~25%)? ├─ 縱向加速度是否更平順 (std 是否降低)? └─ 打滑時間是否 < 1 s? 數據分析: - 畫圖: slip(t) 對比 (OFF 紅、ON 藍) - 計算: 平均縱向 g (m/s²) 5.2.4 TC ON - 彎道測試（3 趟，如有彎道） 場景: 轉向加速 (方向盤 ≈ 30°) + 全油門，通過彎道 參數設定: ├─ Kp = 2.5 (moderate) ├─ Ki = 0.5 └─ 自適應 target slip (根據 |ψ̇|) 觀察重點: ├─ 出彎 yaw rate 峰值: max(|ψ̇|)? ├─ Oversteer 徵兆 (yaw rate 尖峰) ├─ 加速度 (a_x, a_y) 是否均衡 └─ slip 是否穩定在 0.10~0.12? 5.2.5 TC ON - 低 μ 起步測試（5 趟） 場景: 低 μ 地面 (濕碎石)，全油門起步 關鍵觀察: ├─ 起步時打滑時間 (λ > 0.3 持續多久)? ├─ 穩定後的 slip 值是否在目標範圍? ├─ 連續 5 次起步中胎溫上升對 slip 的影響 └─ 預期: 打滑時間逐次縮短 (胎溫升高 μ 增加) 5.3 工況組合表（建議先做順序） 優先級 地面 TC 狀態 彎度 趟數 預期時間 備註 1 高 μ OFF 直線 3 10 min Baseline 2 高 μ ON 直線 3 10 min 主要對比 3 中 μ OFF 直線 3 10 min 4 中 μ ON 直線 3 10 min 5 低 μ OFF 直線 3 10 min 最難 6 低 μ ON 直線 5 15 min 重點 7 高 μ ON 彎 30° 3 15 min 穩定性 8 低 μ ON 彎 30° 3 15 min 進階 完 - - - - 100 min ≈ 半天 6. 數據檢查表 6.1 實時檢查清單（場邊用） 在每趟實驗後，用此表快速檢查數據品質： ### 6.1.1 CAN 數據完整性 測試編號: ______ 日期: ______ 地面條件: ______ □ 訊號檢查 □ 前左輪速：< 2 s 無跳動？ Y / N □ 前右輪速：< 2 s 無跳動？ Y / N □ 後左輪速：< 2 s 無跳動？ Y / N □ 後右輪速：< 2 s 無跳動？ Y / N □ IMU 加速度：在合理範圍 (< 2g, < 1g)? Y / N □ IMU 角速度：max(|ψ̇|) < 100°/s? Y / N □ 馬達扭矩：在 [0, T_max] 範圍? Y / N □ 時間同步 □ 所有訊號長度一致（同一時間範圍）? Y / N □ 無掉幀或時間戳異常？ Y / N □ 統計檢查 □ 車速：0 ~ 60 km/h 正常遞增? Y / N □ Slip ratio：0 ~ 0.5 正常變化? Y / N □ 加速度：0~1 g，無 spike > 3g? Y / N 6.2 離線分析檢查單（Python 用） 6.2.1 基礎統計量 def verify_data_quality(df_log): """ 輸入: DataFrame with 'time', 'v', 'slip_L', 'slip_R', 'a_x', 'psi_dot', 'T_cmd' 輸出: 品質報告 """ print("=" * 60) print("DATA QUALITY REPORT") print("=" * 60) # 訊號範圍檢查 checks = { "v (m/s)": (df_log['v'].min(), df_log['v'].max(), "0 ~ 25"), "slip_L": (df_log['slip_L'].min(), df_log['slip_L'].max(), "-0.2 ~ 0.6"), "slip_R": (df_log['slip_R'].min(), df_log['slip_R'].max(), "-0.2 ~ 0.6"), "a_x (m/s²)": (df_log['a_x'].min(), df_log['a_x'].max(), "-1 ~ 2"), "psi_dot (°/s)": (df_log['psi_dot'].min() * 180/3.14, df_log['psi_dot'].max() * 180/3.14, "-50 ~ 50"), "T_cmd (N·m)": (df_log['T_cmd'].min(), df_log['T_cmd'].max(), "0 ~ T_max"), } for signal, (v_min, v_max, range_str) in checks.items(): print(f"{signal:20} | Min: {v_min:8.3f} | Max: {v_max:8.3f} | Range: {range_str}") # 異常檢測 print("\n" + "=" * 60) print("ANOMALY DETECTION") print("=" * 60) # 檢查加速度 spike a_x_spike = (df_log['a_x'].diff().abs() > 0.5).sum() # 0.5 m/s² 變化 print(f"Acceleration spikes (Δa > 0.5 m/s²): {a_x_spike} frames") # 檢查 slip 不連續 slip_jump = ((df_log['slip_L'].diff().abs() > 0.1).sum() + (df_log['slip_R'].diff().abs() > 0.1).sum()) print(f"Slip discontinuities (Δλ > 0.1): {slip_jump} frames") # 檢查時間戳 dt = df_log['time'].diff().dropna() dt_expected = 0.01 # 100 Hz dt_anomaly = (dt[dt > 1.5 * dt_expected]).count() print(f"Timestamp gaps (Δt > 15ms): {dt_anomaly} frames") 6.2.2 性能指標計算 def compute_performance_metrics(df_log, tc_status="ON"): """計算實驗的性能指標""" print(f"\n{'='*60}") print(f"PERFORMANCE METRICS (TC {tc_status})") print(f"{'='*60}\n") # 1. 加速性能 v_0_to_60_mask = (df_log['v'] >= 0) & (df_log['v'] <= 60/3.6) if v_0_to_60_mask.sum() > 0: t_0_60 = df_log[v_0_to_60_mask]['time'].max() - df_log[v_0_to_60_mask]['time'].min() print(f"[1] Acceleration 0~60 km/h: {t_0_60:.2f} s") # 2. 打滑統計 slip_avg = (df_log['slip_L'] + df_log['slip_R']) / 2 slip_high_mask = slip_avg > 0.25 t_slip_high = (slip_high_mask.sum()) * 0.01 # 100 Hz print(f"[2] High slip time (λ > 0.25): {t_slip_high:.2f} s") print(f" Max slip: {slip_avg.max():.3f}") print(f" Mean slip (steady): {slip_avg[v_0_to_60_mask].mean():.3f}") # 3. 縱向加速度 a_x_mean = df_log['a_x'].mean() a_x_std = df_log['a_x'].std() print(f"[3] Longitudinal acceleration: {a_x_mean:.2f} ± {a_x_std:.2f} m/s²") # 4. 側向穩定性（如有彎道） if df_log['psi_dot'].abs().max() > 5 * 3.14159 / 180: # > 5°/s psi_dot_peak = df_log['psi_dot'].abs().max() * 180 / 3.14159 print(f"[4] Peak yaw rate: {psi_dot_peak:.1f} °/s") # 5. 扭矩歷史 print(f"[5] Torque limiting:") print(f" Max commanded: {df_log['T_cmd'].max():.0f} N·m") print(f" Avg limited: {(df_log['T_cmd'] < df_log['T_driver'] * 0.95).mean() * 100:.1f}%") return { 't_0_60': t_0_60 if v_0_to_60_mask.sum() > 0 else None, 'slip_max': slip_avg.max(), 'slip_mean': slip_avg.mean(), 'a_x_mean': a_x_mean, 'a_x_std': a_x_std, } 6.2.3 對比分析表 def compare_tc_on_off(df_log_off, df_log_on): """對比 TC OFF vs ON 的性能""" print("\n" + "="*80) print("COMPARISON: TC OFF vs ON") print("="*80 + "\n") m_off = compute_performance_metrics(df_log_off, "OFF") m_on = compute_performance_metrics(df_log_on, "ON") # 製表 import pandas as pd comparison = pd.DataFrame({ 'Metric': [ '0~60 km/h Time (s)', 'Max Slip Ratio', 'Mean Slip Ratio', 'Longitudinal g (mean)', 'Acceleration Smoothness (std)', ], 'TC OFF': [ f"{m_off['t_0_60']:.2f}" if m_off['t_0_60'] else "—", f"{m_off['slip_max']:.3f}", f"{m_off['slip_mean']:.3f}", f"{m_off['a_x_mean']:.2f}", f"{m_off['a_x_std']:.2f}", ], 'TC ON': [ f"{m_on['t_0_60']:.2f}" if m_on['t_0_60'] else "—", f"{m_on['slip_max']:.3f}", f"{m_on['slip_mean']:.3f}", f"{m_on['a_x_mean']:.2f}", f"{m_on['a_x_std']:.2f}", ], }) # 計算改善百分比 if m_off['t_0_60'] and m_on['t_0_60']: t_improve = (m_off['t_0_60'] - m_on['t_0_60']) / m_off['t_0_60'] * 100 comparison.loc[0, 'Improvement %'] = f"{t_improve:+.1f}%" slip_improve = (m_off['slip_max'] - m_on['slip_max']) / m_off['slip_max'] * 100 comparison.loc[1, 'Improvement %'] = f"{slip_improve:+.1f}%" print(comparison.to_string(index=False)) print("\n") 6.3 數據導出與可視化 Script import pandas as pd import matplotlib.pyplot as plt def plot_tc_analysis(df_log, save_path="tc_analysis.png"): """生成四子圖分析視圖""" fig, axes = plt.subplots(2, 2, figsize=(14, 8)) # Subplot 1: 車速 & Slip ax1 = axes[0, 0] ax1.plot(df_log['time'], df_log['v'] * 3.6, 'b-', label='Vehicle Speed', linewidth=2) ax1_slip = ax1.twinx() ax1_slip.plot(df_log['time'], (df_log['slip_L'] + df_log['slip_R'])/2, 'r--', label='Avg Slip Ratio', linewidth=2) ax1.set_xlabel('Time (s)') ax1.set_ylabel('Speed (km/h)', color='b') ax1_slip.set_ylabel('Slip Ratio', color='r') ax1.grid(True, alpha=0.3) ax1.set_title('Vehicle Speed & Slip Ratio vs Time') # Subplot 2: 左右輪 Slip 對比 ax2 = axes[0, 1] ax2.plot(df_log['time'], df_log['slip_L'], 'g-', label='Left Wheel', linewidth=2) ax2.plot(df_log['time'], df_log['slip_R'], 'orange', label='Right Wheel', linewidth=2) ax2.axhline(y=0.1, color='r', linestyle='--', alpha=0.5, label='Target (0.1)') ax2.axhline(y=0.2, color='r', linestyle='--', alpha=0.5, label='Target (0.2)') ax2.set_xlabel('Time (s)') ax2.set_ylabel('Slip Ratio') ax2.legend() ax2.grid(True, alpha=0.3) ax2.set_title('Left vs Right Wheel Slip') # Subplot 3: 縱向加速度 ax3 = axes[1, 0] ax3.plot(df_log['time'], df_log['a_x'], 'purple', linewidth=2) ax3.set_xlabel('Time (s)') ax3.set_ylabel('Acceleration (m/s²)') ax3.grid(True, alpha=0.3) ax3.set_title(f'Longitudinal Acceleration (Mean: {df_log["a_x"].mean():.2f}, Std: {df_log["a_x"].std():.2f})') # Subplot 4: 扭矩命令 ax4 = axes[1, 1] ax4.plot(df_log['time'], df_log['T_driver'], 'b-', label='T_driver', linewidth=2) ax4.plot(df_log['time'], df_log['T_cmd'], 'r-', label='T_cmd (limited)', linewidth=2) ax4.set_xlabel('Time (s)') ax4.set_ylabel('Torque (N·m)') ax4.legend() ax4.grid(True, alpha=0.3) ax4.set_title('Motor Torque: Driver Input vs TC Command') plt.tight_layout() plt.savefig(save_path, dpi=150) print(f"Figure saved to {save_path}") return fig 7. 程式碼框架 7.1 上位機 Python 主程序框架 """ TC 開發主程序框架 (Python 3.8+) 用途: 離線分析 CAN 數據，測試控制邏輯 """ import pandas as pd import numpy as np from pathlib import Path import matplotlib.pyplot as plt class TCController: def __init__(self, Kp=3.0, Ki=0.5, R_rear=0.32, R_front=0.32): self.Kp = Kp self.Ki = Ki self.R_rear = R_rear self.R_front = R_front self.integral = 0.0 self.dt = 0.01 # 100 Hz def estimate_vehicle_speed(self, omega_fl, omega_fr, a_x, alpha=0.8): """車速估測 (Complementary Filter)""" v_wheel = (omega_fl + omega_fr) / 2 * self.R_front return max(v_wheel, 0.1) def compute_slip_ratio(self, omega_rl, omega_rr, v_safe): """計算後輪 slip ratio""" slip_L = (omega_rl * self.R_rear - v_safe) / max(v_safe, 0.1) slip_R = (omega_rr * self.R_rear - v_safe) / max(v_safe, 0.1) return slip_L, slip_R, (slip_L + slip_R) / 2 def compute_target_slip(self, psi_dot): """自適應 target slip""" psi_dot_deg = abs(psi_dot) * 180 / 3.14159 if psi_dot_deg < 5: return 0.18 elif psi_dot_deg < 15: return 0.15 - 0.03 * (psi_dot_deg - 5) / 10 else: return 0.10 def pi_control(self, slip_error): """PI 控制器""" p_term = self.Kp * slip_error self.integral = max(-1.0, min(1.0, self.integral + slip_error * self.dt)) i_term = self.Ki * self.integral return p_term + i_term def compute_torque_command(self, T_driver, slip_avg, slip_L, slip_R, v, psi_dot): """完整扭矩命令生成""" # 低速關閉 TC if v < 3 / 3.6: return T_driver, "LOW_SPEED" # 單輪過度打滑 if abs(slip_L) > 0.4 or abs(slip_R) > 0.4: return 0.5 * T_driver, "SINGLE_WHEEL_SLIP" # 過度 yaw psi_dot_deg = abs(psi_dot) * 180 / 3.14159 if psi_dot_deg > 25: return 0.3 * T_driver, "SPIN_RISK" # 正常 TC lambda_target = self.compute_target_slip(psi_dot) error = lambda_target - slip_avg delta_T = self.pi_control(error) T_cmd = T_driver * (1 + delta_T) T_cmd = max(0.3 * T_driver, min(T_driver, T_cmd)) return T_cmd, f"NORMAL (λ*={lambda_target:.2f})" def main(): # 1. 讀取 CAN 數據 log_file = Path("tc_test_20260313.csv") # 修改為你的文件 df_raw = pd.read_csv(log_file) # 2. 初始化控制器 tc = TCController(Kp=3.5, Ki=0.5) # 3. 處理數據 df_processed = df_raw.copy() df_processed['v'] = df_raw.apply( lambda row: tc.estimate_vehicle_speed(row['omega_fl'], row['omega_fr'], row['a_x']), axis=1 ) df_processed[['slip_L', 'slip_R', 'slip_avg']] = df_raw.apply( lambda row: pd.Series(tc.compute_slip_ratio(row['omega_rl'], row['omega_rr'], df_processed.loc[row.name, 'v'])), axis=1 ) # 4. 計算 TC 命令 df_processed[['T_cmd', 'tc_status']] = df_raw.apply( lambda row: pd.Series(tc.compute_torque_command( row['T_driver'], df_processed.loc[row.name, 'slip_avg'], df_processed.loc[row.name, 'slip_L'], df_processed.loc[row.name, 'slip_R'], df_processed.loc[row.name, 'v'], row['psi_dot'] )), axis=1 ) # 5. 性能分析 print("Performance Analysis Complete!") # 6. 生成報告 plot_tc_analysis(df_processed, save_path="tc_analysis_result.png") df_processed.to_csv("tc_processed_data.csv", index=False) if __name__ == "__main__": main() 7.2 STM32 CubeIDE C 程式碼框架 /* * TC_Controller.h * STM32 CAN 實時 TC 控制模組 */ #ifndef TC_CONTROLLER_H #define TC_CONTROLLER_H #include "stdint.h" #include "stdbool.h" // 結構體定義 typedef struct { float omega_fl, omega_fr, omega_rl, omega_rr; // rad/s float a_x, a_y, psi_dot; // IMU float T_driver, T_cmd; // N·m } TCInput_t; typedef struct { float v; // m/s float slip_L, slip_R, slip_avg; float lambda_target; float T_cmd_final; uint8_t tc_status; // 0=OFF, 1=NORMAL, 2=SAFETY } TCOutput_t; typedef struct { float Kp, Ki; float integral; float R_rear, R_front; float dt; } TCController_t; // 公開函數 void TC_Init(TCController_t *ctrl, float Kp, float Ki); void TC_Update(TCController_t *ctrl, TCInput_t *input, TCOutput_t *output); #endif /** * TC_Controller.c - 實作 */ void TC_Init(TCController_t *ctrl, float Kp, float Ki) { ctrl->Kp = Kp; ctrl->Ki = Ki; ctrl->integral = 0.0f; ctrl->R_rear = 0.32f; ctrl->R_front = 0.32f; ctrl->dt = 0.01f; // 100 Hz } static float estimate_vehicle_speed(float omega_fl, float omega_fr, float R_front) { float v = ((omega_fl + omega_fr) / 2.0f) * R_front; return (v > 0.1f) ? v : 0.1f; } static void compute_slip_ratio(float omega_rl, float omega_rr, float v, float R_rear, float *slip_L, float *slip_R, float *slip_avg) { float v_safe = (v > 0.1f) ? v : 0.1f; *slip_L = ((omega_rl * R_rear) - v_safe) / v_safe; *slip_R = ((omega_rr * R_rear) - v_safe) / v_safe; *slip_avg = (*slip_L + *slip_R) / 2.0f; } static float compute_target_slip(float psi_dot_deg) { if (psi_dot_deg < 5.0f) { return 0.18f; } else if (psi_dot_deg < 15.0f) { return 0.15f - 0.03f * (psi_dot_deg - 5.0f) / 10.0f; } else { return 0.10f; } } void TC_Update(TCController_t *ctrl, TCInput_t *input, TCOutput_t *output) { // 1. 車速估測 output->v = estimate_vehicle_speed(input->omega_fl, input->omega_fr, ctrl->R_front); // 2. Slip ratio 計算 compute_slip_ratio(input->omega_rl, input->omega_rr, output->v, ctrl->R_rear, &output->slip_L, &output->slip_R, &output->slip_avg); // 3. Target slip 自適應 float psi_dot_deg = fabsf(input->psi_dot) * 180.0f / 3.14159f; output->lambda_target = compute_target_slip(psi_dot_deg); // 4. 扭矩命令 (Safety First) output->tc_status = 1; // NORMAL if (output->v < 0.83f) { // < 3 km/h output->T_cmd_final = input->T_driver; output->tc_status = 0; // OFF } else if (fabsf(output->slip_L) > 0.4f || fabsf(output->slip_R) > 0.4f) { output->T_cmd_final = input->T_driver * 0.5f; output->tc_status = 2; // SAFETY } else if (psi_dot_deg > 25.0f) { output->T_cmd_final = input->T_driver * 0.3f; output->tc_status = 2; // SAFETY } else { // PI 控制 float error = output->lambda_target - output->slip_avg; float p_term = ctrl->Kp * error; ctrl->integral += error * ctrl->dt; if (ctrl->integral > 1.0f) ctrl->integral = 1.0f; if (ctrl->integral < -1.0f) ctrl->integral = -1.0f; float i_term = ctrl->Ki * ctrl->integral; float delta_T = p_term + i_term; float T_raw = input->T_driver * (1.0f + delta_T); // 飽和限縮 float T_min = input->T_driver * 0.3f; float T_max = input->T_driver * 1.0f; output->T_cmd_final = (T_raw > T_max) ? T_max : ((T_raw < T_min) ? T_min : T_raw); } output->T_cmd = output->T_cmd_final; } 附錄 A: 常見問題與調試 Q1: Slip ratio 經常偏高（> 0.5），控制不穩定 原因可能 ： 輪胎半徑 R 估測錯誤 前輪打滑（後驅時不應該發生，除非彎度很大） 輪速感測器信號雜訊大 排查 ： 用非驅動輪直線加速，手工驗證 R 值 比較 FL/FR 輪速，差異 > 5% 表示有問題 加低通濾波到輪速訊號 Q2: TC 限縮扭矩後加速反而變慢 原因可能 ： Target slip 設定太低 Kp/Ki 過大，過度限縮 解決 ： 逐次提高 target slip (0.15 → 0.18 → 0.20)，找最快點 降低 Kp (5.0 → 3.5 → 2.5)，觀察 slip 波動 Q3: 彎道時車輛過度 yaw，無法控制 原因 ： Target slip 在彎道時未降低 IMU yaw rate 閾值設定不當 解決 ： 確認自適應邏輯正確執行（加 log 輸出） 提高彎道 target slip 降幅（0.15 → 0.08） 附錄 B: 建議進階項目 路面摩擦力估測 ：從加速度飽和點推估 μ，動態調整 target slip 單輪扭矩分配 ：左右驅動輪獨立控制，改善彎道穩定性 Neural Network 增益 ：結合你之前研究的 observer，用 NN 預測最佳 Kp/Ki 實時濾波優化 ：應用 Kalman filter 融合前輪和 IMU 速度估測 最後建議 ：先用 Python 在桌上測試演算法邏輯（週 1-2），確認無誤後移植到 STM32（週 3-4），場測時邊跑邊調參數，逐次優化。祝你實驗順利！ 🚗💨