如何转换LSM9DS1 MEMS传感器数据生成欧拉角/四元数并修复漂移问题

修复步骤 #

• 1. 完成陀螺仪零偏校准
  静止状态下采集100~200组陀螺仪原始数据，取平均值作为零偏值，每次读取陀螺仪数据后先减去零偏再输入Madgwick滤波，示例校准代码：

// 初始化阶段校准，校准过程要求传感器保持静止
let gyroBias = {x:0, y:0, z:0};
const CALIB_COUNT = 200;
for(let i=0; i<CALIB_COUNT; i++) {
  // 累加原始陀螺仪数据
  gyroBias.x += data.gyro_x_dps;
  gyroBias.y += data.gyro_y_dps;
  gyroBias.z += data.gyro_z_dps;
}
// 计算平均零偏
gyroBias.x /= CALIB_COUNT;
gyroBias.y /= CALIB_COUNT;
gyroBias.z /= CALIB_COUNT;

// 后续数据处理时先消除零偏再做单位转换
gyro_x = (data.gyro_x_dps - gyroBias.x) * 0.07 / 57.295779513;

• 2. 确认Madgwick滤波参数配置正确
  • 你代码中陀螺仪转换系数的0.07是采样间隔（单位为秒），请确认该值和实际采样频率完全匹配，采样频率14Hz对应0.07s，若采样频率变动需同步修改该值
  • 调整滤波beta参数：默认beta值一般在0.03~0.1之间，静止使用场景可适当调小beta降低加速度计/磁力计的修正权重，避免过度修正导致姿态被强行拉回0°
• 3. 修正坐标系匹配问题
  你代码中已经对加速度计、陀螺仪、磁力计的轴做了交换映射，请确认映射后的坐标系符合Madgwick滤波的约定：
  • 加速度计Z轴向上为正，陀螺仪逆时针转动为正
  • 磁力计坐标系和加速度计、陀螺仪坐标系严格对齐，若使用地磁数据建议先完成磁力计的硬铁/软铁校准
• 4. 修正四元数转欧拉角的计算逻辑
  不要直接使用四元数的quat.x分量转换角度，需使用标准的四元数转欧拉角公式，避免万向锁同时得到正确的360°范围角度，示例代码：

const q = quat;
// 绕X轴旋转角度roll 范围-180°~180°
const sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
const cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
const roll = Math.atan2(sinr_cosp, cosr_cosp) * 57.295779513;

// 绕Y轴旋转角度pitch 范围-90°~90°
const sinp = 2 * (q.w * q.y - q.z * q.x);
let pitch;
if (Math.abs(sinp) >= 1) {
  pitch = (Math.sign(sinp) * Math.PI/2) * 57.295779513;
} else {
  pitch = Math.asin(sinp) * 57.295779513;
}

// 绕Z轴旋转角度yaw 范围-180°~180°
const siny_cosp = 2 * (q.w * q.z + q.x * q.y);
const cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
const yaw = Math.atan2(siny_cosp, cosy_cosp) * 57.295779513;

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