#!/usr/bin/env python3 # -*- coding: utf-8 -*- """ 车辆运动模型校准数据分析工具 解析$CAL格式日志，提取差速运动模型参数 """ import argparse import json import re import sys from pathlib import Path from typing import List, Dict, Tuple import numpy as np import matplotlib import matplotlib.pyplot as plt from scipy import optimize from collections import defaultdict # 可选：配置中文字体以避免警告 # 如果图表中文显示异常，可运行: python tools/fix_matplotlib_font.py 测试 try: import platform system = platform.system() if system == 'Windows': matplotlib.rcParams['font.sans-serif'] = ['Microsoft YaHei', 'SimHei', 'SimSun'] elif system == 'Darwin': matplotlib.rcParams['font.sans-serif'] = ['PingFang SC', 'Heiti SC'] else: matplotlib.rcParams['font.sans-serif'] = ['WenQuanYi Micro Hei', 'Noto Sans CJK SC'] matplotlib.rcParams['axes.unicode_minus'] = False except: pass # 字体配置失败也不影响功能，只是可能有警告 # 添加项目路径 sys.path.append(str(Path(__file__).parent.parent)) # ENU坐标已在单片机端计算，不再需要geo转换函数 class CalibrationData: """校准数据点""" def __init__(self, line: str): # $CAL,seq,time_ms,state,throttle_pwm,steering_pwm,enu_x,enu_y,enu_z,hdg,pitch,roll,gx,gy,gz,ax,ay,az parts = line.strip().split(',') if len(parts) < 18: raise ValueError(f"Invalid CAL line: {line}") self.seq = int(parts[1]) self.time_ms = int(parts[2]) self.state = parts[3] self.throttle_pwm = int(parts[4]) self.steering_pwm = int(parts[5]) # 直接读取ENU坐标（不再需要从经纬度转换） self.enu_x = float(parts[6]) # 东向坐标 (m) self.enu_y = float(parts[7]) # 北向坐标 (m) self.enu_z = float(parts[8]) # 天向坐标 (m) self.hdg = float(parts[9]) self.pitch = float(parts[10]) self.roll = float(parts[11]) self.gx = float(parts[12]) / 1000.0 # 毫度/秒 -> 度/秒 self.gy = float(parts[13]) / 1000.0 self.gz = float(parts[14]) / 1000.0 self.ax = float(parts[15]) / 1000.0 # 毫g -> g self.ay = float(parts[16]) / 1000.0 self.az = float(parts[17]) / 1000.0 # 待计算字段 self.velocity = 0.0 # 前向速度 m/s self.yaw_rate = 0.0 # 角速度 rad/s # 兼容性字段（已废弃） self.lat = 0.0 self.lon = 0.0 self.alt = self.enu_z class CalibrationAnalyzer: """校准数据分析器""" def __init__(self, log_file: str): self.log_file = log_file self.data: List[CalibrationData] = [] self.states_data: Dict[str, List[CalibrationData]] = defaultdict(list) def parse_log(self): """解析日志文件""" print(f"解析日志文件: {self.log_file}") with open(self.log_file, 'r', encoding='utf-8', errors='ignore') as f: for line in f: if line.startswith('$CAL'): try: data = CalibrationData(line) self.data.append(data) except Exception as e: print(f"警告: 解析行失败: {e}") continue if not self.data: raise ValueError("未找到有效的$CAL数据") print(f"成功解析 {len(self.data)} 条数据点") # ENU坐标已在单片机端计算完成，这里不需要原点转换 print(f"ENU坐标范围: X=[{min(d.enu_x for d in self.data):.2f}, {max(d.enu_x for d in self.data):.2f}], " + f"Y=[{min(d.enu_y for d in self.data):.2f}, {max(d.enu_y for d in self.data):.2f}] (m)") def compute_enu_and_velocity(self): """计算速度（ENU坐标已在单片机端计算完成）""" print("计算速度和角速度...") # ENU坐标已在单片机端计算完成，这里只需要计算速度和角速度 for i, data in enumerate(self.data): # 计算速度（相邻点差分） if i > 0: dt = (data.time_ms - self.data[i-1].time_ms) / 1000.0 # 秒 if dt > 0: dx = data.enu_x - self.data[i-1].enu_x dy = data.enu_y - self.data[i-1].enu_y data.velocity = np.sqrt(dx**2 + dy**2) / dt # 角速度（从航向角差分） dhdg = data.hdg - self.data[i-1].hdg # 处理角度跳变 if dhdg > 180: dhdg -= 360 elif dhdg < -180: dhdg += 360 data.yaw_rate = np.deg2rad(dhdg) / dt # 按状态分组 for data in self.data: self.states_data[data.state].append(data) print(f"数据分组完成，共 {len(self.states_data)} 个状态") for state, dlist in self.states_data.items(): print(f" {state}: {len(dlist)} 个点") def analyze_forward_model(self) -> Dict: """分析前向运动模型""" print("\n=== 分析前向运动模型 ===") results = {} # 分析巡航阶段的PWM-速度关系 cruise_states = ['CRUISE_LOW', 'CRUISE_MID', 'CRUISE_HIGH'] pwm_velocity_pairs = [] for state in cruise_states: if state in self.states_data: dlist = self.states_data[state] # 取后半段数据（稳态） stable_data = dlist[len(dlist)//2:] if stable_data: avg_pwm = np.mean([d.throttle_pwm for d in stable_data]) avg_vel = np.mean([d.velocity for d in stable_data]) pwm_velocity_pairs.append((avg_pwm, avg_vel)) print(f"{state}: PWM={avg_pwm:.0f}, 速度={avg_vel:.2f} m/s") # 拟合线性模型: v = k * (pwm - pwm_center) + bias if len(pwm_velocity_pairs) >= 2: pwms = np.array([p[0] for p in pwm_velocity_pairs]) vels = np.array([p[1] for p in pwm_velocity_pairs]) # 线性拟合 def linear_model(pwm, k, bias): return k * (pwm - 1500) + bias popt, _ = optimize.curve_fit(linear_model, pwms, vels) k_forward, bias = popt results['pwm_to_velocity'] = { 'model': 'linear', 'k': float(k_forward), 'bias': float(bias), 'formula': f'v = {k_forward:.6f} * (pwm - 1500) + {bias:.6f}', 'pwm_velocity_pairs': [(float(p), float(v)) for p, v in pwm_velocity_pairs] } print(f"PWM-速度模型: v = {k_forward:.6f} * (pwm - 1500) + {bias:.6f}") # 分析加速度 accel_states = ['ACCEL_LOW', 'ACCEL_MID', 'ACCEL_HIGH'] accel_results = {} for state in accel_states: if state in self.states_data: dlist = self.states_data[state] if len(dlist) > 5: velocities = [d.velocity for d in dlist] times = [(d.time_ms - dlist[0].time_ms) / 1000.0 for d in dlist] # 线性拟合速度-时间 if len(times) > 1: coeffs = np.polyfit(times, velocities, 1) accel = coeffs[0] accel_results[state] = float(accel) print(f"{state}: 加速度 = {accel:.3f} m/s²") results['acceleration'] = accel_results # 分析制动 if 'BRAKE' in self.states_data: dlist = self.states_data['BRAKE'] if len(dlist) > 5: velocities = [d.velocity for d in dlist] times = [(d.time_ms - dlist[0].time_ms) / 1000.0 for d in dlist] if len(times) > 1: coeffs = np.polyfit(times, velocities, 1) decel = coeffs[0] results['deceleration'] = float(decel) print(f"制动减速度: {decel:.3f} m/s²") return results def analyze_steering_model(self) -> Dict: """分析转向模型""" print("\n=== 分析转向模型 ===") results = {} turn_states = ['TURN_LEFT_LIGHT', 'TURN_RIGHT_LIGHT', 'TURN_LEFT_HEAVY', 'TURN_RIGHT_HEAVY'] pwm_yawrate_pairs = [] for state in turn_states: if state in self.states_data: dlist = self.states_data[state] # 取中间段数据（稳态转向） start_idx = len(dlist) // 4 end_idx = 3 * len(dlist) // 4 stable_data = dlist[start_idx:end_idx] if stable_data: avg_steering_pwm = np.mean([d.steering_pwm for d in stable_data]) avg_yaw_rate = np.mean([d.yaw_rate for d in stable_data]) avg_velocity = np.mean([d.velocity for d in stable_data]) # 转向PWM偏移量（1500为中心） steering_offset = avg_steering_pwm - 1500 pwm_yawrate_pairs.append((steering_offset, avg_yaw_rate, avg_velocity)) turning_radius = abs(avg_velocity / avg_yaw_rate) if abs(avg_yaw_rate) > 0.01 else float('inf') print(f"{state}: 舵机PWM={avg_steering_pwm:.0f} ({steering_offset:+.0f}), " f"角速度={avg_yaw_rate:.3f} rad/s, 速度={avg_velocity:.2f} m/s, " f"转向半径={turning_radius:.2f} m") # 拟合转向模型: ω = k_steering * steering_offset if len(pwm_yawrate_pairs) >= 2: offsets = np.array([p[0] for p in pwm_yawrate_pairs]) yaw_rates = np.array([p[1] for p in pwm_yawrate_pairs]) # 线性拟合（过原点） k_steering = np.sum(offsets * yaw_rates) / np.sum(offsets**2) results['pwm_to_yaw_rate'] = { 'model': 'linear', 'k': float(k_steering), 'formula': f'ω = {k_steering:.8f} * (steering_pwm - 1500)', 'data': [(float(o), float(y), float(v)) for o, y, v in pwm_yawrate_pairs] } print(f"转向模型: ω = {k_steering:.8f} * (steering_pwm - 1500)") return results def analyze_delays(self) -> Dict: """分析响应延迟""" print("\n=== 分析响应延迟 ===") results = {} # 检测WARMUP到ACCEL_LOW的响应延迟 if 'WARMUP' in self.states_data and 'ACCEL_LOW' in self.states_data: warmup_last = self.states_data['WARMUP'][-1] accel_first = self.states_data['ACCEL_LOW'][0] # 找到速度开始上升的点 threshold_velocity = 0.1 # m/s for d in self.states_data['ACCEL_LOW']: if d.velocity > threshold_velocity: delay_ms = d.time_ms - accel_first.time_ms results['throttle_response_delay_ms'] = float(delay_ms) print(f"油门响应延迟: {delay_ms} ms") break return results def generate_plots(self, output_dir: Path): """生成可视化图表""" print("\n=== 生成可视化图表 ===") output_dir.mkdir(parents=True, exist_ok=True) # 1. 速度-时间曲线 fig, ax = plt.subplots(figsize=(14, 6)) times = [(d.time_ms - self.data[0].time_ms) / 1000.0 for d in self.data] velocities = [d.velocity for d in self.data] throttles = [d.throttle_pwm for d in self.data] ax.plot(times, velocities, 'b-', linewidth=1.5, label='速度 (m/s)') ax.set_xlabel('时间 (秒)', fontsize=12) ax.set_ylabel('速度 (m/s)', fontsize=12, color='b') ax.tick_params(axis='y', labelcolor='b') ax.grid(True, alpha=0.3) # 双轴显示PWM ax2 = ax.twinx() ax2.plot(times, throttles, 'r--', linewidth=1, alpha=0.6, label='油门PWM') ax2.set_ylabel('油门PWM', fontsize=12, color='r') ax2.tick_params(axis='y', labelcolor='r') # 标注状态 current_state = self.data[0].state state_start_time = times[0] for i, (t, d) in enumerate(zip(times, self.data)): if d.state != current_state or i == len(self.data) - 1: ax.axvspan(state_start_time, t, alpha=0.1, color='gray') ax.text((state_start_time + t) / 2, max(velocities) * 0.9, current_state, ha='center', fontsize=8, rotation=0) current_state = d.state state_start_time = t plt.title('速度与油门PWM随时间变化', fontsize=14) fig.tight_layout() plt.savefig(output_dir / 'velocity_time.png', dpi=150) print(f"保存: {output_dir / 'velocity_time.png'}") plt.close() # 2. 轨迹图 fig, ax = plt.subplots(figsize=(10, 10)) xs = [d.enu_x for d in self.data] ys = [d.enu_y for d in self.data] # 按状态着色 state_colors = {} color_idx = 0 colors_list = plt.cm.tab10(np.linspace(0, 1, 10)) for state in self.states_data.keys(): state_colors[state] = colors_list[color_idx % 10] color_idx += 1 for state, dlist in self.states_data.items(): xs_state = [d.enu_x for d in dlist] ys_state = [d.enu_y for d in dlist] ax.plot(xs_state, ys_state, color=state_colors[state], linewidth=2, label=state, marker='o', markersize=2) ax.set_xlabel('东向 (m)', fontsize=12) ax.set_ylabel('北向 (m)', fontsize=12) ax.set_title('车辆运动轨迹 (ENU坐标)', fontsize=14) ax.grid(True, alpha=0.3) ax.axis('equal') ax.legend(fontsize=8, loc='best') plt.tight_layout() plt.savefig(output_dir / 'trajectory.png', dpi=150) print(f"保存: {output_dir / 'trajectory.png'}") plt.close() # 3. PWM-速度关系图 fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(14, 6)) # 前向速度 cruise_states = ['CRUISE_LOW', 'CRUISE_MID', 'CRUISE_HIGH'] for state in cruise_states: if state in self.states_data: dlist = self.states_data[state] stable_data = dlist[len(dlist)//2:] pwms = [d.throttle_pwm for d in stable_data] vels = [d.velocity for d in stable_data] ax1.scatter(pwms, vels, label=state, alpha=0.5, s=20) ax1.set_xlabel('油门PWM', fontsize=12) ax1.set_ylabel('速度 (m/s)', fontsize=12) ax1.set_title('PWM-速度关系', fontsize=14) ax1.grid(True, alpha=0.3) ax1.legend() # 转向角速度 turn_states = ['TURN_LEFT_LIGHT', 'TURN_RIGHT_LIGHT', 'TURN_LEFT_HEAVY', 'TURN_RIGHT_HEAVY'] for state in turn_states: if state in self.states_data: dlist = self.states_data[state] start_idx = len(dlist) // 4 end_idx = 3 * len(dlist) // 4 stable_data = dlist[start_idx:end_idx] steering_offsets = [d.steering_pwm - 1500 for d in stable_data] yaw_rates = [d.yaw_rate for d in stable_data] ax2.scatter(steering_offsets, yaw_rates, label=state, alpha=0.5, s=20) ax2.axhline(0, color='k', linestyle='--', linewidth=0.5) ax2.axvline(0, color='k', linestyle='--', linewidth=0.5) ax2.set_xlabel('舵机PWM偏移 (相对1500)', fontsize=12) ax2.set_ylabel('角速度 (rad/s)', fontsize=12) ax2.set_title('舵机PWM-角速度关系', fontsize=14) ax2.grid(True, alpha=0.3) ax2.legend() plt.tight_layout() plt.savefig(output_dir / 'pwm_relationships.png', dpi=150) print(f"保存: {output_dir / 'pwm_relationships.png'}") plt.close() # 4. 加速度分析 fig, ax = plt.subplots(figsize=(12, 6)) accel_states = ['ACCEL_LOW', 'ACCEL_MID', 'ACCEL_HIGH', 'BRAKE'] for state in accel_states: if state in self.states_data: dlist = self.states_data[state] times_rel = [(d.time_ms - dlist[0].time_ms) / 1000.0 for d in dlist] vels = [d.velocity for d in dlist] ax.plot(times_rel, vels, marker='o', markersize=3, label=state, linewidth=2) ax.set_xlabel('相对时间 (秒)', fontsize=12) ax.set_ylabel('速度 (m/s)', fontsize=12) ax.set_title('加速/制动特性', fontsize=14) ax.grid(True, alpha=0.3) ax.legend() plt.tight_layout() plt.savefig(output_dir / 'acceleration.png', dpi=150) print(f"保存: {output_dir / 'acceleration.png'}") plt.close() def generate_json_report(self, output_file: Path): """生成JSON参数报告""" print("\n=== 生成JSON参数报告 ===") forward_model = self.analyze_forward_model() steering_model = self.analyze_steering_model() delays = self.analyze_delays() report = { 'metadata': { 'log_file': str(self.log_file), 'total_samples': len(self.data), 'duration_seconds': (self.data[-1].time_ms - self.data[0].time_ms) / 1000.0, 'enu_range': { 'x_min': min(d.enu_x for d in self.data), 'x_max': max(d.enu_x for d in self.data), 'y_min': min(d.enu_y for d in self.data), 'y_max': max(d.enu_y for d in self.data), 'note': 'ENU coordinates computed onboard, relative to first RTK fix' }, 'states': {state: len(dlist) for state, dlist in self.states_data.items()} }, 'differential_drive_model': { 'forward_model': forward_model, 'steering_model': steering_model, 'delays': delays }, 'recommended_config': self._generate_recommended_config(forward_model, steering_model, delays) } with open(output_file, 'w', encoding='utf-8') as f: json.dump(report, f, indent=2, ensure_ascii=False) print(f"JSON报告已保存: {output_file}") return report def _generate_recommended_config(self, forward_model: Dict, steering_model: Dict, delays: Dict) -> Dict: """生成推荐的配置参数""" config = {} # 前向模型参数 if 'pwm_to_velocity' in forward_model: config['MC_CFG_FORWARD_K'] = forward_model['pwm_to_velocity']['k'] config['MC_CFG_FORWARD_BIAS'] = forward_model['pwm_to_velocity']['bias'] # 转向模型参数 if 'pwm_to_yaw_rate' in steering_model: config['MC_CFG_STEERING_K'] = steering_model['pwm_to_yaw_rate']['k'] # 加速度限制 if 'acceleration' in forward_model: max_accel = max(forward_model['acceleration'].values()) if forward_model['acceleration'] else 1.0 config['MC_CFG_MAX_ACCELERATION'] = max_accel if 'deceleration' in forward_model: config['MC_CFG_MAX_DECELERATION'] = abs(forward_model['deceleration']) # 响应延迟 if 'throttle_response_delay_ms' in delays: config['MC_CFG_RESPONSE_DELAY_MS'] = delays['throttle_response_delay_ms'] return config def main(): parser = argparse.ArgumentParser(description='车辆运动模型校准数据分析工具') parser.add_argument('log_file', type=str, help='校准日志文件路径 (.log或.txt)') parser.add_argument('-o', '--output', type=str, default='calibration_results', help='输出目录 (默认: calibration_results)') args = parser.parse_args() log_file = Path(args.log_file) if not log_file.exists(): print(f"错误: 日志文件不存在: {log_file}") sys.exit(1) output_dir = Path(args.output) print("=" * 60) print("车辆运动模型校准数据分析工具") print("=" * 60) # 创建分析器 analyzer = CalibrationAnalyzer(str(log_file)) # 解析日志 analyzer.parse_log() # 计算ENU坐标和速度 analyzer.compute_enu_and_velocity() # 生成可视化图表 analyzer.generate_plots(output_dir) # 生成JSON报告 json_file = output_dir / 'calibration_parameters.json' report = analyzer.generate_json_report(json_file) # 打印推荐配置 print("\n" + "=" * 60) print("推荐的配置参数 (可复制到motion_config.h):") print("=" * 60) for key, value in report['recommended_config'].items(): if isinstance(value, float): print(f"#define {key:<35} ({value:.8f}f)") else: print(f"#define {key:<35} ({value})") print("\n" + "=" * 60) print("分析完成！") print(f"输出目录: {output_dir.absolute()}") print("=" * 60) if __name__ == '__main__': main()