#!/usr/bin/env python3 # -*- coding: utf-8 -*- """ 手动提取PWM-速度关系（使用航向角和位移计算前向速度） """ import sys import numpy as np import math def main(): if len(sys.argv) < 2: print("用法: python extract_pwm_manual.py calibration.log") return log_file = sys.argv[1] # 读取所有数据 data_points = [] with open(log_file, 'r', encoding='utf-8', errors='ignore') as f: for line in f: if not line.startswith('$CAL'): continue try: parts = line.strip().split(',') if len(parts) < 18: continue data_points.append({ 'seq': int(parts[1]), 'time_ms': int(parts[2]), 'state': parts[3], 'throttle': int(parts[4]), 'steering': int(parts[5]), 'enu_x': float(parts[6]), 'enu_y': float(parts[7]), 'enu_z': float(parts[8]), 'hdg': float(parts[9]), 'pitch': float(parts[10]), 'roll': float(parts[11]) }) except: continue print(f"解析了 {len(data_points)} 条数据") # 分析每个巡航状态的平均速度 cruise_states = ['FWD_VLOW_C', 'FWD_LOW_C', 'FWD_MID_C', 'FWD_HIGH_C', 'FWD_VHIGH_C', 'FWD_MAX_C'] print("\n========== 前进速度分析（巡航阶段）==========") pwm_speed_pairs = [] for state_name in cruise_states: state_data = [d for d in data_points if d['state'] == state_name] if len(state_data) < 20: continue # 取中间80%的数据（去除加速和减速影响） start_idx = len(state_data) // 10 end_idx = len(state_data) - len(state_data) // 10 stable_data = state_data[start_idx:end_idx] if len(stable_data) < 10: continue # 计算总位移和总时间 total_dist = 0 for i in range(1, len(stable_data)): dx = stable_data[i]['enu_x'] - stable_data[i-1]['enu_x'] dy = stable_data[i]['enu_y'] - stable_data[i-1]['enu_y'] total_dist += math.sqrt(dx*dx + dy*dy) total_time = (stable_data[-1]['time_ms'] - stable_data[0]['time_ms']) / 1000.0 if total_time > 0: avg_speed = total_dist / total_time pwm = stable_data[0]['throttle'] pwm_speed_pairs.append((pwm, avg_speed)) print(f"PWM {pwm}: 速度 {avg_speed:.3f} m/s (距离={total_dist:.2f}m, 时长={total_time:.1f}s)") # 线性拟合 if len(pwm_speed_pairs) >= 3: pwms = np.array([p[0] for p in pwm_speed_pairs]) speeds = np.array([p[1] for p in pwm_speed_pairs]) # v = k × (1500 - pwm) + bias x = 1500 - pwms y = speeds coef = np.polyfit(x, y, 1) k = coef[0] bias = coef[1] print(f"\n========== 线性拟合结果 ==========") print(f"模型：v = {k:.6f} × (1500 - pwm) + ({bias:.6f})") print(f"\n验证拟合质量：") for pwm, v_measured in pwm_speed_pairs: v_predicted = k * (1500 - pwm) + bias error = abs(v_measured - v_predicted) print(f" PWM {pwm}: 实测 {v_measured:.3f} m/s, 预测 {v_predicted:.3f} m/s, 误差 {error:.3f} m/s") # 预测极限 max_speed = k * (1500 - 1000) + bias print(f"\n========== 推荐参数 ==========") print(f"#define MC_CFG_FORWARD_K ({k:.6f}f)") print(f"#define MC_CFG_FORWARD_BIAS ({bias:.6f}f)") print(f"#define MC_CFG_MAX_FORWARD_MPS ({max_speed:.3f}f) // 预测1000PWM最大速度") print(f"#define MC_CFG_FORWARD_DEADZONE_PWM (30) // 可选死区") # 分析转向 print("\n\n========== 转向速度分析（原地转向）==========") turn_states_left = ['TURN_L_VLOW', 'TURN_L_LOW', 'TURN_L_MID', 'TURN_L_HIGH', 'TURN_L_MAX'] turn_states_right = ['TURN_R_VLOW', 'TURN_R_LOW', 'TURN_R_MID', 'TURN_R_HIGH', 'TURN_R_MAX'] turn_pwm_yaw = [] for state_name in turn_states_left + turn_states_right: state_data = [d for d in data_points if d['state'] == state_name] if len(state_data) < 20: continue # 取中间数据 start_idx = len(state_data) // 4 end_idx = len(state_data) - len(state_data) // 4 stable_data = state_data[start_idx:end_idx] if len(stable_data) < 5: continue # 计算航向变化率 hdg_changes = [] for i in range(1, len(stable_data)): dhdg = stable_data[i]['hdg'] - stable_data[i-1]['hdg'] # 处理360°跳变 if dhdg > 180: dhdg -= 360 elif dhdg < -180: dhdg += 360 dt = (stable_data[i]['time_ms'] - stable_data[i-1]['time_ms']) / 1000.0 if dt > 0: yaw_rate_deg = dhdg / dt hdg_changes.append(yaw_rate_deg) if hdg_changes: avg_yaw_deg = np.mean(hdg_changes) avg_yaw_rad = avg_yaw_deg * math.pi / 180.0 pwm = stable_data[0]['steering'] turn_pwm_yaw.append((pwm, avg_yaw_rad)) print(f"PWM {pwm}: 角速度 {avg_yaw_rad:.4f} rad/s ({avg_yaw_deg:.2f} °/s, {len(hdg_changes)}个样本)") # 拟合转向模型 if len(turn_pwm_yaw) >= 3: pwms = np.array([p[0] for p in turn_pwm_yaw]) yaw_rates = np.array([p[1] for p in turn_pwm_yaw]) # 分左转和右转 left_mask = pwms < 1500 right_mask = pwms > 1500 if np.sum(left_mask) >= 2: x_left = 1500 - pwms[left_mask] y_left = yaw_rates[left_mask] k_left = np.polyfit(x_left, y_left, 1)[0] print(f"\n左转模型：ω = {k_left:.8f} × (1500 - pwm)") print(f"#define MC_CFG_STEERING_K_LEFT ({k_left:.8f}f)") if np.sum(right_mask) >= 2: x_right = pwms[right_mask] - 1500 y_right = -yaw_rates[right_mask] # 取反，因为右转是负角速度 k_right = np.polyfit(x_right, y_right, 1)[0] print(f"\n右转模型：ω = -{k_right:.8f} × (pwm - 1500)") print(f"#define MC_CFG_STEERING_K_RIGHT ({k_right:.8f}f)") if __name__ == '__main__': main()