基于Python的EuRoC数据集IMU位姿求解偏差问题问询

你忽略的核心问题 #

• IMU原始数据未预处理：EuRoC数据集的IMU数据包含固有噪声、零偏误差，直接积分会导致误差快速累积，最终偏离真值。必须先做零偏校准（计算静止状态下的均值作为偏差并扣除）、低通滤波（过滤高频噪声）。
• 加速度坐标系未转换：IMU输出的加速度是机体坐标系下的数值，包含重力加速度分量。你直接积分的是机体系加速度，而位姿计算需要的是世界坐标系下的线加速度。必须用当前姿态（由角速度推导的姿态）将机体系加速度转换到世界系，再减去重力分量（如Z轴减去9.81m/s²）。
• 姿态积分方式错误：直接对角速度积分得到欧拉角的方式存在万向锁问题，且误差累积极快。正确做法是用四元数更新姿态，通过角速度求解四元数微分方程，再将四元数转换为欧拉角。
• 时间戳处理逻辑错误：你用dt = dt - dt[0]得到的是从起点到当前的总时间，而积分需要的是相邻时间戳的差值（即dt = np.diff(timestamps_sec)），cumtrapz积分时应基于相邻时刻的时间差计算。

可处理IMU数据的工具包 #

• imu_filter_madgwick（ROS）：成熟的IMU姿态估计工具，支持Madgwick/Mahony滤波，可直接处理EuRoC格式数据，输出四元数或欧拉角。
• OpenVINS：专注视觉惯性里程计（VIO）的开源框架，完美适配EuRoC数据集，可融合IMU与相机数据得到高精度位姿，也支持单独IMU惯性导航。
• Basalt：ETH Zurich推出的VIO框架，针对EuRoC数据集做了深度优化，IMU预积分模块专业，适合学术与工程场景。
• SciPy integrate.solve_ivp：若要自行实现姿态积分，用该函数求解四元数微分方程，比直接用cumtrapz精度更高。

修正后核心步骤示例 #

import numpy as np

# 假设已完成IMU零偏校准，得到校准后的角速度gyro_calib、加速度accel_calib
timestamps_sec = nparr[:,0] / 1e9
dt = np.diff(timestamps_sec)  # 相邻时间差

# 四元数更新姿态
def update_quaternion(quat, gyro, dt):
    wx, wy, wz = gyro
    omega_mat = np.array([
        [0, -wx, -wy, -wz],
        [wx, 0, wz, -wy],
        [wy, -wz, 0, wx],
        [wz, wy, -wx, 0]
    ])
    dq_dt = 0.5 * omega_mat @ quat
    return quat + dq_dt * dt

# 初始化姿态、速度、位置
quat = np.array([1, 0, 0, 0])  # 初始四元数（无旋转）
vel = np.zeros(3)
pos = np.zeros(3)
poses = []

for i in range(len(dt)):
    # 1. 更新姿态并归一化四元数
    quat = update_quaternion(quat, gyro_calib[i], dt[i])
    quat /= np.linalg.norm(quat)
    
    # 四元数转旋转矩阵（用于加速度坐标系转换）
    R = np.array([
        [1-2*quat[2]**2-2*quat[3]**2, 2*quat[1]*quat[2]-2*quat[0]*quat[3], 2*quat[1]*quat[3]+2*quat[0]*quat[2]],
        [2*quat[1]*quat[2]+2*quat[0]*quat[3], 1-2*quat[1]**2-2*quat[3]**2, 2*quat[2]*quat[3]-2*quat[0]*quat[1]],
        [2*quat[1]*quat[3]-2*quat[0]*quat[2], 2*quat[2]*quat[3]+2*quat[0]*quat[1], 1-2*quat[1]**2-2*quat[2]**2]
    ])
    
    # 2. 加速度转换到世界系并去除重力
    accel_world = R @ accel_calib[i]
    accel_world[2] -= 9.81  # 扣除Z轴重力分量
    
    # 3. 积分得到速度与位置
    vel += accel_world * dt[i]
    pos += vel * dt[i]
    
    # 转欧拉角（可选）
    roll = np.arctan2(2*(quat[0]*quat[1]+quat[2]*quat[3]), 1-2*(quat[1]**2+quat[2]**2))
    pitch = np.arcsin(2*(quat[0]*quat[2]-quat[3]*quat[1]))
    yaw = np.arctan2(2*(quat[0]*quat[3]+quat[1]*quat[2]), 1-2*(quat[2]**2+quat[3]**2))
    
    poses.append((pos[0], pos[1], pos[2], roll, pitch, yaw))

内容的提问来源于stack exchange，提问作者Bruhnion