[發明專利]一種平面磁場校正方法及其裝置

在審

• 申請號: 201910241202.8
• 申請日: 2019-03-28
• 公開（公告）號: CN110031020A
• 公開（公告）日: 2019-07-19
• 發明（設計）人: 干曉明;祝崢
• 申請（專利權）人: 廣州英卓電子科技有限公司
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 廣州世超知識產權代理事務所(普通合伙) 44498
• 代理人: 張超

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軸地磁傳感器，用于檢測地磁場的強度和方向信息并輸出。