icm20602校准方法及代码示例

加速度计校准方法及代码示例

实验平台

芯片：icm20602+stm32f7
软件：keil5、匿名上位机v4.34

加速度计、陀螺仪校准意义

MEMS（微机电系统）器件由于制造工艺精度问题，都会存在一定的误差。对传感器进行校准可以消除传感器本身或制造过程中的误差和偏差。通过校准，可以提高传感器的精度和可靠性，确保其输出与实际运动和姿态一致，提高姿态估计的准确性。

陀螺仪校准原理及实现

原理

陀螺仪测量的是三轴的角速度。静止情况下，测得的三轴角速度应该为0。但实际情况是三轴的角速度都有一个零点偏移，时间一长，累积出的角度便会越来越飘。因此陀螺仪校准的方法是：上电后的一段静止时间内，采集几百组陀螺仪数据，进行取平均，得到三轴的零点偏移值。之后获取到陀螺仪数据后都要减去零点偏移值 。

注：零点偏移会随着温度的变化而变化，如果校准要考虑温度因素的话，解决方法有2种，一是保持传感器温度不变，二是进行温度补偿标定。本文icm20602使用的环境温度变化不大，因此没有考虑温度因素。

代码实现

/*代码示例，不一定可以直接运行*/
/*
GyrRaw[X]为陀螺仪原始值，GyrOff[X]为零点偏移值。
*/
#define GYR_SAMPLE_NUM 200
void GyrCali(void){
    static bool isCaliFlag = 0;
    static int count = 0;
    if(!isCaliFlag && count < GYR_SAMPLE_NUM){
        GyrSum[X] += GyrRaw[X];
        GyrSum[Y] += GyrRaw[Y];
        GyrSum[Z] += GyrRaw[Z];
        count ++;
        if(count == GYR_SAMPLE_NUM){
            GyrOff[X] = GyrSum[X]/(float)GYR_SAMPLE_NUM;
            GyrOff[Y] = GyrSum[Y]/(float)GYR_SAMPLE_NUM;
            GyrOff[Z] = GyrSum[Z]/(float)GYR_SAMPLE_NUM;
            isCaliFlag = true;
        }
    }
    
}

加速度计校准原理及实现

原理

加速度计的校准不使用陀螺仪的校准方法：水平静止一段时间后减去零点偏移。如果使用，则无人机在斜坡起飞时，会认为倾斜时才是水平面，导致无人机无法正常飞行。加速度计的校准采用重力参考标定 方法。核心思想是静态放置情况下，无论加速度计的位置在哪，所测的的加速度模值始终应该是当地重力加速度。 即
$$acc{x}^2+acc{y}^2+acc{z}^2=G^2（1）$$
加速度计校准主要校准比例系数和零位偏移，建立数学模型。其中accx_m为加速度计的测量值，acc为校准后的加速度计的值。
$$accx=a1\ast acc{x}_m+a2（2）$$
$$accy=a3\ast acc{y}_m+a4（3）$$
$$accz=a5\ast acc{z}_m+a6（4）$$
将上述公式代入公式（1）中，并在matlab中使用lsqcurvefit函数进行最小二乘拟合，求出a1-a6参数，完成加速度计的校准。该方法只需要校准一次，之后从加速度计获取数值后经过公式（2）-（4）获得校准值。
相关寄存器设置
在icm20602初始化时，需要设置加速度计的量程，即设置ACCEL_CONFIG 寄存器中的[4:3]位。本文使用的量程是正负8g,对应的[4:3]=10。在代码中体现为：

Icm20602_Spi_Tran(ele->hspi,MPU_RA_ACCEL_CONFIG,(2 << 3)); //加速度计量程正负8g

2<<3即为10000，10左移3位。

加速度计的量程会影响分辨率，从数据手册可以知道正负8g量程对应的分辨率为4096 LSB/g。

加速度计返回的值范围从-31768~+32767。在加速度计量程为正负8g情况下，假如加速度计返回的值为x，那么将x换算成加速度的单位（m/s^2），公式为：
$$ac{c}_m=\frac{x}{4096}\ast G=x\ast 0.0023877$$

数据采集

为了从飞控获取加速度计的原始测量值，可以通过匿名协议 将3个数据通过发到至PC端的上位机软件：匿名上位机。飞控中如何移植匿名协议不再赘述，请自行查阅网上资料。
在匿名上位机软件中可以通过EXCEL功能保存数据到csv文件中以便后续进行数据分析。

在采集数据过程中，最好是缓慢地旋转飞控板6个面并保持一段时间 。采集回的数据如下图表格所示，数据经过100倍有放大。利用excel功能画出三轴数据的变化曲线，选择突变较为严重的区间并删除数据，保留六个面静止时的数据即可。

matlab校准算法

代码如下：

clear;clc;


acc_measure = readmatrix('acc_data_tobecali.csv'); %包含采集的三轴加速度计数据，最好是6个面进行采集
acc_measure = acc_measure/100;% 转化为m/s^2
G=9.78*9.78* ones(size(acc_measure, 1),1);

% 创建选项结构体并设置'FunctionTolerance'的值
options = optimoptions('lsqcurvefit', 'FunctionTolerance', 1e-1);

f=@(a,acc_measure)(a(1)*acc_measure(:,1)+a(2)).^2+(a(3)*acc_measure(:,2)+a(4)).^2+(a(5)*acc_measure(:,3)+a(6)).^2;
a0=[1 0 1 0 1 0];
a=lsqcurvefit(f,a0,acc_measure,G,[],[],options);

%% 生成画图数据
%原始数据的G
raw_G = ones(size(acc_measure, 1),1);
raw_G(:,1) = sqrt(acc_measure(:, 1).^2+acc_measure(:, 2).^2+acc_measure(:, 3).^2);

%校正后的acc数据
acc = zeros(size(acc_measure, 1),size(acc_measure, 2));
acc(:, 1) = a(1)*acc_measure(:,1)+a(2);
acc(:, 2) = a(3)*acc_measure(:,2)+a(4);
acc(:, 3) = a(5)*acc_measure(:,3)+a(6);
%校正后的G值
cali_G = sqrt(acc(:, 1).^2+acc(:, 2).^2+acc(:, 3).^2);

%% 画图
x = 1:size(acc_measure, 1);

% 分块绘制数据
figure;
subplot(2, 2, 1);  % 第一个块
plot(x,acc_measure(:,1),'b',x,acc(:,1),'r');
title('X-axis-acc');
legend('acc_{measure}','acc');

subplot(2, 2, 2);  % 第二个块
plot(x,acc_measure(:,2),'b',x,acc(:,2),'r');
title('Y-axis-acc');
legend('acc_{measure}','acc');

subplot(2, 2, 3);  % 第三个块
plot(x,acc_measure(:,3),'b',x,acc(:,3),'r');
title('Z-axis-acc');
legend('acc_{measure}','acc');

subplot(2, 2, 4);  % 第四个块
plot(x,raw_G,'b',x,cali_G(:,1),'r');
title('G-value');
legend('G_{raw}','G_{cali}');

校准结果

程序运行后，a矩阵即为6个待求的参数。校准后G(红线)值更加接近9.78，校准算法起了作用！。

图为校准前后的三轴加速度及G值对比

图为校准前后的G值对比

参考资料

https://zhuanlan.zhihu.com/p/24452753
https://blog.csdn.net/weixin_50622833/article/details/124869778