【IMU】6轴数据校准算法

算法概述

本算法用于惯性测量单元的自动校准，通过多次采样和优化选择，确定传感器的最佳偏移量。

算法步骤

1. 初始化

• converged = false（加速度计收敛标志）
• new_offset = {0,0,0}（最优偏移量）
• last_average = {0,0,0}（上次平均值）

2. 主循环（重复CALIB_CYCLES次）

   FOR i = 1 to CALIB_CYCLES:
   	步骤1.1: 采集50次传感器数据，计算平均值this_avg
    	步骤1.2: 计算与上次平均值的差异this_diff = norm(last_avg - this_avg)
    	步骤1.3: 判断收敛，和最佳值更新
    	步骤2.4: 更新last_average = this_avg
   
   

   步骤1.3: 判断收敛，和最佳值更新:
   	IF this_diff < 1.0:
       - 更新滑动平均：last_average = 0.5 × this_avg + 0.5 × last_average
       - 如果首次收敛或新值更优，更新 new_offset = last_average
       - 设置 converged = true
   	else if (this_diff < best_diff):  // 未收敛但更优
       - 更新最优平均值：best_avg = 0.5×acc_avg + 0.5×acc_last_average
       - 更新最优差值：best_diff = this_diff
   

如果收敛则取new_offset作为最后的offset值，否则使用best_avg作为offset值

代码

最后得到的6轴校准代码如下，参考了《多旋翼无人机嵌入式飞控开发实战》（奚海蛟，叶贵强）。这里做了部分修改，主要是对两个参数添加了修正：

• 异常检测：在该步骤中，(240 / Utils::getAccelerometerRange())这个边界值是一个经验值，Utils::getAccelerometerRange()获取加速度量程的半值，比如±2g量程取2.
• 收敛判断：在两个仪器的“步骤1.3: 判断收敛，和最佳值更新”中，引入了acc_threshold和gyro_threshold，这是根据量程，采样率ODR，和均方根误差RMS-noise计算出来的。具体的计算方法在下一节说明

    uint8_t BMI2700::_Calibration()
       {
           if (!this->_calibrating)
           {
               this->_calibrating = true;
   
               bool acc_converged = false;     // 加速度聚合状态
               bool gyro_converged = false;    // 角速度聚合状态
               Vector3f acc_best_avg = {0};    // 50次测量中最优加速度
               Vector3f gyro_best_avg = {0};   // 50次测量中最优角速度
               Vector3f new_acc_offset = {0};  // 加速度最终需要修正的偏移量
               Vector3f new_gyro_offset = {0}; // 角速度最终需要修正的偏移量
   
               Vector3f acc_last_average = {0};  // 上50次测量的角速度平均值
               Vector3f gyro_last_average = {0}; // 上50次测量的角速度平均值
               Vector3l acc_raw_sum = {0};       // 加速度原始值累计和
               Vector3l gyro_raw_sum = {0};      // 角速度原始值累积和
               Vector3i acc_start = {0};         // 每50次数据读取前加速度状态
               Vector3i acc_raw = {0};           // 每50次数据读取前加速度状态
               Vector3i gyro_start = {0};        // 每50次数据读取前角速度状态
               Vector3i gyro_raw = {0};          // 每50次数据读取前角速度状态
               Vector3f acc_avg = {0};           // 50次累计加速度均值
               Vector3f gyro_avg = {0};          // 50次累计角速度均值
   
               uint8_t sum_cnt = 0;      // 50次的累加计数器
               float acc_best_diff = 0;  // 加速度最优方差
               float gyro_best_diff = 0; // 角速度最优方差
               float acc_diff_norm = 0;  // 加速度方差
               float gyro_diff_norm = 0; // 角速度方差
               float acc_threshold = Utils::calculate_acc_convergence_threshold(50);
               float gyro_threshold = Utils::calculate_gyro_convergence_threshold(50);
               this->_log("acc_threshold: %f\r\n", acc_threshold);
               this->_log("gyro_threshold: %f\r\n", gyro_threshold);
   
               this->_delay_us(1000000);
               for (int count = 0; count < 120; count++)
               {
                   /* 累计50次坐标和 */
                   acc_raw_sum.x = acc_raw_sum.y = acc_raw_sum.z = gyro_raw_sum.x = gyro_raw_sum.y = gyro_raw_sum.z = 0;
                   /* 记录起始坐标 */
                   this->read_gyro_accel(&acc_start, &gyro_start);
                   // this->_log("New acc offset: %d, %d, %d\r\n", acc_start.x,acc_start.y,acc_start.z);
                   for (sum_cnt = 0; sum_cnt < 50; sum_cnt++)
                   {
                       this->read_gyro_accel(&acc_raw, &gyro_raw);
                       acc_raw_sum.x += acc_raw.x;
                       acc_raw_sum.y += acc_raw.y;
                       acc_raw_sum.z += acc_raw.z;
                       gyro_raw_sum.x += gyro_raw.x;
                       gyro_raw_sum.y += gyro_raw.y;
                       gyro_raw_sum.z += gyro_raw.z;
                       this->_delay_us(1000);
                   }
                   float test = sqrtf3(acc_raw.x - acc_start.x, acc_raw.y - acc_start.y, acc_raw.z - acc_start.z);
                   if (test > (240 / Utils::getAccelerometerRange()))
                   {
                       continue; // 采样50次后允许加速度方差最大值（实际测量值）
                   }
   
                   acc_avg.x = acc_raw_sum.x / (float)sum_cnt;
                   acc_avg.y = acc_raw_sum.y / (float)sum_cnt;
                   acc_avg.z = acc_raw_sum.z / (float)sum_cnt;
                   gyro_avg.x = gyro_raw_sum.x / (float)sum_cnt;
                   gyro_avg.y = gyro_raw_sum.y / (float)sum_cnt;
                   gyro_avg.z = gyro_raw_sum.z / (float)sum_cnt;
   
                   acc_diff_norm = sqrtf3((acc_last_average.x - acc_avg.x), (acc_last_average.y - acc_avg.y), (acc_last_average.z - acc_avg.z));
                   this->_log("acc_diff_norm: %f\r\n", acc_diff_norm);
                   gyro_diff_norm = sqrtf3((gyro_last_average.x - gyro_avg.x), (gyro_last_average.y - gyro_avg.y), (gyro_last_average.z - gyro_avg.z));
   
                   if (count == 0)
                   {
                       acc_best_diff = acc_diff_norm;
                       acc_best_avg.x = acc_avg.x;
                       acc_best_avg.y = acc_avg.y;
                       acc_best_avg.z = acc_avg.z;
                       gyro_best_diff = gyro_diff_norm;
                       gyro_best_avg.x = gyro_avg.x;
                       gyro_best_avg.y = gyro_avg.y;
                       gyro_best_avg.z = gyro_avg.z;
                   }
                   else
                   {
                     /* 步骤1.3: 判断收敛，和最佳值更新 */
                       if (acc_diff_norm < acc_threshold) // 加速度方差
                       {
                           acc_last_average.x = (acc_avg.x * 0.5f) + (acc_last_average.x * 0.5f);
                           acc_last_average.y = (acc_avg.y * 0.5f) + (acc_last_average.y * 0.5f);
                           acc_last_average.z = (acc_avg.z * 0.5f) + (acc_last_average.z * 0.5f);
                           if (!acc_converged || (sqrtf3(acc_last_average.x, acc_last_average.y, acc_last_average.z) < sqrtf3(new_acc_offset.x, new_acc_offset.y, new_acc_offset.z)))
                           {
                               acc_converged = true;
                               new_acc_offset.x = acc_last_average.x;
                               new_acc_offset.y = acc_last_average.y;
                               new_acc_offset.z = acc_last_average.z;
                               this->_log("new_acc_offset: %f, %f, %f\r\n", new_acc_offset.x, new_acc_offset.y, new_acc_offset.z);
                           }
                       }
                       else if (acc_diff_norm < acc_best_diff)
                       {
                           acc_best_diff = acc_diff_norm;
                           acc_best_avg.x = (acc_avg.x * 0.5f) + (acc_last_average.x * 0.5f);
                           acc_best_avg.y = (acc_avg.y * 0.5f) + (acc_last_average.y * 0.5f);
                           acc_best_avg.z = (acc_avg.z * 0.5f) + (acc_last_average.z * 0.5f);
                       }
                       acc_last_average.x = acc_avg.x;
                       acc_last_average.y = acc_avg.y;
                       acc_last_average.z = acc_avg.z;
   										/* 步骤1.3: 判断收敛，和最佳值更新 */
                       if (gyro_diff_norm < gyro_threshold) // 角速度方差
                       {
                           gyro_last_average.x = (gyro_avg.x * 0.5f) + (gyro_last_average.x * 0.5f);
                           gyro_last_average.y = (gyro_avg.y * 0.5f) + (gyro_last_average.y * 0.5f);
                           gyro_last_average.z = (gyro_avg.z * 0.5f) + (gyro_last_average.z * 0.5f);
                           if (!gyro_converged || (sqrtf3(gyro_last_average.x, gyro_last_average.y, gyro_last_average.z) < sqrtf3(new_gyro_offset.x, new_gyro_offset.y, new_gyro_offset.z)))
                           {
                               gyro_converged = true;
                               new_gyro_offset.x = gyro_last_average.x;
                               new_gyro_offset.y = gyro_last_average.y;
                               new_gyro_offset.z = gyro_last_average.z;
                               this->_log("new_gyro_offset: %f, %f, %f\r\n", new_gyro_offset.x, new_gyro_offset.y, new_gyro_offset.z);
                           }
                       }
                       else if (gyro_diff_norm < gyro_best_diff)
                       {
                           gyro_best_diff = gyro_diff_norm;
                           gyro_best_avg.x = (gyro_avg.x * 0.5f) + (gyro_last_average.x * 0.5f);
                           gyro_best_avg.y = (gyro_avg.y * 0.5f) + (gyro_last_average.y * 0.5f);
                           gyro_best_avg.z = (gyro_avg.z * 0.5f) + (gyro_last_average.z * 0.5f);
                       }
                       gyro_last_average.x = gyro_avg.x;
                       gyro_last_average.y = gyro_avg.y;
                       gyro_last_average.z = gyro_avg.z;
                   }
               }
   
               if (!acc_converged)
               {
                   // xQueueSendToBack(logQueue, "acc did not converge\r\n", LOG_WAIT);
                   this->_acc_offset.x = acc_best_avg.x;
                   this->_acc_offset.y = acc_best_avg.y;
                   this->_acc_offset.z = acc_best_avg.z - this->_acc_resolution;
               }
               else
               {
                   // xQueueSendToBack(logQueue, "acc calibrate success\r\n", LOG_WAIT);
                   this->_acc_offset.x = new_acc_offset.x;
                   this->_acc_offset.y = new_acc_offset.y;
                   this->_acc_offset.z = new_acc_offset.z - this->_acc_resolution;
               }
               if (!gyro_converged)
               {
                   // xQueueSendToBack(logQueue, "gyro did not converge\r\n", LOG_WAIT);
                   this->_gyr_offset.x = gyro_best_avg.x;
                   this->_gyr_offset.y = gyro_best_avg.y;
                   this->_gyr_offset.z = gyro_best_avg.z;
               }
               else
               {
                   // xQueueSendToBack(logQueue, "gyro calibrate success\r\n", LOG_WAIT);
                   this->_gyr_offset.x = new_gyro_offset.x;
                   this->_gyr_offset.y = new_gyro_offset.y;
                   this->_gyr_offset.z = new_gyro_offset.z;
               }
               // Param_SaveAccelOffset(&mpu6050.acc_offset);
               // Param_SaveGyroOffset(&mpu6050.gyro_offset);
               this->_calibrating = false;
               return 0;
           }
           return 0;
       }

收敛阈值计算

首先明确收敛的定义。

对于一个静止放置的IMU单元，理想状态下，它的值应该是稳定于一个定量的。而现实情况是，在环境噪声，元件内部漂移，出厂时的内部校准误差等的影响下，这个值会加上一个噪声。这个噪声的大小用均方差（RMS）表示，其量纲与被测量一致，这个值通常在传感元件的参考手册中给出，并且在不同的采样频率下有不同的值，用*RMS-noise(?Hz)*表示。

也就是说，只要判断仪器的输出量的RMS与该值基本一致就可以判断其处于稳定状态。那么应该怎么确定两个值基本一致呢？用统计和概率的角度来看，噪声可以看作是以定量为中心，以*RMS-noise(?Hz)为方差的正态分布事件，既然是正态分布，那么就可以使用$3\sigma$法则。因此可以使用3倍RMS-noise(?Hz)*作为阈值。

当然，这个值的单位是物理单位，与从传感器直接得到的不同，是不能直接用作传感器读数的校准的。这两个值的转换系数可以从参考手册中得到，也称为Sensitivity，也可自己计算。在这里转换系数用$S_{LSBperG}$表示，*RMS-noise(?Hz)*用$RM{S}_{noise}$表示，转换后的阈值用$T_{LSB}$表示则转换公式如下：
$$T_{LSB}=RM{S}_{noise}\times S_{LSBperG}\times 3$$
这里用量纲阐明其含义：
$$LSB=g\times (LSB/g)\times 3$$
单位LSB指最低有效位，在这里指在传感数据对应寄存器中的单位1