版权声明:本文为博主原创文章,未经博主允许不得转载。
博主:shenshikexmu
联系方式:[email protected]
问题
考虑到IMU中,x,y,z轴的度量单位并不相同,假设各轴之间相互直。
那么加速度传感器在静止状态(也就是只受重力的状态下),各个姿态只受重力的,x,y,z轴值(假设x,y,z轴相互垂直并且度量单位都一致,如mpu9250三轴的度量单位都是2048,16g量程的情况下),在三维空间中,重力点都在一个球面上,但各轴之间的度量单位都会有偏差,所以各姿态重力点都落在一个椭球面上,椭球的中心,就是加速度的偏移量,也就是校准值。
在磁力计上,由于测量磁场强度,在环境不变的情况下,传感器每个姿态感受磁场强度是相同,所以不需要静止状态,磁力计测量的x,y,z轴值,在没有偏差的情况下,在传感器内部x,y,z轴相互垂直的情况下,在三维空间中组成一个圆球面。但是磁力计存在Hard Iron Distortion和Soft Iron Distortion。使得x,y,z轴度量单位不相同,各轴也并非相互垂直,(说明一下,任意椭球的三个轴都是相互垂直的,几何上,椭球最长的轴与最短的轴相互垂直,从代数的角度看,对称正定矩阵 ,其中B为对角线大于0表示各轴长度,其他位置为0的矩阵, 为旋转矩阵, ,所以磁通量的空间坐标虽然形成一个椭球,椭球各轴相互垂直,但这个垂直的轴已经不是传感器x,y,z轴了)椭球球心也并非[0,0,0]坐标磁通量在三维空间组成的椭球球心,是磁力计的校准值的一部分。
数学模型
所以问题在于给定椭球球面上的点,如何求椭球球心。其实就是一个椭球拟合问题。
从几何的角度表示上式的椭球为
上式写成矩阵形式
其中
,表示球面上的点。
,表示球心。
,椭球的x轴长度
,椭球的y轴长度
,椭球的z轴长度
算法实现
function[ Center,Scale_axis] = fit_elliposoid9( data )
% input data is n*3, n points of the ellipsoid surface
% Least Square Method
% a(1)x^2+a(2)y^2+a(3)z^2+a(4)xy+a(5)xz+a(6)yz+a(7)x+a(8)y+a(9)z=1
% output Center is 1*3, center of elliposoid,
% Scale_axis is 1*3, 3 axis' scale
% author Zhang Xin
x=data(:,1);
y=data(:,2);
z=data(:,3);
D=[x.*x y.*y z.*z x.*y x.*z y.*z x y z ];
a=inv(D'*D)*D'*ones(size(x));
M=[a(1) a(4)/2 a(5)/2;...
a(4)/2 a(2) a(6)/2;...
a(5)/2 a(6)/2 a(3)];
Center=-1/2*[a(7),a(8),a(9)]*inv(M);
SS=Center*M*Center'+1;
[U,V]=eig(M);
[~,n1]=max(abs(U(:,1)));
[~,n2]=max(abs(U(:,2)));
[~,n3]=max(abs(U(:,3)));
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);
figure
plot3(x,y,z,'b.',Center(1),Center(2),Center(3),'ro');
axis equal
xlabel('X');
ylabel('Y');
zlabel('Z');
end
加速度校正
http://v.youku.com/v_show/id_XMjcxMzIyOTc4MA==.html?spm=a2h0j.11185381.listitem_page1.5~A
磁力计校正
http://v.youku.com/v_show/id_XMjcxMjk2Nzk1Ng==.html?spm=a2h0j.11185381.listitem_page1.5!2~A
补充
新写的一篇关于IMU校正与姿态融合的博客IMU校正以及姿态融合,里面有更好校正参数。
具体请参考github开源代码:IMUCalibration-Gesture.
赞助:如果您觉得此文对您所要做的工作有帮助,欢迎打赏。或者帮忙在github上标星星。