04 Sensor Calibration

# 传感器校准方案

一、陀螺仪校准方案

延续 Crazyflie2 陀螺仪校准方案,在初次上电时,计算陀螺仪三个轴的方差与平均值.

使用一个最大长度为 1024 的环形缓冲区,存储最新的 1024 组陀螺仪测量值
通过计算陀螺仪输出值方差,确认飞机已经放置平稳并且陀螺仪工作正常.
确认第 2 步正常后,计算静止时 1024 组陀螺仪输出值的平均值,作为陀螺仪的校准值

陀螺仪偏差计算源代码:

/**
 * Adds a new value to the variance buffer and if it is full
 * replaces the oldest one. Thus a circular buffer.
 */
static void sensorsAddBiasValue(BiasObj* bias, int16_t x, int16_t y, int16_t z)
{
  bias->bufHead->x = x;
  bias->bufHead->y = y;
  bias->bufHead->z = z;
  bias->bufHead++;

  if (bias->bufHead >= &bias->buffer[SENSORS_NBR_OF_BIAS_SAMPLES])
  {
    bias->bufHead = bias->buffer;
    bias->isBufferFilled = true;
  }
}

/**
 * Checks if the variances is below the predefined thresholds.
 * The bias value should have been added before calling this.
 * @param bias  The bias object
 */
static bool sensorsFindBiasValue(BiasObj* bias)
{
  static int32_t varianceSampleTime;
  bool foundBias = false;

  if (bias->isBufferFilled)
  {
    sensorsCalculateVarianceAndMean(bias, &bias->variance, &bias->mean);

    if (bias->variance.x < GYRO_VARIANCE_THRESHOLD_X &&
        bias->variance.y < GYRO_VARIANCE_THRESHOLD_Y &&
        bias->variance.z < GYRO_VARIANCE_THRESHOLD_Z &&
        (varianceSampleTime + GYRO_MIN_BIAS_TIMEOUT_MS < xTaskGetTickCount()))
    {
      varianceSampleTime = xTaskGetTickCount();
      bias->bias.x = bias->mean.x;
      bias->bias.y = bias->mean.y;
      bias->bias.z = bias->mean.z;
      foundBias = true;
      bias->isBiasValueFound = true;
    }
  }

  return foundBias;
}

校准陀螺仪输出值:

    sensorData.gyro.x = (gyroRaw.x - gyroBias.x) * SENSORS_DEG_PER_LSB_CFG;
    sensorData.gyro.y = (gyroRaw.y - gyroBias.y) * SENSORS_DEG_PER_LSB_CFG;
    sensorData.gyro.z = (gyroRaw.z - gyroBias.z) * SENSORS_DEG_PER_LSB_CFG;
    applyAxis3fLpf((lpf2pData *)(&gyroLpf), &sensorData.gyro); //低通滤波器,去除高频干扰

二、加速度计校准方案

1. 重力加速度校准

由于在不同地球纬度和海拔下,重力加速度 g 值不同,需要使用加速度计对 g 进行实际测试,用于校准传感器.参考 Crazyflie2 加速度计校准方案:

陀螺仪校准完成后,立刻进行加速度计校准.
使用 buffer 保存 200 组加速度计测量值
通过合成重力加速度在三个轴的分量,计算重力加速度在静止状态下的值.

参考:不同地球纬度和海拔下的不同重力加速度值 g

计算静止状态下重力加速度值:

/**
 * Calculates accelerometer scale out of SENSORS_ACC_SCALE_SAMPLES samples. Should be called when
 * platform is stable.
 */
static bool processAccScale(int16_t ax, int16_t ay, int16_t az)
{
    static bool accBiasFound = false;
    static uint32_t accScaleSumCount = 0;

    if (!accBiasFound)
    {
        accScaleSum += sqrtf(powf(ax * SENSORS_G_PER_LSB_CFG, 2) + powf(ay * SENSORS_G_PER_LSB_CFG, 2) + powf(az * SENSORS_G_PER_LSB_CFG, 2));
        accScaleSumCount++;

        if (accScaleSumCount == SENSORS_ACC_SCALE_SAMPLES)
        {
            accScale = accScaleSum / SENSORS_ACC_SCALE_SAMPLES;
            accBiasFound = true;
        }
    }

    return accBiasFound;
}

通过重力加速度值,校准加速度计测量值:

    accScaled.x = (accelRaw.x) * SENSORS_G_PER_LSB_CFG / accScale;
    accScaled.y = (accelRaw.y) * SENSORS_G_PER_LSB_CFG / accScale;
    accScaled.z = (accelRaw.z) * SENSORS_G_PER_LSB_CFG / accScale;

2. 水平校准

理想状态下,加速度传感器在飞机上完全水平的进行安装,进而可以使用 0 位置作为飞机的水平面,但是由于加速度计在安装时不可避免的存在一定的倾角,导致飞控错误的估计水平位置,导致飞机向某个方向偏飞.因此需要设置一定的校准策略来平衡这种误差.

将飞机放置在一个水平面上,计算飞机 cosRoll sinRoll cosPitch sinPitch .理想状态下 cosRoll cosPitch 为 1 ,sinPitch sinRoll 为 0 .如果不是水平安装sinPitch sinRoll 不为 0,cosRoll cosPitch 不为 1 .
将步骤 1 的 cosRoll sinRoll cosPitch sinPitch 或对应的 Roll Pitch 角度值保存到飞机,用于校准.

加速度计水平校准:

/**
 * Compensate for a miss-aligned accelerometer. It uses the trim
 * data gathered from the UI and written in the config-block to
 * rotate the accelerometer to be aligned with gravity.
 */
static void sensorsAccAlignToGravity(Axis3f *in, Axis3f *out)
{
    //TODO: need cosPitch calculate firstly
    Axis3f rx;
    Axis3f ry;

    // Rotate around x-axis
    rx.x = in->x;
    rx.y = in->y * cosRoll - in->z * sinRoll;
    rx.z = in->y * sinRoll + in->z * cosRoll;

    // Rotate around y-axis
    ry.x = rx.x * cosPitch - rx.z * sinPitch;
    ry.y = rx.y;
    ry.z = -rx.x * sinPitch + rx.z * cosPitch;

    out->x = ry.x;
    out->y = ry.y;
    out->z = ry.z;
}