cyy/imu_decode

Fork 0

Files

MobKBK 8531c4dc9e first commit

2026-05-26 03:28:37 +08:00

4.5 KiB

Raw Permalink Blame History

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 正常触发。

依赖

pip install numpy matplotlib pyserial scipy

用法

实时模式

python main_odometry.py COM5
python main_odometry.py COM5 921600
python main_odometry.py COM5 --save traj.csv

回放模式

python main_odometry.py --replay traj.csv

回放时按 空格键 暂停/继续。

调试

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 格式

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

4.5 KiB Raw Permalink Blame History Unescape Escape