本文为博主“声时刻”原创文章,未经博主允许不得转载。
联系方式:shenshikexmu@163.com
问题
考虑到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轴度量单位不相同,各轴也并非相互垂直,(说明一下,任意椭球的三个轴都是相互垂直的,几何上,椭球最长的轴与最短的轴相互垂直,从代数的角度看,对称正定矩阵 A = R ′ B R A=R'BR A=R′BR,其中B为对角线大于0表示各轴长度,其他位置为0的矩阵, R R R为旋转矩阵, R ′ R = I R'R=I R′R=I,所以磁通量的空间坐标虽然形成一个椭球,椭球各轴相互垂直,但这个垂直的轴已经不是传感器x,y,z轴了)椭球球心也并非[0,0,0]坐标磁通量在三维空间组成的椭球球心,是磁力计的校准值的一部分。
数学模型
所以问题在于给定椭球球面上的点,如何求椭球球心。其实就是一个椭球拟合问题。
a 1 x 2 + a 2 y 2 + a 3 z 2 + a 4 x y + a 5 x z + a 6 y z + a 7 x + a 8 y + a 9 z = 1 (1) a_1x^2+a_2y^2+a_3z^2+a_4xy+a_5xz+a_6yz+a_7x+a_8y+a_9z=1 \tag{1} a1x2+a2y2+a3z2+a4xy+a5xz+a6yz+a7x+a8y+a9z=1(1)
从几何的角度表示上式的椭球为
[ x − c x y − c y z − c z ] [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] T [ λ 1 0 0 0 λ 2 0 0 0 λ 3 ] [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] [ x − c x y − c y z − c z ] = 1 + [ c x c y c z ] [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] T [ λ 1 0 0 0 λ 2 0 0 0 λ 3 ] [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] [ c x c y c z ] (2) \begin{bmatrix} x-c_x & y-c_y&z-c_z\end{bmatrix}\begin{bmatrix} r_{11} & r_{12} &r_{13} \\ r_{21} & r_{22}&r_{23} \\r_{31} &r_{32}&r_{33}\end{bmatrix}^T\begin{bmatrix} \lambda_1 & 0 &0 \\ 0 & \lambda_2&0 \\0 &0&\lambda_3\end{bmatrix}\begin{bmatrix} r_{11} & r_{12} &r_{13} \\ r_{21} & r_{22}&r_{23} \\r_{31} &r_{32}&r_{33}\end{bmatrix}\begin{bmatrix} x-c_x \\ y-c_y\\z-c_z\end{bmatrix}\\= 1+\begin{bmatrix} c_x & c_y&c_z\end{bmatrix}\begin{bmatrix} r_{11} & r_{12} &r_{13} \\ r_{21} & r_{22}&r_{23} \\r_{31} &r_{32}&r_{33}\end{bmatrix}^T\begin{bmatrix} \lambda_1 & 0 &0 \\ 0 & \lambda_2&0 \\0 &0&\lambda_3\end{bmatrix}\begin{bmatrix} r_{11} & r_{12} &r_{13} \\ r_{21} & r_{22}&r_{23} \\r_{31} &r_{32}&r_{33}\end{bmatrix}\begin{bmatrix} c_x \\ c_y\\c_z\end{bmatrix} \tag{2} [x−cxy−cyz−cz] r11r21r31r12r22r32r13r23r33 T λ1000λ2000λ3 r11r21r31r12r22r32r13r23r33 x−cxy−cyz−cz =1+[cxcycz] r11r21r31r12r22r32r13r23r33 T λ1000λ2000λ3 r11r21r31r12r22r32r13r23r33 cxcycz (2)
上式写成矩阵形式
[
X
−
C
]
M
[
X
−
C
]
T
=
1
+
C
M
C
T
(3)
[X-C]M[X-C]^T=1+CMC^T\tag{3}
[X−C]M[X−C]T=1+CMCT(3)
X
M
X
T
−
2
C
M
X
T
+
C
M
C
T
=
1
+
C
M
C
T
(4)
XMX^T-2CMX^T+CMC^T=1+CMC^T\tag{4}
XMXT−2CMXT+CMCT=1+CMCT(4)
其中
X
=
[
x
y
z
]
X=\begin{bmatrix}x &y & z\end{bmatrix}
X=[xyz],表示球面上的点。
C
=
[
c
x
c
y
c
z
]
C=\begin{bmatrix}c_x &c_y & c_z\end{bmatrix}
C=[cxcycz],表示球心。
M
=
R
T
B
R
=
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
T
[
λ
1
0
0
0
λ
2
0
0
0
λ
3
]
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
=
[
a
1
a
4
/
2
a
5
/
2
a
4
/
2
a
2
a
6
/
2
a
5
/
2
a
6
/
2
a
3
]
M=R^TBR=\begin{bmatrix} r_{11} & r_{12} &r_{13} \\ r_{21} & r_{22}&r_{23} \\r_{31} &r_{32}&r_{33}\end{bmatrix}^T\begin{bmatrix} \lambda_1 & 0 &0 \\ 0 & \lambda_2&0 \\0 &0&\lambda_3\end{bmatrix}\begin{bmatrix} r_{11} & r_{12} &r_{13} \\ r_{21} & r_{22}&r_{23} \\r_{31} &r_{32}&r_{33}\end{bmatrix}=\begin{bmatrix} 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{bmatrix}
M=RTBR=
r11r21r31r12r22r32r13r23r33
T
λ1000λ2000λ3
r11r21r31r12r22r32r13r23r33
=
a1a4/2a5/2a4/2a2a6/2a5/2a6/2a3
C = − 1 2 [ a 7 a 8 a 9 ] ( M ) − 1 C=-\frac{1}{2}[a_7\ a_8\ a_9](M)^{-1} C=−21[a7 a8 a9](M)−1
S
S
=
C
M
C
T
+
1
SS=CMC^T+1
SS=CMCT+1
x
s
c
a
l
e
=
S
S
λ
1
x_{scale}=\sqrt{\frac{SS}{\lambda_1}}
xscale=λ1SS,椭球的x轴长度
y
s
c
a
l
e
=
S
S
λ
2
y_{scale}=\sqrt{\frac{SS}{\lambda_2}}
yscale=λ2SS,椭球的y轴长度
z
s
c
a
l
e
=
S
S
λ
3
z_{scale}=\sqrt{\frac{SS}{\lambda_3}}
zscale=λ3SS,椭球的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
mean_x=mean(data(:,1));
mean_y=mean(data(:,2));
mean_z=mean(data(:,3));
x=data(:,1)-mean_x;
y=data(:,2)-mean_y;
z=data(:,3)-mean_z;
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+[mean_x,mean_y,mean_z]);
Scale_axis=round(Scale_axis);
figure
plot3(data(:,1),data(:,2),data(:,3),'b.',Center(1),Center(2),Center(3),'ro');
axis equal
xlabel('X');
ylabel('Y');
zlabel('Z');
end
加速度校正
加速度校准,椭球拟合
磁力计校正
磁力计校正,椭圆拟合
补充
新写的一篇关于IMU校正与姿态融合的博客IMU校正以及姿态融合,里面有更好校正参数。
具体请参考github开源代码:IMUCalibration-Gesture.
赞助:如果您觉得此文对您所要做的工作有帮助,欢迎在github上标星星。