# 车辆运动模型自动校准系统

完整的运动模型校准解决方案，包含MCU自动测试程序和Python数据分析工具。

## 系统组成

### 1. MCU端（STM32H7）

**核心文件**：
- `STM32H743/APL/motion_calibration_task.c/h` - 自动校准任务
- `STM32H743/APL/运动模型校准使用说明.md` - MCU端使用说明

**功能特性**：
- ✅ 自动化测试序列（48秒完成）
- ✅ 多档位直线加速/巡航测试
- ✅ 多强度转向测试
- ✅ 制动响应测试
- ✅ 20Hz高频数据采集
- ✅ SBUS CH8安全保护（<1500运行，≥1500紧急停止）
- ✅ CH7自动触发（>1800启动）

**测试序列**：
| 状态 | PWM | 时长 | 说明 |
|------|-----|------|------|
| WARMUP | 1500/1500 | 2s | 预热 |
| ACCEL_LOW | 1600/1500 | 3s | 低速加速 |
| CRUISE_LOW | 1600/1500 | 5s | 低速巡航 |
| ACCEL_MID | 1700/1500 | 3s | 中速加速 |
| CRUISE_MID | 1700/1500 | 5s | 中速巡航 |
| ACCEL_HIGH | 1800/1500 | 3s | 高速加速 |
| CRUISE_HIGH | 1800/1500 | 5s | 高速巡航 |
| TURN_LEFT_LIGHT | 1600/1600 | 4s | 轻左转 |
| TURN_RIGHT_LIGHT | 1600/1400 | 4s | 轻右转 |
| TURN_LEFT_HEAVY | 1600/1700 | 4s | 重左转 |
| TURN_RIGHT_HEAVY | 1600/1300 | 4s | 重右转 |
| BRAKE | 1500/1500 | 2s | 制动 |

### 2. Python工具链

**核心文件**：
- `python/tools/calibration_logger.py` - 数据记录工具
- `python/tools/calibration_analyzer.py` - 数据分析工具
- `python/tools/校准工具使用说明.md` - Python工具使用说明

**功能特性**：
- ✅ 实时串口数据记录
- ✅ 自动检测测试结束
- ✅ GPS/IMU数据解析
- ✅ ENU坐标转换
- ✅ 差速运动模型拟合
- ✅ 多维度可视化（速度曲线、轨迹图、PWM关系图等）
- ✅ JSON格式参数输出
- ✅ 推荐配置自动生成

## 快速开始

### 准备工作

1. **安装Python依赖**：
```bash
pip install pyserial numpy scipy matplotlib
```

2. **编译烧录固件**：
   - 项目已集成校准任务，直接编译即可
   - 烧录到STM32H7

3. **硬件准备**：
   - 将车辆放置在空旷场地（至少50m×50m）
   - 连接UART5到电脑（USB转串口）
   - 确认串口号（Windows: 设备管理器，Linux: `ls /dev/ttyUSB*`）

### 步骤1：采集数据

```bash
# 启动数据记录
python python/tools/calibration_logger.py -p COM5

# 触发校准（在车辆上）
# 1. CH8拨到 < 1500
# 2. CH7拨到 > 1800
# 3. MCU自动执行48秒测试序列
# 4. 全程监控，必要时CH8≥1500紧急停止

# 记录完成后会生成：calibration_YYYYMMDD_HHMMSS.log
```

### 步骤2：分析数据

```bash
# 分析日志文件
python python/tools/calibration_analyzer.py calibration_20251126_143000.log

# 查看输出目录：calibration_results/
# - calibration_parameters.json  （JSON参数文件）
# - velocity_time.png             （速度-时间曲线）
# - trajectory.png                （运动轨迹图）
# - pwm_relationships.png         （PWM关系图）
# - acceleration.png              （加速度特性）
```

### 步骤3：应用参数

1. 打开生成的 `calibration_parameters.json`
2. 复制 `recommended_config` 中的参数
3. 更新到 `STM32H743/FML/motion_config.h`
4. 重新编译、烧录、测试

**示例配置**：
```c
#define MC_CFG_FORWARD_K                    (0.13767502f)
#define MC_CFG_FORWARD_BIAS                 (-0.23053784f)
#define MC_CFG_STEERING_K                   (0.00698132f)
#define MC_CFG_MAX_ACCELERATION             (1.00000000f)
#define MC_CFG_RESPONSE_DELAY_MS            (50)
```

## 数据格式

### MCU输出（UART5, 115200）

```
$CAL,seq,time_ms,state,throttle_pwm,steering_pwm,lat,lon,alt,hdg,pitch,roll,gx,gy,gz,ax,ay,az
```

**示例**：
```
$CAL,0,1000,WARMUP,1500,1500,39.831867,116.280907,47.50,90.00,0.10,-0.05,12,-5,3,0,1,-1000
$CAL,1,1050,WARMUP,1500,1500,39.831867,116.280907,47.50,90.01,0.11,-0.06,13,-4,2,1,2,-999
```

### JSON参数文件

```json
{
  "metadata": { ... },
  "differential_drive_model": {
    "forward_model": {
      "pwm_to_velocity": {
        "k": 0.137675,
        "bias": -0.230538,
        "formula": "v = 0.137675 * (pwm - 1500) + -0.230538"
      },
      "acceleration": { ... }
    },
    "steering_model": {
      "pwm_to_yaw_rate": {
        "k": 0.00698132,
        "formula": "ω = 0.00698132 * (steering_pwm - 1500)"
      }
    },
    "delays": {
      "throttle_response_delay_ms": 50.0
    }
  },
  "recommended_config": { ... }
}
```

## 可视化输出

### 1. velocity_time.png
- 速度-时间曲线（蓝色实线）
- 油门PWM（红色虚线）
- 状态标注（灰色背景区域）

### 2. trajectory.png
- 车辆ENU轨迹
- 不同状态用不同颜色表示
- 轴等比例显示

### 3. pwm_relationships.png
- 左图：油门PWM vs 速度
- 右图：舵机PWM偏移 vs 角速度

### 4. acceleration.png
- 不同PWM档位的加速度曲线
- 制动减速度曲线

## 运动模型

### 差速驱动（Differential Drive）

**前向模型**：
```
v = k_forward * (throttle_pwm - 1500) + bias
```

**转向模型**：
```
ω = k_steering * (steering_pwm - 1500)
```

**参数意义**：
- `k_forward`: PWM到速度的增益 (m/s per PWM)
- `bias`: 速度偏置（补偿死区）
- `k_steering`: PWM到角速度的增益 (rad/s per PWM)

## 数据质量要求

✅ **良好数据**：
- 总样本数 > 800
- GPS定位质量 ≥ 4（固定解）
- 卫星数 ≥ 8
- 巡航速度方差 < 0.1 m/s

❌ **需重新测试**：
- GPS跳点
- 速度突变
- 转向角速度异常

## 重复测试建议

建议在不同条件下测试3次以上：
- 不同地面材质（水泥/草地/土地）
- 不同天气（干燥/潮湿）
- 不同坡度（平地/上坡/下坡）
- 不同负载（空载/满载）

记录每次测试的环境信息，取平均参数。

## 故障排查

| 问题 | 原因 | 解决方法 |
|------|------|----------|
| 无法启动校准 | CH8≥1500或CH7<1800 | 检查遥控器通道位置 |
| 数据记录中断 | SBUS信号丢失 | 检查接收器连接 |
| 分析工具报错 | 日志文件格式错误 | 确认$CAL行完整 |
| 拟合结果异常 | GPS数据质量差 | 在GPS信号好的场地重测 |

## 文件结构

```
Lawnmower_STM32H7/
├── STM32H743/
│   └── APL/
│       ├── motion_calibration_task.c/h      # MCU校准任务
│       └── 运动模型校准使用说明.md          # MCU使用说明
├── python/
│   └── tools/
│       ├── calibration_logger.py            # 数据记录工具
│       ├── calibration_analyzer.py          # 数据分析工具
│       ├── 校准工具使用说明.md              # Python工具说明
│       └── test_calibration_data.log        # 测试数据示例
└── README_运动模型校准.md                   # 本文件
```

## 进阶用法

### 自定义PWM档位

编辑 `motion_calibration_task.c`：
```c
#define CAL_PWM_LOW_THROTTLE        (1600U)
#define CAL_PWM_MID_THROTTLE        (1700U)
#define CAL_PWM_HIGH_THROTTLE       (1800U)
```

### 调整测试时长

```c
#define CAL_DURATION_ACCEL          (3000U)  // 毫秒
#define CAL_DURATION_CRUISE         (5000U)
```

### 修改采样频率

```c
#define CAL_TASK_PERIOD_MS          (50U)    // 50ms = 20Hz
```

### 批量分析

```bash
for log in calibration_*.log; do
  python python/tools/calibration_analyzer.py "$log" -o "results_$(basename $log .log)"
done
```

## 技术支持

- MCU程序占用：Flash ~6KB, RAM ~1KB
- Python依赖：numpy, scipy, matplotlib
- 推荐Python版本：3.8+

## 更新日志

**v1.0 (2025-11-26)**
- 初始版本
- 完整的MCU自动校准任务
- Python数据记录与分析工具
- 差速运动模型拟合
- 多维度可视化

## 作者

创建日期：2025-11-26