149 lines
4.5 KiB
Markdown
149 lines
4.5 KiB
Markdown
# 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`
|