# 传感器校准方案
一、陀螺仪校准方案
延续 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;
}
以上代码目的是计算合力,以下以存在俯仰角度 15 度偏差为例,进行分析
飞机方向图:
分析图:
三、激光传感器校准方案
请参考: 07-激光传感器VL53L1x移植与调试-附源码
