Adafruit LSM6DS3TR-C传感器校准与姿态解算技术求助

一、加速度计偏移寄存器写入方法 #

LSM6DS3TR-C的X_OFS_USR(0x73)、Y_OFS_USR(0x74)、Z_OFS_USR(0x75)是8位有符号补码寄存器，单位为对应量程下的1 LSB。浮点偏移值需按以下步骤转换写入：

1. 确定当前加速度计量程对应的灵敏度（如±2g量程对应16384 LSB/g）；
2. 将浮点偏移值（单位g）转换为LSB数：offset_lsb = offset_g * 灵敏度；
3. 将LSB数截断为8位有符号整数（超出-128~127范围需检查硬件安装）；
4. 写入对应寄存器（写入前需将加速度计设为待机模式，写入后恢复正常模式）。

示例代码：

// ±2g量程下，X轴校准偏移为-0.007g
float acc_x_offset_g = -0.007f;
int16_t acc_x_offset_lsb = (int16_t)(acc_x_offset_g * 16384.0f);
int8_t acc_x_offset_reg = (int8_t)acc_x_offset_lsb;
// 写入X_OFS_USR寄存器
I2C_WriteRegister(LSM6DS3_ADDR, 0x73, acc_x_offset_reg);

二、补码数据处理说明 #

传感器输出的加速度计、陀螺仪、磁力计数据均为16位有符号补码，可直接以int16_t类型读取，无需额外转换为无符号二进制。计算姿态时，可直接用整型运算，或除以灵敏度转换为物理单位（g、dps、高斯）后用浮点运算，只要保留符号即可。

示例读取代码：

int16_t acc_x;
uint8_t acc_x_l = I2C_ReadRegister(LSM6DS3_ADDR, 0x28);
uint8_t acc_x_h = I2C_ReadRegister(LSM6DS3_ADDR, 0x29);
acc_x = (int16_t)((acc_x_h << 8) | acc_x_l);
// 转换为物理单位（±2g量程）
float acc_x_g = acc_x / 16384.0f;

三、开机自动校准流程 #

1. 加速度计校准（静止状态） #

• 开机后等待3~5秒，确保传感器完全静止；
• 连续读取100次加速度计数据，取平均值作为偏移值；
• 将偏移值转换为LSB后写入偏移寄存器，校准后静止时X/Y轴应接近0g，Z轴接近±1g（取决于安装方向）。

2. 陀螺仪校准（静止状态） #

• 保持传感器静止，连续读取100次陀螺仪数据，取平均值作为偏移值；
• 可选择写入陀螺仪偏移寄存器，或在读取数据后实时减去偏移值（更灵活）。

3. 磁力计校准（全方向旋转） #

• 提示用户缓慢旋转传感器，覆盖所有空间方向（至少6个面）；
• 记录X/Y/Z轴的最大值与最小值；
• 计算硬铁偏移：offset = (max + min)/2；
• 计算软铁缩放因子：取各轴量程的平均值，以该值为基准修正各轴缩放比例。

四、Basic AHRS（互补滤波）参考代码 #

以下为适配LSM6DS3TR-C + LIS3MDL的互补滤波姿态融合代码：

#include <math.h>

// 传感器灵敏度参数（根据量程调整）
#define ACC_SENS 16384.0f  // ±2g
#define GYRO_SENS 131.0f   // ±250dps
#define MAG_SENS 6842.0f   // ±4高斯（LIS3MDL）

// 校准参数（开机校准后填充）
float acc_offset[3] = {0};
float gyro_offset[3] = {0};
float mag_offset[3] = {0};
float mag_scale[3] = {1.0f, 1.0f, 1.0f};

// 姿态数据
float pitch = 0.0f, roll = 0.0f, yaw = 0.0f;
const float dt = 0.01f;       // 10ms采样周期
const float alpha = 0.98f;    // 互补滤波系数，越接近1越信任陀螺仪

void AHRS_Update(int16_t acc_raw[3], int16_t gyro_raw[3], int16_t mag_raw[3]) {
    // 加速度计数据处理与姿态计算
    float acc_x = (acc_raw[0] - acc_offset[0]) / ACC_SENS;
    float acc_y = (acc_raw[1] - acc_offset[1]) / ACC_SENS;
    float acc_z = (acc_raw[2] - acc_offset[2]) / ACC_SENS;
    float acc_pitch = atan2(acc_y, sqrt(acc_x*acc_x + acc_z*acc_z)) * 180.0f / M_PI;
    float acc_roll = atan2(-acc_x, acc_z) * 180.0f / M_PI;

    // 陀螺仪积分更新姿态
    float gyro_x = (gyro_raw[0] - gyro_offset[0]) / GYRO_SENS;
    float gyro_y = (gyro_raw[1] - gyro_offset[1]) / GYRO_SENS;
    float gyro_z = (gyro_raw[2] - gyro_offset[2]) / GYRO_SENS;
    pitch += gyro_y * dt;
    roll += gyro_x * dt;
    yaw += gyro_z * dt;

    // 互补滤波融合加速度计数据
    pitch = alpha * pitch + (1 - alpha) * acc_pitch;
    roll = alpha * roll + (1 - alpha) * acc_roll;

    // 磁力计数据处理与Yaw修正
    float mag_x = (mag_raw[0] - mag_offset[0]) * mag_scale[0] / MAG_SENS;
    float mag_y = (mag_raw[1] - mag_offset[1]) * mag_scale[1] / MAG_SENS;
    float mag_z = (mag_raw[2] - mag_offset[2]) * mag_scale[2] / MAG_SENS;
    // 姿态补偿磁力计数据
    float mag_x_comp = mag_x * cos(pitch*M_PI/180.0f) + mag_z * sin(pitch*M_PI/180.0f);
    float mag_y_comp = mag_x*sin(roll*M_PI/180.0f)*sin(pitch*M_PI/180.0f) + mag_y*cos(roll*M_PI/180.0f) - mag_z*sin(roll*M_PI/180.0f)*cos(pitch*M_PI/180.0f);
    float mag_yaw = atan2(-mag_y_comp, mag_x_comp) * 180.0f / M_PI;
    // 融合Yaw
    yaw = alpha * yaw + (1 - alpha) * mag_yaw;
}

// 开机校准函数
void Sensor_Calibrate() {
    // 加速度计校准
    int32_t acc_sum[3] = {0};
    const int SAMPLES = 100;
    for(int i=0; i<SAMPLES; i++) {
        int16_t acc_raw[3];
        Read_Accelerometer(acc_raw); // 自行实现I2C读取函数
        acc_sum[0] += acc_raw[0];
        acc_sum[1] += acc_raw[1];
        acc_sum[2] += acc_raw[2];
        __delay_ms(10);
    }
    acc_offset[0] = acc_sum[0]/(float)SAMPLES;
    acc_offset[1] = acc_sum[1]/(float)SAMPLES;
    acc_offset[2] = acc_sum[2]/(float)SAMPLES - ACC_SENS; // Z轴补偿至1g

    // 陀螺仪校准
    int32_t gyro_sum[3] = {0};
    for(int i=0; i<SAMPLES; i++) {
        int16_t gyro_raw[3];
        Read_Gyroscope(gyro_raw);
        gyro_sum[0] += gyro_raw[0];
        gyro_sum[1] += gyro_raw[1];
        gyro_sum[2] += gyro_raw[2];
        __delay_ms(10);
    }
    gyro_offset[0] = gyro_sum[0]/(float)SAMPLES;
    gyro_offset[1] = gyro_sum[1]/(float)SAMPLES;
    gyro_offset[2] = gyro_sum[2]/(float)SAMPLES;

    // 磁力计校准
    int16_t mag_max[3] = {-32768, -32768, -32768};
    int16_t mag_min[3] = {32767, 32767, 32767};
    __delay_ms(5000); // 等待用户旋转传感器
    for(int i=0; i<500; i++) {
        int16_t mag_raw[3];
        Read_Magnetometer(mag_raw);
        mag_max[0] = mag_raw[0]>mag_max[0] ? mag_raw[0] : mag_max[0];
        mag_min[0] = mag_raw[0]<mag_min[0] ? mag_raw[0] : mag_min[0];
        mag_max[1] = mag_raw[1]>mag_max[1] ? mag_raw[1] : mag_max[1];
        mag_min[1] = mag_raw[1]<mag_min[1] ? mag_raw[1] : mag_min[1];
        mag_max[2] = mag_raw[2]>mag_max[2] ? mag_raw[2] : mag_max[2];
        mag_min[2] = mag_raw[2]<mag_min[2] ? mag_raw[2] : mag_min[2];
        __delay_ms(10);
    }
    // 硬铁偏移
    mag_offset[0] = (mag_max[0]+mag_min[0])/2.0f;
    mag_offset[1] = (mag_max[1]+mag_min[1])/2.0f;
    mag_offset[2] = (mag_max[2]+mag_min[2])/2.0f;
    // 软铁缩放
    float mag_range[3] = {(mag_max[0]-mag_min[0])/2.0f, (mag_max[1]-mag_min[1])/2.0f, (mag_max[2]-mag_min[2])/2.0f};
    float avg_range = (mag_range[0]+mag_range[1]+mag_range[2])/3.0f;
    mag_scale[0] = avg_range/mag_range[0];
    mag_scale[1] = avg_range/mag_range[1];
    mag_scale[2] = avg_range/mag_range[2];
}

五、姿态跳变问题排查要点 #

1. 校准有效性：确认加速度计/陀螺仪校准时传感器完全静止，磁力计校准覆盖足够多空间方向；
2. 采样周期精度：用定时器触发采样，确保dt值准确，避免固定延迟导致的积分误差；
3. 滤波系数调整：若跳变严重，减小alpha值（如0.95），提升加速度计/磁力计的修正权重；
4. 数据读取正确性：检查I2C寄存器地址、设备地址是否匹配，避免读取错误数据；
5. 量程匹配：确保代码中灵敏度参数与传感器实际设置的量程一致（如±4g量程对应灵敏度为8192 LSB/g）。

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