IMU加速度、磁力计校正－－椭球拟合

本文为博主“声时刻”原创文章，未经博主允许不得转载。
联系方式：[email protected]

问题

考虑到IMＵ中，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轴度量单位不相同，各轴也并非相互垂直，（说明一下，任意椭球的三个轴都是相互垂直的，几何上，椭球最长的轴与最短的轴相互垂直，从代数的角度看，对称正定矩阵$A=R^{\prime }BR$，其中B为对角线大于0表示各轴长度，其他位置为0的矩阵，$R$为旋转矩阵，$R^{\prime }R=I$，所以磁通量的空间坐标虽然形成一个椭球，椭球各轴相互垂直，但这个垂直的轴已经不是传感器x,y,z轴了）椭球球心也并非[0,0,0]坐标磁通量在三维空间组成的椭球球心，是磁力计的校准值的一部分。

数学模型

所以问题在于给定椭球球面上的点，如何求椭球球心。其实就是一个椭球拟合问题。

$$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\text{\hspace{1em}(1)}$$

从几何的角度表示上式的椭球为

$$\begin{gathered} \left[\begin{array}{ccc}x-c_x& y-c_y& z-c_z\end{array}\right]{\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]}^T\left[\begin{array}{ccc}{\lambda }_1& 0& 0\\ 0& {\lambda }_2& 0\\ 0& 0& {\lambda }_3\end{array}\right]\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]\left[\begin{array}{c}x-c_x\\ y-c_y\\ z-c_z\end{array}\right] \\ =1+\left[\begin{array}{ccc}{c}_x& c_y& c_z\end{array}\right]{\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]}^T\left[\begin{array}{ccc}{\lambda }_1& 0& 0\\ 0& {\lambda }_2& 0\\ 0& 0& {\lambda }_3\end{array}\right]\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]\left[\begin{array}{c}{c}_x\\ c_y\\ c_z\end{array}\right]\text{\hspace{1em}(2)} \end{gathered}$$

上式写成矩阵形式

$$[X-C]M[X-C{]}^T=1+CM{C}^T\text{\hspace{1em}(3)}$$
$$XM{X}^T-2CM{X}^T+CM{C}^T=1+CM{C}^T\text{\hspace{1em}(4)}$$

其中
$X=\left[\begin{array}{ccc}x& y& z\end{array}\right]$，表示球面上的点。
$C=\left[\begin{array}{ccc}{c}_x& c_y& c_z\end{array}\right]$，表示球心。
$M=R^{T}BR={\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]}^T\left[\begin{array}{ccc}{\lambda }_1& 0& 0\\ 0& {\lambda }_2& 0\\ 0& 0& {\lambda }_3\end{array}\right]\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]=\left[\begin{array}{ccc}{a}_1& a_4/2& a_5/2\\ a_4/2& a_2& a_6/2\\ a_5/2& a_6/2& a_3\end{array}\right]$

$C=-\frac{1}{2}[a_7{a}_8{a}_9](M{)}^{-1}$

$SS=CM{C}^T+1$
$x_{scale}=\sqrt{\frac{SS}{{\lambda }_1}}$，椭球的x轴长度
$y_{scale}=\sqrt{\frac{SS}{{\lambda }_2}}$，椭球的y轴长度
$z_{scale}=\sqrt{\frac{SS}{{\lambda }_3}}$，椭球的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);                       %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);

figure
plot3(x,y,z,'b.',Center(1),Center(2),Center(3),'ro');
axis equal
xlabel('X');
ylabel('Y');
zlabel('Z');

end
         

加速度校正

加速度校准，椭球拟合

磁力计校正

磁力计校正，椭圆拟合

补充

新写的一篇关于IMU校正与姿态融合的博客IMU校正以及姿态融合，里面有更好校正参数。
具体请参考github开源代码：IMUCalibration-Gesture.

赞助：如果您觉得此文对您所要做的工作有帮助，欢迎打赏。或者帮忙在github上标星星。

---