#!/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()
|
