[发明专利]一种平面磁场校正方法及其装置

权利要求书

1.一种平面磁场校正方法，其特征在于：具体包括以下步骤：

步骤一：将包括有九轴传感器的校正装置安装固定在待校正的设备上，做初始化校准；

步骤二：将所述设备在任一个平面旋转一圈，获取3轴地磁传感器上地磁数据；

步骤三：将上述步骤二所获取的地磁数据拟合圆形，输出地磁修正值；

步骤四：结合3轴加速传感器和3轴陀螺仪感应器记录的数据，对步骤三解析出的地磁修正值进行坐标系旋转，计算出修正后的航向角。

2.根据权利要求1所述的一种平面磁场校正方法，其特征在于：所述地磁修正值X，Y的计算方法为：对于获得的地磁原始数据，记录为(x1,y1)、(x2，y2)、(x3、y3)…，x的补偿量设为offsetX，y的补偿量设为offsetY；拟合后的圆心设为(centreX，centreY)，半径为R；

X1＝∑x；Y1＝∑y；

X2＝∑x^2；Y2＝∑y^2；

X3＝∑x^3；Y3＝∑y^3；

X1Y1＝∑(x*y)；X1Y2＝∑(x*(y^2))；

X2Y1＝∑((x^2)*y)；

N＝pointX.size()；

C＝N*X2-X1*X1；

D＝N*X1Y1-X1*Y1；

E＝N*X3+N*X1Y2-(X2+Y2)*X1；

G＝N*Y2-Y1*Y1；

H＝N*X2Y1+N*Y3-(X2+Y2)*Y1；

a＝(H*D-E*G)/(C*G-D*D)；a＝x点集的最大值–x点集的最小值；

b＝(H*C-E*D)/(D*D-G*C)；b＝y点集的最大值–y点集的最小值；

c＝-(a*X1+b*Y1+X2+Y2)/N；

R^2＝(a^2+b^2-4*c)/4；

得到X的补偿量offsetX＝2*R/a；

得到Y的补偿量offsetY＝2*R/b；

centreX＝(x点集的最大值–x点集的最小值)/2；

centreY＝(y点集的最大值–y点集的最小值)/2；

则地磁修正值：x＝x*offsetX–centreX；

y＝y*offsetY–centreY；

根据权利要求2所述的一种平面磁场校正方法，其特征在于：结合3轴加速传感器和3轴陀螺仪感应器记录的数据，对所述地磁修正值进行坐标系旋转，设四元数为q0,q1,q2,q3，加速度的参考系旋转矩阵A为

3轴地磁传感器数据则3轴地磁传感器在物体坐标系里的表示变换到参考坐标系为：

设gx，gy,gz表示3轴陀螺仪三个轴数据；ax,ay,az表示3轴加速度感应器三个轴数据；mx,my,mz表示3轴地磁传感器三个轴数据；Ki＝0.005；Kp＝2；halfT＝0.005；

辅助量：q0q0＝q0*q0；q0q1＝q0*q1；q0q2＝q0*q2；q0q3＝q0*q3；

q1q1＝q1*q1；q1q2＝q1*q2；q1q3＝q1*q3；q2q2＝q2*q2；q3q3＝q3*q3

将加速度的原始数据，归一化，得到单位3轴加速度感应器三个轴数据：

norm^2＝ax^2+ay^2+az^2；

ax＝ax/norm；

ay＝ay/norm；

az＝az/norm；

将3轴地磁传感器的原始数据，归一化，得到单位3轴地磁传感器三个轴数据：

norm^2＝mx^2+my^2+mz^2；

mx＝mx/norm；

my＝my/norm；

mz＝mz/norm；

则参考磁通方向，正确的磁场方向矢量{bx,0,bz}：：

hx＝2*mx*(0.5-q2q2-q3q3)+2*my*(q1q2-q0q3)+2*mz*(q1q3+q0q2)；

hy＝2*mx*(q1q2+q0q3)+2*my*(0.5-q1q1-q3q3)+2*mz*(q2q3-q0q1)；

hz＝2*mx*(q1q3-q0q2)+2*my*(q2q3+q0q1)+2*mz*(0.5-q1q1-q2q2)

bx^2＝(hx*hx+hy*hy)^2

bz＝hz；

设物体的xyz轴方向上的加速度vx，vy，vz：

vx＝2*(q1q3–q0q2)；

vy＝2(q0q1+q2q3)；

vz＝q0q0-q1q1-q2q2+q3q3；

转化到正确磁场矢量{wx,wy,wz}：

wx＝2*bx*(0.5-q2q2-q3q3)+2bz(q1q3-q0q2)；

wy＝2bx(q1q2-q0q3)+2bz(q0q1+q2q3)；

wz＝2bx(q0q2)+q1q3+2bz(0.5-q1q1-q2q2)；

四元数代表了物体的一个姿态，测量数据代表另一姿态，两者误差量：

ex＝(ay*vz-az*vy)+(my*wz-mz*wy)；

ey＝(az*vx-ax*vz)+(mz*wx-mx*wz)；

ez＝(ax*vy-ay*vx)+(mx*wy-my*wx)；

将叉乘误差进行积分，则积分误差比例增益：

exInt＝ex*Ki；

eyInt＝ey*Ki；

ezInt＝ez*Ki；

调整后3轴陀螺仪数据：

gx＝gx+Kp*ex+exInt；

gy＝gy+Kp*ey+eyInt；

gz＝gz+Kp*ez+ezInt；

更新四元数据:

q0＝q0+(-q1*gx–q2*gy–q3*gz)*halfT；

q1＝q1+(q0*gx+q2*gz-q3*gy)*halfT；

q2＝q2+(q0*gy-q1*gz+q3*gx)*halfT；

q3＝q3+(q0*gz+q1*gy-q2*gx)*halfT；

将四元数单位化：

norm＝(q0^2+q1^2+q2^2+q3^2)^2；

q0＝q0/norm；

q1＝q1/norm；

q2＝q2/norm；

q3＝q3/norm；

根据四元数积分误差比例增益，姿态校正补偿后获得正确的四元数，计算出修正后的航向角yaw为：

yaw＝atan2(2*q0*q3+2q1*q2,1-2q2*q2-2q3*q3)/3.14*180。

3.一种平面磁场校正装置，其特征在于，包括：九轴传感器、电源、存储有可被执行用于实现如权利要求1至3中任一项所述方法的程序的存储器以及用于执行如权利要求1至3中任一项所述方法的程序的数据处理器；

所述九轴传感器包括：

3轴陀螺仪，用于检测物体相对于地理坐标系的角速度信号并输出；

3轴加速度感应器，用于检测物体在载体坐标系中三个轴的加速度信号并输出；

3轴地磁传感器，用于检测地磁场的强度和方向信息并输出。