关于磁力计偏置值的标定实践
关于磁力计偏置值的标定实践
- 前言
- 采集数据
- 处理数据
- 后记
前言
前段时间上闲鱼入手了SKYE ORBIT无人机套装，因为没有APP，飞不起来，商家都打折甩手卖出去。我倒腾了半天，最后还是决定换个飞控，原本的模块暂时没有换，把电调的线给拉了出来，凑了一下手头上的stm32的开发板，上面刚好有一个MPU9250，所以就决定在闲暇时间自己写飞控了。使用Mathony算法解算姿态时发现加入了磁力计，Yaw角反而变得奇奇怪怪的。于是上网搜了一波，应该是没有对磁力计进行矫正的原因，所以就有了这篇博客。先上数据，感兴趣的同学自行百度下载。
链接：https://pan.baidu.com/s/16yPr0g-RRif5M-lGoMUdFQ
提取码：0cgu
主要参考的博客有：
- https://blog.csdn.net/qq_31073871/article/details/87008583
- https://blog.csdn.net/shenshikexmu/article/details/70143455
- https://blog.csdn.net/qq_39554681/article/details/89527267
采集数据
多说无益，献上采集结果：
怎么采集数据是个大问题，需要两方面的准备：
- 工具
首先需要有一个方便好用的能够记录单片机通过I2C接口读取到的磁力计XYZ轴的数据。噔噔蹬蹬！匿名地面站，这个地面站or上位机现在已经是v6.5版本了，我看到网上有2.x版本的随便下载，所以这里我提供给大家我以前做四轴的时候下载的v1.4版本以及串口传输协议的总结。
链接：https://pan.baidu.com/s/1nIZyPpm93HJLGYtscjrhVw
提取码：etg8
单片机的串口发送程序大家自己琢磨琢磨，留点实践的空间给大家，tips如下：
#define __Byte(_temp,i) (*((char *)(&_temp) + i))
temp[cnt++]=__Byte(GCS_STATUS.ROL,1);
temp[cnt++]=__Byte(GCS_STATUS.ROL,0);
地面站的使用方式，先打开exe文件之后，选择进入“开发者版本“， 然后打开高级收吗，之后点开“Excel”写入，勾选需要保存的数据，记得要修改写入频率，500Hz太高了。最后打开串口，采集完数据之后关闭串口就ok了。之后就是处理数据的欢乐时光了。
2. 采集装置
我们需要的是能够让MPU9250绕着中心旋转，从上面我推荐的博客文章里面提到的链接（把传感器固定在足球的表面，旋转球面记录数据）可以知道，如果想要很好地补偿，需要令采集的数据围绕着一个点转动。在这里我使用的是遥控器的手机支架夹住不是“飞控板“的临时飞控板（其实是个小车的板子）……，然后接上串口线，打开上位机，转动万向节，记录数据。如下图所示：
爱摄影的也可以使用三脚架，将板子固定在三脚架的水平台上转动。
处理数据
处理数据就没有太多可以说的了，因为比较简单，得到csv文件之后，在matlab中使用load函数加载数据，然后按照前言中提到的博客的方法，先使用最小二乘法，其实就是伪逆，求出对应的拟合方程的系数，然后按照系数之间的数学关系求解出中心偏置值，这三个值将用于后续的代码中。
tips:
MPU9250的磁力计有出厂设置的标定值，可用于后续读取使用。
其中，前面的offset就是我们的标定值
之所以没有用到x,y,z轴长的校正，是因为经过程序处理后，磁力计读取的xyz值构成的散点图相对来说比较圆，所以只需要修改中心点就行了，不需要对轴长进行伸缩。
matlab处理代码如下：
clear; clc;
load('imu_data3.csv');
plt_t = (0:length(imu_data3)-1)/500;
imu_data = imu_data3;
%%
ax = imu_data(:,1);ay = imu_data(:,2);az = imu_data(:,3);
gx = imu_data(:,4);gy = imu_data(:,5);gz = imu_data(:,6);
mx = imu_data(:,7);my = imu_data(:,8);mz = imu_data(:,9);
figure(1);
subplot(311);
plot(plt_t,ax,'LineWidth',1.5); hold on
plot(plt_t,ay,'LineWidth',1.5);
plot(plt_t,az,'LineWidth',1.5); hold off;
subplot(312);
plot(plt_t,gx,'LineWidth',1.5); hold on
plot(plt_t,gy,'LineWidth',1.5);
plot(plt_t,gz,'LineWidth',1.5); hold off;
subplot(313);
plot(plt_t,mx,'LineWidth',1.5); hold on
plot(plt_t,my,'LineWidth',1.5);
plot(plt_t,mz,'LineWidth',1.5); hold off;
%%
len = length(imu_data);
onevec = ones(len,1);
inc = zeros(3,1);
mx_ = mx; my_ = my; mz_ = mz;
for loop=1:5
Phi = [mx.*mx, my.*my, mz.*mz, mx.*my, mx.*mz, my.*mz, mx, my, mz];
P = pinv(Phi)*onevec;
M=[P(1) P(4)/2 P(5)/2;...
P(4)/2 P(2) P(6)/2;...
P(5)/2 P(6)/2 P(3)];
Center=-1/2*[P(7),P(8),P(9)]/M;
SS=Center*M*Center'+1;
[U,V]=eig(M); %Matlab计算特征值是大小顺序
[~,n1]=max(abs(U(:,1))); %输出的，但和xyz轴顺序不
[~,n2]=max(abs(U(:,2))); %不同，这个操作就是让特征
[~,n3]=max(abs(U(:,3))); %值和xyz轴对应上。
lambda(n1)=V(1,1); lambda(n2)=V(2,2); lambda(n3)=V(3,3);
Scale_axis=[sqrt(SS/lambda(1)),sqrt(SS/lambda(2)),sqrt(SS/lambda(3))];
Center=round(Center);
Scale_axis=round(Scale_axis);
disp(['magnetics loop:----' num2str(loop)]);
x0 = Center(1); disp(['x0:' num2str(x0)]);
y0 = Center(2); disp(['y0:' num2str(y0)]);
z0 = Center(3); disp(['z0:' num2str(z0)]);
sx0 = Scale_axis(1); disp(['sx0:' num2str(sx0)]);
sy0 = Scale_axis(2); disp(['sy0:' num2str(sy0)]);
sz0 = Scale_axis(3); disp(['sz0:' num2str(sz0)]);
disp('------loop end-------');
mx = mx-x0; my = my-y0; mz = mz-z0;
inc = inc + [x0 y0 z0]';
end
mx = mx_; my = my_; mz = mz_;
x0 = inc(1); disp(['mag_x0:' num2str(x0)]);
y0 = inc(2); disp(['mag_y0:' num2str(y0)]);
z0 = inc(3); disp(['mag_z0:' num2str(z0)]);
figure(2);
scatter3(mx,my,mz,'.'); hold on;
scatter3(mx-x0,my-y0,mz-z0,'r.');
axis equal; hold off;
需要特别注意的是，这里我求伪逆的时候使用了loop的循环结构，有一点点闭环控制的意思，大家可以把这个for循环去掉，仔细琢磨一下为什么我要这么做，原因其实比较简单。最后是修正的效果图：
红色是偏置后的散点图，我们可以从XY,XZ,YZ三个方向更直观地查看效果：
可见，三个方向的原点都基本是散点图的中心，偏置值基本正确。
后记
一般而言IMU模块主要包含三种传感器，（1）对噪声比较敏感的加速度计，求解角度准确但运动时噪声多。（2）陀螺仪，通过积分求解角度，对噪声不敏感，但是有积累误差。（3）磁力计，他的存在是因为单靠加速度计和陀螺仪没有办法求解出稳定的没有零飘的角度，其实主要是因为处于水平状态的加速度计没有办法测量yaw角。但是磁力计本身容易受到周围磁场的影响，因此在高压电，变压器这些强磁东西周围，四轴飞行器容易炸鸡。关于标定，比较粗糙的标定是，（1）陀螺仪在开机时静置，多次采样求平均，得到偏置值，以后每次陀螺仪采样后都减去对应轴的偏置值；（2）磁力计的椭圆拟合（离线处理）；（3）加速度计，在运行的时候弄个低通滤波。
在捷联惯性导航这一块我也是小白，说的不对的或有需要改进的欢迎指出。