first commit

README_ODOMETRY.md

@@ -0,0 +1,148 @@
+# IMU 惯性三维里程计 + 3D 坐标显示
+
+## 概述
+
+基于 STM32 IMU 串口解码系统，在 PC 端实现 3D 轨迹重建和实时可视化。下位机 (STM32) 已完成 EKF 姿态估计，PC 端接收四元数姿态和滤波后加速度数据，进行偏置标定、重力补偿与双重积分，重建三维运动轨迹。
+
+## 架构
+
+```
+STM32 (200Hz)
+  └── 串口帧 (48 bytes): header + timestamp(uint32) + gyro[3] + accel[3] + quat[4] + CRC16
+        │
+PC 端管线:
+  ├── imu_decode.py          # 串口帧解析 (CRC 校验 + 解包 48 字节帧)
+  ├── trajectory_tracker.py  # 核心算法 (四元数→旋转, 偏置标定, 重力补偿, 双重积分, ZUPT)
+  ├── visualize_3d.py        # matplotlib 3D 动画窗口
+  └── main_odometry.py       # 主入口 (串联所有模块)
+```
+
+## 串口协议 (v2)
+
+| 偏移 | 大小 | 内容 |
+|------|------|------|
+| 0 | 2B | 帧头 0xAA 0x55 |
+| 2 | 4B | 时间戳 uint32_t (ms, 自启动) |
+| 6 | 12B | 滤波后 Gyro[3] (float32 × 3) |
+| 18 | 12B | 滤波后 Accel[3] (float32 × 3) |
+| 30 | 16B | 四元数 qw, qx, qy, qz (float32 × 4) |
+| 46 | 2B | CRC16 (对前 46 字节) |
+
+**坐标系**: 右手系, X-前 Y-左 Z-上 (Z-up)
+
+## 算法管线
+
+```
+串口帧 (已解析)
+  ├── timestamp ms ──→ dt = Δts / 1000 (精确时间步长)
+  ├── gyro[3] rad/s ──→ 减 gyro_bias → ZUPT 静止检测
+  ├── accel[3] m/s^2 ──→ 减 accel_bias (EKF 四元数标定)
+  └── qw/qx/qy/qz ──→ 四元数 → 旋转矩阵 (scipy Rotation.from_quat)
+                │
+        a_world = R @ (a_body - accel_bias)
+        a_linear = a_world - [0, 0, 9.81]
+                │
+        加速度死区 (|a_i| < 0.03 → 0)
+        ZUPT: ‖gyro‖ < 0.05 AND ‖a_linear‖ < 0.20 AND var(a_linear) < 0.005
+                │
+        梯形积分 → 速度 → 位置 (使用时间戳真实 dt)
+                │
+        matplotlib 3D 实时轨迹显示
+```
+
+### 关键: 加速度计偏置标定
+
+EKF 四元数与加速度计之间存在不一致时会导致积分漂移。启动时采集 200 帧静止数据，利用 EKF 四元数标定：
+
+```
+R = from_quat(q_mean)
+gravity_body_expected = R^T @ [0, 0, 9.81]
+accel_bias = mean(accel_measured) - gravity_body_expected
+```
+
+标定后 `R @ (accel - bias) ≈ [0, 0, 9.81]`，静止时 `a_linear ≈ 0`，ZUPT 正常触发。
+
+## 依赖
+
+```bash
+pip install numpy matplotlib pyserial scipy
+```
+
+## 用法
+
+### 实时模式
+
+```bash
+python main_odometry.py COM5
+python main_odometry.py COM5 921600
+python main_odometry.py COM5 --save traj.csv
+```
+
+### 回放模式
+
+```bash
+python main_odometry.py --replay traj.csv
+```
+
+回放时按 **空格键** 暂停/继续。
+
+### 调试
+
+```bash
+python imu_decode.py COM5          # 仅解析帧并打印
+python test_3d_demo.py             # 模拟数据 3D 演示(无需串口)
+```
+
+## 文件说明
+
+### `imu_decode.py`
+串口帧解析模块。48 字节帧 = 2B header + 44B payload + 2B CRC16。
+
+- `parse_frame(payload)` → dict: timestamp_ms, gyro(3), accel(3), quat(4)
+- `quat_to_euler(qw,qx,qy,qz)` → yaw, pitch, roll (deg)
+
+### `trajectory_tracker.py`
+核心跟踪算法：
+
+- `quat_to_rotation(qw,qx,qy,qz)` — 四元数 → 旋转矩阵
+- `rotate_accel(accel_body, R)` — 机体→世界坐标
+- `gravity_compensate(a_world)` — 减重力 [0, 0, 9.81]
+- `apply_deadzone(a)` — 加速度死区
+- `Tracker` — 位置/速度/姿态跟踪器
+- `Tracker.calibrate_from_samples()` — 利用四元数标定偏置
+
+| ZUPT 参数 | 默认值 |
+|-----------|--------|
+| `zupt_threshold_accel` | 0.20 m/s^2 |
+| `zupt_threshold_gyro` | 0.05 rad/s |
+| `zupt_frames` | 15 帧 |
+| `deadzone_threshold` | 0.03 m/s^2 |
+| `var_window_size` | 30 帧 |
+| `zupt_var_threshold` | 0.005 m^2/s^4 |
+
+### `visualize_3d.py`
+matplotlib 3D 实时显示：蓝色轨迹线、红点当前位置、原点 RGB 坐标轴、自适应等比例坐标。
+
+### `main_odometry.py`
+运行入口：串口采集 + 3D 显示、CSV 保存、回放。dt 由时间戳差值计算。
+
+## CSV 格式
+
+```csv
+timestamp_ms,gyro_x,gyro_y,gyro_z,accel_x,accel_y,accel_z,qw,qx,qy,qz,pos_x,pos_y,pos_z
+12345,0.001,0.002,-0.001,0.75,-0.05,9.81,0.996,0.001,-0.002,-0.036,0.000,0.000,0.000
+...
+```
+
+## 验证结果
+
+| 场景 | 结果 |
+|------|------|
+| 25s 静止 (偏置标定后) | 0.000m 漂移 |
+| 2 m/s^2 × 0.5s 运动 | 0.250m 位移 (理论值) |
+
+## 调试
+
+- 漂移：检查标定输出 `accel_bias` 是否合理 (通常 X/Y < 0.8, Z < 0.1)
+- ZUPT 不触发：增大 `zupt_threshold_accel` 或减小 `zupt_var_threshold`
+- 3D 卡顿：增大 `refresh_interval`