地磁矫正问题
讨论矫正问题之前先了解一下地磁传感器测得数据具体代表什么。本文中使用的地磁传感器为\(MPU9250\)内置地磁传感器.
首先分析理想情况，地磁传感器所测位置的地磁数据可以理解为三维坐标系下的向量在地磁传感器三维坐标下的投影，如下图：
由于我们使用地磁传感器主要用来修正\(yaw\)轴角度，并且传感器的使用场景大部分为水平放置，这里我们忽略地磁传感器测得的\(z\)轴分量，仅关注由于传感器的旋转引起的\(xy\)轴分量变化。
在水平放置的前提下，同理可将地磁数据作为一个二维向量，所测得的数据为该向量在\(xy\)坐标下的投影。由于地磁向量方向大小不变，在\(xy\)平面旋转地磁传感器时(相当于旋转二维\(xy\)坐标系)，可以等效为一个大小不变的向量围绕原点旋转，得到的轨迹理论上为标准圆形。
对于标准的坐标为原点的轨迹，我们可以很容易得到地磁向量与地磁传感器\(xy\)轴的夹角。但是实际得到的数据往往是椭圆形，甚至是原点不在原点且带有偏移角的椭圆，实际数据如下图：
在使用罗盘传感器之前，需要对其进行校准以消除两个主要误 差。一个是失调误差，这原本是由传感器和电路的失调误差引起 的。另一个是标度误差。这两种误差都容易受到周围磁环境的干 扰。例如，如果有一个x轴向的外部磁场施加到传感器上，就会 给出外部x轴失调误差。同时，x轴标度也将与y轴和z轴不同。
通常用于校准磁传感器的方法是在\(xy\)平面上转动传感器绕圈，然 后抽取数据。一个地点的地磁场强度是一个常数值，因此绘制的 数据应该是一个圆；然而，事实上，我们将看到一个椭圆形，这 意味着我们需要移动椭圆并重新缩放到以零为中心的圆。
最小二乘法椭圆矫正
椭圆方程
标准椭圆方程为：
\[\frac{x^2}{a^2}+\frac{y^2}{b^2}=1(a>b>0)
\]
偏心且带有偏移角的椭圆方程一般表达式为：
\[x^2+Axy+By^2+Cx+Dy+E=0
\]
将上式变换为标准椭圆形式，首先对其平移
\[x=x_1-x_0\\
y=y_1-y_0
\]
代入(2)：
\[(x_1-x_0)^2+A(x_1-x_0)(y_1-y_0)+B(y_1-y_0)^2+C(x_1-x_0)+D(y_1-y_0)+E=0\\
x_1^2+Ax_1y_1+By_1^2+(-2x_0-Ay_0+C)x_1+(-Ax_0-2By_0+D)y_1+(x_0^2+Ax_0y_0+By_0^2-Cx_0-Dy_0+E)=0
\]
中心点为坐标原点的椭圆关于原点对称，即对于图像上任意一点\((x,y)\)以及对应的\((-x,-y)\)均满足椭圆方程，通俗一些就是椭圆方程代入\((x,y)\)和\((-x,-y)\)表达式不变，对于(4)而言要使其满足原点对称，项\(x_1,y_1\)的系数应该为0
\[-2x_0-Ay_0+C=0\\
-Ax_0-2By_0+D=0\\
可得\\
x_0=\frac{AD-2BC}{A^2-4B}\\
y_0=\frac{AC-2D}{A^2-4B}
\]
中心点为\(x_c=-x_0;y_c=-y_0\)，此时方程形式为：
\[x_1^2+Ax_1y_1+By_1^2-x_0^2-Ax_0y_0-By_0^2+E=0
\]
再通过把坐标旋转把原椭圆放在旋转后的坐标下即可得到带偏移角的椭圆：
旋转矩阵为
\[\left[
\matrix{
x_1\\
y_1
}
\right]=
\left[
\matrix{
cos\theta&sin\theta\\
-sin\theta&cos\theta
}
\right]\left[
\matrix{
x_t\\
y_t
}
\right]
\\
x_1=x_tcos\theta +y_tsin\theta\\
y_1=-x_tsin\theta+y_tcos\theta
\]
由于旋转矩阵为正交矩阵即旋转矩阵的逆矩阵为旋转矩阵的转置
\[\left[
\matrix{
cos\theta&sin\theta\\
-sin\theta&cos\theta
}
\right]^T
\left[
\matrix{
cos\theta&sin\theta\\
-sin\theta&cos\theta
}
\right]=E
\]
参考旋转矩阵（Rotation Matrix)的推导 - 知乎
将（9）代入椭圆方程（6）
\[(x_tcos\theta+y_tsin\theta)^2+A(x_tcos\theta+y_tsin\theta)(-x_tsin\theta+y_tcos\theta)+B(-x_tsin\theta+y_tcos\theta)^2-x_0^2-Ax_0y_0-By_0^2+E=0\\
\]
化简得：
\[(cos^2\theta -Asin\theta cos\theta+Bsin^2\theta)x_t^2+(2sin\theta cos\theta+A(cos^2\theta-sin^2\theta)-2Bsin\theta cos\theta)x_ty_t\\
(sin^2\theta+Asin\theta cos\theta+Bcos^2\theta)y_t^2-x_0^2-Ax_0y_0-By_0^2+E=0
\]
无偏移角得椭圆关于x轴和y轴对称，即项\(x_ty_t\)的系数为0
\[2sin\theta cos\theta+A(cos^2\theta-sin^2\theta)-2Bsin\theta cos\theta=0\\
A=\frac{2sin\theta cos\theta(B-1)}{cos^2\theta-sin^2\theta}\\
tan2\theta=\frac{2tan\theta}{1-tan^2\theta}=\frac{2sin\theta cos\theta}{cos^2\theta-sin^2\theta}\\
=\frac{A}{B-1}
\]
可得偏移角为：
\[\theta=\frac{1}{2}arctan(\frac{A}{B-1})
\]
由标准椭圆转换到偏移角的角度为\(\theta=-\theta\)
此时椭圆方程为
\[(cos^2\theta -Asin\theta cos\theta+Bsin^2\theta)x_t^2+(sin^2\theta+Asin\theta cos\theta+Bcos^2\theta)y_t^2=x_0^2+Ax_0y_0+By_0^2-E\\
\frac{x_t^2}{\frac{x_0^2+Ax_0y_0+By_0^2-E}{cos^2\theta -Asin\theta cos\theta+Bsin^2\theta}}+\frac{y_t^2}{\frac{x_0^2+Ax_0y_0+By_0^2-E}{sin^2\theta+Asin\theta cos\theta+Bcos^2\theta}}=1
\]
可得
\[a=\sqrt\frac{x_0^2+Ax_0y_0+By_0^2-E}{cos^2\theta -Asin\theta cos\theta+Bsin^2\theta}\\
b=\sqrt\frac{x_0^2+Ax_0y_0+By_0^2-E}{sin^2\theta+Asin\theta cos\theta+Bcos^2\theta}
\]
最小二乘法拟合椭圆
对于椭圆方程
\[F(x,y)=x^2+Axy+By^2+Cx+Dy+E=0
\]
最小二乘法的核心是对于采集到的一组数据{\(x,y|x_1...x_n;y_1...y_n\)}使方差和最小，即：
\[\sigma=\sum_{i=1}^n(x_i^2+Ax_iy_i+By_i^2+Cx_i+Dy_i+E-0)^2
\]
对\(\sigma\)分别求\(ABCDE\)偏导数为0的值即为方差最小的取值
\[\frac{d\sigma}{dA}=2(x_i^2+Ax_iy_i+By_i^2+Cx_i+Dy_i+E)x_iy_i=0\\
\frac{d\sigma}{dB}=2(x_i^2+Ax_iy_i+By_i^2+Cx_i+Dy_i+E)y_i^2=0\\
\frac{d\sigma}{dC}=2(x_i^2+Ax_iy_i+By_i^2+Cx_i+Dy_i+E)x_i=0\\
\frac{d\sigma}{dD}=2(x_i^2+Ax_iy_i+By_i^2+Cx_i+Dy_i+E)y_i=0\\
\frac{d\sigma}{dE}=2(x_i^2+Ax_iy_i+By_i^2+Cx_i+Dy_i+E)*0=0\\
\]
对于偏导等于0情况，忽略常数2，写为矩阵形式为：
\[\left[
\matrix{
\sum x_i^2y_i^2&\sum x_iy_i^3&\sum x_i^2y_i&\sum x_iy_i^2&\sum x_iy_i\\
\sum x_iy_i^3&\sum y_i^4&\sum x_iy_i^2&\sum y_i^3&\sum y_i^2\\
\sum x_i^2y_i&\sum x_iy_i^2&\sum x_i^2&\sum x_iy_i&\sum x_i\\
\sum x_iy_i^2&\sum y_i^3&\sum x_iy_i&\sum y_i^2&\sum y_i\\
\sum x_iy_i&\sum y_i^2&\sum x_i&\sum y_i&\sum 1
}
\right]
\left[
\matrix{
A\\
B\\
C\\D\\E
}
\right]=-
\left[
\matrix{
\sum x_i^3y_i\\
\sum x_i^2y_i^2\\
\sum x_i^3\\
\sum x_i^2y_i\\
\sum x_i^2
}
\right]
\]
简化为：
\[M_1
\left[
\matrix{
A\\
B\\
C\\D\\E
}
\right]=M_2\\
\left[
\matrix{
A\\
B\\
C\\D\\E
}
\right]=M_1^{-1}M_2
\]
即可得到\(ABCDE\)参数。
MATLAB拟合实验
对于\(MPU9250\)数据采用直接与MATLAB进行串口通信，具体代码如下：
下位机上传：
//定义共用体便于发送数据
#pragma pack(1)
typedef struct
{
int16_t acc[3];
int16_t mag[3];
int16_t gyro[3];
} Frame;
#pragma pack()
union Data_send_
{
Frame sent;
int8_t str_send[18];
};
union Data_send_ hex;
//需要注意的是这里发送的原始数据并未进行任何处理，帧头为‘\r’
void Matlab_print(void)
{
printf("\r");
HAL_UART_Transmit(&huart1,hex.str_send,18, 0xff);
}
MATLAB上位机
%%%%%%%%%%串口数据处理%%%%%%%%%%%%%
clear all
delete(instrfindall);
%%设置串口波特率
port = serial('com5','BaudRate',115200,'BytesAvailableFcnMode','byte');
fopen(port)
%定义帧格式
frame = zeros(1,9,'int16');
%定义地磁数据
magf = [];
i = 1;
tic
while true
%0.01s读取一次
pause(0.01-toc)
%读取一个字节数据
frame_head = fread(port,1,'int8');
%该字节为帧头‘\r’
if frame_head == 13
%继续读取九个数据
frame = fread(port,9,'int16');
magf(i,1) = frame(4);
magf(i,2) = frame(5);
magf(i,3) = frame(6);
disp(magf(i,:));
i = i+1;
%自定义读取数据长度
if i>2000
break;
end
end
tic;
end
plot(magf(:,1),magf(:,2),'o')
%关闭串口
fclose(port);
MATLAB拟合椭圆函数
function [ellipse] = ellipsefit(x,y)
% 采用最小二乘法进行椭圆拟合
% 采用椭圆一般式子：x^2 + A*x*y + B*y^2 + C*x + D*y + E = 0;
xlength = length(x);
xmax = max(x);
ymax = max(y);
if(xlength ~= length(y) | xlength < 5)
warning('椭圆拟合至少需要四个点数据');
else
M1 = [ sum(x.^2.*y.^2), sum(x.*y.^3), sum(x.^2.*y), sum(x.*y.^2), sum(x.*y)
sum(x.*y.^3), sum(y.^4), sum(x.*y.^2), sum(y.^3), sum(y.^2)
sum(x.^2.*y), sum(x.*y.^2), sum(x.^2), sum(x.*y), sum(x)
sum(x.*y.^2), sum(y.^3), sum(x.*y), sum(y.^2), sum(y)
sum(x.*y), sum(y.^2), sum(x), sum(y), xlength] ;
%M1满秩才可逆
if rank(M1) == 5
M2 = -[ sum(x.^3.*y); sum(x.^2.*y.^2); sum(x.^3); sum(x.^2.*y); sum(x.^2)];
G = inv(M1)*M2;
[A,B,C,D,E] = deal(G(1),G(2),G(3),G(4),G(5));
ellipse = [A,B,C,D,E];
Xp = (A*D-2*B*C)/(A*A-4*B);
Yp = (A*C-2*D)/(A*A-4*B);
Xc = -Xp;
Yc = -Yp;
theta_r = 0.5*atan(A/(B-1));
theta_offset = - theta_r;
a = sqrt((Xp^2+A*Xp*Yp+B*Yp^2-E)/(cos(theta_r)^2-A*sin(theta_r)*cos(theta_r)+B*sin(theta_r)^2));
b = sqrt((Xp^2+A*Xp*Yp+B*Yp^2-E)/(sin(theta_r)^2+A*sin(theta_r)*cos(theta_r)+B*cos(theta_r)^2));
%绘图
plot(x,y,'bo')
grid on;
hold on;
fimplicit(@(x,y)x.^2 + A.*x.*y + B.*y.^2 + C.*x + D.*y + E ,'-r','LineWidth', 3)
plot(Xc,Yc,'ro','LineWidth', 3);
plot([Xc Xc+a*cos(theta_offset)],[Yc Yc+a*sin(theta_offset)],'--p','LineWidth', 3);
title(['圆心坐标为(' num2str(Xc) ',' num2str(Yc) '),偏移角为' num2str(theta_offset*180/pi) '°,长轴为' num2str(a) ',短轴为' num2str(b) ...
newline 'x^2 + ' num2str(A) 'xy + ' num2str(B) 'y^2 + ' num2str(C) 'x + ' num2str(D) 'y + ' num2str(E) '= 0']);
axis equal
end
end
end
具体效果：
映射到标准椭圆过程为：
\[x_1=x-x_c\\
y_1=y-y_c\\
\left[
\matrix{
x_t\\
y_t
}
\right]=
\left[
\matrix{
cos\theta&-sin\theta\\
sin\theta&cos\theta
}
\right]\left[
\matrix{
x_1\\
y_1
}
\right]\\=
\left[
\matrix{
cos\theta&sin\theta\\
-sin\theta&cos\theta
}
\right]
\left[
\matrix{
x-x_c\\
y-y_c
}\right]
\]
根据采集的\(xy\)数据计算\(yaw\)角为
\[yaw=arctan(\frac{y_t}{x_t})
\]
最终采用角速度计融合地磁数据互补滤波，效果如下
互补滤波代码：
void MPU_process(float* acc,float* mag,float* gyro,float* angle)
{
static float last_pitch,last_roll,last_yaw;
//+-16g
acc[0] = G*hex.sent.acc[0]/2048;
acc[1] = G*hex.sent.acc[1]/2048;
acc[2] = G*hex.sent.acc[2]/2048;
//15微特斯拉/lsb
mag[0] = hex.sent.mag[0];
mag[1] = hex.sent.mag[1];
mag[2] = hex.sent.mag[2];
//+-250度/s
gyrof[0] = hex.sent.gyro[0]/32.8;
gyrof[1] = hex.sent.gyro[1]/32.8;
gyrof[2] = hex.sent.gyro[2]/32.8;
//角速度积分
angle[0] = gyrof[0]*0.01;
angle[1] = gyrof[1]*0.01;
angle[2] = gyrof[2]*0.01;
//加速度计算适用于静止或匀速运动
angle[3] = atan((float)acc[0]/(float)acc[2])*57.297;
angle[4] = atan((float)acc[1]/(float)acc[2])*57.297;
angle[5] = atan2((-sin(Theta)*(mag[0]-XC)+cos(Theta)*(mag[1]-YC)),(cos(Theta)*(mag[0]-XC)+sin(Theta)*(mag[1]-YC)))*57.297;
//互补滤波
angle[0] = 0.2*angle[3]+0.8*(last_pitch+angle[0]);
angle[1] = 0.2*angle[4]+0.8*(last_roll+angle[1]);
angle[2] = 0.2*angle[5]+0.8*(last_yaw+angle[2]);
last_pitch = angle[0];
last_roll = angle[1];
last_yaw = angle[2];
}